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


SensorManager::SensorManager() {}

void SensorManager::sensorSetup() {
  Wire.begin();
  //wait for the sensor to start before continue
  if (myIMU.begin() == false) {
    delay(1000);
  }
  //start sensorfunction and start autocalibration
  //once calibration is enabled it attempts to every 5 min
  myIMU.enableGyroIntegratedRotationVector(100);  //send data every 100ms
  // myIMU.enableAccelerometer(100);                 //Send data update every 100ms
  // myIMU.enableStepCounter(500);  //Send data update every 500ms
}
//get sensordata
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;
  }
}
//calculate Quintillions to Euler angles from -1π to +1π
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;
}
SensorManager::acceleration SensorManager::getAcelleration() {
  float x = myIMU.getAccelX();
  float y = myIMU.getAccelY();
  float z = myIMU.getAccelZ();
  acceleration Acceleration = { x, y, z };

  return Acceleration;
}

bool SensorManager::sensorTap() {
  int taps = 0;
  if (myIMU.dataAvailable() == true) {
    int taps = myIMU.getStepCount();
  }
  if (taps) {
    return true;
  }
  else {
    return false;
  }
}