use crate::bail;
use crate::fat_error::{ContextExt, FatResult};
use crate::hal::Box;
use crate::hal::{DetectionResult, Moistures, Sensor};
use crate::log::{LogMessage, LOG_ACCESS};
use alloc::format;
use alloc::string::ToString;
use async_trait::async_trait;
use canapi::id::{classify, plant_id, MessageKind, IDENTIFY_CMD_OFFSET};
use canapi::SensorSlot;
use embassy_embedded_hal::shared_bus::blocking::i2c::I2cDevice;
use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
use embassy_time::{Duration, Timer, WithTimeout};
use embedded_can::{Frame, Id};
use esp_hal::gpio::Output;
use esp_hal::i2c::master::I2c;
use esp_hal::pcnt::unit::Unit;
use esp_hal::twai::{EspTwaiFrame, StandardId, Twai, TwaiConfiguration};
use esp_hal::{Async, Blocking};
use log::{error, info, warn};
use pca9535::{GPIOBank, Pca9535Immediate, StandardExpanderInterface};

const REPEAT_MOIST_MEASURE: usize = 10;

#[async_trait(?Send)]
pub trait SensorInteraction {
    async fn measure_moisture_hz(&mut self) -> FatResult<Moistures>;
}

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 {
    PulseCounter {
        signal_counter: Unit<'static, 0>,
        sensor_expander:
            Pca9535Immediate<I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Blocking>>>,
    },
    CanBus {
        twai_config: Option<TwaiConfiguration<'static, Blocking>>,
        can_power: Output<'static>,
    },
}

#[async_trait(?Send)]
impl SensorInteraction for SensorImpl {
    async fn measure_moisture_hz(&mut self) -> FatResult<Moistures> {
        match self {
            SensorImpl::PulseCounter {
                signal_counter,
                sensor_expander,
                ..
            } => {
                let mut result = Moistures::default();
                for plant in 0..crate::hal::PLANT_COUNT {
                    result.sensor_a_hz[plant] =
                        Self::inner_pulse(plant, Sensor::A, signal_counter, sensor_expander)
                            .await?;
                    info!(
                        "Sensor {} {:?}: {}",
                        plant,
                        Sensor::A,
                        result.sensor_a_hz[plant]
                    );
                    result.sensor_b_hz[plant] =
                        Self::inner_pulse(plant, Sensor::B, signal_counter, sensor_expander)
                            .await?;
                    info!(
                        "Sensor {} {:?}: {}",
                        plant,
                        Sensor::B,
                        result.sensor_b_hz[plant]
                    );
                }
                Ok(result)
            }

            SensorImpl::CanBus {
                twai_config,
                can_power,
            } => {
                can_power.set_high();
                let config = twai_config.take().expect("twai config not set");
                let mut twai = config.into_async().start();

                loop {
                    let rec = twai.receive();
                    match rec {
                        Ok(_) => {}
                        Err(err) => {
                            info!("Error receiving CAN message: {err:?}");
                            break;
                        }
                    }
                }

                Timer::after_millis(10).await;

                let mut moistures = Moistures::default();
                let _ = Self::wait_for_can_measurements(&mut twai, &mut moistures)
                    .with_timeout(Duration::from_millis(5000))
                    .await;

                can_power.set_low();

                let config = twai.stop().into_blocking();
                twai_config.replace(config);

                Ok(moistures)
            }
        }
    }
}

