//THEM COI, QUAY LẠI, chuyển sang NEWTON//
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include "HX711.h"
#include <avr/wdt.h>
// -------------------- Khai báo chung --------------------
#define BUTTON_NEXT 8       // Nút chuyển chế độ
#define BUTTON_SELECT 9     // Nút xác nhận chế độ
#define BUTTON_ADJUST 4     // Nút điều chỉnh
#define BUTTON_MODE 5       // Nút chuyển giữa các chế độ cài đặt
#define BUTTON_STARTSTOP 6  // Nút bắt đầu/dừng chương trình
#define BUTTON_BACK 7  // Nút dừng khẩn cấp

#define LED_PROGRAM1 A0
#define LED_PROGRAM2 A1

#define STEP_PIN 10         // Chân điều khiển xung động cơ
#define DIR_PIN 11          // Chân điều khiển chiều động cơ
#define EN_PIN 13           // Chân kích hoạt động cơ

#define LOADCELL_DOUT_PIN 3
#define LOADCELL_SCK_PIN 2

LiquidCrystal_I2C lcd(0x27, 16, 2); // Khởi tạo đối tượng LCD
HX711 scale;

int button7Pressed = 0;  // Biến đếm số lần nhấn nút 7
int selectedProgram = 0;    // Chương trình được chọn (0: Program 1, 1: Program 2)
bool programSelected = false;
bool isRunning = false;
bool holding = false;
bool movingToUpper = true;
bool returning = false;
bool isHolding = false;  // Theo dõi trạng thái đang giữ
bool returningToZero = false;  // Theo dõi trạng thái quay về Load Cell = 0
bool button7State = false;

unsigned long holdStartTime = 0;  // Thời điểm bắt đầu giữ
unsigned long treatmentStartTime = 0;  // Thời gian bắt đầu điều trị
long stepsTaken = 0;           // Số bước động cơ đã di chuyển

float currentWeight = 0.0;     // Trọng lượng hiện tại từ Load Cell
float calibration_factor = 500;

int microStep = 4;
float angleStep = 1.8;
float stepsPerRound = microStep * 360.0 / angleStep; // 800 bước/vòng động cơ
int gearboxRatio = 14; // Hộp số 14:1

long stepsPerOutputRound = gearboxRatio * stepsPerRound; // Số bước thực tế để trục đầu ra hộp số quay 1 vòng

int speedOptions[] = {25, 30, 35};  // Các giá trị tốc độ có thể chọn (cả Program 1 và Program 2)
int currentSpeedIndex = 0;  // Chỉ số tốc độ hiện tại (dùng chung cho cả hai chương trình)

float speeds = 1.0 / (speedOptions[currentSpeedIndex] / 60.0 * stepsPerRound) / 2.0 * 1000000;

// -------------------- Chương trình 1 --------------------
float weightThresholds[] = {2000, 2500, 3000};
int currentThresholdIndex = 0;

unsigned int holdTimes[] = {2, 3, 4};
int currentHoldTimeIndex = 0;

enum Mode { ADJUST_HOLD_TIME, ADJUST_THRESHOLD, ADJUST_SPEED };  // Thêm chế độ ADJUST_SPEED
Mode currentModeProgram1 = ADJUST_HOLD_TIME;

// -------------------- Chương trình 2 --------------------
float upperLimits[] = {1500, 2000, 2500};
int currentUpperLimitIndex = 0;

float lowerLimits[] = {3000, 3500, 4000};
int currentLowerLimitIndex = 0;

unsigned int treatmentTimes[] = {60, 90, 120};
int currentTreatmentTimeIndex = 0;

unsigned int holdTimeProgram2[] = {2, 3, 4}; // Thời gian giữ cho Program 2
int currentHoldTimeProgram2Index = 0;
enum Mode2 { ADJUST_UPPER_LIMIT, ADJUST_LOWER_LIMIT, ADJUST_TREATMENT_TIME, ADJUST_HOLD_TIME_PROGRAM2, ADJUST_SPEED_PROGRAM2 };  // Thêm chế độ ADJUST_SPEED_PROGRAM2
Mode2 currentModeProgram2 = ADJUST_UPPER_LIMIT;

