From 008d3af930fe908a135d0c890adbdf9a95103ee8 Mon Sep 17 00:00:00 2001
From: Hartmut Seichter <hartmut@technotecture.com>
Date: Mon, 12 Dec 2022 23:18:34 +0100
Subject: [PATCH] update to try with powerwall

---
 src/main.rs    |   2 +-
 src/tracker.rs | 116 ++++++++++++++++++++++++++++---------------------
 2 files changed, 68 insertions(+), 50 deletions(-)

diff --git a/src/main.rs b/src/main.rs
index dcb0af0..01e26e3 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -48,7 +48,7 @@ fn main() {
 
         .add_startup_system(offaxis_camera_setup)
         .add_startup_system(setup_tracker)
-        .add_system(update_tracker)
+        // .add_system(update_tracker)
         
         .add_plugin(CameraProjectionPlugin::<OffAxisProjection>::default())
         .add_system(simulate_viewer)
diff --git a/src/tracker.rs b/src/tracker.rs
index 72ccce7..1a64fc6 100644
--- a/src/tracker.rs
+++ b/src/tracker.rs
@@ -1,39 +1,42 @@
 use bevy::prelude::*;
 
-extern crate vrpn;
 extern crate bytes;
+extern crate vrpn;
+
+use bytes::{Bytes, BytesMut};
+use std::{
+    io::{Read,self},
+    net::{SocketAddr, TcpStream},
+};
 
-// extern crate tokio;
 
-use bytes::Bytes;
-use std::net::{SocketAddr, TcpStream};
 use vrpn::{
+    codec::peek_u32,
     constants::MAGIC_DATA,
     cookie::check_ver_nonfile_compatible,
-    handler::{HandlerCode, TypedHandler},
-    sync_io::{read_cookie, write_cookie, EndpointSyncTcp},
-    tracker::PoseReport,
-    CookieData, Message, Result, TypeDispatcher, Unbuffer,
+    message::MessageSize,
+    sync_io::{read_cookie, write_cookie},
+    CookieData, Message, Result, SequencedGenericMessage, Unbuffer,
 };
 
 #[derive(Component)]
 pub struct Tracker {
-    endpoint: EndpointSyncTcp,
-    dispatcher: TypeDispatcher
-    // stream: ConnectionIpStream,
-    // connection: Arc<ConnectionIp>
+    stream: TcpStream, // endpoint: EndpointSyncTcp,
+                       // dispatcher: TypeDispatcher
+                       // stream: ConnectionIpStream,
+                       // connection: Arc<ConnectionIp>
 }
 
-#[derive(Debug)]
-struct TrackerHandler {}
+// #[derive(Debug)]
+// struct TrackerHandler {}
 
-impl TypedHandler for TrackerHandler {
-    type Item = PoseReport;
-    fn handle_typed(&mut self, msg: &Message<PoseReport>) -> Result<HandlerCode> {
-        info!("{:?}\n   {:?}", msg.header, msg.body);
-        Ok(HandlerCode::ContinueProcessing)
-    }
-}
+// impl TypedHandler for TrackerHandler {
+//     type Item = PoseReport;
+//     fn handle_typed(&mut self, msg: &Message<PoseReport>) -> Result<HandlerCode> {
+//         info!("{:?}\n   {:?}", msg.header, msg.body);
+//         Ok(HandlerCode::ContinueProcessing)
+//     }
+// }
 
 impl Tracker {
     pub fn from_info(server_ip: &str, port: u16) -> Self {
@@ -50,51 +53,66 @@ impl Tracker {
         // let ping_client = ping::Client::new(sender, Arc::clone(&connection))
         //     .expect("should be able to create ping client");
 
+        let addr: SocketAddr = format!("{}:{}", server_ip.to_string(), port)
+            .parse()
+            .unwrap();
 
-        let addr: SocketAddr = "127.0.0.1:3883".parse().unwrap();
         let mut stream = TcpStream::connect(addr).unwrap();
         stream.set_nodelay(true).unwrap();
-    
+
         // We first write our cookie, then read and check the server's cookie, before the loop.
         write_cookie(&mut stream, CookieData::from(MAGIC_DATA)).unwrap();
         let cookie_buf = read_cookie(&mut stream).unwrap();
-        let mut cookie_buf = Bytes::from(&cookie_buf[..]);
-    
+
+        // let mut cookie_buf = Bytes::from(&cookie_buf[..]);
+
         // CookieData::unbuffer_ref(&mut cookie_buf)
-        //     .and_then(|msg| check_ver_nonfile_compatible(msg.version))?;
-    
-        let mut endpoint = EndpointSyncTcp::new(stream);
-        let mut dispatcher = TypeDispatcher::new();
-        let _ = dispatcher.add_typed_handler(Box::new(TrackerHandler {}), None).unwrap();
-
-        Tracker { 
-            endpoint,
-            dispatcher
-        }
+        // .and_then(|msg| check_ver_nonfile_compatible(msg.version)).unwrap();
 
+        Tracker { stream }
     }
 
-    fn update(&self) {
-        self.endpoint.poll_endpoint(&mut self.dispatcher).unwrap();
-    }
+    pub fn update(&mut self) -> vrpn::Result<()> {
 
-}
 
-pub fn setup_tracker(mut commands: Commands)
-{
-    commands.spawn(Tracker::from_info("127.0.0.1",3883));
+        let mut buf = BytesMut::new();
 
-}
+        // Read the message header and padding
+        buf.resize(24, 0);
+        self.stream.read_exact(buf.as_mut())?;
 
-pub fn update_tracker(mut query:Query<&mut Tracker>)
-{
-    for mut tracker in query.iter_mut() {
+        // Peek the size field, to compute the MessageSize.
+        let total_len = peek_u32(&buf.clone().freeze())?.unwrap();
+        let size = MessageSize::from_length_field(total_len);
 
-        // tracker.update();
+        // Read the body of the message
+        let mut body_buf = BytesMut::new();
+        body_buf.resize(size.padded_body_size(), 0);
+        self.stream.read_exact(body_buf.as_mut())?;
 
-        // tracker.connection.poll_endpoints();
-        // tracker
+        // Combine the body with the header
+        buf.extend_from_slice(&body_buf[..]);
+        let mut buf = buf.freeze();
+
+        // Unbuffer the message.
+        let unbuffered = SequencedGenericMessage::unbuffer_ref(&mut buf)?;
+        eprintln!("{:?}", Message::from(unbuffered));
+
+        Ok(())
     }
 }
 
+pub fn setup_tracker(mut commands: Commands) {
+    // commands.spawn(Tracker::from_info("127.0.0.1", 3883));
+}
 
+// pub fn update_tracker(mut query:Query<&mut Tracker>)
+// {
+//     for mut tracker in query.iter_mut() {
+
+//         // tracker.update();
+
+//         // tracker.connection.poll_endpoints();
+//         // tracker
+//     }
+// }