
use embassy_executor::Spawner;
use embassy_futures::select::Either;
use embassy_sync::{blocking_mutex::raw::NoopRawMutex, pubsub::{DynSubscriber, PubSubChannel, Publisher}};
use esp_radio::ble::controller::BleConnector;
use static_cell::StaticCell;
use trouble_host::{prelude::*, types::gatt_traits::FromGattError};
use log::*;

use crate::{backoff::Backoff, events::Prediction};

#[gatt_server]
struct SerialServer {
    serial_service: SerialService,
    location_service: LocationService,
    renderbug_service: RenderbugService
}

#[gatt_service(uuid="00000000-f43a-48c4-8411-efe6a38a5d75")]
struct RenderbugService {
    // 32 bit float, 0 exponent, m/s velocity unit
    #[descriptor(uuid=descriptors::CHARACTERISTIC_PRESENTATION_FORMAT, read, value = [0x14, 0x0, 0x27, 0x12, 0x01, 0x00, 0x00])]
    #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, read, value = "Speed (m/s)")]
    #[characteristic(uuid="00000001-f43a-48c4-8411-efe6a38a5d75", notify)]
    speed: f32,

    // 32 bit float, 0 exponent, unitless
    #[descriptor(uuid=descriptors::CHARACTERISTIC_PRESENTATION_FORMAT, read, value = [0x14, 0x0, 0x27, 0x00, 0x01, 0x00, 0x00])]
    #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, read, value = "Latitude")]
    #[characteristic(uuid="00000002-f43a-48c4-8411-efe6a38a5d75", notify)]
    latitude: f32,

    // 32 bit float, 0 exponent, unitless
    #[descriptor(uuid=descriptors::CHARACTERISTIC_PRESENTATION_FORMAT, read, value = [0x14, 0x0, 0x27, 0x00, 0x01, 0x00, 0x00])]
    #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, read, value = "Longitude")]
    #[characteristic(uuid="00000003-f43a-48c4-8411-efe6a38a5d75", notify)]
    longitude: f32,
}

#[gatt_service(uuid=service::LOCATION_AND_NAVIGATION)]
struct LocationService {
    #[characteristic(uuid=characteristic::LOCATION_AND_SPEED, notify)]
    location_data: LocationData,
    #[characteristic(uuid=characteristic::LN_FEATURE, read, value=0b0000101)] // Location and speed fields
    features: u8,
}

#[derive(Default, Debug)]
struct LocationData {
    coordinates: [i32; 2],
    velocity: u16
}

impl FixedGattValue for LocationData {
    fn as_gatt(&self) -> &[u8] {
        //let databuf = [0;Self::SIZE];
        unsafe { core::slice::from_raw_parts(self as *const Self as *const u8, Self::SIZE) }
    }
    
    const SIZE: usize = core::mem::size_of::<Self>();
    
    fn from_gatt(data: &[u8]) -> Result<Self, FromGattError> {
        if data.len() != Self::SIZE {
            Err(FromGattError::InvalidLength)
        } else {
            // SAFETY
            // - Pointer is considered "valid" as per the rules outlined for validity in std::ptr v1.82.0
            // - Pointer was generated from a slice of bytes matching the size of the type implementing Primitive, and all types implementing Primitive are valid for all possible configurations of bits
            // - Primitive trait is constrained to require Copy
            unsafe { Ok((data.as_ptr() as *const Self).read_unaligned()) }
        }
    }
}

#[gatt_service(uuid="6E400001-B5A3-F393-E0A9-E50E24DCCA9E")]
struct SerialService {
    // The outside world writes here
    #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, read, value = "RX")]
    #[characteristic(uuid = "6E400002-B5A3-F393-E0A9-E50E24DCCA9E", write)]
    rx: heapless::String<128>,

    // The outside world reads from here
    #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, read, value = "TX")]
    #[characteristic(uuid = "6E400003-B5A3-F393-E0A9-E50E24DCCA9E", notify)]
    tx: heapless::String<128>
}

#[embassy_executor::task]
async fn client_prediction_task(mut src: DynSubscriber<'static, Prediction>, sink: Publisher<'static, NoopRawMutex, Prediction, 5, 1, 1>) {
    debug!("Started BLE client prediction stream");
    loop {
        let _ = sink.try_publish(src.next_message_pure().await);
    }
}

static STATIC_RESOURCES: StaticCell<HostResources<DefaultPacketPool, 1, 1>> = StaticCell::new();
static STATIC_STACK: StaticCell<Stack<'static, ExternalController<BleConnector<'static>, 1>, DefaultPacketPool>> = StaticCell::new();
static STATIC_SERVER: StaticCell<SerialServer> = StaticCell::new();
static STATIC_CLIENT_PREDICTIONS: StaticCell<PubSubChannel<NoopRawMutex, Prediction, 5, 1, 1>> = StaticCell::new();

