renderbug-bike/src/simdata.rs

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,
    PartitionNotFound
}

impl<E> From<E> for SimDataError<E> {
    fn from(value: E) -> Self {
        SimDataError::DecodeError(value)
    }
}

#[derive(Debug)]
pub enum StreamType {
    IMU,
    GPS,
    Annotations
}

impl TryFrom<i8> for StreamType {    
    type Error = ();
    
    fn try_from(value: i8) -> Result<Self, Self::Error> {
        match value {
            1 => Ok(StreamType::IMU),
            2 => Ok(StreamType::GPS),
            3 => Ok(StreamType::Annotations),
            _ => Err(())
        }
    }
}

impl From<StreamType> for i8 {
    fn from(value: StreamType) -> Self {
        match value {
            StreamType::IMU => 1,
            StreamType::GPS => 2,
            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
    }
}