#include <AccelStepper.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <EEPROM.h>
#include "esp_task_wdt.h"
#include "freertos/FreeRTOS.h"

// ================== PIN MAPPING ==================
#define PIN_STEP 26
#define PIN_DIR 27
#define PIN_ENABLE 33
// === LIMIT SWITCH PINTU ===
#define LIMIT_DOOR_OPEN 34
#define LIMIT_DOOR_CLOSE 35
// === FINAL LIMIT LIFT ===
#define FINAL_LIMIT_UP 32
#define FINAL_LIMIT_DOWN 25
// === TOMBOL MANUAL PINTU ===
#define BTN_OPEN 13
#define BTN_CLOSE 12
// === TOMBOL MANUAL MOTOR LIFT ===
#define BTN_DIR_UP 23
#define BTN_DIR_DOWN 14
// === EMERGENCY & MODE ===
#define BTN_EMERGENCY 19
#define SW_INSPECTION 18
// === UART (CTB MCC) ===
#define RXD2 16
#define TXD2 17
// === UART (COP) ===
#define RXD1 9
#define TXD1 10
// === I2C OLED (PAKAI DEFAULT WIRE) ===
#define I2C_SDA 21
#define I2C_SCL 22
// === RELAY MOTOR LIFT ===
#define RELAY_MOTOR_UP 2
#define RELAY_MOTOR_DOWN 4

// ================== OBJECTS ==================
AccelStepper doorStepper(AccelStepper::DRIVER, PIN_STEP, PIN_DIR);
Adafruit_SSD1306 display(128, 64, &Wire);

// ================== VARIABLES ==================
enum DoorState {DOOR_STOP, DOOR_OPENING, DOOR_CLOSING, DOOR_ERROR};
DoorState doorState = DOOR_STOP;

enum CabinState {CABIN_STOP, CABIN_UP, CABIN_DOWN};
CabinState cabinState = CABIN_STOP;

// Auto-close variables
unsigned long doorOpenTime = 0;
bool autoCloseActive = false;
const unsigned long autoCloseDelay = 3000;

// Homing variables
long steps_per_door = 0;
bool is_calibrated = false;
const int EEPROM_ADDRESS = 0;
const unsigned long HOMING_TIMEOUT = 5000;

// Debounce variables
const unsigned long DEBOUNCE_DELAY = 50;
unsigned long lastDebounceTime[10] = {0};
int lastState[10] = {HIGH};

// Assign index for each pin
#define BTN_OPEN_IDX 0
#define BTN_CLOSE_IDX 1
#define BTN_DIR_UP_IDX 2
#define BTN_DIR_DOWN_IDX 3
#define BTN_EMERGENCY_IDX 4
#define SW_INSPECTION_IDX 5
#define LIMIT_DOOR_OPEN_IDX 6
#define LIMIT_DOOR_CLOSE_IDX 7
#define FINAL_LIMIT_UP_IDX 8
#define FINAL_LIMIT_DOWN_IDX 9

// Display refresh timer
unsigned long lastDisplayUpdateTime = 0;
const unsigned long displayUpdateInterval = 80;

// Door stepper params
const float MAX_SPEED = 900.0f;
const float ACCEL = 600.0f;
const float HOMING_SPEED = 200.0f;

// UART command variables for AUTO mode
String mainMasterCommand = "";
unsigned long lastSerial2ReadTime = 0;
const unsigned long SERIAL2_TIMEOUT = 1000;

// UART command variables for COP
String copCommand = "";
unsigned long lastSerial1ReadTime = 0;
const unsigned long SERIAL1_TIMEOUT = 1000;

// Error handling variables (non-blocking)
bool inErrorState = false;
unsigned long errorStartTime = 0;
const unsigned long ERROR_DISPLAY_DURATION = 5000;
bool emergencyActive = false;

// Variabel untuk parser UART non-blocking
String uartBuffer2 = "";
String uartBuffer1 = "";
const int UART_MAX_BUFFER_SIZE = 32;

// ================== FUNCTIONS ==================
int digitalReadFiltered(int pin, int index) {
  int reading = digitalRead(pin);
  if (reading != lastState[index]) {
    lastDebounceTime[index] = millis();
  }
  lastState[index] = reading;
  if ((millis() - lastDebounceTime[index]) > DEBOUNCE_DELAY) {
    return reading;
  }
  return lastState[index];
}

