simulation: rewrite the simulation data processing to share a lot of the same code on both the build script and the chip, allowing for more general data streaming possibilities

src/simdata.rs

@@ -1,3 +1,32 @@
+use rmp::{Marker, decode::{ExtMeta, RmpRead, ValueReadError}, encode::{RmpWrite, ValueWriteError}};
+
+pub trait RmpData: Sized {
+    fn from_rmp<Reader: RmpRead>(reader: &mut Reader) -> Result<Self, SimDataError<ValueReadError<Reader::Error>>>;
+    fn write_rmp<Writer: RmpWrite>(&self, writer: &mut Writer) -> Result<(), ValueWriteError<Writer::Error>>;
+}
+
+pub trait EventRecord: RmpData {
+    fn field_count() -> usize;
+}
+
+#[derive(Debug)]
+pub enum SimDataError<E> {
+    StreamIndexMissing,
+    InvalidChunkSize { expected: usize, found: usize },
+    MissingTimecode,
+    BadString,
+    DecodeError(E),
+    EndOfStream,
+    UnsupportedStreamType(ExtMeta),
+    EventHeaderMissing
+}
+
+impl<E> From<E> for SimDataError<E> {
+    fn from(value: E) -> Self {
+        SimDataError::DecodeError(value)
+    }
+}
+
 #[derive(Debug)]
 pub enum StreamType {
     IMU,
@@ -26,4 +55,187 @@ impl From<StreamType> for i8 {
             StreamType::Annotations => 3
         }
     }
