#include <Arduino.h>
#include <mbed.h>
#include <rtos.h>
using namespace mbed;
using namespace rtos;
using namespace std;

#define NUM_TASKS       (2)
#define TASK_PRIORITY   (osPriorityAboveNormal)

const char       *TASK_NAMES[]  = {"T0","T1"};
const uint32_t    LED_PINS[]    = {2,4};
rtos::Thread threads[NUM_TASKS];

void task(void *parameter) {
  uint32_t id = (uint32_t)parameter;
  uint32_t state = 0;
  pinMode( LED_PINS[id], OUTPUT );
  ThisThread::sleep_for (1ms);
  while (1) {
    digitalWrite( LED_PINS[id], state ^=1 );
    //osThreadYield();
    ThisThread::yield();
  }
}

void setup() {
  for (uint32_t i=0; i < NUM_TASKS; i++) {
    threads[i].start( callback(task, (void*)i) ); 
    threads[i].set_priority(TASK_PRIORITY);
  }
}

void loop() {
}