#![no_std]
#![no_main]

use static_cell::StaticCell;

use core::mem;
use embassy_nrf::interrupt::{self, InterruptExt};
use embassy_nrf::peripherals::TWISPI0;
use embassy_time::Timer;
use nrf_softdevice::ble::advertisement_builder::{
    Flag, LegacyAdvertisementBuilder, LegacyAdvertisementPayload, ServiceList, ServiceUuid16,
};
use nrf_softdevice::ble::{gatt_server, peripheral, TxPower};
use nrf_softdevice::{raw, Softdevice};

use defmt::*;
use embassy_executor::Spawner;
use embassy_nrf::gpio::{Level, Output, OutputDrive};
use embassy_nrf::twim;
use embassy_nrf::{bind_interrupts, peripherals};
use {defmt_rtt as _, panic_probe as _};

//
//
// BLE code
//
//

#[nrf_softdevice::gatt_service(uuid = "181a")]
struct EnvironmentalService {
    #[characteristic(uuid = "2a6e", read, notify)]
    temperature: u32,
    #[characteristic(uuid = "2a6f", read, notify)]
    humidity: u32,
}

#[nrf_softdevice::gatt_server]
struct Server {
    bas: EnvironmentalService,
}

#[embassy_executor::task]
async fn softdevice_task(sd: &'static Softdevice) -> ! {
    sd.run().await
}

#[embassy_executor::task]
async fn ble_task(sd: &'static Softdevice, server: &'static Server) {
    static ADV_DATA: LegacyAdvertisementPayload = LegacyAdvertisementBuilder::new()
        .flags(&[Flag::GeneralDiscovery, Flag::LE_Only])
        .services_16(ServiceList::Complete, &[ServiceUuid16::BATTERY])
        .full_name("CommonSense")
        .build();

    static SCAN_DATA: LegacyAdvertisementPayload = LegacyAdvertisementBuilder::new()
        .services_128(
            ServiceList::Complete,
            &[0x9e7312e0_2354_11eb_9f10_fbc30a62cf38_u128.to_le_bytes()],
        )
        .build();

    loop {
        let config = peripheral::Config {
            tx_power: TxPower::Plus4dBm,
            ..Default::default()
        };

        let adv = peripheral::ConnectableAdvertisement::ScannableUndirected {
            adv_data: &ADV_DATA,
            scan_data: &SCAN_DATA,
        };

        let conn = unwrap!(peripheral::advertise_connectable(sd, adv, &config).await);

        let _e = gatt_server::run(&conn, server, |_| {}).await;

        Timer::after_secs(10).await;
    }
}

//
//
// Sensing code
//
//

struct GenericTempHumiditySensor<T: twim::Instance> {
    address: u8,
    driver: twim::Twim<'static, T>,
}

// TODO: Implement CRC checks
impl<T: twim::Instance> GenericTempHumiditySensor<T> {
    fn new(address: u8, driver: twim::Twim<'static, T>) -> Self {
        Self { address, driver }
    }

    /// The procedure for reading out the temperature and humidity and converting them to floats is
    /// detailed in the datasheet of the SHT41-AD1B-R2.
    async fn read_temp_and_humidity(&mut self) -> (f32, f32) {
        let mut buf = [0u8; 6];

        unwrap!(self.driver.blocking_write(self.address, &mut [0xFD]));
        Timer::after_millis(10).await;
        unwrap!(self.driver.blocking_read(self.address, &mut buf));

        let temp_int = u16::from_be_bytes([buf[0], buf[1]]);
        let temp_float: f32 = -45f32 + 175f32 * (temp_int as f32 / (2u32.pow(16) - 1) as f32);

        let rh_int = u16::from_be_bytes([buf[3], buf[4]]);
        let rh_float: f32 = -6f32 + 125f32 * (rh_int as f32 / (2u32.pow(16) - 1) as f32);

        return (temp_float, rh_float);
    }
}

bind_interrupts!(struct Irqs {
    SPIM0_SPIS0_TWIM0_TWIS0_SPI0_TWI0 => twim::InterruptHandler<peripherals::TWISPI0>;
});

type TempHumiditySensor = GenericTempHumiditySensor<TWISPI0>;

#[embassy_executor::task]
async fn measurement_task(mut sensor: TempHumiditySensor, server: &'static Server) {
    loop {
        let (temp, humidity) = sensor.read_temp_and_humidity().await;
        println!("{} °C\t{} %", temp, humidity);

        let temp: u32 = (temp * 100.0) as u32;
        let humidity: u32 = (humidity * 100.0) as u32;

        unwrap!(server.bas.temperature_set(&temp));
        unwrap!(server.bas.humidity_set(&humidity));

        Timer::after_secs(5).await;
    }
}

//
//
// Main code
//
//

static SERVER: StaticCell<Server> = StaticCell::new();

#[embassy_executor::main]
async fn main(spawner: Spawner) {
    // Set up peripherals

    interrupt::SPIM0_SPIS0_TWIM0_TWIS0_SPI0_TWI0.set_priority(interrupt::Priority::P2);

    let mut embassy_config = embassy_nrf::config::Config::default();
    embassy_config.gpiote_interrupt_priority = interrupt::Priority::P2;
    embassy_config.time_interrupt_priority = interrupt::Priority::P2;
    let p = embassy_nrf::init(embassy_config);

    let config = twim::Config::default();
    let twi = twim::Twim::new(p.TWISPI0, Irqs, p.P0_14, p.P0_13, config);
    let sensor = TempHumiditySensor::new(0x44, twi);

    let mut led = Output::new(p.P0_20, Level::Low, OutputDrive::Standard);

    // Set up softdevice

    let config = nrf_softdevice::Config {
        clock: Some(raw::nrf_clock_lf_cfg_t {
            source: raw::NRF_CLOCK_LF_SRC_RC as u8,
            rc_ctiv: 16,
            rc_temp_ctiv: 2,
            accuracy: raw::NRF_CLOCK_LF_ACCURACY_500_PPM as u8,
        }),
        conn_gap: Some(raw::ble_gap_conn_cfg_t {
            conn_count: 1,
            event_length: 24,
        }),
        conn_gatt: Some(raw::ble_gatt_conn_cfg_t { att_mtu: 256 }),
        gatts_attr_tab_size: Some(raw::ble_gatts_cfg_attr_tab_size_t {
            attr_tab_size: raw::BLE_GATTS_ATTR_TAB_SIZE_DEFAULT,
        }),
        gap_role_count: Some(raw::ble_gap_cfg_role_count_t {
            adv_set_count: 1,
            periph_role_count: 1,
        }),
        gap_device_name: Some(raw::ble_gap_cfg_device_name_t {
            p_value: b"CommonSense" as *const u8 as _,
            current_len: 9,
            max_len: 9,
            write_perm: unsafe { mem::zeroed() },
            _bitfield_1: raw::ble_gap_cfg_device_name_t::new_bitfield_1(
                raw::BLE_GATTS_VLOC_STACK as u8,
            ),
        }),
        ..Default::default()
    };

    let sd = Softdevice::enable(&config);
    let server = unwrap!(Server::new(sd));
    let server: &'static mut Server = SERVER.init(server);

    // Start tasks

    unwrap!(spawner.spawn(softdevice_task(sd)));
    unwrap!(spawner.spawn(ble_task(sd, server)));
    unwrap!(spawner.spawn(measurement_task(sensor, server)));

    loop {
        led.set_high();
        Timer::after_millis(500).await;
        led.set_low();
        Timer::after_millis(500).await;
    }
}