From 5231c3c9b785e278defa619d6e8907e1d0088b6b Mon Sep 17 00:00:00 2001
From: Robert Sammelson <robert@sammelson.com>
Date: Sat, 13 May 2023 19:03:41 -0400
Subject: [PATCH] Create general-purpose command functions

This will allow additional commands to be implemented more easily
---
 src/obd2/mod.rs | 70 +++++++++++++++++++++++++++++++++----------------
 1 file changed, 47 insertions(+), 23 deletions(-)

diff --git a/src/obd2/mod.rs b/src/obd2/mod.rs
index 5d46302..65635e2 100644
--- a/src/obd2/mod.rs
+++ b/src/obd2/mod.rs
@@ -1,3 +1,5 @@
+use log::{debug, trace};
+
 mod device;
 
 type Result<T> = std::result::Result<T, Error>;
@@ -8,13 +10,46 @@ pub struct Obd2 {
 }
 
 impl Obd2 {
-    pub fn get_vin(&mut self) -> Result<String> {
+    pub fn obd_command(&mut self, mode: u8, pid: u8) -> Result<Vec<u8>> {
+        let result = self.command(&format!("{:02x}{:02x}", mode, pid))?;
+
+        if result.first() != Some(&(0x40 | mode)) {
+            todo!()
+        }
+        if result.get(1) != Some(&pid) {
+            todo!()
+        }
+
+        Ok(result.split_at(2).1.to_vec())
+    }
+
+    fn command(&mut self, command: &str) -> Result<Vec<u8>> {
         let response = self
             .device
-            .cmd("0902")?
-            .ok_or(Error::Other("no response to get vin command".to_owned()))?;
+            .cmd(command)?
+            .ok_or(Error::Other("no response to command".to_owned()))?;
+
+        trace!(
+            "Sent OBD command {:?} and got response {:?}",
+            command,
+            response
+        );
+
+        let data = self.parse_command_multiline(response)?;
+
+        debug!("Sent OBD command {:?} and got data {:?}", command, data);
+
+        let result = data
+            .iter()
+            .map(|s| u8::from_str_radix(s, 16).map_err(|e| e.into()))
+            .collect();
+
+        result
+    }
+
+    fn parse_command_multiline(&mut self, response: String) -> Result<Vec<String>> {
         let mut n_idx = 0;
-        let data: Vec<String> = response
+        Ok(response
             .split('\n')
             .filter_map(|l| l.split_once(':'))
             .flat_map(|(idx, data)| {
@@ -24,24 +59,13 @@ impl Obd2 {
                 n_idx = (n_idx + 1) % 0x10;
                 data.split_whitespace().map(|s| s.to_owned())
             })
-            .collect();
+            .collect())
+    }
 
-        if data.get(0).map(|s| s.as_str()) != Some("49") {
-            todo!()
-        }
-        if data.get(1).map(|s| s.as_str()) != Some("02") {
-            todo!()
-        }
-
-        let vin = String::from_utf8(
-            data.split_at(3)
-                .1
-                .iter()
-                .map(|s| u8::from_str_radix(s, 16).map_err(|e| e.into()))
-                .collect::<Result<_>>()?,
-        )?;
-
-        Ok(vin)
+    pub fn get_vin(&mut self) -> Result<String> {
+        let mut result = self.obd_command(0x09, 0x02)?;
+        result.remove(0); // do not know what this byte is
+        Ok(String::from_utf8(result)?)
     }
 }
 
@@ -61,12 +85,12 @@ impl From<device::Error> for Error {
 
 impl From<std::num::ParseIntError> for Error {
     fn from(e: std::num::ParseIntError) -> Self {
-        Error::Other(format!("Invalid data recieved: {:?}", e))
+        Error::Other(format!("invalid data recieved: {:?}", e))
     }
 }
 
 impl From<std::string::FromUtf8Error> for Error {
     fn from(e: std::string::FromUtf8Error) -> Self {
-        Error::Other(format!("Invalid string recieved: {:?}", e))
+        Error::Other(format!("invalid string recieved: {:?}", e))
     }
 }