#include "demo.h"

#include <stdio.h>

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

TF_AccelerometerV2 devices[4];
char *uids[4] = {"replace_a", "replace_b", "replace_c", "replace_d"};
int8_t dids[4] = {0, 1, 2, 3};
int32_t counter[4];

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

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

void callback_periodic_accelerometer(TF_AccelerometerV2 *device,
                                     int32_t x,
                                     int32_t y,
                                     int32_t z,
                                     void *user_data) {
  // receive user data as int8_t
  int8_t *id = (int8_t *) user_data;
  counter[*id]++;

  if (*id == 0 && counter[*id] % 1000 == 0) {
    printf("Acceleration for channel %d: %d, %d, %d | num: %d\n",
           *id, x, y, z, counter[*id]);
  }
}

void demo_setup(TF_HalContext *hal) {
  for (int i=0; i<4; i++) {
    check(tf_accelerometer_v2_create(&devices[i], &uids[i][0], hal), "create acc");
    tf_accelerometer_v2_set_configuration(&devices[i],
                                          TF_ACCELEROMETER_V2_DATA_RATE_1600HZ,
                                          TF_ACCELEROMETER_V2_FULL_SCALE_2G);
    tf_accelerometer_v2_set_acceleration_callback_configuration(&devices[i], 1, false);
    tf_accelerometer_v2_register_acceleration_callback(&devices[i],
                                                       callback_periodic_accelerometer,
                                                       &dids[i]);
  }
}

void demo_loop(TF_HalContext *hal) {
   for (int i=0; i<4; i++) {
    tf_accelerometer_v2_callback_tick(&devices[i], 250);
  }
}