// -------------------- Prototype các hàm --------------------
void resetToProgramSelection();
void updateProgramSelection();
void setupProgram1();
void loopProgram1();
void setupProgram2();
void loopProgram2();
void handleEmergencyStop();
void moveMotor(bool forward);
void moveMotorProgram2(bool forward);
void stopMotor();
void updateWeight();
void displayWeight();
void runHoldCycle();
void runHoldCycleProgram2();
void runTreatmentCycleProgram2();
void returnToStart(bool slow);
void returnToZeroWeight();  // Khai báo hàm returnToZeroWeight

// -------------------- Setup --------------------
void setup() {
  wdt_disable(); // Tắt Watchdog Timer khi khởi động
  delay(100); // Đợi một chút để đảm bảo bộ nhớ ổn định
  lcd.init();
  lcd.backlight();

  pinMode(BUTTON_NEXT, INPUT_PULLUP);
  pinMode(BUTTON_SELECT, INPUT_PULLUP);
  pinMode(BUTTON_ADJUST, INPUT_PULLUP);
  pinMode(BUTTON_MODE, INPUT_PULLUP);
  pinMode(BUTTON_STARTSTOP, INPUT_PULLUP);
  pinMode(BUTTON_BACK, INPUT_PULLUP);
  
  pinMode(LED_PROGRAM1, OUTPUT);
  pinMode(LED_PROGRAM2, OUTPUT);
  digitalWrite(LED_PROGRAM1, LOW); // Đảm bảo đèn tắt ban đầu
  digitalWrite(LED_PROGRAM2, LOW); 

  pinMode(STEP_PIN, OUTPUT);
  pinMode(DIR_PIN, OUTPUT);
  pinMode(EN_PIN, OUTPUT);
  digitalWrite(EN_PIN, HIGH); // Vô hiệu hóa động cơ ban đầu

  scale.begin(LOADCELL_DOUT_PIN, LOADCELL_SCK_PIN);
  scale.set_scale(calibration_factor);
  scale.tare();

  resetToProgramSelection();
}

// -------------------- Loop --------------------
void loop() {
    static bool waitingForRelease = false;

    if (digitalRead(BUTTON_BACK) == LOW && !waitingForRelease) {  
        delay(200);  
        waitingForRelease = true;

        if (!button7State) {
            stopMotor();
            lcd.clear();
            lcd.setCursor(0, 0);
            lcd.print("HOLD");
            button7State = true;
        } else {
            lcd.clear();
            lcd.setCursor(0, 0);
            lcd.print("Returning...");
            button7State = false;
            returnToZeroWeight(); // Gọi hàm quay về
        }
    }

    if (digitalRead(BUTTON_BACK) == HIGH && waitingForRelease) {
        waitingForRelease = false;
    }

    if (!button7State) { 
        if (selectedProgram == 0) {
            loopProgram1();
        } else {
            loopProgram2();
        }
    }
}

