#include <Arduino.h>
#include <stdint.h>
#include "error.h"
#include "IC74HC595.h"
#include "matrix.h"

uint8_t matrixcolumns[8] = {0};

int pokeMatrixPixel(uint8_t x, uint8_t y)
{
    if (x > 7 || y > 7) {
	ERROR(ERRNO_OUTOFBOUNDS);
    }
    // Because of the alignment of the screen in our demo, we treat columns as the X axis
    matrixcolumns[x] = matrixcolumns[x] | (1 << y);
    return ERRNO_SUCCESS;
}

void resetMatrix()
{
    matrixcolumns[0] = 0;
    matrixcolumns[1] = 0;
    matrixcolumns[2] = 0;
    matrixcolumns[3] = 0;
    matrixcolumns[4] = 0;
    matrixcolumns[5] = 0;
    matrixcolumns[6] = 0;
    matrixcolumns[7] = 0;
}

void displayMatrix()
{
    for (int i = 0; i < 8; i++) {
	writeLEDMatrix(matrixcolumns[i], ~(1 << i));
    }
}

void writeLEDMatrix(uint8_t row, uint8_t col)
{
    digitalWrite(matrixcols.updatepin, LOW);
    shiftOut(matrixcols.datapin, matrixcols.clockpin, LSBFIRST, row);
    shiftOut(matrixcols.datapin, matrixcols.clockpin, MSBFIRST, col);
    digitalWrite(matrixcols.updatepin, HIGH);
}

void testMatrix()
{
    uint8_t row = 0;
    uint8_t col = 0;
    for ( int i = 0; i < 8; i++ ) {
	row = (1 << i);
	col = ~(1 << i);
	Serial.printf("%d = (%d, %d)\n", i, row, col);
	writeLEDMatrix(row, col);
    }
};