#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <MPU6050_light.h> 
#include <Ultrasonic.h>

// Pins
const int trigPin = 7; // Ultrasonic sensor trigger (for distance)
const int echoPin = 6; // Ultrasonic sensor echo (for distance)
const int joystickXPin = A0;   // Joystick HORZ for switching modes, left to reset angle
const int joystickYPin = A1;   // Joystick VERT for toggling units (CM/M) and down for laser control
const int laserPin = 10;       // Laser diode control pin
const int speakerPin = 8;      // Speaker pin (buzzer)
int led = 9;                   // LED pin

// Create objects
LiquidCrystal_I2C lcd(0x27, 16, 2); // I2C LCD at address 0x3F
Ultrasonic ultrasonic(trigPin, echoPin); // Ultrasonic sensor for height
MPU6050 mpu(Wire);  // MPU6050 sensor for angle measurement

// Variables
int currentMode = 0; // 0: Height, 1: Distance, 2: Angle
bool isMetric = true; // true: CM, false: M
bool laserOn = false; // Laser diode state
float baseAngle = 0.0; // Base angle for angle measurement reset
int distance; // For height measurement
int height; // For height measurement

unsigned long lastUpdateTime = 0; // Timer for updating measurements

void setup() {
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(laserPin, OUTPUT);
  pinMode(speakerPin, OUTPUT);
  pinMode(led, OUTPUT);

  lcd.init(); // Initialize the LCD
  lcd.backlight(); // Turn on the LCD backlight

  Wire.begin(); // Initialize I2C communication
  mpu.begin();  // Initialize the MPU6050 sensor
  mpu.calcGyroOffsets(); // Calibrate the gyroscope

  updateDisplay();
}

void loop() {
  int xValue = analogRead(joystickXPin);
  int yValue = analogRead(joystickYPin);

  if (xValue < 300) { // Joystick moved left
    playSound(200); // Sound for left movement
    digitalWrite(led, HIGH); // LED on for left movement
    delay(200);
    digitalWrite(led, LOW);
    if (currentMode == 2) {
      resetAngle();  // Reset angle to 0 degrees
    } else {
      currentMode = (currentMode + 1) % 3;
      updateDisplay();
    }
    delay(1000);  // 1-second delay
  } else if (xValue > 700) { // Joystick moved right
    playSound(400); // Sound for right movement
    digitalWrite(led, HIGH); // LED on for right movement
    delay(200);
    digitalWrite(led, LOW);
    currentMode = (currentMode - 1 + 3) % 3;
    updateDisplay();
    delay(1000);  // 1-second delay
  }

  if (yValue < 300) { // Joystick moved up
    playSound(600); // Sound for changing unit to CM/M
    digitalWrite(led, HIGH); // LED on for up movement
    delay(200);
    digitalWrite(led, LOW);
    changeUnit();
    delay(1000);  // 1-second delay
  } else if (yValue > 700) { // Joystick moved down
    playSound(800); // Sound for laser toggle
    digitalWrite(led, HIGH); // LED on for down movement
    delay(200);
    digitalWrite(led, LOW);
    toggleLaser();
    delay(1000);  // 1-second delay
  }

  // Update the measurements every 100ms
  if (millis() - lastUpdateTime >= 100) {
    lastUpdateTime = millis();
    switch (currentMode) {
      case 0: measureHeight(); break;
      case 1: measureDistance(); break;
      case 2: measureAngle(); break;
    }
  }
}

// Measure height using ultrasonic sensor and display on LCD
void measureHeight() {
  distance = ultrasonic.read(); // Read the distance from the ultrasonic sensor
  height = 210 - distance; // Adjust for sensor position (210 cm)

  if (height <= 50) { // If height is less than or equal to 50 cm
    playSound(1000); // Sound for "No Entry"
    digitalWrite(led, HIGH); // Turn on the LED
    delay(1000);
    digitalWrite(led, LOW);
    lcd.clear(); // Clear the display
    lcd.setCursor(4, 1);
    lcd.print("No Entry");
  } else { // If height is greater than 50 cm
    lcd.clear(); // Clear the display
    lcd.setCursor(0, 0);
    lcd.print("YOUR HEIGHT: ");
    
    if (isMetric) {
      lcd.print(height);  // Display in CM
      lcd.print(" CM");
    } else {
      float heightM = height / 100.0; // Convert to meters
      lcd.print(heightM, 2); // Display in M with 2 decimal places
      lcd.print(" M");
    }
    
    lcd.setCursor(4, 1);
    lcd.print("  Entry ");
  }
  delay(1000);
}

// Measure distance using ultrasonic sensor and display on LCD
void measureDistance() {
  long duration = measureUltrasonic();
  int distance = duration * 0.0344 / 2;

  lcd.clear();
  lcd.setCursor(0, 0);
  if (isMetric) {
    lcd.print("Distance: ");
    lcd.print(distance);
    lcd.print(" CM");
  } else {
    float distanceM = distance / 100.0;
    if (distanceM < 0) {  // Ensure no negative values
      distanceM = 0;
    }
    lcd.print("Distance: ");
    lcd.print(distanceM, 2);
    lcd.print(" M");
  }
  delay(1000);
}

// Measure angle using MPU6050 and display on LCD
void measureAngle() {
  mpu.update();
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(" Angle: ");
  lcd.print(int(mpu.getAngleZ() - baseAngle)); // Adjusted by base angle
  lcd.print(" Deg");
  delay(1000);
}

// Reset angle measurement to 0 degrees
void resetAngle() {
  mpu.update();
  baseAngle = mpu.getAngleZ();
  lcd.clear();
  lcd.print("Angle Reset");
  delay(1000);  // 1-second delay
  updateDisplay();
}

// Toggle between CM and M units
void changeUnit() {
  isMetric = !isMetric;
  updateDisplay();
}

// Toggle laser diode on and off
void toggleLaser() {
  laserOn = !laserOn;
  digitalWrite(laserPin, laserOn ? HIGH : LOW);
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(laserOn ? "Laser: ON " : "Laser: OFF");
  delay(1000);  // 1-second delay
  updateDisplay();
}

// Update display based on current mode
void updateDisplay() {
  lcd.clear();
  switch (currentMode) {
    case 0: lcd.print("Mode: Height"); break;
    case 1: lcd.print("Mode: Distance"); break;
    case 2: lcd.print("Mode: Angle"); break;
  }
}

// Measure distance using the ultrasonic sensor
long measureUltrasonic() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  return pulseIn(echoPin, HIGH);
}

// Play different sounds for joystick movements and other actions
void playSound(int frequency) {
  tone(speakerPin, frequency, 200); // Play sound with given frequency for 200ms
  delay(200); // Short delay to distinguish between sounds
}