From 821b15b59993b0ae45cadd9a7240dbc9876c9230 Mon Sep 17 00:00:00 2001
From: Frederik Menke <frederik.menke@rwth-aachen.de>
Date: Sat, 9 Dec 2023 19:58:03 +0100
Subject: [PATCH] Parse "ok" lines to count buffers

---
 red/src/printer/gcode/g91.rs |  2 +-
 red/src/printer/mod.rs       | 58 ++++++++++++++++++++++++++++--------
 2 files changed, 46 insertions(+), 14 deletions(-)

diff --git a/red/src/printer/gcode/g91.rs b/red/src/printer/gcode/g91.rs
index 5887290..b15f689 100644
--- a/red/src/printer/gcode/g91.rs
+++ b/red/src/printer/gcode/g91.rs
@@ -1,6 +1,6 @@
 use super::*;
 
-/// Auto Home
+/// Relative Positioning
 #[derive(Debug)]
 pub struct G91Command;
 
diff --git a/red/src/printer/mod.rs b/red/src/printer/mod.rs
index c44239b..17c3d4f 100644
--- a/red/src/printer/mod.rs
+++ b/red/src/printer/mod.rs
@@ -3,6 +3,7 @@ pub mod gcode;
 use bytes::BytesMut;
 use futures::sink::SinkExt;
 use futures::stream::{SplitSink, SplitStream, StreamExt};
+use regex::Regex;
 use std::time::Duration;
 use std::{fmt::Write, io, rc::Rc, str};
 use tokio;
@@ -12,7 +13,7 @@ use tokio_util::codec::{Decoder, Encoder, Framed};
 
 pub use gcode::{GcodeCommand, GcodeReply};
 
-use crate::printer::gcode::G91Command;
+use crate::printer::gcode::M114Command;
 
 use self::gcode::GcodeReplyError;
 
@@ -27,6 +28,8 @@ pub enum PrinterError {
     PrinterTaskDown,
     OutputChannelDropped,
     GcodeReply(GcodeReplyError),
+    // If the "ok" line can't be parsed
+    ConfirmationError { parsed_string: String },
     NoResponseFromPrinter(String),
 }
 
@@ -35,9 +38,10 @@ pub struct State {
 }
 
 pub struct Printer {
+    pub state: Rc<State>,
     serial_tx: SplitSink<Framed<SerialStream, LineCodec>, String>,
     serial_rx: SplitStream<Framed<SerialStream, LineCodec>>,
-    pub state: Rc<State>,
+    last_buffer_capacity: usize,
 }
 
 impl Printer {
@@ -47,9 +51,7 @@ impl Printer {
         command: Box<dyn GcodeCommand>,
     ) -> Result<GcodeReply, PrinterError> {
         let command_text = format!("{}\n", command.command());
-        println!("sending in: {:?}", command_text);
         self.serial_tx.send(command_text.clone()).await.unwrap();
-        // TODO: figure out how big a simple "OK"-reply is and use `with_capacity` for the `reply`
         let mut reply = String::with_capacity(RECV_BUFFER_CAPACITY);
         loop {
             // TODO: add timeout below
@@ -63,8 +65,8 @@ impl Printer {
                 ))?;
             let line = line.unwrap();
             if line.contains("ok") {
+                self.last_buffer_capacity = Self::parse_ok(&line)?;
                 let reply = command.parse_reply(&reply);
-                println!("got reply: {:?}", reply);
                 return reply.map_err(PrinterError::GcodeReply);
             } else {
                 reply.push_str(&line);
@@ -103,25 +105,44 @@ impl Printer {
                     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
-                    .expect("Could not write to serial port!");
+                break;
             }
         }
+        println!("Sending M114 command");
 
-        Ok(Printer {
+        let mut res = Printer {
             serial_rx,
             serial_tx,
             state: Rc::new(State {
                 position: (0.0, 0.0, 0.0, 0.0),
             }),
-        })
+            last_buffer_capacity: 0, // this is updated on the next call to `send_gcode()`
+        };
+
+        res.send_gcode(Box::new(M114Command))
+            .await
+            .expect("Could not ask for current position!");
+
+        Ok(res)
+    }
+
+    /// Parse the "Ok" confirmation line that the printer sends after every successfully received
+    /// command.
+    fn parse_ok(line: &str) -> Result<usize, PrinterError> {
+        let make_err = || PrinterError::ConfirmationError {
+            parsed_string: line.to_string(),
+        };
+        let re = Regex::new(r"ok P(\d+) B(\d+)").unwrap();
+        let captures = re.captures(line).ok_or_else(make_err)?;
+        captures
+            .get(1)
+            .unwrap()
+            .as_str()
+            .parse()
+            .map_err(|_| make_err())
     }
 }
 
@@ -157,3 +178,14 @@ impl Encoder<String> for LineCodec {
         Ok(())
     }
 }
+
+#[cfg(test)]
+mod test {
+    use super::Printer;
+
+    #[test]
+    fn test_parse_ok() {
+        let buffer_cap = Printer::parse_ok("ok P10 B4564").unwrap();
+        assert!(buffer_cap == 10);
+    }
+}