// --- PHẦN 1: KHAI BÁO (Không thay đổi) ---
#include <ESP32Servo.h>
#define MODE_BUTTON_PIN 4
#define START_BUTTON_PIN 2
#define STATUS_LED_PIN 10
#define SERVO_STRIP_PIN 18
#define SERVO_SHRINK_PIN 19
#define SERVO_HEAT_PIN 1
#define RELAY_MAIN_LIFT_PIN 0
#define RELAY_PLATFORM_LIFT_PIN 5
#define RELAY_STRIP_PIN 6
#define RELAY_TWIST_PIN 7
#define RELAY_HEAT_PIN 8

Servo servoStrip, servoShrink, servoHeat;

enum MachineState {
  STATE_HIDDEN,         // 0: Máy đang ẩn
  STATE_OPENING_ALL,    // 1: Đang thực hiện chuỗi Mở Hoàn Toàn
  STATE_READY_TO_WORK,  // 2: Đã mở hoàn toàn, sẵn sàng làm việc
  STATE_WORKING,        // 3: Đang thực hiện quy trình nối dây
  STATE_CLOSING_ALL     // 4: Đang thực hiện chuỗi Đóng Hoàn Toàn
};
MachineState currentState = STATE_HIDDEN;
unsigned long modeButtonTime = 0;
unsigned long startButtonTime = 0;

// --- HÀM SETUP (Không thay đổi) ---
void setup() {
  Serial.begin(115200);
  pinMode(MODE_BUTTON_PIN, INPUT_PULLUP);
  pinMode(START_BUTTON_PIN, INPUT_PULLUP);
  pinMode(STATUS_LED_PIN, OUTPUT);
  pinMode(RELAY_MAIN_LIFT_PIN, OUTPUT);
  pinMode(RELAY_PLATFORM_LIFT_PIN, OUTPUT);
  pinMode(RELAY_STRIP_PIN, OUTPUT);
  pinMode(RELAY_TWIST_PIN, OUTPUT);
  pinMode(RELAY_HEAT_PIN, OUTPUT);
  servoStrip.attach(SERVO_STRIP_PIN);
  servoShrink.attach(SERVO_SHRINK_PIN);
  servoHeat.attach(SERVO_HEAT_PIN);
  digitalWrite(STATUS_LED_PIN, LOW);
  digitalWrite(RELAY_MAIN_LIFT_PIN, HIGH);
  digitalWrite(RELAY_PLATFORM_LIFT_PIN, HIGH);
  digitalWrite(RELAY_STRIP_PIN, HIGH);
  digitalWrite(RELAY_TWIST_PIN, HIGH);
  digitalWrite(RELAY_HEAT_PIN, HIGH);
  servoStrip.write(0); servoShrink.write(0); servoHeat.write(0);

  Serial.println("=========================================");
  Serial.println("MAY NOI DAY TU DONG KHOI DONG THANH CONG");
  Serial.println("Trang thai: May an duoi ban. Nhan nut [Che Do] de mo may.");
  Serial.println("=========================================");
}

// --- HÀM LOOP ---
void loop() {
  handleButtons();
  runStateMachine();
}

// --- HÀM XỬ LÝ NÚT BẤM (Đã được đơn giản hóa) ---
void handleButtons() {
  // --- Xử lý Nút Chế Độ (Mở/Đóng) ---
  if (digitalRead(MODE_BUTTON_PIN) == LOW && millis() - modeButtonTime > 1000) {
    modeButtonTime = millis();
    if (currentState == STATE_HIDDEN) {
      currentState = STATE_OPENING_ALL; // Bắt đầu chuỗi mở
    } else if (currentState == STATE_READY_TO_WORK) {
      currentState = STATE_CLOSING_ALL; // Bắt đầu chuỗi đóng
    }
  }

  // --- Xử lý Nút Bắt Đầu ---
  if (digitalRead(START_BUTTON_PIN) == LOW && millis() - startButtonTime > 1000) {
    startButtonTime = millis();
    if (currentState == STATE_READY_TO_WORK) {
      currentState = STATE_WORKING; // Chỉ hoạt động khi máy đã sẵn sàng
    }
  }
}

