#include "functions.h"
#include "prototypes.h"
#include "my_fastled.h"

uint16_t XYsafe(int x, int y) {
  if ( x >= LED_WIDTH) return 0;
  if ( y >= LED_HEIGHT) return 0;
  if ( x < 0) return 0;
  if ( y < 0) return 0;

  // Invert y
  y = LED_HEIGHT - 1 - y;

  if (y & 1) x = LED_WIDTH - 1 - x;

  // Invert x
  //x = LED_WIDTH - 1 - x;

  return y*LED_WIDTH+x;
}

void setPixel(int x, int y, CRGB color) {
  if ( x >= LED_WIDTH) return;
  if ( y >= LED_HEIGHT) return;
  if ( x < 0) return;
  if ( y < 0) return;

  uint16_t index = XYsafe(x, y);

  leds[index] = color;
}

void setPixel(int i, CRGB color) {
  int x = i % LED_WIDTH;
  int y = i / LED_WIDTH;
  setPixel(x, y, color);
}

void setPixel(Window win, int x, int y, CRGB color) {
  if (x >= win.w || y >= win.h || x < 0 || y < 0) return;
  setPixel(win.x + x, win.y + y, color);
}

void addPixelColor(int i, CRGB color) {
    leds[i] += color;
}

void clear(Window window, CRGB color) {
  for ( byte y = 0; y < window.h; y++) {
    for ( byte x = 0; x < window.w; x++) {
      setPixel(window, x, y, color);
    }
  }
}

void clear(Window window) {
	clear(window, CRGB(0));
}

void clear() {
	Window w = {0, 0, LED_WIDTH, LED_HEIGHT};
	clear(w);
}

void blur(fract8 blur_amount) {
	// Based on FastLED code
	for (uint8_t i=0; i<LED_HEIGHT; i++) blur_row(i, blur_amount);
	for (uint8_t i=0; i<LED_WIDTH; i++)  blur_column(i, blur_amount);
}

void blur_row(uint8_t row_index, fract8 blur_amount) {
	CRGB* row = &leds[row_index * LED_WIDTH];
	CRGB carryover = CRGB::Black;
	for (uint8_t x=0; x<LED_WIDTH; x++) {
		CRGB seep = row[x].nscale8(blur_amount);
		row[x] += carryover;
		if (x>0) row[x-1] += seep;
		carryover = seep;
	}
}

void blur_column(uint8_t col_index, fract8 blur_amount) {
	CRGB carryover = CRGB::Black;
	for (uint8_t y=0; y<LED_HEIGHT; y++) {
		uint16_t led_index = XYsafe(col_index, y);
		CRGB seep = leds[led_index].nscale8(blur_amount);
		leds[led_index] += carryover;
		if (y>0) leds[XYsafe(col_index, y-1)] += seep;
		carryover = seep;
	}
}



struct Matrix multiply(struct Matrix m1, struct Matrix m2) {
  Matrix r = {
      .a11 = m1.a11*m2.a11 + m1.a12*m2.a21,
      .a12 = m1.a11*m2.a12 + m1.a12*m2.a22,
      .a21 = m1.a21*m2.a11 + m1.a22*m2.a21,
      .a22 = m1.a21*m2.a12 + m1.a22*m2.a22
    };
  return r;
};

struct Vector multiply(struct Matrix m, struct Vector v) {
  Vector r = {
      .x1 = (m.a11*v.x1) + (m.a12*v.x2),
      .x2 = (m.a21*v.x1) + (m.a22*v.x2)
    };
  return r;
}

struct Vector add(struct Vector v1, struct Vector v2) {
  Vector r = {
    .x1 = v1.x1 + v2.x2,
    .x2 = v1.x2 + v2.x2
  };
  return r;
}