#include "bluetooth.h"
#include "image.h"
#ifndef VECTOR_H
#include "vector.h"
#define VECTOR_H
#endif

TaskHandle_t imuTask;
TaskHandle_t bluetoothTask;

void setup()
{
  Serial.begin(115200);
  while (!Serial)
    delay(10);
  Serial.println("starting");

  bluetooth_setup();
  camera_setup();
  //  imu_setup();
  //
  //  xTaskCreatePinnedToCore(
  //                    task1,   /* Task function. */
  //                    "imuTask",     /* name of task. */
  //                    10000,       /* Stack size of task */
  //                    NULL,        /* parameter of the task */
  //                    1,           /* priority of the task */
  //                    &imuTask,      /* Task handle to keep track of created task */
  //                    0);          /* pin task to core 0 */
}

// void task1(void * pvParameters) {
//   // use core 0
//
//   //calibrate();
//   for(;;){
//     send_bluetooth(2.0);
//     delay(10); // bluetooth stack will go into congestion, if too many packets are sent
//     check_bluetooth_connection();
//   }
// }

void loop()
{
  Vector pos = get_light_pos();
  send_bluetooth(pos);
  delay(10); // bluetooth stack will go into congestion, if too many packets are sent
  check_bluetooth_connection();
}