#include "SensorManager.h"
#include <Wire.h>

SensorManager::SensorManager() {}

void SensorManager::sensorSetup() {
  Serial.println();
  Serial.println("BNO080 Read Example");

  delay(1000);  // Wait for BNO to boot

  Wire.begin();

  if (myIMU.begin() == false) {
    delay(1000);
    Serial.println(".");
  }

  Wire.setClock(400000);  //Increase I2C data rate to 400kHz
  myIMU.calibrateAll();   //Turn on cal for Accel, Gyro, and Mag
  myIMU.enableGyroIntegratedRotationVector(100);
  myIMU.enableMagnetometer(100);     //Send data update every 100ms
  myIMU.saveCalibration();           //Saves the current dynamic calibration data (DCD) to memory
  myIMU.requestCalibrationStatus();  //Sends command to get the latest calibration status

  if (myIMU.calibrationComplete() == true) {
    Serial.println("Calibration data successfully stored");
  }

  Serial.println(F("magnetometer rotation enabled"));
}

SensorManager::Rotation SensorManager::readLoop() {
  if (myIMU.dataAvailable() == true) {
    float i = myIMU.getQuatI();
    float j = myIMU.getQuatJ();
    float k = myIMU.getQuatK();
    float w = myIMU.getQuatReal();

    Rotation rotation = { i, j, k, w };
    return rotation;
  }
  else {
    float i = myIMU.getQuatI();
    float j = myIMU.getQuatJ();
    float k = myIMU.getQuatK();
    float w = myIMU.getQuatReal();

    Rotation rotation = { i, j, k, w };
    return rotation;
  }
}