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

LiquidCrystal_I2C lcd(0x27, 16, 2);
Servo servoDepan, servoBelakangKiri, servoBelakangKanan;

const int trigPin = 12, echoPin = 13;
const int servoPins[] = {10, 9, 11}; // Depan, BelakangKiri, BelakangKanan
const int buttonManualPin = 3, buttonModePin = 6, ledRed = 5, ledGreen = 4;
const int thresholdDown = 28, thresholdUp = 32;

long duration;
int distance;
bool gearDown = false, manualMode = false;
unsigned long lastDebounceTime = 0, lastManualPressTime = 0;
const unsigned long debounceDelay = 300;

void setup() {
  pinMode(trigPin, OUTPUT); pinMode(echoPin, INPUT);
  pinMode(buttonManualPin, INPUT_PULLUP); pinMode(buttonModePin, INPUT_PULLUP);
  pinMode(ledRed, OUTPUT); pinMode(ledGreen, OUTPUT);

  servoDepan.attach(servoPins[0]);
  servoBelakangKiri.attach(servoPins[1]);
  servoBelakangKanan.attach(servoPins[2]);

  lcd.init(); lcd.backlight(); lcd.print("Landing Gear:");
  
  for (int i = 0; i < 3; i++) {
    digitalWrite(ledRed, HIGH); digitalWrite(ledGreen, HIGH);
    delay(300);
    digitalWrite(ledRed, LOW); digitalWrite(ledGreen, LOW);
    delay(300);
  }
}

void loop() {
  if (digitalRead(buttonModePin) == LOW && (millis() - lastDebounceTime > debounceDelay)) {
    manualMode = !manualMode;
    lastDebounceTime = millis();
    if (!manualMode) {
      // Ketika beralih ke mode otomatis, sesuaikan posisi gear berdasarkan jarak
      digitalWrite(trigPin, LOW); delayMicroseconds(2);
      digitalWrite(trigPin, HIGH); delayMicroseconds(10);
      digitalWrite(trigPin, LOW);
      duration = pulseIn(echoPin, HIGH);
      distance = duration * 0.034 / 2;
      if (distance < thresholdDown && !gearDown) {
        gearDown = true;
        gerakkanServo(90, 180);
        flashLed(ledRed);
      } else if (distance > thresholdUp && gearDown) {
        gearDown = false;
        gerakkanServo(180, 90);
        flashLed(ledGreen);
      }
    }
  }

  // Set LED berdasarkan status gear (turun: merah, naik: hijau)
  digitalWrite(ledRed, gearDown); digitalWrite(ledGreen, !gearDown);
  lcd.setCursor(0, 0); lcd.print(manualMode ? "Mode: MANUAL   " : "Mode: AUTO     ");
  
  manualMode ? kontrolManual() : kontrolOtomatis();
}

void kontrolOtomatis() {
  digitalWrite(trigPin, LOW); delayMicroseconds(2);
  digitalWrite(trigPin, HIGH); delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.034 / 2;

  if (distance < thresholdDown && !gearDown) {
    gearDown = true; gerakkanServo(90, 180); flashLed(ledRed);
  } else if (distance > thresholdUp && gearDown) {
    gearDown = false; gerakkanServo(180, 90); flashLed(ledGreen);
  }

  lcd.setCursor(0, 1); lcd.print(gearDown ? "TURUN " : "NAIK  ");
  lcd.setCursor(10, 1); lcd.print(distance); lcd.print("cm ");
}

void kontrolManual() {
  if (digitalRead(buttonManualPin) == LOW && (millis() - lastManualPressTime > debounceDelay)) {
    gearDown = !gearDown; // Toggle posisi gear
    gerakkanServo(gearDown ? 90 : 180, gearDown ? 180 : 90);
    flashLed(gearDown ? ledRed : ledGreen);
    lastManualPressTime = millis();
  }

  lcd.setCursor(0, 1); lcd.print(gearDown ? "TURUN " : "NAIK  ");
  lcd.setCursor(10, 1); lcd.print("      ");
}

void gerakkanServo(int dari, int ke) {
  int step = (dari < ke) ? 1 : -1;
  for (int angle = dari; angle != ke + step; angle += step) {
    servoDepan.write(angle); servoBelakangKiri.write(angle); servoBelakangKanan.write(angle);
    delay(5);
  }
}

void flashLed(int ledPin) {
  digitalWrite(ledRed, LOW); digitalWrite(ledGreen, LOW); // Matikan kedua LED
  delay(50);
  digitalWrite(ledPin, HIGH); delay(500);
  digitalWrite(ledPin, LOW); delay(50);
  digitalWrite(ledRed, gearDown); digitalWrite(ledGreen, !gearDown); // Set LED sesuai status gear
}