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


SensorManager::SensorManager() {}

void SensorManager::sensorSetup() {
  Wire.setClockStretchLimit(150000L);  // Default stretch limit 150mS
  Wire.begin();
  //wait for the sensor to start before continue
  if (myIMU.begin() == false) {
    delay(1000);
    Serial.println(".");
  }
  //start sensorfunction and start autocalibration
  //once calibration is enabled it attempts to every 5 min

  Wire.setClock(400000);                          //Increase I2C data rate to 400kHz
  myIMU.calibrateAll();                           //Turn on cal for Accel, Gyro, and Mag
  myIMU.enableGyroIntegratedRotationVector(100);  //send data every 100ms
  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::RotationQuintillions SensorManager::getQuintillions() {
  if (myIMU.dataAvailable() == true) {
    float i = myIMU.getQuatI();
    float j = myIMU.getQuatJ();
    float k = myIMU.getQuatK();
    float w = myIMU.getQuatReal();

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

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

SensorManager::eulerAngles SensorManager::getEulerAngles() {
  SensorManager::RotationQuintillions rotation = getQuintillions();
  float roll = atan2(2.0f * (rotation.w * rotation.i + rotation.j * rotation.k), 1.0f - 2.0f * (rotation.i * rotation.i + rotation.j * rotation.j));
  float pitch = asin(2.0f * (rotation.w * rotation.j - rotation.k * rotation.i));
  float yaw = atan2(2.0f * (rotation.w * rotation.k + rotation.i * rotation.j), 1.0f - 2.0f * (rotation.j * rotation.j + rotation.k * rotation.k));
  eulerAngles EulerAngles = { roll, pitch, yaw };
  return EulerAngles;
}