#include "status_led.h"
#include "main.h"
#include "stm32h7xx_hal.h"
#include "ts_state_machine.h"
#include <stdint.h>

#warning enable the second LED

typedef struct {
    uint8_t red : 1;
    uint8_t green : 1;
    uint8_t blue : 1;
} LedColor;

typedef enum {
    LED_1,
    LED_2
} LedId;

uint32_t count = 0;
bool main_led = false;
LedColor led_1 = (LedColor) {.red = 0, .green = 0, .blue = 0};
LedColor led_2 = (LedColor) {.red = 0, .green = 0, .blue = 0};
#define ITER_COUNT 20
#define INTERN_ERROR_COUNT 7500

static void set_led_color_hw(LedColor color) {
    GPIO_PinState red = color.red ? GPIO_PIN_RESET : GPIO_PIN_SET;
    GPIO_PinState green = color.green ? GPIO_PIN_RESET : GPIO_PIN_SET;
    GPIO_PinState blue = color.blue ? GPIO_PIN_RESET : GPIO_PIN_SET;

    HAL_GPIO_WritePin(STATUS_LED_R_GPIO_Port, STATUS_LED_R_Pin, red);
    HAL_GPIO_WritePin(STATUS_LED_G_GPIO_Port, STATUS_LED_G_Pin, green);
    HAL_GPIO_WritePin(STATUS_LED_B_GPIO_Port, STATUS_LED_B_Pin, blue);
}

static void set_led_color(LedId id, LedColor color) {
    switch (id) {
        case LED_1:
            led_1 = color;
            break;
        case LED_2:
            led_2 = color;
            break;
    }

    set_led_color_hw(main_led ? led_1 : led_2);
    count++;

    if (count > ITER_COUNT) {
        count = 0;
        main_led = !main_led;
    }
}

void set_led_internal_error() {
    led_1 = (LedColor) {.red = 1, .green = 0, .blue = 0};
    led_2 = (LedColor) {.red = 0, .green = 0, .blue = 0};

    set_led_color_hw(main_led ? led_1 : led_2);
    count++;

    if (count > INTERN_ERROR_COUNT) {
        count = 0;
        main_led = !main_led;
    }
}

/**
  * @brief Sets the status LEDs to the given state
  * 
  * @param state The state to set the LEDs to
  *
  * State -> Color mapping:
  * TS_INACTIVE       -> LED_1:  green, LED_2: off
  * TS_ACTIVE         -> LED_1:   blue, LED_2: off
  * TS_PRECHARGE      -> LED_1:   blue, LED_2: cyan
  * TS_DISCHARGE      -> LED_1:   blue, LED_2: magenta
  * TS_ERROR          -> LED_1:    red, LED_2: <see below>
  * TS_CHARGING_CHECK -> LED_1: yellow, LED_2: cyan
  * TS_CHARGING       -> LED_1: yellow, LED_2: off
  *
  * Error -> LED_2 mapping:
  * TS_ERRORKIND_NONE              -> off
  * TS_ERRORKIND_SLAVE_TIMEOUT     -> blue
  * TS_ERRORKIND_SLAVE_PANIC       -> magenta
  * TS_ERRORKIND_SHUNT_TIMEOUT     -> green
  * TS_ERRORKIND_SHUNT_OVERCURRENT -> yellow
  * TS_ERRORKIND_SHUNT_OVERTEMP    -> red
  */

void status_led_state(TSState state, TSErrorKind error) {
    switch (state) {
        case TS_INACTIVE:
            set_led_color(LED_1, (LedColor) {.red = 0, .green = 1, .blue = 0});
            set_led_color(LED_2, (LedColor) {.red = 0, .green = 0, .blue = 0});
            break;
        case TS_ACTIVE:
            set_led_color(LED_1, (LedColor) {.red = 0, .green = 0, .blue = 1});
            set_led_color(LED_2, (LedColor) {.red = 0, .green = 0, .blue = 0});
            break;
        case TS_PRECHARGE:
            set_led_color(LED_1, (LedColor) {.red = 0, .green = 0, .blue = 1});
            set_led_color(LED_2, (LedColor) {.red = 0, .green = 1, .blue = 1});
            break;
        case TS_DISCHARGE:
            set_led_color(LED_1, (LedColor) {.red = 0, .green = 0, .blue = 1});
            set_led_color(LED_2, (LedColor) {.red = 1, .green = 0, .blue = 1});
            break;
        case TS_ERROR:
            set_led_color(LED_1, (LedColor) {.red = 1, .green = 0, .blue = 0});
            switch (error) {
                case TS_ERRORKIND_NONE:
                    set_led_color(LED_2, (LedColor) {.red = 0, .green = 0, .blue = 0});
                    break;
                case TS_ERRORKIND_SLAVE_TIMEOUT:
                    set_led_color(LED_2, (LedColor) {.red = 0, .green = 0, .blue = 1});
                    break;
                case TS_ERRORKIND_SLAVE_PANIC:
                    set_led_color(LED_2, (LedColor) {.red = 1, .green = 0, .blue = 1});
                    break;
                case TS_ERRORKIND_SHUNT_TIMEOUT:
                    set_led_color(LED_2, (LedColor) {.red = 0, .green = 1, .blue = 0});
                    break;
                case TS_ERRORKIND_SHUNT_OVERCURRENT:
                    set_led_color(LED_2, (LedColor) {.red = 1, .green = 1, .blue = 0});
                    break;
                case TS_ERRORKIND_SHUNT_OVERTEMP:
                    set_led_color(LED_2, (LedColor) {.red = 1, .green = 0, .blue = 0});
                    break;
            }
            break;
        case TS_CHARGING_CHECK:
            set_led_color(LED_1, (LedColor) {.red = 1, .green = 1, .blue = 0});
            set_led_color(LED_2, (LedColor) {.red = 0, .green = 1, .blue = 1});
            break;
        case TS_CHARGING:
            set_led_color(LED_1, (LedColor) {.red = 1, .green = 1, .blue = 0});
            set_led_color(LED_2, (LedColor) {.red = 0, .green = 0, .blue = 0});
            break;
    }
}