#include "rgb_controller.h"

#include "board.h"
#include "charger_control.h"
#include "main.h"
#include "tim.h"
#include <string.h>

/* Второй светодиод в цепочке WS2812 */
static RGB_t ws2812_led1 = { .R = 0, .G = 0, .B = 0 };

#define WS2812_ERROR_BLINK_MS    100
#define WS2812_ERROR_BLINK_PAUSE 30

/* Яркость обоих WS2812 на плате, 0..255 */
#define WS2812_BRIGHTNESS 60

RGB_State_t LED_State;
RGB_Cycle_t LED_Cycle;

static const RGB_Cycle_t color_estop = {
    .Color1 = { .R = 250, .G = 0, .B = 0 },
    .Color2 = { .R = 0, .G = 0, .B = 0 },
    .Tr = 40,
    .Th = 10,
    .Tf = 40,
    .Tl = 10,
};

static const RGB_Cycle_t color_unlock = {
	.Color1 = { .R = 255, .G = 0, .B = 0 },
	.Color2 = { .R = 0, .G = 0, .B = 0 },
	.Tr = 10,
	.Th = 10,
	.Tf = 10,
	.Tl = 10,
 };

/* Same init shimmer as AC22/tkzu22: hue=85, sat breathes 30↔250 (not fade-to-black). */
static uint8_t lighting_init_active;

static void LED_HsvToRgb(uint8_t hue, uint8_t sat, uint8_t val, RGB_t *out)
{
    uint8_t region;
    uint8_t remainder;
    uint8_t p;
    uint8_t q;
    uint8_t t;

    if (sat == 0U) {
        out->R = out->G = out->B = val;
        return;
    }
    region = (uint8_t)(hue / 43U);
    remainder = (uint8_t)((hue - (uint8_t)(region * 43U)) * 6U);
    p = (uint8_t)((val * (255U - sat)) >> 8);
    q = (uint8_t)((val * (255U - ((sat * remainder) >> 8))) >> 8);
    t = (uint8_t)((val * (255U - ((sat * (255U - remainder)) >> 8))) >> 8);
    switch (region) {
    case 0:
        out->R = val;
        out->G = t;
        out->B = p;
        break;
    case 1:
        out->R = q;
        out->G = val;
        out->B = p;
        break;
    case 2:
        out->R = p;
        out->G = val;
        out->B = t;
        break;
    case 3:
        out->R = p;
        out->G = q;
        out->B = val;
        break;
    case 4:
        out->R = t;
        out->G = p;
        out->B = val;
        break;
    default:
        out->R = val;
        out->G = p;
        out->B = q;
        break;
    }
}

/** AC22-compatible init «перелив»: fixed green hue, saturation pulse. */
static void LED_RenderInitShimmer(void)
{
    static uint8_t sat = 250U;
    static uint8_t rising = 0U;
    const uint8_t hue = 85U;
    const uint8_t val = 245U;

    if (sat >= 250U) {
        rising = 0U;
    }
    if (sat <= 30U) {
        rising = 1U;
    }
    if (rising) {
        sat = (sat <= 246U) ? (uint8_t)(sat + 4U) : 250U;
    } else {
        sat = (sat >= 34U) ? (uint8_t)(sat - 4U) : 30U;
    }
    LED_HsvToRgb(hue, sat, val, &LED_State.color);
}

static LightingConfigPacket_t committed_lighting = {
    .mode = 0,
    .roles = {
        { 0, 128, 0, 100, 0, 40, 0 },
        { 0, 0, 255, 100, 1, 55, 100 },
        { 0, 255, 0, 100, 1, 40, 87 },
        { 255, 255, 255, 100, 0, 40, 0 },
        { 255, 0, 0, 100, 1, 50, 87 },
    },
};
static LightingConfigPacket_t preview_lighting;
static uint32_t preview_expires_at;
static uint8_t preview_active;

