#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();
}

sketch.ino

diagram.json

#pragma once

template <class T>
class EFSM {
public:
  EFSM(void (* const)(EFSM<void*>&));
  EFSM(void (* const)(EFSM<T>&), T& data);

  void loop();
  void newState(void (* const)(EFSM<T>&));

  T& data;

private:
  void (*state_)(EFSM<T>&){};
  static void* default_;
};

typedef EFSM<void*> FSM;


template <class T>
EFSM<T>::EFSM(void (* const state)(EFSM<void*>&)) : data(default_) {
  newState(state);
}

template <class T>
EFSM<T>::EFSM(void (* const state)(EFSM<T>&), T& data) : data(data) {
  newState(state);
}

template <class T>
void EFSM<T>::loop() {
  state_(*this);
}

template <class T>
void EFSM<T>::newState(void (* const state)(EFSM<T>&)) {
  state_ = state;
}

template <class T>
void* EFSM<T>::default_ = nullptr;

fsm.h

#pragma once

class Timeout {
public:
  Timeout() {};
  Timeout(unsigned long const);

  void set(unsigned long const);
  void reset();
  bool timeout() const;

private:
  unsigned long start_;
  unsigned long timeout_;
};

timeout.h

#include <Arduino.h>

#include "timeout.h"


Timeout::Timeout(unsigned long const timeout) {
  set(timeout);
}

void Timeout::set(unsigned long const timeout) {
  timeout_ = timeout;
  reset();
}

void Timeout::reset() {
  start_ = millis();
}

bool Timeout::timeout() const {
  return millis() - start_ >= timeout_;
}