/*
 * ESP32 Bluetooth BLE IMU Data Streamer
 * 
 * and an HMC5883L magnetometer. The data is then processed to compute orientation (roll, pitch, yaw).
 * 
 * Features:
 * 1. Calibrates built in offsets to get rest-inertial plane.
 * 2. Uses Madgwick filter to combine accelerometer, gyroscope, and magnetometer readings.
 * 3. Computes moving averages for accelerometer readings to smooth data.
 * 4. Streams computed orientation and position over Bluetooth LE (BLE).
 * 5. Uses an interrupt-driven approach to detect when new IMU data is available.
 * 
 * External Libraries:
 * - Wire.h:        I2C communication with sensors.
 * - MadgwickAHRS.h: Filter for sensor fusion.
 * - BLE related headers: For Bluetooth Low Energy communication.
 * 
 * Author: Noah Schliesman
 * Updated: 10/12/23
 */

#include <Wire.h>
#include <MadgwickAHRS.h>
#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>
#include <stdint.h>
#include <stdbool.h>
#include <Adafruit_SPIFlash.h>

#define WEIGHT_FACTOR 0.001
#define CALIBRATED_OFFSETS 100
#define MOVING_AVG_PARAMETER 5
#define SERVICE_UUID        "1f6449a7-7af7-462d-af40-ef09c0ff7628"
#define CHARACTERISTIC_UUID "cf92960e-478b-48c6-b8d1-d12f8e84337c"
#define MPU6050_ADDRESS 0x68 // MPU Address for Low Logic
#define HMC5883L_ADDRESS 0x1E // HMC Address
#define SCK 18
#define MISO 19
#define MOSI 23
#define CS 5

Madgwick filter;

BLECharacteristic *pCharacteristic;
BLEServer* pServer;

Adafruit_SPIFlash SD;

const int intPin = 38; // GPIO19 as INT
//volatile bool dataReady = false; // Initialize INT state to false
unsigned long previousTime = 0; // Moving Average Previous Time

//Initialize Data
float ax, ay, az, gx, gy, gz, roll, pitch, heading;
float vx, vy, vz, xx, xy, xz = 0;
float awx, awy, awz = 0; // Weight init
float filteredAx, filteredAy, filteredAz;

// For the X-axis
double accX_samples[MOVING_AVG_PARAMETER];
int accX_index = 0;
double accX_sum = 0.0;
bool accX_filled = false;

// For the Y-axis
double accY_samples[MOVING_AVG_PARAMETER];
int accY_index = 0;
double accY_sum = 0.0;
bool accY_filled = false;

// For the Z-axis
double accZ_samples[MOVING_AVG_PARAMETER];
int accZ_index = 0;
double accZ_sum = 0.0;
bool accZ_filled = false;
    
// Compute Moving Average for x
double computeMovingAverageX(double newSample) {
    accX_sum -= accX_samples[accX_index];
    accX_sum += newSample;
    accX_samples[accX_index] = newSample;
    accX_index = (accX_index + 1) % MOVING_AVG_PARAMETER;
    if (!accX_filled && accX_index == 0) accX_filled = true;
    return accX_filled ? (accX_sum / MOVING_AVG_PARAMETER) : (accX_sum / accX_index);
}

// Compute Moving Average for y
double computeMovingAverageY(double newSample) {
    accY_sum -= accY_samples[accY_index];
    accY_sum += newSample;
    accY_samples[accY_index] = newSample;
    accY_index = (accY_index + 1) % MOVING_AVG_PARAMETER;
    if (!accY_filled && accY_index == 0) accY_filled = true;
    return accY_filled ? (accY_sum / MOVING_AVG_PARAMETER) : (accY_sum / accY_index);
}

// Compute Moving Average for z
double computeMovingAverageZ(double newSample) {
    accZ_sum -= accZ_samples[accZ_index];
    accZ_sum += newSample;
    accZ_samples[accZ_index] = newSample;
    accZ_index = (accZ_index + 1) % MOVING_AVG_PARAMETER;
    if (!accZ_filled && accZ_index == 0) accZ_filled = true;
    return accZ_filled ? (accZ_sum / MOVING_AVG_PARAMETER) : (accZ_sum / accZ_index);
}

// BLE Bluetooth Callback Set
class MyCallbacks: public BLECharacteristicCallbacks {
    void onWrite(BLECharacteristic *pCharacteristic) {
      std::string value = pCharacteristic->getValue();
      if (value.length() > 0) {
        Serial.println("*********");
        Serial.print("New value: ");
        for (int i = 0; i < value.length(); i++)
        Serial.print(value[i]);
        Serial.println();
        Serial.println("*********");
      }
    }
};

