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

int 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;

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

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

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

  leds[y*LED_WIDTH+x] = 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 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);
}



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