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

#include "hal_linux/hal_linux.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 {
  int32_t counter;
  TF_AccelerometerV2 device;
};

struct ChannelData channels[NUM];
char *uids[NUM] = {"MGA", "MFm", "JpE", "Jff"};

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_12800HZ,
                                        TF_ACCELEROMETER_V2_FULL_SCALE_2G);
  }
}

int main(int argc, char **argv) {
    printf("Hello World!\n");

    TF_Port ports[9] = {{
        .chip_select_pin=23,
        .port_name = 'A'
    }, {
        .chip_select_pin=22,
        .port_name = 'B'
    }, {
        .chip_select_pin=25,
        .port_name = 'C'
    }, {
        .chip_select_pin=26,
        .port_name = 'D'
    }, {
        .chip_select_pin=27,
        .port_name = 'E'
    }, {
        .chip_select_pin=24,
        .port_name = 'F'
    }, {
        .chip_select_pin=7,
        .port_name = 'G'
    }, {
        .chip_select_pin=6,
        .port_name = 'H'
    }, {
        .chip_select_pin=5,
        .port_name = 'I'
    }};

    check(tf_hal_linux_init(&hal, "/dev/spidev0.0", 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 < 4; ++i) {
        printf("Channel %d: Packet counter is %d after %f seconds.\n", i, channels[i].counter, bench_time);
        counter_sum += channels[i].counter;
    }
    printf("Combined: %f packets per second\n", counter_sum / bench_time);
    return 0;
}