// BLE Setup for Bluetooth Handling
void BLESetup() {
   BLEDevice::init("LiftSwitch");
   // Assign the created server to the global variable pServer
   pServer = BLEDevice::createServer();
   BLEService *pService = pServer->createService(SERVICE_UUID);
   pCharacteristic = pService->createCharacteristic(
                      CHARACTERISTIC_UUID,
                      BLECharacteristic::PROPERTY_READ |
                      BLECharacteristic::PROPERTY_WRITE |
                      BLECharacteristic::PROPERTY_NOTIFY
                    );
   pCharacteristic->setCallbacks(new MyCallbacks());
   pCharacteristic->setValue("Lift Switch Connecting...");
   pService->start();
   BLEAdvertising *pAdvertising = pServer->getAdvertising();
   pAdvertising->start();
}

// Write Function for I2C
void I2CwriteByte(uint8_t address, uint8_t subAddress, uint8_t usrData) {
  Wire.beginTransmission(address);
  Wire.write(subAddress);
  Wire.write(usrData);
  Wire.endTransmission();
}

// Convert Acceleration in terms of g
float convertRawAcceleration(int aRaw) {
  float a = (aRaw * 2.0) / 32768.0;
  return a;
}

// Convert Gyro
float convertRawGyro(int gRaw) {
  float g = (gRaw * 250.0) / 32768.0;
  return g;
}

void readSensorData(int16_t &accX, int16_t &accY, int16_t &accZ, int16_t &gyroX, int16_t &gyroY, int16_t &gyroZ, int16_t &temperature, int16_t &magX, int16_t &magY, int16_t &magZ) {
  // Read accelerometer and gyroscope data from MPU6050
  Wire.beginTransmission(MPU6050_ADDRESS);
  Wire.write(0x3B); // Start with the first register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_ADDRESS, 14, true); // Read 14 registers

  accX = Wire.read() << 8 | Wire.read(); // ACC_XOUT_H and ACC_XOUT_L
  accY = Wire.read() << 8 | Wire.read(); // ACC_YOUT_H and ACC_YOUT_L
  accZ = Wire.read() << 8 | Wire.read(); // ACC_ZOUT_H and ACC_ZOUT_L
  temperature = Wire.read() << 8 | Wire.read(); // TEMP_OUT_H and TEMP_OUT_L
  gyroX = Wire.read() << 8 | Wire.read(); // GYRO_XOUT_H and GYRO_XOUT_L
  gyroY = Wire.read() << 8 | Wire.read(); // GYRO_YOUT_H and GYRO_YOUT_L
  gyroZ = Wire.read() << 8 | Wire.read(); // GYRO_ZOUT_H and GYRO_ZOUT_L

  // Read magnetometer data from HMC5883L
  Wire.beginTransmission(HMC5883L_ADDRESS);
  Wire.write(0x03); // Start with the first register 0x03 (X-axis MSB)
  Wire.endTransmission(false);
  Wire.requestFrom(HMC5883L_ADDRESS, 6, true); // Read 6 registers
  
  if (Wire.available() >= 6) {
    magX = Wire.read() << 8 | Wire.read(); // X-axis data
    magZ = Wire.read() << 8 | Wire.read(); // Z-axis data (HMC5883L is often mounted sideways)
    magY = Wire.read() << 8 | Wire.read(); // Y-axis data (HMC5883L is often mounted sideways)
  } else {
    // Handle the case when magnetometer data is not available
    magX = 0.0;
    magY = 0.0;
    magZ = 0.0;
  }
}

void setup() {
  // Serial Initialization
  Serial.begin(115200); 
  Wire.begin(); 

  BLESetup();
  pServer->getAdvertising()->start();
    
  pinMode(intPin, INPUT);
//  attachInterrupt(digitalPinToInterrupt(intPin), handleInterrupt, RISING);

  // MPU-6050 Wake-up
  I2CwriteByte(MPU6050_ADDRESS, 0x6B, 0x00); // Wake Up MPU-6050

  //FIFO Setup 
  I2CwriteByte(MPU6050_ADDRESS, 0x23, 0x00); // Clear FIFO_EN register
  I2CwriteByte(MPU6050_ADDRESS, 0x6A, 0x60); // Enable I2C master mode and reset FIFO
  I2CwriteByte(MPU6050_ADDRESS, 0x23, 0xF8); // Enable ACC and GYRO for FIFO

  //MPU Setup
  I2CwriteByte(MPU6050_ADDRESS, 0x38, 0x01); // Enable INT
  I2CwriteByte(MPU6050_ADDRESS, 0x37, 0x00); // Configure INT active high, push-pull

  // HMC5883L Setup
  I2CwriteByte(MPU6050_ADDRESS, 0x24, 0x06); // Configure MPU CLK to 100 kHz
  I2CwriteByte(MPU6050_ADDRESS, 0x25, HMC5883L_ADDRESS << 1 | 0x80); // Configure HMC to read
  I2CwriteByte(MPU6050_ADDRESS, 0x26, 0x03); // Tell HMC where to start read
  I2CwriteByte(MPU6050_ADDRESS, 0x27, 0x06); // Set up 6 byte read (2 bytes per axis)
  
  previousTime = millis(); // For differential time
  Serial.println("MPU-6050 setup complete!"); // Serial Feedback  

}

