#include "status_led.h"
#include "stm32f3xx_hal_gpio.h"
#include "ts_state_machine.h"
#include <stdint.h>

#define STATUS_LED_GPIO_PORT GPIOA //TODO: change to correct values for the board

#define STATUS_LED_1_RED_PIN GPIO_PIN_10 //TODO: change to correct values for the board
#define STATUS_LED_1_GREEN_PIN GPIO_PIN_11
#define STATUS_LED_1_BLUE_PIN GPIO_PIN_12

#define STATUS_LED_2_RED_PIN GPIO_PIN_13 //TODO: change to correct values for the board
#define STATUS_LED_2_GREEN_PIN GPIO_PIN_14
#define STATUS_LED_2_BLUE_PIN GPIO_PIN_15

//TODO: change to correct values for the board
#error "GPIO pins for status LEDs are not set correctly"

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

typedef enum {
    LED_1,
    LED_2
} LedId;

void status_led_init() {
    GPIO_InitTypeDef GPIO_InitStruct = {0};

    GPIO_InitStruct.Pin = STATUS_LED_1_RED_PIN | STATUS_LED_1_GREEN_PIN | STATUS_LED_1_BLUE_PIN | STATUS_LED_2_RED_PIN | STATUS_LED_2_GREEN_PIN | STATUS_LED_2_BLUE_PIN;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;

    HAL_GPIO_Init(STATUS_LED_GPIO_PORT, &GPIO_InitStruct);
}

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_GPIO_PORT, STATUS_LED_1_RED_PIN, red);
            HAL_GPIO_WritePin(STATUS_LED_GPIO_PORT, STATUS_LED_1_GREEN_PIN, green);
            HAL_GPIO_WritePin(STATUS_LED_GPIO_PORT, STATUS_LED_1_BLUE_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;
    }
}