#include <FastLED.h>

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

CRGB leds[NUM_LEDS + 1];
CRGBPalette16 currentPalette = {
  0xFF0000, 0x7F0000, 0xAB5500, 0x552A00, 0xABAB00, 0x555500, 0x00FF00, 0x007F00,
  0x00AB55, 0x00552A, 0x0000FF, 0x00007F, 0x5500AB, 0x2A0055, 0xAB0055, 0x55002A
};

struct Point {
  int16_t x, y;
  int8_t dx, dy;
};
#define POINTS 8
Point points[POINTS];

void setup() {
  FastLED.addLeds<NEOPIXEL, 3>(leds, NUM_LEDS);
  for (uint8_t i = 0; i < POINTS; i++) {
    points[i].x = random16() % (WIDTH << 8);
    points[i].y = random16() % (HEIGHT << 8);
    points[i].dx = (random8() - 128) / 2;
    points[i].dy = (random8() - 128) / 2;
  }
}

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


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

  // draw the background animation, just like the XYmatrix example
  // but with added distortion
  uint32_t yHueDelta = (int32_t)sin16(ms * 11) * 3;
  uint32_t xHueDelta = (int32_t)cos16(ms * 11) * 3;
  uint32_t startHue = ms << 8;
  uint32_t lineStartHue = startHue - (HEIGHT + 1) / 2 * yHueDelta;
  int16_t yd2 = sin16(ms * 3) / 4;
  int16_t xd2 = sin16(ms * 7) / 4;
  for (byte y = 0; y < HEIGHT; y++) {
    uint32_t pixelHue = lineStartHue - (WIDTH + 1) / 2 * xHueDelta;
    uint32_t xhd = xHueDelta;
    lineStartHue += yHueDelta;
    yHueDelta += yd2;
    for (byte x = 0; x < WIDTH; x++) {
      leds[XY(x, y)] = ColorFromPaletteExtended(currentPalette, pixelHue >> 7, 255, LINEARBLEND);
      pixelHue += xhd;
      xhd += xd2;
    }
  }

  // move the points, and bounce off the edges
  for (uint8_t i = 0; i < POINTS; i++) {
    points[i].x += points[i].dx;
    points[i].y += points[i].dy;
    if (points[i].x <= 0) {
      points[i].dx = -points[i].dx;
      points[i].x = -points[i].x;
    } else if (points[i].x >= WIDTH << 8) {
      points[i].dx = -points[i].dx;
      points[i].x = (WIDTH << 9) - points[i].x;
    }
    if (points[i].y <= 0) {
      points[i].dy = -points[i].dy;
      points[i].y = -points[i].y;
    } else if (points[i].y >= HEIGHT << 8) {
      points[i].dy = -points[i].dy;
      points[i].y = (HEIGHT << 9) - points[i].y;
    }
  }

   // change `effect` every second repeating: 0,1,2,3,4,5,0,1,2... 
  static uint8_t effect = 0;
  EVERY_N_MILLIS(1000) {
    if (++effect > 5) effect = 0;
  }


  // make a colour
  // bigger multipliers change the colour faster
  // use prime numbers as the multipliers for the longest loop-time
  CRGB plot_colour = CRGB(128 + sin16(ms * 13) / 256, 128 + sin16(ms * 17) / 256, 128 + sin16(ms * 29) / 256);

  // draw lines between each point
  for (uint8_t i = 0; i < POINTS; i++) {
    // leds[XY(points[i].x >> 8, points[i].y >> 8)] = plot_colour;
    // wu_pixel(points[i].x, points[i].y, &plot_colour);
    switch (effect) {
      case 0:
        plotLine(points[i].x >> 8, points[i].y >> 8, points[(i + 1) % POINTS].x >> 8, points[(i + 1) % POINTS].y >> 8, &plot_colour);
        break;
      case 1:
      default:
        plotLineAA(points[i].x >> 8, points[i].y >> 8, points[(i + 1) % POINTS].x >> 8, points[(i + 1) % POINTS].y >> 8, &plot_colour);
        break;
        // plotLineWuFPAA(points[i].x, points[i].y, points[(i + 1) % POINTS].x, points[(i + 1) % POINTS].y, &plot_colour);
        // break;
    }
  }
  FastLED.show();
}

void plotLineWuAA() {
  
}

// http://members.chello.at/~easyfilter/Bresenham.pdf § 1.7
void plotLine(uint8_t x0, uint8_t y0, uint8_t x1, uint8_t y1, CRGB * col) {
  uint8_t dx = abs(x1 - x0);
  int8_t sx = x0 < x1 ? 1 : -1;
  int8_t dy = -abs(y1 - y0);
  int8_t sy = y0 < y1 ? 1 : -1;
  int16_t err = dx + dy; /* error value e_xy */
  while (true) {   /* loop */
    leds[XY(x0, y0)] = *col;
    if (x0 == x1 && y0 == y1) break;
    int16_t e2 = 2 * err;
    if (e2 >= dy) { /* e_xy+e_x > 0 */
      err += dy;
      x0 += sx;
    }
    if (e2 <= dx) { /* e_xy+e_y < 0 */
      err += dx;
      y0 += sy;
    }
  }
}

