// Link al blog: https://programmersqtcpp.blogspot.com/

#include<Servo.h>
#include "treno.h"

GenericFSM plFsm;
Treno trenoA(11);
//===========================================================

const byte LED_A = 13;
const byte LED_B = 12;

const byte PIN_PULS  = 5;
const byte PREMUTO   = LOW;

bool lampeggioAbilitato = 0;
bool avvioChiusura = 0;

const byte PIN_SERVO_A = 4;
const byte MAXPOS = 90;
const byte MINPOS = 0;
byte   pos = MINPOS;
Servo myservoA;

//===========================================================

void setup(void)
{
  Serial.begin(115200);
  pinMode(PIN_PULS, INPUT_PULLUP);
  pinMode(LED_A, OUTPUT);
  pinMode(LED_B, OUTPUT);
  myservoA.attach(PIN_SERVO_A);
  myservoA.write(pos);
  plFsm.setState(1);   // imposto lo stato iniziale
  /*for (byte i=0; i<255; i++) {
      analogWrite(11, i);
      delay(500);
    }*/
  

}

//===========================================================

void loop(void)
{
  trenoA.run();
  //trenoA.stop();
  trenoA.command(1);

  //now = millis();

  if (digitalRead(PIN_PULS) == PREMUTO)
  {
    avvioChiusura = 1;
  }

  gestionePL();
  lampeggiante();
}

//===========================================================

void gestionePL(void)
{
  plFsm.run();
  // azzera tempo trascorso se fase cambiata
  /*if (fasePL != fasePLprec)
    {
      fasePLprec = fasePL;
      tPL = now;
    }*/

  // questo potrebbe essere integrato nella classe
  unsigned long trascorso = millis() - plFsm.getTimer();


  // fase attesa comando chiusura P.L.
  if ((plFsm.getState() == 1) && (avvioChiusura == 1))
  {

    lampeggioAbilitato = 1;
    plFsm.setState(2);  // CHANGE STATE
  }

  // fase pausa 3s con solo lampeggio luci
  else if ((plFsm.getState() == 2) && (trascorso >= 3000))
  {
    // non posso usare onEnter(), vedi stato 3
    plFsm.setState(3);  // CHANGE STATE
  }

  // fase chiusura lenta
  else if (plFsm.getState() == 3)
  {
    if (plFsm.onEnter()) {
      Serial.println("Entro in Fase 3");
    }
    if (trascorso >= 30) {
      pos += (pos < MAXPOS);
      myservoA.write(pos);
      //tPL = now;
      plFsm.setTimer();
      if (pos == MAXPOS)
      {
        plFsm.setState(4);  // CHANGE STATE
        //fasePL = 4;
      }
    }
  }

  // fase pausa 10s chiuso
  else if ((plFsm.getState() == 4) && (trascorso >= 10000))
  {
    plFsm.setState(5);  // CHANGE STATE
    //fasePL = 5;
  }

  // fase apertura lenta
  else if ((plFsm.getState() == 5) && (trascorso >= 30))
  {
    pos -= (pos > MINPOS);
    myservoA.write(pos);
    //tPL = now;
    plFsm.setTimer();
    if (pos == MINPOS)
    {
      plFsm.setState(6);  // CHANGE STATE
      //fasePL = 6;
    }
  }

  // fase pausa 3s con solo lampeggio luci
  else if ((plFsm.getState() == 6) && (trascorso >= 3000))
  {
    avvioChiusura = 0;
    lampeggioAbilitato = 0;
    plFsm.setState(1);  // CHANGE STATE
    //fasePL = 1;
  }


  else  // se nessuna delle precedenti
  {
    //  nessuna operazione
  }
}

//===========================================================

void lampeggiante(void)
{
  if (lampeggioAbilitato)
  {
    bool sl = ((millis() % 1000)  <  500);
    digitalWrite(LED_B, sl);
    digitalWrite(LED_A, !sl);
  }
  else
  {
    digitalWrite(LED_A, LOW);   // entrambi spenti
    digitalWrite(LED_B, LOW);
  }
}

//===========================================================