#include "headerFile.h"

// SensorManager::Rotation offset;

void setup() {
  Serial.begin(9600);
  Serial.println("startup");
  
  connectivity.connectWiFi(ssid, pass);
  sensorManager.sensorSetup();

	//ws server address, port and URL
	webSocket.begin("145.28.160.108", 8001, "");
	// try every 500 again if connection has failed
	webSocket.setReconnectInterval(500);
}

void loop() {
  SensorManager::eulerAngles eulerRotation = sensorManager.getEulerAngles();

  // Subtract offset
  // rotation.i -= offset.i;
  // rotation.j -= offset.j;
  // rotation.k -= offset.k;
  // rotation.w -= offset.w;

  // Convert quaternion to Euler angles in radians


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

  Serial.print(eulerRotation.roll);
  Serial.print(" ");
  Serial.print(eulerRotation.pitch);
  Serial.print(" ");
  Serial.print(eulerRotation.yaw);
  sendData(eulerRotation.roll, eulerRotation.pitch, eulerRotation.yaw);

  Serial.println();
  webSocket.loop();
}
//   if (Serial.available()) {
//     String command = Serial.readStringUntil('\n');
//     command.trim(); // remove any trailing whitespace
//     if (command == "setZeroPoint") {
//       setZeroPoint();
//     }
//   }
// }
// void setZeroPoint() {
//   offset = sensorManager.readLoop();
// }

void sendData(float roll, float pitch, float yaw){
  String message = "{\"Sensor\": 1, \"roll\":\"" + String(roll) + "\",\"pitch\":\"" + String(pitch) + "\",\"yaw\":\"" + String(yaw) + "\"}";
  webSocket.sendTXT(message);
}