#include <ESP32Servo.h>              
#include <LiquidCrystal_I2C.h>         

#define TRIG_PIN_1 5              
#define ECHO_PIN_1 18                               
#define TRIG_PIN_2 25                
#define ECHO_PIN_2 26                 
#define LDR_PIN 34                    
#define LED_PIN_1 2                
#define LED_PIN_2 4                 
#define SERVO_PIN 16                 
#define BUZZER_PIN 27                
#define BUTTON_PIN 32         

Servo brakeServo;
LiquidCrystal_I2C lcd(0x27, 20, 4);

int safeDistance = 30;
int lightThreshold = 1000;
bool autoMode = true;  

void setup() {
  // ตั้งค่าพินต่างๆ
  pinMode(TRIG_PIN_1, OUTPUT);
  pinMode(ECHO_PIN_1, INPUT);
  pinMode(TRIG_PIN_2, OUTPUT);
  pinMode(ECHO_PIN_2, INPUT);
  pinMode(LDR_PIN, INPUT);
  pinMode(LED_PIN_1, OUTPUT);
  pinMode(LED_PIN_2, OUTPUT);
  pinMode(BUZZER_PIN, OUTPUT);
  pinMode(BUTTON_PIN, INPUT);  

  // ตั้งค่าเซอร์โวและ LCD
  brakeServo.attach(SERVO_PIN);
  lcd.init();
  lcd.clear(); 
  lcd.backlight();
  brakeServo.write(0);                // ปล่อยเบรก
  delay(500);
  digitalWrite(LED_PIN_1, LOW);       // ปิดไฟหน้า
  digitalWrite(LED_PIN_2, LOW);       // ปิดไฟหน้า
  Serial.begin(115200);               
}

void loop() {
 

  if (autoMode) {
    int distance1 = readUltrasonicDistance(TRIG_PIN_1, ECHO_PIN_1);
    int distance2 = readUltrasonicDistance(TRIG_PIN_2, ECHO_PIN_2);
    
    int lightLevel = analogRead(LDR_PIN);

    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Front Distance:");
    lcd.print(distance1);
    lcd.print("cm");
    
    lcd.setCursor(0, 1);
    lcd.print("Back Distance:");
    lcd.print(distance2);
    lcd.print("cm");
    
    lcd.setCursor(0, 2);
    lcd.print("Light : ");
    lcd.print(lightLevel);

    // เช็คระยะเพื่อควบคุมเซอร์โว (เบรก)
    if (distance1 < safeDistance || distance2 < safeDistance) {
      brakeServo.write(90); // เบรก หมุนเซอร์โวไป90 องศา
      lcd.setCursor(12, 3);
      lcd.print("Brake!!");
      digitalWrite(BUZZER_PIN, HIGH); // เปิด Buzzer
    } else {
      brakeServo.write(0);  // ปล่อยเบรก
      lcd.setCursor(12, 3);
      lcd.print("No Brake");
      digitalWrite(BUZZER_PIN, LOW);  // ปิด Buzzer
    }

    if (lightLevel < lightThreshold) {
      digitalWrite(LED_PIN_1, HIGH);  // เปิดไฟ
      digitalWrite(LED_PIN_2, HIGH);  
      lcd.setCursor(0, 3);
      lcd.print("lights: ON");
    } else {
      digitalWrite(LED_PIN_1, LOW);   // ปิดไฟ
      digitalWrite(LED_PIN_2, LOW);   
      lcd.setCursor(0, 3);
      lcd.print("lights: OFF");
    }
  }

  delay(1000); 
}

// ฟังก์ชันสำหรับอ่านค่าระยะจาก Ultrasonic Sensor
int readUltrasonicDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delay(200);
  digitalWrite(trigPin, HIGH);
  delay(100);
  digitalWrite(trigPin, LOW);
  
  long duration = pulseIn(echoPin, HIGH); // วัดระยะเวลาที่สัญญาณสะท้อนกลับมา
  int distance = duration * 0.034 / 2;     // คำนวณระยะทาง (cm)
  return distance;
}