+}
+
+#[derive(Debug, Default)]
+pub struct StreamIndex {
+    pub count: usize
+}
+
+impl RmpData for StreamIndex {
+    fn from_rmp<Reader: RmpRead>(reader: &mut Reader) -> Result<Self, SimDataError<ValueReadError<Reader::Error>>> {
+        rmp::decode::read_array_len(reader).map(|count| {
+            Self {
+                count: count as usize
+            }
+        }).map_err(|_| { SimDataError::StreamIndexMissing })
+    }
+
+    fn write_rmp<Writer: RmpWrite>(&self, writer: &mut Writer) -> Result<(), ValueWriteError<Writer::Error>> {
+        rmp::encode::write_array_len(writer, self.count as u32)?;
+        Ok(())
+    }
+}
+
+#[derive(Debug)]
+pub struct StreamHeader {
+    pub id: StreamType,
+    pub size: usize,
+}
+
+impl RmpData for StreamHeader {
+    fn from_rmp<Reader: RmpRead>(reader: &mut Reader) -> Result<Self, SimDataError<ValueReadError<Reader::Error>>> {
+        let meta = rmp::decode::read_ext_meta(reader)?;
+        if let Ok(id) = meta.typeid.try_into() {
+            Ok(Self {
+                id,
+                size: meta.size as usize
+            })
+        } else {
+            Err(SimDataError::UnsupportedStreamType(meta))
+        }
+    }
+
+    fn write_rmp<Writer: RmpWrite>(&self, writer: &mut Writer) -> Result<(), ValueWriteError<Writer::Error>> {
+        todo!()
+    }
+}
+
+#[derive(Debug)]
+pub struct EventStreamHeader {
+    pub count: usize
+}
+
+impl RmpData for EventStreamHeader {
+    fn from_rmp<Reader: RmpRead>(reader: &mut Reader) -> Result<Self, SimDataError<ValueReadError<Reader::Error>>> {
+        Ok(Self {
+            count: rmp::decode::read_array_len(reader)? as usize
+        })
+    }
+
+    fn write_rmp<Writer: RmpWrite>(&self, writer: &mut Writer) -> Result<(), ValueWriteError<Writer::Error>> {
+        rmp::encode::write_array_len(writer, self.count as u32)?;
+        Ok(())
+    }
+}
+
+#[derive(Default, Debug)]
+pub struct StreamEvent<Event: EventRecord> {
+    pub timecode: f64,
+    pub data: Event
+}
+
+impl<Event: EventRecord> RmpData for StreamEvent<Event> {
+    fn from_rmp<Reader: RmpRead>(reader: &mut Reader) -> Result<Self, SimDataError<ValueReadError<Reader::Error>>> {
+        let chunk_len = rmp::decode::read_array_len(reader).map_err(|_| { SimDataError::EventHeaderMissing })? as usize;
+        // Add 1 to the field count for the timestamp
+        if chunk_len != Event::field_count() + 1 {
+            Err(SimDataError::InvalidChunkSize { expected: Event::field_count(), found: chunk_len })
+        } else {
+            let timecode = rmp::decode::read_f64(reader).map_err(|_| { SimDataError::MissingTimecode })?;
+            Ok(Self {
+                timecode,
+                data: Event::from_rmp(reader)?
+            })
+        }
+    }
+
+    fn write_rmp<Writer: RmpWrite>(&self, writer: &mut Writer) -> Result<(), ValueWriteError<Writer::Error>> {
+        rmp::encode::write_array_len(writer, Event::field_count() as u32 + 1)?;
+        rmp::encode::write_f64(writer, self.timecode)?;
+        self.data.write_rmp(writer)?;
+        Ok(())
+    }
+}
+
+pub struct GPSReading {
+    pub lat: f64,
+    pub lon: f64
+}
+
+impl EventRecord for GPSReading {
+    fn field_count() -> usize {
+        2
+    }
+}
+
+impl EventRecord for IMUReading {
+    fn field_count() -> usize {
+        6
+    }
+}
+
+impl RmpData for GPSReading {
+    fn from_rmp<Reader: RmpRead>(reader: &mut Reader) -> Result<Self, SimDataError<ValueReadError<Reader::Error>>> {
+        Ok(Self {
+            lat: rmp::decode::read_f64(reader)?,
+            lon: rmp::decode::read_f64(reader)?
+        })
+    }
+
+    fn write_rmp<Writer: RmpWrite>(&self, writer: &mut Writer) -> Result<(), ValueWriteError<Writer::Error>> {
+        rmp::encode::write_f64(writer, self.lat)?;
+        rmp::encode::write_f64(writer, self.lon)?;
+        Ok(())
+    }
+}
+
+pub struct IMUReading {
+    pub accel_x: f64,
+    pub accel_y: f64,
+    pub accel_z: f64,
+    pub gyro_x: f64,
+    pub gyro_y: f64,
+    pub gyro_z: f64
+}
+
+impl RmpData for IMUReading {
+    fn from_rmp<Reader: RmpRead>(reader: &mut Reader) -> Result<Self, SimDataError<ValueReadError<Reader::Error>>> {
+        Ok(Self {
+            accel_x: rmp::decode::read_f64(reader)?,
+            accel_y: rmp::decode::read_f64(reader)?,
+            accel_z: rmp::decode::read_f64(reader)?,
+
+            gyro_x: rmp::decode::read_f64(reader)?,
+            gyro_y: rmp::decode::read_f64(reader)?,
+            gyro_z: rmp::decode::read_f64(reader)?,
+        })
+    }
+
+    fn write_rmp<Writer: RmpWrite>(&self, writer: &mut Writer) -> Result<(), ValueWriteError<Writer::Error>> {
+        rmp::encode::write_f64(writer, self.accel_x)?;
+        rmp::encode::write_f64(writer, self.accel_y)?;
+        rmp::encode::write_f64(writer, self.accel_z)?;
+
+        rmp::encode::write_f64(writer, self.gyro_x)?;
+        rmp::encode::write_f64(writer, self.gyro_y)?;
+        rmp::encode::write_f64(writer, self.gyro_z)?;
+        Ok(())
+    }
+}
+
+#[derive(Debug, Default)]
+pub struct AnnotationReading {
+    pub buf: [u8;32]
+}
+
+impl RmpData for AnnotationReading {
+    fn from_rmp<Reader: RmpRead>(reader: &mut Reader) -> Result<Self, SimDataError<ValueReadError<Reader::Error>>> {
+        let mut buf = [0; 32];
+        rmp::decode::read_str(reader, &mut buf).map_err(|_| { SimDataError::BadString })?;
+        Ok(Self {
+            buf
+        })
+    }
+
+    fn write_rmp<Writer: RmpWrite>(&self, writer: &mut Writer) -> Result<(), ValueWriteError<Writer::Error>> {
+        rmp::encode::write_str(writer, core::str::from_utf8(&self.buf).unwrap())?;
+        Ok(())
+    }
+}
+
+impl EventRecord for AnnotationReading {
+    fn field_count() -> usize {
+        1
+    }
 }