#include "rpi_leddriver.h"

#include <stdio.h>
#include <string.h>
#include <unistd.h>

#include "../../rpi_param.h"
#include "../common.h"

#include "../dma/rpi_dma.h"
#include "../dma/rpi_videocore.h"
#include "../gpio/rpi_gpio.h"
#include "../smi/rpi_smi.h"

#if PHYS_REG_BASE == PI_4_REG_BASE // Timings for RPi v4 (1.5 GHz)
#define SMI_TIMING 10, 15, 30, 15  // 400 ns cycle time
#else                              // Timings for RPi v0-3 (1 GHz)
#define SMI_TIMING 10, 10, 20, 10  // 400 ns cycle time
#endif

#define TX_TEST 0      // If non-zero, use dummy Tx data
#define LED_NBITS 24   // Number of data bits per LED
#define LED_PREBITS 4  // Number of zero bits before LED data
#define LED_POSTBITS 4 // Number of zero bits after LED data
#define BIT_NPULSES 3  // Number of O/P pulses per LED bit

// Length of data for 1 row (1 LED on each channel)
#define LED_DLEN (LED_NBITS * BIT_NPULSES)

// Transmit data type, 8 or 16 bits
#if LED_NCHANS > 8
#define TXDATA_T uint16_t
#else
#define TXDATA_T uint8_t
#endif

// Ofset into Tx data buffer, given LED number in chan
#define LED_TX_OSET(n) (LED_PREBITS + (LED_DLEN * (n)))

// Size of data buffers & NV memory, given number of LEDs per chan
#define TX_BUFF_LEN(n) (LED_TX_OSET(n) + LED_POSTBITS)
#define TX_BUFF_SIZE(n) (TX_BUFF_LEN(n) * sizeof(TXDATA_T))
#define VC_MEM_SIZE (PAGE_SIZE + TX_BUFF_SIZE(CHAN_MAXLEDS))

/* Global */
MEM_MAP vc_mem;
TXDATA_T *txdata;
TXDATA_T tx_buffer[TX_BUFF_LEN(CHAN_MAXLEDS)];

void swap_bytes();

void leddriver_setup() {
  videocore_setup(&vc_mem, VC_MEM_SIZE);
  gpio_setup();
  smi_setup(LED_NCHANS, SMI_TIMING, &vc_mem, TX_BUFF_LEN(CHAN_MAXLEDS), &txdata);
}

void leddriver_close() {
  videocore_close(&vc_mem);
  smi_close(LED_NCHANS);
  gpio_close();
}

void set_color(uint32_t rgb, int index) {
  int msk;
  TXDATA_T *txd = &(tx_buffer[LED_TX_OSET(index)]);

  // For each bit of the 24-bit RGB values..
  for (size_t n = 0; n < LED_NBITS; n++) {
    // Mask to convert RGB to GRB, M.S bit first
    msk = n == 0 ? 0x800000 : n == 8 ? 0x8000 : n == 16 ? 0x80 : msk >> 1;
    // 1st byte or word is a high pulse on all lines
    txd[0] = (TXDATA_T)0xffff;
    // 2nd has high or low bits from data
    // 3rd is a low pulse
    txd[1] = txd[2] = 0;
    if (rgb & msk) {
      txd[1] = (TXDATA_T)0xffff;
    }
    txd += BIT_NPULSES;
  }
}

// Set Tx data for 8 or 16 chans, 1 LED per chan, given 1 RGB val per chan
// Logic 1 is 0.8us high, 0.4 us low, logic 0 is 0.4us high, 0.8us low
void rgb_txdata(int *rgbs, int index) {
  int i, n, msk;
  TXDATA_T *txd = &(tx_buffer[LED_TX_OSET(index)]);

  // For each bit of the 24-bit RGB values..
  for (n = 0; n < LED_NBITS; n++) {
    // Mask to convert RGB to GRB, M.S bit first
    msk = n == 0 ? 0x800000 : n == 8 ? 0x8000 : n == 16 ? 0x80 : msk >> 1;
    // 1st byte or word is a high pulse on all lines
    txd[0] = (TXDATA_T)0xffff;
    // 2nd has high or low bits from data
    // 3rd is a low pulse
    txd[1] = txd[2] = 0;
    for (i = 0; i < LED_NCHANS; i++) {
      if (rgbs[i] & msk)
        txd[1] |= (1 << i);
    }
    txd += BIT_NPULSES;
  }
}

