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

// Define the number of steps per revolution for your stepper motor
const int stepsPerRevolution = 200;

// Define the steps per 0.1mm of travel for your specific setup
const int stepsPer0_1mm = 38; // Adjust this value based on your setup

const int backlash = 360;

// Define the pin connections for the TB6600 driver
const int stepPin = 2;
const int dirPin = 3;

// Define the pin connections for the push buttons and potentiometer
const int buttonForwardPin = 4;
const int buttonReversePin = 5;
const int setHomeButtonPin = 6;      // Button to set the home position
const int setSecondButtonPin = 7;    // Button to set the second position
const int goToHomeButtonPin = 8;    // Button to go to the home position
const int goToSecondButtonPin = 9;  // Button to go to the second position
const int potentiometerPin = A0;

// Create an AccelStepper object
AccelStepper stepper(AccelStepper::DRIVER, stepPin, dirPin);

// Variables to keep track of the stepper position and positions
float currentPosition = 0;
float homePosition = 0;
float secondPosition = 0;
float backlashPosition = 0;
// Store the target position for the goToPosition function
int targetPosition = 0;

// Initialize the LCD with the I2C address
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Flag to indicate if the motor has been homed
bool homed = false;
bool forwardButtonPressed = false;
bool reverseButtonPressed = false;
unsigned long forwardButtonPressStartTime = 0;
unsigned long reverseButtonPressStartTime = 0;
const unsigned long shortPressDuration = 20; // Adjust the short press duration (in milliseconds)
const unsigned long longPressDuration = 1000; // Adjust the long press duration (in milliseconds)
int shortPressIncrement = 1; // 0.1mm increment
int longPressIncrement = 10; // 1mm increment
bool setHomeButtonPressed = false;
unsigned long setHomeButtonPressStartTime = 0;


float currentDistanceFromHome = 0.0;
void setup() {
  // Set up the push buttons, potentiometer, and button pins as inputs
  pinMode(buttonForwardPin, INPUT_PULLUP);
  pinMode(buttonReversePin, INPUT_PULLUP);
  pinMode(setHomeButtonPin, INPUT_PULLUP);
  pinMode(setSecondButtonPin, INPUT_PULLUP);
  pinMode(goToHomeButtonPin, INPUT_PULLUP);
  pinMode(goToSecondButtonPin, INPUT_PULLUP);
  
  // Initialize the LCD
  lcd.init();
  lcd.backlight();
  // Read the potentiometer value to adjust the motor speed
  int potValue = analogRead(potentiometerPin);
  int motorSpeed = map(potValue, 0, 1023, 20, 1500); // Change the speed range as needed
  // Set the maximum speed and acceleration for the stepper motor
  stepper.setMaxSpeed(motorSpeed); 
  stepper.setAcceleration(8000); 
  stepper.setSpeed(motorSpeed);
}

