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

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

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,
 };

RGB_Cycle_t color_unknown = {
	.Color1 = { .R = 64, .G = 0, .B = 0 },
	.Color2 = { .R = 64, .G = 0, .B = 0 },
	.Tr = 50,
	.Th = 10,
	.Tf = 50,
	.Tl = 0,
};

RGB_Cycle_t color_light = {
	.Color1 = { .R = 0, .G = 255, .B = 0 },
	.Color2 = { .R = 0, .G = 255, .B = 0 },
	.Tr = 50,
	.Th = 10,
	.Tf = 50,
	.Tl = 0,
};

RGB_Cycle_t color_disabled = {
	.Color1 = { .R = 250, .G = 0, .B = 0 },
	.Color2 = { .R = 32, .G = 0, .B = 0 },
	.Tr = 50,
	.Th = 10,
	.Tf = 50,
	.Tl = 0,
};

RGB_Cycle_t color_unplugged = {
	.Color1 = { .R = 0, .G = 128, .B = 0 },
	.Color2 = { .R = 0, .G = 128, .B = 0 },
	.Tr = 50,
	.Th = 10,
	.Tf = 50,
	.Tl = 0,
};

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

RGB_Cycle_t color_charging = {
	.Color1 = { .R = 0, .G = 255, .B = 0 },
	.Color2 = { .R = 0, .G = 32, .B = 0 },
	.Tr = 50,
	.Th = 10,
	.Tf = 50,
	.Tl = 0,
};

RGB_Cycle_t color_finished = {
	.Color1 = { .R = 255, .G = 255, .B = 255 },
	.Color2 = { .R = 255, .G = 255, .B = 255 },
	.Tr = 50,
	.Th = 10,
	.Tf = 50,
	.Tl = 0,
};

RGB_Cycle_t color_error = {
	.Color1 = { .R = 255, .G = 0, .B = 0 },
	.Color2 = { .R = 32, .G = 0, .B = 0 },
	.Tr = 50,
	.Th = 10,
	.Tf = 50,
	.Tl = 0,
};

void LED_Write(){
    if(CONN.chargingError != CONN_NO_ERROR){
      LED_SetColor(&color_error);
      return;
    }
    if(CONN.connControl == CMD_FORCE_UNLOCK){
      LED_SetColor(&color_unlock);
      return;
    }
    if(CONN.connControl == CMD_STOP){
      LED_SetColor(&color_estop);
      return;
    }
    switch(CONN.connState){
    case Unknown:
      LED_SetColor(&color_unknown);
      break;
    case Unplugged:
      LED_SetColor(&color_unplugged);
      break;
    case Disabled:
      LED_SetColor(&color_error);
      break;
    case Preparing:
      LED_SetColor(&color_preparing);
      break;
    case AuthRequired:
      LED_SetColor(&color_preparing);
      break;
    case WaitingForEnergy:
      LED_SetColor(&color_charging);
      break;
    case ChargingPausedEV:
      LED_SetColor(&color_charging);
      break;
    case ChargingPausedEVSE:
      LED_SetColor(&color_charging);
      break;
    case Charging:
      LED_SetColor(&color_charging);
      break;
    case AuthTimeout:
      LED_SetColor(&color_finished);
      break;
    case Finished:
      LED_SetColor(&color_finished);
      break;
    case FinishedEVSE:
      LED_SetColor(&color_finished);
      break;
    case FinishedEV:
      LED_SetColor(&color_finished);
      break;
    case Replugging:
      LED_SetColor(&color_preparing);
      break;
    default:
      LED_SetColor(&color_unknown);
      break;
    }
  }

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(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_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;
        }
        LED_Ws2812ErrorBlinkUpdate();
        RGB_SetColor(&LED_State.color);
        WS2812_SetColor(&LED_State.color);
    }
}