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

LiquidCrystal_I2C lcd(0x27,20,4);

AccelStepper stepperR(1,2,3);
AccelStepper stepperL(1,8,9);
 
int upBtn = 5;
int downBtn = 6;
int selectBtn = 7;
int stopBtn = 4;

int dcMotor = 14;
int solenoid = 15;
int extractorFan = 10;

bool RELAY_ACTIVE = true;

enum STATES{
  MOTOR_R_HIGH = 0,
  MOTOR_R_LOW = 1,
  MOTOR_L_HIGH = 2,
  MOTOR_L_LOW = 3,
  SET_MIN = 4,
  SET_SEC = 5,
  GO = 6,
  STOP = 7,
  WAIT = 8,
};

STATES state;

enum SEQUENCE{
  START = 0,
  MOVE = 1,
  END = 2,
  COUNTDOWN = 3,
};

SEQUENCE seq;

int stepperAcc = 100;
int stepperMaxSpeed = 200;
int stepperSpeed = 150;

int lowPosR = 0;
int highPosR = 0;
int lowPosL = 0;
int highPosL = 0;

int minutes = 1;
int seconds = 59;
unsigned long mill;

int countDown = 0;
int totalCountdownTime = 60;

unsigned long waitBetweenMill;
unsigned long oneSecMill;

bool Once2SecWait = true;

bool stepDirR = true;
bool stepDirL = true;

const int positionTolerance = 10; // Define a tolerance range of ±1 step

void setup()
{  
  lcd.init();                     
  lcd.backlight();

  lcd.clear();
  
  stepperR.setMaxSpeed(stepperMaxSpeed);
  stepperR.setAcceleration(stepperAcc);
  stepperL.setMaxSpeed(stepperMaxSpeed);
  stepperL.setAcceleration(stepperAcc);

  pinMode(upBtn, INPUT_PULLUP);
  pinMode(downBtn, INPUT_PULLUP);
  pinMode(selectBtn, INPUT_PULLUP);
  pinMode(stopBtn, INPUT_PULLUP);

  pinMode(dcMotor, OUTPUT);
  pinMode(solenoid, OUTPUT);
  pinMode(extractorFan, OUTPUT);

  Serial.begin(9600);


  // seq = MOVE_R_UP;
  state = MOTOR_R_HIGH;
  
  UpdateLCD();
}
 


void HandleUpButton(){

  if(digitalRead(upBtn) == 0){


    if(state == MOTOR_R_HIGH || state == MOTOR_R_LOW)
    {
      stepperR.setSpeed(stepperSpeed);
      stepperR.runSpeed();
    }
    else if(state == MOTOR_L_HIGH || state == MOTOR_L_LOW)
    {
      stepperL.setSpeed(stepperSpeed);
      stepperL.runSpeed();
    }
    else if(state == SET_MIN)
    {
      delay(250);
      minutes++;
      UpdateLCD();
    }
    else if(state == SET_SEC)
    {
      delay(250);
      seconds++;
      if(seconds > 59){
        seconds = 0;
      }
      UpdateLCD();
    }
   
  }   
}


void HandleDownButton(){
  if(digitalRead(downBtn) == 0){


    if(state == MOTOR_R_HIGH || state == MOTOR_R_LOW)
    {
      stepperR.setSpeed(-stepperSpeed);
      stepperR.runSpeed();
    }
    else if(state == MOTOR_L_HIGH || state == MOTOR_L_LOW)
    {
      stepperL.setSpeed(-stepperSpeed);
      stepperL.runSpeed();
    }
    else if(state == SET_MIN)
    {
      delay(250);
      minutes--;
      if(minutes < 0)
      minutes = 0;
      UpdateLCD();
    }
    else if(state == SET_SEC)
    {
      delay(250);
      seconds--;
      if(seconds < 0){
        seconds = 59;
      }
      UpdateLCD();
    }

}
}

