use crate::bail;
use crate::config::PlantControllerConfig;
use crate::fat_error::{ContextExt, FatError, FatResult};
use crate::hal::battery::BatteryInteraction;
use crate::hal::esp::{hold_disable, hold_enable, Esp};
use crate::hal::rtc::RTCModuleInteraction;
use crate::hal::water::TankSensor;
use crate::hal::{
    BoardInteraction, DetectionResult, FreePeripherals, Moistures, Sensor, I2C_DRIVER, PLANT_COUNT,
    TIME_ACCESS,
};
use crate::log::{LogMessage, LOG_ACCESS};
use alloc::boxed::Box;
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::{Flex, Input, InputConfig, Level, Output, OutputConfig, Pull};
use esp_hal::i2c::master::I2c;
use esp_hal::twai::{EspTwaiError, EspTwaiFrame, StandardId, Twai, TwaiConfiguration, TwaiMode};
use esp_hal::{twai, Async, Blocking};
use esp_println::println;
use ina219::address::{Address, Pin};
use ina219::calibration::UnCalibrated;
use ina219::configuration::{Configuration, OperatingMode, Resolution};
use ina219::SyncIna219;
use log::{error, info, warn};
use measurements::Resistance;
use measurements::{Current, Voltage};
use pca9535::{GPIOBank, Pca9535Immediate, StandardExpanderInterface};

const MPPT_CURRENT_SHUNT_OHMS: f64 = 0.05_f64;
const TWAI_BAUDRATE: twai::BaudRate = twai::BaudRate::B125K;

pub enum Charger<'a> {
    SolarMpptV1 {
        mppt_ina: SyncIna219<
            I2cDevice<'a, CriticalSectionRawMutex, I2c<'static, Blocking>>,
            UnCalibrated,
        >,
        solar_is_day: Input<'a>,
        charge_indicator: Output<'a>,
    },
    ErrorInit {},
}

impl<'a> Charger<'a> {
    pub(crate) fn get_mppt_current(&mut self) -> FatResult<Current> {
        match self {
            Charger::SolarMpptV1 { mppt_ina, .. } => {
                let v = mppt_ina.shunt_voltage()?;
                let shunt_voltage = Voltage::from_microvolts(v.shunt_voltage_uv().abs() as f64);
                let shut_value = Resistance::from_ohms(MPPT_CURRENT_SHUNT_OHMS);
                let current = shunt_voltage.as_volts() / shut_value.as_ohms();
                Ok(Current::from_amperes(current))
            }
            Charger::ErrorInit { .. } => {
                bail!("hardware error during init");
            }
        }
    }

    pub(crate) fn get_mptt_voltage(&mut self) -> FatResult<Voltage> {
        match self {
            Charger::SolarMpptV1 { mppt_ina, .. } => {
                let v = mppt_ina.bus_voltage()?;
                Ok(Voltage::from_millivolts(v.voltage_mv() as f64))
            }
            Charger::ErrorInit { .. } => {
                bail!("hardware error during init");
            }
        }
    }
}

impl Charger<'_> {
    pub(crate) fn power_save(&mut self) {
        if let Charger::SolarMpptV1 { mppt_ina, .. } = self {
            let _ = mppt_ina
                .set_configuration(Configuration {
                    reset: Default::default(),
                    bus_voltage_range: Default::default(),
                    shunt_voltage_range: Default::default(),
                    bus_resolution: Default::default(),
                    shunt_resolution: Default::default(),
                    operating_mode: OperatingMode::PowerDown,
                })
                .map_err(|e| {
                    info!(
                        "Error setting ina mppt configuration during deep sleep preparation{e:?}"
                    );
                });
        }
    }
    fn set_charge_indicator(&mut self, charging: bool) -> FatResult<()> {
        if let Self::SolarMpptV1 {
            charge_indicator, ..
        } = self
        {
            charge_indicator.set_level(charging.into());
        }
        Ok(())
    }

    fn is_day(&self) -> bool {
        match self {
            Charger::SolarMpptV1 { solar_is_day, .. } => solar_is_day.is_high(),
            _ => true,
        }
    }
}