void loop() {
  // Read the potentiometer value to adjust the motor speed
  int potValue = analogRead(potentiometerPin);
  int motorSpeed = map(potValue, 0, 1023, 20, 1500); // Change the speed range as needed
  // Set the maximum speed and acceleration for the stepper motor
  stepper.setMaxSpeed(motorSpeed); 
  stepper.setAcceleration(4000);
  stepper.setSpeed(motorSpeed);

// Check if the forward button is pressed
if (digitalRead(buttonForwardPin) == LOW) {
  if (!forwardButtonPressed) {
    // Button was just pressed
    forwardButtonPressed = true;
    forwardButtonPressStartTime = millis();
  } else {
    // Button is being held
    unsigned long currentTime = millis();
    if (currentTime - forwardButtonPressStartTime >= longPressDuration) {
      // Long press detected, increment position by 1mm
      stepper.move(longPressIncrement * stepsPer0_1mm);
      currentPosition += longPressIncrement * stepsPer0_1mm;
    } else if (currentTime - forwardButtonPressStartTime >= shortPressDuration) {
      // Short press detected, increment position by 0.1mm
      stepper.move(shortPressIncrement * stepsPer0_1mm);
      currentPosition += shortPressIncrement * stepsPer0_1mm;
    }
    // Add a small delay after the move
    delay(100); // Add this line to introduce a delay
  }
} else {
  // Button is released
  forwardButtonPressed = false;
}

// Check if the reverse button is pressed
if (digitalRead(buttonReversePin) == LOW) {
  if (!reverseButtonPressed) {
    // Button was just pressed
    reverseButtonPressed = true;
    reverseButtonPressStartTime = millis();
  } else {
    // Button is being held
    unsigned long currentTime = millis();
    if (currentTime - reverseButtonPressStartTime >= longPressDuration) {
      // Long press detected, decrement position by 1mm
      stepper.move(-longPressIncrement * stepsPer0_1mm);
      currentPosition -= longPressIncrement * stepsPer0_1mm;
    } else if (currentTime - reverseButtonPressStartTime >= shortPressDuration) {
      // Short press detected, decrement position by 0.1mm
      stepper.move(-shortPressIncrement * stepsPer0_1mm);
      currentPosition -= shortPressIncrement * stepsPer0_1mm;
    }
    // Add a small delay after the move
    delay(100); // Add this line to introduce a delay
  }
} else {
  // Button is released
  reverseButtonPressed = false;
}

// If the set home button is pressed, set the current position as the home location
  if (digitalRead(setHomeButtonPin) == LOW) {
    homePosition = currentPosition;
    delay(100);
    backlashPosition = currentPosition - backlash;
    delay(100);
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Home set    ");
    delay(1500);  
    homed = true; 
    secondPosition = 0;
  }

  // If the set second button is pressed, set the current position as the second position
  if (digitalRead(setSecondButtonPin) == LOW) {
    secondPosition = currentPosition;
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Second set ");
    float distanceFromHome = ((secondPosition - homePosition) / stepsPer0_1mm) / 10;
    lcd.setCursor(0, 1);
    lcd.print("Distance:");
    lcd.print(distanceFromHome, 2); 
    lcd.print("mm");
    delay(2000);  
    lcd.clear();
  }

  // If the go to home button is pressed, move the motor to the home position
  if (digitalRead(goToHomeButtonPin) == LOW) {
    if (homed) { // Only move to home if the motor has been homed
      targetPosition = homePosition;
      stepper.setMaxSpeed(1500); 
      stepper.setAcceleration(4000); 
      stepper.setSpeed(1500);

      stepper.moveTo(homePosition);
      while (stepper.run()) {
        // Wait for the stepper to reach the target position
      }
      delay(500);
      targetPosition = backlashPosition;
      stepper.moveTo(backlashPosition);
      while (stepper.run()) {
        // Wait for the stepper to reach the target position
      }
      delay(500);
      targetPosition = homePosition;
      stepper.moveTo(homePosition);
      while (stepper.run()) {
        // Wait for the stepper to reach the target position
      }
    }
  }

  // If the go to second button is pressed, move the motor to the second position
  if (digitalRead(goToSecondButtonPin) == LOW) {
    stepper.setMaxSpeed(motorSpeed); 
    stepper.setAcceleration(4000); 
    stepper.setSpeed(motorSpeed);

    targetPosition = secondPosition;
    stepper.moveTo(secondPosition);
    while (stepper.run()) {
      // Wait for the stepper to reach the target position
    }
  }

float distanceStepsFromHome = currentPosition - homePosition;
currentDistanceFromHome = (distanceStepsFromHome / stepsPer0_1mm) / 10.0; // Calculate distance in 0.1mm units

// Calculate speed percentage (0 to 100) based on motorSpeed
int speedPercentage = map(motorSpeed, 20, 1500, 1, 99);

lcd.setCursor(0, 0);
lcd.print("Dis:");
char distanceStr[10];
dtostrf(currentDistanceFromHome, 5, 2, distanceStr);
lcd.print(distanceStr);
lcd.print("mm");
lcd.setCursor(0, 1);
lcd.print("Speed:");
lcd.setCursor(7, 1);
lcd.print(speedPercentage);
lcd.print("%   ");

}

// Function to go to a specified position
void goToPosition(int targetPosition) {
  if (homed) { // Only move to a position if the motor has been homed
    stepper.moveTo(targetPosition);
    while (stepper.run()) {
      // Wait for the stepper to reach the target position
    }
  }
}