#include "demo.h"

#include <stdio.h>

#include "../bindings/bricklet_accelerometer_v2.h"

#define NUM 4
#define HIGH_THROUGHPUT true
#define BRICKLET_FULL_SCALE TF_ACCELEROMETER_V2_FULL_SCALE_2G
#define BRICKLET_DATA_RATE_PERIODIC TF_ACCELEROMETER_V2_DATA_RATE_1600HZ
#define BRICKLET_DATA_RATE_CONTINUOUS TF_ACCELEROMETER_V2_DATA_RATE_12800HZ

struct ChannelData {
  int8_t id;
  int32_t counter;
  int32_t last_time;
  TF_AccelerometerV2 device;
  TF_HalContext *hal;
};

struct ChannelData channels[NUM];
char *uids[NUM] = {"replace_a", "replace_b", "replace_c", "replace_d"};

void check(int rc, char *msg) {
  if (rc >= 0) {
    return;
  }

  tf_hal_log_error("Failed to %s: %d", msg, rc);
}

bool has_one_second_passed(struct ChannelData *channel) {
  bool has_passed = false;
  uint32_t now = tf_hal_current_time_us(channel->hal);

  if ((now - channel->last_time) > 999999) {
    has_passed = true;
    channel->last_time = now;
  }

  return has_passed;
}

void callback_continuous_accelerometer(TF_AccelerometerV2 *device,
                                       int16_t acceleration[30],
                                       void *user_data) {

  // receive user data as ChannelData
  struct ChannelData *channel = (struct ChannelData *) user_data;
  channel->counter += 30;

  if (has_one_second_passed(channel)) {
    printf("Number of samples per second for channel %d: %d\n",
           channel->id, channel->counter);
    channel->counter = 0;
  }

}

void callback_periodic_accelerometer(TF_AccelerometerV2 *device,
                                     int32_t x,
                                     int32_t y,
                                     int32_t z,
                                     void *user_data) {

  // receive user data as ChannelData
  struct ChannelData *channel = (struct ChannelData *) user_data;
  channel->counter++;

  if (has_one_second_passed(channel)) {
    printf("Number of samples per second for channel %d: %d\n",
           channel->id, channel->counter);
    channel->counter = 0;
  }

}

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].id = i;
    channels[i].counter = 0;
    channels[i].last_time = tf_hal_current_time_us(hal);
    channels[i].hal = hal;

    if (HIGH_THROUGHPUT) {
      // register continuous callback
      tf_accelerometer_v2_register_continuous_acceleration_16_bit_callback(&channels[i].device,
                                                                           callback_continuous_accelerometer,
                                                                           &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,
                                            BRICKLET_DATA_RATE_CONTINUOUS,
                                            BRICKLET_FULL_SCALE);
    } else {
      // register periodic callback
      tf_accelerometer_v2_register_acceleration_callback(&channels[i].device,
                                                         callback_periodic_accelerometer,
                                                         &channels[i]);
      tf_accelerometer_v2_set_acceleration_callback_configuration(&channels[i].device,
                                                                  1,
                                                                  false);
      tf_accelerometer_v2_set_configuration(&channels[i].device,
                                            BRICKLET_DATA_RATE_PERIODIC,
                                            BRICKLET_FULL_SCALE);
    }

  }
}

void demo_loop(TF_HalContext *hal) {
  for (int i=0; i<NUM; i++) {
    tf_accelerometer_v2_callback_tick(&channels[i].device, 250);
  }
}