/*
   Fichier principal.
   Auteur : Ivann
*/

#include "Grove_I2C_Motor_Driver.h"
#include "moteur.h"
#include "Emetteur.h"
#include "capteur.h"
#include "Position.h"

// Les librairies permettant de gérer les moteurs et le module suiveur de ligne.
Moteur moteur;
CapteurDeLigne capteur;
Emetteur emit;
Position pos;

int state = 0; // Variable d'état permettant de savoir si l'on est dans un virage ou sur un replacement sur la voie.
int vitesse;

void setup() {
  // Configuration initiale des moteurs et du module suiveur de ligne.
  capteur.setup();
  moteur.setup();

  pinMode(capteur.getSignalPinRight(), INPUT); // initialiser les suiveurs de ligne en sortie digitale 0 ou 1
  pinMode(capteur.getSignalPinMiddle(), INPUT);   // initialiser les suiveurs de ligne en sortie digitale 0 ou 1
  pinMode(capteur.getSignalPinLeft(), INPUT);  // initialiser les suiveurs de ligne en sortie digitale 0 ou 1.
  pinMode(capteur.getfourchePin(), INPUT);     // initialiser le tachymetre en entrée digitale 0 ou 1.
  Serial.begin(9600); // Connexion au port série.
  moteur.moveForward(); // On indique aux moteurs d'avancer.
  emit.setup();
  pos.startTravel(0, 4);
}

void loop() {
  Serial.println(state);

  if ((capteur.capteurDroit() && capteur.capteurMid()) || (capteur.capteurGauche() && capteur.capteurMid())) {
    if (pos.toPerform() > 0) {
      if (pos.toPerform() == 0) {
        // Tourner à droite.
        moteur.turnRight();
        delay(500);
      } else if (pos.toPerform() == 1) {
        // Tourner à gauche.
        moteur.turnLeft();
        delay(500);
      } else if (pos.toPerform() == 999) {
        // On s'arrête.
        moteur.stopMoving();
        delay(10000);
      }
    } else {
      // Tout droit.
      moteur.moveForward();
      delay(500);
    }
    pos.passNextNode();

  } else if (capteur.capteurDroit() && (!(capteur.capteurMid()))) {
    moteur.turnRight(); // On tourne à droite.
    state = 1;

    // Si le capteur central est activé.
  } else if (capteur.capteurMid() && (!(capteur.capteurDroit())) && (!(capteur.capteurGauche()))) {
    moteur.moveForward();
    state = 0;

    //capteur.calculateDistance(emit);

    // Si le capteur de gauche est activé.
  } else if (capteur.capteurGauche() && (!(capteur.capteurMid()))) {
    moteur.turnLeft();
    state = 1;

    // Si aucun capteur n'est activé.
  } else if (!(capteur.capteurGauche()) && (capteur.capteurMid()) && (capteur.capteurDroit())) {
    if (state == 0) {
      moteur.turnRight();
    }

    // Si les capteurs latéraux sont activés.
  } else if ((capteur.capteurGauche() && (capteur.capteurDroit()) )) {
    moteur.turnRight();
    state = 0;

    // Si tous les capteurs sont activés.
  } else if (capteur.capteurGauche() && capteur.capteurDroit() && capteur.capteurMid()) {
    moteur.turnRight();
    state = 0;
  }
}