#include "MagicRTOZ.h"

STIMER_INIT(myCallback, self, void)
{
  const char strings[4][20] = {
    "Hello World!\0",
    "How are you?\0",
    "This is MagicRTOZ!\0",
    "May be fun!\0",
  };
  static uint8_t counter = 0;
  Serial.println(strings[counter]);
  counter = (counter + 1) & 3;
}
STIMER_END

TASK_INIT(myCallback2, self, void)
{
  static slist_t myList = SLIST_NEW();

  static int myData1 = 10;
  static selement_t myDataElement1 = SLIST_ELEMENT_NEW(&myData1, 1);
  static int position1 = slist_selement_insert(&myList, &myDataElement1);
  
  static int myData2 = -5;
  static selement_t myDataElement2 = SLIST_ELEMENT_NEW(&myData2, 0);
  static int position2 = slist_selement_insert(&myList, &myDataElement2);

  static int myData3 = 15;
  static selement_t myDataElement3 = SLIST_ELEMENT_NEW(&myData3, 0);
  static int position3 = slist_selement_insert(&myList, &myDataElement3);

  for(;;)
  {

    SLIST_FOREACH_INIT(myList, buffer, int)
    {
      Serial.print("Position ");
      Serial.print(SLIST_FOREACH_POSITION);
      Serial.print(": ");
      Serial.println(*buffer);
      TASK_DELAY(2000);
    }
    SLIST_FOREACH_END

  }
}
TASK_END

TASK_INIT(myCallback3, self, void)
{
  static gpio_t pin_13;
  GPIO.init(&pin_13, 5, GPIO_GROUP_B, GPIO_MODE_OUTPUT, GPIO_PULL_RESISTOR_NONE);

  for(;;)
  {
    GPIO.toggle(&pin_13);
    TASK_DELAY(1000);
  }
  
}
TASK_END

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

  static stimer_t myTimer = STIMER_NEW("myTimer", myCallback, nullptr);
  static task_t myTask = TASK_NEW("myTask", myCallback2, nullptr);
  static task_t myBlink = TASK_NEW("myBlink", myCallback3, nullptr);

  stimer_install(&myTimer, 1, 500, STIMER_RUN_MODE_FOREVER);
  Task.install(&myTask, 2);
  Task.install(&myBlink, 0);
}

void loop()
{
  process_schedule();
  process_run();

  delay(1);
}