// A demonstration of the various easing functions available in FastLED

#define USE_SIN_32
#include <FastLED.h>

#define BRIGHTNESS 255
#define SUBSAMPLE 16

#define kMatrixWidth 16
#define kMatrixHeight 16
#define NUM_LEDS ((kMatrixWidth) * (kMatrixHeight))

CRGB leds[NUM_LEDS + 1];

uint16_t XY(const uint8_t x, const uint8_t y) {
  if (x >= kMatrixWidth) return NUM_LEDS;
  if (y >= kMatrixHeight) return NUM_LEDS;
  if (y & 1)
    return (y + 1) * kMatrixWidth - 1 - x;
  else
    return y * kMatrixWidth + x;
}

void setup() {
  FastLED.addLeds<WS2812B, 9, GRB>(leds, NUM_LEDS);
  FastLED.addLeds<WS2812B, 8, GRB>(leds, NUM_LEDS);
  FastLED.addLeds<WS2812B, 7, GRB>(leds, NUM_LEDS);
  FastLED.addLeds<WS2812B, 6, GRB>(leds, NUM_LEDS);
  FastLED.addLeds<WS2812B, 5, GRB>(leds, NUM_LEDS);
  FastLED.addLeds<WS2812B, 4, GRB>(leds, NUM_LEDS);
  FastLED.addLeds<WS2812B, 3, GRB>(leds, NUM_LEDS);
  FastLED.addLeds<WS2812B, 2, GRB>(leds, NUM_LEDS);
  Serial.begin(2000000);
}

void loop() {
  static uint8_t panel = 0;
  static uint16_t phase = 0;
  CRGB col = CRGB(255 / SUBSAMPLE, 255 / SUBSAMPLE, 255 / SUBSAMPLE);

  for (uint16_t x = 0; x < kMatrixWidth * SUBSAMPLE; x++) {
    uint8_t x8 = uint8_t(255 * x / kMatrixWidth / SUBSAMPLE);
    uint16_t x16 = uint16_t(65535 * x / (kMatrixWidth - 1) / SUBSAMPLE);
    // Serial.print(x8);
    // Serial.print("\t");
    // Serial.print(x16);
    // Serial.print("\t");

    x8 += phase >> 4;
    x16 += phase << 4;
    uint16_t y = x16;
    switch (panel) {
      case 0: y = ease8InOutQuad(x8) * 256; break;
      case 1: y = ease8InOutCubic(x8) * 256; break;
      case 2: y = ease8InOutApprox(x8) * 256; break;
      case 3: y = sin8(x8 - 64) * 256; break;
      case 4: y = quadwave8(x8) * 256; break;
      case 5: y = cubicwave8(x8) * 256; break;
      case 6: y = triwave8(x8) * 256; break;
      case 7: y = squarewave8(x8) * 256; break;
    }
    

    // Serial.print(y);
    // Serial.print("\t");
    y = 65535 - y; // invert, so +Y goes up
    y /= 65536 / ((kMatrixHeight - 1) * 256);
    // Serial.println(y);
    wu_pixel(x * 256 / SUBSAMPLE, y, col);
  }

  FastLED[panel].showLeds(BRIGHTNESS);
  memset(leds, 0, NUM_LEDS * 3 + 3);
  panel = (panel + 1) % 8;
  if (panel == 0)
    phase += 33;

}

void wu_pixel(uint16_t x, uint16_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++) {
    uint8_t local_x = (x >> 8) + (i & 1);
    uint8_t local_y = (y >> 8) + ((i >> 1) & 1);
    if (uint16_t xy = XY(local_x, local_y); xy < NUM_LEDS)
      leds[xy] += col % wu[i];
  }
}