#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);

static constexpr const char* TAG = "leds";

static led_strip_handle_t led_strip = nullptr;
static SemaphoreHandle_t led_mutex = nullptr;

static bool leds_ready() {
    if (led_strip != nullptr && led_mutex != nullptr) {
        return true;
    }

    ESP_LOGE(TAG, "LEDs used before successful initialization");
    return false;
}

void init_leds() {
    if (led_strip != nullptr || led_mutex != nullptr) {
        ESP_ERROR_CHECK(led_strip != nullptr && led_mutex != nullptr
                            ? ESP_OK
                            : ESP_ERR_INVALID_STATE);
        return;
    }

    led_mutex = xSemaphoreCreateMutex();
    ESP_ERROR_CHECK(led_mutex != nullptr ? ESP_OK : ESP_ERR_NO_MEM);

    /// 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));

    // Clear the strip once during initialization. Runtime clear() is lazy.
    ESP_ERROR_CHECK(led_strip_clear(led_strip));
}

void LEDController::set_led(uint32_t led, uint32_t color) {
    if (!leds_ready() || xSemaphoreTake(led_mutex, portMAX_DELAY) != pdTRUE) {
        return;
    }

    const esp_err_t err = led_strip_set_pixel(
        led_strip, led, (color >> 16) & 0xFF, (color >> 8) & 0xFF, color & 0xFF);
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "Failed to set LED %u: %s", led, esp_err_to_name(err));
    }
    xSemaphoreGive(led_mutex);
}

void LEDController::flush() {
    if (!leds_ready() || xSemaphoreTake(led_mutex, portMAX_DELAY) != pdTRUE) {
        return;
    }

    const esp_err_t err = led_strip_refresh(led_strip);
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "Failed to refresh LEDs: %s", esp_err_to_name(err));
    }
    xSemaphoreGive(led_mutex);
}

void LEDController::clear() {
    clear(LEDColor::OFF);
}

void LEDController::clear(LEDColor color) {
    clear_range(0, LED_COUNT, color);
}

void LEDController::clear_indicators() {
    clear_indicators(LEDColor::OFF);
}

void LEDController::clear_indicators(LEDColor color) {
    clear_range(LED_SHAPE_COUNT, LED_INDICATOR_COUNT, color);
}

void LEDController::clear_shape_display() {
    clear_shape_display(LEDColor::OFF);
}

void LEDController::clear_shape_display(LEDColor color) {
    clear_range(0, LED_SHAPE_COUNT, color);
}

void LEDController::clear_range(uint32_t first_led, uint32_t led_count, LEDColor color) {
    if (!leds_ready() || xSemaphoreTake(led_mutex, portMAX_DELAY) != pdTRUE) {
        return;
    }

    const uint32_t value = color.value();
    for (uint32_t led = first_led; led < first_led + led_count; ++led) {
        const esp_err_t err = led_strip_set_pixel(
            led_strip, led, (value >> 16) & 0xFF, (value >> 8) & 0xFF, value & 0xFF);
        if (err != ESP_OK) {
            ESP_LOGE(TAG, "Failed to clear LED %u: %s", led, esp_err_to_name(err));
        }
    }
    xSemaphoreGive(led_mutex);
}