#include <Wire.h>
#include <MPU6050.h>
#include <Adafruit_Sensor.h>
#include <LiquidCrystal.h>

MPU6050 mpu;
LiquidCrystal lcd(12, 11, 5, 4, 3, 2);  // RS, Enable, D4, D5, D6, D7

const int ledPin = 13;
const int buzzerPin = 6;
const int buttonPin = 7;

bool buzzerActive = false;
bool ledActive = false;
bool buttonState = false;
bool lastButtonState = false;
bool buttonReleased = true;
bool calibrationRequested = false;
bool calibrating = false;
bool calibrationComplete = false;
unsigned long calibrateStartTime = 0;
const unsigned long calibrationDuration = 3000;
int calibrationHeight = 0;
bool acknowledge200cm = false;
bool buzzerLedActive = false;
unsigned long buzzerLedStartTime = 0;
const unsigned long buzzerLedDuration = 2000;

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

  lcd.begin(16, 2); // Initialize the LCD with 16 columns and 2 rows
  lcd.clear();

  Wire.begin();
  
  mpu.initialize(); // Correctly initialize the MPU6050 sensor

  pinMode(ledPin, OUTPUT);
  pinMode(buzzerPin, OUTPUT);
  pinMode(buttonPin, INPUT_PULLUP);
}

void loop() {
  int distance = measureDistance();

  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  lcd.setCursor(0, 0);
  lcd.print("Height: ");
  lcd.print(distance);
  lcd.print(" cm");

  if (calibrationComplete) {
    if (distance > 200 && !acknowledge200cm) {
      if (!buzzerLedActive) {
        activateBuzzerAndLED();
        buzzerLedStartTime = millis();
        buzzerLedActive = true;
        lcd.setCursor(0, 1);
        lcd.print("Press button x 2    ");
      } else if (buzzerLedActive && millis() - buzzerLedStartTime >= buzzerLedDuration) {
        deactivateBuzzerAndLED();
        buzzerLedActive = false;
      }
    } else if (buzzerActive && ledActive) {
      if (distance > 200) {
        lcd.setCursor(0, 1);
        lcd.print("Press button x 2    ");
      } else {
        lcd.setCursor(0, 1);
        lcd.print("                    ");
      }
      deactivateBuzzerAndLED();
      acknowledge200cm = false;
    } else if (distance < 200 && !buzzerLedActive) {
      activateBuzzerAndLED();
      buzzerLedStartTime = millis();
      buzzerLedActive = true;
      lcd.setCursor(0, 1);
      lcd.print("Press button x 2    ");
    } else if (distance > 200 && distance < 300 && buzzerLedActive && millis() - buzzerLedStartTime >= buzzerLedDuration) {
      deactivateBuzzerAndLED();
      buzzerLedActive = false;
    }
  } else if (buzzerActive && ledActive) {
    lcd.setCursor(0, 1);
    lcd.print("Press button x 2    ");
  } else if (calibrating) {
    if (millis() - calibrateStartTime >= calibrationDuration) {
      calibrating = false;
      calibrationComplete = true;
      calibrationHeight = distance;
      lcd.setCursor(0, 1);
      lcd.print("Calibrating Complete");
      delay(2000);
      lcd.clear();
    } else {
      lcd.setCursor(0, 1);
      lcd.print("Calibrating...      ");
    }
  } else if (calibrationRequested) {
    calibrating = true;
    calibrationRequested = false;
    calibrateStartTime = millis();
    lcd.setCursor(0, 1);
    lcd.print("Calibrating:        ");
  } else {
    lcd.setCursor(0, 1);
    lcd.print("Calibrate? Press    ");
  }

  if (buttonReleased && buttonState) {
    if (!calibrationComplete && !calibrating && !calibrationRequested) {
      calibrationRequested = true;
      lcd.clear();
      lcd.print("Calibrating:        ");
    } else if (calibrationComplete) {
      if (distance > 200 && !acknowledge200cm) {
        acknowledge200cm = true;
      } else if (acknowledge200cm) {
        if (distance < 200) {
          acknowledge200cm = false;
        } else if (distance > 200) {
          if (!buzzerLedActive) {
            activateBuzzerAndLED();
            buzzerLedStartTime = millis();
            buzzerLedActive = true;
            lcd.setCursor(0, 1);
            lcd.print("Press button x 2    ");
          } else if (buzzerLedActive && millis() - buzzerLedStartTime >= buzzerLedDuration) {
            deactivateBuzzerAndLED();
            buzzerLedActive = false;
          }
        }
      }
    }
    buttonReleased = false;
  }

  if (!buttonState) {
    buttonReleased = true;
  }

  int reading = digitalRead(buttonPin);

  if (reading != lastButtonState) {
    if (reading == LOW) {
      buttonState = !buttonState;
    }
    delay(50);
  }

  lastButtonState = reading;

  delay(1000);
}

int measureDistance() {
  int16_t ax, ay, az;
  mpu.getAcceleration(&ax, &ay, &az);
  double total_acceleration = sqrt(ax * ax + ay * ay + az * az);
  int distance = total_acceleration / 9.81 * 100;

  if (calibrating && distance < calibrationHeight) {
    distance = calibrationHeight - distance;
  } else if (calibrationComplete) {
    distance = max(distance - calibrationHeight + 2, 2);
  }

  return distance;
}

void activateBuzzerAndLED() {
  digitalWrite(ledPin, HIGH);
  tone(buzzerPin, 1000);
  buzzerActive = true;
  ledActive = true;
}

void deactivateBuzzerAndLED() {
  digitalWrite(ledPin, LOW);
  noTone(buzzerPin);
  buzzerActive = false;
  ledActive = false;
}