// --- Déclarations des pins ---
#define STEP_G 2
#define DIR_G 3
#define STEP_D 4
#define DIR_D 5

#define JOY_X A0
#define JOY_Y A1
#define JOY_BTN 0

// --- Constantes mécaniques ---
const float entraxe = 350.0;         // Distance entre les moteurs (mm)
const float longueurMax = 600.0;     // Longueur max de chaque courroie (mm)
const float hauteurMax = sqrt(longueurMax*longueurMax-(entraxe/2)*(entraxe/2));

const float diametrePoulie = 12.0;   // en mm
const int pasParTour = 200;
const float mmParPas = 3.1415*diametrePoulie/pasParTour;

const float ecartX = 34;
const float ecartY = 40;
const float coteNacelle = 80;

// --- Position actuelle (en mm) ---
float posX = entraxe / 2;
float posY = (diametrePoulie/2) + (coteNacelle/2)+20;
float lastPosX;
float lastPosY;

// --- Initialisation ---
void setup() {
  pinMode(STEP_G, OUTPUT);
  pinMode(DIR_G, OUTPUT);
  pinMode(STEP_D, OUTPUT);
  pinMode(DIR_D, OUTPUT);
  pinMode(JOY_BTN, INPUT_PULLUP);

  Serial.begin(9600);

  // Calcul initial des longueurs
  lastPosX = posX;
  lastPosY = posY;
}

// --- Boucle principale ---
void loop() {
  // === Lecture joystick ===
  int joyX = analogRead(JOY_X);
  int joyY = analogRead(JOY_Y);
  bool boutonAppuye = digitalRead(JOY_BTN) == LOW;

  if (boutonAppuye) {
    // Action à effectuer
    Serial.println("Bouton appuyé !");
    delay(300); // anti-rebond simple
  }

  // Traduire joystick → déplacement
  int dx = map(joyX, 0, 1023, -10, 10);
  int dy = map(joyY, 0, 1023, -10, 10);
  
  // Seuil mort pour éviter bruit
  if (abs(dx) < 2) dx = 0;
  if (abs(dy) < 2) dy = 0;

  // Appliquer le déplacement
  posX = lastPosX + dx;
  posY = lastPosY + dy;

  // Contraintes
  posX = constrain(posX, 0, entraxe);
  posY = constrain(posY, (diametrePoulie/2) + (coteNacelle/2)+20, hauteurMax);

  deplacerVersXY(posX, posY, lastPosX, lastPosY);

  lastPosX = posX;
  lastPosY = posY;

  delay(20);
}

// --- Déplacement synchronisé des moteurs ---
void deplacerMoteursSynchrones(int stepsG, int stepsD) {
  int dirG = stepsG > 0 ? HIGH : LOW;
  int dirD = stepsD > 0 ? HIGH : LOW;

  stepsG = abs(stepsG);
  stepsD = abs(stepsD);

  digitalWrite(DIR_G, dirG);
  digitalWrite(DIR_D, dirD);

  int maxSteps = max(stepsG, stepsD);
  int compteurG = 0;
  int compteurD = 0;

  for (int i = 0; i < maxSteps; i++) {
    if ((long)i * stepsG / maxSteps > compteurG) {
      digitalWrite(STEP_G, HIGH);
      compteurG++;
    }
    if ((long)i * stepsD / maxSteps > compteurD) {
      digitalWrite(STEP_D, HIGH);
      compteurD++;
    }
    delayMicroseconds(800);
    digitalWrite(STEP_G, LOW);
    digitalWrite(STEP_D, LOW);
    delayMicroseconds(800);
  }
}

void deplacerVersXY(float xCible, float yCible, float xActuel, float yActuel) {
  // Pas d'échantillonnage spatial (plus petit = plus fluide, plus lent)
  const float pas_mm = 0.5;

  // Calcul du vecteur déplacement
  float dx = xCible - xActuel;
  float dy = yCible - yActuel;
  float distance = sqrt(dx * dx + dy * dy);

  if (distance < 0.1) return; // Déplacement trop petit

  int steps = distance / pas_mm;
  float pasX = dx / steps;
  float pasY = dy / steps;

  for (int i = 0; i <= steps; i++) {
    float x = xActuel + pasX * i;
    float y = yActuel + pasY * i;

    // Longueurs de courroie pour chaque moteur
    float longueurG = sqrt((x-ecartX) * (x-ecartX) + (y-ecartY) * (y-ecartY));
    float longueurD = sqrt(sq(entraxe - x-ecartX) + (y-ecartY) * (y-ecartY));

    // Longueurs précédentes statiques
    static float longueurG_prec = sqrt(sq(xActuel-ecartX) + sq(yActuel-ecartY));
    static float longueurD_prec = sqrt(sq(entraxe - xActuel-ecartX) + sq(yActuel-ecartY));

    int stepsG = (longueurG - longueurG_prec) / mmParPas;
    int stepsD = (longueurD - longueurD_prec) / mmParPas;

    deplacerMoteursSynchrones(stepsG, stepsD);

    longueurG_prec += stepsG * mmParPas;
    longueurD_prec += stepsD * mmParPas;
  }

}