#include "rgb_controller.h"

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

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(FireAlarm_IsLatched() || CONN.chargingError == CONN_ERR_FIRE_ALARM){
      LED_SetColor(&color_error);
      return;
    }
    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;

}


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

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