static RGB_Cycle_t LED_CycleFromRole(const LightingRolePacket_t *role)
{
    const uint8_t fade_to_black = role->fadeTo >= 95;
    RGB_Cycle_t cycle = {
        .Color1 = {
            .R = (uint8_t)((uint16_t)role->r * role->brightness / 100),
            .G = (uint8_t)((uint16_t)role->g * role->brightness / 100),
            .B = (uint8_t)((uint16_t)role->b * role->brightness / 100),
        },
        .Tr = (uint8_t)((100 - role->speed) < 5 ? 5 : (100 - role->speed)),
        .Th = 10,
        .Tf = (uint8_t)((100 - role->speed) < 5 ? 5 : (100 - role->speed)),
        .Tl = fade_to_black ? 10 : 0,
    };

    if (role->effect == 0) {
        cycle.Color2 = cycle.Color1;
    } else {
        cycle.Color2.R = (uint8_t)((uint16_t)cycle.Color1.R * (100 - role->fadeTo) / 100);
        cycle.Color2.G = (uint8_t)((uint16_t)cycle.Color1.G * (100 - role->fadeTo) / 100);
        cycle.Color2.B = (uint8_t)((uint16_t)cycle.Color1.B * (100 - role->fadeTo) / 100);
    }
    return cycle;
}

static uint8_t LED_IsLightingConfigValid(const LightingConfigPacket_t *config)
{
    if (config == NULL || config->mode > 1 || (config->flags & 0xFE) != 0 ||
        config->reserved != 0 || ((config->flags & 0x01) && config->previewSec == 0) ||
        (!(config->flags & 0x01) && config->previewSec != 0)) {
        return 0;
    }

    for (uint8_t i = 0; i < 5; i++) {
        const LightingRolePacket_t *role = &config->roles[i];
        if (role->brightness > 100 || role->effect > 1 || role->speed > 100 || role->fadeTo > 100) {
            return 0;
        }
    }
    return 1;
}

uint8_t LED_ApplyLightingConfig(const LightingConfigPacket_t *config)
{
    if (!LED_IsLightingConfigValid(config)) {
        return 0;
    }

    if (config->flags & 0x01) {
        memcpy(&preview_lighting, config, sizeof(preview_lighting));
        preview_active = 1;
        preview_expires_at = HAL_GetTick() + ((uint32_t)config->previewSec * 1000U);
    } else {
        memcpy(&committed_lighting, config, sizeof(committed_lighting));
        preview_active = 0;
    }
    return 1;
}

void LED_Write(void)
{
    const LightingConfigPacket_t *lighting = &committed_lighting;
    RGB_Cycle_t cycle;

    lighting_init_active = 0;

    if (CONN.connControl == CMD_STOP) {
        LED_SetColor(&color_estop);
        return;
    }
    if (CONN.chargingError != CONN_NO_ERROR || CONN.connState == Disabled) {
        cycle = LED_CycleFromRole(&lighting->roles[4]);
        LED_SetColor(&cycle);
        return;
    }

    if (preview_active && (int32_t)(HAL_GetTick() - preview_expires_at) >= 0) {
        preview_active = 0;
    }
    if (preview_active) {
        lighting = &preview_lighting;
    }

    if (lighting->mode == 1) {
        RGB_Cycle_t off = { 0 };
        LED_SetColor(&off);
        return;
    }
    if (CONN.connControl == CMD_FORCE_UNLOCK) {
        LED_SetColor(&color_unlock);
        return;
    }
    if (CONN.connState == Unknown) {
        lighting_init_active = 1;
        return;
    }

    switch (CONN.connState) {
    case Unplugged:
        cycle = LED_CycleFromRole(&lighting->roles[0]);
        break;
    case Preparing:
    case AuthRequired:
    case Replugging:
        cycle = LED_CycleFromRole(&lighting->roles[1]);
        break;
    case WaitingForEnergy:
    case ChargingPausedEV:
    case ChargingPausedEVSE:
    case Charging:
        cycle = LED_CycleFromRole(&lighting->roles[2]);
        break;
    case AuthTimeout:
    case Finished:
    case FinishedEVSE:
    case FinishedEV:
        cycle = LED_CycleFromRole(&lighting->roles[3]);
        break;
    default:
        lighting_init_active = 1;
        return;
    }
    LED_SetColor(&cycle);
}

