#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "driver/gpio.h"
#include "esp_log.h"
#include "esp_task_wdt.h"

#define LED_RED_GPIO    4
#define LED_GREEN_GPIO  5
#define LED_BLUE_GPIO   6

#define TAG "LED_CONTROL"

typedef enum {
    LED_CMD_SOLID,
    LED_CMD_BLINK_SINGLE,
    LED_CMD_BLINK_MULTI,
    LED_CMD_BLINK_ALTERNATE,
    LED_CMD_STOP
} led_command_type_t;

typedef struct {
    led_command_type_t type;
    bool red;
    bool green;
    bool blue;
    bool red2;
    bool green2;
    bool blue2;
    int blink_interval_ms;
} led_command_t;

static QueueHandle_t led_queue = NULL;
static TaskHandle_t led_task_handle = NULL;

void led_control_task(void *pvParameters) {
    led_command_t current_cmd = {LED_CMD_STOP, false, false, false, false, false, false, 0};
    bool is_first_state = true;
    TickType_t last_wake_time = xTaskGetTickCount();
    
    ESP_ERROR_CHECK(esp_task_wdt_add(NULL));
    ESP_ERROR_CHECK(esp_task_wdt_status(NULL));
    
    while (1) {
        esp_task_wdt_reset();

        if (xQueueReceive(led_queue, &current_cmd, 0) == pdTRUE) {
            is_first_state = true; // Reset to start with first state on new command
            last_wake_time = xTaskGetTickCount(); // Reset the last wake time
        }

        switch (current_cmd.type) {
            case LED_CMD_SOLID:
                gpio_set_level(LED_RED_GPIO, current_cmd.red);
                gpio_set_level(LED_GREEN_GPIO, current_cmd.green);
                gpio_set_level(LED_BLUE_GPIO, current_cmd.blue);
                vTaskDelayUntil(&last_wake_time, pdMS_TO_TICKS(100));
                break;

            case LED_CMD_BLINK_SINGLE:
                gpio_set_level(LED_RED_GPIO, current_cmd.red);
                gpio_set_level(LED_GREEN_GPIO, current_cmd.green);
                gpio_set_level(LED_BLUE_GPIO, current_cmd.blue);
                vTaskDelayUntil(&last_wake_time, pdMS_TO_TICKS(current_cmd.blink_interval_ms));
                gpio_set_level(LED_RED_GPIO, false);
                gpio_set_level(LED_GREEN_GPIO, false);
                gpio_set_level(LED_BLUE_GPIO, false);
                vTaskDelayUntil(&last_wake_time, pdMS_TO_TICKS(current_cmd.blink_interval_ms));
                break;

            case LED_CMD_BLINK_MULTI:
                if (is_first_state) {
                    gpio_set_level(LED_RED_GPIO, current_cmd.red);
                    gpio_set_level(LED_GREEN_GPIO, current_cmd.green);
                    gpio_set_level(LED_BLUE_GPIO, current_cmd.blue);
                } else {
                    gpio_set_level(LED_RED_GPIO, current_cmd.red2);
                    gpio_set_level(LED_GREEN_GPIO, current_cmd.green2);
                    gpio_set_level(LED_BLUE_GPIO, current_cmd.blue2);
                }
                is_first_state = !is_first_state;
                vTaskDelayUntil(&last_wake_time, pdMS_TO_TICKS(current_cmd.blink_interval_ms));
                break;

            case LED_CMD_BLINK_ALTERNATE:
                if (is_first_state) {
                    gpio_set_level(LED_RED_GPIO, current_cmd.red);
                    gpio_set_level(LED_GREEN_GPIO, current_cmd.green);
                    gpio_set_level(LED_BLUE_GPIO, current_cmd.blue);
                } else {
                    gpio_set_level(LED_RED_GPIO, current_cmd.red2);
                    gpio_set_level(LED_GREEN_GPIO, current_cmd.green2);
                    gpio_set_level(LED_BLUE_GPIO, current_cmd.blue2);
                }
                is_first_state = !is_first_state;
                vTaskDelayUntil(&last_wake_time, pdMS_TO_TICKS(current_cmd.blink_interval_ms));
                break;

            case LED_CMD_STOP:
                gpio_set_level(LED_RED_GPIO, false);
                gpio_set_level(LED_GREEN_GPIO, false);
                gpio_set_level(LED_BLUE_GPIO, false);
                vTaskDelayUntil(&last_wake_time, pdMS_TO_TICKS(100));
                break;
        }
    }
}

void init_led_control() {
    gpio_config_t io_conf = {
        .intr_type = GPIO_INTR_DISABLE,
        .mode = GPIO_MODE_OUTPUT,
        .pin_bit_mask = (1ULL << LED_RED_GPIO) | (1ULL << LED_GREEN_GPIO) | (1ULL << LED_BLUE_GPIO),
        .pull_down_en = 0,
        .pull_up_en = 0,
    };
    gpio_config(&io_conf);
    
    led_queue = xQueueCreate(5, sizeof(led_command_t));
    xTaskCreate(led_control_task, "led_control_task", 2048, NULL, 5, &led_task_handle);
    
    ESP_LOGI(TAG, "Advanced LED control system initialized");
}

void set_led_solid(bool red, bool green, bool blue) {
    led_command_t cmd = {LED_CMD_SOLID, red, green, blue};
    xQueueSend(led_queue, &cmd, 0);
}

void set_led_blink_single(bool red, bool green, bool blue, int blink_interval_ms) {
    led_command_t cmd = {LED_CMD_BLINK_SINGLE, red, green, blue, false, false, false, blink_interval_ms};
    xQueueSend(led_queue, &cmd, 0);
}

void set_led_blink_multi(bool red1, bool green1, bool blue1, 
                         bool red2, bool green2, bool blue2, 
                         int blink_interval_ms) {
    led_command_t cmd = {
        LED_CMD_BLINK_MULTI,
        red1, green1, blue1,
        red2, green2, blue2,
        blink_interval_ms
    };
    xQueueSend(led_queue, &cmd, 0);
}

void set_led_blink_alternate(bool red1, bool green1, bool blue1, 
                             bool red2, bool green2, bool blue2, 
                             int blink_interval_ms) {
    led_command_t cmd = {
        LED_CMD_BLINK_ALTERNATE,
        red1, green1, blue1,
        red2, green2, blue2,
        blink_interval_ms
    };
    xQueueSend(led_queue, &cmd, 0);
}

void stop_leds() {
    led_command_t cmd = {LED_CMD_STOP};
    xQueueSend(led_queue, &cmd, 0);
}