#include <AccelStepper.h>
#include <MultiStepper.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>
#include <Button.h>

//for outputs
#define motorPWM    13
#define servoPWM_1  6
#define servoPWM_2  5

//For display to initialize
LiquidCrystal_I2C lcd(0x27, 20, 4);

//For servo to initialize
Servo servo1;
Servo servo2;

//for buttons to initialize
Button BTN_UP(A0);
  bool btnUpRead;
Button BTN_OK(A1);
  bool btnOkRead;
Button BTN_DN(A2);
  bool btnDnRead;
Button entryConvPin(2);
  bool entryConvRead;
Button exitConvPin(3);
  bool exitConvRead;
Button entrySlicingPin(4);
  bool entrySlicingRead;
Button slicingLimit_1(A3);
  bool slicingLimitRead_1;
Button slicingLimit_2(A6);
  bool slicingLimitRead_2;

//For Stepper Motor Initialization
AccelStepper conveyorMotor(AccelStepper::DRIVER, 11, 10);
AccelStepper slicingMotor(AccelStepper::DRIVER, 9, 7);
#define EN_1        12
#define EN_2        8

//For Stepper Speed Calculation
float speed_1, speed_2, rpm_1 = 15.0, rpm_2 = 60.0;
float stepsperrev_1 = 2038.0, stepsperrev_2 = 200.0;
int microStepping_2 = 1/8;

//for calculating the steps based on the length of the slicing
float distanceInMM = 500.0, pitch = 4.0;
float mmToSteps = distanceInMM * stepsperrev_2 / (pitch * microStepping_2);

//For Storing the status of the machine
String state;

//For Storing important value for the machine to run
float timeToRun = 1,    //default value is 1 mins.
      acceleration = 5, //default values is 5 per time
      speed = 5;        //default is full speed
      


void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);

  //Declaring all outputs
  pinMode(motorPWM, OUTPUT);
  servo1.attach(servoPWM_1);
  servo1.attach(servoPWM_2);

  //Declaring all inputs
  BTN_UP.begin();
  BTN_OK.begin();
  BTN_DN.begin();
  entryConvPin.begin();
  exitConvPin.begin();
  entrySlicingPin.begin();
  slicingLimit_1.begin();
  slicingLimit_2.begin();

  //For Stepper's Enable Pin
  pinMode(EN_1, OUTPUT);
    digitalWrite(EN_1, HIGH);
  pinMode(EN_2, OUTPUT);
    digitalWrite(EN_2, HIGH);

  //bypass the status first:
  state ="ready";
}

void speedCalculation() {
  //Reading the potentiometers valuea
  //rpm = map(analogRead(A0), 0, 1023, 1, 15);      //Removed for further research
  
  //Calculation for Speed based on RPM
  speed_1 = (rpm_1 * stepsperrev_1) / 60.0;
  speed_2 = (rpm_2 * stepsperrev_2 * microStepping_2) / 60.0;
  
  //Setting the max speed & Acceleration
  conveyorMotor.setMaxSpeed(speed_1);
  conveyorMotor.setAcceleration(speed_1);
  slicingMotor.setMaxSpeed(speed_2);
  slicingMotor.setAcceleration(speed_2);
}

void loop() {
  readInputs();
  //peelingRun(motorPWM, speed, acceleration, timeToRun, signal);  //input at the last is in seconds
  //for stepper motor tests
  speedCalculation();//Enabling the stepper motor
  slicingSequence();

}

