#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include "HX711.h"

// Chân điều khiển động cơ bước
#define STEP1_PIN 10
#define DIR1_PIN 11
#define EN1_PIN 13

// Chân load cell
#define LOADCELL_DOUT_PIN 3
#define LOADCELL_SCK_PIN 2

// Chân nút nhấn
#define BUTTON_THRESHOLD 4
#define BUTTON_HOLDTIME 5
#define BUTTON_STARTSTOP 6
#define BUTTON_EMERGENCY 7

// Load cell
HX711 scale;
float calibration_factor = 500.0; // Hệ số hiệu chuẩn
float weightThresholds[] = {1.02, 1.53, 2.04}; // Ngưỡng trọng lượng (kg)
int currentThresholdIndex = 0;

// Giữ thời gian (giây)
unsigned int holdTimes[] = {10, 15, 20};
int currentHoldTimeIndex = 0;

// LCD I2C
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Biến điều khiển
bool isRunning = false;
bool holding = false;
bool returning = false;
bool emergencyStop = false;
unsigned long holdStartTime = 0;

// Biến theo dõi số bước
long totalSteps = 0;   // Tổng số bước đã tiến lên
long stepsToReturn = 0; // Số bước cần để quay lại vị trí ban đầu

// Lọc trọng lượng
const int numReadings = 10;
float readings[numReadings];
int readIndex = 0;
float total = 0;
float averageWeight = 0;

void setup() {
  Serial.begin(9600);

  // Thiết lập các chân điều khiển
  pinMode(STEP1_PIN, OUTPUT);
  pinMode(DIR1_PIN, OUTPUT);
  pinMode(EN1_PIN, OUTPUT);
  pinMode(BUTTON_THRESHOLD, INPUT_PULLUP);
  pinMode(BUTTON_HOLDTIME, INPUT_PULLUP);
  pinMode(BUTTON_STARTSTOP, INPUT_PULLUP);
  pinMode(BUTTON_EMERGENCY, INPUT_PULLUP);

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

  // Tắt động cơ ban đầu
  digitalWrite(EN1_PIN, HIGH);
  digitalWrite(STEP1_PIN, LOW);
  digitalWrite(DIR1_PIN, LOW);

  // LCD
  lcd.init();
  lcd.backlight();
  lcd.print("System Ready");

  delay(2000);
  clearReadings();
  updateLCD();
}

void loop() {
  // Cập nhật giá trị trọng lượng
  updateWeight();

  // Dừng khẩn cấp
  if (digitalRead(BUTTON_EMERGENCY) == LOW) {
    handleEmergencyStop();
    return;
  }

  // Hiển thị thời gian giữ khi chưa nhấn Start
  if (!isRunning) {
    lcd.setCursor(0, 0);
    lcd.print("Hold: ");
    lcd.print(holdTimes[currentHoldTimeIndex]);
    lcd.print("s       ");
    lcd.setCursor(0, 1);
    lcd.print("Th: ");
    lcd.print(weightThresholds[currentThresholdIndex], 2);
    lcd.print("kg       ");
  }

  // Xử lý nút nhấn
  handleButtons();

  // Khi nhấn Start
  if (isRunning && !holding && !returning) {
    lcd.setCursor(0, 0);
    lcd.print("W: ");
    lcd.print(averageWeight, 2);
    lcd.print("kg       ");
    lcd.setCursor(0, 1);
    lcd.print("                "); // Xóa dòng dưới

    if (averageWeight < weightThresholds[currentThresholdIndex]) {
      moveMotor(true); // Quay động cơ để kéo tải trọng
      totalSteps++;    // Ghi nhận số bước đã tiến lên
    } else {
      stopMotor();
      holding = true;
      holdStartTime = millis();
      stepsToReturn = totalSteps; // Ghi nhận số bước cần quay lại
      Serial.println("Holding position...");
    }
  }

  // Hiển thị thời gian đếm ngược khi giữ
  if (holding) {
    unsigned long elapsedHoldTime = (millis() - holdStartTime) / 1000;
    unsigned int remainingHoldTime = holdTimes[currentHoldTimeIndex] - elapsedHoldTime;

    lcd.setCursor(0, 0);
    lcd.print("Hold: ");
    lcd.print(remainingHoldTime);
    lcd.print("s       ");
    if (remainingHoldTime == 0) {
      holding = false;
      returning = true;
      Serial.println("Returning to initial position...");
    }
  }

  // Trả động cơ về vị trí ban đầu
  if (returning) {
    if (stepsToReturn > 0) { // Quay lại vị trí ban đầu
      moveMotor(false); // Quay động cơ ngược lại
      stepsToReturn--;  // Giảm số bước còn lại
    } else {
      stopMotor();
      returning = false;
      isRunning = false;
      totalSteps = 0; // Reset số bước
      Serial.println("Returned to initial position.");
      lcd.setCursor(0, 1);
      lcd.print("System Ready     ");
    }
  }
}