void stopCabinMotor() {
  digitalWrite(RELAY_MOTOR_UP, LOW);
  digitalWrite(RELAY_MOTOR_DOWN, LOW);
  cabinState = CABIN_STOP;
  mainMasterCommand = "";
}

void displayError(const char* message) {
  if (!inErrorState) {
    Serial.println(message);
    stopCabinMotor();
    doorStepper.stop();
    inErrorState = true;
    errorStartTime = millis();
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(0, 0);
    display.println("--- SYSTEM ERROR ---");
    display.setCursor(0, 15);
    display.println(message);
    display.display();
  }
}

// ================== NEW FUNCTION FOR COP COMMUNICATION ==================
void handleCopCommunication() {
    // Baca perintah dari COP (misalnya, tombol kabin ditekan)
    while (Serial1.available()) {
        char c = Serial1.read();
        if (c == '\n') {
            uartBuffer1.trim();
            // Teruskan perintah dari COP ke master utama
            Serial2.println(uartBuffer1); 
            uartBuffer1 = "";
        } else if (uartBuffer1.length() < UART_MAX_BUFFER_SIZE) {
            uartBuffer1 += c;
        }
    }
}

void handleAutoMode() {
  // Pengecekan limit switch untuk menghentikan motor secara otomatis
  if (digitalReadFiltered(FINAL_LIMIT_UP, FINAL_LIMIT_UP_IDX) == LOW && mainMasterCommand == "UP") {
    stopCabinMotor();
    Serial2.println("FINAL_LIMIT_UP_ACTIVE");
  }
  if (digitalReadFiltered(FINAL_LIMIT_DOWN, FINAL_LIMIT_DOWN_IDX) == LOW && mainMasterCommand == "DOWN") {
    stopCabinMotor();
    Serial2.println("FINAL_LIMIT_DOWN_ACTIVE");
  }

  // Baca perintah dari master
  while (Serial2.available()) {
    char c = Serial2.read();
    if (c == '\n') {
      uartBuffer2.trim();
      lastSerial2ReadTime = millis();
      // Perintah kabin hanya diizinkan jika pintu sudah tertutup penuh
      if (digitalReadFiltered(LIMIT_DOOR_CLOSE, LIMIT_DOOR_CLOSE_IDX) == LOW && doorState == DOOR_STOP) {
        if (uartBuffer2 == "UP" && digitalReadFiltered(FINAL_LIMIT_UP, FINAL_LIMIT_UP_IDX) == HIGH) {
          mainMasterCommand = "UP";
        } else if (uartBuffer2 == "DOWN" && digitalReadFiltered(FINAL_LIMIT_DOWN, FINAL_LIMIT_DOWN_IDX) == HIGH) {
          mainMasterCommand = "DOWN";
        } else if (uartBuffer2 == "STOP_CABIN") {
          mainMasterCommand = "STOP";
        }
      }
      if (uartBuffer2 == "OPEN" && cabinState == CABIN_STOP) {
        doorState = DOOR_OPENING;
        doorStepper.moveTo(steps_per_door);
        autoCloseActive = true;
      } else if (uartBuffer2 == "CLOSE") {
        doorState = DOOR_CLOSING;
        doorStepper.moveTo(0);
      } else {
        Serial1.println(uartBuffer2);
      }
      uartBuffer2 = "";
    } else if (uartBuffer2.length() < UART_MAX_BUFFER_SIZE) {
      uartBuffer2 += c;
    }
  }

  if (millis() - lastSerial2ReadTime > SERIAL2_TIMEOUT) {
    mainMasterCommand = "";
  }

  // Eksekusi perintah kabin
  if (mainMasterCommand == "UP") {
    digitalWrite(RELAY_MOTOR_UP, HIGH);
    digitalWrite(RELAY_MOTOR_DOWN, LOW);
    cabinState = CABIN_UP;
  } else if (mainMasterCommand == "DOWN") {
    digitalWrite(RELAY_MOTOR_DOWN, HIGH);
    digitalWrite(RELAY_MOTOR_UP, LOW);
    cabinState = CABIN_DOWN;
  } else if (mainMasterCommand == "STOP") {
    stopCabinMotor();
  }
}

