#include "drv/ws2812B/ws2812b.h"

#include "kern/dma/dma_manager.h"
#include "kern/mem.h"
#include "kern/panic.h"

ws2812b_t* ws2812b_new(spi_select_t spi_select, int* ec)
{
  spi_opts_t spi_opts = DEFAULT_SPI_OPTS;
  spi_opts.endianness = ENDIANNESS_BIG;
  spi_t* spi = reserve_spi(spi_select, &spi_opts, ec);

  if (*ec) {
    return NULL;
  }

  ws2812b_t* drv = kalloc(sizeof(ws2812b_t));
  drv->spi = spi;
  return drv;
}

void ws2812b_delete(ws2812b_t* drv)
{
  release_spi(drv->spi);
  kfree(drv);
}

void ws2812b_latch(ws2812b_t* drv)
{
  for (int i = 0; i < 20; ++i) {
    spi_write_8_sync(drv->spi, 0);
  }
}

#undef BIT
#define BIT(b, n) (!!((b) & (1 << (n))))
static inline void byte_compile(uint8_t byte, uint8_t compl [3])
{
  compl [0] = 0 | 1 << 7 | BIT(byte, 7) << 6 | 0 << 5 | 1 << 4 |
              BIT(byte, 6) << 3 | 0 << 2 | 1 << 1 | BIT(byte, 5) << 0;
  compl [1] = 0 | 0 << 7 | 1 << 6 | BIT(byte, 4) << 5 | 0 << 4 | 1 << 3 |
              BIT(byte, 3) << 2 | 0 << 1 | 1 << 0;
  compl [2] = 0 | BIT(byte, 2) << 7 | 0 << 6 | 1 << 5 | BIT(byte, 1) << 4 |
              0 << 3 | 1 << 2 | BIT(byte, 0) << 1 | 0 << 0;
}

void ws2812b_write_rgb_sync(
    ws2812b_t* drv, uint8_t red, uint8_t green, uint8_t blue)
{
  spi_t* spi = drv->spi;

  uint8_t comp[3];

  byte_compile(green, comp);
  spi_write_8_sync(spi, comp[0]);
  spi_write_8_sync(spi, comp[1]);
  spi_write_8_sync(spi, comp[2]);

  byte_compile(red, comp);
  spi_write_8_sync(spi, comp[0]);
  spi_write_8_sync(spi, comp[1]);
  spi_write_8_sync(spi, comp[2]);

  byte_compile(blue, comp);
  spi_write_8_sync(spi, comp[0]);
  spi_write_8_sync(spi, comp[1]);
  spi_write_8_sync(spi, comp[2]);
}

uint8_t* ws2812b_compile_rgb(rgb_t* out_, size_t arr_len)
{
  uint8_t* out = (uint8_t*)out_;
  uint8_t* spi_out = kalloc(arr_len * 9);

  if (!spi_out) {
    panic("Unable to allocate spi_out\n");
  }

  size_t i;
  size_t j;

  for (i = 0, j = 0; i < arr_len * 3; ++i, j += 3) {
    // stuff
    uint8_t c = out[i];
    spi_out[j] = 0 | (1 << 7) | ((c & (1 << 7)) << 6) | (0 << 5) | (1 << 4) |
                 ((c & (1 << 6)) << 3) | (0 << 2) | (1 << 1) |
                 ((c & (1 << 5)) << 0);

    spi_out[j + 1] = 0 | (0 << 7) | (1 << 6) | ((c & (1 << 4)) << 5) |
                     (0 << 4) | (1 << 3) | ((c & (1 << 3)) << 2) | (0 << 1) |
                     (1 << 0);

    spi_out[j + 2] = 0 | ((c & (1 << 2)) << 7) | (0 << 6) | (1 << 5) |
                     ((c & (1 << 1)) << 4) | (0 << 3) | (1 << 2) |
                     ((c & (1 << 0)) << 1) | (0 << 0);
  }

  return spi_out;
}