//
// fsmTasks - (Finite State Machine) Multi-Tasks
//

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

// 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)

// Yields the current task.
#define TSK_YIELD(tsk) \
  do { \
    TSK_WAIT_UNTIL(tsk, (tsk)->state = 0); \
    (tsk)->state = 1; \
  } 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)

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

// Declaration of threads
struct ProtoThread tsk1, tsk2, tsk3;

// Definition of function Task1
void task1(struct ProtoThread *tsk) {
  TSK_BEGIN(tsk);
  
  while (true) {  // Task internal loop
    TSK_DELAY(500);
    static unsigned long counter;
    tskPrint(1, counter);
    counter++;
    TSK_YIELD(tsk);
  }
  TSK_END(tsk);
}

// Definition of function Task2
void task2(struct ProtoThread *tsk) {
  TSK_BEGIN(tsk);
  
  while (true) {  // Task internal loop
    TSK_DELAY(1500);
    static unsigned long counter;
    tskPrint(2, counter);
    counter++;
    TSK_YIELD(tsk);
  }
  TSK_END(tsk);
}

// Definition of function Task3
void task3(struct ProtoThread *tsk) {
  TSK_BEGIN(tsk);
  
  while (true) {  // Task internal loop
    TSK_DELAY(1000);
    static unsigned long counter;
    tskPrint(3, counter);
    counter++;
    TSK_YIELD(tsk);
  }
  TSK_END(tsk);
}

// Initialize configuration parameters
void setup() {
  Serial.begin(9600);
  TSK_INIT(&tsk1);      // Initializes task1
  TSK_INIT(&tsk2);      // Initializes task2
  TSK_INIT(&tsk3);      // Initializes task2
}

// Main (infinite) loop
void loop() {
  task1(&tsk1);         // Execute task1
  task2(&tsk2);         // Execute task2
  task3(&tsk3);         // Execute task2
}

// task print function
void tskPrint(uint8_t tskNum, unsigned long counter) {
  Serial.print(" [");
  Serial.print(millis());
  Serial.print("] Task{");
  Serial.print(tskNum);
  Serial.print( "} -> #");
  Serial.println(counter);
}