void UpdateLCD(){

  switch(state){
    case MOTOR_R_HIGH:
    lcd.clear();
    lcd.setCursor(0,1);
    lcd.print("    RIGHT  MOTOR    ");
    lcd.setCursor(0,2);
    lcd.print("   Set High Point   ");
    break;
    case MOTOR_R_LOW:
    lcd.clear();
    lcd.setCursor(0,1);
    lcd.print("    RIGHT  MOTOR    ");
    lcd.setCursor(0,2);
    lcd.print("   Set Low Point    ");
    break;
    case MOTOR_L_HIGH:
    lcd.clear();
    lcd.setCursor(0,1);
    lcd.print("    LEFT   MOTOR    ");
    lcd.setCursor(0,2);
    lcd.print("   Set High Point   ");
    break;
    case MOTOR_L_LOW:
    lcd.clear();
    lcd.setCursor(0,1);
    lcd.print("    LEFT   MOTOR    ");
    lcd.setCursor(0,2);
    lcd.print("   Set Low Point    ");
    break;
    case SET_MIN:
    lcd.setCursor(5,0);
    lcd.print("SET  TIMER");
    lcd.setCursor(5,2);
    lcd.print(">");
    lcd.print(TwoDigitFormat(minutes));
    lcd.print("<: ");
    lcd.print(TwoDigitFormat(seconds));
    lcd.setCursor(7,2);
    lcd.blink();
    break;
    case SET_SEC:
    lcd.setCursor(5,0);
    lcd.print("SET  TIMER");
    lcd.setCursor(6,2);
    lcd.print(TwoDigitFormat(minutes));
    lcd.print(" :>");
    lcd.print(TwoDigitFormat(seconds));
    lcd.print("<");
    lcd.setCursor(12,2);
    lcd.blink();
    break;
    case WAIT: // "Cycle Finished - Press Select to clear"
    lcd.setCursor(0,0);
    lcd.print("   CYCLE FINISHED   ");
    lcd.setCursor(0,2);
    lcd.print("    Press Select    ");
    lcd.setCursor(0,3);
    lcd.print("      to clear      ");
    break;
  }
}

String TwoDigitFormat(int num){

  String s;
  if(num <= 9)
  {
    s = "0";
    s += num;
    return s;
  }
  else {
    s = num;
    return s;
  }

}

void HandleSelectButton(){
  if(!digitalRead(selectBtn)){
  delay(250);
  switch (state){
   case MOTOR_R_HIGH:
    highPosR = stepperR.currentPosition();
    state = MOTOR_R_LOW;
    break;
    case MOTOR_R_LOW:
    lowPosR = stepperR.currentPosition();
    state = MOTOR_L_HIGH;
    break;
    case MOTOR_L_HIGH:
    highPosL = stepperL.currentPosition();
    state = MOTOR_L_LOW;
    break;
    case MOTOR_L_LOW:
    lowPosL = stepperL.currentPosition();
    state = SET_MIN;
    lcd.clear();
    break;
    case SET_MIN:
    state = SET_SEC;
    lcd.clear();
    break;
    case SET_SEC:
    lcd.clear();
    lcd.noBlink();
    state = GO;
    seq = START;
    Once2SecWait = true;
    break;
    case WAIT:
    lcd.clear();
    state = MOTOR_R_HIGH;
    break;
  }
  UpdateLCD();
  }
}

void HandleBackButton(){
  if(!digitalRead(stopBtn)){
  delay(250);
  switch (state){
   case MOTOR_R_HIGH:
    state = MOTOR_R_HIGH;
    break;
    case MOTOR_R_LOW:
    state = MOTOR_R_HIGH;
    break;
    case MOTOR_L_HIGH:
    state = MOTOR_R_LOW;
    break;
    case MOTOR_L_LOW:
    state = MOTOR_L_HIGH;
    lcd.clear();
    break;
    case SET_MIN:
    lcd.noBlink();
    state = MOTOR_L_LOW;
    lcd.clear();
    break;
    case SET_SEC:
    lcd.clear();
    lcd.noBlink();
    state = SET_MIN;
    break;
  }
  UpdateLCD();
  }
}
void ResetMotors() {
  stepperR.setCurrentPosition(0);
  stepperL.setCurrentPosition(0);
  stepDirR = true;
  stepDirL = true;
  highPosR = 0;
  lowPosR = 0;
  highPosL = 0;
  lowPosL = 0;
  minutes = 1;
  seconds = 59;
  Once2SecWait = true;
 
}

