#include <AccelStepper.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.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
#define SW_AUTO 5
// === UART (CTB MCC) ===
#define RXD2 16
#define TXD2 17
// === 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};
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; // 3 detik

// Homing variables
long steps_per_door = 0; // Variabel untuk menyimpan langkah hasil kalibrasi
bool is_calibrated = false;

// Debounce variables
const unsigned long DEBOUNCE_DELAY = 50; // 50ms
unsigned long lastDebounceTimeBtnOpen = 0;
int lastBtnOpenState = HIGH;
unsigned long lastDebounceTimeBtnClose = 0;
int lastBtnCloseState = HIGH;
unsigned long lastDebounceTimeBtnDirUp = 0;
int lastBtnDirUpState = HIGH;
unsigned long lastDebounceTimeBtnDirDown = 0;
int lastBtnDirDownState = HIGH;
unsigned long lastDebounceTimeBtnEmergency = 0;
int lastBtnEmergencyState = HIGH;
unsigned long lastDebounceTimeSwInspection = 0;
int lastSwInspectionState = HIGH;
unsigned long lastDebounceTimeSwAuto = 0;
int lastSwAutoState = HIGH;
unsigned long lastDebounceTimeLimitDoorOpen = 0;
int lastLimitDoorOpenState = HIGH;
unsigned long lastDebounceTimeLimitDoorClose = 0;
int lastLimitDoorCloseState = HIGH;
unsigned long lastDebounceTimeFinalLimitUp = 0;
int lastFinalLimitUpState = HIGH;
unsigned long lastDebounceTimeFinalLimitDown = 0;
int lastFinalLimitDownState = HIGH;


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


// Door stepper params (tune to your mechanism)
const float MAX_SPEED = 900.0f;
const float MIN_SPEED = 200.0f;
const float ACCEL = 600.0f;
const long SLOWDOWN_ZONE = 200L;


// ================== FUNCTIONS ==================

int debouncedDigitalRead(int pin, unsigned long* lastDebounceTime, int* lastState) {
  int reading = digitalRead(pin);
  if (reading != *lastState) {
    *lastDebounceTime = millis();
  }
  *lastState = reading;
  if ((millis() - *lastDebounceTime) > DEBOUNCE_DELAY) {
    return reading;
  }
  return *lastState;
}

void homingAndCalibration() {
  Serial.println("Memulai proses homing dan kalibrasi...");
  
  // Jika pintu sudah tertutup saat startup, lewati kalibrasi
  if (debouncedDigitalRead(LIMIT_DOOR_CLOSE, &lastDebounceTimeLimitDoorClose, &lastLimitDoorCloseState) == LOW) {
    Serial.println("Pintu sudah tertutup. Kalibrasi homing dilewati.");
    doorStepper.setCurrentPosition(0);
    // Asumsi nilai default jika kalibrasi dilewati
    steps_per_door = 1000; 
    is_calibrated = true;
    return;
  }

  // Langkah 1: Gerak ke arah 'tutup' sampai limit switch tertutup tersentuh (posisi 0)
  Serial.println("Mencari posisi 0 (pintu tertutup)...");
  doorStepper.setMaxSpeed(MIN_SPEED); // Gunakan kecepatan rendah untuk homing
  doorStepper.setAcceleration(ACCEL);
  doorStepper.moveTo(-20000); // Gerak ke arah negatif (tutup)
  
  while(debouncedDigitalRead(LIMIT_DOOR_CLOSE, &lastDebounceTimeLimitDoorClose, &lastLimitDoorCloseState) == HIGH) {
    doorStepper.run();
  }
  
  doorStepper.stop();
  doorStepper.setCurrentPosition(0); // Atur posisi ini sebagai 0
  Serial.println("Posisi 0 ditemukan.");

  // Langkah 2: Gerak ke arah 'buka' dan hitung langkahnya
  Serial.println("Mengukur langkah dari pintu tutup ke pintu buka...");
  doorStepper.setMaxSpeed(MAX_SPEED);
  doorStepper.setAcceleration(ACCEL);
  doorStepper.moveTo(20000); // Gerak ke arah positif (buka)
  
  long start_position = doorStepper.currentPosition();
  while(debouncedDigitalRead(LIMIT_DOOR_OPEN, &lastDebounceTimeLimitDoorOpen, &lastLimitDoorOpenState) == HIGH) {
    doorStepper.run();
  }
  
  doorStepper.stop();
  long end_position = doorStepper.currentPosition();
  doorStepper.setCurrentPosition(end_position);
  
  // Simpan nilai langkah
  steps_per_door = end_position - start_position;
  is_calibrated = true;
  
  Serial.print("Kalibrasi selesai. Steps per door: ");
  Serial.println(steps_per_door);
}

