#include <Arduino.h>





// GYROSCOPE
#include <Adafruit_BNO08x.h>
#include <Adafruit_NeoPixel.h>

// For SPI mode, we need a CS pin
#define BNO08X_CS 10
#define BNO08X_INT 9

// #define FAST_MODE

// For SPI mode, we also need a RESET 
//#define BNO08X_RESET 5
// but not for I2C or UART
#define BNO08X_RESET -1

struct euler_t {
  float yaw;
  float pitch;
  float roll;
} ypr;

Adafruit_BNO08x  bno08x(BNO08X_RESET);
sh2_SensorValue_t sensorValue;

#ifdef FAST_MODE
  // Top frequency is reported to be 1000Hz (but freq is somewhat variable)
  sh2_SensorId_t reportType = SH2_GYRO_INTEGRATED_RV;
  long reportIntervalUs = 2000;
#else
  // Top frequency is about 250Hz but this report is more accurate
  sh2_SensorId_t reportType = SH2_ARVR_STABILIZED_RV;
  long reportIntervalUs = 5000;
#endif
void setReports(sh2_SensorId_t reportType, long report_interval) {
  Serial.println("Setting desired reports");
  if (! bno08x.enableReport(reportType, report_interval)) {
    Serial.println("Could not enable stabilized remote vector");
  }
}





// ADDRESSABLE LEDS
#define LED_NORTH 1
#define LED_SOUTH 3
#define LED_WEST 0
#define LED_EAST 2
#define LED_DIN D10
#define NUM_PIXELS 4

Adafruit_NeoPixel pixels(NUM_PIXELS, LED_DIN, NEO_GRB + NEO_KHZ800);

float rad_to_brightness(float x) {
  // x is in [-PI, +PI]
  int heaviside = x >= 0;
  return heaviside * (sin(2*x)/2 + 1);
}






void setup(void) {

  Serial.begin(115200);
  while (!Serial) delay(10);     // will pause Zero, Leonardo, etc until serial console opens



  // GYROSCOPE
  Serial.println("Initializing Gyroscope...");
  // Try to initialize!
  if (!bno08x.begin_I2C()) {
  //if (!bno08x.begin_UART(&Serial1)) {  // Requires a device with > 300 byte UART buffer!
  //if (!bno08x.begin_SPI(BNO08X_CS, BNO08X_INT)) {
    Serial.println("Failed to find BNO08x chip");
    while (1) { delay(10); }
  }
  Serial.println("BNO08x Found!");

  setReports(reportType, reportIntervalUs);

  Serial.println("Reading events");



  // ADDRESSABLE LEDS
  Serial.println("Initializing addressable LEDs...");
  pixels.begin();



  delay(100);
}

void quaternionToEuler(float qr, float qi, float qj, float qk, euler_t* ypr, bool degrees = false) {

    float sqr = sq(qr);
    float sqi = sq(qi);
    float sqj = sq(qj);
    float sqk = sq(qk);

    ypr->yaw = atan2(2.0 * (qi * qj + qk * qr), (sqi - sqj - sqk + sqr));
    ypr->pitch = asin(-2.0 * (qi * qk - qj * qr) / (sqi + sqj + sqk + sqr));
    ypr->roll = atan2(2.0 * (qj * qk + qi * qr), (-sqi - sqj + sqk + sqr));

    if (degrees) {
      ypr->yaw *= RAD_TO_DEG;
      ypr->pitch *= RAD_TO_DEG;
      ypr->roll *= RAD_TO_DEG;
    }
}

void quaternionToEulerRV(sh2_RotationVectorWAcc_t* rotational_vector, euler_t* ypr, bool degrees = false) {
    quaternionToEuler(rotational_vector->real, rotational_vector->i, rotational_vector->j, rotational_vector->k, ypr, degrees);
}

void quaternionToEulerGI(sh2_GyroIntegratedRV_t* rotational_vector, euler_t* ypr, bool degrees = false) {
    quaternionToEuler(rotational_vector->real, rotational_vector->i, rotational_vector->j, rotational_vector->k, ypr, degrees);
}

void loop() {

  // Gather data from gyroscope.
  if (bno08x.wasReset()) {
    Serial.print("sensor was reset ");
    setReports(reportType, reportIntervalUs);
  }
  
  if (bno08x.getSensorEvent(&sensorValue)) {
    // in this demo only one report type will be received depending on FAST_MODE define (above)
    switch (sensorValue.sensorId) {
      case SH2_ARVR_STABILIZED_RV:
        quaternionToEulerRV(&sensorValue.un.arvrStabilizedRV, &ypr, false);
      case SH2_GYRO_INTEGRATED_RV:
        // faster (more noise?)
        quaternionToEulerGI(&sensorValue.un.gyroIntegratedRV, &ypr, false);
        break;
    }

    //Serial.print(sensorValue.status);     Serial.print("\t");  // This is accuracy in the range of 0 to 3

    // Make each LED lid up when the gyro is tilted in the corresponding direction.
    pixels.clear();

    // ..North/South (pitch)
    pixels.setPixelColor(LED_NORTH, pixels.Color(0, rad_to_brightness(ypr.pitch)*255, 0));
    pixels.setPixelColor(LED_SOUTH, pixels.Color(0, rad_to_brightness(-ypr.pitch)*255, 0));

    // ..Eats/West (roll)
    pixels.setPixelColor(LED_EAST, pixels.Color(0, 0, rad_to_brightness(ypr.roll)*255));
    pixels.setPixelColor(LED_WEST, pixels.Color(0, 0, rad_to_brightness(-ypr.roll)*255));
    
    pixels.show();


    // DEBUG
    Serial.print("pitch:"); Serial.print(ypr.pitch); Serial.print(",");
    Serial.print("roll:"); Serial.print(ypr.roll); Serial.print(",");
    Serial.print("west_bright:"); Serial.print(rad_to_brightness(ypr.roll)); Serial.println();
  }

  delay(10);
}