//
// Created by robtor on 09.12.22.
//

#include "../Inc/LCD_I2C_Driver.h"

namespace floatpump {


    void LCD_I2C_Driver::m_lcdSendCmnd(char cmnd) {
        char data_u, data_l;    //upper and lower nibble to transmit
        char backlight = (m_backlight << 3);
        uint8_t data_t[4];      //data to transmit

        data_u = (cmnd & 0xf0);         //get higher nibble
        data_l = ((cmnd << 4) & 0xf0);  //get lower nibble

        data_t[0] = backlight | data_u | 0x04; //EN=1, RS=0 (XXXX 1100)
        data_t[1] = backlight | data_u | 0x00; //EN=0, RS=0 (XXXX 1000)
        data_t[2] = backlight | data_l | 0x04; //EN=1, RS=0
        data_t[3] = backlight | data_l | 0x00; //EN=0, RS=0

        HAL_I2C_Master_Transmit(&m_handle, m_displayAddr, (uint8_t *) data_t, 4, 100);
    }

    void LCD_I2C_Driver::m_lcdSendData(char data) {
        char data_u, data_l;        //upper and lower nibble of data
        char backlight = (m_backlight << 3);
        uint8_t data_t[4];          //data to transmit

        data_u = (data & 0xf0);         //get higher nibble
        data_l = ((data << 4) & 0xf0);  //get lower nibble

        data_t[0] = backlight | data_u | 0x05; //EN=1, RS=1 (XXXX 1101)
        data_t[1] = backlight | data_u | 0x01; //EN=0, RS=1 (XXXX 1001)
        data_t[2] = backlight | data_l | 0x05; //EN=1, RS=1
        data_t[3] = backlight | data_l | 0x01; //EN=0, RS=1

        HAL_I2C_Master_Transmit(&m_handle, m_displayAddr, (uint8_t *) data_t, 4, 100);
    }

    void LCD_I2C_Driver::LCDInitialize() {
        // Initialization by Instruction (from HD44780U Datasheet)
        HAL_Delay(50);  // wait for >40ms
        m_lcdSendCmnd(0x30);
        HAL_Delay(5);  // wait for >4.1ms
        m_lcdSendCmnd (0x30);
        HAL_Delay(1);  // wait for >100us
        m_lcdSendCmnd (0x30);
        HAL_Delay(10); // Wait for command execution lengt

        // Configuration
        m_lcdSendCmnd (0x20);  // Set Interface to 4-bit mode
        HAL_Delay(10);


        m_lcdSendCmnd (0x28); // Function set 00001 DL N F - - => DL=0 (4 bit mode), N = 1 (2 line display) F = 0 (5x8 characters)
        HAL_Delay(1);

        m_lcdSendCmnd (0x08); // Display on/off control 00001 D C B => D=0 Display on/off, C=0 Cursor on/off, B=0 Cursor blink on/off
        HAL_Delay(1);

        m_lcdSendCmnd (0x01);  // clear display 00000001
        HAL_Delay(1);
        HAL_Delay(1);

        m_lcdSendCmnd (0x06); //Entry mode set 000001 I/D S => I/D = 1 increment cursor, S = 0 display shift
        HAL_Delay(1);

        m_lcdSendCmnd (0x0C); // Display on/off control 00001 D C B => D=1 Display on/off, C=0 Cursor on/off, B=0 Cursor blink on/off
        HAL_Delay(1);

        m_powered = true;
        m_cursor = false;
        m_cursorBlink = false;

        m_incrementDecrement = true;
        m_autoShift = false;
    }

    void LCD_I2C_Driver::LCDSendCString(char *str) {
        while (*str) m_lcdSendData(*str++);
    }

    void LCD_I2C_Driver::LCDClearDisplay(void) {
        m_lcdSendCmnd (0x01);
        HAL_Delay(1);
    }

    void LCD_I2C_Driver::LCDSetBacklight(bool enabled) {
        m_backlight = enabled;

        //Send dummy command to force backlight set
        uint8_t dummyPacket = (enabled << 3);
        HAL_I2C_Master_Transmit(&m_handle, m_displayAddr, &dummyPacket, 1, 100);
    }

    void LCD_I2C_Driver::LCDPower(bool enabled) {
        char com_build = 0x08 | (enabled << 2) | (m_cursor << 1) | (m_cursorBlink);
        m_lcdSendCmnd (com_build); // Display on/off control 00001 D C B => D=0 Display on/off, C=0 Cursor on/off, B=0 Cursor blink on/off
        m_powered = enabled;
    }

    void LCD_I2C_Driver::LCDCursor(bool enabled) {
        char com_build = 0x08 | (m_powered << 2) | (enabled << 1) | (m_cursorBlink);
        m_lcdSendCmnd (com_build); // Display on/off control 00001 D C B => D=0 Display on/off, C=0 Cursor on/off, B=0 Cursor blink on/off
        m_cursor = enabled;
    }

    void LCD_I2C_Driver::LCDCursorBlink(bool enabled) {
        char com_build = 0x08 | (m_powered << 2) | (m_cursor << 1) | (enabled);
        m_lcdSendCmnd (com_build); // Display on/off control 00001 D C B => D=0 Display on/off, C=0 Cursor on/off, B=0 Cursor blink on/off
        m_cursorBlink = enabled;
    }

    void LCD_I2C_Driver::LCDCursorIncDec(bool enabled) {
        char com_build = 0x04 | (enabled << 1) | m_autoShift;
        m_lcdSendCmnd(com_build);
        m_incrementDecrement = enabled;
    }

    void LCD_I2C_Driver::LCDDisplayShift(bool enabled) {
        char com_build = 0x04 | (m_incrementDecrement << 1) | enabled;
        m_lcdSendCmnd(com_build);
        m_autoShift = enabled;
    }

    void LCD_I2C_Driver::LCDSetCursor(uint8_t x, uint8_t y) {
        char addr = 0x00;
        char startat = 0x00;

        // Generate start addresses for each row
        // Remember that Controller assumes only two lines of ram, line 2 starting at 0x40
        // |0x00 -------------- line 1 -------------- 0x13||0x14 -------------- line 3 -------------- 0x27|
        // |0x40 -------------- line 2 -------------- 0x53||0x54 -------------- line 4 -------------- 0x67|

        switch(y) {
            case 0: startat = 0x00; break;
            case 1: startat = 0x40; break;
            case 2: startat = 0x14; break;
            case 3: startat = 0x54; break;
        }

        addr = startat + x;
        m_lcdSendCmnd(( 0x80 | addr));
        HAL_Delay(1);
    }

} // floatpump