// Le code ci-dessous permet de déplacer le système de pollinisation artificielle. Les valeurs #define peuvent être modifiées selon le besoin de l'utilisateur, s'il n'est pas indiqué au contraire.
// Les unités de mesure sont en mm (milimètres), sauf indiqué par contraire

#include <Stepper.h>  // La bibliothèque pour déplacer les moteurs de pas-à-pas unipolaires
#include "parametres.h"
#define SPR 2048               // La quantité de pas dans une rotation du moteur !!! UNE CONSTANTE DU MOTEUR 28BYJ-48, NE CHANGE PAS !!!
#define THETA 0.0030679615625  // 5.625/64 degrées en RAD !!! UNE CONSTANTE DU MOTEUR 28BYJ-48, NE CHANGE PAS !!!
#define R 6                    // Le rayon de la partie attachée à la courroie !!! UNE CONSTANTE DU SYSTÈME, NE CHANGE PAS !!!

// La quantité de sous-divisions à prendre selon DIV_DIST
const int XDIV = (XMAX / DIV_DIST);
float translationConst = 0.0195068092;

// Définition des moteurs pas à pas et les broches de sorties de l'Arduino
// Stepper motor1(SPR, 8, 10, 9, 11);
// Stepper motor2(SPR, 4, 6, 5, 7);
Stepper motor1(SPR, 8, 10, 9, 11);
Stepper motor2(SPR, 4, 6, 5, 7);

// SÉQUENCE IN1, IN2, IN3, IN4 POUR LES PAS ET LA PILOTE ULN2003 !!

// Définition des valeurs qui seront utilisées lors de l'excécution du programme
int i, counter = 0;
float x = 0, y = 0;

// Mise à jour de la position Y du système
void updateVert(float *addressVar, int motorA, int motorB) {
  *addressVar += translationConst * (motorA - motorB) / 2;
}

// Mise à jour de la position X du système
void updateHor(float *addressVar, int motorA, int motorB) {
  *addressVar += translationConst * (motorA + motorB) / 2;
}

// Déplacer le système sur la droite des X. dir=1 indique x++. dir=-1 indique x--.
void moveHor(int dir, float *x) {
  if (dir == 1) {
    motor1.step(-STEP_INCR);
    motor2.step(STEP_INCR);
    updateHor(x, 1, 1);
  } else if (dir == -1) {
    motor1.step(STEP_INCR);
    motor2.step(-STEP_INCR);
    updateHor(x, -1, -1);
  }
}

// Déplacer le système sur la droite des Y. dir=1 indique y++. dir=-1 indique y--.
void moveVert(int dir, float *y) {
  if (dir == 1) {
    motor1.step(-STEP_INCR);
    motor2.step(-STEP_INCR);
    updateVert(y, 1, -1);
  } else if (dir == -1) {
    motor1.step(STEP_INCR);
    motor2.step(STEP_INCR);
    updateVert(y, -1, 1);
  }
}

// Pose une question et le stocke dans une variable de type float
void floatQR(char question[], float *addressVar) {
  Serial.print(question);
  while (Serial.available() == 0) {
  }
  *addressVar = Serial.parseFloat();
  Serial.println(*addressVar);
}

// Pose une question et le stocke dans une variable de type int
void intQR(char question[], int *addressVar) {
  Serial.print(question);
  while (Serial.available() == 0) {
  }
  *addressVar = Serial.parseInt();
  Serial.println(*addressVar);
}

// Imprime les coordonnées à chaque intervalle COORDINATE_INTERVAL
void printCoordinates(float x, float y, int *counter) {
  *counter = *counter + 1;
  if (*counter == (COORDINATE_INTERVAL - 1)) {
    Serial.print(x);
    Serial.print("\t");
    Serial.print(y);
    Serial.print("\n");
    *counter = 0;
  }
}

// Imprime les valeurs des paramètres actuels du système
void printInformation(void) {
  Serial.print("XMAX (mm) = ");
  Serial.println(XMAX);
  Serial.print("YMAX (mm) = ");
  Serial.println(YMAX);
  Serial.print("DIV_DIST (mm) = ");
  Serial.println(DIV_DIST);
  Serial.print("Constante de déplacement (translationConst) = ");
  Serial.println(translationConst, 10);
}

// Vérifie si l'utilisateur a saisi une entrée valide
void validEntry(int *addressVar) {
  if (*addressVar == 0) {
    Serial.println("Saisi non-valide, veuillez réessayer.");
  }
}

