#include <Servo.h>

#define NB_AIGUILLAGES          21   // nombre d'aiguillages à piloter (mettez vos propres valeurs)
#define START_PIN               22   // Première des pins affectées aux servos 

char msgString[12];  // Tableau qui va recevoir les caractères envoyés
const int initPos = 1500;  // Position initiale des servos (en microsecondes)

typedef struct { // Déclaration d'une structure "Aiguillage"
  int val;   // Angle (en microsecondes)
  char sens; // Sens de rotation
  long del;  // Tempo (en millisecondes)
  int cur;   // Position courante
  bool act;  // En cours de changement
  unsigned long prev; // Dernière valeur de temps stockée
} Aiguillage ;

Aiguillage aig[NB_AIGUILLAGES]; // Création d'un tableau d'aiguillages (aig[ ]) à partir de la structure "Aiguillage"
Servo myServo[NB_AIGUILLAGES];  // Création d'un tableau d'objets servos à partir du constructeur Servo

void receipt () {
  // Lecture du port série
  char c;
  while (Serial.available()) {
    c = Serial.read();
    if (c == '<') {               // Caractère délimiteur de début de message
      sprintf(msgString, "");
    }
    else if (c == '>')  {         // Caractère délimiteur de fin de message
      parse(msgString);           // Appel de la fonction parse()
    }
    else {                        // Caractères du message
      sprintf(msgString, "%s%c", msgString, c);
    }
  }
}

void parse(char *msgString) {
  int n;  // Numéro de l'aiguillage
  int v;  // Valeur à atteindre pour le servo
  int d;  // Vitesse de rotation

  sscanf(msgString, "%d%d%d", &n, &v, &d);

  aig[n].val = v; // Affectation des différentes valeurs pour l'aiguillage n
  aig[n].del = d;
  aig[n].act = true;
  aig[n].prev = 0;
  if (aig[n].val > aig[n].cur) aig[n].sens = '+';      // Le sens est positif
  else if (aig[n].val < aig[n].cur) aig[n].sens = '-'; // Le sens est négatif
  else aig[n].act = false; // La position demandée est la même que la position courante, on ne fait rien
}

void process () {
  // Application des modifications
  for (int i = 0; i < NB_AIGUILLAGES; i++) {
    if (aig[i].act == true) {                           // Une action est en cours sur cet aiguillage
      unsigned long currentMillis = millis();           // On affecte à la variable "currentMillis" l'heure courante
      if (currentMillis - aig[i].prev >= aig[i].del) {  // Si le délais d'attente pour cet aiguillage est dépassé
        if (aig[i].sens == '+') aig[i].cur ++;          // On incrémente la position courante de cet aiguillage de 1
        else if (aig[i].sens == '-') aig[i].cur --;     // On décrémente la position courante de cet aiguillage de 1
        myServo[i].writeMicroseconds(aig[i].cur);       // On envoie au servo la valeur à appliquer
        aig[i].prev = currentMillis;                    // Le prev pour cet aiguillage prend la valeur currentMillis
      }
      if (aig[i].cur == aig[i].val) {                   // Le servo a atteint son angle final
        aig[i].act = false;                             // La valeur de act (action) est modifiée à false
      }
    }
  }
}


void setup() {

  /////--- COMMUNICATION SERIE---////
  Serial.begin(115200);

  ///////////--- SERVOS---///////////
  for (int i = 0; i < NB_AIGUILLAGES; i++) {            // Initialisation de chaque élément des tableaux myServo et aig
    myServo[i].attach(START_PIN + i);
    myServo[i].writeMicroseconds(initPos);
    delay(200);
    aig[i].val = initPos;
    aig[i].del = 0;
    aig[i].cur = initPos;
    aig[i].act = false;
    aig[i].prev = 0;
    delay(200);
  }
  Serial.println("Init. ok");
}

void loop() {
  receipt();
  process();
}