#include "headerFile.h"

SensorManager sensorManager;
ESP8266WiFiMulti wifi;

void setup() {
  Serial.begin(9600);
  Serial.println("startup");
    delay(5000);

  //Wifi
  WiFi.mode(WIFI_STA);
  wifi.addAP(ssid, pass);
  while (WiFi.status() != WL_CONNECTED) {
    Serial.println("connecting to wifi");
    delay(1000);
  }
  Serial.println(WiFi.localIP());

  Wire.setClockStretchLimit(150000L);  // Default stretch limit 150mS
  sensorManager.sensorSetup();
}

void loop() {
  SensorManager::Rotation rotation = sensorManager.readLoop();

  // Convert quaternion to Euler angles in radians
  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));

  // Convert to degrees
  float rollDegrees = roll * 180.0f / PI;
  float pitchDegrees = pitch * 180.0f / PI;
  float yawDegrees = yaw * 180.0f / PI;

  Serial.print(roll);
  Serial.print(" ");
  Serial.print(pitch);
  Serial.print(" ");
  Serial.print(yaw);

  Serial.println();
}