/*
02  implemented blink
*/

#include <Bounce2.h>

#define NUMELEMENTS(x) (sizeof(x) / sizeof(x[0]))

const uint8_t numLights = 2;

enum class PinIndex : uint8_t
{
  Red,
  Green,
  Orange,
};

enum class LightState : uint8_t
{
  Red,
  Orange,
  Green,
  Off,
  Blink,
};

struct Light
{
  uint8_t pins[3];
  LightState lightState;
  uint32_t blinkInterval;
  uint32_t blinkStartTime;
  bool blinkLedState;

  void run()
  {
    switch (lightState)
    {
      case LightState::Red:
        red();
        break;
      case LightState::Orange:
        orange();
        break;
      case LightState::Green:
        green();
        break;
      case LightState::Off:
        off();
        break;
      case LightState::Blink:
        blink(false);
        break;
    }
  }

  void begin()
  {
    for (uint8_t cnt = 0; cnt < NUMELEMENTS(pins); cnt++)
      pinMode(pins[cnt], OUTPUT);
  }

  void green()
  {
    blink(true);
    off();
    digitalWrite(pins[(int)PinIndex::Green], HIGH);
  }

  void orange()
  {
    blink(true);
    off();
    digitalWrite(pins[(int)PinIndex::Orange], HIGH);
  }

  void red()
  {
    blink(true);
    off();
    digitalWrite(pins[(int)PinIndex::Red], HIGH);
  }

  void off()
  {
    blink(true);
    for (uint8_t cnt = 0; cnt < NUMELEMENTS(pins); cnt++)
      digitalWrite(pins[cnt], LOW);
  }

  void blink(bool reset)
  {
    if (reset && !blinkLedState)
    {
      Serial.print(millis());
      Serial.print(F("\t"));
      Serial.print(pins[(int)PinIndex::Orange]);
      Serial.println(F(" reset blink"));
      blinkLedState = true;
    }

    if (reset == true)
    {
      //Serial.println("x");
      return;
    }

    if (millis() - blinkStartTime >= blinkInterval)
    {
      blinkStartTime = millis();
      blinkLedState = !blinkLedState;
      Serial.print(millis());
      Serial.print(F("\t"));
      Serial.print(F("pin "));
      Serial.print(pins[(int)PinIndex::Orange]);
      Serial.print(F(" blink state = "));
      Serial.println(blinkLedState);
      digitalWrite(pins[(int)PinIndex::Orange], blinkLedState);
    }
  }

  void print()
  {
    static LightState oldLightState = LightState::Red;
    if (oldLightState != lightState)
    {
      Serial.print(F("lightState changed from "));
      Serial.print((int)oldLightState);
      Serial.print(F(" to "));
      Serial.println((int)lightState);

      oldLightState = lightState;
    }
  }
};

Light allLights[] = {
  {{ 2, 3, 4 },  LightState::Off, 400, 0, true},
  { { 5, 6, 6 }, LightState::Off, 400, 0, true},
};

enum class MainState : uint16_t
{
  GR,
  OR,
  RR1,
  RG,
  RB,
  RR2,
  OFF,
  START,
};

struct FsmState
{
  uint32_t duration;
  LightState lightStates[numLights];
  MainState nextState;
};

FsmState fsmStates[] = {
  {10000, { LightState::Green, LightState::Red },  MainState::OR   },
  { 4000, { LightState::Orange, LightState::Red }, MainState::RR1  },
  { 1000, { LightState::Red, LightState::Red },    MainState::RG   },
  { 4000, { LightState::Red, LightState::Green },  MainState::RB   },
  { 2000, { LightState::Red, LightState::Blink },  MainState::RR2  },
  { 1000, { LightState::Red, LightState::Red },    MainState::GR   },
  { 0,    { LightState::Off, LightState::Off },    MainState::START}, // OFF
  { 1000, { LightState::Red, LightState::Red },    MainState::GR   }, // START
};

void fsm()
{
  static MainState state = MainState::OFF;
  static MainState oldState = MainState::OFF;

  static uint32_t startTime;

  for (uint8_t cnt = 0; cnt < NUMELEMENTS(allLights); cnt++)
  {
    allLights[cnt].lightState = fsmStates[(int)state].lightStates[cnt];
  }
  if (oldState != state)
  {
    Serial.print(millis());
    Serial.print(F("\tFSM state changed from "));
    Serial.print((int)oldState);
    Serial.print(F(" to "));
    Serial.print((int)state);
    Serial.print(F(", new duration = "));
    Serial.println(fsmStates[(int)state].duration);

    oldState = state;
  }

  if (millis() - startTime >= fsmStates[(int)state].duration)
  {
    // set start time for next state
    startTime = millis();

    switch (state)
    {
      case MainState::GR:
        state = MainState::OR;
        break;
      case MainState::OR:
        state = MainState::RR1;
        break;
      case MainState::RR1:
        state = MainState::RG;
        break;
      case MainState::RG:
        state = MainState::RB;
        break;
      case MainState::RB:
        state = MainState::RR2;
        break;
      case MainState::RR2:
        state = MainState::GR;
        break;
      case MainState::OFF:
        // wait for a trigger to switch to state GR
        if (Serial.available())
        {
          char ch = Serial.read();
          if (ch == 's')
          {
            Serial.println(F("Switching to START"));
            state = MainState::START;
          }
        }
        break;
      case MainState::START:
        // set start time for next state
        startTime = millis();
        // switch to GR state
        state = MainState::GR;
        break;
    }
  }

  for (uint8_t cnt = 0; cnt < NUMELEMENTS(allLights); cnt++)
  {
    allLights[cnt].run();
  }
}

void setup()
{
  Serial.begin(115200);

  Serial.println(F("=== DEBUG ==="));
  for (uint8_t lightCnt = 0; lightCnt < NUMELEMENTS(allLights); lightCnt++)
  {
    Serial.print(F("light "));
    Serial.println(lightCnt);
    Serial.print(F("pins "));
    for (uint8_t pinCnt = 0; pinCnt < NUMELEMENTS(allLights[lightCnt].pins); pinCnt++)
    {
      Serial.print(allLights[lightCnt].pins[pinCnt]);
      Serial.print(F(" "));
    }
    Serial.println();

    allLights[lightCnt].begin();
  }
  Serial.println(F("============="));
}

void loop()
{
  fsm();
}