src/lib.rs - platform/external/rust/pica - Git at Google

 // Copyright 2022 Google LLC
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
 // You may obtain a copy of the License at
 //
 //     https://www.apache.org/licenses/LICENSE-2.0
 //
 // Unless required by applicable law or agreed to in writing, software
 // distributed under the License is distributed on an "AS IS" BASIS,
 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 // See the License for the specific language governing permissions and
 // limitations under the License.

 use anyhow::Result;
 use pdl_runtime::Packet;
 use serde::{Deserialize, Serialize};
 use std::collections::HashMap;
 use std::fmt::Display;
 use std::path::PathBuf;
 use std::pin::Pin;
 use thiserror::Error;
 use tokio::sync::{broadcast, mpsc, oneshot};

 pub mod packets;
 mod pcapng;

 use packets::uci::{self, *};

 mod device;
 use device::{Device, MAX_DEVICE, MAX_SESSION};

 mod session;

 mod mac_address;
 pub use mac_address::MacAddress;

 mod app_config;
 pub use app_config::AppConfig;

 pub type UciPacket = Vec<u8>;
 pub type UciStream = Pin<Box<dyn futures::stream::Stream<Item = Vec<u8>> + Send>>;
 pub type UciSink = Pin<Box<dyn futures::sink::Sink<Vec<u8>, Error = anyhow::Error> + Send>>;

 /// Handle allocated for created devices or anchors.
 /// The handle is unique across the lifetime of the Pica context
 /// and callers may assume that one handle is never reused.
 pub type Handle = usize;

 /// Ranging measurement produced by a ranging estimator.
 #[derive(Clone, Copy, Default, Debug)]
 pub struct RangingMeasurement {
     pub range: u16,
     pub azimuth: i16,
     pub elevation: i8,
 }

 /// Trait matching the capabilities of a ranging estimator.
 /// The estimator manages the position of the devices, and chooses
 /// the algorithm used to generate the ranging measurements.
 pub trait RangingEstimator: Send + Sync {
     /// Evaluate the ranging measurement for the two input devices
     /// identified by their respective handle. The result is a triplet
     /// containing the range, azimuth, and elevation of the right device
     /// relative to the left device.
     /// Return `None` if the measurement could not be estimated, e.g. because
     /// the devices are out of range.
     fn estimate(&self, left: &Handle, right: &Handle) -> Option<RangingMeasurement>;
 }

 /// Pica emulation environment.
 /// All the devices added to this environment are emulated as if they were
 /// from the same physical space.
 pub struct Pica {
     counter: usize,
     devices: HashMap<Handle, Device>,
     anchors: HashMap<MacAddress, Anchor>,
     command_rx: Option<mpsc::Receiver<PicaCommand>>,
     command_tx: mpsc::Sender<PicaCommand>,
     event_tx: broadcast::Sender<PicaEvent>,
     ranging_estimator: Box<dyn RangingEstimator>,
     pcapng_dir: Option<PathBuf>,
 }

 #[derive(Error, Debug, Clone, PartialEq, Eq)]
 pub enum PicaCommandError {
     #[error("Device already exists: {0}")]
     DeviceAlreadyExists(MacAddress),
     #[error("Device not found: {0}")]
     DeviceNotFound(MacAddress),
 }

 pub enum PicaCommand {
     // Connect a new device.
     Connect(UciStream, UciSink),
     // Disconnect the selected device.
     Disconnect(usize),
     // Execute ranging command for selected device and session.
     Ranging(usize, u32),
     // Send an in-band request to stop ranging to a peer controlee identified by address and session id.
     StopRanging(MacAddress, u32),
     // UCI packet received for the selected device.
     UciPacket(usize, Vec<u8>),
     // Create Anchor
     CreateAnchor(
         MacAddress,
         oneshot::Sender<Result<Handle, PicaCommandError>>,
     ),
     // Destroy Anchor
     DestroyAnchor(
         MacAddress,
         oneshot::Sender<Result<Handle, PicaCommandError>>,
     ),
 }

 impl Display for PicaCommand {
     fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
         let cmd = match self {
             PicaCommand::Connect(_, _) => "Connect",
             PicaCommand::Disconnect(_) => "Disconnect",
             PicaCommand::Ranging(_, _) => "Ranging",
             PicaCommand::StopRanging(_, _) => "StopRanging",
             PicaCommand::UciPacket(_, _) => "UciPacket",
             PicaCommand::CreateAnchor(_, _) => "CreateAnchor",
             PicaCommand::DestroyAnchor(_, _) => "DestroyAnchor",
         };
         write!(f, "{}", cmd)
     }
 }

 #[derive(Clone, Debug, Serialize)]
 #[serde(untagged)]
 pub enum PicaEvent {
     // A UCI connection was added
     Connected {
         handle: Handle,
         mac_address: MacAddress,
     },
     // A UCI connection was lost
     Disconnected {
         handle: Handle,
         mac_address: MacAddress,
     },
 }

 #[derive(Clone, Copy, Debug, Serialize, Deserialize)]
 pub enum Category {
     Uci,
     Anchor,
 }

 #[derive(Debug, Clone, Copy)]
 struct Anchor {
     handle: Handle,
     #[allow(unused)]
     mac_address: MacAddress,
 }

 fn make_measurement(
     mac_address: &MacAddress,
     local: RangingMeasurement,
     remote: RangingMeasurement,
 ) -> ShortAddressTwoWayRangingMeasurement {
     if let MacAddress::Short(address) = mac_address {
         ShortAddressTwoWayRangingMeasurement {
             mac_address: u16::from_le_bytes(*address),
             status: uci::Status::Ok,
             nlos: 0, // in Line Of Sight
             distance: local.range,
             aoa_azimuth: local.azimuth as u16,
             aoa_azimuth_fom: 100, // Yup, pretty sure about this
             aoa_elevation: local.elevation as u16,
             aoa_elevation_fom: 100, // Yup, pretty sure about this
             aoa_destination_azimuth: remote.azimuth as u16,
             aoa_destination_azimuth_fom: 100,
             aoa_destination_elevation: remote.elevation as u16,
             aoa_destination_elevation_fom: 100,
             slot_index: 0,
             rssi: u8::MAX,
         }
     } else {
         panic!("Extended address is not supported.")
     }
 }

 impl Pica {
     pub fn new(ranging_estimator: Box<dyn RangingEstimator>, pcapng_dir: Option<PathBuf>) -> Self {
         let (command_tx, command_rx) = mpsc::channel(MAX_SESSION * MAX_DEVICE);
         let (event_tx, _) = broadcast::channel(16);
         Pica {
             devices: HashMap::new(),
             anchors: HashMap::new(),
             counter: 0,
             command_rx: Some(command_rx),
             command_tx,
             event_tx,
             ranging_estimator,
             pcapng_dir,
         }
     }

     pub fn events(&self) -> broadcast::Receiver<PicaEvent> {
         self.event_tx.subscribe()
     }

     pub fn commands(&self) -> mpsc::Sender<PicaCommand> {
         self.command_tx.clone()
     }

     fn get_device_mut(&mut self, device_handle: usize) -> Option<&mut Device> {
         self.devices.get_mut(&device_handle)
     }

     fn get_device(&self, device_handle: usize) -> Option<&Device> {
         self.devices.get(&device_handle)
     }

     fn get_category(&self, mac_address: &MacAddress) -> Option<Category> {
         if self.anchors.contains_key(mac_address) {
             Some(Category::Anchor)
         } else if self
             .devices
             .iter()
             .any(|(_, device)| device.mac_address == *mac_address)
         {
             Some(Category::Uci)
         } else {
             None
         }
     }

     fn send_event(&self, event: PicaEvent) {
         // An error here means that we have
         // no receivers, so ignore it
         let _ = self.event_tx.send(event);
     }

     /// Handle an incoming stream of UCI packets.
     /// Reassemble control packets when fragmented, data packets are unmodified.
     async fn read_routine(
         mut uci_stream: impl futures::stream::Stream<Item = Vec<u8>> + Unpin,
         cmd_tx: mpsc::Sender<PicaCommand>,
         handle: Handle,
         pcapng_file: Option<&pcapng::File>,
     ) -> anyhow::Result<()> {
         use futures::stream::StreamExt;

         loop {
             let mut complete_packet: Option<Vec<u8>> = None;
             loop {
                 let packet = uci_stream
                     .next()
                     .await
                     .ok_or(anyhow::anyhow!("input packet stream closed"))?;
                 let header =
                     packets::uci::CommonPacketHeader::parse(&packet[0..COMMON_HEADER_SIZE])?;

                 if let Some(file) = pcapng_file {
                     file.write(&packet, pcapng::Direction::Tx)?;
                 }

                 match &mut complete_packet {
                     Some(complete_packet) => {
                         complete_packet.extend_from_slice(&packet[HEADER_SIZE..])
                     }
                     None => complete_packet = Some(packet),
                 }

                 if header.get_pbf() == packets::uci::PacketBoundaryFlag::Complete
                     || header.get_mt() == packets::uci::MessageType::Data
                 {
                     break;
                 }
             }

             cmd_tx
                 .send(PicaCommand::UciPacket(handle, complete_packet.unwrap()))
                 .await
                 .unwrap()
         }
     }

     /// Segment a stream of UCI packets.
     async fn write_routine(
         mut uci_sink: impl futures::sink::Sink<Vec<u8>> + Unpin,
         mut packet_rx: mpsc::UnboundedReceiver<UciPacket>,
         _handle: Handle,
         pcapng_file: Option<&pcapng::File>,
     ) -> anyhow::Result<()> {
         use futures::sink::SinkExt;

         loop {
             let complete_packet = packet_rx
                 .recv()
                 .await
                 .ok_or(anyhow::anyhow!("output packet stream closed"))?;
             let mut offset = HEADER_SIZE;
             let mt = parse_message_type(complete_packet[0]);

             while offset < complete_packet.len() {
                 let remaining_length = complete_packet.len() - offset;
                 let fragment_length = std::cmp::min(
                     remaining_length,
                     if mt == MessageType::Data {
                         MAX_DATA_PACKET_PAYLOAD_SIZE
                     } else {
                         MAX_CTRL_PACKET_PAYLOAD_SIZE
                     },
                 );
                 let pbf = if fragment_length == remaining_length {
                     PacketBoundaryFlag::Complete
                 } else {
                     PacketBoundaryFlag::NotComplete
                 };

                 let mut packet = Vec::with_capacity(HEADER_SIZE + fragment_length);

                 packet.extend_from_slice(&complete_packet[0..HEADER_SIZE]);
                 const PBF_MASK: u8 = 0x10;
                 packet[0] &= !PBF_MASK;
                 packet[0] |= (pbf as u8) << 4;

                 match mt {
                     MessageType::Data => {
                         packet[2..4].copy_from_slice(&(fragment_length as u16).to_le_bytes())
                     }
                     _ => packet[3] = fragment_length as u8,
                 }

                 packet.extend_from_slice(&complete_packet[offset..offset + fragment_length]);

                 if let Some(file) = pcapng_file {
                     file.write(&packet, pcapng::Direction::Rx)?;
                 }

                 uci_sink
                     .send(packet)
                     .await
                     .map_err(|_| anyhow::anyhow!("output packet sink closed"))?;

                 offset += fragment_length;
             }
         }
     }

     pub fn add_device(&mut self, stream: UciStream, sink: UciSink) -> Result<Handle> {
         let (packet_tx, packet_rx) = mpsc::unbounded_channel();
         let pica_tx = self.command_tx.clone();
         let disconnect_tx = self.command_tx.clone();
         let pcapng_dir = self.pcapng_dir.clone();

         let handle = self.counter;
         self.counter += 1;

         log::debug!("[{}] Connecting device", handle);

         let mac_address = MacAddress::Short((handle as u16).to_be_bytes());
         let mut device = Device::new(handle, mac_address, packet_tx, self.command_tx.clone());
         device.init();

         self.send_event(PicaEvent::Connected {
             handle,
             mac_address: device.mac_address,
         });

         self.devices.insert(handle, device);

         // Spawn and detach the connection handling task.
         // The task notifies pica when exiting to let it clean
         // the state.
         tokio::task::spawn(async move {
             let pcapng_file = if let Some(dir) = pcapng_dir {
                 let full_path = dir.join(format!("device-{}.pcapng", handle));
                 log::debug!("Recording pcapng to file {}", full_path.as_path().display());
                 Some(pcapng::File::create(full_path).unwrap())
             } else {
                 None
             };

             let _ = tokio::try_join!(
                 async { Self::read_routine(stream, pica_tx, handle, pcapng_file.as_ref()).await },
                 async { Self::write_routine(sink, packet_rx, handle, pcapng_file.as_ref()).await }
             );

             disconnect_tx
                 .send(PicaCommand::Disconnect(handle))
                 .await
                 .unwrap()
         });

         Ok(handle)
     }

     fn disconnect(&mut self, device_handle: usize) {
         log::debug!("[{}] Disconnecting device", device_handle);

         if let Some(device) = self.devices.get(&device_handle) {
             self.send_event(PicaEvent::Disconnected {
                 handle: device_handle,
                 mac_address: device.mac_address,
             });
             self.devices.remove(&device_handle);
         }
     }

     fn ranging(&mut self, device_handle: usize, session_id: u32) {
         log::debug!("[{}] Ranging event", device_handle);
         log::debug!("  session_id={}", session_id);

         let device = self.get_device(device_handle).unwrap();
         let session = device.session(session_id).unwrap();

         let mut data_transfer = Vec::new();
         let mut measurements = Vec::new();

         // Look for compatible anchors.
         for mac_address in session.get_dst_mac_address() {
             if let Some(other) = self.anchors.get(mac_address) {
                 let Some(local) = self
                     .ranging_estimator
                     .estimate(&device.handle, &other.handle)
                 else {
                     continue;
                 };
                 let Some(remote) = self
                     .ranging_estimator
                     .estimate(&other.handle, &device.handle)
                 else {
                     continue;
                 };
                 measurements.push(make_measurement(mac_address, local, remote));
             }
         }

         // Look for compatible ranging sessions in other devices.
         for peer_device in self.devices.values() {
             if peer_device.handle == device_handle {
                 continue;
             }

             if peer_device.can_start_ranging(session, session_id) {
                 let peer_mac_address = peer_device
                     .session(session_id)
                     .unwrap()
                     .app_config
                     .device_mac_address
                     .unwrap();
                 let local = self
                     .ranging_estimator
                     .estimate(&device.handle, &peer_device.handle)
                     .unwrap_or(Default::default());
                 let remote = self
                     .ranging_estimator
                     .estimate(&peer_device.handle, &device.handle)
                     .unwrap_or(Default::default());
                 measurements.push(make_measurement(&peer_mac_address, local, remote));
             }

             if device.can_start_data_transfer(session_id)
                 && peer_device.can_receive_data_transfer(session_id)
             {
                 data_transfer.push(peer_device);
             }
         }

         // TODO: Data transfer should be limited in size for
         // each round of ranging
         for peer_device in data_transfer.iter() {
             peer_device
                 .tx
                 .send(
                     DataMessageRcvBuilder {
                         application_data: session.data().clone().into(),
                         data_sequence_number: 0x01,
                         pbf: PacketBoundaryFlag::Complete,
                         session_handle: session_id,
                         source_address: session.app_config.device_mac_address.unwrap().into(),
                         status: uci::Status::Ok,
                     }
                     .build()
                     .encode_to_vec()
                     .unwrap(),
                 )
                 .unwrap();
         }
         if session.is_session_info_ntf_enabled() {
             device
                 .tx
                 .send(
                     // TODO: support extended address
                     ShortMacTwoWaySessionInfoNtfBuilder {
                         sequence_number: session.sequence_number,
                         session_token: session_id,
                         rcr_indicator: 0,            //TODO
                         current_ranging_interval: 0, //TODO
                         two_way_ranging_measurements: measurements,
                         vendor_data: vec![],
                     }
                     .build()
                     .encode_to_vec()
                     .unwrap(),
                 )
                 .unwrap();

             let device = self.get_device_mut(device_handle).unwrap();
             let session = device.session_mut(session_id).unwrap();

             session.sequence_number += 1;
         }

         // TODO: Clean the data only when all the data is transfered
         let device = self.get_device_mut(device_handle).unwrap();
         let session = device.session_mut(session_id).unwrap();

         session.clear_data();
     }

     fn uci_packet(&mut self, device_handle: usize, packet: Vec<u8>) {
         match self.get_device_mut(device_handle) {
             Some(device) => device.receive_packet(packet),
             None => log::error!("Device {} not found", device_handle),
         }
     }

     fn pica_command(&mut self, command: PicaCommand) {
         use PicaCommand::*;
         match command {
             Connect(stream, sink) => {
                 let _ = self.add_device(stream, sink);
             }
             Disconnect(device_handle) => self.disconnect(device_handle),
             Ranging(device_handle, session_id) => self.ranging(device_handle, session_id),
             StopRanging(mac_address, session_id) => {
                 self.stop_controlee_ranging(&mac_address, session_id)
             }
             UciPacket(device_handle, packet) => self.uci_packet(device_handle, packet),
             CreateAnchor(mac_address, pica_cmd_rsp_tx) => {
                 self.create_anchor(mac_address, pica_cmd_rsp_tx)
             }
             DestroyAnchor(mac_address, pica_cmd_rsp_tx) => {
                 self.destroy_anchor(mac_address, pica_cmd_rsp_tx)
             }
         }
     }

     /// Run the internal pica event loop.
     pub async fn run(mut self) -> Result<()> {
         let Some(mut command_rx) = self.command_rx.take() else {
             anyhow::bail!("missing pica command receiver")
         };
         loop {
             if let Some(command) = command_rx.recv().await {
                 self.pica_command(command)
             }
         }
     }

     // Handle the in-band StopRanging command sent from controller to the controlee with
     // corresponding mac_address and session_id.
     fn stop_controlee_ranging(&mut self, mac_address: &MacAddress, session_id: u32) {
         for device in self.devices.values_mut() {
             let Some(session) = device.session_mut(session_id) else {
                 continue;
             };

             if session.app_config.device_mac_address != Some(*mac_address) {
                 continue;
             }

             if session.session_state() == SessionState::SessionStateActive {
                 session.stop_ranging_task();
                 session.set_state(
                     SessionState::SessionStateIdle,
                     ReasonCode::SessionStoppedDueToInbandSignal,
                 );
                 device.n_active_sessions = device.n_active_sessions.saturating_sub(1);
                 if device.n_active_sessions == 0 {
                     device.set_state(DeviceState::DeviceStateReady);
                 }
             } else {
                 log::warn!("stop_controlee_ranging: session is not active !");
             }
         }
     }

     #[allow(clippy::map_entry)]
     fn create_anchor(
         &mut self,
         mac_address: MacAddress,
         rsp_tx: oneshot::Sender<Result<Handle, PicaCommandError>>,
     ) {
         log::debug!("[_] Create anchor");
         log::debug!("  mac_address: {}", mac_address);

         let status = if self.get_category(&mac_address).is_some() {
             Err(PicaCommandError::DeviceAlreadyExists(mac_address))
         } else {
             let handle = self.counter;
             self.counter += 1;

             assert!(self
                 .anchors
                 .insert(
                     mac_address,
                     Anchor {
                         handle,
                         mac_address,
                     },
                 )
                 .is_none());

             Ok(handle)
         };

         rsp_tx.send(status).unwrap_or_else(|err| {
             log::error!("Failed to send create-anchor command response: {:?}", err)
         })
     }

     fn destroy_anchor(
         &mut self,
         mac_address: MacAddress,
         rsp_tx: oneshot::Sender<Result<Handle, PicaCommandError>>,
     ) {
         log::debug!("[_] Destroy anchor");
         log::debug!("  mac_address: {}", mac_address);

         let status = match self.anchors.remove(&mac_address) {
             None => Err(PicaCommandError::DeviceNotFound(mac_address)),
             Some(anchor) => Ok(anchor.handle),
         };

         rsp_tx.send(status).unwrap_or_else(|err| {
             log::error!("Failed to send destroy-anchor command response: {:?}", err)
         })
     }
 }

 /// Run the internal pica event loop.
 /// As opposed to Pica::run, the context is passed under a mutex, which
 /// allows synchronous access to the context for device creation.
 pub async fn run(this: &std::sync::Mutex<Pica>) -> Result<()> {
     // Extract the mpsc receiver from the Pica context.
     // The receiver cannot be cloned.
     let Some(mut command_rx) = this.lock().unwrap().command_rx.take() else {
         anyhow::bail!("missing pica command receiver");
     };

     loop {
         if let Some(command) = command_rx.recv().await {
             this.lock().unwrap().pica_command(command)
         }
     }
 }
	// Copyright 2022 Google LLC
	//
	// Licensed under the Apache License, Version 2.0 (the "License");
	// you may not use this file except in compliance with the License.
	// You may obtain a copy of the License at
	//
	// https://www.apache.org/licenses/LICENSE-2.0
	//
	// Unless required by applicable law or agreed to in writing, software
	// distributed under the License is distributed on an "AS IS" BASIS,
	// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
	// See the License for the specific language governing permissions and
	// limitations under the License.

	use anyhow::Result;
	use pdl_runtime::Packet;
	use serde::{Deserialize, Serialize};
	use std::collections::HashMap;
	use std::fmt::Display;
	use std::path::PathBuf;
	use std::pin::Pin;
	use thiserror::Error;
	use tokio::sync::{broadcast, mpsc, oneshot};

	pub mod packets;
	mod pcapng;

	use packets::uci::{self, *};

	mod device;
	use device::{Device, MAX_DEVICE, MAX_SESSION};

	mod session;

	mod mac_address;
	pub use mac_address::MacAddress;

	mod app_config;
	pub use app_config::AppConfig;

	pub type UciPacket = Vec<u8>;
	pub type UciStream = Pin<Box<dyn futures::stream::Stream<Item = Vec<u8>> + Send>>;
	pub type UciSink = Pin<Box<dyn futures::sink::Sink<Vec<u8>, Error = anyhow::Error> + Send>>;

	/// Handle allocated for created devices or anchors.
	/// The handle is unique across the lifetime of the Pica context
	/// and callers may assume that one handle is never reused.
	pub type Handle = usize;

	/// Ranging measurement produced by a ranging estimator.
	#[derive(Clone, Copy, Default, Debug)]
	pub struct RangingMeasurement {
	pub range: u16,
	pub azimuth: i16,
	pub elevation: i8,
	}

	/// Trait matching the capabilities of a ranging estimator.
	/// The estimator manages the position of the devices, and chooses
	/// the algorithm used to generate the ranging measurements.
	pub trait RangingEstimator: Send + Sync {
	/// Evaluate the ranging measurement for the two input devices
	/// identified by their respective handle. The result is a triplet
	/// containing the range, azimuth, and elevation of the right device
	/// relative to the left device.
	/// Return `None` if the measurement could not be estimated, e.g. because
	/// the devices are out of range.
	fn estimate(&self, left: &Handle, right: &Handle) -> Option<RangingMeasurement>;
	}

	/// Pica emulation environment.
	/// All the devices added to this environment are emulated as if they were
	/// from the same physical space.
	pub struct Pica {
	counter: usize,
	devices: HashMap<Handle, Device>,
	anchors: HashMap<MacAddress, Anchor>,
	command_rx: Option<mpsc::Receiver<PicaCommand>>,
	command_tx: mpsc::Sender<PicaCommand>,
	event_tx: broadcast::Sender<PicaEvent>,
	ranging_estimator: Box<dyn RangingEstimator>,
	pcapng_dir: Option<PathBuf>,
	}

	#[derive(Error, Debug, Clone, PartialEq, Eq)]
	pub enum PicaCommandError {
	#[error("Device already exists: {0}")]
	DeviceAlreadyExists(MacAddress),
	#[error("Device not found: {0}")]
	DeviceNotFound(MacAddress),
	}

	pub enum PicaCommand {
	// Connect a new device.
	Connect(UciStream, UciSink),
	// Disconnect the selected device.
	Disconnect(usize),
	// Execute ranging command for selected device and session.
	Ranging(usize, u32),
	// Send an in-band request to stop ranging to a peer controlee identified by address and session id.
	StopRanging(MacAddress, u32),
	// UCI packet received for the selected device.
	UciPacket(usize, Vec<u8>),
	// Create Anchor
	CreateAnchor(
	MacAddress,
	oneshot::Sender<Result<Handle, PicaCommandError>>,
	),
	// Destroy Anchor
	DestroyAnchor(
	MacAddress,
	oneshot::Sender<Result<Handle, PicaCommandError>>,
	),
	}

	impl Display for PicaCommand {
	fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
	let cmd = match self {
	PicaCommand::Connect(_, _) => "Connect",
	PicaCommand::Disconnect(_) => "Disconnect",
	PicaCommand::Ranging(_, _) => "Ranging",
	PicaCommand::StopRanging(_, _) => "StopRanging",
	PicaCommand::UciPacket(_, _) => "UciPacket",
	PicaCommand::CreateAnchor(_, _) => "CreateAnchor",
	PicaCommand::DestroyAnchor(_, _) => "DestroyAnchor",
	};
	write!(f, "{}", cmd)
	}
	}

	#[derive(Clone, Debug, Serialize)]
	#[serde(untagged)]
	pub enum PicaEvent {
	// A UCI connection was added
	Connected {
	handle: Handle,
	mac_address: MacAddress,
	},
	// A UCI connection was lost
	Disconnected {
	handle: Handle,
	mac_address: MacAddress,
	},
	}

	#[derive(Clone, Copy, Debug, Serialize, Deserialize)]
	pub enum Category {
	Uci,
	Anchor,
	}

	#[derive(Debug, Clone, Copy)]
	struct Anchor {
	handle: Handle,
	#[allow(unused)]
	mac_address: MacAddress,
	}

	fn make_measurement(
	mac_address: &MacAddress,
	local: RangingMeasurement,
	remote: RangingMeasurement,
	) -> ShortAddressTwoWayRangingMeasurement {
	if let MacAddress::Short(address) = mac_address {
	ShortAddressTwoWayRangingMeasurement {
	mac_address: u16::from_le_bytes(*address),
	status: uci::Status::Ok,
	nlos: 0, // in Line Of Sight
	distance: local.range,
	aoa_azimuth: local.azimuth as u16,
	aoa_azimuth_fom: 100, // Yup, pretty sure about this
	aoa_elevation: local.elevation as u16,
	aoa_elevation_fom: 100, // Yup, pretty sure about this
	aoa_destination_azimuth: remote.azimuth as u16,
	aoa_destination_azimuth_fom: 100,
	aoa_destination_elevation: remote.elevation as u16,
	aoa_destination_elevation_fom: 100,
	slot_index: 0,
	rssi: u8::MAX,
	}
	} else {
	panic!("Extended address is not supported.")
	}
	}

	impl Pica {
	pub fn new(ranging_estimator: Box<dyn RangingEstimator>, pcapng_dir: Option<PathBuf>) -> Self {
	let (command_tx, command_rx) = mpsc::channel(MAX_SESSION * MAX_DEVICE);
	let (event_tx, _) = broadcast::channel(16);
	Pica {
	devices: HashMap::new(),
	anchors: HashMap::new(),
	counter: 0,
	command_rx: Some(command_rx),
	command_tx,
	event_tx,
	ranging_estimator,
	pcapng_dir,
	}
	}

	pub fn events(&self) -> broadcast::Receiver<PicaEvent> {
	self.event_tx.subscribe()
	}

	pub fn commands(&self) -> mpsc::Sender<PicaCommand> {
	self.command_tx.clone()
	}

	fn get_device_mut(&mut self, device_handle: usize) -> Option<&mut Device> {
	self.devices.get_mut(&device_handle)
	}

	fn get_device(&self, device_handle: usize) -> Option<&Device> {
	self.devices.get(&device_handle)
	}

	fn get_category(&self, mac_address: &MacAddress) -> Option<Category> {
	if self.anchors.contains_key(mac_address) {
	Some(Category::Anchor)
	} else if self
	.devices
	.iter()
	.any(\|(_, device)\| device.mac_address == *mac_address)
	{
	Some(Category::Uci)
	} else {
	None
	}
	}

	fn send_event(&self, event: PicaEvent) {
	// An error here means that we have
	// no receivers, so ignore it
	let _ = self.event_tx.send(event);
	}

	/// Handle an incoming stream of UCI packets.
	/// Reassemble control packets when fragmented, data packets are unmodified.
	async fn read_routine(
	mut uci_stream: impl futures::stream::Stream<Item = Vec<u8>> + Unpin,
	cmd_tx: mpsc::Sender<PicaCommand>,
	handle: Handle,
	pcapng_file: Option<&pcapng::File>,
	) -> anyhow::Result<()> {
	use futures::stream::StreamExt;

	loop {
	let mut complete_packet: Option<Vec<u8>> = None;
	loop {
	let packet = uci_stream
	.next()
	.await
	.ok_or(anyhow::anyhow!("input packet stream closed"))?;
	let header =
	packets::uci::CommonPacketHeader::parse(&packet[0..COMMON_HEADER_SIZE])?;

	if let Some(file) = pcapng_file {
	file.write(&packet, pcapng::Direction::Tx)?;
	}

	match &mut complete_packet {
	Some(complete_packet) => {
	complete_packet.extend_from_slice(&packet[HEADER_SIZE..])
	}
	None => complete_packet = Some(packet),
	}

	if header.get_pbf() == packets::uci::PacketBoundaryFlag::Complete
	\|\| header.get_mt() == packets::uci::MessageType::Data
	{
	break;
	}
	}

	cmd_tx
	.send(PicaCommand::UciPacket(handle, complete_packet.unwrap()))
	.await
	.unwrap()
	}
	}

	/// Segment a stream of UCI packets.
	async fn write_routine(
	mut uci_sink: impl futures::sink::Sink<Vec<u8>> + Unpin,
	mut packet_rx: mpsc::UnboundedReceiver<UciPacket>,
	_handle: Handle,
	pcapng_file: Option<&pcapng::File>,
	) -> anyhow::Result<()> {
	use futures::sink::SinkExt;

	loop {
	let complete_packet = packet_rx
	.recv()
	.await
	.ok_or(anyhow::anyhow!("output packet stream closed"))?;
	let mut offset = HEADER_SIZE;
	let mt = parse_message_type(complete_packet[0]);

	while offset < complete_packet.len() {
	let remaining_length = complete_packet.len() - offset;
	let fragment_length = std::cmp::min(
	remaining_length,
	if mt == MessageType::Data {
	MAX_DATA_PACKET_PAYLOAD_SIZE
	} else {
	MAX_CTRL_PACKET_PAYLOAD_SIZE
	},
	);
	let pbf = if fragment_length == remaining_length {
	PacketBoundaryFlag::Complete
	} else {
	PacketBoundaryFlag::NotComplete
	};

	let mut packet = Vec::with_capacity(HEADER_SIZE + fragment_length);

	packet.extend_from_slice(&complete_packet[0..HEADER_SIZE]);
	const PBF_MASK: u8 = 0x10;
	packet[0] &= !PBF_MASK;
	packet[0] \|= (pbf as u8) << 4;

	match mt {
	MessageType::Data => {
	packet[2..4].copy_from_slice(&(fragment_length as u16).to_le_bytes())
	}
	_ => packet[3] = fragment_length as u8,
	}

	packet.extend_from_slice(&complete_packet[offset..offset + fragment_length]);

	if let Some(file) = pcapng_file {
	file.write(&packet, pcapng::Direction::Rx)?;
	}

	uci_sink
	.send(packet)
	.await
	.map_err(\|_\| anyhow::anyhow!("output packet sink closed"))?;

	offset += fragment_length;
	}
	}
	}

	pub fn add_device(&mut self, stream: UciStream, sink: UciSink) -> Result<Handle> {
	let (packet_tx, packet_rx) = mpsc::unbounded_channel();
	let pica_tx = self.command_tx.clone();
	let disconnect_tx = self.command_tx.clone();
	let pcapng_dir = self.pcapng_dir.clone();

	let handle = self.counter;
	self.counter += 1;

	log::debug!("[{}] Connecting device", handle);

	let mac_address = MacAddress::Short((handle as u16).to_be_bytes());
	let mut device = Device::new(handle, mac_address, packet_tx, self.command_tx.clone());
	device.init();

	self.send_event(PicaEvent::Connected {
	handle,
	mac_address: device.mac_address,
	});

	self.devices.insert(handle, device);

	// Spawn and detach the connection handling task.
	// The task notifies pica when exiting to let it clean
	// the state.
	tokio::task::spawn(async move {
	let pcapng_file = if let Some(dir) = pcapng_dir {
	let full_path = dir.join(format!("device-{}.pcapng", handle));
	log::debug!("Recording pcapng to file {}", full_path.as_path().display());
	Some(pcapng::File::create(full_path).unwrap())
	} else {
	None
	};

	let _ = tokio::try_join!(
	async { Self::read_routine(stream, pica_tx, handle, pcapng_file.as_ref()).await },
	async { Self::write_routine(sink, packet_rx, handle, pcapng_file.as_ref()).await }
	);

	disconnect_tx
	.send(PicaCommand::Disconnect(handle))
	.await
	.unwrap()
	});

	Ok(handle)
	}

	fn disconnect(&mut self, device_handle: usize) {
	log::debug!("[{}] Disconnecting device", device_handle);

	if let Some(device) = self.devices.get(&device_handle) {
	self.send_event(PicaEvent::Disconnected {
	handle: device_handle,
	mac_address: device.mac_address,
	});
	self.devices.remove(&device_handle);
	}
	}

	fn ranging(&mut self, device_handle: usize, session_id: u32) {
	log::debug!("[{}] Ranging event", device_handle);
	log::debug!(" session_id={}", session_id);

	let device = self.get_device(device_handle).unwrap();
	let session = device.session(session_id).unwrap();

	let mut data_transfer = Vec::new();
	let mut measurements = Vec::new();

	// Look for compatible anchors.
	for mac_address in session.get_dst_mac_address() {
	if let Some(other) = self.anchors.get(mac_address) {
	let Some(local) = self
	.ranging_estimator
	.estimate(&device.handle, &other.handle)
	else {
	continue;
	};
	let Some(remote) = self
	.ranging_estimator
	.estimate(&other.handle, &device.handle)
	else {
	continue;
	};
	measurements.push(make_measurement(mac_address, local, remote));
	}
	}

	// Look for compatible ranging sessions in other devices.
	for peer_device in self.devices.values() {
	if peer_device.handle == device_handle {
	continue;
	}

	if peer_device.can_start_ranging(session, session_id) {
	let peer_mac_address = peer_device
	.session(session_id)
	.unwrap()
	.app_config
	.device_mac_address
	.unwrap();
	let local = self
	.ranging_estimator
	.estimate(&device.handle, &peer_device.handle)
	.unwrap_or(Default::default());
	let remote = self
	.ranging_estimator
	.estimate(&peer_device.handle, &device.handle)
	.unwrap_or(Default::default());
	measurements.push(make_measurement(&peer_mac_address, local, remote));
	}

	if device.can_start_data_transfer(session_id)
	&& peer_device.can_receive_data_transfer(session_id)
	{
	data_transfer.push(peer_device);
	}
	}

	// TODO: Data transfer should be limited in size for
	// each round of ranging
	for peer_device in data_transfer.iter() {
	peer_device
	.tx
	.send(
	DataMessageRcvBuilder {
	application_data: session.data().clone().into(),
	data_sequence_number: 0x01,
	pbf: PacketBoundaryFlag::Complete,
	session_handle: session_id,
	source_address: session.app_config.device_mac_address.unwrap().into(),
	status: uci::Status::Ok,
	}
	.build()
	.encode_to_vec()
	.unwrap(),
	)
	.unwrap();
	}
	if session.is_session_info_ntf_enabled() {
	device
	.tx
	.send(
	// TODO: support extended address
	ShortMacTwoWaySessionInfoNtfBuilder {
	sequence_number: session.sequence_number,
	session_token: session_id,
	rcr_indicator: 0, //TODO
	current_ranging_interval: 0, //TODO
	two_way_ranging_measurements: measurements,
	vendor_data: vec![],
	}
	.build()
	.encode_to_vec()
	.unwrap(),
	)
	.unwrap();

	let device = self.get_device_mut(device_handle).unwrap();
	let session = device.session_mut(session_id).unwrap();

	session.sequence_number += 1;
	}

	// TODO: Clean the data only when all the data is transfered
	let device = self.get_device_mut(device_handle).unwrap();
	let session = device.session_mut(session_id).unwrap();

	session.clear_data();
	}

	fn uci_packet(&mut self, device_handle: usize, packet: Vec<u8>) {
	match self.get_device_mut(device_handle) {
	Some(device) => device.receive_packet(packet),
	None => log::error!("Device {} not found", device_handle),
	}
	}

	fn pica_command(&mut self, command: PicaCommand) {
	use PicaCommand::*;
	match command {
	Connect(stream, sink) => {
	let _ = self.add_device(stream, sink);
	}
	Disconnect(device_handle) => self.disconnect(device_handle),
	Ranging(device_handle, session_id) => self.ranging(device_handle, session_id),
	StopRanging(mac_address, session_id) => {
	self.stop_controlee_ranging(&mac_address, session_id)
	}
	UciPacket(device_handle, packet) => self.uci_packet(device_handle, packet),
	CreateAnchor(mac_address, pica_cmd_rsp_tx) => {
	self.create_anchor(mac_address, pica_cmd_rsp_tx)
	}
	DestroyAnchor(mac_address, pica_cmd_rsp_tx) => {
	self.destroy_anchor(mac_address, pica_cmd_rsp_tx)
	}
	}
	}

	/// Run the internal pica event loop.
	pub async fn run(mut self) -> Result<()> {
	let Some(mut command_rx) = self.command_rx.take() else {
	anyhow::bail!("missing pica command receiver")
	};
	loop {
	if let Some(command) = command_rx.recv().await {
	self.pica_command(command)
	}
	}
	}

	// Handle the in-band StopRanging command sent from controller to the controlee with
	// corresponding mac_address and session_id.
	fn stop_controlee_ranging(&mut self, mac_address: &MacAddress, session_id: u32) {
	for device in self.devices.values_mut() {
	let Some(session) = device.session_mut(session_id) else {
	continue;
	};

	if session.app_config.device_mac_address != Some(*mac_address) {
	continue;
	}

	if session.session_state() == SessionState::SessionStateActive {
	session.stop_ranging_task();
	session.set_state(
	SessionState::SessionStateIdle,
	ReasonCode::SessionStoppedDueToInbandSignal,
	);
	device.n_active_sessions = device.n_active_sessions.saturating_sub(1);
	if device.n_active_sessions == 0 {
	device.set_state(DeviceState::DeviceStateReady);
	}
	} else {
	log::warn!("stop_controlee_ranging: session is not active !");
	}
	}
	}

	#[allow(clippy::map_entry)]
	fn create_anchor(
	&mut self,
	mac_address: MacAddress,
	rsp_tx: oneshot::Sender<Result<Handle, PicaCommandError>>,
	) {
	log::debug!("[_] Create anchor");
	log::debug!(" mac_address: {}", mac_address);

	let status = if self.get_category(&mac_address).is_some() {
	Err(PicaCommandError::DeviceAlreadyExists(mac_address))
	} else {
	let handle = self.counter;
	self.counter += 1;

	assert!(self
	.anchors
	.insert(
	mac_address,
	Anchor {
	handle,
	mac_address,
	},
	)
	.is_none());

	Ok(handle)
	};

	rsp_tx.send(status).unwrap_or_else(\|err\| {
	log::error!("Failed to send create-anchor command response: {:?}", err)
	})
	}

	fn destroy_anchor(
	&mut self,
	mac_address: MacAddress,
	rsp_tx: oneshot::Sender<Result<Handle, PicaCommandError>>,
	) {
	log::debug!("[_] Destroy anchor");
	log::debug!(" mac_address: {}", mac_address);

	let status = match self.anchors.remove(&mac_address) {
	None => Err(PicaCommandError::DeviceNotFound(mac_address)),
	Some(anchor) => Ok(anchor.handle),
	};

	rsp_tx.send(status).unwrap_or_else(\|err\| {
	log::error!("Failed to send destroy-anchor command response: {:?}", err)
	})
	}
	}

	/// Run the internal pica event loop.
	/// As opposed to Pica::run, the context is passed under a mutex, which
	/// allows synchronous access to the context for device creation.
	pub async fn run(this: &std::sync::Mutex<Pica>) -> Result<()> {
	// Extract the mpsc receiver from the Pica context.
	// The receiver cannot be cloned.
	let Some(mut command_rx) = this.lock().unwrap().command_rx.take() else {
	anyhow::bail!("missing pica command receiver");
	};

	loop {
	if let Some(command) = command_rx.recv().await {
	this.lock().unwrap().pica_command(command)
	}
	}
	}