#include "arch.h"
#include "arch/arm/cortex-m4/mpu.h"
#include "arch/stm32l4xxx/peripherals/clock.h"
#include "arch/stm32l4xxx/peripherals/dma.h"
#include "arch/stm32l4xxx/peripherals/irq.h"
#include "arch/stm32l4xxx/peripherals/rcc.h"
#include "arch/stm32l4xxx/peripherals/spi.h"
#include "arch/stm32l4xxx/peripherals/system.h"
#include "drv/ws2812B/ws2812b.h"
#include "kern/delay.h"
#include "kern/systick/systick_manager.h"
#include "kern/dma/dma_manager.h"
#include "kern/gpio/gpio_manager.h"
#include "kern/gpio/sysled.h"
#include "kern/init.h"
#include "kern/log.h"
#include "kern/mem.h"
#include "kern/mpu/mpu_manager.h"
#include "kern/panic.h"
#include "kern/priv.h"
#include "user/syscall.h"

void on_hard_fault()
{
  panic("Hard fault encountered!\n");
}

#ifdef ARCH_STM32L4

void configure_spi1()
{
  int ec = 0;

  gpio_enable_alternate_function(
      GPIO_ALTERNATE_FUNCTION_SPI1_MOSI, GPIO_PIN_PA7, &ec);
  if (ec) {
    panic("Unable to set pin PA7 (ec=%d)\n", ec);
  }
  gpio_enable_alternate_function(
      GPIO_ALTERNATE_FUNCTION_SPI1_NSS, GPIO_PIN_PA4, &ec);
  if (ec) {
    panic("Unable to set pin PA4 (ec=%d)\n", ec);
  }
  gpio_enable_alternate_function(
      GPIO_ALTERNATE_FUNCTION_SPI1_SCK, GPIO_PIN_PA5, &ec);
  if (ec) {
    panic("Unable to set pin PA5 (ec=%d)\n", ec);
  }

  regset(RCC.apb2en_r, rcc_spi1en, 1);

  uint32_t reg = 0;
  regset(reg, spi_ldma_tx, 0);
  regset(reg, spi_ldma_rx, 0);
  regset(reg, spi_frxth, 0);
  regset(reg, spi_ds, SPI_DATA_SIZE_8_BITS);
  regset(reg, spi_txeie, 0);
  regset(reg, spi_rxneie, 0);
  regset(reg, spi_errie, 0);
  regset(reg, spi_frf, 0);
  regset(reg, spi_nssp, 0);
  regset(reg, spi_ssoe, 0);
  regset(reg, spi_txdmaen, 0);
  regset(reg, spi_rxdmaen, 0);
  SPI1.c_r2 = reg;

  reg = 0;
  regset(reg, spi_bidimode, 0);
  regset(reg, spi_crcen, 0);
  regset(reg, spi_crcnext, 0);
  regset(reg, spi_crcl, 0);
  regset(reg, spi_rxonly, 0);
  regset(reg, spi_ssm, 1);
  regset(reg, spi_ssi, 1);
  regset(reg, spi_lsbfirst, 0);
  regset(reg, spi_spe, 1);
  regset(reg, spi_br, SPI_BAUD_FPCLK_DIV_32);
  regset(reg, spi_mstr, 1);
  regset(reg, spi_cpol, 0);
  regset(reg, spi_cpha, 0);
  SPI1.c_r1 = reg;
}

static uint8_t* compiled;
static size_t compiled_len;

extern uint8_t sintable[256];

static uint32_t time;

static void on_systick(void* nil)
{
  // klogf("Systick.\n");
  ++time;
}

static void spi_write(uint8_t byte)
{
  while (!regget(SPI1.s_r, spi_txe))
    ;
  SPI1.dl_r = byte;
  asm volatile("nop");
}

