use embedded_hal::i2c::I2c;
use std::thread::sleep;
use std::time::Duration;

pub struct Lcd<I: I2c> {
    i2c: I,
    addr: u8,
    backlight: bool,
}

impl<I: I2c> Lcd<I> {
    const RS: u8 = 0b001;
    const EN: u8 = 0b100;
    const BL: u8 = 0b1000;

    pub fn new(i2c: I, addr: u8) -> Result<Self, I::Error> {
        let mut lcd = Self { i2c, addr, backlight: true };
        lcd.init()?;
        Ok(lcd)
    }

    fn write_nibble(&mut self, nibble: u8, rs: u8) -> Result<(), I::Error> {
        let bl = if self.backlight { Self::BL } else { 0 };
        let data = nibble | rs | bl;
        self.i2c.write(self.addr, &[data | Self::EN])?;
        sleep(Duration::from_micros(1));
        self.i2c.write(self.addr, &[data & !Self::EN])?;
        sleep(Duration::from_micros(50));
        Ok(())
    }

    fn write_byte(&mut self, byte: u8, rs: u8) -> Result<(), I::Error> {
        self.write_nibble(byte & 0xF0, rs)?;
        self.write_nibble((byte & 0x0F) << 4, rs)?;
        Ok(())
    }

    fn cmd(&mut self, c: u8) -> Result<(), I::Error> {
        self.write_byte(c, 0)?;
        if c <= 0x03 {
            sleep(Duration::from_millis(2));
        }
        Ok(())
    }

    fn data(&mut self, d: u8) -> Result<(), I::Error> {
        self.write_byte(d, Self::RS)
    }

    fn init(&mut self) -> Result<(), I::Error> {
        sleep(Duration::from_millis(50));
        for _ in 0..3 {
            self.write_nibble(0x30, 0)?;
            sleep(Duration::from_millis(5));
        }
        self.write_nibble(0x20, 0)?;
        sleep(Duration::from_millis(1));

        self.cmd(0x28)?;
        self.cmd(0x0C)?;
        self.cmd(0x06)?;
        self.cmd(0x01)?;
        sleep(Duration::from_millis(2));
        Ok(())
    }

    pub fn clear(&mut self) -> Result<(), I::Error> {
        self.cmd(0x01)?;
        sleep(Duration::from_millis(2));
        Ok(())
    }

    pub fn set_cursor(&mut self, col: u8, row: u8) -> Result<(), I::Error> {
        let addr = match row {
            0 => 0x00,
            1 => 0x40,
            2 => 0x14,
            3 => 0x54,
            _ => 0x00,
        };
        self.cmd(0x80 | (addr + col))
    }

    pub fn create_char(&mut self, location: u8, data: &[u8; 8]) -> Result<(), I::Error> {
        self.cmd(0x40 | (location << 3))?;
        for &b in data {
            self.data(b)?;
        }
        self.set_cursor(0, 0)
    }



    pub fn print(&mut self, s: &str) -> Result<(), I::Error> {
        for b in s.bytes() {
            self.data(b)?;
        }
        Ok(())
    }
}