void setup() {
  Serial.begin(9600);
  // Initialization de la vitesse des moteurs
  motor1.setSpeed(SPEED);
  motor2.setSpeed(SPEED);
  int j = -1;
  while (j != 1 && j != 2) {
    validEntry(&j);
    intQR("Souhaitez-vous mettre le système à (0,0)? (1 = NON ; 2 = OUI): ", &j);
  }
  while (j == 2) {
    float cx = 0, cy = 0, dx, dy;
    floatQR("Saisissez une distance de changement de x (en mm): ", &dx);
    floatQR("Saisissez une distance de changement de y (en mm): ", &dy);
    if (dx >= 0 && dy >= 0) {
      while (cx < dx) {
        moveHor(1, &cx);
        printCoordinates(cx, cy, &counter);
      }
      while (cy < dy) {
        moveVert(1, &cy);
        printCoordinates(cx, cy, &counter);
      }
    } else if (dx <= 0 && dy >= 0) {
      while (cx > dx) {
        moveHor(-1, &cx);
        printCoordinates(cx, cy, &counter);
      }
      while (cy < dy) {
        moveVert(1, &cy);
        printCoordinates(cx, cy, &counter);
      }
    } else if (dx <= 0 && dy <= 0) {
      while (cx > dx) {
        moveHor(-1, &cx);
        printCoordinates(cx, cy, &counter);
      }
      while (cy > dy) {
        moveVert(-1, &cy);
        printCoordinates(cx, cy, &counter);
      }
    } else {
      while (cx < dx) {
        moveHor(1, &cx);
        printCoordinates(cx, cy, &counter);
      }
      while (cy > dy) {
        moveVert(-1, &cy);
        printCoordinates(cx, cy, &counter);
      }
    }
    j = -1;
    while (j != 1 && j != 2) {
      validEntry(&j);
      intQR("Souhaitez-vous effectuer un autre déplacement pour mieux trouver (0,0)? (1 = NON ; 2 = OUI): ", &j);
    }
  }

  j = -1;
  while (j != 1 && j != 2) {
    validEntry(&j);
    intQR("Souhaitez-vous effectuer une calibration du système? (1 = NON ; 2 = OUI): ", &j);
  }
  while (j == 2) {
    int pas = 0;
    float cx = 0, xi, xf;
    floatQR("Veuillez saisir la position initiale (en mm): ", &xi);
    while (cx < 50) {
      moveHor(1, &cx);
      pas++;
    }
    floatQR("Veuillez saisir la position finale (en mm): ", &xf);
    while (cx > 0) {
      moveHor(-1, &cx);
    }
    translationConst = (xf - xi) / pas;
    Serial.print("Constante de translation de ");
    Serial.println(translationConst, 10);

    if (translationConst > 0) {
      j = -1;
      while (j != 1 && j != 2) {
        validEntry(&j);
        intQR("Souhaitez-vous effectuer une autre calibration du système? (1 = NON ; 2 = OUI): ", &j);
      }
    } else {
      Serial.println("Saisi éronné. Veuillez réessayer.");
      j = 2, translationConst = R * THETA * STEP_INCR;
    }
  }
}

void loop() {
  int active = 0;
  while (active != 1) {
    printInformation();
    intQR("Souhaitez-vous mettre le système en marche? (1 = OUI): ", &active);
  }
  // Déplacement du système de la position (0, 0) à (XMAX, YMAX)
  for (i = XDIV; i >= 1; i--) {
    // Verifie si le système est sur une sous-division impaire et fait un déplacement de YMAX à 0
    if (i % 2 == 1) {
      while (y > 0) {
        moveVert(-1, &y);
        printCoordinates(x, y, &counter);
      }
      // Exécuté quand le système est sur une sous-division paire et fait un déplacement de 0 à YMAX
    } else {
      while (y < YMAX) {
        moveVert(1, &y);
        printCoordinates(x, y, &counter);
      }
    }
    // Déplace le système à la position (XMAX/(i), YMAX), d'où i est la sous-division actuelle du système
    while (x < (XMAX / i)) {
      moveHor(1, &x);
      printCoordinates(x, y, &counter);
    }
  }
  // Déplace le système à la position (XMAX, YMAX)
  while (y < YMAX) {
    moveVert(1, &y);
    printCoordinates(x, y, &counter);
  }
  // Maintenant que le système est à la position (XMAX, YMAX), on travaille à l'envers pour atteindre (0, 0)
  for (i = XDIV; i >= 1; i--) {
    if (i % 2 == 1) {
      while (y < YMAX) {
        moveVert(1, &y);
        printCoordinates(x, y, &counter);
      }
    } else {
      while (y > 0) {
        moveVert(-1, &y);
        printCoordinates(x, y, &counter);
      }
    }
    while (x > (XMAX - XMAX / i)) {
      moveHor(-1, &x);
      printCoordinates(x, y, &counter);
    }
  }
  while (y > 0) {
    moveVert(-1, &y);
    printCoordinates(x, y, &counter);
  }
}