void slicingSequence() {
  //Run the ff sequence:
  /*
    1. Run the motor for peeler at a desired set time
    2. once done, motor for peeler will spin at 25% to throw the potatoes out
    3. If a potato has dropped in the conveyorMotor, the sensor will detect (entryConvSensor)
    4. conveyorMotor will start to spin
    5. if a potato has reached at the last sensor of the conveyoir (exitConvSensor), the motor at the potatoPeeler will stop
      conveyorMotor as well will stop
    6. if a potato has detected in the slicing area, it will commence slicing
    7. after slicing, the feeder will retract and then starts the conveyorMotor and peeler back
  */
  //for storing the states in string format
  static String lastState = "";

  //for storing the states of the sensors
  static bool entryState = false, lastEntryState = false, exitState, lastExitState = false,
              slicingState = false, lastSlicingState = false;

  //for storing the potato counter for the conveyor
  static int potatoCount, lastPotatoCount = 0;

  //if entry sensor detected a potato
  if(entryConvRead && entryState == false) {
    //sets the entryState as tru
    entryState = true;

    //checks if there is changes with the last entry state
    if(entryState != lastEntryState) {
      //increments the potatoCount by 1
      potatoCount+=1;

      //for displaying the count of potatoes in serial
      if(potatoCount != lastPotatoCount) {
        Serial.print("PotatoCount: ");
        Serial.println(potatoCount);

        //stores the current potatoCount to lastPotatoCOunt
        lastPotatoCount = potatoCount;

        //sets the last entry state as the current state
        lastEntryState = entryState;
      }
    }
  }
  //if entry sensor got pulled low, resets all flags
  else if (!entryConvRead && entryState == true) {
    //sets the entryState as false
    entryState = false;

    //sets the last entry state as the current state
    lastEntryState = entryState;
  }

  //once the potatoCount is above zero, it will then run the stepper motor
  //by sending state signal running
  if(potatoCount > 0 && state == "ready") {
    //sets the state to running
    state = "convRun";
  }

  //runs the stepper motors
  if(state == "convRun") {
    stepperRun(1, true);
    stepperRun(2, false);
  }
  else if (state == "convStop") {
    stepperRun(1, false);
    stepperRun(2, false);
  }
  else if (state == "slicingRun") {
    stepperRun(1, false);
    stepperRun(2, true);
  }
  else if (state == "ready") {
    stepperRun(1, false);
    stepperRun(2, false);
  }

  //if exit sensor detected a potato
  if(exitConvRead && exitState == false) {
    //sets the exitState as true
    exitState = true;

    //checks if there is changes with the last exit state
    if(exitState != lastExitState) {
      //before decrementing, checks if value is 0 to avoid negative values
      if(potatoCount <= 0) {
        //sets the potatoCount to 0
        potatoCount = 0;
      }
      else {
        //decrements the potatoCount by 1
        potatoCount-=1;
      }
      
      //throws the value to the serial monitor
      Serial.print("PotatoCount: ");
      Serial.println(potatoCount);

      //sets the last entry state as the current state
      lastExitState = exitState;
    }
  }
  //if exit sensor got pulled low, resets all flag and stops the conveyor
  else if (!exitConvRead && exitState == true) {
    //checking if there is now a difference between the last and current potatoCount
    if(potatoCount != lastPotatoCount) {
      //sets the state to Stop
      state = "convStop";

      //stores the current potatoCount to lastPotatoCOunt
      lastPotatoCount = potatoCount;
    }
    
    //sets the entryState as false
    exitState = false;

    //sets the last entry state as the current state
    lastExitState = exitState;
  }

  //if conveyorStops, checks if the sensor at the slicing detected a potato
  if(entrySlicingRead && slicingState == false) {
    //sets the slicing state to true
    slicingState = true;

    if (slicingState != lastSlicingState) {
      //sets the lastSlicingState to the current slicingState
      lastSlicingState = slicingState;
    }
  }
  else if(!entrySlicingRead && slicingState == true) {
    //sets the slicing state to true
    slicingState = false;

    if(slicingState != lastSlicingState) {
      //sets the state to slicingRun
      state = "slicingRun";

      //sets the lastSlicingState to the current slicingState
      lastSlicingState = slicingState;
    }
  }

  //if the limitSwitch is triggered in the slicing area after retraction
  if(slicingLimitRead_1 && state == "slicingDone") {
    //sets the state to ready
    state = "ready";
  }
}

//for reading intputs
void readInputs() {
  //For PushButtons
  if (BTN_UP.pressed()) {
    btnUpRead = true;
  }
  else if (BTN_UP.released()) {
    btnUpRead = false;
  }
  if (BTN_DN.pressed()) {
    btnDnRead = true;
  }
  else if (BTN_DN.released()) {
    btnDnRead = false;
  }
  if (BTN_OK.pressed()) {
    btnOkRead = true;
  }
  else if (BTN_OK.released()) {
    btnOkRead = false;
  }

  //For IR Sensors
  if (entryConvPin.pressed()) {
    entryConvRead = true;
  }
  else if (entryConvPin.released()) {
    entryConvRead = false;
  }
  if (exitConvPin.pressed()) {
    exitConvRead = true;
  }
  else if (exitConvPin.released()) {
    exitConvRead = false;
  }
  if (entrySlicingPin.pressed()) {
    entrySlicingRead = true;
  }
  else if (entrySlicingPin.released()) {
    entrySlicingRead = false;
  }

  //for Limit Switches
  if (slicingLimit_1.pressed()) {
    slicingLimitRead_1 = true;
  }
  else if (slicingLimit_1.released()) {
    slicingLimitRead_1 = false;
  }
  if (slicingLimit_2.pressed()) {
    slicingLimitRead_2 = true;
  }
  else if (slicingLimit_2.released()) {
    slicingLimitRead_2 = false;
  }
}