static void write_rgb(uint8_t red, uint8_t green, uint8_t blue)
{
#undef BIT
#define BIT(b, n) (!!((b) & (1 << (n))))
  spi_write(
      (1 << 7) | (BIT(green, 7) << 6) | (0 << 5) | (0 << 4) | (1 << 3) |
      (BIT(green, 6) << 2) | (0 << 1) | (0 << 0));
  spi_write(
      (1 << 7) | (BIT(green, 5) << 6) | (0 << 5) | (0 << 4) | (1 << 3) |
      (BIT(green, 4) << 2) | (0 << 1) | (0 << 0));
  spi_write(
      (1 << 7) | (BIT(green, 3) << 6) | (0 << 5) | (0 << 4) | (1 << 3) |
      (BIT(green, 2) << 2) | (0 << 1) | (0 << 0));
  spi_write(
      (1 << 7) | (BIT(green, 1) << 6) | (0 << 5) | (0 << 4) | (1 << 3) |
      (BIT(green, 0) << 2) | (0 << 1) | (0 << 0));

  spi_write(
      (1 << 7) | (BIT(red, 7) << 6) | (0 << 5) | (0 << 4) | (1 << 3) |
      (BIT(red, 6) << 2) | (0 << 1) | (0 << 0));
  spi_write(
      (1 << 7) | (BIT(red, 5) << 6) | (0 << 5) | (0 << 4) | (1 << 3) |
      (BIT(red, 4) << 2) | (0 << 1) | (0 << 0));
  spi_write(
      (1 << 7) | (BIT(red, 3) << 6) | (0 << 5) | (0 << 4) | (1 << 3) |
      (BIT(red, 2) << 2) | (0 << 1) | (0 << 0));
  spi_write(
      (1 << 7) | (BIT(red, 1) << 6) | (0 << 5) | (0 << 4) | (1 << 3) |
      (BIT(red, 0) << 2) | (0 << 1) | (0 << 0));

  spi_write(
      (1 << 7) | (BIT(blue, 7) << 6) | (0 << 5) | (0 << 4) | (1 << 3) |
      (BIT(blue, 6) << 2) | (0 << 1) | (0 << 0));
  spi_write(
      (1 << 7) | (BIT(blue, 5) << 6) | (0 << 5) | (0 << 4) | (1 << 3) |
      (BIT(blue, 4) << 2) | (0 << 1) | (0 << 0));
  spi_write(
      (1 << 7) | (BIT(blue, 3) << 6) | (0 << 5) | (0 << 4) | (1 << 3) |
      (BIT(blue, 2) << 2) | (0 << 1) | (0 << 0));
  spi_write(
      (1 << 7) | (BIT(blue, 1) << 6) | (0 << 5) | (0 << 4) | (1 << 3) |
      (BIT(blue, 0) << 2) | (0 << 1) | (0 << 0));
}

void latch()
{
  for (int i = 0; i < 20; ++i) {
    spi_write(0);
  }
}

static uint8_t amp(uint8_t in)
{
  uint8_t out = in;

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

  return out;
}

/* Main function. This gets executed from the interrupt vector defined above. */
int main()
{
  gpio_reserved_pin_t sysled = get_sysled();

  klogf("Start\n");
  klogf("sintable: %p\n", sintable);
  klogf("sintable[5]: %d\n", sintable[5]);
  klogf("Flashed with OpenOCD!\n");

  systick_add_callback(on_systick, NULL);
  enable_systick(1000);

  /* Enable interrupts. */
  regset(SCB.stcs_r, scb_tickint, 1);

  /* Start the systick. */
  regset(SCB.stcs_r, scb_enable, 1);

#define SIZE 128
  rgb_t rgb[SIZE];
  for (int i = 0; i < SIZE; ++i) {
    rgb[i].g = 0xff;
    rgb[i].r = 0xff;
    rgb[i].b = 0xff;
  }

  uint8_t red = 0x40;
  uint8_t green = 0x40;
  uint8_t blue = 0x40;

  // compiled = ws2812b_compile_rgb(rgb, SIZE);
  // compiled_len = SIZE * 9;

  configure_spi1();

  for (int i = 0; i < 100; ++i) {
    write_rgb(0, 0, 0);
  }

  latch();

  for (;;) {
    set_gpio_pin_high(sysled);

    latch();

    int i;
    for (i = 0; i < SIZE; ++i) {
      red = byte_sin((i * 4 * 2 + time / 1000) & 0xff) / 2;
      green = byte_sin((i * 4 * 3 + time / 1000) & 0xff) / 2;
      blue = 0;
      // blue = amp(byte_sin((i * 2 * 3 + time / 1000) & 0xff)) / 2;

      // uint8_t red = byte_sin((i * 8 + 64 + time / 100) & 0xff);
      // uint8_t green = byte_sin((i * 8 + 0 + time / 100) & 0xff);
      // uint32_t blue = (((red - 128) * (green - 128)) / 256) + 128;

      write_rgb(red, green, blue);
    }

    // write_rgb(0,0,0);
    // write_rgb(0,0,0);
    // write_rgb(0,0,0);
    // write_rgb(0,0,0);
    // write_rgb(0,0,0);
    // write_rgb(0,0,0);
    // write_rgb(0,0,0);
    // write_rgb(0,0,0);
    // write_rgb(red, green, blue);

    set_gpio_pin_low(sysled);

    latch();
  }

  for (;;) {
    spi_write(0);
  }
}

#endif