use arduino_hal::delay_ms;
use arduino_hal::delay_us;
use embedded_hal::digital::v2::OutputPin;

// The CMDs codes are documented here:
// https://www.openhacks.com/uploadsproductos/eone-1602a1.pdf
const INIT_CMD: u8 = 0x03; // 0b00000011
const MODE_4_BITS_CMD: u8 = 0x02; // 0b00000010
const DISPLAY_MODE_16X2_CMD: u8 = 0x28; // 0b00101000
const ENABLE_SCREEN_CMD: u8 = 0x0C; // 0b00001100
const CLEAN_SCREEN_CMD: u8 = 0x01; // 0b00000001
const SHIFT_LEFT_CMD: u8 = 0x10; // 0b10000
const SHIFT_RIGHT_CMD: u8 = 0x14; // 0b10100
const MOVE_CURSOR_CMD: u8 = 0x80; // 0b10000000

const ROW_OFFSETS: [u8; 2] = [0x00, 0x40];

pub enum Direction {
    Left = SHIFT_LEFT_CMD as isize,
    Right = SHIFT_RIGHT_CMD as isize,
}

pub struct StnScreen<'a, RS, EN, D4, D5, D6, D7> {
    rs: &'a mut RS,
    en: &'a mut EN,
    d4: &'a mut D4,
    d5: &'a mut D5,
    d6: &'a mut D6,
    d7: &'a mut D7,
}

impl<
        'a,
        RS: OutputPin,
        EN: OutputPin,
        D4: OutputPin,
        D5: OutputPin,
        D6: OutputPin,
        D7: OutputPin,
    > StnScreen<'a, RS, EN, D4, D5, D6, D7>
{
    pub fn new(
        rs: &'a mut RS,
        en: &'a mut EN,
        d4: &'a mut D4,
        d5: &'a mut D5,
        d6: &'a mut D6,
        d7: &'a mut D7,
    ) -> Self {
        let mut display = StnScreen {
            rs,
            en,
            d4,
            d5,
            d6,
            d7,
        };

        // Initializing of LCM on page 16
        // https://z3d9b7u8.stackpathcdn.com/pdf-down/L/C/D/LCD-1602A_CA.pdf
        delay_ms(16);
        display.write_nibble(INIT_CMD, false);
        delay_ms(5);
        display.write_nibble(INIT_CMD, false);
        delay_us(100);
        display.write_nibble(INIT_CMD, false);
        delay_us(100);
        display.write_nibble(MODE_4_BITS_CMD, false);
        delay_us(100);
        display.send_cmd(DISPLAY_MODE_16X2_CMD, false);
        display.send_cmd(ENABLE_SCREEN_CMD, false);
        display.clear();

        display
    }

    fn pulse_enable(&mut self) {
        self.en.set_low().ok();
        delay_us(1);
        self.en.set_high().ok();
        delay_us(1);
        self.en.set_low().ok();
        delay_us(100);
    }

    fn send_cmd(&mut self, cmd: u8, is_data: bool) {
        self.write_nibble((cmd >> 4) & 0x0F, is_data);
        self.write_nibble(cmd & 0x0F, is_data);
        delay_ms(10);
    }

    fn write_nibble(&mut self, nibble: u8, is_data: bool) {
        self.rs.set_state(is_data.into()).ok();
        self.d4.set_state(last_bit(nibble >> 0).into()).ok();
        self.d5.set_state(last_bit(nibble >> 1).into()).ok();
        self.d6.set_state(last_bit(nibble >> 2).into()).ok();
        self.d7.set_state(last_bit(nibble >> 3).into()).ok();
        self.pulse_enable();
    }

    pub fn clear(&mut self) {
        self.send_cmd(CLEAN_SCREEN_CMD, false);
    }

    pub fn shift(&mut self, dir: Direction) {
        let cmd = dir as u8;
        self.send_cmd(cmd, false);
    }

    pub fn set_cursor(&mut self, col: u8, row: u8) {
        let row_offset = ROW_OFFSETS[row as usize];
        let address_position = row_offset | col;
        self.send_cmd(MOVE_CURSOR_CMD | address_position, false);
    }

    pub fn write_char(&mut self, c: char) {
        let c = c as u8;
        self.write_nibble((c >> 4) & 0x0F, true);
        self.write_nibble(c & 0x0F, true);
        delay_us(100);
    }

    pub fn write_str<S: AsRef<str>>(&mut self, s: S) {
        let s = s.as_ref();
        for c in s.chars() {
            self.write_char(c);
        }
    }

    pub fn write_token(&mut self, mut token: u32) {
        let mut buff = [0u8; 7];
        for (n, chr) in buff.iter_mut().enumerate() {
            *chr = if n == 3 {
                '-' as u8
            } else {
                let ch = (token % 10) as u8 + '0' as u8;
                token /= 10;
                ch
            }
        }
        buff.reverse();
        let s = core::str::from_utf8(&buff).unwrap();
        self.write_str(s);
    }

    pub fn write_u8(&mut self, mut num: u8) {
        if num == 0 {
            self.write_char('0');
            return;
        }

        let mut buff = [0u8; 3];
        let num_len = num.ilog10() as usize;
        for i in (0..=num_len).rev() {
            buff[i] = num % 10 + '0' as u8;
            num /= 10;
        }
        self.write_str(core::str::from_utf8(&buff[..=num_len]).unwrap());
    }

    pub fn write_u32(&mut self, num: u32) {
        // This function is for debugging purposes only, it does not show
        // The 0's to the right of the number
        // The max number in an u32 is: 4,294,967,295
        let mut buff = [0; 10];
        let len = num_chars(num, &mut buff);
        self.write_str(&core::str::from_utf8(&buff[..len]).unwrap());
    }
}

fn last_bit(n: u8) -> bool {
    (n & 1) != 0
}

fn num_rev(mut num: u32) -> u32 {
    let mut rev_num = 0;
    while num > 0 {
        let digit = num % 10;
        rev_num = rev_num * 10 + digit;
        num /= 10;
    }
    rev_num
}

fn num_chars(n: u32, output: &mut [u8]) -> usize {
    let mut n = num_rev(n);
    let mut i = 0;
    while n > 0 {
        output[i] = (n % 10) as u8 + '0' as u8;
        n = n / 10;
        i += 1;
    }
    i
}