#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"

#include "driver/gpio.h"
#include "driver/ledc.h"

#include "user_board.h"

#define BREATHE_LED_CYCLE 2500

TaskHandle_t xTaskLEDsHandle = NULL;
void vTaskLEDs(void *pvParameters) {
    ledc_timer_config_t ledc_timer = {
        .duty_resolution = LEDC_TIMER_13_BIT,
        .freq_hz = 5000,
        .speed_mode = LEDC_LOW_SPEED_MODE,
        .timer_num = LEDC_TIMER_0,
        .clk_cfg = LEDC_AUTO_CLK
    };

    ledc_timer_config(&ledc_timer);

    ledc_channel_config_t ledc_channels[BOARD_LED_NUM] = {
        {
            .channel = LEDC_CHANNEL_0,
            .duty = 0,
            .gpio_num = BOARD_RGB_LED_R_PIN,
            .speed_mode = LEDC_LOW_SPEED_MODE,
            .hpoint = 0,
            .timer_sel = LEDC_TIMER_0,
            .flags.output_invert = 1
        },
        {
            .channel = LEDC_CHANNEL_1,
            .duty = 0,
            .gpio_num = BOARD_RGB_LED_G_PIN,
            .speed_mode = LEDC_LOW_SPEED_MODE,
            .hpoint = 0,
            .timer_sel = LEDC_TIMER_0,
            .flags.output_invert = 1
        },
        {
            .channel = LEDC_CHANNEL_2,
            .duty = 0,
            .gpio_num = BOARD_RGB_LED_B_PIN,
            .speed_mode = LEDC_LOW_SPEED_MODE,
            .hpoint = 0,
            .timer_sel = LEDC_TIMER_0,
            .flags.output_invert = 1
        },
        {
            .channel = LEDC_CHANNEL_3,
            .duty = 0,
            .gpio_num = BOARD_RGB_LED_W_PIN,
            .speed_mode = LEDC_LOW_SPEED_MODE,
            .hpoint = 0,
            .timer_sel = LEDC_TIMER_0,
            .flags.output_invert = 1
        },
        {
            .channel = LEDC_CHANNEL_4,
            .duty = 0,
            .gpio_num = BOARD_LED1_PIN,
            .speed_mode = LEDC_LOW_SPEED_MODE,
            .hpoint = 0,
            .timer_sel = LEDC_TIMER_0,
            .flags.output_invert = 1
        },
        {
            .channel = LEDC_CHANNEL_5,
            .duty = 0,
            .gpio_num = BOARD_LED2_PIN,
            .speed_mode = LEDC_LOW_SPEED_MODE,
            .hpoint = 0,
            .timer_sel = LEDC_TIMER_0,
            .flags.output_invert = 1
        }
    };

    for(uint8_t i = 0; i < BOARD_LED_NUM; i++) {
        ledc_channel_config(&ledc_channels[i]);
    }

    ledc_fade_func_install(0);

    uint32_t fade_matrix[8][4] = {
        { 8191, 0, 0, 0 },
        { 8191, 8191, 0, 0 },
        { 0, 8191, 0, 0 },
        { 0, 8191, 8191, 0 },
        { 0, 0, 8191, 0 },
        { 0, 0, 8191, 8191 },
        { 0, 0, 0, 8191 },
        { 8191, 0, 0, 8191 }
    };

    uint8_t current_phase = 0;

    for(;;) {
        for(uint8_t i = 0; i < 4; i++) {
            ledc_set_fade_with_time(ledc_channels[i].speed_mode,
                ledc_channels[i].channel, fade_matrix[current_phase][i], BREATHE_LED_CYCLE);
            ledc_fade_start(ledc_channels[i].speed_mode,
                ledc_channels[i].channel, LEDC_FADE_NO_WAIT);
        }
        current_phase++;
        if(current_phase == 8) {
            for(uint8_t i = 0; i < 4; i++) {
                ledc_set_duty(ledc_channels[i].speed_mode, ledc_channels[i].channel, 0);
            }

            vTaskSuspend(NULL);
        }
        vTaskDelay(pdMS_TO_TICKS(BREATHE_LED_CYCLE));
    }
}