#include "fsm.h"
#include "timeout.h"

typedef EFSM<Timeout> timeFSM;


// Button handling.
void startState(timeFSM& fsm) {
  Serial.println("Press button A to start.");
  fsm.newState(waitState);
}

void waitState(timeFSM& fsm) {
  if (digitalRead(2) == LOW) {
    Serial.println("Entering run state, press B to restart, press C to stop.");
    fsm.data.set(400);
    fsm.newState(runState);
  }
}

void runState(timeFSM& fsm) {
  if (digitalRead(3) == LOW) {
    Serial.println("Restarting.");
    fsm.data.set(600);
    fsm.newState(startState);
  }
  if (digitalRead(4) == LOW) {
    Serial.println("Entering stop state.");
    fsm.data.set(200);
    fsm.newState(stopState);
  }
}

void stopState(timeFSM&) {}


// LED handling.
void ledOn(timeFSM& fsm) {
  if (fsm.data.timeout()) {
    digitalWrite(LED_BUILTIN, HIGH);
    fsm.data.reset();
    fsm.newState(ledOff);
  }
}

void ledOff(timeFSM& fsm) {
  if (fsm.data.timeout()) {
    digitalWrite(LED_BUILTIN, LOW);
    fsm.data.reset();
    fsm.newState(ledOn);
  }
}


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

  for (size_t i = 2; i <= 4; i++) {
    pinMode(i, INPUT_PULLUP);
  }
  pinMode(LED_BUILTIN, OUTPUT);
}

void loop() {
  static Timeout timeout(600);
  static timeFSM ledFsm(ledOn, timeout);
  ledFsm.loop();

  static timeFSM btnFsm(startState, timeout);
  btnFsm.loop();
}