#include <avr/wdt.h>

// ================= DEBUG LEVEL =================
#define DEBUG_NONE    0
#define DEBUG_ERROR   1
#define DEBUG_INFO    2
#define DEBUG_DEBUG   3

constexpr uint8_t DEBUG_LEVEL = DEBUG_INFO;        // Ubah sesuai kebutuhan
constexpr uint32_t LOG_THROTTLE_MS = 200;          // Jeda minimal antar pesan

// ================= MACRO LOGGING =================
uint32_t last_log_time = 0;

#define LOG(level, msg) \
  do { \
    if (DEBUG_LEVEL >= level) { \
      uint32_t now = millis(); \
      if (now - last_log_time >= LOG_THROTTLE_MS) { \
        Serial.print(F("[")); \
        Serial.print(millis()); \
        Serial.print(F("] ")); \
        Serial.println(F(msg)); \
        last_log_time = now; \
      } \
    } \
  } while(0)

#define LOG_DEBUG(msg)    LOG(DEBUG_DEBUG, msg)
#define LOG_INFO(msg)     LOG(DEBUG_INFO,  msg)
#define LOG_WARNING(msg)  LOG(DEBUG_INFO,  msg)
#define LOG_ERROR(msg)    LOG(DEBUG_ERROR, msg)

// ================= MOTOR COMMAND =================
enum MotorCommand { CMD_NONE, CMD_STOP, CMD_UP, CMD_DOWN };

// ================= CONFIG =================
constexpr uint8_t JUMLAH_LANTAI      = 4;
constexpr uint8_t SCAN_TIME          = 8;
constexpr uint8_t DEBOUNCE_COUNT     = 6;
constexpr uint32_t MAX_TRAVEL_TIME   = 12000;
constexpr uint32_t DOOR_OPEN_TIME    = 6000;
constexpr uint32_t DOOR_DWELL_TIME   = 5000;
constexpr uint32_t DOOR_CLOSE_TIME   = 4000;
constexpr uint32_t TRANSIENT_TOLERANCE = 500;
constexpr uint32_t MOTOR_INTERLOCK_DELAY = 50;

// ================= PIN MAPPING =================
#define LS_ACTIVE(i)      ((i < JUMLAH_LANTAI) ? (!(PINC & _BV(i))) : false)

bool readButton(uint8_t i) {
  switch(i) {
    case 0: return !(PIND & _BV(2));
    case 1: return !(PIND & _BV(3));
    case 2: return !(PIND & _BV(4));
    case 3: return !(PIND & _BV(5));
    default: return false;
  }
}

#define SAFETY_OK()       (PIND & _BV(6))
#define DOOR_OPEN_LS()    (!(PINC & _BV(4)))
#define DOOR_CLOSE_LS()   (!(PINC & _BV(5)))

// Tombol manual baru
#define BUTTON_OPEN()     (!(PINC & _BV(6)))   // A6 = PC6
#define BUTTON_CLOSE()    (!(PINC & _BV(7)))   // A7 = PC7

// Outputs
#define RELAY_UP_ON()     (PORTB |=  _BV(0))
#define RELAY_UP_OFF()    (PORTB &= ~_BV(0))
#define RELAY_DN_ON()     (PORTB |=  _BV(1))
#define RELAY_DN_OFF()    (PORTB &= ~_BV(1))
#define RELAY_BRAKE_ON()  (PORTD |=  _BV(7))
#define RELAY_BRAKE_OFF() (PORTD &= ~_BV(7))
#define RELAY_OPEN_ON()   (PORTB |=  _BV(2))
#define RELAY_OPEN_OFF()  (PORTB &= ~_BV(2))
#define RELAY_CLOSE_ON()  (PORTB |=  _BV(3))
#define RELAY_CLOSE_OFF() (PORTB &= ~_BV(3))
#define RELAY_LAMP_ON()   (PORTB |=  _BV(4))
#define RELAY_LAMP_OFF()  (PORTB &= ~_BV(4))
#define BUZZER_ON()       (PORTB |=  _BV(5))
#define BUZZER_OFF()      (PORTB &= ~_BV(5))

// ================= MOTOR COMMAND FUNCTIONS =================
MotorCommand pending_motor_cmd = CMD_NONE;
MotorCommand current_motor_cmd = CMD_STOP;
uint32_t motor_interlock_timer = 0;

void requestMotorCommand(MotorCommand cmd) {
  if (cmd == current_motor_cmd && pending_motor_cmd == CMD_NONE) return;

  if (cmd == CMD_STOP) {
    RELAY_UP_OFF();
    RELAY_DN_OFF();
    RELAY_BRAKE_ON();
    current_motor_cmd = CMD_STOP;
    pending_motor_cmd = CMD_NONE;
    motor_interlock_timer = 0;
    return;
  }

  if (pending_motor_cmd != CMD_NONE) return;

  RELAY_UP_OFF();
  RELAY_DN_OFF();

  pending_motor_cmd = cmd;
  motor_interlock_timer = millis();
}