void homingAndCalibration() {
  Serial.println("Memulai proses homing dan kalibrasi...");
  EEPROM.get(EEPROM_ADDRESS, steps_per_door);

  if (steps_per_door > 0 && steps_per_door < 30000) {
    Serial.println("Nilai kalibrasi ditemukan di EEPROM.");
    is_calibrated = true;
    doorStepper.setCurrentPosition(0);
    Serial.print("Steps per door: ");
    Serial.println(steps_per_door);
    return;
  }

  Serial.println("Nilai kalibrasi tidak ditemukan. Melakukan kalibrasi baru.");
  Serial.println("Mencari posisi 0 (pintu tertutup)...");
  doorStepper.setMaxSpeed(HOMING_SPEED);
  doorStepper.setAcceleration(ACCEL);
  doorStepper.moveTo(-30000);
  unsigned long homing_start_time = millis();
  while (digitalReadFiltered(LIMIT_DOOR_CLOSE, LIMIT_DOOR_CLOSE_IDX) == HIGH) {
    doorStepper.run();
    if (millis() - homing_start_time > HOMING_TIMEOUT) {
      displayError("Homing Error: Timeout limit tutup");
      return;
    }
    esp_task_wdt_reset();
  }
  doorStepper.stop();
  doorStepper.setCurrentPosition(0);
  Serial.println("Posisi 0 ditemukan.");

  Serial.println("Mengukur langkah dari pintu tutup ke pintu buka...");
  doorStepper.setMaxSpeed(MAX_SPEED);
  doorStepper.setAcceleration(ACCEL);
  doorStepper.moveTo(30000);
  homing_start_time = millis();
  while (digitalReadFiltered(LIMIT_DOOR_OPEN, LIMIT_DOOR_OPEN_IDX) == HIGH) {
    doorStepper.run();
    if (millis() - homing_start_time > HOMING_TIMEOUT) {
      displayError("Homing Error: Timeout limit buka");
      return;
    }
    esp_task_wdt_reset();
  }
  doorStepper.stop();
  long end_position = doorStepper.currentPosition();
  doorStepper.setCurrentPosition(end_position);
  steps_per_door = end_position;
  EEPROM.put(EEPROM_ADDRESS, steps_per_door);
  EEPROM.commit();
  is_calibrated = true;
  Serial.print("Kalibrasi selesai. Steps per door: ");
  Serial.println(steps_per_door);
}

void printStatusToSerial() {
  bool emergency = (digitalReadFiltered(BTN_EMERGENCY, BTN_EMERGENCY_IDX) == LOW);
  const char* modeStr = (digitalReadFiltered(SW_INSPECTION, SW_INSPECTION_IDX) == LOW) ? "INSPECTION" : "AUTO";
  const char* cabinStr = (cabinState == CABIN_UP) ? "UP" : (cabinState == CABIN_DOWN) ? "DOWN" : "IDLE";
  const char* doorStr = "IDLE";
  if (doorState == DOOR_OPENING) {
    doorStr = "OPENING";
  } else if (doorState == DOOR_CLOSING) {
    doorStr = "CLOSING";
  } else {
    doorStr = (doorStepper.currentPosition() == 0) ? "CLOSE" : "OPEN";
  }
  int remaining = 0;
  if ((doorState == DOOR_OPENING || doorState == DOOR_STOP) && autoCloseActive) {
    unsigned long elapsed = millis() - doorOpenTime;
    if (elapsed < autoCloseDelay) {
      remaining = (autoCloseDelay - elapsed) / 1000 + 1;
    }
  }
  Serial.print("Mode: ");
  Serial.print(modeStr);
  Serial.print(" | Dir: ");
  Serial.print(cabinStr);
  Serial.print(" | Door: ");
  Serial.print(doorStr);
  if (remaining > 0) {
    Serial.print(" | AutoClose in ");
    Serial.print(remaining);
    Serial.print("s");
  }
  Serial.print(" | EMG: ");
  Serial.println(emergency ? "ON" : "OFF");
  Serial.print(" | L_UP: ");
  Serial.print((digitalReadFiltered(FINAL_LIMIT_UP, FINAL_LIMIT_UP_IDX) == LOW) ? "ON" : "OFF");
  Serial.print(" | L_DN: ");
  Serial.println((digitalReadFiltered(FINAL_LIMIT_DOWN, FINAL_LIMIT_DOWN_IDX) == LOW) ? "ON" : "OFF");
}

