#include <Wire.h>
#include <MPU6050.h>
#include <LiquidCrystal_I2C.h>

MPU6050 mpu1, mpu2;
LiquidCrystal_I2C lcd(0x27, 16, 2); // Set the LCD I2C address

// Threshold angles (in degrees)
const float MIN_THRESHOLD = 30.0; // Minimum angle for a proper curl
const float MAX_THRESHOLD = 135.0; // Maximum angle for a proper curl

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

    // Initialize I2C communication on ESP32-S3
    Wire.begin(8, 10);  // SDA = GPIO 8, SCL = GPIO 10

    // Initialize the two MPU6050 accelerometers
    mpu1.initialize();
    mpu2.initialize();

    if (!mpu1.testConnection() || !mpu2.testConnection()) {
        Serial.println("MPU6050 connection failed");
        while (1);
    }

    // Initialize LCD
    lcd.begin(16, 2);
    lcd.backlight();
    lcd.setCursor(0, 0);
    lcd.print("Posture Monitor");
    delay(2000);
    lcd.clear();
}

float calculateAngle(int16_t ax, int16_t ay) {
    // Calculate the angle in degrees based on the accelerometer data
    return atan2(ay, ax) * (180.0 / PI);
}

void loop() {
    // Get raw accelerometer readings from both MPU6050s
    int16_t ax1, ay1, az1; // Upper arm
    int16_t ax2, ay2, az2; // Forearm

    mpu1.getAcceleration(&ax1, &ay1, &az1);
    mpu2.getAcceleration(&ax2, &ay2, &az2);

    // Calculate angles for both sensors
    float angle1 = calculateAngle(ax1, ay1); // Angle for upper arm
    float angle2 = calculateAngle(ax2, ay2); // Angle for forearm

    // Calculate the angle difference
    float angleDifference = abs(angle1 - angle2);

    // Debugging output
    Serial.print("Angle 1: ");
    Serial.print(angle1);
    Serial.print("°, Angle 2: ");
    Serial.print(angle2);
    Serial.print("°, Difference: ");
    Serial.print(angleDifference);
    Serial.println("°");

    // Output feedback on the LCD
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Angle1: ");
    lcd.print(angle1);
    lcd.setCursor(0, 1);
    lcd.print("Angle2: ");
    lcd.print(angle2);
    delay(1000);

    // Check posture based on angle difference
    if (angleDifference < MIN_THRESHOLD) {
        lcd.clear();
        lcd.print("Posture: Under");
    } else if (angleDifference > MAX_THRESHOLD) {
        lcd.clear();
        lcd.print("Posture: Over");
    } else {
        lcd.clear();
        lcd.print("Posture: Good");
    }

    delay(1000); // Update every second
}