#include <math.h>
#include <stdint.h>
#include <stdio.h>

#include "byte_math.h"
#include "ch573/gpio.h"
#include "ch573/pfic.h"
#include "ch573/pwr.h"
#include "ch573/uart.h"
#include "clock.h"
#include "panic.h"
#include "spi.h"
#include "string.h"
#include "sysled.h"
#include "system.h"
#include "systick.h"
#include "ws2812b.h"

#define PFIC_I CH573_PFIC__PFIC_T_INTF
#define PFIC ch573_pfic__pfic

#define GPIO_PORT_A ch573_gpio__gpio_port_a
#define GPIO_PORT_B ch573_gpio__gpio_port_b
#define GPIO_PORT CH573_GPIO__GPIO_PORT_T_INTF

#define UART1 ch573_uart__uart1
#define UART CH573_UART__UART_T_INTF

#define PWR1 ch573_pwr__pwr_mgmt
#define PWR CH573_PWR__PWR_MGMT_T_INTF

#define AS_BYTE(n) ((n) * 256)

#define min(a, b) (a) < (b) ? (a) : (b)

uint8_t amp(uint8_t in, uint8_t n)
{
  uint32_t out = in;

  for (int i = 0; i < n; ++i) {
    out = (out * in) / 256;
  }

  return min(out, 255);
}

uint32_t collatz(uint32_t n)
{
  uint32_t c = 0;

  while (n > 1) {
    if (n % 2 == 0) {
      n /= 2;
    } else {
      n = n * 3 + 1;
    }
    c++;
  }

  return c;
}

static void set_system_clock_60Mhz(void)
{
  clock_cfg_t clk_cfg = {0};
  clk_cfg.sel = CLOCK_SELECTION_PLL;
  clk_cfg.pll_clock_divisor = 8;
  if (set_system_clock(&clk_cfg)) {
    printf("Failed to set system clock.\n");
    panic(0xff);
  }
}

#define N_LEDS 150
#define N_DMA_XFER 2

extern int uart1_FILE_get(FILE* f);

static inline uint8_t byte_scale(uint8_t v, uint8_t scale)
{
  uint16_t acc = v;
  return (acc * scale) >> 8;
}

uint8_t clip(int x)
{
  if (x > 240) {
    return 240;
  }

  if (x < 0) {
    return 0;
  }

  return (uint8_t)x;
}

#define TIME_STEP 1

rgb_t get_rgb(uint32_t time, size_t x)
{
  int r = 0, g = 0, b = 0;

  uint8_t time8 = time & 0xff;
  int w = calc_w(time8 * TIME_STEP + x * 4);

  r = 0xff;
  g = byte_scale(byte_sin(time8 * TIME_STEP + x * 2), 0x90);
  b = byte_scale(byte_sin(time8 * TIME_STEP + x * 2), 0x20);

  rgb_t color;
  color.color = 0;
  color.r = clip(r + w);
  color.g = clip(byte_scale(g, AS_BYTE(0.75)) + w);
  color.b = clip(byte_scale(b, AS_BYTE(0.75)) + w);
  return color;
}


static volatile uint32_t time = 0;
static volatile int spin_lock = 0;

On_SysTick() {
  time ++;
  spin_lock = 0;
}

/*
 * Main routine. This is called on_reset once everything else has been set up.
 */
int main(void)
{
  char buf[N_LEDS * TOTAL_BYTES_PER_LED];

  PFIC_I.vector_table_control.set(PFIC, 1);
  PFIC->interrupt_priority_threshold = 0x10;
  PFIC->interrupt_enable |= IRQ_SysTick;

  set_system_clock_60Mhz();
  set_systick(250000);

  enable_sysled();
  enable_spi();

  size_t n = sizeof(buf);
  struct ws2812b_buf ws_buf;
  make_wsb2812b(&ws_buf, buf, n);
  ws_buf.byte_order = BYTE_ORDER_GRB;

  rgb_t color;

  GPIO_PORT.dir.set(GPIO_PORT_B, DIR_OUT, 7);

  while (1) {
    ws_buf.cur = 0;
    for (int i = 0; i < N_LEDS; ++i) {
      rgb_t rgb = get_rgb(time, i);
      write_rgb(&ws_buf, rgb);
    }

    while (spin_lock) {
      set_sysled(1); // Setting the sysled helps me to measure down time.
    }
    set_sysled(0);

    spin_lock = 1;

    for (int j = 0; j < N_DMA_XFER; ++ j) {
      wait_for_dma();
      start_dma(&ws_buf);
    }
  }

  return 0;
}