#include <ESP32Servo.h>

// =====================================================
// 1. KHAI BAO CHAN SERVO
// =====================================================

const int SERVO_FL = 18;   // Front Left  - truoc trai
const int SERVO_FR = 19;   // Front Right - truoc phai
const int SERVO_RL = 4;    // Rear Left   - sau trai
const int SERVO_RR = 5;    // Rear Right  - sau phai

Servo servoFL;
Servo servoFR;
Servo servoRL;
Servo servoRR;

// =====================================================
// 2. NUT DIEU KHIEN KINH
// =====================================================

// Cua truoc trai
const int BTN_FL_UP   = 13;
const int BTN_FL_DOWN = 14;

// Cua truoc phai
const int BTN_FR_UP   = 16;
const int BTN_FR_DOWN = 17;

// Cua sau trai
const int BTN_RL_UP   = 25;
const int BTN_RL_DOWN = 26;

// Cua sau phai
const int BTN_RR_UP   = 27;
const int BTN_RR_DOWN = 32;

// =====================================================
// 3. NUT DIEU KHIEN XI-NHAN
// =====================================================

const int BTN_LEFT   = 33;
const int BTN_RIGHT  = 21;
const int BTN_HAZARD = 12;

// =====================================================
// 4. LED
// =====================================================

const int LED_LEFT   = 23;
const int LED_RIGHT  = 22;
const int LED_HAZARD = 2;

// =====================================================
// 5. VI TRI 4 KINH
// =====================================================

// 0  = kinh dong
// 90 = kinh mo hoan toan

int posFL = 0;
int posFR = 0;
int posRL = 0;
int posRR = 0;

const int MIN_POS = 0;
const int MAX_POS = 90;

// =====================================================
// 6. BIEN DIEU KHIEN NHAP NHAY
// =====================================================

unsigned long previousBlinkMillis = 0;
const unsigned long blinkInterval = 500;

bool blinkState = false;

// =====================================================
// 7. TRANG THAI XI-NHAN
// =====================================================

bool leftSignal = false;
bool rightSignal = false;
bool hazardSignal = false;

// =====================================================
// 8. BIEN DE CHONG DOI NUT
// =====================================================

bool lastLeftButton = HIGH;
bool lastRightButton = HIGH;
bool lastHazardButton = HIGH;

unsigned long lastButtonTime = 0;
const unsigned long debounceDelay = 50;

// =====================================================
// 9. TIMER CHO KINH
// =====================================================

unsigned long previousWindowMillis = 0;
const unsigned long windowInterval = 40;

// =====================================================
// SETUP
// =====================================================

void setup() {

  Serial.begin(115200);

  // -------------------------------
  // Nut cua kinh
  // -------------------------------

  pinMode(BTN_FL_UP,   INPUT_PULLUP);
  pinMode(BTN_FL_DOWN, INPUT_PULLUP);

  pinMode(BTN_FR_UP,   INPUT_PULLUP);
  pinMode(BTN_FR_DOWN, INPUT_PULLUP);

  pinMode(BTN_RL_UP,   INPUT_PULLUP);
  pinMode(BTN_RL_DOWN, INPUT_PULLUP);

  pinMode(BTN_RR_UP,   INPUT_PULLUP);
  pinMode(BTN_RR_DOWN, INPUT_PULLUP);

  // -------------------------------
  // Nut den
  // -------------------------------

  pinMode(BTN_LEFT,   INPUT_PULLUP);
  pinMode(BTN_RIGHT,  INPUT_PULLUP);
  pinMode(BTN_HAZARD, INPUT_PULLUP);

  // -------------------------------
  // LED
  // -------------------------------

  pinMode(LED_LEFT, OUTPUT);
  pinMode(LED_RIGHT, OUTPUT);
  pinMode(LED_HAZARD, OUTPUT);

  digitalWrite(LED_LEFT, LOW);
  digitalWrite(LED_RIGHT, LOW);
  digitalWrite(LED_HAZARD, LOW);

  // -------------------------------
  // Gan servo
  // -------------------------------

  servoFL.attach(SERVO_FL);
  servoFR.attach(SERVO_FR);
  servoRL.attach(SERVO_RL);
  servoRR.attach(SERVO_RR);

  // Vi tri ban dau: kinh dong
  servoFL.write(posFL);
  servoFR.write(posFR);
  servoRL.write(posRL);
  servoRR.write(posRR);

  Serial.println("================================");
  Serial.println(" MO PHONG KINH XE + XI NHAN");
  Serial.println("================================");
}

// =====================================================
// HAM XU LY 1 CUA KINH
// =====================================================

void controlWindow(
  int btnUp,
  int btnDown,
  Servo &servo,
  int &position
) {

  bool upPressed = (digitalRead(btnUp) == LOW);
  bool downPressed = (digitalRead(btnDown) == LOW);

  // Neu nhan UP
  if (upPressed && !downPressed) {

    if (position < MAX_POS) {
      position++;
      servo.write(position);
    }
  }

  // Neu nhan DOWN
  if (downPressed && !upPressed) {

    if (position > MIN_POS) {
      position--;
      servo.write(position);
    }
  }
}

