//#include "led_task.h"

#include "FreeRTOS.h"
#include "task.h"
#include "cmsis_os.h"
#include "main.h"
#include <stdbool.h>
//#include "led_rgb.h"

xTaskHandle ledTaskHandle;

typedef enum LED_STATE{
    LED_IDLE,              ///< No car connected
    LED_CONNECTED_IDLE,    ///< Car connected, waiting for permission
    LED_CHARGING,          ///< Car connected, charging
    LED_ERROR,             ///< Any error
}LED_STATE;

LED_STATE curr_state = LED_IDLE;


uint8_t charge_val = 0;
bool charge_dir = 1;
void led_pulse_charging(void) {
    if (charge_val >= 255) charge_dir = 0;
    if (charge_val <= 10) charge_dir = 1;
    if (charge_dir)
        charge_val += 1;
    if (!charge_dir)
        charge_val -= 1;
    led_setHSV(85, 255, charge_val);          // TODO: Check it
//    led_setRGB(0, charge_val, 0);
}

uint8_t idle_val = 241;
bool idle_dir = 1;
uint8_t idle_ctr = 0;

void led_pulse_idle(void) {
    idle_ctr++;
    if (idle_ctr < 40) return;
    idle_ctr = 0;
    if (idle_val >= 255) idle_dir = 0;
    if (idle_val <= 231) idle_dir = 1;
    if (idle_dir)
        idle_val += 1;
    if (!idle_dir)
        idle_val -= 1;
    led_setHSV(85, idle_val, idle_val);
}

//#include "charge_inf_data.h"
//#include "evcom_data.h"


void led_main(void *pvParameters)
{
    (void) pvParameters;
    led_init(NULL, 0, 0, 0);
    int time_blue = 0;
    int time_blue_low = 0;
    curr_state = LED_IDLE;
	while(1)
	{

		uint8_t R_level, G_level, B_level;

		R_level = !HAL_GPIO_ReadPin (GPIOA, GPIO_PIN_7);
        G_level = !HAL_GPIO_ReadPin (GPIOA, GPIO_PIN_6);
        B_level = !HAL_GPIO_ReadPin (GPIOA, GPIO_PIN_5);
        // If no connection & no contactor closed

        if (B_level == false){

            if(time_blue > 70 && time_blue < 130) curr_state = LED_IDLE;
            time_blue_low++;
            if(time_blue_low>7){ //hold low for 35ms, anti debounce
            	time_blue = 0;
            }
        }

        if (B_level == true){
            time_blue_low = 0;
            time_blue++;
        }

        if(time_blue>120) curr_state = LED_CONNECTED_IDLE;

        if (G_level == true){
            curr_state = LED_CHARGING;
        }
        if (R_level == true){
            curr_state = LED_ERROR;
        }
        if(HAL_GetTick()<10000)curr_state = LED_IDLE;
        // Set color for LED
        switch (curr_state) {
            case LED_IDLE:
            //led_pulse_idle();
                led_setRGB(255, 10, 60);
                break;
            case LED_CONNECTED_IDLE:
            // led_setRGB(0, 0, 255);
                led_setRGB(255, 10, 60);
                break;
            case LED_CHARGING:
            // led_pulse_charging();
                led_setRGB(0, 0, 255);
                break;
            default:
            // led_setRGB(255, 0, 0);
                led_setRGB(255, 130, 90);
                break;
        }
        
        osDelay(5);
	}
}


void led_task_init(void)
{
	xTaskCreate(led_main, "RGB Task", 128, NULL, 1, &ledTaskHandle);
}