// ************ // (FSM) Multi-Tasks Macro // ************ //

// Definition of thread structures
struct tskThread {
  int state;
  unsigned long lastExecutionTime;
};

// Defines tasks threads names
typedef struct tskThread TSK_THREAD;

// Initializes the state of the task
#define TSK_INIT(tsk)     (tsk)->state = 0

// Begins the task by switching on the current state of the task.
#define TSK_BEGIN(tsk)    switch ((tsk)->state) { case 0:

// Ends the task.
#define TSK_END(tsk)      }

// Waits until a given condition is met before proceeding to the next state.
#define TSK_WAIT_UNTIL(tsk, condition) \
  do { \
    (tsk)->state = __LINE__; \
    case __LINE__: \
    if (!(condition)) return; \
  } while (0)

// Delays the current task for a given amount of time.
#define TSK_DELAY(time) \
  do { \
    TSK_WAIT_UNTIL(tsk, (millis() - tsk->lastExecutionTime) >= time); \
    tsk->lastExecutionTime = millis(); \
  } while(0)

// Yields the current task.
#define TSK_YIELD(tsk) \
  do { \
    TSK_WAIT_UNTIL(tsk, (tsk)->state = 0); \
    (tsk)->state = 1; \
  } while(0)

// Executes task thread
#define TSK_RUN(f) (f)

// Console Log
#define LOG(...) \
  Serial.print(" ["); \
  Serial.print(millis()); \
  Serial.print("] "); \
  Serial.println(__VA_ARGS__);

// ********************* // END OF MACRO // ********************* //

TSK_THREAD tsk1, tsk2;

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

  TSK_INIT( &tsk1 );
  TSK_INIT( &tsk2 );
}

void loop() {   // Infinite Loop
  TSK_RUN ( task1 ( &tsk1 ) );
  TSK_RUN ( task2 ( &tsk2 ) );
}

// Função da tarefa 1
void task1(struct tskThread *tsk) {
  TSK_BEGIN(tsk);
  while (1) {
    TSK_DELAY(500);
    LOG("Task 1");
  }
  TSK_END(tsk);
}

// Função da tarefa 2
void task2(struct tskThread *tsk) {
  TSK_BEGIN(tsk);
  while (1) {
    TSK_DELAY(1000);
    LOG("Task 2");
  }
  TSK_END(tsk);
}