// https://github.com/FastLED/FastLED/pull/202

#include <FastLED.h>

#define WIDTH 24
#define HEIGHT 24
#define NUM_LEDS ((WIDTH) * (HEIGHT))

CRGB leds[NUM_LEDS + 1];  // an extra pixel to receive out-of-bounds assignments

CRGBPalette16 currentPalette = {
  0xFF0000, 0x7F0000, 0xAB5500, 0x552A00,
  0xABAB00, 0x555500, 0x00FF00, 0x007F00,
  0x00AB55, 0x00552A, 0x0000FF, 0x00007F,
  0x5500AB, 0x2A0055, 0xAB0055, 0x55002A
};


void setup() {
  FastLED.addLeds<NEOPIXEL, 3>(leds, NUM_LEDS);
}


uint16_t XY(uint8_t x, uint8_t y) {
  if (x >= WIDTH || y >= HEIGHT)
    return NUM_LEDS;
  return y * WIDTH + x;
}


void loop()
{
  uint32_t ms = millis();

  // just like the XYmatrix example but with added distortion
  uint32_t yHueDelta = (int32_t)sin16(ms * 11) * 1;
  uint32_t xHueDelta = (int32_t)cos16(ms * 17) * 1;
  uint32_t startHue = ms << 9;
  uint32_t lineStartHue = startHue - (HEIGHT + 1) / 2 * yHueDelta;
  uint32_t yd2 = (int32_t)sin16(ms * 3) / 8;
  uint32_t xd2 = (int32_t)sin16(ms * 7) / 8;
  for (byte y = 0; y < HEIGHT; y++) {
    uint32_t pixelHue = lineStartHue - (WIDTH + 1) / 2 * xHueDelta;
    uint32_t xhd = xHueDelta;
    for (byte x = 0; x < WIDTH; x++) {
      // every 4 seconds, toggle between 4- and 8-bit colour mixing
      if (ms & 4096)
        leds[XY(x, y)] = ColorFromPaletteExtended(currentPalette, pixelHue >> 7, 255, LINEARBLEND);
      else
        leds[XY(x, y)] = ColorFromPalette(currentPalette, pixelHue >> 15, 255, LINEARBLEND);
      pixelHue += xhd;
      xhd += xd2;
    }
    lineStartHue += yHueDelta;
    yHueDelta += yd2;
  }
  FastLED.show();
}


// from: https://github.com/FastLED/FastLED/pull/202
// Everything is commented out because the pull request
// was eventually merged. Thanks Zach!

// 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);
// }