#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <AccelStepper.h>

// Constants
#define CYCLE_SELECT_PIN 2
#define LOAD_SENSOR_PIN A1
#define BUZZER_PIN 8
#define TORQUE_MIN 100
#define TORQUE_MAX 1000

// Cycle durations (in milliseconds)
const unsigned long LIGHT_CYCLE_DURATION = 8000;
const unsigned long NORMAL_CYCLE_DURATION = 16000;
const unsigned long HEAVY_CYCLE_DURATION = 24000;

// Cycle stage durations (in milliseconds)
const unsigned long LIGHT_WASH_DURATION = 3000;
const unsigned long LIGHT_RINSE_DURATION = 3000;
const unsigned long NORMAL_WASH_DURATION = 6000;
const unsigned long NORMAL_RINSE_DURATION = 6000;
const unsigned long HEAVY_WASH_DURATION = 9000;
const unsigned long HEAVY_RINSE_DURATION = 9000;

LiquidCrystal_I2C lcd(0x27, 16, 2);
AccelStepper motor(AccelStepper::DRIVER, 9, 10);

// Global Variables
int selectedCycle = 0;  // 0: Light, 1: Normal, 2: Heavy
unsigned long startTime = 0;
unsigned long stageStartTime = 0;
int currentStage = 0;  // 0: Wash, 1: Rinse
bool washingInProgress = false;

// Function Prototypes
void startCycle();
void stopCycle();
void updateTorque();
void displayCycleInstructions();
void displayCycleStage();

void setup() {
  Serial.begin(9600);
  lcd.begin(16, 2);
  lcd.backlight();

  pinMode(BUZZER_PIN, OUTPUT);
  pinMode(CYCLE_SELECT_PIN, INPUT_PULLUP);

  // Motor setup
  motor.setMaxSpeed(TORQUE_MAX);
  motor.setAcceleration(500);
  displayCycleInstructions();
}

void loop() {
  bool cycleSelected = false;

  if (digitalRead(CYCLE_SELECT_PIN) == LOW) {
    selectedCycle = (selectedCycle + 1) % 3;  // Cycle through the cleaning modes
    cycleSelected = true;
    displayCycleInstructions();
    delay(500);  // Debounce the button
  }

  if (cycleSelected && !washingInProgress) {
    startCycle();
  }

  if (washingInProgress) {
    updateTorque();

    // Check if current stage has ended
    unsigned long currentTime = millis();
    unsigned long stageDuration = (selectedCycle == 0) ? (currentStage == 0 ? LIGHT_WASH_DURATION : LIGHT_RINSE_DURATION) :
                                  (selectedCycle == 1) ? (currentStage == 0 ? NORMAL_WASH_DURATION : NORMAL_RINSE_DURATION) :
                                  (currentStage == 0 ? HEAVY_WASH_DURATION : HEAVY_RINSE_DURATION);
    if (currentTime - stageStartTime >= stageDuration) {
      nextCycleStage();
    }

    // Check if overall cycle duration has ended
    unsigned long cycleDuration = (selectedCycle == 0) ? LIGHT_CYCLE_DURATION :
                                  (selectedCycle == 1) ? NORMAL_CYCLE_DURATION : HEAVY_CYCLE_DURATION;
    if (currentTime - startTime >= cycleDuration) {
      stopCycle();  // Stop the cycle after the duration
    }
  }
}

void startCycle() {
  if (!washingInProgress) {
    washingInProgress = true;
    startTime = millis();
    stageStartTime = startTime;
    currentStage = 0;  // Start with the wash stage

    // Display selected cycle and instructions
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Selected cycle:");
    lcd.setCursor(0, 1);
    lcd.print(selectedCycle == 0 ? "Light" : selectedCycle == 1 ? "Normal" : "Heavy");
    delay(3000);
    lcd.clear();

    // Buzzer feedback
    tone(BUZZER_PIN, 1000, 500);  // Short beep to indicate start
  }
}

void stopCycle() {
  washingInProgress = false;
  motor.setSpeed(0);
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Cycle Complete");
  noTone(BUZZER_PIN);
  delay(1000);
  displayCycleInstructions();
}

void nextCycleStage() {
  currentStage = (currentStage + 1) % 2;  // 0: Wash, 1: Rinse
  stageStartTime = millis();
  displayCycleStage();

  // Buzzer feedback
  if (currentStage == 0) {
    tone(BUZZER_PIN, 1000, 500);  // Short beep to indicate wash stage
  } else {
    tone(BUZZER_PIN, 2000, 500);  // Higher-pitched beep to indicate rinse stage
  }
}

void updateTorque() {
  // Simulate load input using load sensor reading
  int loadValue = analogRead(LOAD_SENSOR_PIN);
  int torque = map(loadValue, 0, 1023, TORQUE_MIN, TORQUE_MAX);
  motor.setSpeed(torque);

  // Display the current torque value
  lcd.setCursor(0, 1);
  lcd.print("Torque: ");
  lcd.print(torque);
}

void displayCycleInstructions() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Select cycle:");
  lcd.setCursor(0, 1);
  lcd.print(selectedCycle == 0 ? "Light" : selectedCycle == 1 ? "Normal" : "Heavy");
}

void displayCycleStage() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(selectedCycle == 0 ? "Light" : selectedCycle == 1 ? "Normal" : "Heavy");
  lcd.setCursor(0, 1);
  lcd.print(currentStage == 0 ? "Washing" : "Rinsing");
}