// Including Required Libraries
#include <AccelStepper.h>  // You need to install this library from Library manager. Writer "AccelStepper" in search
#include <Encoder.h>       // You need to install this library from Library manager. Writer "Encoder" in search



// Pins for Inputs
const int encoderPinA = 3;     // Rotary Encoder Pin A
const int encoderPinB = 4;     // Rotary Encoder Pin B
const int potPin = A0;         // Potentiometer (Analog Input)
const int estopPin = 2;        // Emergency Stop Button
const int limitSwitchPin = 6;  // Limit Switch (NPN Proximity)
const int resetPin = 7;        // Reset Button

// Create a Rotary Encoder Object
Encoder rotaryEncoder(encoderPinA, encoderPinB);

// Pins for stepper Driver
const int stepPin = 8;  // Step pin for stepper driver
const int dirPin = 9;   // Direction pin for stepper driver
const int enPin = 10;   // Enable pin for stepper driver


// Stepper Motor Object (Using AccelStepper for smoother control)
AccelStepper stepper(AccelStepper::DRIVER, stepPin, dirPin);

// Pins for Outputs
const int relay1Pin = 11;  // Relay 1
const int relay2Pin = 12;  // Relay 2

bool relayLogic = LOW;  // Relay working Logic to operate Relay (Set it as HIGH/LOW based on your relay)

volatile bool emergencyStop = false;  // flag to be set on emergency button press



// Constants
const int forwardDistance = 1000;    // Pulses to move forward (adjust for your setup)
const int backwardDistance = -1000;  // Pulses to move back

// Global Variables
long encoderPosition = 0;
int encoderStep = 20;  // Number of encoder pulses to trigger movement
float speedModifier = 1.0;
bool forwardCut = false;
void emergencyBtnPressed() {
  emergencyStop = true;  // set emergency flag true
}
void setup() {
  // Initialize pins
  pinMode(estopPin, INPUT_PULLUP);
  pinMode(limitSwitchPin, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(estopPin), emergencyBtnPressed, FALLING);
  pinMode(resetPin, INPUT_PULLUP);
  pinMode(relay1Pin, OUTPUT);
  pinMode(relay2Pin, OUTPUT);
  pinMode(enPin, OUTPUT);
  // Set up stepper motor parameters
  stepper.setMaxSpeed(1000);
  stepper.setAcceleration(500);

  stopAll();  // Initiall stop everything
  // Homing routine on startup
  homeStepper();

  // Initialize encoder position
  encoderPosition = rotaryEncoder.read();
}

void loop() {
  // Check for emergency stop
  if (emergencyStop) {
    // Stop everything and reset homing
    stopAll();
    // Reset button functionality
    while (digitalRead(resetPin) != LOW) {
      ;
    }
    homeStepper();
    emergencyStop = false;  // Reset emergency stop flag
  }
  if(digitalRead(resetPin) == LOW){
    // Check if user has pressed Reset button take stepper back to home
    homeStepper();
  }

  // Read the potentiometer for adjusting the speed modifier
  speedModifier = analogRead(potPin) / 1023.0;  // Scale from 0 to 1

  // Read the rotary encoder position
  long newPosition = rotaryEncoder.read();

  // Check if the encoder has reached the set threshold for movement
  if (abs(newPosition - encoderPosition) >= encoderStep) {
    if (forwardCut == false) {  // Check if motor has made a backward cut previously then make forward cut now
      // Move the stepper motor forward
      moveStepper(forwardDistance);
      forwardCut = true;  // Set the forward cut flag
    } else {
      // Move the stepper motor back
      moveStepper(backwardDistance);
      forwardCut = false;  // Reset forward cut flag
    }
    // Update encoder position
    encoderPosition = newPosition;
  }
}
void turnOnRelay1() {
  digitalWrite(relay1Pin, relayLogic);  // Turn on relay 1
}
void turnOffRelay1() {
  digitalWrite(relay1Pin, !relayLogic);  // Turn off relay 1
}
void turnOnRelay2() {
  digitalWrite(relay2Pin, relayLogic);  // Turn on relay 2
}
void turnOffRelay2() {
  digitalWrite(relay2Pin, !relayLogic);  // Turn off relay 2
}
// Function to move the stepper motor
void moveStepper(int distance) {
  digitalWrite(enPin, LOW);  // Enable stepper driver

  stepper.setSpeed(1000 * speedModifier);
  stepper.move(distance);
  while (stepper.distanceToGo() != 0) {
    if (emergencyStop) {  // if emergency button is pressed get out  of loop
      return;
    }

    stepper.run();
  }
  digitalWrite(enPin, HIGH);  // Disable stepper driver
}

// Function to home the stepper motor
void homeStepper() {
  // Move the stepper motor backward slowly until the limit switch is triggered
  digitalWrite(enPin, LOW);  // Enable stepper driver
  stepper.setSpeed(-200);    // Slow speed for homing
  while (digitalRead(limitSwitchPin) != LOW) {
    if (emergencyStop) {  // if emergency button is pressed get out  of loop
      return;
    }
    stepper.runSpeed();
  }
  // Stop the motor and set position to 0
  stepper.setCurrentPosition(0);
  digitalWrite(enPin, HIGH);  // Disable stepper driver
  forwardCut = false;  // Reset forward cut flag
}

// Function to stop all actions (emergency stop)
void stopAll() {
  stepper.stop();
  digitalWrite(enPin, HIGH);             // Disable stepper driver
  digitalWrite(relay1Pin, !relayLogic);  // Turf off relay
  digitalWrite(relay2Pin, !relayLogic);  // Turn off relay
}