#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>
#include <Wire.h>
#include <math.h>

#define SERVICE_UUID        "d86aecf2-d87d-489f-b664-b02de82b2fc0"
#define CHARACTERISTIC_UUID "d86aecf2-d87d-489f-b664-b02de82b2fc0"

long accelX, accelY, accelZ;
float gForceX, gForceY, gForceZ;
float accelMag;

long gyroX, gyroY, gyroZ;
float rotX, rotY, rotZ;
float gyroMag;

int16_t tempRaw;
float temperature;

float pitch, roll;

BLECharacteristic *pCharacteristic = NULL;

void setup() {
  Serial.begin(115200);

  // BLE setup
  BLEDevice::init("MPU6050_BLE");
  BLEServer *pServer = BLEDevice::createServer();
  BLEService *pService = pServer->createService(SERVICE_UUID);

  pCharacteristic = pService->createCharacteristic(
    CHARACTERISTIC_UUID,
    BLECharacteristic::PROPERTY_READ |
    BLECharacteristic::PROPERTY_WRITE |
    BLECharacteristic::PROPERTY_NOTIFY
  );

  pService->start();
  BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
  pAdvertising->addServiceUUID(SERVICE_UUID);
  pAdvertising->setScanResponse(true);
  pAdvertising->setMinPreferred(0x06);
  pAdvertising->setMinPreferred(0x12);
  BLEDevice::startAdvertising();

  Serial.println("BLE advertising started");

  Wire.begin();
  setupMPU();
}

void loop() {
  recordAccelRegisters();
  recordGyroRegisters();
  recordTemp();
  computeOrientation();
  computeMagnitudes();

  String data = formatData();
  Serial.println(data);

  pCharacteristic->setValue(data.c_str());
  pCharacteristic->notify();

  delay(200); // Update rate
}

void setupMPU() {
  Wire.beginTransmission(0x68);
  Wire.write(0x6B); // Power management
  Wire.write(0);
  Wire.endTransmission();

  Wire.beginTransmission(0x68);
  Wire.write(0x1B); // Gyro config
  Wire.write(0);
  Wire.endTransmission();

  Wire.beginTransmission(0x68);
  Wire.write(0x1C); // Accel config
  Wire.write(0);
  Wire.endTransmission();
}

void recordAccelRegisters() {
  Wire.beginTransmission(0x68);
  Wire.write(0x3B);
  Wire.endTransmission();
  Wire.requestFrom(0x68, 6);
  while (Wire.available() < 6);
  accelX = Wire.read() << 8 | Wire.read();
  accelY = Wire.read() << 8 | Wire.read();
  accelZ = Wire.read() << 8 | Wire.read();
  processAccelData();
}

void processAccelData() {
  gForceX = accelX / 16384.0;
  gForceY = accelY / 16384.0;
  gForceZ = accelZ / 16384.0;
}

void recordGyroRegisters() {
  Wire.beginTransmission(0x68);
  Wire.write(0x43);
  Wire.endTransmission();
  Wire.requestFrom(0x68, 6);
  while (Wire.available() < 6);
  gyroX = Wire.read() << 8 | Wire.read();
  gyroY = Wire.read() << 8 | Wire.read();
  gyroZ = Wire.read() << 8 | Wire.read();
  processGyroData();
}

void processGyroData() {
  rotX = gyroX / 131.0;
  rotY = gyroY / 131.0;
  rotZ = gyroZ / 131.0;
}

void recordTemp() {
  Wire.beginTransmission(0x68);
  Wire.write(0x41);
  Wire.endTransmission();
  Wire.requestFrom(0x68, 2);
  while (Wire.available() < 2);
  tempRaw = Wire.read() << 8 | Wire.read();
  temperature = (tempRaw / 340.0) + 36.53;
}

void computeOrientation() {
  pitch = atan2(gForceY, gForceZ) * 180 / PI;
  roll = atan2(-gForceX, sqrt(gForceY * gForceY + gForceZ * gForceZ)) * 180 / PI;
}

void computeMagnitudes() {
  accelMag = sqrt(gForceX * gForceX + gForceY * gForceY + gForceZ * gForceZ);
  gyroMag = sqrt(rotX * rotX + rotY * rotY + rotZ * rotZ);
}

String formatData() {
  return String(rotX) + "," + String(rotY) + "," + String(rotZ) + "," +
         String(gForceX) + "," + String(gForceY) + "," + String(gForceZ) + "," +
         String(gyroMag) + "," + String(accelMag) + "," +
         String(pitch) + "," + String(roll) + "," +
         String(temperature);
}