Integrate hand gateway and spatial force rendering

src/serial_core/serial.rs

@@ -1,4 +1,5 @@
 use crate::serial_core::codec::Codec;
+use crate::serial_core::codecs::hand_gateway::{HandGatewayCodec, HandGatewayFrame};
 use crate::serial_core::codecs::tactile_a::TactileACodec;
 use crate::serial_core::frame::TactileAFrame;
 use crossbeam_channel::{Receiver, Sender};
@@ -6,6 +7,7 @@ use std::io::{Read, Write};
 use std::time::{Duration, Instant};
 
 const POLL_INTERVAL_MS: u64 = 10;
+const DEFAULT_HAND_GATEWAY_NODE_SAMPLE_COUNTS: &[u16] = &[84, 84, 84, 84, 84, 70, 44];
 
 #[derive(Debug, Default, Clone, Copy, PartialEq, Eq)]
 pub struct SerialIoStats {
@@ -13,9 +15,32 @@ pub struct SerialIoStats {
     pub tx_bytes: u64,
 }
 
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+pub enum SerialProtocol {
+    TactileA,
+    HandGateway,
+}
+
 /// Runs the serial polling loop on the calling (background) thread.
 /// Sends decoded pressure matrix data (Vec<i32>) to the output channel.
 pub fn run_serial_loop(
+    port: &mut dyn ReadWrite,
+    rows: usize,
+    cols: usize,
+    protocol: SerialProtocol,
+    cancel_rx: &Receiver<()>,
+    sample_tx: &Sender<Vec<i32>>,
+    stats_tx: Option<&Sender<SerialIoStats>>,
+) {
+    match protocol {
+        SerialProtocol::TactileA => {
+            run_tactile_a_loop(port, rows, cols, cancel_rx, sample_tx, stats_tx)
+        }
+        SerialProtocol::HandGateway => run_hand_gateway_loop(port, cancel_rx, sample_tx, stats_tx),
+    }
+}
+
+fn run_tactile_a_loop(
     port: &mut dyn ReadWrite,
     rows: usize,
     cols: usize,
@@ -86,6 +111,97 @@ pub fn run_serial_loop(
     }
 }
 
+fn run_hand_gateway_loop(
+    port: &mut dyn ReadWrite,
+    cancel_rx: &Receiver<()>,
+    sample_tx: &Sender<Vec<i32>>,
+    stats_tx: Option<&Sender<SerialIoStats>>,
+) {
+    let session_started_at = Instant::now();
+    let mut codec = HandGatewayCodec::new(DEFAULT_HAND_GATEWAY_NODE_SAMPLE_COUNTS);
+    let mut buffer = [0u8; 1024];
+    let poll_interval = Duration::from_millis(POLL_INTERVAL_MS);
+    let mut io_stats = SerialIoStats::default();
+
+    loop {
+        if cancel_rx.try_recv().is_ok() {
+            break;
+        }
+
+        let deadline = Instant::now() + poll_interval;
+        loop {
+            if Instant::now() >= deadline {
+                break;
+            }
+
+            match port.read(&mut buffer) {
+                Ok(n) if n > 0 => {
+                    io_stats.rx_bytes += n as u64;
+                    publish_stats(stats_tx, io_stats);
+                    if let Ok(frames) = codec.decode(&buffer[..n], session_started_at) {
+                        for frame in frames {
+                            let HandGatewayFrame::DataRep(rep) = frame;
+                            // println!(
+                            //     "[hand-packet-raw] bytes={} timestamp_us={} config=0x{:08X} valid_config=0x{:08X} block_count={} payload_bytes={} raw={}",
+                            //     rep.raw.len(),
+                            //     rep.timestamp,
+                            //     rep.config,
+                            //     rep.valid_config,
+                            //     rep.block_count,
+                            //     rep.nodes
+                            //         .iter()
+                            //         .map(|node| node.payload.len())
+                            //         .sum::<usize>(),
+                            //     format_hex_bytes(&rep.raw)
+                            // );
+                            let mut vals = Vec::new();
+                            let mut parse_ok = true;
+                            for node in &rep.nodes {
+                                match HandGatewayCodec::parse_node_payload(&node.payload) {
+                                    Ok(node_values) => {
+                                        vals.extend(node_values.into_iter().map(|raw| {
+                                            let raw = raw as i32;
+                                            if raw < 15 { 0 } else { raw }
+                                        }));
+                                    }
+                                    Err(err) => {
+                                        parse_ok = false;
+                                        eprintln!("[hand-packet-values] parse error: {err}");
+                                        break;
+                                    }
+                                }
+                            }
+
+                            if parse_ok {
+                                // println!("[hand-packet-values] samples={}", vals.len());
+                                let _ = sample_tx.try_send(vals);
+                            }
+                        }
+                    }
+                }
+                Ok(_) => {
+                    std::thread::sleep(Duration::from_millis(1));
+                }
+                Err(ref e) if e.kind() == std::io::ErrorKind::TimedOut => {
+                    continue;
+                }
+                Err(e) => {
+                    eprintln!("[serial] hand gateway read error: {e}");
+                    return;
+                }
+            }
+        }
+    }
+}
+
+fn format_hex_bytes(bytes: &[u8]) -> String {
+    bytes
+        .iter()
+        .map(|byte| format!("{byte:02X}"))
+        .collect::<Vec<_>>()
+        .join(" ")
+}
+
 fn publish_stats(stats_tx: Option<&Sender<SerialIoStats>>, stats: SerialIoStats) {
     if let Some(tx) = stats_tx {
         let _ = tx.try_send(stats);