#include <AccelStepper.h>
const int ledPin = 17;
// Define pins for stepper motor drivers
#define X_STEP_PIN 3
#define X_DIR_PIN 2
#define Y_STEP_PIN 5
#define Y_DIR_PIN 4
#define Z_STEP_PIN 6
#define Z_DIR_PIN 7

#define saveButton 8
#define retrieveButton 10
#define saveButton_b 11
#define retrieveButton_b 12
#define BUTTON1_PIN 0
#define BUTTON2_PIN 1
#define BUTTON3_PIN 9

// Define pins for joystick and potentiometers
#define JOYSTICK_X A0
#define JOYSTICK_Y A1
#define JOYSTICK_Z A2
#define SPEED_POT_X A3
#define DAMPING_POT_X A4
#define xPOT_PIN A5


#define stepsPerRevolution 5
// Define the acceleration and deceleration rates
#define ACCELERATION 500
#define DECELERATION 100
// Define the maximum and minimum acceleration values 200-10
#define maxAccel 200
#define minAccel 10
int val = 0; // variable to store the input pin value

// Define stepper motor direction constants
#define DIRECTION_CW true    // Clockwise direction
#define DIRECTION_CCW false  // Counterclockwise direction

// Initialize button states
int button1State = HIGH;
int button2State = HIGH;
int button3State = HIGH;
// Initialize direction variable
bool stepperDirection = DIRECTION_CW;

// Define variables for joystick and button states
int saveButtonState = 0;
int retrieveButtonState = 0;
int saveButtonState_b = 0;
int retrieveButtonState_b = 0;

// Define variables for saved motor position
long savedPosition = 0;
long savedPosition_b = 0;

// Define variables for joystick and potentiometers readings
int joy_x, joy_y, joy_z ;
int speed_pot_X_val, damping_pot_X_val;


// Define stepper motors objects with steps per revolution and driver pins
AccelStepper x_stepper(AccelStepper::DRIVER, X_STEP_PIN, X_DIR_PIN);
AccelStepper y_stepper(AccelStepper::DRIVER, Y_STEP_PIN, Y_DIR_PIN);
AccelStepper z_stepper(AccelStepper::DRIVER, Z_STEP_PIN, Z_DIR_PIN);

void setup() {
pinMode(ledPin, OUTPUT);

// Set up serial communication for debugging
  Serial.begin(9600);

  // Set up pins for buttons as inputs with pull-up resistors
  pinMode(BUTTON1_PIN, INPUT_PULLUP);
  pinMode(BUTTON2_PIN, INPUT_PULLUP);
  pinMode(BUTTON3_PIN, INPUT_PULLUP);
  pinMode(saveButton, INPUT);
  pinMode(retrieveButton, INPUT);
  pinMode(saveButton_b, INPUT);
  pinMode(retrieveButton_b, INPUT);

 // Set the motor speed and acceleration/deceleration rates

  x_stepper.setAcceleration(-DECELERATION);
  y_stepper.setAcceleration(-DECELERATION);
  z_stepper.setAcceleration(-DECELERATION);


  // Set maximum speed and acceleration for each stepper motor object 
  x_stepper.setMaxSpeed(200);
  x_stepper.setAcceleration(500);
  y_stepper.setMaxSpeed(200);
  y_stepper.setAcceleration(500);
  z_stepper.setMaxSpeed(200);
  z_stepper.setAcceleration(500);

  

}

void loop() {


 // Read the potentiometer value and map it to the deceleration rate
  int xpotValue = analogRead(xPOT_PIN);
  int deceleration = map(xpotValue, 0, 1023, 0, 10);
  // Set the deceleration rate for the stepper motor
  x_stepper.setAcceleration(-deceleration);
  y_stepper.setAcceleration(-deceleration);
  z_stepper.setAcceleration(-deceleration);

 // Move the stepper motor based on the joystick Y value
  x_stepper.move(joy_x);
  val = digitalRead(18); // read the input pin
  if (val == LOW) { // if button is pressed
  x_stepper.run();
   

  } 

// Read the button states and change the direction of the stepper motor accordingly
  button1State = digitalRead(BUTTON1_PIN);
  button2State = digitalRead(BUTTON2_PIN);
  button3State = digitalRead(BUTTON3_PIN);

if (button1State == LOW) {
    stepperDirection = DIRECTION_CW;  // Set the direction to clockwise
  }

  if (button1State == HIGH) {
    stepperDirection = DIRECTION_CCW;  // Set the direction to counterclockwise
  }
  x_stepper.setPinsInverted(stepperDirection);


if (button2State == LOW) {
    stepperDirection = DIRECTION_CW;  // Set the direction to clockwise
  }

  if (button2State == HIGH) {
    stepperDirection = DIRECTION_CCW;  // Set the direction to counterclockwise
  }
  y_stepper.setPinsInverted(stepperDirection);


if (button3State == LOW) {
    stepperDirection = DIRECTION_CW;  // Set the direction to clockwise
  }

  if (button3State == HIGH) {
    stepperDirection = DIRECTION_CCW;  // Set the direction to counterclockwise
  }
  z_stepper.setPinsInverted(stepperDirection);



  saveButtonState = digitalRead(saveButton);
  retrieveButtonState = digitalRead(retrieveButton);
  saveButtonState_b = digitalRead(saveButton_b);
  retrieveButtonState_b = digitalRead(retrieveButton_b);

if (saveButtonState == HIGH) {
   
    digitalWrite(ledPin, HIGH);
    }


    if (retrieveButtonState == HIGH) {
   
    digitalWrite(ledPin, LOW);
    }
// Save motor position on save button press
  if (saveButtonState == HIGH) {
   
    savedPosition = z_stepper.currentPosition();
    
  }


  
  // Save motor position b on save button press
  if (saveButtonState_b == HIGH) {
    savedPosition_b = z_stepper.distanceToGo();
   
z_stepper.moveTo(saveButtonState_b);
  }

  // Move motor to saved position on retrieve button press
  if (retrieveButtonState == HIGH) {
     z_stepper.moveTo(savedPosition);
  }
    // Move motor to saved position on retrieve button press
  if (retrieveButtonState_b == HIGH) {
    z_stepper.moveTo(savedPosition);
  }

  // Run stepper motor
  z_stepper.run();
  

  
  // Read joystick values and map them to -100 to +100 range 
  joy_x = map(analogRead(JOYSTICK_X),0,1023,-100,100);
  joy_y = map(analogRead(JOYSTICK_Y),0,1023,-100,100);
  joy_z = map(analogRead(JOYSTICK_Z),0,1023,-100,100);

   // Read potentiometer values and map them to appropriate ranges 
   speed_pot_X_val = map(analogRead(SPEED_POT_X),0,1023,0,200); // Speed range: [10-200] steps/s 
   damping_pot_X_val = map(analogRead(DAMPING_POT_X),0,1023,0,1000); // Damping range: [-50--10] %

   

   // Set speed and damping for each stepper motor object based on potentiometer readings 
   x_stepper.setMaxSpeed(speed_pot_X_val * (1 + damping_pot_X_val/100.0));
   y_stepper.setMaxSpeed(speed_pot_X_val * (1 + damping_pot_X_val/100.0));
   z_stepper.setMaxSpeed(speed_pot_X_val * (1 + damping_pot_X_val/100.0));
   // Move each stepper motor based on joystick readings 
   x_stepper.move(joy_x);
   y_stepper.move(joy_y);
   z_stepper.move(joy_z);
   // Run each stepper motor until it reaches its target position 

   x_stepper.run();
   y_stepper.run();
   z_stepper.run();
   
  


    }