/**
 * ============================================================================
 * === SSD1306 128x64 IIC LCD Interface
 * ============================================================================
 * 
 * Provides a simple interface for rendering text to an SSD1306 LCD.
 * 
 * digimint
 */

#include "iic.h"
#include "lcd/FontComicSansMS16.h"
#include "lcd/ssd1306.h"
#include "lcd/SymbolsFont.h"
#include "sdkconfig.h"

#include <driver/i2c.h>
#include <esp_log.h>
#include <freertos/FreeRTOS.h>
#include <freertos/portmacro.h>
#include <freertos/semphr.h>
#include <freertos/task.h>

#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>

uint8_t LCD_data[8][128];
bool LCD_rowChanged[8];
StaticSemaphore_t _lcSemaphore_buf;

volatile bool _lcDirty = false;
SemaphoreHandle_t _lcSemaphore = NULL;

static inline void _setPx(uint16_t x, uint16_t y){
    uint16_t bank = y >> 3;
    uint16_t posi = y &  0b111;

    LCD_data[bank][x] |= (1 << posi);
    LCD_rowChanged[bank] = true;
}

static inline void _clrPx(uint16_t x, uint16_t y){
    uint16_t bank = y >> 3;
    uint16_t posi = y &  0b111;

    LCD_data[bank][x] &= ~(1 << posi);
    LCD_rowChanged[bank] = true;
}

void LCD_renderChar(font_t font, const char oChr, uint16_t x, uint16_t y, bool invert){
    char chr = oChr;
    if(chr < ' ' || chr > '~'){
        chr = ' ';
    }

    chr -= ' ';
    uint_fast16_t charsPerRow = ((font.width & 0b111) == 0) ? (font.width >> 3) : ((font.width >> 3) + 1);
    const char* toRender = &(font.fTable)[chr*(font.cSize)];
    for(uint16_t xx = 0; xx < font.width; ++xx){
        for(uint16_t yy = 0; yy < font.height; ++yy){
            if(
                toRender[yy*charsPerRow + (xx >> 3)]
                & (1 << (7-(xx & 0b111)))
            ){
                invert ? _clrPx(xx+x, yy+y) : _setPx(xx+x, yy+y);
            }else{
                invert ? _setPx(xx+x, yy+y) : _clrPx(xx+x, yy+y);
            }
        }
    }
}

void LCD_renderText(font_t font, const char* str, uint16_t x, uint16_t y, bool invert){
    uint_fast16_t i = 0;
    while(*str != 0){
        LCD_renderChar(font, *str, font.width*i+x, y, invert);
        ++str;
        ++i;
    }
}

bool LCD_updateDisplay(bool block){
    if(!xSemaphoreTake(_lcSemaphore, 0)){
        if(!block) return false;
        else xSemaphoreTake(_lcSemaphore, portMAX_DELAY);
    }
    for(uint32_t j = 0; j < 8; ++j){
        // if(LCD_rowChanged[j] == false) continue;
        for(uint32_t i = 0; i < 128; ++i){
            iicData(IIC_NUM_LCD, IIC_LCD_ADDR, LCD_data[j][i]);
        }
        LCD_rowChanged[j] = false;
    }
    xSemaphoreGive(_lcSemaphore);
    return true;
}

// void T_LCD_updateTask(void* params){
//     while(1){
//         Display_render();
//         vTaskDelay(50 / portTICK_PERIOD_MS);
//         bool needsUpdate = false;
//         for(uint8_t i = 0; i < 8; ++i) needsUpdate = needsUpdate || LCD_rowChanged[i];
//         if(needsUpdate == true) LCD_updateDisplay(true);
//     }
// }

void LCD_init(TaskHandle_t* updateTaskHandle){
    ESP_LOGI(LCD_TAG, "Initializing SSD1306 LCD Interface...");

    // Initialize the semaphore for LCD access.
    _lcSemaphore = xSemaphoreCreateBinaryStatic(&_lcSemaphore_buf);
    xSemaphoreGive(_lcSemaphore);

    ESP_LOGD(LCD_TAG, "  Initializing IIC");
    iicInit(
        IIC_NUM_LCD,
        CONFIG_LCD_SCL,
        CONFIG_LCD_SDA,
        true,
        200000
    );

    ESP_LOGD(LCD_TAG, "  Sending LCD initialization commands");

    /* Shouldn't be necessary to take the semaphore here, but better safe than
     * sorry.
     */
    if(!xSemaphoreTake(_lcSemaphore, 0)) esp_system_abort("Semaphore error during LCD initialization!");
    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0xAE);
    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0xD5);
    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0x80);
    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0xA8);
    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0x3F);
    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0xD3);
    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0x00);
    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0x40);
    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0x8D);
    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0x14);
    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0x20);
    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0x00);
    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0xA1);
    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0xC8);
    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0xDA);
    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0x12);
    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0x81);
    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0xCF);
    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0xD9);
    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0xF1);
    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0xDB);
    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0x40);
    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0xA4);
    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0xA6);
    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0xAF);

    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0x00);
    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0x10);
    
    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0x21);
    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0x00);
    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0x7F);

    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0x22);
    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0x00);
    iicCommand(IIC_NUM_LCD, IIC_LCD_ADDR, 0x07);
    xSemaphoreGive(_lcSemaphore);

    ESP_LOGD(LCD_TAG, "  Initializing data buffer");
    for(uint32_t j = 0; j < 8; ++j){
        for(uint32_t i = 0; i < 128; ++i){
            LCD_data[j][i] = 0;
        }
    }

    ESP_LOGD(LCD_TAG, "  Clearing Display");
    if(!LCD_updateDisplay(false)){
        ESP_LOGW(LCD_TAG, "Unable to clear LCD display (communication semaphore already in use).");
    }

    // ESP_LOGD(LCD_TAG, "  Spawning LCD update task");
    // TaskHandle_t tsk;
    // if(updateTaskHandle == NULL){
    //     updateTaskHandle = &tsk;
    // }
    // xTaskCreate(&T_LCD_updateTask, "LCD Update Task", 2048, NULL, 2, updateTaskHandle);

    ESP_LOGI(LCD_TAG, "Ready.");
}