// --- HÀM MÁY TRẠNG THÁI (Logic mới) ---
void runStateMachine() {
  switch (currentState) {
    case STATE_OPENING_ALL:
      Serial.println("\n[LOG] Lenh mo may. Bat dau chuoi mo hoan toan...");
      digitalWrite(STATUS_LED_PIN, HIGH); // Báo bận

      // Bước 1: Nâng máy chính
      Serial.println("[LOG] -> Buoc 1: Nang may chinh len khoi mat ban...");
      digitalWrite(RELAY_MAIN_LIFT_PIN, LOW); // Bật motor nâng chính (LED Tím SÁNG)
      delay(4000);
      digitalWrite(RELAY_MAIN_LIFT_PIN, HIGH);
      Serial.println("[LOG]    -> May chinh da duoc nang xong.");
      delay(1000);

      // Bước 2: Nâng sàn phụ
      Serial.println("[LOG] -> Buoc 2: Mo san lam viec...");
      digitalWrite(RELAY_PLATFORM_LIFT_PIN, LOW); // Bật motor nâng phụ (LED Xanh Dương SÁNG)
      delay(3000);
      digitalWrite(RELAY_PLATFORM_LIFT_PIN, HIGH);
      Serial.println("[LOG]    -> San lam viec da mo xong.");
      
      Serial.println("\n[LOG] MAY DA SAN SANG. Hay dat day vao va nhan nut [Bat Dau].");
      currentState = STATE_READY_TO_WORK;
      break;

    case STATE_READY_TO_WORK:
      digitalWrite(STATUS_LED_PIN, !digitalRead(STATUS_LED_PIN)); // Nhấp nháy đèn báo sẵn sàng
      delay(250);
      break;

    case STATE_WORKING:
      digitalWrite(STATUS_LED_PIN, HIGH);
      runFullProcess();
      Serial.println("\n[LOG] QUY TRINH HOAN THANH!");
      Serial.println("[LOG] San sang cho lan lam viec tiep theo hoac nhan [Che Do] de dong may.");
      currentState = STATE_READY_TO_WORK;
      break;

    case STATE_CLOSING_ALL:
      Serial.println("\n[LOG] Lenh dong may. Bat dau chuoi dong hoan toan...");
      digitalWrite(STATUS_LED_PIN, HIGH); // Báo bận

      // Bước 1: Hạ sàn phụ
      Serial.println("[LOG] -> Buoc 1: Dong san lam viec...");
      digitalWrite(RELAY_PLATFORM_LIFT_PIN, LOW); // Bật motor nâng phụ (để hạ)
      delay(3000);
      digitalWrite(RELAY_PLATFORM_LIFT_PIN, HIGH);
      Serial.println("[LOG]    -> San lam viec da dong xong.");
      delay(1000);

      // Bước 2: Hạ máy chính
      Serial.println("[LOG] -> Buoc 2: Ha may chinh xuong duoi mat ban...");
      digitalWrite(RELAY_MAIN_LIFT_PIN, LOW); // Bật motor nâng chính (để hạ)
      delay(4000);
      digitalWrite(RELAY_MAIN_LIFT_PIN, HIGH);
      
      Serial.println("\n[LOG] May da duoc cat giau. Tro ve che do cho.");
      digitalWrite(STATUS_LED_PIN, LOW);
      currentState = STATE_HIDDEN;
      break;
  }
}

// Hàm runFullProcess() không thay đổi, giữ nguyên như cũ
void runFullProcess() {
  Serial.println("\n[LOG] Nhan lenh [Bat Dau]. Bat dau quy trinh noi day...");
  delay(1000);
  Serial.println("[LOG] -> Cong doan 1: Tuot vo day...");
  servoStrip.write(90); delay(500);
  digitalWrite(RELAY_STRIP_PIN, LOW); Serial.println("[LOG]    -> Day nhiet DANG BAT."); delay(2000);
  digitalWrite(RELAY_STRIP_PIN, HIGH); Serial.println("[LOG]    -> Day nhiet DA TAT.");
  servoStrip.write(0); delay(1000);
  Serial.println("[LOG] -> Cong doan 2: Xoan loi dong...");
  digitalWrite(RELAY_TWIST_PIN, LOW); Serial.println("[LOG]    -> Motor xoan DANG CHAY."); delay(3000);
  digitalWrite(RELAY_TWIST_PIN, HIGH); Serial.println("[LOG]    -> Motor xoan DA DUNG."); delay(1000);
  Serial.println("[LOG] -> Cong doan 3: Cat va dat gen co nhiet...");
  servoShrink.write(120); delay(2000);
  servoShrink.write(0); delay(1000);
  Serial.println("[LOG] -> Cong doan 4: Gia nhiet lam co gen...");
  servoHeat.write(80); delay(500);
  digitalWrite(RELAY_HEAT_PIN, LOW); Serial.println("[LOG]    -> Dau kho nhiet DANG BAT."); delay(4000);
  digitalWrite(RELAY_HEAT_PIN, HIGH); Serial.println("[LOG]    -> Dau kho nhiet DA TAT.");
  servoHeat.write(0); delay(1000);
}