pub struct V4<'a> {
    esp: Esp<'a>,
    tank_sensor: TankSensor<'a>,
    charger: Charger<'a>,
    rtc_module: Box<dyn RTCModuleInteraction + Send>,
    battery_monitor: Box<dyn BatteryInteraction + Send>,
    config: PlantControllerConfig,

    awake: Output<'a>,
    light: Output<'a>,
    general_fault: Output<'a>,
    pump_expander: Pca9535Immediate<I2cDevice<'a, CriticalSectionRawMutex, I2c<'static, Blocking>>>,
    pump_ina: Option<
        SyncIna219<I2cDevice<'a, CriticalSectionRawMutex, I2c<'static, Blocking>>, UnCalibrated>,
    >,
    can_power: Output<'static>,

    extra1: Output<'a>,
    extra2: Output<'a>,
    twai_config: Option<TwaiConfiguration<'static, Blocking>>
}


pub(crate) async fn create_v4(
    peripherals: FreePeripherals<'static>,
    esp: Esp<'static>,
    config: PlantControllerConfig,
    battery_monitor: Box<dyn BatteryInteraction + Send>,
    rtc_module: Box<dyn RTCModuleInteraction + Send>,
) -> Result<Box<dyn BoardInteraction<'static> + Send + 'static>, FatError> {
    info!("Start v4");
    let mut awake = Output::new(peripherals.gpio21, Level::High, OutputConfig::default());
    awake.set_high();

    let mut general_fault = Output::new(peripherals.gpio23, Level::Low, OutputConfig::default());
    general_fault.set_low();

    let twai_config = Some(TwaiConfiguration::new(
        peripherals.twai,
        peripherals.gpio0,
        peripherals.gpio2,
        TWAI_BAUDRATE,
        TwaiMode::Normal,
    ));

    let extra1 = Output::new(peripherals.gpio6, Level::Low, OutputConfig::default());
    let extra2 = Output::new(peripherals.gpio15, Level::Low, OutputConfig::default());

    let one_wire_pin = Flex::new(peripherals.gpio18);
    let tank_power_pin = Output::new(peripherals.gpio11, Level::Low, OutputConfig::default());
    let flow_sensor_pin = Input::new(
        peripherals.gpio4,
        InputConfig::default().with_pull(Pull::Up),
    );

    let tank_sensor = TankSensor::create(
        one_wire_pin,
        peripherals.adc1,
        peripherals.gpio5,
        tank_power_pin,
        flow_sensor_pin,
        peripherals.pcnt1,
    )?;

    let can_power = Output::new(peripherals.gpio22, Level::Low, OutputConfig::default());

    let solar_is_day = Input::new(peripherals.gpio7, InputConfig::default());
    let light = Output::new(peripherals.gpio10, Level::Low, Default::default());
    let charge_indicator = Output::new(peripherals.gpio3, Level::Low, Default::default());

    info!("Start pump expander");
    let pump_device = I2cDevice::new(I2C_DRIVER.get().await);
    let mut pump_expander = Pca9535Immediate::new(pump_device, 32);
    for pin in 0..8 {
        let _ = pump_expander.pin_into_output(GPIOBank::Bank0, pin);
        let _ = pump_expander.pin_into_output(GPIOBank::Bank1, pin);
        let _ = pump_expander.pin_set_low(GPIOBank::Bank0, pin);
        let _ = pump_expander.pin_set_low(GPIOBank::Bank1, pin);
    }

    info!("Start mppt");
    let mppt_current = I2cDevice::new(I2C_DRIVER.get().await);
    let mppt_ina = match SyncIna219::new(mppt_current, Address::from_pins(Pin::Vcc, Pin::Gnd)) {
        Ok(mut ina) => {
            // Prefer higher averaging for more stable readings
            let _ = ina.set_configuration(Configuration {
                reset: Default::default(),
                bus_voltage_range: Default::default(),
                shunt_voltage_range: Default::default(),
                bus_resolution: Default::default(),
                shunt_resolution: Resolution::Avg128,
                operating_mode: Default::default(),
            });
            Some(ina)
        }
        Err(err) => {
            info!("Error creating mppt ina: {err:?}");
            None
        }
    };

    info!("Start pump current sensor");
    let pump_current_dev = I2cDevice::new(I2C_DRIVER.get().await);
    let pump_ina = match SyncIna219::new(pump_current_dev, Address::from_pins(Pin::Gnd, Pin::Sda)) {
        Ok(ina) => Some(ina),
        Err(err) => {
            info!("Error creating pump ina: {err:?}");
            None
        }
    };

    let charger = match mppt_ina {
        Some(mut mppt_ina) => {
            mppt_ina.set_configuration(Configuration {
                reset: Default::default(),
                bus_voltage_range: Default::default(),
                shunt_voltage_range: Default::default(),
                bus_resolution: Default::default(),
                shunt_resolution: Resolution::Avg128,
                operating_mode: Default::default(),
            })?;

            Charger::SolarMpptV1 {
                mppt_ina,
                solar_is_day,
                charge_indicator,
            }
        }
        None => Charger::ErrorInit {},
    };

    info!("Assembling final v4 board interaction object");
    let v = V4 {
        rtc_module,
        esp,
        awake,
        tank_sensor,
        light,
        general_fault,
        pump_expander,
        config,
        battery_monitor,
        pump_ina,
        charger,
        extra1,
        extra2,
        can_power,
        twai_config
    };
    Ok(Box::new(v))
}