void displayModeInfo(const char* mode, const char* cabinDir, bool emergency) {
  display.clearDisplay();
  display.setTextSize(2);
  display.setCursor(3, 0);
  display.setTextColor(SSD1306_WHITE);
  display.println("Orch_CTBDD");
  display.drawLine(0, 18, 128, 18, SSD1306_WHITE);
  display.setTextSize(1);
  display.setCursor(0, 22);
  display.print("MODE");
  display.setCursor(25, 22);
  display.print(":");
  display.setCursor(31, 22);
  display.println(mode);
  display.setCursor(0, 33);
  display.print("DIR");
  display.setCursor(25, 33);
  display.print(":");
  display.setCursor(31, 33);
  display.println(cabinDir);
  display.setCursor(0, 44);
  display.print("DOOR");
  display.setCursor(25, 44);
  display.print(":");
  if (doorState == DOOR_OPENING) {
    display.print("OPENING");
  } else if (doorState == DOOR_CLOSING) {
    display.print("CLOSING");
  } else if (doorStepper.currentPosition() == steps_per_door) {
    display.print("OPEN");
  } else if (doorStepper.currentPosition() == 0) {
    display.print("CLOSE");
  } else {
    display.print("IDLE");
  }

  if ((doorState == DOOR_OPENING || doorState == DOOR_STOP) && autoCloseActive) {
    unsigned long elapsed = millis() - doorOpenTime;
    if (elapsed < autoCloseDelay) {
      int remaining = (autoCloseDelay - elapsed) / 1000 + 1;
      display.setCursor(55, 44);
      display.print(",Close in ");
      display.print(remaining);
      display.print("s");
    }
  }
  display.setCursor(0, 56);
  display.print("F.LM");
  display.setCursor(25, 56);
  display.print(":");
  const char* finalLimitStatus;
  if (digitalReadFiltered(FINAL_LIMIT_UP, FINAL_LIMIT_UP_IDX) == LOW) {
    finalLimitStatus = "UP ACTIVE";
  } else if (digitalReadFiltered(FINAL_LIMIT_DOWN, FINAL_LIMIT_DOWN_IDX) == LOW) {
    finalLimitStatus = "DN ACTIVE";
  } else {
    finalLimitStatus = "OFF";
  }
  display.print(finalLimitStatus);
  if (emergency) {
    display.fillRect(0, 22, 128, 40, SSD1306_WHITE);
    display.setTextColor(SSD1306_BLACK);
    display.setCursor(8, 28);
    display.setTextSize(1);
    display.println("*** DARURAT! ***");
    display.setCursor(8, 44);
    display.println("Lift dihentikan.");
    display.setTextColor(SSD1306_WHITE);
  } else {
    display.setTextSize(1);
    display.setCursor(72, 33);
    display.print("EM.S:OFF");
  }
  display.display();
}

void updateDisplay() {
  if (millis() - lastDisplayUpdateTime >= displayUpdateInterval) {
    lastDisplayUpdateTime = millis();
    bool emergency = (digitalReadFiltered(BTN_EMERGENCY, BTN_EMERGENCY_IDX) == LOW);
    const char* modeStr = (digitalReadFiltered(SW_INSPECTION, SW_INSPECTION_IDX) == LOW) ? "INSPECTION" : "AUTO";
    const char* cabinStr = (cabinState == CABIN_UP) ? "UP" : (cabinState == CABIN_DOWN) ? "DOWN" : "IDLE";
    if (doorState == DOOR_ERROR) {
      displayError("Kalibrasi Gagal!");
      return;
    }
    if (inErrorState) {
      displayError("");
      return;
    }
    displayModeInfo(modeStr, cabinStr, emergency);
    printStatusToSerial();
  }
}