// Cập nhật giá trị trọng lượng trung bình
void updateWeight() {
  total -= readings[readIndex];
  readings[readIndex] = scale.get_units();
  total += readings[readIndex];
  readIndex = (readIndex + 1) % numReadings;
  averageWeight = total / numReadings;
}

// Xử lý các nút nhấn
void handleButtons() {
  static unsigned long lastButtonPress = 0;

  if (digitalRead(BUTTON_THRESHOLD) == LOW && millis() - lastButtonPress > 300) {
    lastButtonPress = millis();
    currentThresholdIndex = (currentThresholdIndex + 1) % 3;
    updateLCD();
    Serial.print("Threshold changed to: ");
    Serial.println(weightThresholds[currentThresholdIndex]);
  }

  if (digitalRead(BUTTON_HOLDTIME) == LOW && millis() - lastButtonPress > 300) {
    lastButtonPress = millis();
    currentHoldTimeIndex = (currentHoldTimeIndex + 1) % 3;
    updateLCD();
    Serial.print("Hold time changed to: ");
    Serial.println(holdTimes[currentHoldTimeIndex]);
  }

  if (digitalRead(BUTTON_STARTSTOP) == LOW && millis() - lastButtonPress > 300) {
    lastButtonPress = millis();
    isRunning = !isRunning;
    if (isRunning) {
      Serial.println("System Started");
    } else {
      Serial.println("System Stopped");
      stopMotor();
    }
  }
}

// Điều khiển quay động cơ
void moveMotor(bool forward) {
  digitalWrite(EN1_PIN, LOW);
  digitalWrite(DIR1_PIN, forward ? HIGH : LOW);
  digitalWrite(STEP1_PIN, HIGH);
  delayMicroseconds(500); // Tạo xung
  digitalWrite(STEP1_PIN, LOW);
  delayMicroseconds(500);
}

// Dừng động cơ
void stopMotor() {
  digitalWrite(EN1_PIN, HIGH);
  Serial.println("Motor Stopped.");
}

// Dừng khẩn cấp
void handleEmergencyStop() {
  emergencyStop = true;
  isRunning = false;
  holding = false;
  returning = false;
  stopMotor();
  Serial.println("Emergency Stop Activated!");
  lcd.setCursor(0, 1);
  lcd.print("EMERGENCY STOP  ");
  delay(500);
}

// Làm sạch bộ nhớ đọc giá trị trọng lượng
void clearReadings() {
  for (int i = 0; i < numReadings; i++) {
    readings[i] = 0;
  }
  total = 0;
  averageWeight = 0;
}

// Cập nhật hiển thị LCD
void updateLCD() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Hold: ");
  lcd.print(holdTimes[currentHoldTimeIndex]);
  lcd.print("s       ");
  lcd.setCursor(0, 1);
  lcd.print("Th: ");
  lcd.print(weightThresholds[currentThresholdIndex], 2);
  lcd.print("kg       ");
}