#include "InovaLedDisplay.h"

#include <cstring>

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"


InovaLedDisplay::InovaLedDisplay(uint8_t r0, uint8_t g0, uint8_t clk, uint8_t cs, uint8_t latchParam, uint8_t oeParam, uint8_t addr0Param, uint8_t addr1Param, uint8_t addr2Param) : Adafruit_GFX(NUM_COL, NUM_ROW){
    latch = (gpio_num_t) latchParam;
    oe = (gpio_num_t) oeParam;
    addr0 = (gpio_num_t) addr0Param;
    addr1 = (gpio_num_t) addr1Param;
    addr2 = (gpio_num_t) addr2Param;

    esp_err_t ret;

    spi_bus_config_t buscfg;
    buscfg.miso_io_num=g0;
    buscfg.mosi_io_num=r0;
    buscfg.sclk_io_num=clk;
    buscfg.quadwp_io_num=-1;
    buscfg.quadhd_io_num=-1;
    buscfg.max_transfer_sz=3*600*2*8;
    buscfg.flags=SPICOMMON_BUSFLAG_DUAL;

    spi_device_interface_config_t devcfg;
    devcfg.clock_speed_hz=1*1000*1000;            //Clock out at 1 MHz
    devcfg.mode=0;                                //SPI mode 0
    devcfg.spics_io_num=cs;                       //CS pin
    devcfg.queue_size=1;                          //We want to be able to queue 7 transactions at a time
    devcfg.flags=SPI_DEVICE_HALFDUPLEX;           //Needed to use both data lines for output

    //Initialize the SPI bus and attach our device handle
    ret=spi_bus_initialize(HSPI_HOST, &buscfg, SPI_DMA_CH_AUTO);
    ESP_ERROR_CHECK(ret);
    ret=spi_bus_add_device(HSPI_HOST, &devcfg, &spi);
    ESP_ERROR_CHECK(ret);

    gpio_set_direction(latch, GPIO_MODE_OUTPUT);
    gpio_set_direction(oe, GPIO_MODE_OUTPUT);
    gpio_set_direction(addr0, GPIO_MODE_OUTPUT);
    gpio_set_direction(addr1, GPIO_MODE_OUTPUT);
    gpio_set_direction(addr2, GPIO_MODE_OUTPUT);
}


void InovaLedDisplay::drawPixel(int16_t x, int16_t y, uint16_t color) {
    displayBuffer[y][x / 8] = displayBuffer[y][x / 8] & ~(0b0000000000000011 << (14 - (2 * (x % 8)))); // clear this column (note the bitwise inversion)
    displayBuffer[y][x / 8] = displayBuffer[y][x / 8] | (color << (14 - (2 * (x % 8)))); // write color to column
}


void InovaLedDisplay::updateDisplay(void) {}


void InovaLedDisplay::runDisplay(void) {
    TickType_t xLastWakeTime;
    const TickType_t xFrequency = 1 / portTICK_PERIOD_MS;
    xLastWakeTime = xTaskGetTickCount();

    uint8_t current_row = 0;
    uint8_t i = 0;
    uint16_t data;
    esp_err_t ret;
    run = true;
    while(run) {
        vTaskDelayUntil(&xLastWakeTime, xFrequency);

        for(i = 0; i < (NUM_COL / 8); i++) {
            spi_transaction_t t;
            memset(&t, 0, sizeof(t));
            t.length = 16;
            data = SPI_SWAP_DATA_TX(displayBuffer[current_row][i], 16);
            t.tx_buffer = &data;
            t.flags = SPI_TRANS_MODE_DIO;  // output on two data lines. Even bits go to GREEN, odd to RED
            ret = spi_device_polling_transmit(spi, &t);
            ESP_ERROR_CHECK(ret);
        }

        gpio_set_level(latch, 0);
        gpio_set_level(oe, 1);

        gpio_set_level(addr0, 1 & current_row);
        gpio_set_level(addr1, 1 & (current_row >> 1));
        gpio_set_level(addr2, 1 & (current_row >> 2));

        gpio_set_level(latch, 1);
        gpio_set_level(oe, 0);

        if(++current_row == NUM_ROW) {
            current_row = 0;
        }
    }
}


void InovaLedDisplay::stopDisplay(void) {
    run = false;
}