#include "FiniteState.h"
#include "RepeatButton.h"

#define pbPin1    6
#define pbPin2    4
#define pbPin3    2
#define stopPin   A0

#define ledPin1 7
#define ledPin2 5
#define ledPin3 3

uint8_t pbPins[] = {pbPin1, pbPin2, pbPin3, stopPin};
uint8_t ledPins[] = {ledPin1, ledPin2, ledPin3};
const uint8_t numberOfPBs = sizeof(pbPins) / sizeof(uint8_t);
const uint8_t numberOfLEDs = sizeof(ledPins) / sizeof(uint8_t);

void StateFunction1(id_t id);
void StateFunction2(id_t id);
void StateFunction3(id_t id);
bool PredicateFunction1(id_t state);
bool PredicateFunction2(id_t state);
bool PredicateFunction3(id_t state);
void EventFunction(EventArgs e);

Transition transitions[] = {
  {PredicateFunction1, 0, 1, StateFunction1, EventFunction},                          // State-0 -  Current-State = 0, Next-State = 1
  {PredicateFunction2, 1, 2, StateFunction2, EventFunction, 3000, TRANS_TIMER},       // State-1 -  Current-State = 1, Next-State = 2
  {PredicateFunction3, 1, 2, StateFunction3, EventFunction, 5000, FALSE_TIMER}        // State-2 -  Current-State = 2, Next-State = 0
};
const uint8_t numberOfTransitions = sizeof(transitions) / sizeof(Transition);

FiniteState finiteStateMachine(transitions, numberOfTransitions); // Finite-State Machine Object

RepeatButton buttons[numberOfPBs];

void setup() {
  Serial.begin(115200);
  for (uint8_t index = 0; index < numberOfPBs; index++) {
    buttons[index].buttonMode(pbPins[index], INPUT_PULLUP);
    buttons[index].debounceDelay(20);
  }
  for (uint8_t index = 0; index < numberOfLEDs; index++) {
    pinMode(ledPins[index], OUTPUT);
    digitalWrite(ledPins[index], LOW);
  }
  finiteStateMachine.begin(0);                          // FSM begins with Initial Transition Id
}

void loop() {
  for (uint8_t index = 0; index < numberOfPBs; index++) {
    buttons[index].repeatButton();
  }
  finiteStateMachine.execute();  // Execute the FSM
}

void StateFunction1(id_t id) {
  WriteOutput(id);
}

void StateFunction2(id_t id) {
  if (buttons[3].isPressed()) {
    finiteStateMachine.cancellationTimer(id);
    Serial.println("Cancellation");
  }
  WriteOutput(id);
}

void StateFunction3(id_t id) {
  if (buttons[3].isPressed()) {
    finiteStateMachine.cancellationTimer(id);
    Serial.println("Cancellation");
  }
  WriteOutput(id);
}

bool PredicateFunction1(id_t id) {
  return buttons[0].isPressed();
}

bool PredicateFunction2(id_t id) {
  return buttons[1].isPressed();
}

bool PredicateFunction3(id_t id) {
  return buttons[2].isPressed(true);
}

void EventFunction(EventArgs e) {
  Serial.print("ID-");
  Serial.print(e.id);
  switch (e.action) {
    case ENTRY:
      Serial.println(" :ENTRY");
      //Serial.print("Elapsed Time:");
      //Serial.println(finiteStateMachine.elapsedTime());
      break;
    case EXIT:
      Serial.println(" :EXIT");
      //Serial.print("Elapsed Time:");
      //Serial.println(finiteStateMachine.elapsedTime());
      Serial.println();
      break;
  }
}

void WriteOutput(id_t id) {
  for (uint8_t index = 0; index < numberOfLEDs; index++) {
    if (index == id) continue;
    digitalWrite(ledPins[index], LOW);
  }
  digitalWrite(ledPins[id], HIGH);
}