#include <FastGPIO.h>
//#include <util/delay.h>
//#include <PololuLedStrip.h>
#define APA102_USE_FAST_GPIO
#include <APA102.h>

uint32_t rgb_to_abgr(rgb_color color) {
  uint32_t val = 0xFF000000U;
  val |= ((unsigned long)color.blue  << 16u) & 0xFF0000U;
  val |= ((unsigned long)color.green <<  8u) &   0xFF00U;
  val |= ((unsigned long)color.red         ) &     0xFFU;
  return val;
}

typedef struct {
  uint8_t brightness;
  uint8_t red;
  uint8_t green;
  uint8_t blue;
} argb_t;

template <class Derived>
class LEDWriterBase {
  public:
    unsigned frameId;
    LEDWriterBase() : frameId(0) {};

    template <uint8_t DPIN, uint8_t CPIN>
    void loop(APA102<DPIN,CPIN>& ledStrip, unsigned ledCount, unsigned long usFrame) {
      Derived& derived = *static_cast<Derived*>(this);
      unsigned long usStart = micros();
      ledStrip.startFrame();
      for (unsigned led = 0; led < ledCount; led++) {
        argb_t pixel = derived.getPixel(led, frameId, ledCount);
        ledStrip.sendColor(pixel.red, pixel.green, pixel.blue, pixel.brightness);
      }
      ledStrip.endFrame(ledCount);
      frameId++;
      derived.prepareNextFrame(frameId, ledCount);
      Serial.println(micros() - usStart);
      unsigned long usDiff = micros() - usStart;
      if (usDiff < usFrame) {
        usDiff = usFrame - usDiff;
        if (usDiff > 1000) {
          unsigned long msDiff = usDiff / 1000;
          usDiff = usDiff % 1000;
          delay(msDiff);
        }
        delayMicroseconds(usFrame-usDiff);
      }
    }

    void prepareNextFrame(unsigned frameId, unsigned ledCount) {}
    argb_t getPixel(unsigned ledId, unsigned frameId, unsigned ledCount) {
      return 0;
    }
};

class LED_Rainbow : public LEDWriterBase<LED_Rainbow> {
  public:
    argb_t getPixel(unsigned led, unsigned frame, unsigned ledCount) {
      argb_t res = {.brightness = 31};
      unsigned steps = 3;
      //auto color = hueToRgb(frame+led*steps*frame/10);
      //auto color = hsvToRgb(frame+frame*frame/4+led*steps, 255, 255);
      int s = led-ledCount/2;
      if (s >= 0) s++;
      auto color = hueToRgb(frame*s);
      res.red = color.red;
      res.green = color.green;
      res.blue = color.blue;
      return res;
    }
    // Converts a color from HSV to RGB.
    // h is hue, as a number between 0 and 360.
    // s is the saturation, as a number between 0 and 255.
    // v is the value, as a number between 0 and 255.
    static rgb_color hueToRgb(uint16_t h) {
      uint8_t s = 255;
      uint8_t v = 255;
      uint8_t f = (h % 60) * 255 / 60;
      uint8_t p = 0;
      uint8_t q = 255 - f;
      uint8_t t = f;
      uint8_t r = 0, g = 0, b = 0;
      switch((h / 60) % 6){
          case 0: r = 255; g = f;   b = 0; break;
          case 1: r = q;   g = 255; b = 0; break;
          case 2: r = 0;   g = 255; b = f; break;
          case 3: r = 0;   g = q;   b = 255; break;
          case 4: r = f;   g = 0;   b = 255; break;
          case 5: r = 255; g = 0;   b = q; break;
      }
      return rgb_color(r, g, b);
    }
    static rgb_color hsvToRgb(uint16_t h, uint8_t s, uint8_t v) {
      uint8_t f = (h % 60) * 255 / 60;
      uint8_t p = (255 - s) * (uint16_t)v / 255;
      uint8_t q = (255 - f * (uint16_t)s / 255) * (uint16_t)v / 255;
      uint8_t t = (255 - (255 - f) * (uint16_t)s / 255) * (uint16_t)v / 255;
      uint8_t r = 0, g = 0, b = 0;
      switch((h / 60) % 6){
          case 0: r = v; g = t; b = p; break;
          case 1: r = q; g = v; b = p; break;
          case 2: r = p; g = v; b = t; break;
          case 3: r = p; g = q; b = v; break;
          case 4: r = t; g = p; b = v; break;
          case 5: r = v; g = p; b = q; break;
      }
      return rgb_color(r, g, b);
    }
};

const unsigned vidFrames = 50;
const unsigned vidWidth  = 6;
argb_t ledVideo[vidFrames][vidWidth];

class LED_Video : public LEDWriterBase<LED_Video> {
  private:
    argb_t* ledVideo;
    unsigned vidFrames;
    unsigned vidWidth;
    unsigned frameMod;
  public:
    LED_Video() : ledVideo(NULL), vidFrames(0), vidWidth(0), frameMod(0) {}
    LED_Video(argb_t* ledVideo, unsigned vidFrames, unsigned vidWidth)
      : ledVideo(ledVideo), vidFrames(vidFrames), vidWidth(vidWidth), frameMod(0)
    {}
    void init(argb_t* ledVideo, unsigned vidFrames, unsigned vidWidth) {
      this->ledVideo = ledVideo;
      this->vidFrames = vidFrames;
      this->vidWidth = vidWidth;
      this->frameMod = 0;
    }
    argb_t getPixel(unsigned led, unsigned frame, unsigned ledCount) {
      return ledVideo[frameMod + led];
    }
    void prepareNextFrame(unsigned frameId, unsigned ledCount) {
      frameMod = (frameId % vidFrames) * vidWidth;
      // load frame line from sd card...
    }
};

const uint8_t dataPin = 11;
const uint8_t clockPin = 12;
const unsigned ledCount = 6;
APA102<dataPin, clockPin> ledStrip;

LED_Rainbow rainbow;
LED_Video video;

void setup() {
  FastGPIO::Pin<IO_D3>::setOutputHigh();
  // Start the UART
  Serial.begin(9600);
  video.init(ledVideo[0], vidFrames, vidWidth);

  for (int frame = 0; frame < vidFrames; frame++) {
    for (int led = 0; led < vidWidth; led++) {
      argb_t pixel;
      rgb_color color = LED_Rainbow::hueToRgb(frame+led*30);
      pixel.brightness = 31;
      pixel.red = color.red;
      pixel.blue = color.blue;
      pixel.green = color.green;
      ledVideo[frame][led] = pixel;
    }
  }
}

void loop() {
  //rainbow.loop(ledStrip, ledCount, 1000000/10);
  video.loop(ledStrip, ledCount, 1000000/100);
}