#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <MPU6050.h>

MPU6050 mpu_1;
MPU6050 mpu_2;

#define FLEX_SENSOR_1 34
#define FLEX_SENSOR_2 35
#define FLEX_SENSOR_3 32

#define BUZZER_PIN 18
#define LED_PIN_1 5
#define LED_PIN_2 9
#define BUTTON_PIN 19 // Change to your button pin

#define THRESHOLD_FLEX 100 // Adjust the threshold based on your flex sensor characteristics

Adafruit_SSD1306 display(128, 64, &Wire, -1);

int postureMode = 0; // 0 - Normal, 1 - Sitting in chair, 2 - Sitting on the ground, 3 - One flex sensor enabled

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

  mpu_1.initialize();
  mpu_2.initialize();

  display.begin(0x3C, 4, 15);

  pinMode(FLEX_SENSOR_1, INPUT);
  pinMode(FLEX_SENSOR_2, INPUT);
  pinMode(FLEX_SENSOR_3, INPUT);
  pinMode(BUZZER_PIN, OUTPUT);
  pinMode(LED_PIN_1, OUTPUT);
  pinMode(LED_PIN_2, OUTPUT);
  pinMode(BUTTON_PIN, INPUT_PULLUP);

  // Initialize LEDC
  ledcSetup(0, 5000, 8); // LEDC channel, frequency, resolution
  ledcAttachPin(LED_PIN_1, 0); // Attach LED_PIN_1 to channel 0
  ledcAttachPin(LED_PIN_2, 1); // Attach LED_PIN_2 to channel 1

  display.clearDisplay();
  display.display();
}

void loop() {
  // Check button press to change posture mode
  if (digitalRead(BUTTON_PIN) == LOW) {
    postureMode = (postureMode + 1) % 4; // Cycle through the modes
    delay(500); // Debounce delay
  }

  // Read sensor values
  float flex1 = analogRead(FLEX_SENSOR_1);
  float flex2 = analogRead(FLEX_SENSOR_2);
  float flex3 = analogRead(FLEX_SENSOR_3);

  float angleX1 = mpu_1.getRotationX();
  float angleY1 = mpu_1.getRotationY();
  float angleZ1 = mpu_1.getRotationZ();

  float angleX2 = mpu_2.getRotationX();
  float angleY2 = mpu_2.getRotationY();
  float angleZ2 = mpu_2.getRotationZ();

  // Check sitting posture based on selected mode
  switch (postureMode) {
    case 0:
      // Normal posture detection
      if (flex1 > THRESHOLD_FLEX || flex2 > THRESHOLD_FLEX || flex3 > THRESHOLD_FLEX || angleY1 < 0 || angleY2 < 0) {
        // Bad posture detected
        digitalWrite(LED_PIN_1, HIGH);
        digitalWrite(LED_PIN_2, HIGH);
        tone(BUZZER_PIN, 1000, 500);
        display.clearDisplay();
        display.setTextSize(2);
        display.setTextColor(SSD1306_WHITE);
        display.setCursor(0, 0);
        display.println("Correct Posture!");
        display.display();
      } else {
        // Good posture
        digitalWrite(LED_PIN_1, LOW);
        digitalWrite(LED_PIN_2, LOW);
        noTone(BUZZER_PIN);
        display.clearDisplay();
        display.display();
      }
      break;

    case 1:
      if (flex1 > THRESHOLD_FLEX) {
        // Bad posture detected
        digitalWrite(LED_PIN_1, HIGH);
        digitalWrite(LED_PIN_2, HIGH);
        tone(BUZZER_PIN, 1000, 500);
        display.clearDisplay();
        display.setTextSize(2);
        display.setTextColor(SSD1306_WHITE);
        display.setCursor(0, 0);
        display.println("Correct Posture!");
        display.display();
      } else {
        // Good posture
        digitalWrite(LED_PIN_1, LOW);
        digitalWrite(LED_PIN_2, LOW);
        noTone(BUZZER_PIN);
        display.clearDisplay();
        display.display();
      }
      break;

    case 2:
      // One flex sensor enabled, others ignored
      if (flex1 > THRESHOLD_FLEX || flex2 > THRESHOLD_FLEX || flex3 > THRESHOLD_FLEX || angleY1 < 0 || angleY2 < 0) {
        // Bad posture detected
        digitalWrite(LED_PIN_1, HIGH);
        digitalWrite(LED_PIN_2, HIGH);
        tone(BUZZER_PIN, 1000, 500);
        display.clearDisplay();
        display.setTextSize(2);
        display.setTextColor(SSD1306_WHITE);
        display.setCursor(0, 0);
        display.println("Correct Posture!");
        display.display();
      } else {
        // Good posture
        digitalWrite(LED_PIN_1, LOW);
        digitalWrite(LED_PIN_2, LOW);
        noTone(BUZZER_PIN);
        display.clearDisplay();
        display.display();
      }
      break;
  }

  delay(1000); // Adjust the delay based on your preferences
}