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

#define saveButton 22 // save a
#define retrieveButton 24 // play a
#define saveButton_b 26 // save b
#define retrieveButton_b 28 // play b
#define referencButton 32 // referenc Button
#define BUTTON1_PIN 14 // dir x
#define BUTTON2_PIN 15 // dir y
#define BUTTON3_PIN 16 // dir z

// Define pins for joystick and potentiometers
#define JOYSTICK_X A0
#define JOYSTICK_Y A1
#define JOYSTICK_Z A2
#define SPEED_POT_X A8
#define DAMPING_POT_X A11 
#define xPOT_PIN A14
boolean muoviX, muoviY, muoviZ, enable, enableZ;  //variabili di gestione dei movimenti

// Define the acceleration and deceleration rates
#define ACCELERATION 0
#define DECELERATION 0
//variabili utilizzate dalla libreria AccelStepper
const int maxSpeed = 500;  //stando alla documentazione della libreria questo valore può essere impostato fino a 4000 per un Arduino UNO
const int minSpeed = 0; //velocità minima del motore
// Define the acceleration and deceleration rates
// Define the maximum and minimum acceleration values 200-10
#define maxAccel 500
#define minAccel 0
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;



// Define pins for joystick and potentiometers
const int jX = A0;         //pin analogico che legge i valori per le X
long speedX, valX, mapX;  //variabili di gestione movimenti motore X
const int jY = A1;        //pin analogico che legge i valori per le Y
long speedY, valY, mapY;  //variabili di gestione movimenti motore Y
const int jZ = A2;  //pin analogico che legge i valori per le Z
long speedZ, valZ, mapZ;  //variabili di gestione movimenti motore Z

//const int treshold = 80;  //la lettura dei potenziometri non è mai affidabile al 100%, questo valore aiuta a determinare il punto da considerare come "Stai fermo" nei movimenti
//long tresholdUp, tresholdDown;  //variabili di servizio per espletare il compito descritto sopra
//unsigned long debounceDelay = 0;


















// 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 speed_pot_X_val, damping_pot_X_val;

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



// 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(200);
  y_stepper.setMaxSpeed(0);
  y_stepper.setAcceleration(0);
  z_stepper.setMaxSpeed(0);
  z_stepper.setAcceleration(0);


//calcola range valori entro i quali considerare la posizione del joystick come "Stai fermo"
 // tresholdDown = (maxSpeed / 2) - treshold;
 // tresholdUp = (maxSpeed / 2) + treshold;
  

}

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, 100);
  // Set the deceleration rate for the stepper motor
 // x_stepper.setAcceleration(-deceleration);
 // y_stepper.setAcceleration(-deceleration);
 // z_stepper.setAcceleration(-deceleration);






//esegui lettura analogica dei valori provenienti dai potenziometri del joystick
  valX = analogRead(jX);
  valY = analogRead(jY);
  valZ = analogRead(jZ);
 //mappa i valori letti in funzione della velocità minima e massima
  mapX = map(valX, 0, 1023, minSpeed, maxSpeed);
  mapY = map(valY, 0, 1023, minSpeed, maxSpeed);
  mapZ = map(valZ, 0, 1023, minSpeed, maxSpeed);
 //esegui funzione di comando dei motori
 
























 // Move the stepper motor based on the joystick Y value
  x_stepper.move(joy_x);
  val = digitalRead(50); // 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,1036,-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,15000); // 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();
   
  


    }