#include "leds.h"

#include "app_azure_rtos.h"
#include "main.h"

#include "stm32h7xx_hal_dma.h"
#include "stm32h7xx_hal_gpio.h"
#include "stm32h7xx_hal_spi.h"
#include "stm32h7xx_hal_tim.h"
#include "tx_api.h"

SPI_HandleTypeDef *hspi;
TIM_HandleTypeDef *htim;

extern uint16_t led_buf[256][3];
static size_t led_buf_idx = 0;
static TX_THREAD child_thread;
static void *child_thread_stack;

void led_init(SPI_HandleTypeDef *spi, TIM_HandleTypeDef *pwmtim) {
  hspi = spi;
  htim = pwmtim;

  HAL_GPIO_WritePin(LED_LE_GPIO_Port, LED_LE_Pin, GPIO_PIN_RESET);
  memset(led_buf, 0, sizeof(led_buf));

  if (HAL_SPI_Transmit_DMA(hspi, (const uint8_t *)&led_buf[led_buf_idx], 3) !=
      HAL_OK) {
    Error_Handler();
  }

  __HAL_TIM_SET_COMPARE(htim, PWM_CHANNEL_R, 0x3FFF);
  __HAL_TIM_SET_COMPARE(htim, PWM_CHANNEL_G, 0x13FF);
  __HAL_TIM_SET_COMPARE(htim, PWM_CHANNEL_B, 0x1FFF);
  if (HAL_TIM_PWM_Start(htim, PWM_CHANNEL_R) != HAL_OK) {
    Error_Handler();
  }
  if (HAL_TIM_PWM_Start(htim, PWM_CHANNEL_G) != HAL_OK) {
    Error_Handler();
  }
  if (HAL_TIM_PWM_Start(htim, PWM_CHANNEL_B) != HAL_OK) {
    Error_Handler();
  }
}

void led_set(size_t idx, uint8_t r, uint8_t g, uint8_t b) {
  uint16_t led_set = (1 << idx);
  uint16_t led_unset = ~led_set;
  uint8_t rgb[] = {b, g, r};
  for (size_t time = 0; time < 256; time++) {
    // TODO: Shouldn't time only go up to 254?
    for (size_t i = 0; i < 3; i++) {
      if (time < rgb[i]) {
        led_buf[time][i] |= led_set;
      } else {
        led_buf[time][i] &= led_unset;
      }
    }
  }
}

void led_all_off() { memset(led_buf, 0, sizeof(led_buf)); }

void led_thread_entry(ULONG child_thread_stack_addr) {
  child_thread_stack = (void *)child_thread_stack_addr;
  led_start_animation(ANIM_RAINBOW);
  while (1) {
    tx_thread_sleep(10);
    if (child_thread.tx_thread_state == TX_COMPLETED) {
      tx_thread_sleep(20);
      led_start_animation(ANIM_TE_STARTUP);
    }
  }
}

void led_start_animation(LEDAnimation anim) {
  if (child_thread.tx_thread_stack_start && // Check if any thread was started
                                            // previously
      child_thread.tx_thread_state != TX_COMPLETED &&
      child_thread.tx_thread_state != TX_TERMINATED) {
    if (tx_thread_terminate(&child_thread) != TX_SUCCESS) {
      Error_Handler();
    }
  }
  if (child_thread.tx_thread_stack_start && // Check if any thread was started
                                            // previously
      tx_thread_delete(&child_thread) != TX_SUCCESS) {
    Error_Handler();
  }

  void (*animation_entry)(ULONG);
  switch (anim) {
  case ANIM_TE_STARTUP:
    animation_entry = led_anim_te_startup;
    break;
  case ANIM_FT_STARTUP:
    animation_entry = led_anim_ft_startup;
    break;
  case ANIM_KNIGHT_RIDER:
    animation_entry = led_anim_knight_rider;
    break;
  case ANIM_RAINBOW:
    animation_entry = led_anim_rainbow;
    break;
  default:
    return;
  }

  led_all_off();
  if (tx_thread_create(&child_thread, "LED Animation Thread", animation_entry,
                       0, child_thread_stack, THREAD_STACK_SIZE,
                       THREAD_PRIO_LED_CHILD, THREAD_PRIO_LED_CHILD, 0,
                       TX_AUTO_START) != TX_SUCCESS) {
    Error_Handler();
  }
}

void led_anim_te_startup(ULONG _) {
  led_anim_blinker(0xF2, 0x8B, 0x00, 20, 7, 2);
}

void led_anim_ft_startup(ULONG _) { led_anim_blinker(0xC5, 0, 0, 20, 7, 2); }