void showStartup() {
  display.clearDisplay();
  display.setTextSize(3);
  display.setCursor(132, 0);
  display.setTextColor(SSD1306_WHITE);
  display.print("Orch.Gen");
  display.display();
  for (int x = 132; x > 20; x -= 4) {
    display.clearDisplay();
    display.setTextSize(2);
    display.setCursor(x, 0);
    display.print("Orch.Gen");
    display.display();
    delay(15);
  }
  display.setTextSize(1);
  display.setCursor(38, 25);
  display.print("By Elzabid");
  display.display();
  delay(300);
  for (int i = 0; i < 3; i++) {
    display.setTextSize(1);
    display.setCursor(20, 45);
    display.print("Initializing...");
    display.display();
    delay(300);
    display.clearDisplay();
    display.setCursor(20, 45);
    display.display();
    delay(300);
  }
  display.setTextSize(1);
  display.setCursor(0, 55);
  display.print("Please wait...");
  for (int i = 0; i < 120; i += 6) {
    display.fillRect(0, 50, i, 3, SSD1306_WHITE);
    display.display();
    delay(20);
  }
  Serial.println("Startup selesai. Sistem siap.");
}

void handleAutoClose() {
  if (autoCloseActive && digitalReadFiltered(SW_INSPECTION, SW_INSPECTION_IDX) != LOW) {
    if (millis() - doorOpenTime >= autoCloseDelay) {
      doorState = DOOR_CLOSING;
      doorStepper.moveTo(0);
      autoCloseActive = false;
      Serial.println("Auto-close triggered.");
    }
  }
}

// ================== SETUP ==================
void setup() {
  Serial.begin(115200);
  Serial1.begin(9600, SERIAL_8N1, RXD1, TXD1); // Inisialisasi UART untuk COP
  Serial2.begin(9600, SERIAL_8N1, RXD2, TXD2); // Inisialisasi UART untuk Master
  Serial.println("Sistem dinyalakan.");

  esp_task_wdt_config_t twdt_config = {
    .timeout_ms = 5000,
    .idle_core_mask = (1 << xPortGetCoreID()),
    .trigger_panic = true,
  };
  esp_task_wdt_init(&twdt_config);
  esp_task_wdt_add(NULL);

  pinMode(LIMIT_DOOR_OPEN, INPUT_PULLUP);
  pinMode(LIMIT_DOOR_CLOSE, INPUT_PULLUP);
  pinMode(FINAL_LIMIT_UP, INPUT_PULLUP);
  pinMode(FINAL_LIMIT_DOWN, INPUT_PULLUP);
  pinMode(BTN_OPEN, INPUT_PULLUP);
  pinMode(BTN_CLOSE, INPUT_PULLUP);
  pinMode(BTN_DIR_UP, INPUT_PULLUP);
  pinMode(BTN_DIR_DOWN, INPUT_PULLUP);
  pinMode(BTN_EMERGENCY, INPUT_PULLUP);
  pinMode(SW_INSPECTION, INPUT_PULLUP);

  pinMode(RELAY_MOTOR_UP, OUTPUT);
  pinMode(RELAY_MOTOR_DOWN, OUTPUT);
  pinMode(PIN_ENABLE, OUTPUT);

  digitalWrite(RELAY_MOTOR_UP, LOW);
  digitalWrite(RELAY_MOTOR_DOWN, LOW);
  digitalWrite(PIN_ENABLE, LOW);

  doorStepper.setMaxSpeed(MAX_SPEED);
  doorStepper.setAcceleration(ACCEL);

  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("Alokasi SSD1306 gagal"));
    for (;;);
  }
  showStartup();

  EEPROM.begin(512);
  homingAndCalibration();
}

