use crate::hardware::error::WrappingError;
use crate::hardware::mcp23017::Mcp23017;
use anyhow::bail;
use embedded_hal::digital::PinState;
use embedded_hal::i2c::I2c;
use log::{error, trace};
use std::any::type_name;
use std::fmt::{Debug, Formatter};
use std::sync::Mutex;

pub struct Mcp23017Driver<I2C>
where
    I2C: I2c + Send + Sync,
    I2C::Error: Send,
    I2C::Error: Sync,
    I2C::Error: 'static,
{
    i2c: Mutex<I2C>,
    address: u8,
    bank: u16,
    last_flushed: Option<u16>,
}

impl<I2C> Debug for Mcp23017Driver<I2C>
where
    I2C: I2c + Send + Sync,
    I2C::Error: Send,
    I2C::Error: Sync,
    I2C::Error: 'static,
{
    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
        write!(
            f,
            "Mcp23017Driver<I2C={}> {{ address: {} }}",
            type_name::<I2C>(),
            self.address
        )
    }
}

impl<I2C> Mcp23017Driver<I2C>
where
    I2C: I2c + Send + Sync,
    I2C::Error: Send,
    I2C::Error: Sync,
    I2C::Error: 'static,
{
    pub fn new(i2c: I2C, address: u8) -> Self {
        trace!(
            "Mcp23017Driver<I2C={}>::new(i2c, address: 0x{address:02x})",
            type_name::<I2C>()
        );
        Self {
            i2c: i2c.into(),
            address,
            bank: 0,
            last_flushed: None,
        }
    }
}

impl<I2C> Drop for Mcp23017Driver<I2C>
where
    I2C: I2c + Send + Sync,
    I2C::Error: Send,
    I2C::Error: Sync,
    I2C::Error: 'static,
{
    fn drop(&mut self) {
        trace!("Mcp23017Driver::drop(self: {self:?})");
        self.bank = 0; // We want all pins to be set back to 0
        if let Err(e) = self.flush() {
            error!("Mcp23017Driver: Failed to flush on drop. {self:?} Error: {e}");
        }
    }
}

impl<I2C> Mcp23017 for Mcp23017Driver<I2C>
where
    I2C: I2c + Send + Sync,
    I2C::Error: Send,
    I2C::Error: Sync,
    I2C::Error: 'static,
{
    fn init(&mut self) -> anyhow::Result<()> {
        trace!("Mcp23017Driver::init(self: {self:?})");
        // Set each pin as an output (Addresses 0x00 & 0x01)
        let data: [u8; _] = [0x00, 0x00, 0x00];

        match self.i2c.get_mut() {
            Ok(lock) => lock.write(self.address, &data).map_err(WrappingError)?,
            Err(_) => bail!("Lock was poisoned"),
        }

        Ok(())
    }

    fn set_pin(&mut self, pin: u8, value: PinState) -> anyhow::Result<()> {
        trace!("Mcp23017Driver::set_pin(self: {self:?}, pin: {pin}, value: {value:?})");
        if !(0u8..16u8).contains(&pin) {
            bail!("Invalid Pin ID")
        }
        let pin_mask = 1u16 << pin;

        self.bank = match value {
            PinState::Low => self.bank & !pin_mask,
            PinState::High => self.bank | pin_mask,
        };

        Ok(())
    }

    fn flush(&mut self) -> anyhow::Result<()> {
        trace!("Mcp23017Driver::flush(self: {self:?})");
        if Some(self.bank) != self.last_flushed {
            let bytes = self.bank.to_le_bytes();
            let data: [u8; _] = [0x12, bytes[0], bytes[1]];
            // This blocks while writing
            if let Ok(mut lock) = self.i2c.lock() {
                lock.write(self.address, &data).map_err(WrappingError)?;
                self.i2c.clear_poison();
                self.last_flushed = Some(self.bank);
            }
        }
        Ok(())
    }
}
