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

struct Blinker {
  Timeout timeout;
  uint8_t error;
  uint8_t index : 3;
};

typedef EFSM<Blinker> BlinkFSM;



void pre(BlinkFSM& fsm) {
  if (fsm.data.timeout.timeout()) {
    digitalWrite(LED_BUILTIN, HIGH);

    fsm.data.timeout.set(200);
    if (fsm.data.error & 1 << 7 - fsm.data.index) {
      fsm.data.timeout.set(800);
    }

    fsm.newState(on);
  }
}

void on(BlinkFSM& fsm) {
  if (fsm.data.timeout.timeout()) {
    digitalWrite(LED_BUILTIN, LOW);

    fsm.data.timeout.set(400);
    if (fsm.data.error & 1 << 7 - fsm.data.index) {
      fsm.data.timeout.set(100);
    }

    fsm.newState(post);
  }
}

void post(BlinkFSM& fsm) {
  if (fsm.data.timeout.timeout()) {
    fsm.data.index++;
    fsm.data.timeout.set(400);
    if (fsm.data.error & 1 << 7 - fsm.data.index) {
      fsm.data.timeout.set(100);
    }
    if (not fsm.data.index) {
      fsm.data.timeout.set(2000);
    }

    fsm.newState(pre);
  }
}


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

  pinMode(LED_BUILTIN, OUTPUT);
}

void loop() {
  static Blinker blinker = {500, 0b01100110, 0};
  static BlinkFSM fsm(pre, blinker);

  fsm.loop();
}