/*
   Gestion d'un passage à niveau avec 2 capteurs ILS.

   Bibliothèques utilisées :
   - SlowMotionServo pour les barrières
   - LightDimmer pour le clignotement du feu du PN

*/

#include <Servo.h>

#include <SlowMotionServo.h>

/*
 * Décommentez la ligne ci-dessous pour avoir l'affichage 
 * de l'état courant de l'automate sur les broches 8 à 12
 */
// #define AVEC_AFFICHAGE_ETAT

/*-------------------------------------------------------------------*/
/* Les états possible pour le PN */
enum { PN_OUVERT, PN_SE_FERME, PN_FERME, PN_TEMPO, PN_S_OUVRE };

/*-------------------------------------------------------------------*/
/* Les pin des capteurs et les informations renvoyées                   */
const byte capteurPin1 = 2;
const byte capteurPin2 = 6;
const byte trainPresent = LOW;
const byte trainAbsent = HIGH;




/*-------------------------------------------------------------------*/
/* Les pins des servos */
const byte barriere1Pin1 = 4;
const byte barriere2Pin1 = 5;
const byte barriere1Pin2 = 4;
const byte barriere2Pin2 = 5;

/* Les positions cibles des barrières */
const float positionOuverteBarriere1 = 0.0;
const float positionFermeeBarriere1 = 1.0;
const float positionOuverteBarriere2 = 0.0;
const float positionFermeeBarriere2 = 1.0;

/* Les servos */
SMSSmooth barriere1;
SMSSmooth barriere2;

/*-------------------------------------------------------------------*/
/* La temporisation entre la fin du train et l'ouverture des barrières
   en ms
*/
const unsigned long temporisationOuverture = 5000;

#ifdef AVEC_AFFICHAGE_ETAT
const byte ledEtat = 8;

void initAffEtat()
{
  for (byte i = ledEtat ; i < ledEtat + 5; i++) {
    pinMode(i, OUTPUT);
    digitalWrite(i, LOW);
  }
}

void afficheEtat(const byte etat)
{
  for (byte i = ledEtat ; i < ledEtat + etat; i++) {
    digitalWrite(i, LOW);
  }
  for (byte i = ledEtat + etat + 1 ; i < ledEtat + 5; i++) {
    digitalWrite(i, LOW);
  }
  digitalWrite(ledEtat + etat, HIGH);
}

#define INITAFFETAT() initAffEtat()
#define AFFICHEETAT(e) afficheEtat(e)
#else
#define INITAFFETAT()
#define AFFICHEETAT(e)
#endif

/*-------------------------------------------------------------------*/
/* Intialisations */
void setup()
{
  /* capteur de présence */
  pinMode(capteurPin1, INPUT_PULLUP);
  pinMode(capteurPin2, INPUT_PULLUP);
 
  /* les barrières */
    barriere1.setPin(barriere1Pin1);
    barriere1.setPin(barriere1Pin2);
    barriere1.setSpeed(2.0);
    barriere1.setInitialPosition(0.5);
    barriere1.goTo(positionOuverteBarriere1);
    barriere2.setPin(barriere2Pin1);
    barriere2.setPin(barriere2Pin2);
    barriere2.setSpeed(2.0);
    barriere2.setInitialPosition(0.5);
    barriere2.goTo(positionOuverteBarriere2);    
  INITAFFETAT();
}

/*-------------------------------------------------------------------*/
/* Contrôle du PN */
void loop()
{
  static byte etatPN = PN_OUVERT;
  static unsigned long dateOuverture;

  /* Fait fonctionner les servos */
  SlowMotionServo::update();

  AFFICHEETAT(etatPN);
  
  switch (etatPN)
  {
    case PN_OUVERT:
      /* lecture du capteur pour détecter un train */
      if (digitalRead(capteurPin1) == trainPresent)
      if (digitalRead(capteurPin2) == trainPresent){
        /* train présent */
        /* On ferme les barrière */
        barriere1.goTo(positionFermeeBarriere1);
        barriere2.goTo(positionFermeeBarriere2);
        /* Le PN est en train de se fermer */
        etatPN = PN_SE_FERME;
      }
      break;

    case PN_SE_FERME:
      /* Si les deux servos sont arrêtés, le PN a terminé de se fermer */
      if (barriere1.isStopped() && barriere2.isStopped()) {
        etatPN = PN_FERME;
      }
      break;

    case PN_FERME:
      /* lecture du capteur pour détecter le départ du train */
      if (digitalRead(capteurPin1) == trainAbsent)
      if (digitalRead(capteurPin2) == trainAbsent){
        dateOuverture = millis() + temporisationOuverture;
        etatPN = PN_TEMPO;
      }
      break;

    case PN_TEMPO:
      /* Un train est présent, on retourne à l'état PN_FERME */
      if (digitalRead(capteurPin1) == trainPresent)
      if (digitalRead(capteurPin2) == trainPresent){
        etatPN = PN_FERME;
      }
      else {
        /* Si la date est arrivée, on ouvre */
        if (millis() >= dateOuverture) {
          /* On ouvre les barrière */
          barriere1.goTo(positionOuverteBarriere1);
          barriere2.goTo(positionOuverteBarriere2);
          etatPN = PN_S_OUVRE;
        }
      }
      break;

    case PN_S_OUVRE:
      /* Un train est présent, on retourne à l'état PN_SE_FERME et on ferme la barrière */
      if (digitalRead(capteurPin1) == trainPresent)
      if (digitalRead(capteurPin2) == trainPresent){
        barriere1.goTo(positionFermeeBarriere1);
        barriere2.goTo(positionFermeeBarriere2);
        etatPN = PN_SE_FERME;
      }
      else {
        /* Si les deux servos sont arrêtés, le PN a terminé de s'ouvrir */
        if (barriere1.isStopped() && barriere2.isStopped()) {
          etatPN = PN_OUVERT;
        }
      }
      break;

    default:
      /* normalement impossible */
      break;
  }
}