/*
0.5
  framework for traffic light
*/

#include <Bounce2.h>
#include "libFsmA.h"
#include "misc.h"
#include "z_macros.h"
#include "stFunctions.h"
#include "trafficLight.h"

// all buttons
const uint8_t btnPins[] = { 3};
Bounce *buttons = new Bounce[NUMELEMENTS(btnPins)];

// print output with millis()
const bool enableMillis = true;
// debug fsm
const bool debugFsm = true;

enum StateNames : uint16_t
{
  stBB,   // blink tl and pl
  stRR,   // tl red, pl red
  stGR,   // tl green, pl red
  stX,    // delay cycle
  stOR,   // tl orange, pl red
  stRR1,  // tl red, pl red
  stRG,   // tl red, pl green
  stRB,   // tl red, pl blink green
  stRR2,  // tl red, pl red
};

Timer blinkTimer;  // timer for blink
Timer stateTimer;  // timer for states (how long do we stay in a state)

// create instances of the LEDs for the traffic light
const uint8_t tlRedPin = 6;
Led tlRed = Led(tlRedPin, LOW);
const uint8_t tlGreenPin = 8;
Led tlGreen = Led(tlGreenPin, LOW);
const uint8_t tlYellowPin = 7;
Led tlYellow = Led(tlYellowPin, LOW);


// create instances of the LEDs for the walker light
const uint8_t plRedPin = 10;
Led plRed = Led(plRedPin, LOW);
const uint8_t plGreenPin = 9;
Led plGreen = Led(plGreenPin, LOW);

TrafficLightThreeLeds trafficLight(tlGreen, tlRed, tlYellow);
TrafficLightTwoLeds walkerLight(plGreen, plRed); 

// transitions from state BB
Transition stBB_Transitions[] = {
  { stRR, stBB_stRR_Timer },
};

// transitions from state RR
Transition stRR_Transitions[] = {
  { stGR, stRR_stGR_Timer },
};

// transitions from state GR
Transition stGR_Transitions[] = {
  { stX, stGR_stX_Timer },
};

// transitions from state X
Transition stX_Transitions[] = {
  { stOR, stGR_stX_Timer },
};

// transitions from state OR
Transition stOR_Transitions[] = {
  { stRR1, stOR_stRR1_Timer },
};


State states[] = {
   { stBB, "tl B / pl B", stBB_onEnter, stBB_onState, stBB_onLeave,  stBB_Transitions, 1 },
  { stRR, "tl R / pl R", stRR_onEnter, stRR_onState, stRR_onLeave, stRR_Transitions, 1 },
  { stGR, "tl G / pl R", stGR_onEnter, stGR_onState, stGR_onLeave, stGR_Transitions, 1 },
  { stX, "tl G / pl R", stX_onEnter, stX_onState, stX_onLeave, stX_Transitions, 1 },
  { stOR, "tl O / pl R", stOR_onEnter, stOR_onState, stOR_onLeave, stOR_Transitions, 1 },
  { stRR1, "tl R / pl R", nullptr, nullptr, nullptr, nullptr, 0 },
  { stRG, "tl R / pl G", nullptr, nullptr, nullptr, nullptr, 0 },
  { stRB, "tl R / pl B", nullptr, nullptr, nullptr, nullptr, 0 },
  { stRR2, "tl R / pl R", nullptr, nullptr, nullptr, nullptr, 0 },
};

// the fsm
Fsm fsm = { states, NUMELEMENTS(states), stBB };


void setup()
{
  Serial.begin(115200);
  Serial.println(F("FSM demo"));

  // configure buttons
  for (uint8_t cnt = 0; cnt < NUMELEMENTS(btnPins); cnt++)
  {
    buttons[cnt].attach(btnPins[cnt], INPUT_PULLUP);  // setup the bounce instance for the current button
    buttons[cnt].interval(25);                        // interval in ms
  }

  trafficLight.begin();
  walkerLight.begin();  
   

  // validate fsm
  if (fsm.validate() == false)
  {
    Serial.println(F("Fix your code"));
    for (;;) {}
  }
  else
  {
    Serial.println("Succesfully validated");
  }

  // enable/disable printing of millis() in the fsm
  fsm.enableMillis = enableMillis;
  // enable/disable debugging of fsm
  fsm.debug = debugFsm;

  // print fsm info
  //fsm.print();




}

void loop()
{
  fsm.run();

  return;
  
}