void leddriver_refresh() {
#if LED_NCHANS <= 8
  swap_bytes();
#endif

  while (dma_active(DMA_CHAN_A)) {
    usleep(10);
  }

  memcpy(txdata, tx_buffer, TX_BUFF_SIZE(CHAN_MAXLEDS));
  enable_dma(DMA_CHAN_A);
  start_smi(&vc_mem);
  usleep(10);
}

// Swap adjacent bytes in transmit data
void swap_bytes() {
  uint16_t *wp = (uint16_t *)tx_buffer;
  int len = TX_BUFF_SIZE(CHAN_MAXLEDS);

  len = (len + 1) / 2;
  while (len-- > 0) {
    *wp = __builtin_bswap16(*wp);
    wp++;
  }
}

/* source :
 * https://github.com/adafruit/Adafruit_NeoPixel/blob/216ccdbff399750f5b02d4cc804c598399e39713/Adafruit_NeoPixel.cpp#L2414
 */
uint32_t ColorHSV(uint16_t hue, uint8_t sat, uint8_t val) {

  uint8_t r, g, b;

  // Remap 0-65535 to 0-1529. Pure red is CENTERED on the 64K rollover;
  // 0 is not the start of pure red, but the midpoint...a few values above
  // zero and a few below 65536 all yield pure red (similarly, 32768 is the
  // midpoint, not start, of pure cyan). The 8-bit RGB hexcone (256 values
  // each for red, green, blue) really only allows for 1530 distinct hues
  // (not 1536, more on that below), but the full unsigned 16-bit type was
  // chosen for hue so that one's code can easily handle a contiguous color
  // wheel by allowing hue to roll over in either direction.
  hue = (hue * 1530L + 32768) / 65536;
  // Because red is centered on the rollover point (the +32768 above,
  // essentially a fixed-point +0.5), the above actually yields 0 to 1530,
  // where 0 and 1530 would yield the same thing. Rather than apply a
  // costly modulo operator, 1530 is handled as a special case below.

  // So you'd think that the color "hexcone" (the thing that ramps from
  // pure red, to pure yellow, to pure green and so forth back to red,
  // yielding six slices), and with each color component having 256
  // possible values (0-255), might have 1536 possible items (6*256),
  // but in reality there's 1530. This is because the last element in
  // each 256-element slice is equal to the first element of the next
  // slice, and keeping those in there this would create small
  // discontinuities in the color wheel. So the last element of each
  // slice is dropped...we regard only elements 0-254, with item 255
  // being picked up as element 0 of the next slice. Like this:
  // Red to not-quite-pure-yellow is:        255,   0, 0 to 255, 254,   0
  // Pure yellow to not-quite-pure-green is: 255, 255, 0 to   1, 255,   0
  // Pure green to not-quite-pure-cyan is:     0, 255, 0 to   0, 255, 254
  // and so forth. Hence, 1530 distinct hues (0 to 1529), and hence why
  // the constants below are not the multiples of 256 you might expect.

  // Convert hue to R,G,B (nested ifs faster than divide+mod+switch):
  if (hue < 510) { // Red to Green-1
    b = 0;
    if (hue < 255) { //   Red to Yellow-1
      r = 255;
      g = hue;       //     g = 0 to 254
    } else {         //   Yellow to Green-1
      r = 510 - hue; //     r = 255 to 1
      g = 255;
    }
  } else if (hue < 1020) { // Green to Blue-1
    r = 0;
    if (hue < 765) { //   Green to Cyan-1
      g = 255;
      b = hue - 510;  //     b = 0 to 254
    } else {          //   Cyan to Blue-1
      g = 1020 - hue; //     g = 255 to 1
      b = 255;
    }
  } else if (hue < 1530) { // Blue to Red-1
    g = 0;
    if (hue < 1275) { //   Blue to Magenta-1
      r = hue - 1020; //     r = 0 to 254
      b = 255;
    } else { //   Magenta to Red-1
      r = 255;
      b = 1530 - hue; //     b = 255 to 1
    }
  } else { // Last 0.5 Red (quicker than % operator)
    r = 255;
    g = b = 0;
  }

  // Apply saturation and value to R,G,B, pack into 32-bit result:
  uint32_t v1 = 1 + val;  // 1 to 256; allows >>8 instead of /255
  uint16_t s1 = 1 + sat;  // 1 to 256; same reason
  uint8_t s2 = 255 - sat; // 255 to 0
  return ((((((r * s1) >> 8) + s2) * v1) & 0xff00) << 8) |
         (((((g * s1) >> 8) + s2) * v1) & 0xff00) | (((((b * s1) >> 8) + s2) * v1) >> 8);
}
