// dataHarverster.ino
// last update: 07.03.2023
// author: Chenxiang Zhang
// descreption: data harverster program for Nicla Arduino board,
//              used together with dataHarvester program
// library: ArduinoBLE
//          Arduino_BHY2
//          PacketSerial

// library include
#include <Nicla_System.h>
#include <Arduino_BHY2.h>
#include <PacketSerial.h>

// instantiation a PacketSerial object for data transfering
PacketSerial myPacketSerial;

// flag for telling the board when to start capturing
auto capture { false };

// structure for data packaging
struct __attribute__((__packed__)) Data {
  // time stamp
  float ts;

  // acceleration
  float ax;
  float ay;
  float az;

  // rotation angle from gyroscope
  float gx;
  float gy;
  float gz;

  // geomagnetic
  float mx;
  float my;
  float mz;

  // gravity
  float grx;
  float gry;
  float grz;

  // orientation
  float pitch;
  float roll;
  float heading;

  // temperature
  float t;
  
  // pressure
  float p;
  
  // humidity, comment when doing 4.2
  float h;

  // gas, comment when doing 4.2
  float g;

  // air quality from bsec, uncomment when doing 4.2
  // float iaq;

  // VOC from bsec, uncomment when doing 4.2
  // float voc;
};

// instantiation of sensors
// comment "hum" and "gas", uncomment "bsec" when doing 4.2
SensorXYZ acc(SENSOR_ID_ACC);
SensorXYZ gyro(SENSOR_ID_GYRO);
SensorXYZ mag(SENSOR_ID_MAG);
SensorXYZ gra(SENSOR_ID_GRA);
SensorOrientation ori(SENSOR_ID_ORI);
Sensor tem(SENSOR_ID_TEMP);
Sensor bar(SENSOR_ID_BARO);
Sensor hum(SENSOR_ID_HUM);
Sensor gas(SENSOR_ID_GAS);
//SensorBSEC bsec(uint8_t(171));

void setup() {
  // put your setup code here, to run once:

  //activate board, LED, and sensors
  nicla::begin();
  nicla::leds.begin();
  BHY2.begin();
  acc.begin();
  gyro.begin();
  mag.begin();
  gra.begin();
  ori.begin();
  tem.begin();
  bar.begin();
  // comment hum and gas, uncomment bsec when doing 4.2
  hum.begin();
  gas.begin();
  // bsec.begin();

  // initialize serial reader
  myPacketSerial.begin(115200);
  myPacketSerial.setPacketHandler(&onPacketReceived);

  // LET blinking indicating the finishing of initialization
  pinMode(LED_BUILTIN, OUTPUT);
  for (auto i = 0u; i < 10; i++) {
    nicla::leds.setColor(green);
    delay(25);
    nicla::leds.setColor(off);
    delay(25);
  }
}

void loop() {
  // put your main code here, to run repeatedly:

  // board state update
  myPacketSerial.update();
  BHY2.update();

  if (myPacketSerial.overflow()) {
    for (auto i = 0u; i < 5; i++) {
      nicla::leds.setColor(green);
      delay(25);
      nicla::leds.setColor(off);
      delay(25);
    }
  }

  if (capture == true) {
    auto now = micros() / 1000.0;

    // comment hum and gas, uncomment bsec when doing 4.2
    Data data { now, acc.x(), acc.y(), acc.z(),
                     gyro.x(), gyro.y(), gyro.z(),
                     mag.x(), mag.y(), mag.z(),
                     gra.x(), gra.y(), gra.z(),
                     ori.pitch(), ori.roll(),ori.heading(),
                     tem.value(),
                     bar.value(),
                     hum.value(),
                     gas.value()
                     // bsec.iaq(),
                     // bsec.b_voc_eq() 
                     };
    constexpr size_t dataBufLen { sizeof(Data) };
    uint8_t dataBuf[dataBufLen] {};

    memcpy(dataBuf, reinterpret_cast<void*>(&data), dataBufLen);

    myPacketSerial.send(dataBuf, dataBufLen);

    delay(100);
  }
}

void onPacketReceived(const uint8_t* buffer, size_t size)
{
  uint8_t tempBuffer[size];

  for (auto i = 0u; i < 2; i++) {
    nicla::leds.setColor(green);
    delay(25);
    nicla::leds.setColor(off);
    delay(25);
  }

  memcpy(tempBuffer, buffer, size);

  switch(tempBuffer[0]) {
    case 'R':
      nicla::leds.setColor(green);
      capture = true;
      break;
    case 'S':
      nicla::leds.setColor(off);
      capture = false;
      break;
    default:
      break;
  }
}