void interpolateColors(RGB_t* color1, RGB_t* color2, uint16_t a, uint16_t b, RGB_t *result) {

    // Проверяем, чтобы a не выходила за пределы диапазона
    if (a > b) a = b;

    if(b==0) b = 1;

    // Вычисляем коэффициент смешивания в виде целого числа
    uint16_t t = (a * 255) / b; // t будет от 0 до 255

    // Линейная интерполяция с использованием целых чисел
    result->R = (color1->R * (255 - t) + color2->R * t) / 255;
    result->G = (color1->G * (255 - t) + color2->G * t) / 255;
    result->B = (color1->B * (255 - t) + color2->B * t) / 255;

}


#pragma GCC push_options
#pragma GCC optimize("O2")


#define WS2812_T0H_NOP  25
#define WS2812_T0L_NOP  52
#define WS2812_T1H_NOP  52
#define WS2812_T1L_NOP  25

#define _WS2812_DELAY_NOPS(n) __asm volatile(".rept " #n "\nnop\n.endr" ::: "memory")
#define WS2812_DELAY_NOPS(n)  _WS2812_DELAY_NOPS(n)

static void ws2812_send_pixel(uint8_t r, uint8_t g, uint8_t b)
{
	uint32_t tmp = ~(((uint32_t)g << 16) | ((uint32_t)r << 8) | (uint32_t)b);

	for (int i = 0; i < 24; i++) {
		LED_DATA_GPIO_Port->BSRR = LED_DATA_Pin;
		if (tmp & (1U << 23)) {
			WS2812_DELAY_NOPS(WS2812_T0H_NOP);
			LED_DATA_GPIO_Port->BRR = LED_DATA_Pin;
			WS2812_DELAY_NOPS(WS2812_T0L_NOP);
		} else {
			WS2812_DELAY_NOPS(WS2812_T1H_NOP);
			LED_DATA_GPIO_Port->BRR = LED_DATA_Pin;
			WS2812_DELAY_NOPS(WS2812_T1L_NOP);
		}
		tmp <<= 1;
	}
}

static void ws2812_update(RGB_t *led0, RGB_t *led1)
{
	uint32_t primask = __get_PRIMASK();

	__disable_irq();
	ws2812_send_pixel(led0->R, led0->G, led0->B);
	ws2812_send_pixel(led1->R, led1->G, led1->B);
	__set_PRIMASK(primask);
}

#pragma GCC pop_options

static RGB_t RGB_ScaleBrightness(const RGB_t *color)
{
	RGB_t out = {
		.R = (uint8_t)((uint16_t)color->R * WS2812_BRIGHTNESS / 255),
		.G = (uint8_t)((uint16_t)color->G * WS2812_BRIGHTNESS / 255),
		.B = (uint8_t)((uint16_t)color->B * WS2812_BRIGHTNESS / 255),
	};
	return out;
}

/*
 * Моргание красным на втором WS2812 при chargingError > 0.
 * Логика как Slave-LB24 LED_Task: N вспышек = код ошибки, пауза 30×100 ms.
 * Вызывается из LED_Task каждые 20 ms; шаг FSM — 100 ms.
 */
