#include <Arduino.h>
#include <FastLED.h>
#include <colorutils.h>

enum XY_matrix_config {
  SERPENTINE = 1,
  ROWMAJOR = 2,
  FLIPMAJOR = 4,
  FLIPMINOR = 8
};

// Define LED matrix parameters
#define LED_PIN         21
#define MATRIX_WIDTH    40
#define MATRIX_HEIGHT   12
#define NUM_LEDS        (MATRIX_WIDTH * MATRIX_HEIGHT)
#define XY_MATRIX (SERPENTINE | ROWMAJOR)
// #define XY_MATRIX (SERPENTINE | ROWMAJOR | FLIPMINOR)
CRGB leds[NUM_LEDS + 1];

void setup() {
  Serial.begin(9600);
  FastLED.addLeds<WS2812, LED_PIN, GRB>(leds, NUM_LEDS);
}

void loop() {


  // FastLED.clear();
  // draw_smooth_line(0 << 1, 0 << 1, 40 << 1, 10 << 1, CRGB::White);
  // FastLED.show();
  // while (1) {}
  for (uint8_t j = 0; j < 10 ; j++) {
        for (uint8_t i = 0; i <= MATRIX_WIDTH ; i += 2) {
        FastLED.clear();
        draw_smooth_line(i << 1, 0 << 1, MATRIX_WIDTH - i << 1, 11 << 1, CRGB::White);
        // delay(50);
        FastLED.show();
      }
    }
  Serial.print("Done!  Seconds taken: ");
  Serial.println(millis() / 1000.0, 2);
  while (1) {};

}

unsigned int integer_sqrt(unsigned int n) {
  unsigned int root = 0;
  unsigned int bit = 1 << 30; // The highest bit in a 32-bit integer
  while (bit > n)
    bit >>= 2; // Shift the bit to the right by 2, essentially dividing it by 4
  while (bit != 0) {
    if (n >= root + bit) {
      n -= root + bit;
      root = (root >> 1) + bit;
    } else {
      root >>= 1;
    }
    bit >>= 2;
  }
  return root;
}

// Function to draw a smoothed line using Xiaolin Wu's algorithm along with Hirst line drawing algorithm

void draw_smooth_line(int x1, int y1, int x2, int y2, CRGB color) {

  int dx = x2 - x1;
  int dy = y2 - y1;
  int sx = (dx > 0) ? 1 : -1;
  int sy = (dy > 0) ? 1 : -1;
  dx *= sx;
  dy *= sy;
  // int xySteps = integer_sqrt((dx * dx) + (dy * dy));
  int xySteps = (dx > dy) ? dx : dy;
  int xStep = ((dx << 16) / xySteps) * sx;
  int yStep = ((dy << 16) / xySteps) * sy;
  int x1_16 = x1 << 16;
  int y1_16 = y1 << 16;

  wu_pixel(x1_16 >> 9, y1_16 >> 9, &color);

  while (xySteps--) {
    x1_16 += xStep;
    y1_16 += yStep;
    wu_pixel(x1_16 >> 9, y1_16 >> 9, &color);
  }

}

// Xiaolin Wu's line algorithm for drawing a smoothed line
void wu_pixel(uint32_t x, uint32_t y, CRGB *col) {
  uint8_t xx = x & 0xff, yy = y & 0xff, ix = 255 - xx, iy = 255 - yy;
  uint8_t wu[4] = {((ix * iy) + ix + iy) >> 8, ((xx * iy) + xx) >> 8,
                   ((ix * yy) + ix + yy) >> 8, ((xx * yy) + xx) >> 8
                  };
  for (uint8_t i = 0; i < 4; i++) {
    uint16_t xy = XY((x >> 8) + (i & 1), (y >> 8) + ((i >> 1) & 1));
    CRGB *led = &leds[xy];
    led->r = qadd8(led->r, col->r * wu[i] >> 8);
    led->g = qadd8(led->g, col->g * wu[i] >> 8);
    led->b = qadd8(led->b, col->b * wu[i] >> 8);
  }
}

uint16_t XY(uint8_t x, uint8_t y) {
  uint8_t major, minor, sz_major, sz_minor;
  if (x >= MATRIX_WIDTH || y >= MATRIX_HEIGHT)
    return NUM_LEDS;
  if (XY_MATRIX & ROWMAJOR)
    major = x, minor = y, sz_major = MATRIX_WIDTH,  sz_minor = MATRIX_HEIGHT;
  else
    major = y, minor = x, sz_major = MATRIX_HEIGHT, sz_minor = MATRIX_WIDTH;
  if ((XY_MATRIX & FLIPMAJOR) ^ (minor & 1 && (XY_MATRIX & SERPENTINE)))
    major = sz_major - 1 - major;
  if (XY_MATRIX & FLIPMINOR)
    minor = sz_minor - 1 - minor;
  return (uint16_t) minor * sz_major + major;
}