use embassy_rp::pio::{
    Common, Config, Instance, PioPin, ShiftConfig, ShiftDirection, StateMachine,
};
use embassy_time::Timer;
use fixed::traits::ToFixed;
use fixed_macro::types::U56F8;

use crate::{DhtError, DhtProgram};

pub struct DhtPio<'d, PIO: Instance, const SM: usize> {
    sm: StateMachine<'d, PIO, SM>,
}

impl<'d, PIO: Instance, const SM: usize> DhtPio<'d, PIO, SM> {
    pub fn new(
        dht_program: &DhtProgram<'d, PIO>,
        pio: &mut Common<'d, PIO>,
        mut sm: StateMachine<'d, PIO, SM>,
        dht_pin: impl PioPin,
    ) -> Self {
        let pin = pio.make_pio_pin(dht_pin);

        let mut config = Config::default();
        config.use_program(dht_program.program(), &[]);
        config.set_out_pins(&[&pin]);
        config.set_set_pins(&[&pin]);
        config.set_in_pins(&[&pin]);
        config.shift_in = ShiftConfig {
            threshold: 32,
            direction: ShiftDirection::Left,
            auto_fill: true,
        };
        config.shift_out = ShiftConfig {
            threshold: 32,
            direction: ShiftDirection::Left,
            auto_fill: false,
        };
        config.clock_divider = (U56F8!(125_000_000) / 1_000_000).to_fixed();
        sm.set_config(&config);

        sm.set_pin_dirs(embassy_rp::pio::Direction::Out, &[&pin]);
        sm.set_enable(true);

        Self { sm }
    }

    pub async fn read_data(&mut self) -> Result<(u16, u16), DhtError> {
        let mut timeout = 2000;
        let mut raw: [Option<u32>; 2] = [None; 2];

        self.sm.tx().wait_push(1).await;

        while timeout > 0 && raw[1].is_none() {
            match self.sm.rx().try_pull() {
                Some(d) => {
                    if raw[0].is_none() {
                        raw[0] = Some(d);
                    } else {
                        raw[1] = Some(d);
                    }
                }
                None => (),
            }

            Timer::after_millis(1).await;
            timeout -= 1;
        }

        if timeout <= 0 {
            self.sm.restart();
            return Err(DhtError::Timeout);
        }

        let data = raw[0].unwrap();
        let crc = raw[1].unwrap();

        if Self::compute_crc(data) != crc {
            return Err(DhtError::CrcMismatch(data, crc));
        }

        Ok((
            (data & 0x0000FFFF) as u16,
            ((data & 0xFFFF0000) >> 16) as u16,
        ))
    }

    fn compute_crc(data: u32) -> u32 {
        let mut crc: u32 = 0;
        crc += data & 0x000000FF;
        crc += (data & 0x0000FF00) >> 8;
        crc += (data & 0x00FF0000) >> 16;
        crc += (data & 0xFF000000) >> 24;

        crc % 256
    }
}

impl<'d, PIO: Instance, const SM: usize> Drop for DhtPio<'d, PIO, SM> {
    fn drop(&mut self) {
        self.sm.set_enable(false);
    }
}