#include <FastLED.h>

#define LED_PIN  3
#define COLOR_ORDER       GRB
#define CHIPSET           WS2812B
#define kMatrixWidth      16
#define kMatrixHeight     16
#define kMatrixSerpentine true
#define kMatrixRowMajor   true
#define kMatrixFlipMajor  true
#define kMatrixFlipMinor  false
#define NUM_LEDS          ((kMatrixWidth) * (kMatrixHeight))

CRGB leds_plus_safety_pixel[ NUM_LEDS + 1];
CRGB* const leds( leds_plus_safety_pixel + 1);


void setup() {
  Serial.begin(115200);
  FastLED.addLeds<CHIPSET, LED_PIN, COLOR_ORDER>(leds, NUM_LEDS).setCorrection(UncorrectedColor);
}


void loop() {
  drawFrame();
  FastLED.show();
  FastLED.clear();
}


int16_t XY(int8_t x, int8_t y) {
  uint8_t major, minor, sz_major, sz_minor;
  if (x >= kMatrixWidth || y >= kMatrixHeight || x < 0 || y < 0) return -1;
  if (kMatrixRowMajor)
    major = x, minor = y, sz_major = kMatrixWidth,  sz_minor = kMatrixHeight;
  else
    major = y, minor = x, sz_major = kMatrixHeight, sz_minor = kMatrixWidth;
  if (kMatrixFlipMajor ^ (minor & 1 && kMatrixSerpentine))
    major = sz_major - 1 - major;
  if (kMatrixFlipMinor)
    minor = sz_minor - 1 - minor;
  return (uint16_t) minor * sz_major + major;
}


void drawFrame() {
  float ms                = millis() / 500.f;
  const float kHalfWidth  = kMatrixWidth / 2;
  const float kHalfHeight = kMatrixHeight / 2;

  const float density = 1.0875f + 0.0675f * sinf(ms * 0.29f);
  for (float y = 0; y <= kHalfHeight + 2; y += density)
    for (float x = 0; x <= kHalfWidth + 2; x += density) {
      float hyp = hypotf(x, y) * -3.f;
      float fx  = sinf(ms + hyp / 8.f) * 224.f;
      float fy  = cosf(ms * .78f + hyp / 8.f) * 224.f;
      CRGB col = ColorFromPaletteExtended(RainbowColors_p, (ms * 24.f + hyp * 4.f) * 256.f, 255, LINEARBLEND);
      wu_pixel(256 * (x + kHalfWidth) + fx, 256 * (y + kHalfHeight) + fy, &col);
      if (x) wu_pixel(256 * (-x + kHalfWidth) + fx, 256 * (y + kHalfHeight) + fy, &col);
      if (y) wu_pixel(256 * (x + kHalfWidth) + fx, 256 * (-y + kHalfHeight) + fy, &col);
      if (x && y) wu_pixel(256 * (-x + kHalfWidth) + fx, 256 * (-y + kHalfHeight) + fy, &col);
    }
}


void wu_pixel(int16_t x, int16_t y, CRGB* col) {
  // extract the fractional parts and derive their inverses
  uint8_t xx = x & 0xff, yy = y & 0xff, ix = 255 - xx, iy = 255 - yy;
  // calculate the intensities for each affected pixel
  #define WU_WEIGHT(a, b) ((uint8_t)(((a) * (b) + (a) + (b)) >> 8))
  uint8_t wu[4] = {WU_WEIGHT(ix, iy), WU_WEIGHT(xx, iy),
                   WU_WEIGHT(ix, yy), WU_WEIGHT(xx, yy)
                  };
#undef WU_WEIGHT
  // multiply the intensities by the colour, and saturating-add them to the pixels
  for (uint8_t i = 0; i < 4; i++) {
    int16_t local_x = (x >> 8) + (i & 1);
    int16_t local_y = (y >> 8) + ((i >> 1) & 1);
    int16_t xy      = XY(local_x, local_y);
    if (xy < 0) continue;
    leds[xy].r = qadd8(leds[xy].r, col->r * wu[i] >> 8);
    leds[xy].g = qadd8(leds[xy].g, col->g * wu[i] >> 8);
    leds[xy].b = qadd8(leds[xy].b, col->b * wu[i] >> 8);
  }
}


// from: https://github.com/FastLED/FastLED/pull/202
CRGB ColorFromPaletteExtended(const CRGBPalette16& pal, uint16_t index, uint8_t brightness, TBlendType blendType) {
  // Extract the four most significant bits of the index as a palette index.
  uint8_t index_4bit = (index >> 12);
  // Calculate the 8-bit offset from the palette index.
  uint8_t offset = (uint8_t)(index >> 4);
  // Get the palette entry from the 4-bit index
  const CRGB* entry = &(pal[0]) + index_4bit;
  uint8_t red1      = entry->red;
  uint8_t green1    = entry->green;
  uint8_t blue1     = entry->blue;

  uint8_t blend = offset && (blendType != NOBLEND);
  if (blend) {
    if (index_4bit == 15) {
      entry = &(pal[0]);
    } else {
      entry++;
    }

    // Calculate the scaling factor and scaled values for the lower palette value.
    uint8_t f1 = 255 - offset;
    red1       = scale8_LEAVING_R1_DIRTY(red1, f1);
    green1     = scale8_LEAVING_R1_DIRTY(green1, f1);
    blue1      = scale8_LEAVING_R1_DIRTY(blue1, f1);

    // Calculate the scaled values for the neighbouring palette value.
    uint8_t red2   = entry->red;
    uint8_t green2 = entry->green;
    uint8_t blue2  = entry->blue;
    red2           = scale8_LEAVING_R1_DIRTY(red2, offset);
    green2         = scale8_LEAVING_R1_DIRTY(green2, offset);
    blue2          = scale8_LEAVING_R1_DIRTY(blue2, offset);
    cleanup_R1();

    // These sums can't overflow, so no qadd8 needed.
    red1 += red2;
    green1 += green2;
    blue1 += blue2;
  }
  if (brightness != 255) {
    // nscale8x3_video(red1, green1, blue1, brightness);
    nscale8x3(red1, green1, blue1, brightness);
  }
  return CRGB(red1, green1, blue1);
}