#include <AccelStepper.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

// ================== PIN MAPPING ==================
#define PIN_STEP        11
#define PIN_DIR         10
#define PIN_ENABLE      5
#define LIMIT_DOOR_OPEN   A0
#define LIMIT_DOOR_CLOSE  A1
#define FINAL_LIMIT_UP    A6
#define FINAL_LIMIT_DOWN  A7
#define BTN_OPEN          6
#define BTN_CLOSE         9
#define BTN_DIR_UP        A2
#define BTN_DIR_DOWN      A3
#define BTN_EMERGENCY     2
#define SW_INSPECTION     3
#define SW_AUTO           4
#define RXD               0
#define TXD               1
#define I2C_SDA           21
#define I2C_SCL           22
#define RELAY_MOTOR_UP    7
#define RELAY_MOTOR_DOWN  8

// ================== 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;

unsigned long doorOpenTime = 0;
const unsigned long autoCloseDelay = 3000; // 3 detik
bool autoCloseActive = false;

// ================== FUNCTIONS ==================
void drawProgressBar(uint8_t progress) {
  display.drawRect(0, 56, 128, 8, SSD1306_WHITE); // border
  display.fillRect(0, 56, progress, 8, SSD1306_WHITE); // isi
}

void updateDisplay() {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);

  // Mode
  display.setCursor(0, 0);
  display.print("Mode: ");
  if (digitalRead(SW_INSPECTION) == LOW) display.print("INSPECT");
  else display.print("AUTO");

  // Cabin
  display.setCursor(0, 16);
  display.print("Cabin: ");
  if (cabinState == CABIN_UP) display.print("UP");
  else if (cabinState == CABIN_DOWN) display.print("DOWN");
  else display.print("STOP");

  // Door
  display.setCursor(0, 32);
  display.print("Door: ");
  if (doorState == DOOR_OPENING) display.print("OPENING");
  else if (doorState == DOOR_CLOSING) display.print("CLOSING");
  else display.print("STOPPED");

  // Emergency
  display.setCursor(0, 48);
  display.print("EMG: ");
  if (digitalRead(BTN_EMERGENCY) == LOW) display.print("ACTIVE");
  else display.print("OK");

  // Progress bar auto-close hanya di mode AUTO
  if (autoCloseActive && digitalRead(SW_INSPECTION) != LOW) {
    unsigned long elapsed = millis() - doorOpenTime;
    uint8_t progress = map(elapsed, 0, autoCloseDelay, 0, 128);
    if (progress > 128) progress = 128;
    drawProgressBar(progress);
  }

  display.display();
}

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

void setup() {
  // Pin Modes
  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);
  pinMode(PIN_ENABLE, OUTPUT);

  stopCabinMotor();
  digitalWrite(PIN_ENABLE, LOW);

  doorStepper.setMaxSpeed(1000);
  doorStepper.setAcceleration(500);

  // I2C init multi-board
  #if defined(ESP32)
    Wire.begin(I2C_SDA, I2C_SCL);
  #else
    Wire.begin();
  #endif

  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.display();

  updateDisplay();
}

void loop() {
  bool emergency = (digitalRead(BTN_EMERGENCY) == LOW);

  // Emergency stop
  if (emergency) {
    stopCabinMotor();
    doorStepper.stop();
    doorState = DOOR_STOP;
    autoCloseActive = false;
    updateDisplay();
    return;
  }

  // MODE INSPECTION
  if (digitalRead(SW_INSPECTION) == LOW) {
    if (digitalRead(BTN_DIR_UP) == LOW && digitalRead(FINAL_LIMIT_UP) == HIGH) {
      digitalWrite(RELAY_MOTOR_UP, HIGH);
      digitalWrite(RELAY_MOTOR_DOWN, LOW);
      cabinState = CABIN_UP;
    }
    else if (digitalRead(BTN_DIR_DOWN) == LOW && digitalRead(FINAL_LIMIT_DOWN) == 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 (digitalRead(BTN_OPEN) == LOW && digitalRead(LIMIT_DOOR_OPEN) == HIGH) {
        doorState = DOOR_OPENING;
        doorStepper.setSpeed(500);
      }
      else if (digitalRead(BTN_CLOSE) == LOW && digitalRead(LIMIT_DOOR_CLOSE) == HIGH) {
        doorState = DOOR_CLOSING;
        doorStepper.setSpeed(-500);
      }
      break;

    case DOOR_OPENING:
      if (digitalRead(LIMIT_DOOR_OPEN) == LOW) {
        doorState = DOOR_STOP;
        doorStepper.setSpeed(0);

        // Aktifkan auto-close jika di mode AUTO
        if (digitalRead(SW_INSPECTION) != LOW) {
          doorOpenTime = millis();
          autoCloseActive = true;
        }
      } 
      else if (digitalRead(BTN_CLOSE) == LOW && digitalRead(LIMIT_DOOR_CLOSE) == HIGH) {
        doorState = DOOR_CLOSING;
        doorStepper.setSpeed(-500);
      } 
      else {
        doorStepper.runSpeed();
      }
      break;

    case DOOR_CLOSING:
      if (digitalRead(LIMIT_DOOR_CLOSE) == LOW) {
        doorState = DOOR_STOP;
        doorStepper.setSpeed(0);
        autoCloseActive = false;
      } 
      else if (digitalRead(BTN_OPEN) == LOW && digitalRead(LIMIT_DOOR_OPEN) == HIGH) {
        doorState = DOOR_OPENING;
        doorStepper.setSpeed(500);
        autoCloseActive = false;
      } 
      else {
        doorStepper.runSpeed();
      }
      break;
  }

  // Auto-close process
  if (autoCloseActive && digitalRead(SW_INSPECTION) != LOW) {
    if (millis() - doorOpenTime >= autoCloseDelay) {
      doorState = DOOR_CLOSING;
      doorStepper.setSpeed(-500);
      autoCloseActive = false;
    }
  }

  updateDisplay();
}