#include "SpeedyStepper.h"

#define MOTOR1_STEP_PIN 2
#define MOTOR1_DIR_PIN 5
#define MOTOR2_STEP_PIN 3
#define MOTOR2_DIR_PIN 6

#define JOYSTICK_X_PIN A1
#define JOYSTICK_Y_PIN A0

#define JOYSTICK_THRESHOLD 250
const int MAX_MOTOR_SPEED = 12500;

SpeedyStepper motor1;
SpeedyStepper motor2;
int targetDirection1 = 0;
void setup() {
  motor1.connectToPins(MOTOR1_STEP_PIN, MOTOR1_DIR_PIN);
  motor2.connectToPins(MOTOR2_STEP_PIN, MOTOR2_DIR_PIN);
  Serial.begin(9600);
  motor1.setAccelerationInStepsPerSecondPerSecond(2000);
  motor2.setAccelerationInStepsPerSecondPerSecond(2000);
}

void loop() {
  int xValue = analogRead(JOYSTICK_X_PIN);
  int yValue = analogRead(JOYSTICK_Y_PIN);

  int centeredX = xValue - 512;
  int centeredY = yValue - 512;
  static int lastX, lastY;
  if (centeredX != lastX || centeredY != lastY) {
    Serial.print(centeredX);
    Serial.print(',');
    Serial.print(centeredY);
    Serial.println();
    lastX = centeredX, lastY = centeredY;
  
    // Contrôle du moteur 1 avec arrêt forcé si dans la zone morte
    if (abs(centeredX) > 10) {

      motor1.setSpeedInStepsPerSecond(abs(1000));
      motor1.setupRelativeMoveInSteps(centeredX>0?50000:-50000);
    } else {
      // Forcer l'arrêt en réinitialisant la position cible
      // motor1.setTargetPositionInSteps(motor1.getCurrentPositionInSteps());
      motor1.setupStop();
    }

    // Contrôle du moteur 2 avec arrêt forcé si dans la zone morte
    if (abs(centeredY) > 10) {
      motor2.setSpeedInStepsPerSecond(abs(1000));
      motor2.setupRelativeMoveInSteps(centeredY>0?50000:-50000);
    } else {
      // Forcer l'arrêt en réinitialisant la position cible
      //motor2.setTargetPositionInSteps(motor2.getCurrentPositionInSteps());
      motor2.setupStop();
    }
}
  // Serial.println(centeredX);
  // Processus de mouvement pour les deux moteurs
  motor1.processMovement();
  motor2.processMovement();
}