#include "ws2812b.h"

#include <string.h>

#include "spi.h"
#include "sysled.h"

// either 0b110 when (n) is true, otherwise 0b100
#define BIT_TO_SPI_DATA(v, n) ((BIT_N(v, n)) ? 6 : 4)
#define BIT_N(v, n) ((v) & (1 << (n)))

#define BYTE_1(n) ((n) << 0)
#define BYTE_2(n) ((n) << 8)
#define BYTE_3(n) ((n) << 16)
#define BYTE_4(n) ((n) << 24)

#define COLOR_BYTE_1(c)                                          \
  (BIT_TO_SPI_DATA((c), 7) << 5 | BIT_TO_SPI_DATA((c), 6) << 2 | \
   BIT_TO_SPI_DATA((c), 5) >> 1)

#define COLOR_BYTE_2(c)                                                \
  ((BIT_TO_SPI_DATA((c), 5) & 1) << 7 | BIT_TO_SPI_DATA((c), 4) << 4 | \
   BIT_TO_SPI_DATA((c), 3) << 1 | BIT_TO_SPI_DATA((c), 2) >> 2)

#define COLOR_BYTE_3(c)                                                \
  ((BIT_TO_SPI_DATA((c), 2) & 3) << 6 | BIT_TO_SPI_DATA((c), 1) << 3 | \
   BIT_TO_SPI_DATA((c), 0) << 0)

// Clips a pointer the bottom 14 bits for comparison with the DMA.
#define CLIP_TO_DMA_BITS(ptr) ((uint16_t)(((uint32_t)(ptr)) & 0x3fff))

inline static void complie_color_inline(
    rgb_t color, struct rgb_compiled* out, enum ws2812b_byte_order byte_order)
{
  if (byte_order == BYTE_ORDER_GRB) {
    uint8_t tmp = color.r;
    color.r = color.g;
    color.g = tmp;
  }

  color.r = color.r;
  color.g = color.g;
  color.b = color.b;

  out->first_bits =
      BYTE_1(COLOR_BYTE_1(color.r)) | BYTE_2(COLOR_BYTE_2(color.r)) |
      BYTE_3(COLOR_BYTE_3(color.r)) | BYTE_4(COLOR_BYTE_1(color.g));

  out->second_bits =
      BYTE_1(COLOR_BYTE_2(color.g)) | BYTE_2(COLOR_BYTE_3(color.g)) |
      BYTE_3(COLOR_BYTE_1(color.b)) | BYTE_4(COLOR_BYTE_2(color.b));

  out->last_bits = COLOR_BYTE_3(color.b);
}

void compile_color(
    rgb_t color, struct rgb_compiled* out, enum ws2812b_byte_order byte_order)
{
  complie_color_inline(color, out, byte_order);
}

int write_rgb(struct ws2812b_buf* out, rgb_t color)
{
  struct rgb_compiled comp;
  complie_color_inline(color, &comp, out->byte_order);

  // Make sure there's enough memory.
  if (out->total_alloc < out->cur + TOTAL_BYTES_PER_LED) {
    return 1;
  }

  // Wait for DMA if it's running.
  volatile uint16_t* dma_now = out->dma_now_reg;

  // If the dma_now is not set, we won't wait for the DMA.
  if (dma_now) {
    uint16_t dma_loc = *dma_now;
    uint16_t dma_end = *out->dma_end_reg;
    while ((dma_loc - CLIP_TO_DMA_BITS(out->buf + out->cur) <
            TOTAL_BYTES_PER_LED) &&
           (dma_loc < dma_end)) {
      set_sysled(1); // Little indication to tell if we are waiting for the DMA.
      dma_loc = *dma_now;
    }
    set_sysled(0);
  }

  memcpy(out->buf + out->cur, comp.buf, WIRE_BYTES_PER_COLOR);
  memset(out->buf + out->cur + WIRE_BYTES_PER_COLOR, 0, PADDING_BYTES);

  out->cur += TOTAL_BYTES_PER_LED;

  return 0;
}

void start_dma(struct ws2812b_buf* buf)
{
  buf->dma_end_reg = &SPI0->dma_end;
  buf->dma_now_reg = &SPI0->dma_now;

  dma_transfer(buf->buf, buf->cur);
}

void make_wsb2812b(struct ws2812b_buf* out, void* buf, size_t n_alloc)
{
  memset(out, 0, sizeof(*out));
  out->buf = buf;
  out->total_alloc = n_alloc;
}

void compiled_color_dbg_str(struct rgb_compiled* c, char* out)
{
  uint8_t cur = 0;
  for (int i = 0; i < 9; ++i) {
    cur = c->buf[i];
    for (int j = 0; j < 8; ++j) {
      *(out++) = (cur & 0x80) ? '1' : '0';
      cur <<= 1;
    }
  }
  *(out++) = 0;
}