// -------------------- Program Selection --------------------
void resetToProgramSelection() {
  programSelected = false;
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Select Program:");
  updateProgramSelection();
  
  digitalWrite(LED_PROGRAM1, LOW);
  digitalWrite(LED_PROGRAM2, LOW); // Tắt cả hai đèn ban đầ

  while (!programSelected) {
    if (digitalRead(BUTTON_NEXT) == LOW) {
      delay(200);
      selectedProgram = (selectedProgram + 1) % 2; // Chuyển giữa Program 0 và 1
      updateProgramSelection();
    }
    if (digitalRead(BUTTON_SELECT) == LOW) {
      delay(200);
      programSelected = true; // Xác nhận lựa chọn
    }
  }

  lcd.clear();

  // Đặt lại các trạng thái ban đầu
  isRunning = false;
  holding = false;
  returning = false;
  stepsTaken = 0;
  currentThresholdIndex = 0;
  currentHoldTimeIndex = 0;
  currentUpperLimitIndex = 0;
  currentLowerLimitIndex = 0;
  currentTreatmentTimeIndex = 0;

  // Chuyển đến chương trình đã chọn
  if (selectedProgram == 0) {
    digitalWrite(LED_PROGRAM1, HIGH);
    digitalWrite(LED_PROGRAM2, LOW);
    setupProgram1(); // Chuyển đến thiết lập Program 1
    loopProgram1();  // Cho phép cài đặt thông số
  } else {
    digitalWrite(LED_PROGRAM1, LOW);
    digitalWrite(LED_PROGRAM2, HIGH);
    setupProgram2(); // Chuyển đến thiết lập Program 2
    loopProgram2();  // Cho phép cài đặt thông số
  }
}

void updateProgramSelection() {
  lcd.setCursor(0, 1);
  if (selectedProgram == 0) {
    lcd.print("> Program 1     ");
  } else {
    lcd.print("> Program 2     ");
  }
}

// -------------------- Program 1 --------------------
void setupProgram1() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Program 1 Setup");
  delay(500);  // Đợi để người dùng thấy thông báo
}

