use crate::hal::Sensor;
use crate::log::{log, LogMessage};
use alloc::string::ToString;
use embedded_hal_bus::i2c::MutexDevice;
use esp_idf_hal::can::CanDriver;
use esp_idf_hal::delay::Delay;
use esp_idf_hal::i2c::I2cDriver;
use esp_idf_hal::pcnt::PcntDriver;
use pca9535::{GPIOBank, Pca9535Immediate, StandardExpanderInterface};

const REPEAT_MOIST_MEASURE: usize = 10;

pub trait SensorInteraction {
    async fn measure_moisture_hz(&mut self, plant: usize, sensor: Sensor) -> anyhow::Result<f32>;
}

const MS0: u8 = 1_u8;
const MS1: u8 = 0_u8;
const MS2: u8 = 3_u8;
const MS3: u8 = 4_u8;
const MS4: u8 = 2_u8;
const SENSOR_ON: u8 = 5_u8;

pub enum SensorImpl<'a> {
    PulseCounter {
        signal_counter: PcntDriver<'a>,
        sensor_expander: Pca9535Immediate<MutexDevice<'a, I2cDriver<'a>>>,
    },
    CanBus {
        can: CanDriver<'a>,
    },
}

impl SensorInteraction for SensorImpl<'_> {
    fn measure_moisture_hz(&mut self, plant: usize, sensor: Sensor) -> anyhow::Result<f32> {
        match self {
            SensorImpl::PulseCounter {
                signal_counter,
                sensor_expander,
                ..
            } => {
                let mut results = [0_f32; REPEAT_MOIST_MEASURE];
                for repeat in 0..REPEAT_MOIST_MEASURE {
                    signal_counter.counter_pause()?;
                    signal_counter.counter_clear()?;

                    //Disable all
                    sensor_expander.pin_set_high(GPIOBank::Bank0, MS4)?;

                    let sensor_channel = match sensor {
                        Sensor::A => plant as u32,
                        Sensor::B => (15 - plant) as u32,
                    };

                    let is_bit_set = |b: u8| -> bool { sensor_channel & (1 << b) != 0 };
                    if is_bit_set(0) {
                        sensor_expander.pin_set_high(GPIOBank::Bank0, MS0)?;
                    } else {
                        sensor_expander.pin_set_low(GPIOBank::Bank0, MS0)?;
                    }
                    if is_bit_set(1) {
                        sensor_expander.pin_set_high(GPIOBank::Bank0, MS1)?;
                    } else {
                        sensor_expander.pin_set_low(GPIOBank::Bank0, MS1)?;
                    }
                    if is_bit_set(2) {
                        sensor_expander.pin_set_high(GPIOBank::Bank0, MS2)?;
                    } else {
                        sensor_expander.pin_set_low(GPIOBank::Bank0, MS2)?;
                    }
                    if is_bit_set(3) {
                        sensor_expander.pin_set_high(GPIOBank::Bank0, MS3)?;
                    } else {
                        sensor_expander.pin_set_low(GPIOBank::Bank0, MS3)?;
                    }

                    sensor_expander.pin_set_low(GPIOBank::Bank0, MS4)?;
                    sensor_expander.pin_set_high(GPIOBank::Bank0, SENSOR_ON)?;

                    let delay = Delay::new_default();
                    let measurement = 100; // TODO what is this scaling factor? what is its purpose?
                    let factor = 1000f32 / measurement as f32;

                    //give some time to stabilize
                    delay.delay_ms(10);
                    signal_counter.counter_resume()?;
                    delay.delay_ms(measurement);
                    signal_counter.counter_pause()?;
                    sensor_expander.pin_set_high(GPIOBank::Bank0, MS4)?;
                    sensor_expander.pin_set_low(GPIOBank::Bank0, SENSOR_ON)?;
                    sensor_expander.pin_set_low(GPIOBank::Bank0, MS0)?;
                    sensor_expander.pin_set_low(GPIOBank::Bank0, MS1)?;
                    sensor_expander.pin_set_low(GPIOBank::Bank0, MS2)?;
                    sensor_expander.pin_set_low(GPIOBank::Bank0, MS3)?;
                    delay.delay_ms(10);
                    let unscaled = signal_counter.get_counter_value()? as i32;
                    let hz = unscaled as f32 * factor;
                    log(
                        LogMessage::RawMeasure,
                        unscaled as u32,
                        hz as u32,
                        &plant.to_string(),
                        &format!("{sensor:?}"),
                    );
                    results[repeat] = hz;
                }
                results.sort_by(|a, b| a.partial_cmp(b).unwrap()); // floats don't seem to implement total_ord

                let mid = results.len() / 2;
                let median = results[mid];
                anyhow::Ok(median)
            }
            SensorImpl::CanBus { .. } => {
                todo!()
            }
        }
    }
}