mod pwm;

use crate::hardware::Hardware;
use crate::hardware::mcp3208::Mcp3208;
use crate::hardware::mcp23017::Mcp23017Driver;
use crate::hardware::raspi::pwm::PwmWrapper;
use anyhow::{anyhow, Result};
use embedded_hal_bus::i2c::MutexDevice;
use log::{debug, info, trace};
use rpi_pal::i2c::I2c;
use rpi_pal::pwm::Pwm;
use rpi_pal::spi::SimpleHalSpiDevice;
use rpi_pal::spi::{Bus, Mode, SlaveSelect, Spi};
use std::cell::RefCell;
use std::sync::Mutex;
use crate::hardware::a4963::{A4963Driver, A4963};
use crate::hardware::channelization::PininChannel;

const CLOCK_1MHZ: u32 = 1_000_000;

#[derive(Debug)]
pub struct RaspiHardware {
    // gpio: Gpio,
    // fault: InputPin,
    // fg: InputPin,
    i2c_bus: Mutex<I2c>,
    mcp3208: RefCell<Mcp3208<SimpleHalSpiDevice>>,
    // a4963: RefCell<A4963Driver<SimpleHalSpiDevice>>,
    a4963: Option<A4963Driver<SimpleHalSpiDevice>>,
    // mct8316a: Mutex<SimMct8316a>,
}

impl RaspiHardware {
    pub fn new() -> Result<Self> {
        trace!("RaspiHardware::new()");

        let device = rpi_pal::system::DeviceInfo::new()?;
        info!("Running on {}", device.model());
        debug!("SOC: {}", device.soc());

        // let gpio = Gpio::new()?;

        // let mut fault = gpio.get(25)?.into_input();
        // fault.set_reset_on_drop(false);
        // let mut fg = gpio.get(24)?.into_input();
        // fg.set_reset_on_drop(false);

        Ok(Self {
            // gpio,
            // fault,
            // fg,
            i2c_bus: Mutex::new(I2c::with_bus(0u8)?),
            mcp3208: Mcp3208::new(
                SimpleHalSpiDevice::new(Spi::new(
                    Bus::Spi1,
                    SlaveSelect::Ss1,
                    CLOCK_1MHZ,
                    Mode::Mode0,
                )?),
                3.3f64,
            ).into(),
            a4963: Some(A4963Driver::new(SimpleHalSpiDevice::new(Spi::new(
                Bus::Spi0,
                SlaveSelect::Ss0,
                CLOCK_1MHZ,
                Mode::Mode3,
            )?))),
            // mct8316a: Mct8316AVDriver::new().into(),
            // mct8316a: SimMct8316a::new().into(),
        })
    }
}

impl Hardware for RaspiHardware {
    type Mcp23017<'a> = Mcp23017Driver<MutexDevice<'a, I2c>>;
    type Pwm = PwmWrapper;

    fn new_mcp23017_a(&self) -> Result<Self::Mcp23017<'_>> {
        trace!("RaspiHardware::new_mcp23017_a(self: {self:?})");
        Ok(Mcp23017Driver::new(
            MutexDevice::new(&self.i2c_bus),
            0b0100000,
        ))
    }

    fn new_mcp23017_b(&self) -> Result<Self::Mcp23017<'_>> {
        trace!("RaspiHardware::new_mcp23017_b(self: {self:?})");
        Ok(Mcp23017Driver::new(
            MutexDevice::new(&self.i2c_bus),
            0b0100001,
        ))
    }

    fn new_pwm0(&self) -> Result<Self::Pwm> {
        trace!("RaspiHardware::new_pwm0(self: {self:?})");
        // Unfortunately the current version of rpi_pal assumes an older version
        // of the kernel where pwmchip for RPi5 was 2
        const PWMCHIP: u8 = 0;
        const CHANNEL: u8 = 0;
        Ok(PwmWrapper::new(Pwm::with_pwmchip(PWMCHIP, CHANNEL)?)?)
    }

    // fn new_mct8316a(&self) -> Result<impl Mct8316a + Sync> {
    //     trace!("RaspiHardware::new_mct8316a(self: {self:?})");
    //     Ok(Mct8316AVDriver::new(
    //         MutexDevice::new(&self.mct8316a),
    //         // MutexDevice::new(&self.i2c_bus),
    //         0b0000000,
    //     ))
    // }

    fn new_a4963<'a, 'b>(&'a mut self) -> Result<impl A4963 + Send + 'b> {
        trace!("RaspiHardware::new_a4963(self: {self:?})");
        Ok(self.a4963.take().ok_or_else(|| anyhow!("Cannot construct two A4963 instances"))?)
    }

    fn get_battery_voltage(&self) -> Result<f64> {
        const LOW_RESISTOR: f64 = 100.0;
        const HIGH_RESISTOR: f64 = 374.0;
        trace!("RaspiHardware::get_battery_voltage(self: {self:?})");
        let sensed = self.mcp3208.borrow_mut().read_single(PininChannel::BatterySense as u8)?;
        let battery = sensed * (HIGH_RESISTOR + LOW_RESISTOR) / LOW_RESISTOR;
        Ok(battery)
    }

    // fn get_fault(&self) -> Result<bool> {
    //     trace!("RaspiHardware::get_fault(self: {self:?})");
    //     Ok(self.fault.is_high())
    // }

    // fn get_fg(&self) -> Result<bool> {
    //     trace!("RaspiHardware::get_fg(self: {self:?})");
    //     Ok(self.fg.is_high())
    // }
}