void loopProgram1() {
  if (!isRunning) {
    // Hiển thị giao diện cài đặt
    if (currentModeProgram1 == ADJUST_HOLD_TIME) {
      lcd.setCursor(0, 0);
      lcd.print("Set Hold Time    ");
      lcd.setCursor(0, 1);
      lcd.print((int)holdTimes[currentHoldTimeIndex]); // In giá trị số nguyên
      lcd.print("s    ");
    } else if (currentModeProgram1 == ADJUST_THRESHOLD) { 
      lcd.setCursor(0, 0);
      lcd.print("Set Threshold    ");
      lcd.setCursor(0, 1);
      lcd.print((int)weightThresholds[currentThresholdIndex]); 
      lcd.print("g    ");
    } else if (currentModeProgram1 == ADJUST_SPEED) {  // Điều chỉnh tốc độ
      lcd.setCursor(0, 0);
      lcd.print("Set Speed       ");
      lcd.setCursor(0, 1);
      lcd.print(speedOptions[currentSpeedIndex]);  // Hiển thị tốc độ
      lcd.print("RPM   ");
    }
    if (digitalRead(BUTTON_ADJUST) == LOW) {
      delay(200);
      if (currentModeProgram1 == ADJUST_HOLD_TIME) {
        currentHoldTimeIndex = (currentHoldTimeIndex + 1) % 3;
      } else if (currentModeProgram1 == ADJUST_THRESHOLD) {  
        currentThresholdIndex = (currentThresholdIndex + 1) % 3;
      } else if (currentModeProgram1 == ADJUST_SPEED) {  // Điều chỉnh tốc độ
        currentSpeedIndex = (currentSpeedIndex + 1) % 3;
        speeds = 1.0 / (speedOptions[currentSpeedIndex] / 60.0 * stepsPerRound) / 2.0 * 1000000;  // Cập nhật tốc độ
  }
}
    if (digitalRead(BUTTON_MODE) == LOW) {
      delay(200);
      currentModeProgram1 = (Mode)((currentModeProgram1 + 1) % 3);
    }
    // Bắt đầu chương trình
    if (digitalRead(BUTTON_STARTSTOP) == LOW) {
      delay(1000);
      isRunning = true;
      lcd.clear();
      lcd.print("Starting...");
      delay(500);
    }
  } else {
    // Logic vận hành chương trình
    updateWeight();
    displayWeight();

    if (!holding) {
      if (currentWeight < weightThresholds[currentThresholdIndex]) {
        moveMotor(stepsPerOutputRound, true); // Quay động cơ tiến lên
        stepsTaken += stepsPerOutputRound;
      } else {
        stopMotor(); // Dừng động cơ khi đạt ngưỡng trọng lượng
        holding = true;
        holdStartTime = millis(); // Ghi lại thời điểm bắt đầu giữ
      }
    }

    if (holding) {
      runHoldCycle();
    }
  }
}
// -------------------- Program 2 --------------------
void setupProgram2() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Program 2 Setup");
  delay(500);
}
void loopProgram2() {
  if (!isRunning) {
    switch (currentModeProgram2) {
      case ADJUST_UPPER_LIMIT:
        lcd.setCursor(0, 0);
        lcd.print("Set Upper Limit:   ");
        lcd.setCursor(0, 1);
        lcd.print((int)upperLimits[currentUpperLimitIndex]); 
        lcd.print("g   ");
        break;
      case ADJUST_LOWER_LIMIT:
        lcd.setCursor(0, 0);
        lcd.print("Set Lower Limit:   ");
        lcd.setCursor(0, 1);
        lcd.print((int)lowerLimits[currentLowerLimitIndex]); 
        lcd.print("g   ");
        break;
      case ADJUST_SPEED_PROGRAM2:  // Điều chỉnh tốc độ
        lcd.setCursor(0, 0);
        lcd.print("Set Speed       ");
        lcd.setCursor(0, 1);
        lcd.print(speedOptions[currentSpeedIndex]);  // Hiển thị tốc độ
        lcd.print("RPM   ");
        break;
      case ADJUST_TREATMENT_TIME:
        lcd.setCursor(0, 0);
        lcd.print("Set Treat Time   ");
        lcd.setCursor(0, 1);
        lcd.print((int)treatmentTimes[currentTreatmentTimeIndex]);
        lcd.print("s   ");
        break;
      case ADJUST_HOLD_TIME_PROGRAM2:
        lcd.setCursor(0, 0);
        lcd.print("Set Hold Time   ");
        lcd.setCursor(0, 1);
        lcd.print(holdTimeProgram2[currentHoldTimeProgram2Index]); 
        lcd.print("s   ");
        break;
    }

    if (digitalRead(BUTTON_ADJUST) == LOW) {
      delay(200);
      switch (currentModeProgram2) {
        case ADJUST_UPPER_LIMIT:
          currentUpperLimitIndex = (currentUpperLimitIndex + 1) % 3;
          break;
        case ADJUST_LOWER_LIMIT:
          currentLowerLimitIndex = (currentLowerLimitIndex + 1) % 3;
          break;
        case ADJUST_SPEED_PROGRAM2:  // Điều chỉnh tốc độ
          currentSpeedIndex = (currentSpeedIndex + 1) % 3;
          speeds = 1.0 / (speedOptions[currentSpeedIndex] / 60.0 * stepsPerRound) / 2.0 * 1000000;  // Cập nhật tốc độ
          break;
        case ADJUST_TREATMENT_TIME:
          currentTreatmentTimeIndex = (currentTreatmentTimeIndex + 1) % 3;
          break;
        case ADJUST_HOLD_TIME_PROGRAM2:
          currentHoldTimeProgram2Index = (currentHoldTimeProgram2Index + 1) % 3;
          break;
  }
}
    if (digitalRead(BUTTON_MODE) == LOW) {
      delay(200);
      currentModeProgram2 = (Mode2)((currentModeProgram2 + 1) % 5);
    }

    if (digitalRead(BUTTON_STARTSTOP) == LOW) {
      delay(200);
      isRunning = true;
      lcd.clear();
      lcd.print("Starting...");
      treatmentStartTime = millis();
      delay(1000);
    }
  } else {
    runTreatmentCycleProgram2();
  }
}