// ================== LOOP ==================
void loop() {
  esp_task_wdt_reset();

  // Logika emergency harus selalu dicek terlebih dahulu
  bool emergencyCheck = (digitalReadFiltered(BTN_EMERGENCY, BTN_EMERGENCY_IDX) == LOW);
  if (emergencyCheck) {
    emergencyActive = true;
    Serial.println("EMERGENCY STOP ACTIVE");
  }
  if (emergencyActive) {
    stopCabinMotor();
    doorStepper.stop();
    doorState = DOOR_STOP;
    if (emergencyCheck == HIGH) {
      emergencyActive = false;
      Serial.println("EMERGENCY STOP RELEASED");
    }
    updateDisplay();
    return;
  }

  // Handle error state
  if (inErrorState) {
    if (millis() - errorStartTime > ERROR_DISPLAY_DURATION) {
      inErrorState = false;
      display.clearDisplay();
    } else {
      updateDisplay();
      return;
    }
  }

  if (!is_calibrated) {
    doorState = DOOR_ERROR;
    updateDisplay();
    return;
  }
  
  // Panggil fungsi komunikasi COP
  handleCopCommunication();

  // Logika mode inspeksi
  if (digitalReadFiltered(SW_INSPECTION, SW_INSPECTION_IDX) == LOW) {
    // Pergerakan kabin hanya diizinkan jika pintu TERTUTUP DAN TIDAK sedang bergerak
    if (digitalReadFiltered(LIMIT_DOOR_CLOSE, LIMIT_DOOR_CLOSE_IDX) == LOW && doorState == DOOR_STOP) {
      if (digitalReadFiltered(BTN_DIR_UP, BTN_DIR_UP_IDX) == LOW &&
          digitalReadFiltered(FINAL_LIMIT_UP, FINAL_LIMIT_UP_IDX) == HIGH) {
        digitalWrite(RELAY_MOTOR_UP, HIGH);
        digitalWrite(RELAY_MOTOR_DOWN, LOW);
        cabinState = CABIN_UP;
      }
      else if (digitalReadFiltered(BTN_DIR_DOWN, BTN_DIR_DOWN_IDX) == LOW &&
               digitalReadFiltered(FINAL_LIMIT_DOWN, FINAL_LIMIT_DOWN_IDX) == HIGH) {
        digitalWrite(RELAY_MOTOR_DOWN, HIGH);
        digitalWrite(RELAY_MOTOR_UP, LOW);
        cabinState = CABIN_DOWN;
      }
      else {
        stopCabinMotor();
      }
    } else {
        stopCabinMotor();
    }
    
    // Logika pintu manual di mode inspeksi
    if (digitalReadFiltered(BTN_OPEN, BTN_OPEN_IDX) == LOW && digitalReadFiltered(LIMIT_DOOR_CLOSE, LIMIT_DOOR_CLOSE_IDX) == LOW) {
      doorState = DOOR_OPENING;
      doorStepper.moveTo(steps_per_door);
      autoCloseActive = false;
    }
    else if (digitalReadFiltered(BTN_CLOSE, BTN_CLOSE_IDX) == LOW && digitalReadFiltered(LIMIT_DOOR_OPEN, LIMIT_DOOR_OPEN_IDX) == LOW) {
      doorState = DOOR_CLOSING;
      doorStepper.moveTo(0);
    }
  } else { // Logika mode auto
    handleAutoMode();
  }

  handleAutoClose();
  
  // Logika switch untuk mengontrol status pintu secara keseluruhan
  switch (doorState) {
    case DOOR_OPENING:
      if (doorStepper.distanceToGo() == 0) {
        doorState = DOOR_STOP;
        doorStepper.setCurrentPosition(steps_per_door);
        if (digitalReadFiltered(SW_INSPECTION, SW_INSPECTION_IDX) != LOW) {
          doorOpenTime = millis();
          autoCloseActive = true;
        }
      }
      // Tambahan: Jika tombol tutup ditekan saat membuka, langsung tutup
      else if (digitalReadFiltered(BTN_CLOSE, BTN_CLOSE_IDX) == LOW) {
          doorState = DOOR_CLOSING;
          doorStepper.moveTo(0);
          autoCloseActive = false;
      }
      break;
    case DOOR_CLOSING:
      if (doorStepper.distanceToGo() == 0) {
        doorState = DOOR_STOP;
        doorStepper.setCurrentPosition(0);
      }
      // Tambahan: Jika tombol buka ditekan saat menutup, langsung buka
      else if (digitalReadFiltered(BTN_OPEN, BTN_OPEN_IDX) == LOW) {
          doorState = DOOR_OPENING;
          doorStepper.moveTo(steps_per_door);
      }
      break;
    case DOOR_STOP:
      // Logika ini sudah ditangani di dalam if/else mode inspeksi dan auto
      break;
  }
  
  doorStepper.run();
  updateDisplay();
}