// =====================================================
// DOC NUT XI-NHAN VA HAZARD
// =====================================================

void readSignalButtons() {

  bool currentLeft = digitalRead(BTN_LEFT);
  bool currentRight = digitalRead(BTN_RIGHT);
  bool currentHazard = digitalRead(BTN_HAZARD);

  unsigned long now = millis();

  if (now - lastButtonTime > debounceDelay) {

    // -------------------------------
    // NUT XI-NHAN TRAI
    // -------------------------------

    if (lastLeftButton == HIGH && currentLeft == LOW) {

      leftSignal = !leftSignal;

      // Bat trai thi tat phai
      if (leftSignal) {
        rightSignal = false;
      }

      Serial.print("Xi-nhan trai: ");
      Serial.println(leftSignal ? "BAT" : "TAT");

      lastButtonTime = now;
    }

    // -------------------------------
    // NUT XI-NHAN PHAI
    // -------------------------------

    if (lastRightButton == HIGH && currentRight == LOW) {

      rightSignal = !rightSignal;

      // Bat phai thi tat trai
      if (rightSignal) {
        leftSignal = false;
      }

      Serial.print("Xi-nhan phai: ");
      Serial.println(rightSignal ? "BAT" : "TAT");

      lastButtonTime = now;
    }

    // -------------------------------
    // NUT HAZARD
    // -------------------------------

    if (lastHazardButton == HIGH && currentHazard == LOW) {

      hazardSignal = !hazardSignal;

      Serial.print("Hazard: ");
      Serial.println(hazardSignal ? "BAT" : "TAT");

      lastButtonTime = now;
    }
  }

  lastLeftButton = currentLeft;
  lastRightButton = currentRight;
  lastHazardButton = currentHazard;
}

// =====================================================
// DIEU KHIEN DEN XI-NHAN
// =====================================================

void controlLights() {

  unsigned long now = millis();

  // Tao xung nhap nhay moi 500ms
  if (now - previousBlinkMillis >= blinkInterval) {

    previousBlinkMillis = now;
    blinkState = !blinkState;
  }

  // ---------------------------------
  // HAZARD
  // ---------------------------------

  if (hazardSignal) {

    // Ca 3 LED cung nhap nhay
    digitalWrite(LED_LEFT, blinkState);
    digitalWrite(LED_RIGHT, blinkState);
    digitalWrite(LED_HAZARD, blinkState);
  }

  // ---------------------------------
  // XI-NHAN TRAI
  // ---------------------------------

  else if (leftSignal) {

    digitalWrite(LED_LEFT, blinkState);
    digitalWrite(LED_RIGHT, LOW);
    digitalWrite(LED_HAZARD, LOW);
  }

  // ---------------------------------
  // XI-NHAN PHAI
  // ---------------------------------

  else if (rightSignal) {

    digitalWrite(LED_LEFT, LOW);
    digitalWrite(LED_RIGHT, blinkState);
    digitalWrite(LED_HAZARD, LOW);
  }

  // ---------------------------------
  // TAT HET
  // ---------------------------------

  else {

    digitalWrite(LED_LEFT, LOW);
    digitalWrite(LED_RIGHT, LOW);
    digitalWrite(LED_HAZARD, LOW);
  }
}

// =====================================================
// IN TRANG THAI
// =====================================================

void printWindowStatus() {

  static unsigned long lastPrint = 0;

  if (millis() - lastPrint >= 1000) {

    lastPrint = millis();

    Serial.println("-------------------------------");

    Serial.print("Cua truoc trai  : ");
    Serial.print(posFL);
    Serial.println(" do");

    Serial.print("Cua truoc phai  : ");
    Serial.print(posFR);
    Serial.println(" do");

    Serial.print("Cua sau trai    : ");
    Serial.print(posRL);
    Serial.println(" do");

    Serial.print("Cua sau phai    : ");
    Serial.print(posRR);
    Serial.println(" do");

    Serial.println("-------------------------------");
  }
}

// =====================================================
// LOOP
// =====================================================

void loop() {

  // -------------------------------
  // 1. Dieu khien 4 cua kinh
  // -------------------------------

  if (millis() - previousWindowMillis >= windowInterval) {

    previousWindowMillis = millis();

    controlWindow(
      BTN_FL_UP,
      BTN_FL_DOWN,
      servoFL,
      posFL
    );

    controlWindow(
      BTN_FR_UP,
      BTN_FR_DOWN,
      servoFR,
      posFR
    );

    controlWindow(
      BTN_RL_UP,
      BTN_RL_DOWN,
      servoRL,
      posRL
    );

    controlWindow(
      BTN_RR_UP,
      BTN_RR_DOWN,
      servoRR,
      posRR
    );
  }

  // -------------------------------
  // 2. Doc nut den
  // -------------------------------

  readSignalButtons();

  // -------------------------------
  // 3. Dieu khien den
  // -------------------------------

  controlLights();

  // -------------------------------
  // 4. In trang thai
  // -------------------------------

  printWindowStatus();
}