#include "blk_box_drivers/leds.hpp"
#include "pins.h"
#include "esp_log.h"
#include "esp_err.h"
#include "led_strip.h"
#include "freertos/FreeRTOS.h"
#include "freertos/semphr.h"

const LEDColor LEDColor::OFF = LEDColor::from_rgb(0x00, 0x00, 0x00);
const LEDColor LEDColor::RED = LEDColor::from_rgb(0x17, 0x00, 0x00);
const LEDColor LEDColor::RED_STRONG = LEDColor::from_rgb(0xFF, 0x00, 0x00);
const LEDColor LEDColor::ORANGE = LEDColor::from_rgb(0x17, 0x02, 0x00);
const LEDColor LEDColor::ORANGE_STRONG = LEDColor::from_rgb(0xFF, 0x20, 0x00);
const LEDColor LEDColor::YELLOW = LEDColor::from_rgb(0x07, 0x07, 0x00);
const LEDColor LEDColor::YELLOW_STRONG = LEDColor::from_rgb(0xFF, 0xFF, 0x00);
const LEDColor LEDColor::GREEN = LEDColor::from_rgb(0x00, 0x07, 0x00);
const LEDColor LEDColor::GREEN_STRONG = LEDColor::from_rgb(0x00, 0xFF, 0x00);
const LEDColor LEDColor::BLUE = LEDColor::from_rgb(0x00, 0x00, 0x17);
const LEDColor LEDColor::BLUE_STRONG = LEDColor::from_rgb(0x00, 0x00, 0xFF);
const LEDColor LEDColor::PINK = LEDColor::from_rgb(0x10, 0x00, 0x04);
const LEDColor LEDColor::PINK_STRONG = LEDColor::from_rgb(0xFF, 0x00, 0x80);
const LEDColor LEDColor::WHITE = LEDColor::from_rgb(0x04, 0x04, 0x04);
const LEDColor LEDColor::WHITE_STRONG = LEDColor::from_rgb(0xFF, 0xFF, 0xFF);

const static char* TAG = "leds";

static led_strip_handle_t led_strip = NULL;
static SemaphoreHandle_t led_mutex = NULL;

void init_leds() {
    led_mutex = xSemaphoreCreateMutex();
    if (led_mutex == NULL) {
        ESP_LOGE(TAG, "Failed to create LED mutex");
        return;
    }
    /// LED strip common configuration
    led_strip_config_t strip_config = {
        .strip_gpio_num = PIN_NEOPIXEL,
        .max_leds = LED_COUNT,
        // TODO: switch this over when we switch to the different LEDs
        .led_model = LED_MODEL_WS2812,
        .color_component_format = LED_STRIP_COLOR_COMPONENT_FMT_GRB,
        .flags = {
            .invert_out = false,
        }
    };

    /// RMT backend specific configuration
    led_strip_rmt_config_t rmt_config = {
        .clk_src = RMT_CLK_SRC_DEFAULT,
        .resolution_hz = LED_STRIP_RMT_RES_HZ,
        .mem_block_symbols = RMT_LED_SYMBOLS * RMT_SYMBOLS_PER_LED,
        .flags = {
            .with_dma = false,
        }
    };

    /// Create the LED strip object
    ESP_ERROR_CHECK(led_strip_new_rmt_device(&strip_config, &rmt_config, &led_strip));
}

void LEDController::set_led(uint32_t led, uint32_t color) {
    xSemaphoreTake(led_mutex, portMAX_DELAY);
    ESP_ERROR_CHECK(led_strip_set_pixel(led_strip, led, color >> 16 & 0xFF, color >> 8 & 0xFF, color & 0xFF));
    xSemaphoreGive(led_mutex);
}

void LEDController::flush() {
    xSemaphoreTake(led_mutex, portMAX_DELAY);
    ESP_ERROR_CHECK(led_strip_refresh(led_strip));
    xSemaphoreGive(led_mutex);
}

void LEDController::clear() {
    xSemaphoreTake(led_mutex, portMAX_DELAY);
    ESP_ERROR_CHECK(led_strip_clear(led_strip));
    xSemaphoreGive(led_mutex);
    
}