#[embassy_executor::task]
pub async fn ble_task(ble: BleConnector<'static>, predictions: DynSubscriber<'static, Prediction>, spawner: Spawner) {
    info!("Starting BLE stack");
    let server = STATIC_SERVER.init(SerialServer::new_with_config(GapConfig::Peripheral(PeripheralConfig { name: "Renderbug", appearance: &appearance::light_source::LED_ARRAY })).unwrap());
    let control: ExternalController<esp_radio::ble::controller::BleConnector<'_>, 1> = ExternalController::new(ble);
    let stack = STATIC_STACK.init(trouble_host::new(control, STATIC_RESOURCES.init(HostResources::new())));
    let Host { mut peripheral, mut runner, .. } = stack.build();

    let client_predictions = STATIC_CLIENT_PREDICTIONS.init(PubSubChannel::new());
    spawner.must_spawn(client_prediction_task(predictions, client_predictions.publisher().unwrap()));

    // The host task must be started and ticking beore we can start advertising, so we use a join() here instead of a separate task
    let _ = embassy_futures::join::join(
        runner.run(),
        async {
            loop {
                let advertiser = Backoff::from_secs(5).forever().attempt(async || {
                    info!("Starting BLE advertising");
                    let mut adv_data = [0; 64];
                    let len = AdStructure::encode_slice(
                        &[
                            AdStructure::CompleteLocalName("Renderbug".as_bytes()),
                            AdStructure::Flags(LE_GENERAL_DISCOVERABLE | BR_EDR_NOT_SUPPORTED),
                            AdStructure::ServiceUuids128(&[
                                [ // Serial service
                                    0x6E, 0x40, 0x00, 0x01,
                                    0xB5, 0xA3,
                                    0xF3, 0x93,
                                    0xE0, 0xA9,
                                    0xE5, 0x0E, 0x24, 0xDC, 0xCA, 0x9E
                                    //0x9E, 0xCA, 0xDC, 0x24, 0x0E, 0xE5, 0xA9, 0xE0, 0x93, 0xF3, 0xA3, 0xB5,
                                    //0x01, 0x00, 0x40, 0x6E,
                                ],
                                [ // Renderbug mesh service
                                    0x00, 0x00, 0x00, 0x00,
                                    0xf4, 0x3a,
                                    0x48, 0xc4,
                                    0x84, 0x11,
                                    0xef, 0xe6, 0xa3, 0x8a, 0x5d, 0x75
                                ]
                            ]),
                            AdStructure::ServiceUuids16(&[
                                // Location and navigation
                                [0x18, 0x19]
                            ])
                        ],
                        &mut adv_data[..],
                    ).unwrap();
                    peripheral.advertise(
                        &Default::default(),
                        Advertisement::ConnectableScannableUndirected { adv_data: &adv_data[..len], scan_data: &[] }
                    ).await
                }).await.unwrap();
                info!("Waiting for connection");
                match advertiser.accept().await.and_then(|raw| { raw.with_attribute_server(server) }) {
                    Ok(conn) => {
                        if spawner.spawn(ble_connection(conn, server, client_predictions.dyn_subscriber().unwrap())).is_err() {
                            error!("Unable to spawn task for handling BLE connection! Please increase the embassy task pool size!");
                        }
                    },
                    Err(e) => {
                        error!("Failed to accept new connection: {e:?}");
                    },
                }
            }
        }
    ).await;
}

#[embassy_executor::task]
async fn ble_connection(conn: GattConnection<'static, 'static, DefaultPacketPool>, server: &'static SerialServer<'static>, mut predictions: DynSubscriber<'static, Prediction>) {
    info!("Started handling new connection with {:?}", conn.raw().peer_identity());
    let mut location_data = LocationData::default();
    loop {
        let next = embassy_futures::select::select(conn.next(), predictions.next_message_pure()).await;
        match next {
            Either::First(gatt_event) => {
                match gatt_event {
                    GattConnectionEvent::Disconnected { reason } => {
                        warn!("BLE Disconnected: {reason:?}");
                        return;
                    },
                    GattConnectionEvent::Gatt { event } => {
                        if let GattEvent::Write(event) = event {
                            if event.handle() == server.serial_service.rx.handle {
                                let buf = event.value(&server.serial_service.rx).unwrap();
                                info!("Received serial data {buf:?}");
                            }
                        }
                    },
                    _ => trace!("GATT event")
                }
            },
            Either::Second(prediction) => {
                let evt_str = heapless::format!("{prediction:?}\n").unwrap();
                server.serial_service.tx.notify(&conn, &evt_str).await.unwrap();
                let update_location = match prediction {
                    Prediction::Velocity(v) => {
                        location_data.velocity = (v * 100.0) as u16;
                        server.renderbug_service.speed.notify(&conn, &v).await.unwrap();
                        true
                    },
                    Prediction::Location(coords) => {
                        location_data.coordinates[0] = (coords.x * 1e7) as i32;
                        location_data.coordinates[1] = (coords.y * 1e7) as i32;
                        server.renderbug_service.latitude.notify(&conn, &(coords.x as f32)).await.unwrap();
                        server.renderbug_service.longitude.notify(&conn, &(coords.y as f32)).await.unwrap();
                        true
                    },
                    _ => false
                };

                if update_location {
                    server.location_service.location_data.notify(&conn, &location_data).await.unwrap();
                    // TODO: write out NMEA sentences
                    //let mut nmea_str = heapless::String::from_utf8("foo");
                    //server.nmea_service.tx.notify(&conn, "foo").await.unwrap();
                }
            }
        }
    }
}