#include <Arduino_FreeRTOS.h>
#include <task.h>

#define A_PIN (2)
#define B_PIN (3)
#define NUM_TASKS (4)

const int LED_PIN [] = {6,7,8,9};
TaskHandle_t taskHandles[NUM_TASKS];
volatile int8_t counter = (NUM_TASKS-1);

void rotaryEncoderISR(){
  int8_t step = digitalRead(B_PIN) ? 1 : -1;
  counter = (NUM_TASKS + counter + step) % NUM_TASKS;

  BaseType_t xHigherPriorityTaskWoken = pdFALSE;
  vTaskNotifyGiveFromISR(taskHandles[counter], &xHigherPriorityTaskWoken);
    if (xHigherPriorityTaskWoken) {
    taskYIELD();
  }
}

void task(void *param){
  uint8_t id = (uint8_t)param;
  pinMode(LED_PIN[id], OUTPUT);


  for(;;){
    
     if (ulTaskNotifyTake(pdTRUE, portMAX_DELAY)) {
      digitalWrite(LED_PIN[id], !digitalRead(LED_PIN[id]));
      vTaskDelay(100 / portTICK_PERIOD_MS);
    }
    
  }
}

void setup() {
  pinMode(A_PIN, INPUT_PULLUP);
  pinMode(B_PIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(A_PIN), rotaryEncoderISR, FALLING);
  for(uint8_t i=0; i<NUM_TASKS; i++){
    xTaskCreate(task, "", 96, (uint8_t)i, 1, &taskHandles[i]);
  }
}

void loop() {}