#include "libCharLCD/lcd.h"

namespace
{
        unsigned const LCD_BUSY_FLAG = 0x80;
        unsigned const LCD_INIT_SEQ = 0x30; // initialise
        unsigned const LCD_DISP_CLEAR = 0x01;
        unsigned const LCD_DISP_OFF = 0x08;
        unsigned const LCD_DISP_ON = 0x0C;
        unsigned const LCD_CURSOR_ON = 0x0E;
        unsigned const LCD_CURSOR_OFF = 0x0C;
        unsigned const LCD_CURSOR_BLINK = 0x0F;
        unsigned const LCD_RETURN_HOME = 0x02;
        unsigned const LCD_ENTRY_MODE = 0x06;
        unsigned const LCD_4BIT_MODE = 0x20;
        unsigned const LCD_8BIT_MODE = 0x30;
        unsigned const LCD_1_ROWS = 0x00;
        unsigned const LCD_2_ROWS = 0x08;
        unsigned const LCD_FONT_5x8 = 0x00;
        unsigned const LCD_FONT_5x10 = 0x04;
        unsigned const LCD_POSITION = 0x80;
        unsigned const LCD_CG = 0x40;

        unsigned const LCD_SHIFT = 0x10;
        unsigned const LCD_CURSOR = 0x00;
        unsigned const LCD_DISPLAY = 0x08;
        unsigned const LCD_LEFT = 0x00;
        unsigned const LCD_RIGHT = 0x04;

        unsigned const LCD_ROW1_START = 0x00;
        unsigned const LCD_ROW2_START = 0x40;
} // namespace

// initialise the display
void lcd_t::init()
{
        // initialise low level code now ChibiOS is running
        m_lcd.init();

        m_lcd.writeCtrl4(LCD_INIT_SEQ);

        HAL_Delay(5); // > 4.1ms
        m_lcd.writeCtrl4(LCD_INIT_SEQ);

        HAL_Delay(5);
        m_lcd.writeCtrlWait(LCD_4BIT_MODE | LCD_2_ROWS | LCD_FONT_5x8);

        HAL_Delay(5);

        m_lcd.writeCtrlWait(LCD_4BIT_MODE | LCD_2_ROWS | LCD_FONT_5x8);

        m_lcd.writeCtrlWait(LCD_DISP_OFF);

        m_lcd.writeCtrlWait(LCD_DISP_CLEAR);

        m_lcd.writeCtrlWait(LCD_ENTRY_MODE);
}

/// \brief clear the display
void lcd_t::clear()
{
        m_lcd.writeCtrlWait(LCD_DISP_CLEAR);
}

void lcd_t::enable(bool ena_)
{
        m_lcd.writeCtrlWait(ena_ ? LCD_DISP_ON : LCD_DISP_OFF);
}

void lcd_t::gotoxy(uint8_t x_, uint8_t y_)
{
        if (y_ >= displayYsize())
                return;
        if (x_ >= displayXsize())
                return;
        uint8_t row = (y_ == 1) ? LCD_ROW2_START : LCD_ROW1_START;

        // x_ is already constrained
        m_lcd.writeCtrlWait(LCD_POSITION | row | x_);
}

void lcd_t::printString(char *str_, uint8_t count_)
{
        printString(const_cast<char const *>(str_),count_);
}

void lcd_t::printString(char const *str_, uint8_t count_)
{
        // if passed a null terminated string and a count less than display X size  
        if (count_ > displayXsize())
        {
                count_ = displayXsize();
                while (*str_ && count_--)
                        m_lcd.writeData(*str_++);
                return;
        }
        else
        {
                while (count_--)
                        m_lcd.writeData(*str_++);
        }
}

uint8_t lcd_t::format_num(char *buff, uint8_t digits, uint8_t dp_pos, int val)
{
        digits++;
        uint8_t pos = digits;
        uint8_t dp_loc = pos - dp_pos;
        uint8_t sign = 0;
        if (val < 0)
        {
                sign = 1;
                val = -val;
        }

        buff[pos] = 0;
        while (pos)
        {
                if (pos == dp_loc)
                {
                        buff[--pos] = '.';
                }
                else
                {
                        buff[--pos] = val % 10 + '0';
                        val /= 10;
                        if (val == 0 && pos < dp_loc)
                                break;
                }
        }
        if (sign)
        {
                buff[--pos] = '-';
        }
        return digits;
}

void lcd_t::font_digits(uint8_t digits, uint8_t dp_pos, int val)
{
        char buff[10] = "        ";
        format_num(buff, digits, dp_pos, val);
        printString(buff);
}

void lcd_t::defineChar(uint8_t code, uint8_t* rows)
{
        defineChar(code, const_cast<uint8_t const *>(rows));
}


void lcd_t::defineChar(uint8_t code, uint8_t const * rows)
{
        uint8_t addr =  (code & 0x7)* 8;
        m_lcd.writeCtrlWait(LCD_CG | (addr &0x3F)  );
        for (int i = 0; i < 8; i++)
                m_lcd.writeData(rows[i] & 0x1f);
}