// 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

#define DEBUG 1  // Set Debug level 0-1 to prints result on serial monitor (Set it to 0 when you don't need serial prints)

#if DEBUG == 1
#define debug(x) Serial.print(x)
#define debugln(x) Serial.println(x)
#else
#define debug(x)
#define debugln(x)
#endif

// =========== Encoder Adjustment =============
int encoderStep = 40;  // Number of encoder pulses to trigger movement this will be base value

float allowedFluctuation = 0.2;  // Maximum allowed fluctuation from base value +/- in percentage allowed value 0.0 - 1.0 (0% to 100%)

int totalEncoderSteps = 0;  // New value for encoder steps to trigger the motor

// 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) for speed
const int linearPot = A1;      // Linear Potentiometer for relation with steps
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;
float speedModifier = 1.0;
bool forwardCut = false;
void emergencyBtnPressed() {
  emergencyStop = true;  // set emergency flag true
}
void setup() {
  Serial.begin(9600);
  // 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();
  }

  calculateNewEncoderSteps();  // Calculate Encoder steps value based on potentiometer

  // 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) >= totalEncoderSteps) {
    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 calculateNewEncoderSteps() {
  allowedFluctuation = constrain(allowedFluctuation, 0.0, 1.0);                                                                                          // Make sure the allowed fluctuation will be in 0.0-1.0 range
  int potMap = map(analogRead(linearPot), 0, 1023, encoderStep - (encoderStep * allowedFluctuation), encoderStep + (encoderStep * allowedFluctuation));  // Map the potentiometer value from -100, 100 (Mid value will be 0)
  debug("New Encoder Mapped value:");
  debugln(potMap);
  totalEncoderSteps = potMap;  // Get the total steps from mapped values
}

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
  int dirValue = 1;
  if (distance < 0)
    dirValue = -1;
  stepper.moveTo(distance);
  while (stepper.distanceToGo() != 0) {
    if (emergencyStop) {  // if emergency button is pressed get out  of loop
      return;
    }
    stepper.setSpeed(dirValue * 1000 * (analogRead(potPin) / 1023.0));
    stepper.runSpeed();
  }
  stepper.setCurrentPosition(0);

  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 * speedModifier);    // Slow speed for homing
  while (digitalRead(limitSwitchPin) != LOW) {
    if (emergencyStop) {  // if emergency button is pressed get out  of loop
      return;
    }
    stepper.setSpeed(-200 *  (analogRead(potPin) / 1023.0));    // Slow speed for homing

    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
}