#include <AgileStateMachine.h>
#include "pins.h"

// The Finite State Machine
StateMachine fsm;

// Memorizzo il tempo di cambio stato per stampare la durata dello stato stesso
uint32_t lastChangeState;

// Il tempo totale del rosso, 
// sarà equivalente al verde + giallo dell'altra corsia
uint32_t greenTimeA = 20000;   
uint32_t yellowTimeA = 5000;
uint32_t greenTimeB = 10000;   
uint32_t yellowTimeB = 5000;

// Uso una variabile per determinare quale sarà la corsia a ricevere il verde
enum {LANE_A, LANE_B};
uint8_t nextLane = LANE_A;

// Input State Machine interface
bool inPedestrian;

// The finite state
State *stRedA;
State *stRedB;
State *stGreenA;  
State *stGreenB;
State *stYellowA;  
State *stYellowB;

// Questa funzione di callback viene eseguita ogni volta che uno stato è attivato
void setLight() {
  State* current = fsm.getCurrentState();

  if (current == stRedA )
    for (byte i=0; i<3; i++) { 
      digitalWrite(outs_A1[i], RED[i]);
      digitalWrite(outs_A2[i], RED[i]);
    }
  else if (current == stRedB )
    for (byte i=0; i<3; i++) {
      digitalWrite(outs_B1[i], RED[i]);
      digitalWrite(outs_B2[i], RED[i]);
    }
  else if (current == stGreenA )
    for (byte i=0; i<3; i++) {
      digitalWrite(outs_A1[i], GREEN[i]);
      digitalWrite(outs_A2[i], GREEN[i]);
    }
  else if (current == stGreenB )
    for (byte i=0; i<3; i++) {
      digitalWrite(outs_B1[i], GREEN[i]);
      digitalWrite(outs_B2[i], GREEN[i]);
    }
  else if (current == stYellowA )
    for (byte i=0; i<3; i++) {
      digitalWrite(outs_A1[i], YELLOW[i]);
      digitalWrite(outs_A2[i], YELLOW[i]);
    }
  else if (current == stYellowB )
    for (byte i=0; i<3; i++) {
      digitalWrite(outs_B1[i], YELLOW[i]);
      digitalWrite(outs_B2[i], YELLOW[i]);
    }
}

// Questa funzione di callback viene eseguita in uscita di stYellowA e stYellowB
void changeLane() {
  nextLane = (nextLane == LANE_A) ? LANE_B : LANE_A;
  // nextLane = nextLane ? LANE_A : LANE_B;    
}


// Modello ed avvio la macchina a stati finiti
void setupStateMachine() {
  stRedA = fsm.addState("ROSSO_A", setLight);
  stRedB = fsm.addState("ROSSO_B", setLight);
  stGreenA = fsm.addState("VERDE_A", setLight);
  stGreenB = fsm.addState("VERDE_B", setLight);
  stYellowA = fsm.addState("GIALLO_A", setLight, changeLane);
  stYellowB = fsm.addState("GIALLO_B", setLight, changeLane);

  // Transizioni "semplici": trigger sul timeout dello stato
  stGreenA->addTransition(stYellowA, greenTimeA);
  stYellowA->addTransition(stRedA, yellowTimeA);
  stGreenB->addTransition(stYellowB, greenTimeB);
  stYellowB->addTransition(stRedB, yellowTimeB);

  // Quando siamo in stRedxx tutti i semafori sono rossi: 
  // dobbiamo attendere 2 secondi prima di dare il verde all'altra strada
  stRedB->addTransition(stRedA, []() {
    return ((millis() - stRedB->getEnterTime() >= 2000) && (nextLane == LANE_A));
  });  
  // oppure dare il verde quando è il nostro turno
  stRedA->addTransition(stGreenA, []() {
    // Gestione chiamata pedonale
    if (inPedestrian) {
      inPedestrian = false;
      nextLane = LANE_B;
    }
    return (nextLane == LANE_A);
  });

  // Stesso meccanismo, ma speculare
  stRedA->addTransition(stRedB, []() {
    return ((millis() - stRedA->getEnterTime() >= 2000) && (nextLane == LANE_B));
  });
  stRedB->addTransition(stGreenB,  []() {
    return (nextLane == LANE_B);
  });

  // Gestione chiamata pedonale
  stGreenA->addTransition(stRedA, inPedestrian);
  
  fsm.setInitialState(stRedA);
  fsm.start();
  Serial.print(F("Stato attivo "));
  Serial.print(fsm.getActiveStateName());
}



// Includo il codice per la macchina a stati finiti del semaforo pedonale
#include "pedestrian.h"


void setup() {
  Serial.begin(115200);
	Serial.println("START");
  pinMode(inButton, INPUT_PULLUP);
  for (const byte out : outsPed) pinMode(out, OUTPUT);
  for (const byte out : outs_A1) pinMode(out, OUTPUT);
  for (const byte out : outs_A2) pinMode(out, OUTPUT);
  for (const byte out : outs_B1) pinMode(out, OUTPUT);
  for (const byte out : outs_B2) pinMode(out, OUTPUT);
  testLeds();
  delay(1000);
  setupStateMachine();
  setupPedestrianStateMachine();

  // Imposto lo stato iniziale dei led (rosso A, rosso B)
  for (byte i=0; i<3; i++) { 
    digitalWrite(outs_A1[i], RED[i]);
    digitalWrite(outs_A2[i], RED[i]);
  }
  for (byte i=0; i<3; i++) {
    digitalWrite(outs_B1[i], RED[i]);
    digitalWrite(outs_B2[i], RED[i]);
  }
  lastChangeState = millis();
}


void loop() {
  // Calcola il tempo di ciclo corrente
  totalLoopTime +=  micros() - previousTime;
  previousTime = micros();
  loopCount++;

  // Run State Machine
  // All outputs will be handled inside onEnter callback function
  if (fsm.execute()) {
    Serial.print(F(" +"));
    Serial.print(millis() - lastChangeState);
    Serial.print(F(" ms\nfree heap "));
    Serial.print(freeMemory());
    Serial.print(F(" bytes, loop "));
    Serial.print(averageLoopTime);
    Serial.print(F(" us, Stato attivo "));
    Serial.print(fsm.getActiveStateName());
    lastChangeState = millis();
  }

  fsmPedestrian.execute();  

  // Se sono stati eseguiti 1000 cicli, calcola la media del tempo di ciclo loop()
   if (loopCount == 1000) {
    averageLoopTime = totalLoopTime / loopCount;
    totalLoopTime = 0;
    loopCount = 0;
  }

}