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

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

const int LED_PINS [] = {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;
  xTaskNotifyGive(taskHandles[counter]);
}

void task(void *param) {
  uint8_t id = (uint8_t)param;
  pinMode(LED_PINS[id], OUTPUT);
  TickType_t xLastWakeTime = xTaskGetTickCount();

  for (;;) {
    ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
    if (id == counter) {
      for (int i = 0; i < 5; i++) {
        digitalWrite(LED_PINS[id], !digitalRead(LED_PINS[id]));
        vTaskDelayUntil(&xLastWakeTime, 500 / portTICK_PERIOD_MS); // Toggle LED every 200ms
      }
    }
    else {
      digitalWrite(LED_PINS[id], LOW); // Turn off LED if it's not the selected LED
    }
  }
}

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() {
}