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

const int trigPinFront = 2;
const int echoPinFront = 3;
const int trigPinBack = 4;
const int echoPinBack = 5;
const int trigPinLeft = 18;
const int echoPinLeft = 19;
const int trigPinRight = 26;
const int echoPinRight = 27;
const int buzzerPin = 32;
const int ledPinL = 14;
const int ledPinR = 13;
const int ledHeadlightPin = 33;
const int ledBrakePin = 25; // ขา LED เบรก
const int ldrPin = 34;
const int stopDistance = 30; 
const int brakeDistance = 10; // ระยะทางสำหรับเบรก
const int light = 3000; 
const int modeSwitchPin = 12; // ขาสำหรับเลือกโหมด

LiquidCrystal_I2C lcd(0x27, 20, 4);

enum Mode { AUTO, MANUAL };
Mode currentMode = AUTO; // ค่าเริ่มต้นเป็นโหมดอัตโนมัติ

// ประกาศฟังก์ชันก่อนใช้งาน
long measureDistance(int trigPin, int echoPin);
void displayDistance(long distanceFront, long distanceBack, long distanceLeft, long distanceRight);
void displayHeadlightStatus(int ldrValue);
void displayCarDetection(long distanceFront, long distanceBack);
void controlMirrors(long distanceLeft, long distanceRight);
void toggleMode();

void setup() {
  Serial.begin(9600);
  
  pinMode(trigPinFront, OUTPUT);
  pinMode(echoPinFront, INPUT);
  pinMode(trigPinBack, OUTPUT);
  pinMode(echoPinBack, INPUT);
  pinMode(trigPinLeft, OUTPUT);
  pinMode(echoPinLeft, INPUT);
  pinMode(trigPinRight, OUTPUT);
  pinMode(echoPinRight, INPUT);
  pinMode(buzzerPin, OUTPUT);
  pinMode(ledPinL, OUTPUT);
  pinMode(ledPinR, OUTPUT);
  pinMode(ledHeadlightPin, OUTPUT);
  pinMode(ledBrakePin, OUTPUT); // ตั้งค่าเป็น OUTPUT
  pinMode(modeSwitchPin, INPUT_PULLUP); // สวิตช์โหมด

  lcd.init();
  lcd.backlight();
}

void loop() {
  // ตรวจสอบการเปลี่ยนโหมด
  if (digitalRead(modeSwitchPin) == LOW) {
    toggleMode();
    delay(200); // หน่วงเวลาเพื่อป้องกันการอ่านซ้ำ
  }

  long distanceFront = measureDistance(trigPinFront, echoPinFront);
  long distanceBack = measureDistance(trigPinBack, echoPinBack);
  long distanceLeft = measureDistance(trigPinLeft, echoPinLeft);
  long distanceRight = measureDistance(trigPinRight, echoPinRight);

  Serial.print("Distance Front: ");
  Serial.println(distanceFront); // แสดงระยะทางหน้า
  Serial.print("Distance Back: ");
  Serial.println(distanceBack);   // แสดงระยะทางหลัง 

  displayDistance(distanceFront, distanceBack);
  
int ldrValue = analogRead(ldrPin);
  Serial.print("LDR Value: ");
  Serial.println(ldrValue);
  
  displayHeadlightStatus(ldrValue); 
  
  // การตรวจจับรถขึ้นอยู่กับโหมด
  if (currentMode == AUTO) {
    displayCarDetection(distanceFront, distanceBack);
    controlMirrors(distanceLeft, distanceRight);
    // ตรวจสอบการเบรก
    if (distanceFront < brakeDistance) {
      digitalWrite(ledBrakePin, HIGH); // เปิด LED เบรก
      Serial.println("Brake Light ON");
    } else {
      digitalWrite(ledBrakePin, LOW); // ปิด LED เบรก
      Serial.println("Brake Light OFF");
    }
  } else {
    // โหมด MANUAL
    digitalWrite(ledBrakePin, LOW); // ปิด LED เบรกในโหมด manual
    lcd.setCursor(0, 2);
    lcd.print("Manual Mode       ");
  }

  delay(500);
}

void toggleMode() {
  currentMode = (currentMode == AUTO) ? MANUAL : AUTO; // สลับโหมด
  lcd.setCursor(0, 3);
  if (currentMode == AUTO) {
    lcd.print("Mode: Auto       ");
  } else {
    lcd.print("Mode: Manual     ");
  }
}

void displayDistance(long distanceFront, long distanceBack) {
  lcd.setCursor(0, 0);
  lcd.print("F: "); lcd.print(distanceFront); lcd.print(" cm");
  lcd.setCursor(0, 1);
  lcd.print("B: "); lcd.print(distanceBack); lcd.print(" cm");
}

void displayHeadlightStatus(int ldrValue) {
  if (ldrValue > light) {
    digitalWrite(ledHeadlightPin, HIGH);
    lcd.setCursor(0, 3);
    lcd.print("Headlight ON      ");
  } else {
    digitalWrite(ledHeadlightPin, LOW);
    lcd.setCursor(0, 3);
    lcd.print("Headlight OFF     ");
  }
}

void displayCarDetection(long distanceFront, long distanceBack) {
  if (distanceFront < stopDistance || distanceBack < stopDistance) {
    digitalWrite(buzzerPin, HIGH);
    lcd.setCursor(0, 2);
    lcd.print("Car Detected! ");
  } else {
    digitalWrite(buzzerPin, LOW);
    lcd.setCursor(0, 2);
    lcd.print("No Car       ");
  }
}

void controlMirrors(long distanceLeft, long distanceRight) {
  if (distanceLeft < stopDistance) {
    digitalWrite(ledPinL, HIGH);
  } else {
    digitalWrite(ledPinL, LOW);
  }
  
  if (distanceRight < stopDistance) {
    digitalWrite(ledPinR, HIGH);
  } else {
    digitalWrite(ledPinR, LOW);
  }
}

long measureDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH);
  return (duration * 0.034) / 2; // cm
}