use embedded_hal::pwm::{ErrorKind, ErrorType, SetDutyCycle};
use log::trace;
use rpi_pal::pwm::Pwm;
use std::fmt::{Display, Formatter};
use std::time::Duration;

const PWM_PERIOD: Duration = Duration::from_micros(1000); // 1kHz

pub struct PwmWrapper {
    pwm: Pwm,
}

impl PwmWrapper {
    pub fn new(mut pwm: Pwm) -> anyhow::Result<Self> {
        trace!("PwmWrapper::new(pwm: {pwm:?})");
        pwm.set_period(PWM_PERIOD)?;
        pwm.enable()?;
        pwm.set_reset_on_drop(true);
        Ok(Self {
            pwm
        })
    }
}

#[derive(Debug)]
pub struct ErrorWrapper(rpi_pal::pwm::Error);

impl embedded_hal::pwm::Error for ErrorWrapper {
    fn kind(&self) -> ErrorKind {
        ErrorKind::Other
    }
}

impl Display for ErrorWrapper {
    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
        self.0.fmt(f)
    }
}

impl std::error::Error for ErrorWrapper {}

impl ErrorType for PwmWrapper {
    type Error = ErrorWrapper;
}

impl SetDutyCycle for PwmWrapper {
    fn max_duty_cycle(&self) -> u16 {
        trace!("PwmWrapper::max_duty_cycle()");
        u16::MAX
    }

    fn set_duty_cycle(&mut self, duty: u16) -> Result<(), Self::Error> {
        trace!("PwmWrapper::set_duty_cycle(duty: {duty})");
        self.pwm.set_duty_cycle((duty as f64) / (u16::MAX as f64)).map_err(ErrorWrapper)
    }
}