impl SensorImpl {
    pub async fn autodetect(&mut self) -> FatResult<DetectionResult> {
        match self {
            SensorImpl::PulseCounter { .. } => {
                bail!("Only CAN bus implementation supports autodetection")
            }
            SensorImpl::CanBus {
                twai_config,
                can_power,
            } => {
                // Power on CAN transceiver and start controller
                can_power.set_high();
                let config = twai_config.take().expect("twai config not set");
                let mut as_async = config.into_async().start();
                // Give CAN some time to stabilize
                Timer::after_millis(10).await;

                // Send a few test messages per potential sensor node
                for plant in 0..crate::hal::PLANT_COUNT {
                    for sensor in [Sensor::A, Sensor::B] {
                        let target = StandardId::new(plant_id(
                            IDENTIFY_CMD_OFFSET,
                            sensor.into(),
                            plant as u16,
                        ))
                        .context(">> Could not create address for sensor! (plant: {}) <<")?;
                        let can_buffer = [0_u8; 0];
                        if let Some(frame) = EspTwaiFrame::new(target, &can_buffer) {
                            // Try a few times; we intentionally ignore rx here and rely on stub logic
                            let resu = as_async.transmit_async(&frame).await;
                            match resu {
                                Ok(_) => {
                                    info!("Sent test message to plant {plant} sensor {sensor:?}");
                                }
                                Err(err) => {
                                    info!(
                                        "Error sending test message to plant {plant} sensor {sensor:?}: {err:?}"
                                    );
                                }
                            }
                        } else {
                            info!("Error building CAN frame");
                        }
                    }
                }

                let mut moistures = Moistures::default();
                let _ = Self::wait_for_can_measurements(&mut as_async, &mut moistures)
                    .with_timeout(Duration::from_millis(5000))
                    .await;

                let config = as_async.stop().into_blocking();
                can_power.set_low();
                twai_config.replace(config);

                let result = moistures.into();

                info!("Autodetection result: {result:?}");
                Ok(result)
            }
        }
    }

    async fn wait_for_can_measurements(
        as_async: &mut Twai<'_, Async>,
        moistures: &mut Moistures,
    ) -> FatResult<()> {
        loop {
            match as_async.receive_async().await {
                Ok(can_frame) => match can_frame.id() {
                    Id::Standard(id) => {
                        info!("Received CAN message: {id:?}");
                        let rawid = id.as_raw();
                        match classify(rawid) {
                            None => {}
                            Some(msg) => {
                                info!(
                                    "received message of kind {:?} (plant: {}, sensor: {:?})",
                                    msg.0, msg.1, msg.2
                                );
                                if msg.0 == MessageKind::MoistureData {
                                    let plant = msg.1 as usize;
                                    let sensor = msg.2;
                                    let data = can_frame.data();

                                    match sensor {
                                        SensorSlot::A => {
                                            moistures.sensor_a_hz[plant] = data[0] as f32;
                                        }
                                        SensorSlot::B => {
                                            moistures.sensor_b_hz[plant] = data[0] as f32;
                                        }
                                    }
                                }
                            }
                        }
                    }
                    Id::Extended(ext) => {
                        warn!("Received extended ID: {ext:?}");
                    }
                },
                Err(err) => {
                    error!("Error receiving CAN message: {err:?}");
                }
            }
        }
    }

    pub async fn inner_pulse(
        plant: usize,
        sensor: Sensor,
        signal_counter: &mut Unit<'_, 0>,
        sensor_expander: &mut Pca9535Immediate<
            I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Blocking>>,
        >,
    ) -> FatResult<f32> {
        let mut results = [0_f32; REPEAT_MOIST_MEASURE];
        for sample in results.iter_mut() {
            signal_counter.pause();
            signal_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 measurement = 100; // TODO what is this scaling factor? what is its purpose?
            let factor = 1000f32 / measurement as f32;

            //give some time to stabilize
            Timer::after_millis(10).await;
            signal_counter.resume();
            Timer::after_millis(measurement).await;
            signal_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)?;
            Timer::after_millis(10).await;
            let unscaled = 1337; //signal_counter.get_counter_value()? as i32;
            let hz = unscaled as f32 * factor;
            LOG_ACCESS
                .lock()
                .await
                .log(
                    LogMessage::RawMeasure,
                    unscaled as u32,
                    hz as u32,
                    &plant.to_string(),
                    &format!("{sensor:?}"),
                )
                .await;
            *sample = 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];
        Ok(median)
    }
}

impl From<Moistures> for DetectionResult {
    fn from(value: Moistures) -> Self {
        let mut result = DetectionResult::default();
        for (plant, sensor) in value.sensor_a_hz.iter().enumerate() {
            result.plant[plant].sensor_a = *sensor > 1.0_f32;
        }
        for (plant, sensor) in value.sensor_b_hz.iter().enumerate() {
            result.plant[plant].sensor_b = *sensor > 1.0_f32;
        }
        result
    }
}