#include "InovaLedDisplay.h"

#include <cstring>

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


InovaLedDisplay::InovaLedDisplay(uint8_t r0, uint8_t g0, uint8_t r1, uint8_t g1, uint8_t r2, uint8_t g2, uint8_t clk, 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;
    memset(&buscfg, 0, sizeof(spi_bus_config_t));
    buscfg.miso_io_num=g0;
    buscfg.mosi_io_num=r0;
    buscfg.quadwp_io_num=r1;
    buscfg.quadhd_io_num=g1;
    buscfg.data4_io_num=r2;
    buscfg.data5_io_num=g2;
    buscfg.data6_io_num=37;
    buscfg.data7_io_num=38;
    buscfg.sclk_io_num=clk;
    buscfg.max_transfer_sz=3*600*2*8;
    buscfg.flags=SPICOMMON_BUSFLAG_OCTAL;

    spi_device_interface_config_t devcfg;
    memset(&devcfg, 0, sizeof(spi_device_interface_config_t));
    devcfg.clock_speed_hz=1*1000*1000;            //Clock out at 1 MHz
    devcfg.mode=0;                                //SPI mode 0
    devcfg.spics_io_num=-1;                       //CS pin
    devcfg.queue_size=1;                          //Only one transaction 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(SPI2_HOST, &buscfg, SPI_DMA_CH_AUTO);
    ESP_ERROR_CHECK(ret);
    ret=spi_bus_add_device(SPI2_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) {

    if(x < 0 || x >= _width || y < 0 || y >= _height) return;

    // Adafruit GFX color is 16-bit 5-6-5 rgb
    // We store only 2-bit rg, so we assume any amount of red or green wants that color
    uint8_t r, g, c;
    r = color >> 11; // RRRRRggggggbbbbb
    g = (color >> 5) & 0x3F; // rrrrrGGGGGGbbbbb

    c = 0;
    if(g > 0) c += 1;
    if(r > 0) c += 2;
    c = c << 6;

    // We store the 3 segments of the display 8-bit ints ready to transfer via 8-bit parallel SPI
    // So 2 bits are wasted per byte, but hopefully it's worth it for transfer speed

    //        b0      b1      b2      b3      b4      b5      b6     b7            b0      b1      b2      b3      b4      b5      b6     b7
    // [0][0] r(0,0)  g(0,0)  r(0,8)  g(0,8)  r(0,16) g(0,16) ---    ---    [0][1] r(1,0)  g(1,0)  r(1,8)  g(1,8)  r(1,16) g(1,16) ---    ---    [...]
    // [1][0] r(0,1)  g(0,1)  r(0,9)  g(0,9)  r(0,17) g(0,17) ---    ---    [1][1] r(1,1)  g(1,1)  r(1,9)  g(1,9)  r(1,17) g(1,17) ---    ---    [...]
    // [...]

    uint8_t bitwiseOffset = (y / 8) * 2;
    uint8_t i = backBufferIndex;
    displayBuffer[i][y % 8][x] = displayBuffer[i][y % 8][x] & ~(0b11000000 >> bitwiseOffset); // clear this pixel (note the bitwise inversion)
    displayBuffer[i][y % 8][x] = displayBuffer[i][y % 8][x] | (c >> bitwiseOffset); // write color to pixel
}


void InovaLedDisplay::swapBuffer(void) {
    shouldSwapBuffer = true;

    while(shouldSwapBuffer) {
        vTaskDelay(1 / portTICK_PERIOD_MS);
    }
}


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

    spi_transaction_t t;
    uint8_t current_row = 0;
    esp_err_t ret;
    run = true;
    while(run) {
        vTaskDelayUntil(&xLastWakeTime, xFrequency);

        memset(&t, 0, sizeof(t));
        t.length = _width;
        t.tx_buffer = &displayBuffer[1 - backBufferIndex][current_row];
        t.flags = SPI_TRANS_MODE_OCT;  // output on eight data lines (six utilized). 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 == ROWS_PER_SEGMENT) {
            current_row = 0;

            if(shouldSwapBuffer) {
                backBufferIndex++;
                backBufferIndex = backBufferIndex % 2;
                shouldSwapBuffer = false;
            }
        }
    }
}


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