// Based on the work by DFRobot upgraded by DVSProductions
#include "LiquidCrystal_I2C.h"
#include <inttypes.h>
/*********** mid level commands, for sending data/cmds */
#define COMMAND(value) send(value, 0)
#define printIIC(args) i2c->write(_Addr, args, 1)
size_t LiquidCrystal_I2C::write(uint8_t value) {
    send(value, Rs);
    return 1;
}

inline void LiquidCrystal_I2C::command(uint8_t value) {
    send(value, 0);
}

// When the display powers up, it is configured as follows:
//
// 1. Display clear
// 2. Function set:
//    DL = 1; 8-bit interface data
//    N = 0; 1-line display
//    F = 0; 5x8 dot character font
// 3. Display on/off control:
//    D = 0; Display off
//    C = 0; Cursor off
//    B = 0; Blinking off
// 4. Entry mode set:
//    I/D = 1; Increment by 1
//    S = 0; No shift
//
// Note, however, that resetting the Arduino doesn't reset the LCD, so we
// can't assume that its in that state when a sketch starts (and the
// LiquidCrystal constructor is called).

/*!
Creates a new LCD instance
!*/
LiquidCrystal_I2C::LiquidCrystal_I2C(uint8_t lcd_Addr, uint8_t lcd_cols, uint8_t lcd_rows, PinName sda, PinName scl) {
    _Addr = lcd_Addr << 1;
    _cols = lcd_cols;
    _rows = lcd_rows;
    _backlightval = LCD_NOBACKLIGHT;
    i2c = new I2C(sda, scl);
    i2c->frequency(1000000);
}

 void LiquidCrystal_I2C::init() {
    init_priv();
}

 void LiquidCrystal_I2C::init_priv() {
    _displayfunction = LCD_4BITMODE | LCD_1LINE | LCD_5x8DOTS;
    begin_priv();
}
void LiquidCrystal_I2C::begin_priv(uint8_t dotsize) {
    if (_rows > 1) {
        _displayfunction |= LCD_2LINE;
    }
    _numlines = _rows;
    // for some 1 line displays you can select a 10 pixel high font
    if ((dotsize != 0) && (_rows == 1)) {
        _displayfunction |= LCD_5x10DOTS;
    }
    // SEE PAGE 45/46 FOR INITIALIZATION SPECIFICATION!
    // according to datasheet, we need at least 40ms after power rises above 2.7V
    // before sending commands. Nucleo can turn on way befer 4.5V so we'll wait
    // 50
    wait_us(50000);
    // Now we pull both RS and R/W low to begin commands
    expanderWrite(_backlightval); // reset expanderand turn backlight off (Bit 8 =1)
    wait_us(1000000);
    // put the LCD into 4 bit mode
    // this is according to the hitachi HD44780 datasheet
    // figure 24, pg 46

    // we start in 8bit mode, try to set 4 bit mode
    write4bits(0x03 << 4);
    wait_us(4100); // wait min 4.1ms
    // second try
    write4bits(0x03 << 4);
    wait_us(4100); // wait min 4.1ms
    // third go!
    write4bits(0x03 << 4);
    wait_us(150);
    // finally, set to 4-bit interface
    write4bits(0x02 << 4);
    // set # lines, font size, etc.
    COMMAND(LCD_FUNCTIONSET | _displayfunction);
    // turn the display on with no cursor or blinking default
    _displaycontrol = LCD_DISPLAYON | LCD_CURSOROFF | LCD_BLINKOFF;
    display();
    // clear it off
    clear();
    // Initialize to default text direction (for roman languages)
    _displaymode = LCD_ENTRYLEFT | LCD_ENTRYSHIFTDECREMENT;
    // set the entry mode
    COMMAND(LCD_ENTRYMODESET | LCD_ENTRYLEFT | LCD_ENTRYSHIFTDECREMENT);
    home();
}
void LiquidCrystal_I2C::begin(uint8_t cols, uint8_t lines, uint8_t dotsize) {
    _cols = cols;
    _rows = lines;
    begin_priv();
}

/********** high level commands, for the user! */
void LiquidCrystal_I2C::clear() {
    COMMAND(LCD_CLEARDISPLAY); // clear display, set cursor position to zero
    wait_us(2000);             // this command takes a long time!
}

void LiquidCrystal_I2C::home() {
    COMMAND(LCD_RETURNHOME); // set cursor position to zero
    wait_us(2000);           // this command takes a long time!
}

void LiquidCrystal_I2C::setCursor(uint8_t col, uint8_t row) {
    int row_offsets[] = { 0x00, 0x40, 0x14, 0x54 };
    if (row > _numlines)
        row = _numlines - 1; // we count rows starting w/0
    COMMAND(LCD_SETDDRAMADDR | (col + row_offsets[row]));
}

// Turn the display on/off (quickly)
void LiquidCrystal_I2C::noDisplay() {
    _displaycontrol &= ~LCD_DISPLAYON;
    COMMAND(LCD_DISPLAYCONTROL | _displaycontrol);
}
void LiquidCrystal_I2C::display() {
    _displaycontrol |= LCD_DISPLAYON;
    COMMAND(LCD_DISPLAYCONTROL | _displaycontrol);
}

// Turns the underline cursor on/off
void LiquidCrystal_I2C::noCursor() {
    _displaycontrol &= ~LCD_CURSORON;
    COMMAND(LCD_DISPLAYCONTROL | _displaycontrol);
}
void LiquidCrystal_I2C::cursor() {
    _displaycontrol |= LCD_CURSORON;
    COMMAND(LCD_DISPLAYCONTROL | _displaycontrol);
}

