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