static void LED_Ws2812ErrorBlinkUpdate(void)
{
	static uint8_t err_counter;
	static uint8_t led_state;
	static uint8_t led_pause;
	static uint32_t blink_tick;
	static uint32_t flash_until;
	static CONN_Error_t last_error = CONN_NO_ERROR;

	const CONN_Error_t err = CONN.chargingError;
	const uint32_t now = HAL_GetTick();

	if (err == CONN_NO_ERROR) {
		err_counter = 0;
		led_state = 0;
		led_pause = 0;
		flash_until = 0;
		last_error = CONN_NO_ERROR;
		ws2812_led1.R = 0;
		ws2812_led1.G = 0;
		ws2812_led1.B = 0;
		return;
	}

	if (err != last_error) {
		err_counter = 0;
		led_state = 0;
		led_pause = 0;
		flash_until = 0;
		last_error = err;
	}

	if (now < flash_until) {
		ws2812_led1.R = 255;
		ws2812_led1.G = 0;
		ws2812_led1.B = 0;
		return;
	}

	ws2812_led1.R = 0;
	ws2812_led1.G = 0;
	ws2812_led1.B = 0;

	if ((now - blink_tick) < WS2812_ERROR_BLINK_MS) {
		return;
	}
	blink_tick = now;

	uint8_t led_flash = 0;

	if (led_pause > 0) {
		led_pause--;
	} else if (err_counter < (uint8_t)err) {
		if (led_state == 0) {
			led_state = 1;
		} else {
			led_flash = 1;
			led_state = 0;
			err_counter++;
		}
	} else {
		err_counter = 0;
		led_pause = WS2812_ERROR_BLINK_PAUSE;
	}

	if (led_flash) {
		flash_until = now + WS2812_ERROR_BLINK_MS;
		ws2812_led1.R = 255;
		ws2812_led1.G = 0;
		ws2812_led1.B = 0;
	}
}

void RGB_SetColor(RGB_t *color){
	htim4.Instance->CCR2 = color->R * 100 / 255;
	htim4.Instance->CCR3 = color->G * 100 / 255;
	htim4.Instance->CCR4 = color->B * 100 / 255;
}

void WS2812_SetColor(RGB_t *color){
	RGB_t led0 = RGB_ScaleBrightness(color);
	RGB_t led1 = RGB_ScaleBrightness(&ws2812_led1);
	ws2812_update(&led0, &led1);
}

void LED_SetColor(const RGB_Cycle_t *color){
    memcpy(&LED_Cycle, color, sizeof(RGB_Cycle_t));
}


void LED_Init(){
    RGB_t color = {.R=0, .G=0, .B=0};
    HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_3);
    HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_4);
    RGB_SetColor(&color);
    WS2812_SetColor(&color);
}

void LED_Task(){
	static uint32_t led_tick;
	if((HAL_GetTick() - led_tick) > 20){
		led_tick = HAL_GetTick();
        LED_Ws2812ErrorBlinkUpdate();

        if (lighting_init_active) {
            LED_RenderInitShimmer();
            RGB_SetColor(&LED_State.color);
            WS2812_SetColor(&LED_State.color);
            return;
        }

        LED_State.tick++;
        switch(LED_State.state){
        case LED_RISING:
            interpolateColors(&LED_Cycle.Color2, &LED_Cycle.Color1, LED_State.tick, LED_Cycle.Tr, &LED_State.color);

            if(LED_State.tick>LED_Cycle.Tr){
                LED_State.state = LED_HIGH;
                LED_State.tick = 0;
            }
            break;
        case LED_HIGH:
            memcpy(&LED_State.color, &LED_Cycle.Color1, sizeof(RGB_t));

            if(LED_State.tick>LED_Cycle.Th){
                LED_State.state = LED_FALLING;
                LED_State.tick = 0;
            }
            break;
        case LED_FALLING:
            interpolateColors(&LED_Cycle.Color1, &LED_Cycle.Color2, LED_State.tick, LED_Cycle.Tf, &LED_State.color);

            if(LED_State.tick>LED_Cycle.Tf){
                LED_State.state = LED_LOW;
                LED_State.tick = 0;
            }
            break;
        case LED_LOW:
            memcpy(&LED_State.color, &LED_Cycle.Color2, sizeof(RGB_t));

            if(LED_State.tick>LED_Cycle.Tl){
                LED_State.state = LED_RISING;
                LED_State.tick = 0;
            }
            break;
        default:
            LED_State.state = LED_RISING;
        }
        RGB_SetColor(&LED_State.color);
        WS2812_SetColor(&LED_State.color);
    }
}