void peelingRun(int pwmPin, int speed, int acceleration, float timeToRun, String& signal) {
  // Convert the value of time to run into milliseconds
  static unsigned long timeToRunMillis;
  timeToRunMillis = timeToRun * 60000UL; //converts the minutes into millis

  // For keeping the state of the motor
  static bool isRunning = false, initialized = false;

  // For timer variables
  static unsigned long previousMillis = 0; // Initialize with 0
  unsigned long currentMillis = millis();

  // For acceleration, will increase per 500ms
  static int currentSpeed = 0;
  static unsigned long prevMillis_accel = -1;

  // For displaying value if current speed changed
  static String lastSignal = "";
  if(signal != lastSignal) {
    Serial.println(signal);

    lastSignal = signal;
  }
  // Checks if the command says stop
  if (timeToRun == -1) {
    // Ticks the flag running so that the currentSpeed will decrement
    isRunning = false;
    //resets the start if signal received stopped
  } 
  else {
    // Check if the time has not yet been reached or if timeToRun is 0 (infinite)
    if (currentMillis - previousMillis < timeToRunMillis || timeToRun == 0) {
      // Ticks the flag running so that the currentSpeed will increment
      isRunning = true;

      signal = "motorRunning";

      if(isRunning) {
        if (currentMillis - prevMillis_accel >= 50) {
          // Checks if currentSpeed exceeds the speed set up
          if (currentSpeed >= speed) {
            currentSpeed = 255;
          } else if (currentSpeed < speed) {
            currentSpeed += acceleration; // Increments based on the acceleration
          }

          // Resets the time
          prevMillis_accel = currentMillis;
        }
      }
    } 
    else {
      // Ticks the flag running so that the currentSpeed will decrement
      isRunning = false;

      signal = "motorStopped";

      if (!isRunning) {
        if (currentMillis - prevMillis_accel >= 50) {
            // Checks if current speed exceeds 0 value
            if (currentSpeed <= 0) {
              currentSpeed = 0;

            } else if (currentSpeed != 0) {
              currentSpeed -= acceleration; // Decrements based on the acceleration
            }
            
          // Resets the time
          prevMillis_accel = currentMillis;
        }
      }
    }
  }

  // Runs the motor infinitely or based on the time logic
  analogWrite(pwmPin, currentSpeed);

  
}

void stepperRun(int stepper, bool run) {
  //For Conveyor Motor
  if(stepper == 1 && run == true) {
    //Enabling the stepper motor
    digitalWrite(EN_1, LOW);

    conveyorMotor.setSpeed(speed_1);          //sets the speed of the stepper motor
    conveyorMotor.setAcceleration(speed_1);   //sets the acceleration
    conveyorMotor.move(9999);              //run continously
    conveyorMotor.runSpeedToPosition();       //need to use this function to achieve the set speed
  }
  else if (stepper == 1 && run == false) {
    //Stopping the Stepper Motor
    conveyorMotor.stop();

    //Disabling the stepper motor
    digitalWrite(EN_1, HIGH);
  }

  //For slicing motor
  //for storing extended & retracted bool flag
  static bool extended = false, retracted = false;
  if (stepper == 2 && run == true) {
    //Serial.println(slicingMotor.currentPosition());
    //Enabling the stepper motor
    digitalWrite(EN_2, LOW);

    slicingMotor.setSpeed(speed_2);          //sets the speed of the stepper motor
    slicingMotor.setAcceleration(speed_2);   //sets the acceleration
    
    if (slicingMotor.currentPosition() >= mmToSteps && extended == true) {
      retracted = true;
      extended = false;
    }

    if (retracted == true && extended == false) {
      slicingMotor.moveTo(-999999);
      slicingMotor.runSpeedToPosition();     //need to use this function to achieve the set speed
    }

    if (slicingLimitRead_1 && extended == false && retracted == true) {
      //resets the currentPosition to 0
      slicingMotor.setCurrentPosition(0);

      // If the limit switch at the retract area is triggered, stop the stepper
      slicingMotor.stop();
      digitalWrite(EN_2, HIGH); // Disable the stepper motor

      //sets the state to slicing done
      state = "slicingDone";

    }
    
    if (extended == false && retracted == false) {
      // Continue moving towards the target position
      slicingMotor.moveTo(mmToSteps);
      slicingMotor.runSpeedToPosition();     //need to use this function to achieve the set speed

      if(slicingMotor.currentPosition() == mmToSteps) {
        extended = true;
      }
    }
  }

  if (stepper == 2 && run == false) {
    //Stopping the Stepper Motor
    slicingMotor.stop();

    //resets the statuses
    retracted = false;
    extended = false;

    //resets the currentPosition to 0
    slicingMotor.setCurrentPosition(0);

    //Disabling the stepper motor
    digitalWrite(EN_2, HIGH);
  }

}