#include <Wire.h>

#define MPU_ADDR 0x68
#define LED_PIN 2
#define BUTTON_PIN 4

int16_t AcX, AcY, AcZ;

float referencePitch = 0;
bool calibrated = false;

unsigned long slouchStartTime = 0;
bool slouchActive = false;

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

  pinMode(LED_PIN, OUTPUT);
  pinMode(BUTTON_PIN, INPUT_PULLUP);  // cleaner than external

  // Wake MPU6050
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);

  Serial.println("SpineTrack Initialized");
}

void loop() {

  // -------- Read MPU --------
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_ADDR, 6, true);

  AcX = Wire.read() << 8 | Wire.read();
  AcY = Wire.read() << 8 | Wire.read();
  AcZ = Wire.read() << 8 | Wire.read();

  float Ax = AcX / 16384.0;
  float Ay = AcY / 16384.0;
  float Az = AcZ / 16384.0;

  float pitch = atan2(Ax, sqrt(Ay * Ay + Az * Az)) * 180.0 / PI;

  Serial.print("Pitch: ");
  Serial.println(pitch);

  // -------- Calibration --------
  if (digitalRead(BUTTON_PIN) == LOW) {
    referencePitch = pitch;
    calibrated = true;
    Serial.println("Posture Calibrated!");
    delay(500);
  }

  // -------- Slouch Detection --------
  if (calibrated) {

    float deviation = pitch - referencePitch;

    Serial.print("Deviation: ");
    Serial.println(deviation);

    if (deviation > 15) {   // reduced threshold

      if (!slouchActive) {
        slouchStartTime = millis();
        slouchActive = true;
      }

      if (millis() - slouchStartTime > 3000) {
        Serial.println("⚠ Sustained Forward Slouch!");
        digitalWrite(LED_PIN, HIGH);
        delay(100);
        digitalWrite(LED_PIN, LOW);
        delay(500);
      }

    } else {
      slouchActive = false;
      digitalWrite(LED_PIN, LOW);
      Serial.println("Good Posture");
    }
  }

  Serial.println("-------------------");
  delay(200);
}