#include <LiquidCrystal_I2C.h>
//#include <Adafruit_MPU6050.h>
//#include <I2Cdev.h>
#include <MPU6050.h>
#include <Wire.h>

//Declare display adress might need to be 0x3F
LiquidCrystal_I2C lcd(0x27, 16, 2);

int Index = 0;
int16_t ax, ay, az;
int16_t gx, gy, gz;
long timer = 0;
float dt;
float alpha = 0.98;
float roll = 0, pitch = 0;
float gyroAngleX = 0, gyroAngleY = 0;
float accAngleX = 0, accAngleY = 0;

const int numSamples = 10;
float rollSamples1[numSamples], pitchSamples1[numSamples];
float rollSamples2[numSamples], pitchSamples2[numSamples];
int currentSampleIndex = 0;

// Variables to store the sum of samples for calculating averages
float sumRoll1 = 0, sumPitch1 = 0;
float sumRoll2 = 0, sumPitch2 = 0;

float averageRoll1 = 0, averagePitch1 = 0;
float averageRoll2 = 0, averagePitch2 = 0;


// Declare two MPU6050 objects with different addresses
MPU6050 sensor1(0x68);
MPU6050 sensor2(0x69);


void setup() {
  // put your setup code here, to run once:

  Wire.begin();
  Serial.begin(115200);

  sensor1.initialize();
  sensor2.initialize();

  if (!sensor1.testConnection()) {
    Serial.println("MPU6050 connection failed (sensor 1).");
  } else {
    Serial.println("MPU6050 connection successful (sensor 1).");
  }

  if (!sensor2.testConnection()) {
    Serial.println("MPU6050 connection failed (sensor 2).");
  } else {
    Serial.println("MPU6050 connection successful (sensor 2).");
  }
  // Initialize arrays to zero
  for (int i = 0; i < numSamples; i++) {
    rollSamples1[i] = pitchSamples1[i] = 0;
    rollSamples2[i] = pitchSamples2[i] = 0;
  }

//basic text on display
 lcd.init();
 lcd.backlight();
 lcd.setCursor(4, 0);
 lcd.print("ERP GAUGE");
 delay(2000);
 lcd.clear();
 lcd.setCursor(0, 0);
 lcd.print("CamL:   CamR:   ");
 lcd.setCursor(0, 1);
 lcd.print("CasL:   CasR:   ");
}

void loop() {
  // put your main code here, to run repeatedly:
  //delay(10); // this speeds up the simulation

  delay(10);
  // Read raw values from both sensors
  int16_t ax1, ay1, az1, gx1, gy1, gz1;
  int16_t ax2, ay2, az2, gx2, gy2, gz2;
  
  sensor1.getMotion6(&ax1, &ay1, &az1, &gx1, &gy1, &gz1);
  sensor2.getMotion6(&ax2, &ay2, &az2, &gx2, &gy2, &gz2);

  // Convert accelerometer values to g's (assuming ±2g range)
  float ax1_g = ax1 / 16384.0;
  float ay1_g = ay1 / 16384.0;
  float az1_g = az1 / 16384.0;
  
  float ax2_g = ax2 / 16384.0;
  float ay2_g = ay2 / 16384.0;
  float az2_g = az2 / 16384.0;

  // Calculate the angles for sensor 1
  float pitch1 = atan2(-ax1_g, sqrt(ay1_g * ay1_g + az1_g * az1_g)) * 180.0 / PI;
  float roll1 = atan2(ay1_g, sqrt(ax1_g * ax1_g + az1_g * az1_g)) * 180.0 / PI;

  // Calculate the angles for sensor 2
  float pitch2 = atan2(-ax2_g, sqrt(ay2_g * ay2_g + az2_g * az2_g)) * 180.0 / PI;
  float roll2 = atan2(ay2_g, sqrt(ax2_g * ax2_g + az2_g * az2_g)) * 180.0 / PI;

// Update rolling averages
  // Sensor 1
  sumRoll1 -= rollSamples1[currentSampleIndex];
  sumPitch1 -= pitchSamples1[currentSampleIndex];
  rollSamples1[currentSampleIndex] = roll1;
  pitchSamples1[currentSampleIndex] = pitch1;
  sumRoll1 += roll1;
  sumPitch1 += pitch1;
  
  // Sensor 2
  sumRoll2 -= rollSamples2[currentSampleIndex];
  sumPitch2 -= pitchSamples2[currentSampleIndex];
  rollSamples2[currentSampleIndex] = roll2;
  pitchSamples2[currentSampleIndex] = pitch2;
  sumRoll2 += roll2;
  sumPitch2 += pitch2;
  
  // Calculate the average of the samples
  averageRoll1 = sumRoll1 / numSamples;
  averagePitch1 = sumPitch1 / numSamples;
  averageRoll2 = sumRoll2 / numSamples;
  averagePitch2 = sumPitch2 / numSamples;

 // Move to the next sample index
  currentSampleIndex = (currentSampleIndex + 1) % numSamples;

 // Print results
  Serial.print("Sensor 1 - Avg Roll: ");
  Serial.print(averageRoll1);
  Serial.print(", Avg Pitch: ");
  Serial.println(averagePitch1);
  
  Serial.print("Sensor 2 - Avg Roll: ");
  Serial.print(averageRoll2);
  Serial.print(", Avg Pitch: ");
  Serial.println(averagePitch2);

  delay(100); // Delay for stability

//print vlues onto display
 lcd.setCursor(5, 0);
 lcd.print(averageRoll1, 1);
 lcd.setCursor(13, 0);
 lcd.print(averageRoll2, 1);
 lcd.setCursor(5, 1);
 lcd.print(averagePitch1, 1);
 lcd.setCursor(13, 1);
 lcd.print(averagePitch2, 1);

}