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

#include "byte_math.h"
#include "ch573/gpio.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 "ws2812b.h"

#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 print_reg(uint32_t reg)
{
  // 0x40001008
  printf("register @0x%08x = 0x%08x\n", reg, *(uint32_t*)reg);
}

static void set_system_clock_6Mhz(void)
{
  print_reg(0x40001008);
  printf("Setting system clock to 6Mhz...\n");
  clock_cfg_t clk_cfg = {0};
  clk_cfg.sel = CLOCK_SELECTION_HSE;
  clk_cfg.pll_clock_divisor = 5;
  if (set_system_clock(&clk_cfg)) {
    printf("Failed to set system clock.\n");
  }
  print_reg(0x40001008);
  printf("Done\n");
}

static void set_system_clock_3Mhz(void)
{
  print_reg(0x40001008);
  printf("Setting system clock to 3Mhz...\n");
  clock_cfg_t clk_cfg = {0};
  clk_cfg.sel = CLOCK_SELECTION_HSE;
  clk_cfg.pll_clock_divisor = 10;
  if (set_system_clock(&clk_cfg)) {
    printf("Failed to set system clock.\n");
  }
  print_reg(0x40001008);
  printf("Done\n");
}

static void set_system_clock_60Mhz(void)
{
  print_reg(0x40001008);
  printf("Setting system clock to 60Mhz...\n");
  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");
  }
  print_reg(0x40001008);
  printf("Done\n");
}

static void set_system_clock_30Mhz(void)
{
  print_reg(0x40001008);
  printf("Setting system clock to 30Mhz...\n");
  clock_cfg_t clk_cfg = {0};
  clk_cfg.sel = CLOCK_SELECTION_PLL;
  clk_cfg.pll_clock_divisor = 16;
  if (set_system_clock(&clk_cfg)) {
    printf("Failed to set system clock.\n");
  }
  print_reg(0x40001008);
  printf("Done\n");
}

static void set_system_clock_32kHz()
{
  print_reg(0x40001008);
  printf("Setting system clock to 32kHz...\n");
  clock_cfg_t clk_cfg = {0};
  clk_cfg.sel = CLOCK_SELECTION_LSE;
  if (set_system_clock(&clk_cfg)) {
    printf("Failed to set system clock.\n");
  }
}

static void basic_delay()
{
  set_sysled(1);
  for (int i = 0; i < 30000; ++i) {
    asm volatile("");
  }
  set_sysled(0);
}

static void fast_delay()
{
  for (int i = 0; i < 1000; ++i) {
    asm volatile("");
  }
}

#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);
  // r = byte_scale(byte_sin(time8 * TIME_STEP + x * 2), AS_BYTE(0.25)) +
  //     AS_BYTE(0.75);
  // b = 0x40;
  // g = byte_scale(
  //         0xff - byte_sin(time8 * TIME_STEP + x * 7 + 0x80), AS_BYTE(0.5)) +
  //     AS_BYTE(0.5);

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

/*
 * 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];

  set_system_clock_60Mhz();
  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_GBR;

  printf("Test %u", byte_scale(40, 0x80));

  rgb_t color;
  uint32_t time = 0;

  GPIO_PORT.dir.set(GPIO_PORT_B, DIR_OUT, 7);
  while (1) {
    // Fill the buffer with calculated rgb colors.
    ws_buf.cur = 0;
    for (int i = 0; i < N_LEDS; ++i) {
      rgb_t rgb = get_rgb(time, i);
      write_rgb(&ws_buf, rgb);
    }

    time++;
    GPIO_PORT.out.set(GPIO_PORT_B, ON, 7);
    // printf("WS_BUF: %p, %zu\n", ws_buf.buf, ws_buf.cur);
    for (int i = 0; i < N_DMA_XFER; ++i) {
      wait_for_dma();
      start_dma(&ws_buf);
    }
    basic_delay();
  }

  return 0;
}