void loop() {         
    int16_t accX, accY, accZ, gyroX, gyroY, gyroZ, temperature, magX, magY, magZ;
    char data[2048];
    // Read raw data from IMU
    readSensorData(accX, accY, accZ, gyroX, gyroY, gyroZ, temperature, magX, magY, magZ);
    
    unsigned long currentTime = millis();
    double dt = (double)(currentTime - previousTime) / 1000.0; // Convert to seconds
    previousTime = currentTime;
    
    // Convert from raw data to gravity and degrees/second units offsets calibrated by hand
    ax = convertRawAcceleration(accX);
    ay = convertRawAcceleration(accY);
   // az = convertRawAcceleration(accZ); 

    filteredAx = filteredAx * (1.0 - WEIGHT_FACTOR) + ax * WEIGHT_FACTOR;
    filteredAy = filteredAy * (1.0 - WEIGHT_FACTOR) + ay * WEIGHT_FACTOR;

    // Use the filtered acceleration data for further processing
    ax = filteredAx;
    ay = filteredAy;

    // Compute moving average to smooth out data
    //ax = computeMovingAverageX(ax);
    //ay = computeMovingAverageY(ay);
    //az = computeMovingAverageZ(az);

    if ((ax < 0.05) && (ax > -0.05)) {
      ax = 0;
    }
    if ((ay < 0.05) && (ay > -0.05)) {
      ay = 0;
    }
    //if ((az < 0.05) && (az > -0.05)) {
     // ay = 0;
      //}

    String r1w_data = "accX: " + String(ax,2); 
    //Serial.println(r1w_data);
    String r2w_data = "accY: " + String(ay,2);
    //Serial.println(r2w_data);
    //String r3w_data = "accZ: " + String(az,2);
    //Serial.println(r3w_data);

    // Convert Raw Gyro Data
    gx = convertRawGyro(gyroX);
    gy = convertRawGyro(gyroY);
   // gz = convertRawGyro(gyroZ);

    // Velocity Integration
    vx += ax * dt;
    vy += ay * dt;
    vz += az * dt;

    vx = computeMovingAverageX(vx);
    vy = computeMovingAverageY(vy);
    //vz = computeMovingAverageZ(vz);

    //if ((vx < 0.03) && (vx > -0.03)) {
     // vx = 0;
    //}
    //if ((vy < 0.03) && (vy > -0.03)) {
      //vy = 0;
    //}
    //if ((vz < 0.02) && (vz > -0.02))  {
    //  vz = 0;
    //}
  
    String vr1w_data = "velX: " + String(vx,2);
    //Serial.println(vr1w_data);
    String vr2w_data = "velY: " + String(vy,2);
    //Serial.println(vr2w_data);
    //String vr3w_data = "velZ: " + String(az,2);
    //Serial.println(vr3w_data);

    // Position Integration
    xx += vx * dt;
    xy += vy * dt;
    xz += vz * dt;

    String xr1w_data = "posX: " + String(xx,2);
    //Serial.println(xr1w_data);
    String xr2w_data = "posY: " + String(xy,2);
    //Serial.println(xr2w_data);
    //String xr3w_data = "posZ: " + String(xz,2);
    //Serial.println(xr3w_data);
  
    // Update the filter, which computes orientation
    filter.update(gx, gy, gz, accX, accY, accZ, magX, magY, magZ);

    // Print the heading, pitch and roll
    roll = filter.getRoll();
    pitch = filter.getPitch();
    heading = filter.getYaw();

    sprintf(data, "gx: %.2f, gy: %.2f, gz: %.2f, ax: %.2f, ay: %.2f, az: %.2f, magX: %.2f, magY: %.2f, magZ: %.2f, temperature: %.2f, roll: %.2f, pitch: %.2f, heading: %.2f", gx, gy, gz, ax, ay, az, magX, magY, magZ, temperature, roll, pitch, heading);
    Serial.println(data);
    pCharacteristic->setValue(data);
    pCharacteristic->notify();     
}