From 594a31d983de269d247f5a3b33a14a1da5bd43fa Mon Sep 17 00:00:00 2001
From: Frederik Menke <frederik.menke@rwth-aachen.de>
Date: Sun, 19 Nov 2023 02:10:27 +0100
Subject: [PATCH] Fix reconnecting

---
 red/src/jogger.rs             |  3 +--
 red/src/printer/gcode/m997.rs | 23 +++++++++++++++++++++++
 red/src/printer/gcode/mod.rs  |  2 ++
 red/src/printer/mod.rs        | 32 +++++++++++++++++++++++++++++---
 4 files changed, 55 insertions(+), 5 deletions(-)
 create mode 100644 red/src/printer/gcode/m997.rs

diff --git a/red/src/jogger.rs b/red/src/jogger.rs
index ae6306d..6200e6b 100644
--- a/red/src/jogger.rs
+++ b/red/src/jogger.rs
@@ -10,7 +10,7 @@ use std::time::Duration;
 ///
 /// For a given GCode buffer size on the machine, we can assume
 /// a maximum delay of `TIME_PER_MOVEMENT * BUFFER_COUNT`
-const TIME_PER_MOVEMENT: Duration = Duration::from_millis(20);
+const TIME_PER_MOVEMENT: Duration = Duration::from_millis(50);
 /// Movement speed of gantry when going full throttle in x/y-direction
 /// in units/min (mm/min)
 const FULL_SCALE_SPEED_XY: f64 = 1000.0;
@@ -51,6 +51,5 @@ pub async fn jog(gamepad: Arc<Gamepad>, mut printer: Printer) -> Never {
             velocity: velocity.into(),
         };
         printer.send_gcode(Box::new(command)).await;
-        std::thread::sleep(TIME_PER_MOVEMENT);
     }
 }
diff --git a/red/src/printer/gcode/m997.rs b/red/src/printer/gcode/m997.rs
new file mode 100644
index 0000000..5ea44ea
--- /dev/null
+++ b/red/src/printer/gcode/m997.rs
@@ -0,0 +1,23 @@
+use super::*;
+
+/// Auto Home
+#[derive(Debug)]
+pub struct M997Command;
+
+impl GcodeCommand for M997Command {
+    fn command(&self) -> String {
+        "M997".into()
+    }
+
+    fn parse_reply(&self, reply: &str) -> Result<GcodeReply> {
+        if !reply.is_empty() {
+            Err(GcodeReplyError {
+                sent_command: self.command(),
+                parsed_input: reply.to_string(),
+                problem: "Expected no reply".to_string(),
+            })
+        } else {
+            Ok(GcodeReply::NoReply)
+        }
+    }
+}
diff --git a/red/src/printer/gcode/mod.rs b/red/src/printer/gcode/mod.rs
index 87356d4..6f69d9d 100644
--- a/red/src/printer/gcode/mod.rs
+++ b/red/src/printer/gcode/mod.rs
@@ -13,10 +13,12 @@ mod g0;
 mod g28;
 mod g91;
 mod m114;
+mod m997;
 pub use g0::G0Command;
 pub use g28::G28Command;
 pub use g91::G91Command;
 pub use m114::M114Command;
+pub use m997::M997Command;
 use std::fmt::Debug;
 
 type Result<T> = std::result::Result<T, GcodeReplyError>;
diff --git a/red/src/printer/mod.rs b/red/src/printer/mod.rs
index 16ed268..864821a 100644
--- a/red/src/printer/mod.rs
+++ b/red/src/printer/mod.rs
@@ -3,14 +3,19 @@ pub mod gcode;
 use bytes::BytesMut;
 use futures::sink::SinkExt;
 use futures::stream::StreamExt;
+use futures::TryStreamExt;
+use std::time::Duration;
 use std::{fmt::Write, io, rc::Rc, str};
 use tokio;
 use tokio::sync::mpsc::{channel, Receiver, Sender};
+use tokio::time::timeout;
 use tokio_serial::SerialPortBuilderExt;
 use tokio_util::codec::{Decoder, Encoder};
 
 pub use gcode::{GcodeCommand, GcodeReply};
 
+use crate::printer::gcode::{G91Command, M997Command};
+
 use self::gcode::GcodeReplyError;
 
 #[derive(Debug)]
@@ -63,9 +68,30 @@ impl Printer {
 
             // The printer will send some info after connecting. We need to wait for this
             // to be received as it will otherwise stop responding for some reason:
-            while let Some(stuff) = serial_rx.next().await {
-                if stuff.map(|s| s.contains("Loaded")).unwrap_or(false) {
-                    break;
+            loop {
+                if let Ok(message) = timeout(Duration::from_secs(10), serial_rx.next()).await {
+                    match message {
+                        Some(Ok(reply)) => {
+                            println!("got stuff: {:?}", reply);
+                            if reply.contains("Loaded") {
+                                break;
+                            }
+                            if reply.contains("ok") {
+                                break;
+                            }
+                        }
+                        Some(Err(e)) => {
+                            println!("Error reading from serial port: {:?}", e);
+                        }
+                        None => (),
+                    }
+                } else {
+                    // TODO: Check if port is good by sending some harmless gcode
+                    println!(
+                        "Reading from serial port timed out. Printer might already be initialized."
+                    );
+                    println!("Sending G91 command");
+                    serial_tx.send(G91Command.command() + "\n").await;
                 }
             }