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

static void set_led_color(LedId id, LedColor color) {
    GPIO_PinState red = color.red ? GPIO_PIN_SET : GPIO_PIN_RESET;
    GPIO_PinState green = color.green ? GPIO_PIN_SET : GPIO_PIN_RESET;
    GPIO_PinState blue = color.blue ? GPIO_PIN_SET : GPIO_PIN_RESET;

    switch (id) {
        case LED_1:
            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);
            break;
        case LED_2:
            //HAL_GPIO_WritePin(STATUS_LED_GPIO_PORT, STATUS_LED_2_RED_PIN, red);
            //HAL_GPIO_WritePin(STATUS_LED_GPIO_PORT, STATUS_LED_2_GREEN_PIN, green);
            //HAL_GPIO_WritePin(STATUS_LED_GPIO_PORT, STATUS_LED_2_BLUE_PIN, blue);
            break;
    }
}

/**
  * @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:   off, 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: green, LED_2: cyan
  * TS_CHARGING       -> LED_1: green, 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 = 0, .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 = 0, .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 = 0, .green = 1, .blue = 0});
            set_led_color(LED_2, (LedColor) {.red = 0, .green = 0, .blue = 0});
            break;
    }
}