initial integration with telem viz

ground/src/command.rs

@@ -0,0 +1,121 @@
+use api::client::Client;
+use api::client::command::CommandRegistry;
+use api::macros::IntoCommandDefinition;
+use chrono::TimeDelta;
+use itertools::Itertools;
+use log::{error, trace};
+use nautilus_common::command::{Command, OwnedCommandHeader, SetPin, ValidPriorityCommand};
+use nautilus_common::udp::tokio::AsyncUdpSocketExt;
+use std::fmt::Debug;
+use std::net::SocketAddr;
+use std::sync::Arc;
+use tokio::net::UdpSocket;
+use tokio::select;
+use tokio::sync::RwLock;
+use tokio::sync::mpsc::{Sender, channel};
+use tokio_util::sync::CancellationToken;
+
+pub struct CommandHandler<'a> {
+    cmd: CommandRegistry,
+    flight_addr: &'a RwLock<SocketAddr>,
+    udp: &'a UdpSocket,
+    cancel: CancellationToken,
+}
+
+#[derive(IntoCommandDefinition)]
+struct SetPinCommand {
+    state: bool,
+}
+
+impl<'a> CommandHandler<'a> {
+    pub fn new(
+        client: Arc<Client>,
+        flight_addr: &'a RwLock<SocketAddr>,
+        udp: &'a UdpSocket,
+        cancel: CancellationToken,
+    ) -> Self {
+        Self {
+            cmd: CommandRegistry::new(client),
+            flight_addr,
+            udp,
+            cancel,
+        }
+    }
+
+    pub fn run(self) -> anyhow::Result<()> {
+        let runtime = tokio::runtime::Builder::new_current_thread()
+            .enable_all()
+            .build()?;
+
+        runtime.block_on(async move {
+            let (outgoing_commands_tx, mut outgoing_commands_rx) = channel::<OwnedCommandHeader>(16);
+
+            let commands = ["a", "b"].iter().cartesian_product(0..16)
+                .map(|(bank, i)| {
+                    let command_name = format!("/mcp23017{bank}/set");
+                    let outgoing_commands_tx = outgoing_commands_tx.clone();
+                    self.cmd.register_handler(
+                        format!("switch.{bank}.{i}.set"),
+                        move |header, cmd: SetPinCommand| -> anyhow::Result<_> {
+                            trace!("Setting Switch {bank} {i}");
+
+                            outgoing_commands_tx.try_send_command(
+                                &command_name,
+                                &ValidPriorityCommand {
+                                    inner: SetPin {
+                                        pin: i,
+                                        value: cmd.state,
+                                    },
+                                    valid_until: header.timestamp + TimeDelta::seconds(5),
+                                    priority: 0,
+                                }
+                            )?;
+
+                            Ok("Command Executed Successfully".to_string())
+                        }
+                    )
+                })
+                .collect::<Vec<_>>();
+
+            let mut buffer = [0u8; 512];
+            while !self.cancel.is_cancelled() {
+                select! {
+                    () = self.cancel.cancelled() => { break; }
+                    outgoing = outgoing_commands_rx.recv() => {
+                        match outgoing {
+                            None => break,
+                            Some(outgoing) => {
+                                match self.udp.send_postcard(&outgoing, &mut buffer, *self.flight_addr.read().await).await {
+                                    Ok(()) => {},
+                                    Err(err) => error!("Failed to Send Outgoing {err}"),
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+
+            // Explicit Drops
+            drop(commands);
+            drop(self);
+        });
+
+        Ok(())
+    }
+}
+
+trait SenderExt {
+    fn try_send_command<T: Command + Debug>(&self, name: &str, data: &T) -> anyhow::Result<()>;
+}
+
+impl SenderExt for Sender<OwnedCommandHeader> {
+    fn try_send_command<T: Command + Debug>(&self, name: &str, data: &T) -> anyhow::Result<()> {
+        trace!("{data:?}");
+        let inner_buffer = postcard::to_allocvec(data)?;
+        self.try_send(OwnedCommandHeader {
+            name: name.to_string(),
+            data: inner_buffer,
+        })?;
+        Ok(())
+    }
+}