void updateMotor() {
  if (pending_motor_cmd == CMD_NONE) return;
  if (millis() - motor_interlock_timer < MOTOR_INTERLOCK_DELAY) return;

  if (pending_motor_cmd == CMD_UP) {
    RELAY_BRAKE_OFF();
    RELAY_UP_ON();
    current_motor_cmd = CMD_UP;
  } else if (pending_motor_cmd == CMD_DOWN) {
    RELAY_BRAKE_OFF();
    RELAY_DN_ON();
    current_motor_cmd = CMD_DOWN;
  }
  pending_motor_cmd = CMD_NONE;
}

// ================= STATE =================
enum State {
  IDLE, MOVING_UP, MOVING_DOWN, DOOR_OPENING, DOOR_OPENED, DOOR_CLOSING, FAULT
};

State state = FAULT;
bool fault_latched = false;

uint8_t current_pos = 1;
uint8_t target_pos  = 1;
bool has_target = false;

bool btn_state[JUMLAH_LANTAI] = {};
bool btn_prev[JUMLAH_LANTAI] = {};
bool door_open_state = false;
bool door_close_state = false;
bool safety_ok = true;

// Debounce untuk tombol manual
uint8_t db_btn_open = 0, db_btn_close = 0;

uint8_t db_ls[JUMLAH_LANTAI] = {};
uint8_t db_btn[JUMLAH_LANTAI] = {};
uint8_t db_door_open = 0, db_door_close = 0;
uint8_t db_safety = 0;

uint32_t move_start = 0;
uint32_t door_timer = 0;
uint32_t door_open_start = 0;
uint32_t transient_start = 0;

// ================= HELPER =================
bool debounce(bool raw, uint8_t &cnt) {
  if (raw) {
    if (cnt < DEBOUNCE_COUNT) cnt++;
  } else {
    cnt = 0;
  }
  return cnt >= DEBOUNCE_COUNT;
}

// ================= SETUP =================
void setup() {
  Serial.begin(115200);
  Serial.println(F("=== Sistem Lift 4 Lantai + Tombol Manual Open/Close ==="));

  DDRB |= 0b00111111;        // D8-D13 output
  DDRD |= _BV(7);            // D7 brake output
  DDRD &= ~0b01111100;       // D2-D6 input
  PORTD |= 0b01111100;       // pull-up D2-D6
  
  DDRC &= ~0b11111111;       // A0-A7 semua input
  PORTC |= 0b11111111;       // pull-up A0-A7

  RELAY_LAMP_ON();
  requestMotorCommand(CMD_STOP);
  RELAY_OPEN_OFF();
  RELAY_CLOSE_OFF();
  BUZZER_OFF();

  wdt_enable(WDTO_2S);
  initPosition();

  LOG_INFO("Setup selesai. Tombol manual Open (A6) dan Close (A7) aktif.");
}

// ================= INIT POSITION =================
void initPosition() {
  uint8_t found = 0;
  for (uint8_t i = 0; i < JUMLAH_LANTAI; i++) {
    if (LS_ACTIVE(i)) {
      current_pos = i + 1;
      found++;
    }
  }
  if (found != 1) {
    state = FAULT;
    fault_latched = true;
    LOG_ERROR("Gagal inisialisasi posisi: jumlah LS aktif tidak tepat");
    return;
  }

  door_open_state  = DOOR_OPEN_LS();
  door_close_state = DOOR_CLOSE_LS();
  state = (door_open_state && !door_close_state) ? DOOR_OPENED : IDLE;
  target_pos = current_pos;
  has_target = false;

  LOG_INFO("Posisi awal berhasil dideteksi di lantai ");
  Serial.println(current_pos);
}