void HandleSequence(){
  if(state == GO){
    switch (seq){
   case START:
    lcd.clear();
    stepperR.moveTo(highPosR);
    stepperL.moveTo(highPosL);
    digitalWrite(dcMotor,RELAY_ACTIVE);
    digitalWrite(solenoid,RELAY_ACTIVE);
    digitalWrite(extractorFan,RELAY_ACTIVE);
    seq = MOVE;
    oneSecMill = millis();
    stepDirR = true;
    stepDirL = true;
    waitBetweenMill = millis();
    delay(1);
   break;
   case END:
    Serial.println("END");
    digitalWrite(dcMotor,!RELAY_ACTIVE);
    delay(2000);
    digitalWrite(solenoid,!RELAY_ACTIVE);
    ResetMotors();
    //state = WAIT;
    // UpdateLCD();a
    seq = COUNTDOWN;
    countDown = totalCountdownTime;
    SetCountDownLCD();
    break;
    case COUNTDOWN:
    
     if(millis() - oneSecMill >= 1000){
     oneSecMill = millis();
     if (countDown > 0) 
     {
      countDown--;
      UpdateCountDownLCD();
      if(countDown == 0){
        state = WAIT;
        UpdateLCD();
        digitalWrite(extractorFan,!RELAY_ACTIVE);
      }
     }
     else{
      seq = END;
     }
    }
    break;
    case MOVE:

  //  if(stepDirR){
  //   if (stepperR.distanceToGo() != 0){
  //   stepperR.run();
  //   }
  //   else{
  //     stepperR.moveTo(lowPosR);
  //     stepDirR = false;
  //   }
  //  }
    
  //  if(!stepDirR){
  //   if (stepperR.distanceToGo() != 0){
  //   stepperR.run();
  //   }
  //   else{
  //     stepperR.moveTo(highPosR);
  //     stepDirR = true;
  //   }
  //  }
  if (stepDirR) {
    if (abs(stepperR.distanceToGo()) <= positionTolerance) {
        stepperR.moveTo(lowPosR);
        stepDirR = false;
    } else {
        stepperR.run();
    }
} else {
    if (abs(stepperR.distanceToGo()) <= positionTolerance) {
        stepperR.moveTo(highPosR);
        stepDirR = true;
    } else {
        stepperR.run();
    }
}
    


    if(millis() - waitBetweenMill >= 2000){

      // if(stepDirL){
      //   if (stepperL.distanceToGo() != 0){
      //     stepperL.run();
      //   }
      //   else{
      //     stepperL.moveTo(lowPosL);
      //     stepDirL = false;
      //   }
      // }


      // if(!stepDirL){
      //   if (stepperL.distanceToGo() != 0){
      //     stepperL.run();
      //   }
      //   else{
      //     stepperL.moveTo(highPosL);
      //     stepDirL = true;
      //   }
      // }
      if (stepDirL) {
    if (abs(stepperL.distanceToGo()) <= positionTolerance) {
        stepperL.moveTo(lowPosL);
        stepDirL = false;
    } else {
        stepperL.run();
    }
} else {
    if (abs(stepperL.distanceToGo()) <= positionTolerance) {
        stepperL.moveTo(highPosL);
        stepDirL = true;
    } else {
        stepperL.run();
    }
}

    }
    


   if(millis() - oneSecMill >= 1000){
     oneSecMill = millis();
     if (seconds > 0 || minutes > 0) 
     {
      if (seconds == 0) {
          minutes--;
          seconds = 59;
      } else {
          seconds--;
      }
      
      UpdateTimerLCD();
     }
     else{
      seq = END;
     }
    }

    if(!digitalRead(stopBtn)){
      delay(250);
      lcd.clear();
      seq = END;
    }


    break;
   }

  }

}
void SetCountDownLCD(){
    lcd.setCursor(0,0);
    lcd.print("   CYCLE FINISHED   ");
    lcd.setCursor(0,2);
    lcd.print(" Extracting Cabinet ");
    lcd.setCursor(9,3);
    lcd.print(TwoDigitFormat(countDown));
}
void UpdateCountDownLCD(){
    lcd.setCursor(9,3);
    lcd.print(TwoDigitFormat(countDown));
}
void UpdateTimerLCD() {
    lcd.setCursor(5,0);
    lcd.print("TIME  LEFT");
    lcd.setCursor(6,2);
    lcd.print(TwoDigitFormat(minutes));
    lcd.print(" : ");
    lcd.print(TwoDigitFormat(seconds));
}

void loop()
{
  if(state != GO)
  {
    HandleUpButton();
    HandleDownButton();
    HandleSelectButton();
    HandleBackButton();
    digitalWrite(dcMotor,!RELAY_ACTIVE);
    digitalWrite(solenoid,!RELAY_ACTIVE);
  }
  else
  {
    HandleSequence();
  }
}