#include "led_control.h"
#include "main.h"
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "usbd_custom_hid.h"
#include "bsp_led.h"
#include "bsp_key.h"

static QueueHandle_t command_queue = NULL;

void led_control_command_send_from_isr(const uint8_t *command)
{
  BaseType_t need_yield = pdFALSE;
  xQueueSendFromISR(command_queue, command, &need_yield);
  portYIELD_FROM_ISR(need_yield);
}

void led_control_task_entry(void const *argument)
{
  command_queue = xQueueCreate(4, BSP_LED_COUNT);

  usbd_custom_hid_start(0, USB_BASE);
  bsp_key_scan_start();

  uint8_t command_status[BSP_LED_COUNT];
  while (1) {
    xQueueReceive(command_queue, command_status, portMAX_DELAY);
    bsp_led_set_output(command_status);

    bsp_led_get_status(command_status);
    usbd_custom_hid_send(command_status, BSP_LED_COUNT);
  }
}