/* ---- defines a struct tsk that contains a pointer to a local state --- */
struct tsk { unsigned short t_state; };
typedef struct tsk TSK_SET;

/* --- Define macro constants to represent the different states of a task --- */
#define TSK_WAITING 0 // The task is waiting to be executed
#define TSK_YIELDED 1 // The task has yielded execution to another task
#define TSK_EXITED  2 // The task was terminated by the user
#define TSK_ENDED   3 // The task has completed and cannot be executed again

// Initialize a task function
#define TSK_INIT(tsk) (tsk)->t_state = 0;

// Declaration of a task function
#define TSK_THREAD(name_args) char name_args

// mark the beginning of a task function
#define TSK_BEGIN(tsk) { char TSK_YIELD_FLAG = 1; switch(tsk->t_state) { case 0:

// mark the end of a task function
#define TSK_END(tsk) }; TSK_YIELD_FLAG = 0; \
   TSK_INIT(tsk); return TSK_ENDED; }

// set task label
#define TSK_LINE(s) s = __LINE__; case __LINE__:

// make a task wait until a condition is true
#define TSK_WAIT_UNTIL(tsk, condition)  \
  do {                                  \
    TSK_LINE((tsk)->t_state);                  \
    if(!(condition)) {                  \
      return TSK_WAITING;               \
    }                                   \
  } while(0)

// make a task wait while a condition is true
#define TSK_WAIT_WHILE(tsk, cond)  TSK_WAIT_UNTIL((tsk), !(cond))

// make a task wait for another task to complete
#define TSK_WAIT_THREAD(tsk, thread) TSK_WAIT_WHILE((tsk), TSK_RUN(thread))

// spawn a new task and wait for it to complete
#define TSK_SPAWN(tsk, child, thread)   \
  do {                                  \
    TSK_INIT((child));                  \
    TSK_WAIT_THREAD((tsk), (thread));   \
  } while(0)

// restart a task
#define TSK_RESTART(tsk)                \
  do {                                  \
    TSK_INIT(tsk);                      \
    return TSK_WAITING;                 \
  } while(0)

// exit a task
#define TSK_EXIT(tsk)                   \
  do {                                  \
    TSK_INIT(tsk);                      \
    return TSK_EXITED;                  \
  } while(0)

// check if a task is still running
#define TSK_RUN(f) ((f) < TSK_EXITED)

// make a task yield execution to another task
#define TSK_YIELD(tsk)                  \
  do {                                  \
    TSK_YIELD_FLAG = 0;                 \
    TSK_LINE((tsk)->t_state);                  \
    if(TSK_YIELD_FLAG == 0) {           \
      return TSK_YIELDED;               \
    }                                   \
  } while(0)

// make a task yield execution until a condition is true
#define TSK_YIELD_UNTIL(tsk, cond)      \
  do {                                  \
    TSK_YIELD_FLAG = 0;                 \
    TSK_LINE((tsk)->t_state);                  \
    if((TSK_YIELD_FLAG == 0) || !(cond)) { \
      return TSK_YIELDED;               \
    }                                   \
  } while(0)

// make a task wait for a specified amount of time
#define TSK_DELAY(tsk, delay) \
{ \
  do { \
    static unsigned long tasks_delay; \
    tasks_delay = millis();           \
    TSK_WAIT_UNTIL(tsk, millis()      \
    - tasks_delay >= delay);          \
  } while(false);                     \
}

// Define a structure to represent a semaphore
struct tsk_sem {
  unsigned int count;
};

// initialize a semaphore
#define TSK_SEM_INIT(s, c) (s)->count = c

// make a task wait for a semaphore
#define TSK_SEM_WAIT(tsk, s)              \
  do {                                    \
    TSK_WAIT_UNTIL(tsk, (s)->count > 0);  \
    --(s)->count;                         \
  } while(0)

// signal a semaphore
#define TSK_SEM_SIGNAL(tsk, s) ++(s)->count

// declare a queue
#define TSK_QUEUE_DEFINE(queue_name, queue_size) \
  struct tsk_sem mutex_##queue_name; \
  int queue_##queue_name[queue_size]; \
  int head_##queue_name = 0; \
  int tail_##queue_name = 0; \
  int count_##queue_name = 0

// initialize a queue
#define TSK_QUEUE_INIT(queue_name) \
  TSK_SEM_INIT(&mutex_##queue_name, 1)

// enqueue a value
#define TSK_ENQUEUE(queue_name, value) \
  do { \
    TSK_SEM_WAIT(tsk, &mutex_##queue_name); \
    queue_##queue_name[tail_##queue_name] = value; \
    tail_##queue_name = (tail_##queue_name + 1) % (sizeof(queue_##queue_name)/sizeof(int)); \
    count_##queue_name++; \
    TSK_SEM_SIGNAL(tsk, &mutex_##queue_name); \
  } while(0)

// dequeue a value
#define TSK_DEQUEUE(queue_name, value) \
  do { \
    TSK_SEM_WAIT(tsk, &mutex_##queue_name); \
    value = queue_##queue_name[head_##queue_name]; \
    head_##queue_name = (head_##queue_name + 1) % (sizeof(queue_##queue_name)/sizeof(int)); \
    count_##queue_name--; \
    TSK_SEM_SIGNAL(tsk, &mutex_##queue_name); \
  } while(0)

// Define macros to check if a queue is full or empty
#define TSK_QUEUE_FULL(queue_name) (count_##queue_name >= (sizeof(queue_##queue_name)/sizeof(int)))
#define TSK_QUEUE_EMPTY(queue_name) (count_##queue_name == 0)

// Define a macro to set a flag in a structure
#define TSK_FLAG_SET(struct_name, field, value) (struct_name.field = value)

// Define a macro to get the value of a flag in a structure
#define TSK_FLAG_GET(struct_name, field) (struct_name.field)

//
// Main C++ code
//

// Declaration of tasks identifier
TSK_SET tsk1, tsk2;

// Declaration of threads functions
TSK_THREAD( task1(struct tsk *tsk, int interval, char tsk_n) ) {
  TSK_BEGIN(tsk);
  while(true) {
    Serial.print("Task ");
    Serial.println(tsk_n);
    TSK_DELAY(&tsk1, interval);
  }
  TSK_END(tsk);
}

TSK_THREAD( task2(struct tsk *tsk, int interval, char tsk_n) ) {
  TSK_BEGIN(tsk);
  while(true) {
    Serial.print("Task ");
    Serial.println(tsk_n);
    TSK_DELAY(&tsk2, interval);
  }
  TSK_END(tsk);
}

// Hardware Setup
void setup() {
  Serial.begin(9600);
  TSK_INIT( &tsk1 );
  TSK_INIT( &tsk2 );
}

// Main infinite loop
void loop() {
  TSK_RUN( task1(&tsk1, 250, '1') );
  TSK_RUN( task2(&tsk2, 900, '2') );
}