void printStatusToSerial() {
  bool emergency = (debouncedDigitalRead(BTN_EMERGENCY, &lastDebounceTimeBtnEmergency, &lastBtnEmergencyState) == LOW);
  const char* modeStr = (debouncedDigitalRead(SW_INSPECTION, &lastDebounceTimeSwInspection, &lastSwInspectionState) == LOW) ? "INSPECTION" : "AUTO";
  const char* cabinStr = (cabinState == CABIN_UP) ? "UP" : (cabinState == CABIN_DOWN) ? "DOWN" : "IDLE";
  const char* doorStr = (doorState == DOOR_OPENING) ? "OPENING" : (doorState == DOOR_CLOSING) ? "CLOSING" : "IDLE";
  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");
}

void displayModeInfo(const char* mode, const char* cabinDir, const char* doorStateStr, bool emergency) {
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(3, 0);
  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(":");
  display.setCursor(31, 44);
  display.print(doorStateStr);

  if ((doorState == DOOR_OPENING || doorState == DOOR_STOP) && autoCloseActive) {
    unsigned long elapsed = millis() - doorOpenTime;
    if (elapsed < autoCloseDelay) {
      int remaining = (autoCloseDelay - elapsed) / 1000 + 1;
      int lineX = 56;
      display.drawLine(lineX, 42, lineX, 56, SSD1306_WHITE);
      display.setCursor(lineX + 4, 44);
      display.print("Close in ");
      display.print(remaining);
      display.print("s");
    }
  }

  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(82, 56);
    display.print("EMS OFF");
  }
  display.display();
}


void updateDisplay() {
  if (millis() - lastDisplayUpdateTime >= displayUpdateInterval) {
    lastDisplayUpdateTime = millis();

    bool emergency = (debouncedDigitalRead(BTN_EMERGENCY, &lastDebounceTimeBtnEmergency, &lastBtnEmergencyState) == LOW);
    const char* modeStr = (debouncedDigitalRead(SW_INSPECTION, &lastDebounceTimeSwInspection, &lastSwInspectionState) == LOW) ? "INSPECTION" : "AUTO";
    const char* cabinStr = (cabinState == CABIN_UP) ? "UP" : (cabinState == CABIN_DOWN) ? "DOWN" : "IDLE";
    const char* doorStr = (doorState == DOOR_OPENING) ? "OPENING" : (doorState == DOOR_CLOSING) ? "CLOSING" : "IDLE";
    displayModeInfo(modeStr, cabinStr, doorStr, 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);
  }
  
  // Menambahkan pesan setelah startup selesai
  Serial.println("Startup selesai. Sistem siap.");
}

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


// ================== SETUP ==================
void setup() {
  Serial.begin(115200);
  Serial.println("Sistem dinyalakan."); // Pesan awal di serial monitor

  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(SW_AUTO, INPUT_PULLUP);
  pinMode(RELAY_MOTOR_UP, OUTPUT);
  pinMode(RELAY_MOTOR_DOWN, OUTPUT);

  stopCabinMotor();

  // Stepper settings
  pinMode(PIN_ENABLE, OUTPUT);
  digitalWrite(PIN_ENABLE, LOW);
  doorStepper.setMaxSpeed(MAX_SPEED);
  doorStepper.setAcceleration(ACCEL);
  
  // OLED init
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("Alokasi SSD1306 gagal"));
    for (;;);
  }

  showStartup();
  
  // Panggil fungsi homing dan kalibrasi di sini
  homingAndCalibration();

  updateDisplay();
}