// -------------------- Hỗ trợ --------------------
void moveMotor(long stepCount, bool forward) {
    digitalWrite(EN_PIN, LOW); // ✅ Kích hoạt động cơ
    digitalWrite(DIR_PIN, forward ? HIGH : LOW); // ✅ Xác định chiều quay

    long lastStepTime = micros();

    for (long i = 0; i < stepCount; i++) {
        // 🛑 Kiểm tra nếu nhấn nút BACK (nút dừng khẩn cấp)
        if (digitalRead(BUTTON_BACK) == LOW) {  
            stopMotor();
            returnToZeroWeight();
            return; 
        }

        // 🔄 Cập nhật trọng lượng Loadcell sau mỗi 10 bước quay
        if (i % 200 == 0) {
            updateWeight();
            displayWeight();

            // 🛑 Nếu trọng lượng >= ngưỡng, dừng ngay lập tức
            if (currentWeight >= weightThresholds[currentThresholdIndex]) {
                stopMotor();
//                digitalWrite(coi, LOW); // 🔔 Bật còi báo đạt ngưỡng
                return; // 📌 Dừng ngay lập tức
            }
        }

        // 📌 Điều chỉnh tốc độ động cơ
        while ((micros() - lastStepTime) < speeds);
        lastStepTime = micros();

        // ✅ Gửi xung điều khiển động cơ
        digitalWrite(STEP_PIN, HIGH);
        delayMicroseconds(5);
        digitalWrite(STEP_PIN, LOW);
    }

    stopMotor(); // 🛑 Dừng động cơ khi hoàn thành bước quay
}
void moveMotorProgram2(long stepCount, bool forward) {
    digitalWrite(EN_PIN, LOW); // ✅ Kích hoạt động cơ
    digitalWrite(DIR_PIN, forward ? HIGH : LOW); // ✅ Xác định chiều quay

    long lastStepTime = micros();

    for (long i = 0; i < stepCount; i++) {
        // 🛑 Kiểm tra nếu nhấn nút BACK (nút dừng khẩn cấp)
        if (digitalRead(BUTTON_BACK) == LOW) {  
            stopMotor();
            returnToZeroWeight(); // Quay về vị trí ban đầu nếu có lệnh dừng khẩn cấp
            return; 
        }

        // 🔄 Cập nhật trọng lượng Loadcell sau mỗi 10 bước quay
        if (i % 200 == 0) {
            updateWeight();
            displayWeight();

            // 🛑 Dừng động cơ nếu đạt ngưỡng trên hoặc dưới
            if (forward) {  // 🔼 Khi động cơ đang đi lên
                if (currentWeight >= upperLimits[currentUpperLimitIndex]) {
                    stopMotor();
                    return;
                }
            } else {  // 🔽 Khi động cơ đang đi xuống
                if (currentWeight <= lowerLimits[currentLowerLimitIndex]) {
                    stopMotor();
                    return;
                }
            }
        }

        // 📌 Điều chỉnh tốc độ động cơ
        while ((micros() - lastStepTime) < speeds);
        lastStepTime = micros();

        // ✅ Gửi xung điều khiển động cơ
        digitalWrite(STEP_PIN, HIGH);
        delayMicroseconds(5);
        digitalWrite(STEP_PIN, LOW);
    }

    stopMotor(); // 🛑 Dừng động cơ khi hoàn thành bước quay
}

void stopMotor() {
  digitalWrite(EN_PIN, HIGH);
}

void updateWeight() {
  currentWeight = scale.get_units();
}

void displayWeight() {
  lcd.setCursor(0, 0);
  lcd.print("Weight: ");
  lcd.print(currentWeight, 0);
  lcd.print("g ");
}

