#include <stdio.h>
#include <string.h>
#include <errno.h>
#include <stdlib.h>

#include "hal_raspberry_pi/hal_raspberry_pi.h"

#include "bindings/bricklet_accelerometer_v2.h"

static void check(int rc, const char *c) {
    if (rc < 0) {
        printf("Failed to %s: %s (return code %d, errno %d)\n", c, tf_strerror(rc), rc, errno);
    }
}

TF_HalContext hal;

#define NUM 4

struct ChannelData {
  uint32_t counter;
  TF_AccelerometerV2 device;
};

struct ChannelData channels[NUM];
char *uids[NUM] = {"2kAkfr", "2kBkGe", "2uvASe", "Wdm9R"};

void callback_continuous_accelerometer_16bit(TF_AccelerometerV2 *device,
                                       int16_t acceleration[30],
                                       void *user_data) {
  struct ChannelData *channel = (struct ChannelData *) user_data;
  channel->counter += 1;
}

void callback_continuous_accelerometer_8bit(TF_AccelerometerV2 *device,
                                       int8_t acceleration[60],
                                       void *user_data) {
  struct ChannelData *channel = (struct ChannelData *) user_data;
  channel->counter += 1;
}

void demo_setup(TF_HalContext *hal) {
  for (int i=0; i<NUM; i++) {

    check(tf_accelerometer_v2_create(&channels[i].device, &uids[i][0], hal), "create acc");
    channels[i].counter = 0;

    // register continuous callback
    tf_accelerometer_v2_register_continuous_acceleration_16_bit_callback(&channels[i].device,
                                                                        callback_continuous_accelerometer_16bit,
                                                                        &channels[i]);
    tf_accelerometer_v2_set_continuous_acceleration_configuration(&channels[i].device,
                                                                false,
                                                                false,
                                                                true,
                                                                TF_ACCELEROMETER_V2_RESOLUTION_16BIT);
    tf_accelerometer_v2_set_configuration(&channels[i].device,
                                        TF_ACCELEROMETER_V2_DATA_RATE_6400HZ,
                                        TF_ACCELEROMETER_V2_FULL_SCALE_2G);
  }
}

int main(int argc, char **argv) {
    TF_Port ports[5] = {{
        .chip_select_pin=27,
        .port_name = 'A'
    }, {
        .chip_select_pin=23,
        .port_name = 'B'
    }, {
        .chip_select_pin=24,
        .port_name = 'C'
    }, {
        .chip_select_pin=22,
        .port_name = 'D'
    }, {
        .chip_select_pin=25,
        .port_name = 'E'
    }};

    check(tf_hal_linux_init(&hal, ports, sizeof(ports)/sizeof(ports[0])),
          "init hal");

    demo_setup(&hal);

    uint32_t bench_start = tf_hal_current_time_us(&hal);

    tf_hal_callback_tick(&hal, 5000000);

    float bench_time = (tf_hal_current_time_us(&hal) - bench_start) / 1000000.0;
    
    int32_t counter_sum = 0;
    for (int i = 0; i<NUM; ++i) {
        printf("Channel %d: Packet counter is %u after %f seconds.\n",
               i, channels[i].counter, bench_time);
        counter_sum += channels[i].counter;
    }

    float num_packets = counter_sum / bench_time;
    printf("Combined: %f packets per second\n", num_packets);
    printf("%f samples per channel per second\n", num_packets / NUM * 30.0);
    return 0;
}