use bt_hci::cmd::SyncCmd;
use embassy_executor::Spawner;
use embassy_futures::join::join;
use embassy_nrf::{mode, rng};
use embassy_time::{Duration, Timer};
use nrf_mpsl::MultiprotocolServiceLayer;
use nrf_pac as pac;
use nrf_sdc::{SoftdeviceController, vendor::ZephyrWriteBdAddr};
use sachy_bthome::BtHomeAd;
use sachy_fmt::{info, panic, unwrap};
use static_cell::StaticCell;
use trouble_host::prelude::*;

use crate::{
    constants::{
        PARA_ADV_DURATION_SECS, PARA_BLE_TX_POWER, PARA_MAX_ADV_INTERVAL_MS,
        PARA_MIN_ADV_INTERVAL_MS, PARA_NAME,
    },
    state::{ADC_MEASUREMENT, SHTC3_MEASUREMENT, START_MEASUREMENTS},
};

type BleResources = HostResources<DefaultPacketPool, 0, 0>;
type BleStack = Stack<'static, nrf_sdc::SoftdeviceController<'static>, DefaultPacketPool>;
type BleControl = ControlRunner<'static, SoftdeviceController<'static>, DefaultPacketPool>;
type BlePeripheral = Peripheral<'static, nrf_sdc::SoftdeviceController<'static>, DefaultPacketPool>;

#[embassy_executor::task]
pub async fn task(
    spawner: Spawner,
    controller: nrf_sdc::SoftdeviceController<'static>,
    mpsl: &'static MultiprotocolServiceLayer<'static>,
) {
    spawner.spawn(unwrap!(mpsl_runner(mpsl)));

    let addr = build_addr();

    info!("Our address = {:?}", &addr);

    // Set the bluetooth address
    unwrap!(ZephyrWriteBdAddr::new(addr).exec(&controller).await);

    static RESOURCES: StaticCell<BleResources> = StaticCell::new();
    static STACK: StaticCell<BleStack> = StaticCell::new();

    let resources = RESOURCES.init_with(HostResources::new);
    let stack = STACK.init_with(|| trouble_host::new(controller, resources).build());

    // No need to run the TX/RX, since we are only ever transmitting adv packets
    let (_, control, _) = stack.runner().split();

    spawner.spawn(unwrap!(ble_control_runner(control)));
    spawner.spawn(unwrap!(ble_runner(stack.peripheral())));
}

#[embassy_executor::task]
async fn ble_runner(mut peripheral: BlePeripheral) -> ! {
    let mut start_measurements = unwrap!(START_MEASUREMENTS.receiver());

    loop {
        start_measurements.changed().await;

        ble_advertise(&mut peripheral).await;
    }
}

#[embassy_executor::task]
async fn mpsl_runner(mpsl: &'static MultiprotocolServiceLayer<'static>) -> ! {
    mpsl.run().await
}

#[embassy_executor::task]
async fn ble_control_runner(mut control: BleControl) -> ! {
    loop {
        if let Err(e) = control.run().await {
            panic!("BLE runner error: {:?}", e);
        }
    }
}

async fn ble_advertise(peripheral: &mut BlePeripheral) {
    let (adc, shtc3) = join(ADC_MEASUREMENT.wait(), SHTC3_MEASUREMENT.wait()).await;

    let params: AdvertisementParameters = AdvertisementParameters {
        interval_min: Duration::from_millis(PARA_MIN_ADV_INTERVAL_MS),
        interval_max: Duration::from_millis(PARA_MAX_ADV_INTERVAL_MS),
        tx_power: PARA_BLE_TX_POWER,
        primary_phy: PhyKind::Le1M,
        secondary_phy: PhyKind::LeCodedS2,
        timeout: Some(Duration::from_secs(PARA_ADV_DURATION_SECS)),
        ..Default::default()
    };

    let mut ad = BtHomeAd::default();

    let adv_data = ad
        .add_data(adc.battery)
        .add_data(shtc3.temperature)
        .add_data(adc.lux)
        .add_data(adc.voltage)
        .add_data(shtc3.humidity)
        .add_data(adc.moisture)
        .add_local_name(PARA_NAME)
        .encode();

    let set = [AdvertisementSet {
        address: None,
        params,
        data: Advertisement::NonconnectableNonscannableUndirected { adv_data },
    }];

    let mut handles = AdvertisementSet::handles(&set);

    info!("Starting advertising");
    let advertiser = unwrap!(peripheral.advertise_ext(&set, &mut handles).await);
    Timer::after_secs(PARA_ADV_DURATION_SECS).await;
    drop(advertiser);
    info!("Stopping advertising, sleeping...");
}

pub fn build_sdc<'d, const N: usize>(
    p: nrf_sdc::Peripherals<'d>,
    rng: &'d mut rng::Rng<mode::Async>,
    mpsl: &'d MultiprotocolServiceLayer,
    mem: &'d mut nrf_sdc::Mem<N>,
) -> Result<nrf_sdc::SoftdeviceController<'d>, nrf_sdc::Error> {
    nrf_sdc::Builder::new()?
        .support_adv()
        .support_ext_adv()
        .support_phy_update_peripheral()
        .support_le_2m_phy()
        .support_le_coded_phy()
        .build(p, rng, mpsl, mem)
}

fn build_addr() -> BdAddr {
    let ficr = pac::FICR;
    let high = u64::from(ficr.deviceid(1).read());
    let addr = high << 32 | u64::from(ficr.deviceid(0).read());
    let addr = addr | 0x0000_c000_0000_0000;
    BdAddr::new(unwrap!(addr.to_le_bytes()[..6].try_into()))
}