void runHoldCycle() {
    unsigned long elapsedHoldTime = (millis() - holdStartTime) / 1000;
    lcd.setCursor(0, 1);
    lcd.print("Hold: ");
    lcd.print(holdTimes[currentHoldTimeIndex] - elapsedHoldTime);
    lcd.print("s ");

    // Khi hết thời gian giữ, kiểm tra trọng lượng trước khi quay về
    if (elapsedHoldTime >= holdTimes[currentHoldTimeIndex]) {
        holding = false;
        returning = true;
    }

    if (returning) {
        updateWeight(); // Cập nhật trọng lượng
        displayWeight();
        
        if (currentWeight < 10) { // 🔴 Nếu trọng lượng đã về gần 0, dừng ngay lập tức
            stopMotor();
            lcd.clear();
            lcd.setCursor(0, 0);
            lcd.print("Returned!");
            delay(1000);
            resetToProgramSelection();
            return;
        }

        returnToZeroWeight(); // Gọi hàm mới để quay về đến khi trọng lượng <10g
    }
}

void runHoldCycleProgram2() {
    unsigned long elapsedHoldTime = (millis() - holdStartTime) / 1000;

    // Luôn cập nhật trọng lượng và hiển thị LCD
    updateWeight();
    displayWeight();

    lcd.setCursor(0, 1);
    lcd.print("Hold: ");
    lcd.print(holdTimeProgram2[currentHoldTimeProgram2Index] - elapsedHoldTime);
    lcd.print("s ");

    // 🔴 Kiểm tra đúng giá trị thời gian giữ
    if (elapsedHoldTime >= holdTimeProgram2[currentHoldTimeProgram2Index]) {
        holding = false; // ✅ Kết thúc giữ, tiếp tục chu kỳ
        
    }
}

void runTreatmentCycleProgram2() {
    unsigned long elapsedTreatmentTime = (millis() - treatmentStartTime) / 1000;

    // Cập nhật trọng lượng liên tục
    updateWeight();
    displayWeight();

    // Nếu hết thời gian điều trị, quay về vị trí ban đầu
    if (elapsedTreatmentTime >= treatmentTimes[currentTreatmentTimeIndex]) {
        stopMotor();
        isRunning = false;

        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Treatment Done");
        delay(1000);

        returnToZeroWeight(); // ✅ Gọi hàm quay về, không reset toàn bộ hệ thống
        resetSystem(); // ✅ Quay về menu chọn chương trình
        return;
    }

    // Nếu vẫn trong thời gian điều trị, tiếp tục di chuyển động cơ
    if (holding) {
        runHoldCycleProgram2();
    } else {
        if (movingToUpper) {
            // 🔼 Quay động cơ lên ngưỡng trên
            if (currentWeight < upperLimits[currentUpperLimitIndex]) {
                moveMotorProgram2(stepsPerOutputRound, true);
                stepsTaken++;
            } else {
                stopMotor();
                holding = true;
                holdStartTime = millis();
                movingToUpper = false; // Đảo chiều quay về ngưỡng dưới
            }
        } else {
            // 🔽 Quay động cơ xuống ngưỡng dưới
            if (currentWeight > lowerLimits[currentLowerLimitIndex]) {
                moveMotorProgram2(stepsPerOutputRound, false);
                stepsTaken++;
            } else {
                stopMotor();
                holding = true;
                holdStartTime = millis();
                movingToUpper = true; // Quay lên lại sau khi giữ
            }
        }
    }
}

void returnToZeroWeight() {
    while (true) {
        updateWeight();
        displayWeight();

        // 🔴 DỪNG NGAY LẬP TỨC NẾU TRỌNG LƯỢNG < 30G
        if (currentWeight < 30) { 
            stopMotor();
            lcd.clear();
            lcd.setCursor(0, 0);
            lcd.print("Returned!");
            delay(1000);
            resetSystem(); // Quay về chọn chương trình
            return;
        }

        moveMotor(6400, false); // 🔄 Quay lùi nhưng chỉ 200 bước mỗi lần kiểm tra
    }
}

void resetSystem() {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Select Program:  ");
    delay(500);
    digitalWrite(LED_PROGRAM1, LOW); // Tắt đèn 1
    digitalWrite(LED_PROGRAM2, LOW); // Tắt đèn 2
    wdt_enable(WDTO_15MS); // Kích hoạt Watchdog Timer để reset sau 15ms
    while (1); // Chờ reset
}