// ================== LOOP ==================
void loop() {
  bool emergency = (debouncedDigitalRead(BTN_EMERGENCY, &lastDebounceTimeBtnEmergency, &lastBtnEmergencyState) == LOW);

  // Emergency stop
  if (emergency) {
    stopCabinMotor();
    doorStepper.stop();
    doorState = DOOR_STOP;
    updateDisplay();
    return;
  }
  
  // Pastikan sistem sudah dikalibrasi
  if (!is_calibrated) {
    homingAndCalibration();
  }

  // MODE INSPECTION
  if (debouncedDigitalRead(SW_INSPECTION, &lastDebounceTimeSwInspection, &lastSwInspectionState) == LOW) {
    if (debouncedDigitalRead(BTN_DIR_UP, &lastDebounceTimeBtnDirUp, &lastBtnDirUpState) == LOW && debouncedDigitalRead(FINAL_LIMIT_UP, &lastDebounceTimeFinalLimitUp, &lastFinalLimitUpState) == HIGH) {
      digitalWrite(RELAY_MOTOR_UP, HIGH);
      digitalWrite(RELAY_MOTOR_DOWN, LOW);
      cabinState = CABIN_UP;
    }
    else if (debouncedDigitalRead(BTN_DIR_DOWN, &lastDebounceTimeBtnDirDown, &lastBtnDirDownState) == LOW && debouncedDigitalRead(FINAL_LIMIT_DOWN, &lastDebounceTimeFinalLimitDown, &lastFinalLimitDownState) == HIGH) {
      digitalWrite(RELAY_MOTOR_DOWN, HIGH);
      digitalWrite(RELAY_MOTOR_UP, LOW);
      cabinState = CABIN_DOWN;
    }
    else {
      stopCabinMotor();
    }
  } else {
    stopCabinMotor();
  }


  // DOOR OPERATOR
  switch (doorState) {
    case DOOR_STOP:
      if (debouncedDigitalRead(BTN_OPEN, &lastDebounceTimeBtnOpen, &lastBtnOpenState) == LOW) {
        // Pintu membuka.
        doorState = DOOR_OPENING;
        doorStepper.moveTo(steps_per_door);
        autoCloseActive = false;
      }
      else if (debouncedDigitalRead(BTN_CLOSE, &lastDebounceTimeBtnClose, &lastBtnCloseState) == LOW) {
        // Pintu menutup.
        doorState = DOOR_CLOSING;
        doorStepper.moveTo(0);
      }
      break;

    case DOOR_OPENING:
      if (doorStepper.distanceToGo() == 0) {
        // Pintu sudah terbuka penuh.
        doorState = DOOR_STOP;
        doorStepper.setCurrentPosition(steps_per_door);
        if (debouncedDigitalRead(SW_INSPECTION, &lastDebounceTimeSwInspection, &lastSwInspectionState) != LOW) {
          doorOpenTime = millis();
          autoCloseActive = true;
        }
      }
      else if (debouncedDigitalRead(BTN_CLOSE, &lastDebounceTimeBtnClose, &lastBtnCloseState) == LOW) {
        // Tombol CLOSE ditekan saat pintu sedang membuka, override ke CLOSING.
        doorState = DOOR_CLOSING;
        doorStepper.moveTo(0);
        autoCloseActive = false;
      }
      break;

    case DOOR_CLOSING:
      if (doorStepper.distanceToGo() == 0) {
        // Pintu sudah tertutup rapat.
        doorState = DOOR_STOP;
        doorStepper.setCurrentPosition(0);
      }
      else if (debouncedDigitalRead(BTN_OPEN, &lastDebounceTimeBtnOpen, &lastBtnOpenState) == LOW) {
        // Tombol OPEN ditekan saat pintu sedang menutup, override ke OPENING.
        doorState = DOOR_OPENING;
        doorStepper.moveTo(steps_per_door);
      }
      break;
  }

  // Auto-close process
  if (autoCloseActive && debouncedDigitalRead(SW_INSPECTION, &lastDebounceTimeSwInspection, &lastSwInspectionState) != LOW) {
    if (millis() - doorOpenTime >= autoCloseDelay) {
      doorState = DOOR_CLOSING;
      doorStepper.moveTo(0);
      autoCloseActive = false;
    }
  }

  // Jalankan motor stepper
  doorStepper.run();
  
  updateDisplay();
}