#define GPIO_BUTTONL 25
#define GPIO_BUTTONM 26
#define GPIO_BUTTONR 27
#define LED_GPIO 12

static QueueHandle_t queue;
static const int reset_press = -998;

static void debounce_task(void *argp) {
    unsigned button_gpio = *(unsigned*)argp;
    uint32_t level, state = 0;
    uint32_t mask = 0x7FFFFFFF;
    int event, last = -999;

    for (;;) {
        level = !digitalRead(button_gpio);
        state = (state << 1) | level;

        if ((state & mask) == mask)
            event = button_gpio;
        else
            event = -button_gpio;

        int peeked_event;
        BaseType_t status = xQueuePeek(queue, &peeked_event, 0);
        if (status == pdPASS) {
  
            printf("Peeked event: %d, skipping new event\n", peeked_event);
        } else {
       
            if (event != last) { 
                UBaseType_t queueSize = uxQueueMessagesWaiting(queue);
                printf("Items in queue: %d\n", queueSize);

                if (xQueueSendToBack(queue, &event, 0) == pdPASS) {
                    last = event;
                } else if (event < 0) {
          
                    do {
                        xQueueReset(queue);
                    } while (xQueueSendToBack(queue, &reset_press, 0) != pdPASS);
                    last = event;
                }
            }
        }

        taskYIELD();
    }
}

static void press_task(void *argp) {
    static const uint32_t enable = (1 << GPIO_BUTTONL) | (1 << GPIO_BUTTONM) | (1 << GPIO_BUTTONR); 
    BaseType_t s;
    int event;
    uint32_t state = 0;

    digitalWrite(LED_GPIO, LOW);

    for (;;) {
        s = xQueueReceive(queue, &event, portMAX_DELAY);
        assert(s == pdPASS);

        if (event == reset_press) {
            digitalWrite(LED_GPIO, LOW);
            state = 0;
            printf("RESET!!\n");
            continue;
        }

        if (event >= 0) {
            state |= 1 << event;
        } else {
            state &= ~(1 << -event);
        }

        if (state == enable) {
            digitalWrite(LED_GPIO, HIGH); 
        } else {
            digitalWrite(LED_GPIO, LOW);
        }

        UBaseType_t spacesAvailable = uxQueueSpacesAvailable(queue); 
        printf("Spaces available in queue: %d\n", spacesAvailable);
    }
}

void setup() {
  int app_cpu = xPortGetCoreID();
  static int left = GPIO_BUTTONL; 
  static int mid = GPIO_BUTTONM;
  static int right = GPIO_BUTTONR;
  TaskHandle_t h;
  BaseType_t rc;

  delay(2000);

  queue = xQueueCreate(4, sizeof(int));
  assert(queue);

  pinMode(LED_GPIO, OUTPUT);
  pinMode(GPIO_BUTTONL, INPUT_PULLUP);
  pinMode(GPIO_BUTTONM, INPUT_PULLUP);
  pinMode(GPIO_BUTTONR, INPUT_PULLUP);

  rc = xTaskCreatePinnedToCore(debounce_task, "debounceL", 2048, &left, 1, &h, app_cpu);
  assert(rc == pdPASS);
  assert(h);

  rc = xTaskCreatePinnedToCore(debounce_task, "debounceM", 2048, &mid, 1, &h, app_cpu);
  assert(rc == pdPASS);
  assert(h);

  rc = xTaskCreatePinnedToCore(debounce_task, "debounceR", 2048, &right, 1, &h, app_cpu);
  assert(rc == pdPASS);
  assert(h);

  rc = xTaskCreatePinnedToCore(press_task, "led", 2048, nullptr, 1, &h, app_cpu);
  assert(rc == pdPASS);
  assert(h);
}

void loop() {
  vTaskDelete(nullptr); 
}