#include <FastLED.h>

#define WIDTH 16
#define HEIGHT 16
#define NUM_LEDS ((WIDTH) * (HEIGHT))
#define POINTS 3

CRGB leds[NUM_LEDS + 1];


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


// x & y are Q7.8 fixed-point because there was meant to be a
// line renderer working with sub-pixels endpoints.  That didn't happen.
// dx & dy are Q.7 deltas for x & y.
struct Point {
  int16_t x, y;
  int8_t dx, dy;
};
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;
  }
  Serial.begin(115200);
}


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


void loop()
{
  static int frame; frame++;
  uint32_t ms = millis();
  FastLED.clear();

  // 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);
  plot_colour = 0xffffff;

  wuLineAA( 0, 0, (WIDTH << 8) -1, frame % (HEIGHT << 8), &plot_colour);
  FastLED.show();
  // delay(1000);
}


// blend between two colours by amount/255ths
// updates the first colour in-place
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 wuLineAA(int16_t ox1, int16_t oy1, int16_t ox2, int16_t oy2, CRGB * col) {
  int16_t grad, xd, yd, xm, ym;
  int16_t x1, y1, x2, y2;
  int16_t xgap, ygap, xend, yend, xf, yf;
  int16_t s1, s2;

  int16_t x, y, ix1, ix2, iy1, iy2;

  uint8_t c1, c2;

  xd = (ox2 - ox1);
  yd = (oy2 - oy1);

  // check line gradient
  if (abs(xd) > abs(yd))
  {
    // horizontal(ish) lines

    // if line is back to front
    if (ox2 < ox1)
    {
      // then swap
      x1 = ox2; y1 = oy2;
      x2 = ox1; y2 = oy1;
      xd = (x2 - x1);
      yd = (y2 - y1);
    }
    else
    {
      // otherwise don't swap
      x1 = ox1; y1 = oy1;
      x2 = ox2; y2 = oy2;
      xd = (x2 - x1);
      yd = (y2 - y1);
    }

    // Treat very short lines as unit length, horizontal.
    if (abs(xd) < 25)
    {
      x2 = x1 + 128;
      x1 -= 128;
      grad = 0;
    }
    else
    {
      grad = (((int32_t)yd << 8) / xd);
      Serial.println(grad);
      // if line length is less than 1, lengthen it to 1
      if (xd < 256)
      {
        // find mid point of line
        xm = (x1 + x2) >> 1;
        ym = (y1 + y2) >> 1;

        //recalculate end points so that xd=1
        x1 = xm - 128;
        x2 = xm + 128;
        y1 = ym - (grad >> 1);
        y2 = ym + (grad >> 1);

        xd = 256;
        yd = grad;

      }
    }
    // End Point 1
    // -----------

    // project to find coordinates of end point 1
    xend = (x1 + 128) & 0xff00;
    yend = y1 + (grad * (xend - x1)) >> 8;


    // distance from begining of line to next pixel boundary
    xgap = 255 - ((x1 + 128) >> 8);


    // calc pixel coordinates for the top of the pixel pair
    ix1  = xend >> 8;
    iy1  = yend >> 8;
    // calc pixel intensities
    s1   = ((255 - (yend & 0xff)) * xgap) >> 8;
    s2   = ((yend & 0xff) * xgap) >> 8;

    // plot pixel pair
    crossfade(&leds[XY(ix1, iy1)], col, s2);
    crossfade(&leds[XY(ix1, iy1 + 1)], col, s1);


    ix1++;
    yf = yend + grad;

    // End Point 2
    // -----------

    // project to find coordinates of end point 2
    xend = (x2 + 128) & 0xff00;
    yend = y2 + grad * (xend - x2);

    // distance from end of line to previous pixel boundary
    xgap = (x2 - 128) & 0xff;

    // calc pixel coordinates for the top of the pixel pair
    ix2 = xend >> 8;
    iy2 = yend >> 8;

    // calc pixel intensities
    s1   = ((255 - (yend & 0xff)) * xgap) >> 8;
    s2   = ((yend & 0xff) * xgap) >> 8;

    // plot pixel pair
    crossfade(&leds[XY(ix2, iy2)], col, s2);
    crossfade(&leds[XY(ix2, iy2 + 1)], col, s1);


    // MAIN LOOP
    for (x = ix1; x < ix2; x++)
    {
      c2 = yf & 0xff;
      c1 = 255 - c2;

      crossfade(&leds[XY(x, yf >> 8)], col, c2);
      crossfade(&leds[XY(x, (yf >> 8) + 1)], col, c1);

      yf += grad;
    }

  }
  else
  {
  //   // vertical(ish) lines
  //   // if line is back to front
  //   if (oy2 < oy1)
  //   {
  //     // then swap
  //     x1 = ox2; y1 = oy2;
  //     x2 = ox1; y2 = oy1;
  //     xd = (x2 - x1);
  //     yd = (y2 - y1);
  //   }
  //   else
  //   {
  //     // otherwise don't swap
  //     x1 = ox1; y1 = oy1;
  //     x2 = ox2; y2 = oy2;
  //     xd = (x2 - x1);
  //     yd = (y2 - y1);
  //   }

  //   // Treat very short lines as unit length, horizontal.
  //   if (abs(yd) < (fixpt).1)
  //   {
  //     y2 = y1 + (fixpt).5;
  //     y1 -= (fixpt).5;
  //     grad = 0;
  //   }
  //   else
  //   {
  //     grad = xd / yd;

  //     // if line length is less than 1, lengthen it to 1
  //     if (yd < (fixpt)1)
  //     {
  //       // find mid point of line
  //       xm = (x1 + x2) >> 1;
  //       ym = (y1 + y2) >> 1;

  //       // recalculate end points so that xd=1
  //       x1 = xm - (grad >> 1);
  //       x2 = xm + (grad >> 1);
  //       y1 = ym - (fixpt).5;
  //       y2 = ym + (fixpt).5;

  //       yd = (fixpt)1;
  //       xd = grad;
  //     }
  //   }

  //   // End Point 1
  //   // -----------

  //   // project to find coordinates of end point 1
  //   yend = trunc(y1 + (fixpt).5);
  //   xend = x1 + grad * (yend - y1);

  //   // distance from begining of line to next pixel boundary
  //   ygap = invfracof(y1 + (fixpt).5);


  //   // calc pixel coordinates for the top of the pixel pair
  //   ix1  = int(xend);
  //   iy1  = int(yend);
  //   // calc pixel intensities
  //   s1   = invfracof(xend);
  //   s2   =    fracof(xend);

  //   s1 *= ygap;
  //   s2 *= ygap;

  //   // plot pixel pair
  //   c1   = char((s1.x * Bright) >> 16);
  //   c2   = char((s2.x * Bright) >> 16);
  //   plot640(ix1, iy1, c1);
  //   plot640(ix1 + 1, iy1, c2);


  //   iy1++;
  //   xf = xend + grad;

  //   // End Point 2
  //   // -----------

  //   // project to find coordinates of end point 2
  //   yend = trunc(y2 + (fixpt).5);
  //   xend = x2 + grad * (yend - y2);

  //   // distance from end of line to previous pixel boundary
  //   ygap = fracof(y2 - (fixpt).5);

  //   // calc pixel coordinates for the top of the pixel pair
  //   ix2 = int(xend);
  //   iy2 = int(yend);

  //   // calc pixel intensities
  //   s1   = invfracof(xend);
  //   s2   =    fracof(xend);
  //   s1 *= ygap;
  //   s2 *= ygap;

  //   // plot pixel pair
  //   c1   = char((s1.x * Bright) >> 16);
  //   c2   = char((s2.x * Bright) >> 16);
  //   plot640(ix2, iy2, c1);
  //   plot640(ix2 + 1, iy2, c2);


  //   // MAIN LOOP
  //   for (y = iy1; y < iy2; y++)
  //   {
  //     c2 =  (unsigned char)(((xf.x & 0xffff) * Bright) >> 16);
  //     c1 = ((unsigned char)Bright - c2);

  //     plot640(int(xf), y, c1);
  //     plot640(int(xf) + 1, y, c2);
  //     xf += grad;
  //   }
  }
}

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


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