void led_anim_knight_rider(ULONG _) {
  const size_t num_leds = 5;
  int tail_right = 1;
  size_t tail = 0;
  while (1) {
    size_t x = tail;
    int x_right = tail_right;
    uint8_t red = 0;
    for (size_t i = 0; i < num_leds + 1; i++) {
      led_set(x, red, 0, 0);
      if (x == 8) {
        x_right = 0;
      } else if (x == 0) {
        x_right = 1;
      }
      if (x_right) {
        x++;
      } else {
        x--;
      }
      red += 0xFF / num_leds;
    }
    if (tail == 8) {
      tail_right = 0;
    } else if (tail == 0) {
      tail_right = 1;
    }
    if (tail_right) {
      tail++;
    } else {
      tail--;
    }
    tx_thread_sleep(5);
  }
}

void led_anim_rainbow(ULONG _) {
  size_t tail = 0;
  float offset = 0.0f;
  while (1) {
    for (size_t i = 0; i < N_LEDS; i++) {
      size_t led = (tail + i) % N_LEDS;
      uint8_t r, g, b;
      float hue = fmodf(offset + i * 360.0f / N_LEDS, 360.0f);
      led_hsv_to_rgb(hue, 1, 1, &r, &g, &b);
      led_set(led, r, g, b);
    }
    offset = fmodf(offset + 2.0f, 360.0f);
    tx_thread_sleep(1);
  }
}

void led_anim_blinker(uint8_t r, uint8_t g, uint8_t b,
                      uint32_t brightness_steps, uint32_t next_led_steps,
                      uint32_t delay) {
  uint8_t colors[brightness_steps][3];
  for (int i = brightness_steps - 1; i >= 0; i--) {
    colors[i][0] = r * i / (brightness_steps - 1);
    colors[i][1] = g * i / (brightness_steps - 1);
    colors[i][2] = b * i / (brightness_steps - 1);
  }
  size_t simultaneous_leds = brightness_steps / next_led_steps;
  if (simultaneous_leds * next_led_steps != brightness_steps) {
    simultaneous_leds++;
  }
  int inc = 1;
  for (int i = 0; i < 2; i++) {
    int32_t furthest = 0;
    size_t color_idx = 0;
    while (furthest < 5 + simultaneous_leds) {
      for (size_t offset = 0; offset < simultaneous_leds; offset++) {
        int32_t diff = furthest - offset;
        size_t led_color_idx = color_idx + offset * next_led_steps;
        if (diff < 0 || diff > 4 || led_color_idx >= brightness_steps) {
          continue;
        }
        if (!inc) {
          led_color_idx = brightness_steps - led_color_idx - 1;
        }
        uint8_t *color = colors[led_color_idx];
        led_set(4 + diff, color[0], color[1], color[2]);
        led_set(4 - diff, color[0], color[1], color[2]);
      }
      color_idx++;
      if (color_idx == next_led_steps) {
        color_idx = 0;
        furthest++;
      }
      tx_thread_sleep(delay);
    }
    inc = !inc;
  }
}

void led_hsv_to_rgb(float h, float s, float v, uint8_t *r, uint8_t *g,
                    uint8_t *b) {
  float c = v * s;
  float x = c * (1 - fabs(fmod(h / 60.0, 2) - 1));
  float m = v - c;
  float r1, g1, b1;
  if (h < 60) {
    r1 = c;
    g1 = x;
    b1 = 0;
  } else if (h < 120) {
    r1 = x;
    g1 = c;
    b1 = 0;
  } else if (h < 180) {
    r1 = 0;
    g1 = c;
    b1 = x;
  } else if (h < 240) {
    r1 = 0;
    g1 = x;
    b1 = c;
  } else if (h < 300) {
    r1 = x;
    g1 = 0;
    b1 = c;
  } else {
    r1 = c;
    g1 = 0;
    b1 = x;
  }
  *r = (int)((r1 + m) * 255.0 + 0.5);
  *g = (int)((g1 + m) * 255.0 + 0.5);
  *b = (int)((b1 + m) * 255.0 + 0.5);
}

void HAL_SPI_TxCpltCallback(SPI_HandleTypeDef *handle) {
  if (handle != hspi) {
    return;
  }
  led_buf_idx = (led_buf_idx + 1) % 256;
  HAL_GPIO_WritePin(LED_LE_GPIO_Port, LED_LE_Pin, GPIO_PIN_SET);
  for (size_t i = 0; i < 10; i++) {
    asm("nop" ::: "memory");
  }
  HAL_GPIO_WritePin(LED_LE_GPIO_Port, LED_LE_Pin, GPIO_PIN_RESET);
  for (size_t i = 0; i < 10; i++) {
    asm("nop" ::: "memory");
  }
  HAL_SPI_Transmit_DMA(hspi, (const uint8_t *)&led_buf[led_buf_idx], 3);
}

void HAL_SPI_ErrorCallback(SPI_HandleTypeDef *handle) {
  if (handle != hspi) {
    return;
  }
  volatile uint32_t err = HAL_DMA_GetError(hspi->hdmatx);
  err = err;
}