use canapi::id::{classify, plant_id, MessageKind, IDENTIFY_CMD_OFFSET};
use crate::bail;
use crate::fat_error::{ContextExt, FatError, FatResult};
use canapi::{SensorSlot};
use crate::hal::{DetectionResult, Moistures, Sensor};
use crate::hal::Box;
use crate::log::{LogMessage, LOG_ACCESS};
use alloc::format;
use alloc::string::ToString;
use async_trait::async_trait;
use bincode::config;
use embassy_embedded_hal::shared_bus::blocking::i2c::I2cDevice;
use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
use embassy_time::{Duration, Instant, 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.start();

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

                Timer::after_millis(10).await;
                let can = Self::inner_can(&mut twai).await;

                can_power.set_low();

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

                let value = can?;
                Ok(value)
            }
        }
    }
}



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 {} sensor {:?}",
                                        plant, sensor
                                    );
                                }
                                Err(err) => {
                                    info!("Error sending test message to plant {} sensor {:?}: {:?}", plant, sensor, err);
                                }
                            }
                        } else {
                            info!("Error building CAN frame");
                        }

                    }
                }

                let mut result = DetectionResult::default();
                // Wait for messages to arrive
                let _ = Self::wait_for_can_measurements(&mut as_async, &mut result).with_timeout(Duration::from_millis(5000)).await;

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

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

    async fn wait_for_can_measurements(as_async: &mut Twai<'_, Async>, result: &mut DetectionResult) {
        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;
                                        match sensor {
                                            SensorSlot::A => {
                                                result.plant[plant].sensor_a = true;
                                            }
                                            SensorSlot::B => {
                                                result.plant[plant].sensor_b = true;
                                            }
                                        }
                                    }
                                }
                            }
                        }
                        Id::Extended(ext) => {
                            warn!("Received extended ID: {:?}", ext);
                        }
                    }
                }
                Err(err) => {
                    error!("Error receiving CAN message: {:?}", err);
                    break;
                }
            }
        }
    }

    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 repeat in 0..REPEAT_MOIST_MEASURE {
    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;
    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];
    Ok(median)
}

    async fn inner_can(
        twai: &mut Twai<'static, Blocking>,
    ) -> FatResult<Moistures> {
        [0_u8; 8];
        config::standard();

        let timeout = Instant::now()
            .checked_add(embassy_time::Duration::from_millis(100))
            .context("Timeout")?;
        loop {
            let answer = twai.receive();
            match answer {
                Ok(answer) => {
                    info!("Received CAN message: {:?}", answer);
                }
                Err(error) => match error {
                    nb::Error::Other(error) => {
                        return Err(FatError::CanBusError { error });
                    }
                    nb::Error::WouldBlock => {
                        if Instant::now() > timeout {
                            bail!("Timeout waiting for CAN answer");
                        }
                        Timer::after_millis(10).await;
                    }
                },
            }
        }
    }
}