// ================= SCAN INPUTS =================
void scanInputs() {
  uint32_t now = millis();

  safety_ok = debounce(SAFETY_OK(), db_safety);

  bool open_raw  = DOOR_OPEN_LS();
  bool close_raw = DOOR_CLOSE_LS();

  door_open_state  = debounce(open_raw,  db_door_open);
  door_close_state = debounce(close_raw, db_door_close);

  // Transient detection
  if (door_open_state && door_close_state) {
    if (transient_start == 0) transient_start = now;
  } else {
    transient_start = 0;
  }

  // Floor LS
  uint8_t found = 0;
  for (uint8_t i = 0; i < JUMLAH_LANTAI; i++) {
    if (debounce(LS_ACTIVE(i), db_ls[i])) {
      current_pos = i + 1;
      found++;
    }
  }

  // Tombol lantai biasa
  bool any_button_rising = false;
  for (uint8_t i = 0; i < JUMLAH_LANTAI; i++) {
    bool pressed = debounce(readButton(i), db_btn[i]);
    btn_state[i] = pressed;

    if (pressed && !btn_prev[i]) {
      any_button_rising = true;
      target_pos = i + 1;
      has_target = true;
      LOG_DEBUG("Tombol lantai ditekan: ");
      Serial.println(i + 1);

      if (state == DOOR_CLOSING) {
        state = DOOR_OPENING;
        door_timer = now;
      } else if (state == DOOR_OPENED) {
        door_open_start = 0;
      }
    }
    btn_prev[i] = pressed;
  }

  // === TOMBOL MANUAL OPEN & CLOSE (dengan interlock) ===
  bool btn_open_pressed  = debounce(BUTTON_OPEN(),  db_btn_open);
  bool btn_close_pressed = debounce(BUTTON_CLOSE(), db_btn_close);

  if (btn_open_pressed && !fault_latched) {
    if (state == IDLE || state == DOOR_OPENED || state == DOOR_CLOSING) {
      if (state != DOOR_OPENING) {
        state = DOOR_OPENING;
        door_timer = now;
        LOG_INFO("Tombol MANUAL OPEN ditekan - Pintu dibuka paksa");
      }
    } else {
      LOG_WARNING("Tombol OPEN ditolak: lift sedang bergerak");
    }
  }

  if (btn_close_pressed && !fault_latched) {
    if (state == DOOR_OPENED || state == DOOR_OPENING) {
      state = DOOR_CLOSING;
      door_timer = now;
      LOG_INFO("Tombol MANUAL CLOSE ditekan - Pintu ditutup paksa");
    } else {
      LOG_WARNING("Tombol CLOSE ditolak: kondisi tidak memungkinkan");
    }
  }

  // Fault detection
  bool transient_fault = (door_open_state && door_close_state && (now - transient_start > TRANSIENT_TOLERANCE));
  bool fault_condition = !safety_ok || transient_fault || (found != 1);

  if (fault_condition) {
    if (state != FAULT) LOG_ERROR("Fault terdeteksi!");
    state = FAULT;
    fault_latched = true;
  } else if (fault_latched) {
    if (any_button_rising) {
      fault_latched = false;
      state = (door_open_state && !door_close_state) ? DOOR_OPENED : IDLE;
      if (state == DOOR_OPENED) door_open_start = now;
      LOG_INFO("Fault direset oleh tombol panggil");
    }
  }
}

// ================= PROCESS LOGIC =================
void processLogic() {
  uint32_t now = millis();

  if (fault_latched) return;

  if (state == MOVING_UP || state == MOVING_DOWN) {
    if (now - move_start > MAX_TRAVEL_TIME || !door_close_state) {
      requestMotorCommand(CMD_STOP);
      state = FAULT;
      fault_latched = true;
      LOG_ERROR("Timeout perjalanan atau pintu tidak tertutup");
      return;
    }
    if (current_pos == target_pos) {
      has_target = false;
      state = DOOR_OPENING;
      door_timer = now;
      requestMotorCommand(CMD_STOP);
      LOG_INFO("Sampai di lantai tujuan: ");
      Serial.println(current_pos);
    }
  }

  switch (state) {
    case IDLE:
      if (has_target && target_pos != current_pos) {
        state = (target_pos > current_pos) ? MOVING_UP : MOVING_DOWN;
        move_start = now;
        LOG_INFO("Mulai bergerak ke lantai ");
        Serial.println(target_pos);
      }
      break;

    case DOOR_OPENING:
      if (now - door_timer > DOOR_OPEN_TIME || door_open_state) {
        state = DOOR_OPENED;
        door_open_start = now;
        LOG_INFO("Pintu selesai membuka");
      }
      break;

    case DOOR_OPENED:
      if (door_open_start == 0) door_open_start = now;
      if (has_target && (now - door_open_start > DOOR_DWELL_TIME)) {
        state = DOOR_CLOSING;
        door_timer = now;
        door_open_start = 0;
        LOG_INFO("Waktu tunggu selesai, mulai menutup pintu");
      }
      break;

    case DOOR_CLOSING:
      if (now - door_timer > DOOR_CLOSE_TIME || door_close_state) {
        state = IDLE;
        LOG_INFO("Pintu selesai menutup");
      }
      break;

    default: break;
  }
}

// ================= OUTPUT =================
void updateOutput() {
  uint32_t now = millis();

  if (fault_latched) {
    if ((now / 300) % 2) {
      RELAY_LAMP_ON();
      BUZZER_ON();
    } else {
      RELAY_LAMP_OFF();
      BUZZER_OFF();
    }
    requestMotorCommand(CMD_STOP);
    return;
  }

  MotorCommand desired = CMD_STOP;
  if (state == MOVING_UP)   desired = CMD_UP;
  if (state == MOVING_DOWN) desired = CMD_DOWN;

  requestMotorCommand(desired);

  switch (state) {
    case DOOR_OPENING:
      RELAY_OPEN_ON();
      RELAY_CLOSE_OFF();
      break;
    case DOOR_CLOSING:
      RELAY_CLOSE_ON();
      RELAY_OPEN_OFF();
      break;
    default:
      RELAY_OPEN_OFF();
      RELAY_CLOSE_OFF();
      RELAY_LAMP_ON();
      BUZZER_OFF();
      break;
  }
}

// ================= MAIN LOOP =================
void loop() {
  static uint32_t last_scan = 0;
  uint32_t now = millis();

  if (now - last_scan < SCAN_TIME) return;
  last_scan = now;

  wdt_reset();

  scanInputs();
  processLogic();
  updateMotor();
  updateOutput();
}