From 498a81318e296c494926905df8ca0bebaff6816a Mon Sep 17 00:00:00 2001
From: Frederik Menke <frederik.menke@rwth-aachen.de>
Date: Fri, 21 Oct 2022 12:05:27 +0200
Subject: [PATCH] Create Gcode abstraction

---
 green/Marlin             |  2 +-
 red/Cargo.toml           |  2 ++
 red/src/printer/gcode.rs | 56 ++++++++++++++++++++++++++++++++++++++++
 red/src/printer/mod.rs   |  9 ++++---
 4 files changed, 64 insertions(+), 5 deletions(-)
 create mode 100644 red/src/printer/gcode.rs

diff --git a/green/Marlin b/green/Marlin
index b58bbb5..84230b9 160000
--- a/green/Marlin
+++ b/green/Marlin
@@ -1 +1 @@
-Subproject commit b58bbb57e5ca2a5e6d7af0ade2c191b2e89f6081
+Subproject commit 84230b9a1de5a9d41242839bec29a7188d7b3eca
diff --git a/red/Cargo.toml b/red/Cargo.toml
index ead1142..941b501 100644
--- a/red/Cargo.toml
+++ b/red/Cargo.toml
@@ -9,6 +9,8 @@ edition = "2021"
 bytes = "1.2.1"
 euclid = "0.22.7"
 futures = "0.3.24"
+lazy_static = "1.4.0"
+regex = "1.6.0"
 tokio = { version = "1.21.0", features = ["full"] }
 tokio-serial = "5.4.3"
 tokio-util = { version = "0.7.4", features = ["codec"] }
diff --git a/red/src/printer/gcode.rs b/red/src/printer/gcode.rs
new file mode 100644
index 0000000..b7bd480
--- /dev/null
+++ b/red/src/printer/gcode.rs
@@ -0,0 +1,56 @@
+use lazy_static::lazy_static;
+use regex::Regex;
+
+pub trait GcodeCommand<'a, Response> {
+    fn command(&self) -> &'a str;
+    fn parse_reply(reply: &str) -> Response;
+}
+
+#[derive(Debug)]
+pub struct M114Command;
+
+#[derive(Debug)]
+pub struct M114Reply {
+    x: Option<f64>,
+    y: Option<f64>,
+    z: Option<f64>,
+    e: Option<f64>,
+}
+
+impl M114Command {
+    pub fn new() -> Self {
+        M114Command
+    }
+}
+
+impl GcodeCommand<'_, M114Reply> for M114Command {
+    fn command(&self) -> &'static str {
+        "M114"
+    }
+    fn parse_reply(reply: &str) -> M114Reply {
+        lazy_static! {
+            static ref RE_SET: Vec<Regex> = vec![
+                Regex::new(r"X:(\d+(?:\.\d+))").unwrap(),
+                Regex::new(r"Y:(\d+(?:\.\d+))").unwrap(),
+                Regex::new(r"Z:(\d+(?:\.\d+))").unwrap(),
+                Regex::new(r"E:(\d+(?:\.\d+))").unwrap(),
+            ];
+        }
+
+        let fields: Vec<Option<f64>> = RE_SET
+            .iter()
+            .map(|re| {
+                re.captures(reply)
+                    .and_then(|cpt| cpt.get(1).map(|mtch| mtch.as_str().to_string()))
+                    .and_then(|s| s.parse().ok())
+            })
+            .collect();
+
+        M114Reply {
+            x: fields[0],
+            y: fields[1],
+            z: fields[2],
+            e: fields[3],
+        }
+    }
+}
diff --git a/red/src/printer/mod.rs b/red/src/printer/mod.rs
index 41da1f7..8aa8bd9 100644
--- a/red/src/printer/mod.rs
+++ b/red/src/printer/mod.rs
@@ -1,9 +1,11 @@
+mod gcode;
+
 use bytes::BytesMut;
 use futures::sink::SinkExt;
 use futures::stream::StreamExt;
 use std::{fmt::Write, io, rc::Rc, str};
 use tokio;
-use tokio_serial::{SerialPortBuilderExt};
+use tokio_serial::SerialPortBuilderExt;
 use tokio_util::codec::{Decoder, Encoder};
 
 const DEFAULT_TTY: &str = "/dev/ttyUSB0";
@@ -47,8 +49,7 @@ impl Printer {
                     let reply = reply.unwrap();
                     if reply.contains("Ok") {
                         break;
-                    }
-                    else {
+                    } else {
                         println!("got reply: {}", reply);
                     }
                 }
@@ -95,4 +96,4 @@ impl Encoder<String> for LineCodec {
             .map_err(|e| io::Error::new(io::ErrorKind::InvalidData, e.to_string()))?;
         Ok(())
     }
-}
\ No newline at end of file
+}