#[async_trait(?Send)]
impl<'a> BoardInteraction<'a> for V4<'a> {
    fn get_tank_sensor(&mut self) -> Result<&mut TankSensor<'a>, FatError> {
        Ok(&mut self.tank_sensor)
    }

    fn get_esp(&mut self) -> &mut Esp<'a> {
        &mut self.esp
    }

    fn get_config(&mut self) -> &PlantControllerConfig {
        &self.config
    }

    fn get_battery_monitor(&mut self) -> &mut Box<dyn BatteryInteraction + Send> {
        &mut self.battery_monitor
    }

    fn get_rtc_module(&mut self) -> &mut Box<dyn RTCModuleInteraction + Send> {
        &mut self.rtc_module
    }

    async fn set_charge_indicator(&mut self, charging: bool) -> Result<(), FatError> {
        self.charger.set_charge_indicator(charging)
    }

    async fn deep_sleep(&mut self, duration_in_ms: u64) -> ! {
        self.awake.set_low();
        self.charger.power_save();
        let rtc = TIME_ACCESS.get().await.lock().await;
        self.esp.deep_sleep(duration_in_ms, rtc);
    }

    fn is_day(&self) -> bool {
        self.charger.is_day()
    }

    async fn light(&mut self, enable: bool) -> Result<(), FatError> {
        hold_disable(10);
        self.light.set_level(enable.into());
        hold_enable(10);
        Ok(())
    }

    async fn pump(&mut self, plant: usize, enable: bool) -> FatResult<()> {
        if enable {
            self.pump_expander
                .pin_set_high(GPIOBank::Bank0, plant as u8)?;
        } else {
            self.pump_expander
                .pin_set_low(GPIOBank::Bank0, plant as u8)?;
        }
        Ok(())
    }

    async fn pump_current(&mut self, _plant: usize) -> Result<Current, FatError> {
        // sensor is shared for all pumps, ignore plant id
        match self.pump_ina.as_mut() {
            None => {
                bail!("pump current sensor not available");
            }
            Some(pump_ina) => {
                let v = pump_ina
                    .shunt_voltage()
                    .map_err(|e| FatError::String {
                        error: alloc::format!("{e:?}"),
                    })
                    .map(|v| {
                        let shunt_voltage =
                            Voltage::from_microvolts(v.shunt_voltage_uv().abs() as f64);
                        let shut_value = Resistance::from_ohms(0.05_f64);
                        let current = shunt_voltage.as_volts() / shut_value.as_ohms();
                        Current::from_amperes(current)
                    })?;
                Ok(v)
            }
        }
    }

    async fn fault(&mut self, plant: usize, enable: bool) -> FatResult<()> {
        if enable {
            self.pump_expander
                .pin_set_high(GPIOBank::Bank1, plant as u8)?;
        } else {
            self.pump_expander
                .pin_set_low(GPIOBank::Bank1, plant as u8)?;
        }
        Ok(())
    }
    async fn measure_moisture_hz(&mut self) -> FatResult<Moistures> {
        self.can_power.set_high();
        let config = self.twai_config.take().expect("twai config not set");
        let mut twai = config.into_async().start();

        Timer::after_millis(10).await;

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

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

        self.can_power.set_low();
        Ok(moistures)
    }

    async fn detect_sensors(&mut self) -> FatResult<DetectionResult> {
        self.can_power.set_high();
        let config = self.twai_config.take().expect("twai config not set");
        let mut twai = config.into_async().start();

        Timer::after_millis(1000).await;
        info!("Sending info messages now");
        // Send a few test messages per potential sensor node
        for plant in 0..PLANT_COUNT {
            for sensor in [Sensor::A, Sensor::B] {
                let target =
                    StandardId::new(plant_id(IDENTIFY_CMD_OFFSET, sensor.into(), (plant +1) as u16))
                        .context(">> Could not create address for sensor! (plant: {}) <<")?;
                let can_buffer = [0_u8; 0];
                info!("Sending test message to plant {} sensor {sensor:?} with id {}", plant +1, target.as_raw());
                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 = twai
                        .transmit_async(&frame)
                        .with_timeout(Duration::from_millis(3000))
                        .await;
                    match resu {
                        Ok(_) => {
                        }
                        Err(err) => {
                            info!(
                                        "Error sending test message to plant {} sensor {sensor:?}: {err:?}", plant +1
                                    );
                        }
                    }
                } else {
                    info!("Error building CAN frame");
                }
            }
        }

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


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

        self.can_power.set_low();


        let result = moistures.into();

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


    async fn general_fault(&mut self, enable: bool) {
        hold_disable(23);
        self.general_fault.set_level(enable.into());
        hold_enable(23);
    }

    async fn test(&mut self) -> Result<(), FatError> {
        self.general_fault(true).await;
        Timer::after_millis(100).await;
        self.general_fault(false).await;
        Timer::after_millis(500).await;
        self.extra1.set_high();
        Timer::after_millis(500).await;
        self.extra1.set_low();
        Timer::after_millis(500).await;
        self.extra2.set_high();
        Timer::after_millis(500).await;
        self.extra2.set_low();
        Timer::after_millis(500).await;
        self.light(true).await?;
        Timer::after_millis(500).await;
        self.light(false).await?;
        Timer::after_millis(500).await;
        for i in 0..PLANT_COUNT {
            self.fault(i, true).await?;
            Timer::after_millis(500).await;
            self.fault(i, false).await?;
            Timer::after_millis(500).await;
        }
        for i in 0..PLANT_COUNT {
            self.pump(i, true).await?;
            Timer::after_millis(100).await;
            self.pump(i, false).await?;
            Timer::after_millis(100).await;
        }
        let moisture = self.measure_moisture_hz().await?;
        for plant in 0..PLANT_COUNT {
            let a = moisture.sensor_a_hz[plant].unwrap_or(0.0) as u32;
            let b = moisture.sensor_b_hz[plant].unwrap_or(0.0) as u32;
            LOG_ACCESS
                .lock()
                .await
                .log(LogMessage::TestSensor, a, b, &(plant+1).to_string(), "")
                .await;
        }
        Timer::after_millis(10).await;
        Ok(())
    }

    fn set_config(&mut self, config: PlantControllerConfig) {
        self.config = config;
    }

    async fn get_mptt_voltage(&mut self) -> FatResult<Voltage> {
        self.charger.get_mptt_voltage()
    }

    async fn get_mptt_current(&mut self) -> FatResult<Current> {
        self.charger.get_mppt_current()
    }

    async fn can_power(&mut self, state: bool) -> FatResult<()> {
        if state && self.can_power.is_set_low() {
            self.can_power.set_high();
        } else {
            self.can_power.set_low();
        }
        Ok(())
    }
}


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();
                                info!("Received moisture data: {:?}", data);
                                if let Ok(bytes) = data.try_into() {
                                    let frequency = u32::from_be_bytes(bytes);
                                    match sensor {
                                        SensorSlot::A => {
                                            moistures.sensor_a_hz[plant-1] = Some(frequency as f32);
                                        }
                                        SensorSlot::B => {
                                            moistures.sensor_b_hz[plant-1] = Some(frequency as f32);
                                        }
                                    }
                                } else {
                                    error!("Received moisture data with invalid length: {} (expected 4)", data.len());
                                }
                            }
                        }
                    }
                }
                Id::Extended(ext) => {
                    warn!("Received extended ID: {ext:?}");
                }
            },
            Err(err) => {
                match err {
                    EspTwaiError::BusOff => {
                        bail!("Bus offline")
                    }
                    EspTwaiError::NonCompliantDlc(_) => {}
                    EspTwaiError::EmbeddedHAL(_) => {}
                }
                error!("Error receiving CAN message: {err:?}");
            }
        }
    }
}

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.is_some();
        }
        for (plant, sensor) in value.sensor_b_hz.iter().enumerate() {
            result.plant[plant].sensor_b = sensor.is_some();
        }
        result
    }
}