// Turn on and off the blinking cursor
void LiquidCrystal_I2C::noBlink() {
    _displaycontrol &= ~LCD_BLINKON;
    COMMAND(LCD_DISPLAYCONTROL | _displaycontrol);
}
void LiquidCrystal_I2C::blink() {
    _displaycontrol |= LCD_BLINKON;
    COMMAND(LCD_DISPLAYCONTROL | _displaycontrol);
}

// These commands scroll the display without changing the RAM
void LiquidCrystal_I2C::scrollDisplayLeft(void) {
    COMMAND(LCD_CURSORSHIFT | LCD_DISPLAYMOVE | LCD_MOVELEFT);
}
void LiquidCrystal_I2C::scrollDisplayRight(void) {
    COMMAND(LCD_CURSORSHIFT | LCD_DISPLAYMOVE | LCD_MOVERIGHT);
}

// This is for text that flows Left to Right
void LiquidCrystal_I2C::leftToRight(void) {
    _displaymode |= LCD_ENTRYLEFT;
    COMMAND(LCD_ENTRYMODESET | _displaymode);
}

// This is for text that flows Right to Left
void LiquidCrystal_I2C::rightToLeft(void) {
    _displaymode &= ~LCD_ENTRYLEFT;
    COMMAND(LCD_ENTRYMODESET | _displaymode);
}

// This will 'right justify' text from the cursor
void LiquidCrystal_I2C::autoscroll(void) {
    _displaymode |= LCD_ENTRYSHIFTINCREMENT;
    COMMAND(LCD_ENTRYMODESET | _displaymode);
}

// This will 'left justify' text from the cursor
void LiquidCrystal_I2C::noAutoscroll(void) {
    _displaymode &= ~LCD_ENTRYSHIFTINCREMENT;
    COMMAND(LCD_ENTRYMODESET | _displaymode);
}

// Allows us to fill the first 8 CGRAM locations
// with custom characters
void LiquidCrystal_I2C::createChar(uint8_t location, uint8_t charmap[]) {
    location &= 0x7; // we only have 8 locations 0-7
    COMMAND(LCD_SETCGRAMADDR | (location << 3));
    for (uint8_t i = 0; i != 8; i++)
        write(charmap[i]);
}

// Turn the (optional) backlight off/on
void LiquidCrystal_I2C::noBacklight(void) {
    _backlightval = LCD_NOBACKLIGHT;
    expanderWrite(0);
}

void LiquidCrystal_I2C::backlight(void) {
    _backlightval = LCD_BACKLIGHT;
    expanderWrite(0);
}

/************ low level data pushing commands **********/

// write either command or data
void LiquidCrystal_I2C::send(uint8_t value, uint8_t mode) {
    uint8_t highnib = value & 0xf0;
    uint8_t lownib = (value << 4) & 0xf0;
    write4bits((highnib) | mode);
    write4bits((lownib) | mode);
}

void LiquidCrystal_I2C::write4bits(uint8_t value) {
    expanderWrite(value);
    pulseEnable(value);
}
char buff;
inline void LiquidCrystal_I2C::expanderWrite(uint8_t _data) {
    buff = (_data) | _backlightval;
    printIIC(&buff);
}

void LiquidCrystal_I2C::pulseEnable(uint8_t _data) {
    expanderWrite(_data | En); // En high
    wait_us(1);                // enable pulse must be >450ns

    expanderWrite(_data & ~En); // En low
    wait_us(10);                // commands need > 37us to settle
}

// Alias functions

inline void LiquidCrystal_I2C::cursor_on() {
    cursor();
}

inline void LiquidCrystal_I2C::cursor_off() {
    noCursor();
}

inline void LiquidCrystal_I2C::blink_on() {
    blink();
}

inline void LiquidCrystal_I2C::blink_off() {
    noBlink();
}

inline void LiquidCrystal_I2C::load_custom_character(uint8_t char_num, uint8_t *rows) {
    createChar(char_num, rows);
}

void LiquidCrystal_I2C::setBacklight(uint8_t new_val) {
    if (new_val)
        backlight(); // turn backlight on
    else
        noBacklight(); // turn backlight off
}

void LiquidCrystal_I2C::printstr(const char c[]) {
    // This function is not identical to the function used for "real" I2C displays
    // it's here so the use sketch doesn't have to be changed
    print(c);
}

// unsupported API functions
void LiquidCrystal_I2C::off() {
    noBacklight();
}
void LiquidCrystal_I2C::on() {
    begin_priv();
    backlight();
}
void LiquidCrystal_I2C::setDelay(int cmdDelay, int charDelay) {}
uint8_t LiquidCrystal_I2C::status() {
    return 0;
}
uint8_t LiquidCrystal_I2C::keypad() {
    return 0;
}
uint8_t LiquidCrystal_I2C::init_bargraph(uint8_t graphtype) {
    return 0;
}
void LiquidCrystal_I2C::draw_horizontal_graph(uint8_t row, uint8_t column,
    uint8_t len,
    uint8_t pixel_col_end) {
}
void LiquidCrystal_I2C::draw_vertical_graph(uint8_t row, uint8_t column,
    uint8_t len,
    uint8_t pixel_row_end) {
}
void LiquidCrystal_I2C::setContrast(uint8_t new_val) {}