// http://members.chello.at/~easyfilter/Bresenham.pdf § 7.1
void plotLineAA(uint8_t x0, uint8_t y0, uint8_t x1, uint8_t y1, CRGB * col) {
  int16_t dx = abs(x1 - x0), dy = abs(y1 - y0);
  int8_t sx = x0 < x1 ? 1 : -1, sy = y0 < y1 ? 1 : -1;
  int16_t x2, e2, err = dx - dy;  /* error value e_xy */
  int16_t ed = dx + dy == 0 ? 1 : sqrt16(dx * dx + dy * dy);
  for ( ; ; ) {
    int8_t this_err = 255 * abs(err - dx + dy) / ed;
    crossfade(&leds[XY(x0, y0)], col, this_err);
    e2 = err;
    x2 = x0;
    if (2 * e2 >= -dx) {
      /* x step */
      if (x0 == x1) break;
      if (e2 + dy < ed) {
        int8_t this_err = 255 * (e2 + dy) / ed;
        crossfade(&leds[XY(x0, y0 + sy)], col, this_err);
      }
      err -= dy;
      x0 += sx;
    }
    if (2 * e2 <= dy) {
      /* y step */
      if (y0 == y1) break;
      if (dx - e2 < ed) {
        int8_t this_err = 255 * (dx - e2) / ed;
        crossfade(&leds[XY(x2 + sx, y0)], col, this_err);
      }
      err += dx;
      y0 += sy;
    }
  }
}


// void plotLineFPAA(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1, CRGB * col) {
//   int16_t dx = abs(x1 - x0), dy = abs(y1 - y0);
//   int8_t sx = x0 < x1 ? 1 : -1, sy = y0 < y1 ? 1 : -1;
//   int16_t x2, e2, err = dx - dy;  /* error value e_xy */
//   int16_t ed = dx + dy == 0 ? 1 : sqrt16(dx * dx + dy * dy);
//   for ( ; ; ) {
//     int8_t this_err = 255 * abs(err - dx + dy) / ed;
//     crossfade(&leds[XY(x0, y0)], col, this_err);
//     e2 = err;
//     x2 = x0;
//     if (2 * e2 >= -dx) {
//       /* x step */
//       if (x0 >= x1) break;
//       if (e2 + dy < ed) {
//         int8_t this_err = 255 * (e2 + dy) / ed;
//         crossfade(&leds[XY(x0, y0 + sy)], col, this_err);
//       }
//       err -= dy;
//       x0 += sx;
//     }
//     if (2 * e2 <= dy) {
//       /* y step */
//       if (y0 >= y1) break;
//       if (dx - e2 < ed) {
//         int8_t this_err = 255 * (dx - e2) / ed;
//         crossfade(&leds[XY(x2 + sx, y0)], col, this_err);
//       }
//       err += dx;
//       y0 += sy;
//     }
//   }
// }


void crossfade(CRGB *a, const CRGB *b, uint8_t amount) {
  uint8_t rev = 255 - amount;
  a->red = (a->red * amount + b->red * rev) >> 8;
  a->green = (a->green * amount + b->green * rev) >> 8;
  a->blue = (a->blue * amount + b->blue * rev) >> 8;
}


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)
                  };
  // multiply the intensities by the colour, and saturating-add them to the pixels
  for (uint8_t i = 0; i < 4; i++) {
    uint16_t xy = XY((x >> 8) + (i & 1), (y >> 8) + ((i >> 1) & 1));
    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);
  }
}

// // https://www.researchgate.net/publication/220252560_Fixed-point_digital_differential_analyser_with_antialiasing_FDDAA
// void FPDDAAs(uint16_t X0, uint16_t Y0, uint16_t Xf, uint16_t Yf, uint8_t Gray) {
//   int16_t Ax = Xf - X0; //Line width
//   int16_t m, Ay, MaxColor, South, North, Xp, YS, YC, YN, Center,
//   ColorInc, //Color Increment during Initialization phase
//   TBR; //Total Brush Radiance

//   Ay = Yf - Y0; //Line height
//   m = ((int32_t) Ay << 8) / Ax;
//   MaxColor = 1 << 8;
//   ColorInc = ((int32_t) m * Gray) >> 8;
//   Center = m >> 1; //Up = Gray*(m*m) / 2.0 in FPQ15 format
//   Center = ((int32_t)Center * Center) >> 8; //It is assumed that the slope is always positive
//   Center = (Center << 1) + 32768; //Rounding
//   Center = ((int32_t) Center * Gray) >> 8;
//   South = MaxColor; North = 0;
//   TBR = MaxColor + Center; //South + Center + North
//   Xp = X0; YS = Y0; YC = YS + (1 << 8); YN = YC + (1 << 8);

//   // DrawBrushSC ();
//   do{
//     Xp+=1 << 8; South -= ColorInc;
//     if(South <= 0) {
//       South += MaxColor;
//       YS= YC; YC = YN; YN+=1 << 8;
//     } //Brush up one pixel
//     Center = TBR - South; North = Center - MaxColor;
//     if(North <= 0) {
//       // DrawBrushSC ();
//     } else {
//       Center = MaxColor;
//       // DrawBrushSCN ();
//     }
//   } while (Xp < Xf);
// }


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