#include "stm32f4xx_hal.h"
#include "lcd.h"

static uint8_t switchedTo4BitMode = 0;

void delayUS_DWT(uint32_t us);
void delayUS_DWT_Init(void);

void lcdWriteCommand(uint8_t command) {
    HAL_GPIO_WritePin(LCD_RS_PORT, LCD_RS_PIN, GPIO_PIN_RESET);
    if (switchedTo4BitMode){
        lcdWrite(command);
        lcdWrite(command << 4);
    }else {
        lcdWrite(command);
    }
    if ((command == LCD_COMMAND_CLEAR) || (command == LCD_COMMAND_RETURN_HOME)) {
        delayUS_DWT(1640);
    } else {
        delayUS_DWT(40);
    }
}

void lcdWriteData(uint8_t data) {
    HAL_GPIO_WritePin(LCD_RS_PORT, LCD_RS_PIN, GPIO_PIN_SET);
    if (switchedTo4BitMode){
        lcdWrite(data);
        lcdWrite(data << 4);
    }else {
        lcdWrite(data);
    }
}

void lcdWrite(uint8_t data) {
    //data/command
    HAL_GPIO_WritePin(LCD_DB4_PORT, LCD_DB4_PIN, data & (1 << 4));
    HAL_GPIO_WritePin(LCD_DB5_PORT, LCD_DB5_PIN, data & (1 << 5));
    HAL_GPIO_WritePin(LCD_DB6_PORT, LCD_DB6_PIN, data & (1 << 6));
    HAL_GPIO_WritePin(LCD_DB7_PORT, LCD_DB7_PIN, data & (1 << 7));
    delayUS_DWT(1);
    //EN high
    HAL_GPIO_WritePin(LCD_EN_PORT, LCD_EN_PIN, GPIO_PIN_SET);
    //wait 1 us
    delayUS_DWT(1);
    //EN low
    HAL_GPIO_WritePin(LCD_EN_PORT, LCD_EN_PIN, GPIO_PIN_RESET);
    delayUS_DWT(1);
}

void lcdInit() {
    delayUS_DWT_Init();
    HAL_Delay(15);
    lcdWriteCommand(LCD_COMMAND_FUNCTION_SET | LCD_PARAM_FUNCTION_SET_8BIT); // set interface - 8-bit
    HAL_Delay(4);
    lcdWriteCommand(LCD_COMMAND_FUNCTION_SET | LCD_PARAM_FUNCTION_SET_8BIT); // set interface - 8-bit
    HAL_Delay(1);
    lcdWriteCommand(LCD_COMMAND_FUNCTION_SET | LCD_PARAM_FUNCTION_SET_8BIT); // set interface - 8-bit
    HAL_Delay(1);
    lcdWriteCommand(LCD_COMMAND_FUNCTION_SET); // set interface - 4-bit
    HAL_Delay(1);
    switchedTo4BitMode = 1;
    lcdWriteCommand(LCD_COMMAND_FUNCTION_SET | LCD_PARAM_FUNCTION_SET_2LINES); // set interface - 4-bit, 2 lines, 5x8 font
    lcdWriteCommand(LCD_COMMAND_ON_OFF); // off - nothing to be on
    lcdWriteCommand(LCD_COMMAND_CLEAR); // clear
    lcdWriteCommand(LCD_COMMAND_ENTRY_MODE_SET | LCD_PARAM_ENTRY_MODE_SET_INCREMENT); // scroll
    lcdWriteCommand(LCD_COMMAND_ON_OFF | LCD_PARAM_ON_OFF_DISPLAY); // display on

}

void lcdString(char str[]) {
    uint8_t i = 0;
    while (str[i] != 0) {
        lcdWriteData(str[i++]);
    }
}

void lcdGotoXY(uint8_t x, uint8_t y){
    lcdWriteCommand(LCD_COMMAND_SET_DDRAM_ADDRESS | (x + y * 0x40));
}

void lcdDefineChar(uint8_t charDefinition[], uint8_t code) {
    lcdWriteCommand(LCD_COMMAND_SET_CGRAM_ADDRESS + (code % 8) * 8);
    for (uint8_t i = 0; i < 8; i++) {
        lcdWriteData(charDefinition[i]);
    }
}

void delayUS_DWT_Init(void) {
    CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
    DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
}

#pragma GCC push_options
#pragma GCC optimize ("O3")
void delayUS_DWT(uint32_t us) {
    volatile uint32_t cycles = (SystemCoreClock / 1000000L) * us;
    volatile uint32_t start = DWT->CYCCNT;
    while(DWT->CYCCNT - start < cycles);
}
#pragma GCC pop_options