JE-Skin/src/serial_core/codecs/tactile_a.rs

use crate::serial_core::error::CodecError;
use crate::serial_core::frame::{
    FrameHandler, TactileAFrameMetaData, TactileARepFrame, TactileAReqFrame,
};
use crate::serial_core::record::{write_csv, CsvExporter, CsvImporter, RecordedFrame, Recording};
use crate::serial_core::utils::{calc_crc8_itu, elapsed_millis};
use crate::serial_core::{
    codec::Codec,
    frame::{TactileAFrame, TactileAFrameStatusCode},
};
use async_trait::async_trait;
use csv::StringRecord;
use anyhow::anyhow;
use std::io::Read;
use log::debug;

const FRAME_BUFFER_MIN_LENGTH: usize = 15;

pub struct TactileACodec {
    buffer: Vec<u8>,
    expected_data_len: usize,
}

pub struct TactileACsvExporter {
    channels: usize,
}

pub struct TactileACsvImporter {
    channels: usize,
    data_row: usize,
    packets: Vec<TactileADataPacket>,
}

pub struct TactileAHandler;

#[derive(Clone)]
pub struct TactileADataPacket {
    pub data: Vec<i32>,
    pub dts_ms: u64,
}

impl From<u8> for TactileAFrameStatusCode {
    fn from(value: u8) -> Self {
        match value {
            0 => TactileAFrameStatusCode::Success,
            _ => TactileAFrameStatusCode::Failure,
        }
    }
}

impl TryFrom<&TactileARepFrame> for TactileADataPacket {
    type Error = CodecError;
    fn try_from(value: &TactileARepFrame) -> Result<TactileADataPacket, Self::Error> {
        let data = TactileACodec::parse_data_frame(&value.payload)?;
        let dts_ms = value.dts_ms;
        Ok(TactileADataPacket {
            data: data,
            dts_ms: dts_ms,
        })
    }
}

impl TactileACodec {
    pub fn new(cols: usize, rows: usize) -> TactileACodec {
        Self {
            buffer: Vec::new(),
            expected_data_len: cols * rows * 2,
        }
    }

    pub fn parse_data_frame(data: &[u8]) -> Result<Vec<i32>, CodecError> {
        if data.len() % 2 != 0 {
            return Err(CodecError::InvalidLength);
        }

        let vals: Vec<i32> = data
            .chunks_exact(2)
            .map(|chunk| {
                let raw = u16::from_le_bytes([chunk[0], chunk[1]]) as i32;
                if raw < 15 {
                    0
                } else {
                    raw
                }
            })
            .collect::<Vec<i32>>();

        Ok(vals)
    }

    pub fn build_req_frame(cols: usize, rows: usize) -> anyhow::Result<TactileAFrame> {
        let header = [0x55, 0xAA];
        let payload_len: usize = 9;
        let device_addr: u8 = 0x34;
        let extend_code: u8 = 0x00;
        let func_code: u8 = 0xFB;
        let start_addr: u32 = 7168;
        let except_data_len: usize = cols * rows * 2;
        let checksum: u8 = 0;
        Ok(TactileAFrame::Req(TactileAReqFrame {
            meta: TactileAFrameMetaData {
                header,
                payload_len,
                device_addr,
                extend_code,
                func_code,
                start_addr,
                except_data_len,
                checksum,
            },
        }))
    }
}

impl Codec<TactileAFrame> for TactileACodec {
    fn decode(
        &mut self,
        input: &[u8],
        session_started_at: std::time::Instant,
    ) -> Result<Vec<TactileAFrame>, CodecError> {
        self.buffer.extend_from_slice(input);
        let mut frames: Vec<TactileAFrame> = Vec::new();

        loop {
            if self.buffer.len() < FRAME_BUFFER_MIN_LENGTH {
                break;
            }

            let header_pos = self.buffer.windows(2).position(|w| w == [0xAA, 0x55]);

            let Some(pos) = header_pos else {
                self.buffer.clear();
                break;
            };
            if pos > 0 {
                self.buffer.drain(0..pos);
            }

            if self.buffer.len() < FRAME_BUFFER_MIN_LENGTH {
                break;
            }

            let header = [self.buffer[0], self.buffer[1]];
            let payload_len = u16::from_le_bytes([self.buffer[2], self.buffer[3]]) as usize;
            let device_addr = self.buffer[4];
            let extend_code = self.buffer[5];
            let func_code = self.buffer[6];
            let start_addr = u32::from_le_bytes([
                self.buffer[7],
                self.buffer[8],
                self.buffer[9],
                self.buffer[10],
            ]);
            let except_data_len = u16::from_le_bytes([self.buffer[11], self.buffer[12]]) as usize;
            let status = TactileAFrameStatusCode::from(self.buffer[13]);
            if except_data_len != self.expected_data_len {
                debug!(
                    "unexpected payload length: expected {}, got {}, buffer_len={}",
                    self.expected_data_len,
                    except_data_len,
                    self.buffer.len()
                );
                self.buffer.drain(0..1);
                continue;
            }

            let frame_length = except_data_len + FRAME_BUFFER_MIN_LENGTH;
            if self.buffer.len() < frame_length {
                break;
            }

            let need_check_data = self.buffer[0..14 + except_data_len].to_vec();
            let payload = self.buffer[14..14 + except_data_len].to_vec();
            let crc8_itu_alg = crc::Crc::<u8>::new(&crc::CRC_8_I_432_1);
            let checksum = crc8_itu_alg.checksum(&need_check_data.as_slice());
            if self.buffer[frame_length - 1] != checksum {
                debug!(
                    "checksum mismatch: expected {:02X}, got {:02X}, frame_len={}",
                    checksum,
                    self.buffer[frame_length - 1],
                    frame_length
                );
                self.buffer.drain(0..1);
                continue;
            }
            let dts_ms = elapsed_millis(session_started_at);
            let meta: TactileAFrameMetaData = TactileAFrameMetaData {
                header,
                payload_len,
                device_addr,
                extend_code,
                func_code,
                start_addr,
                except_data_len,
                checksum,
            };
            frames.push(TactileAFrame::Rep({
                TactileARepFrame {
                    meta,
                    status,
                    payload,
                    dts_ms,
                }
            }));

            self.buffer.drain(0..frame_length);
        }

        Ok(frames)
    }

    fn encode(
        &self,
        frame: &TactileAFrame,
    ) -> Result<Vec<u8>, crate::serial_core::error::CodecError> {
        match frame {
            TactileAFrame::Req(f) => {
                let mut req_bytes: Vec<u8> = Vec::new();
                req_bytes.extend_from_slice(f.meta.header.as_slice());
                req_bytes.extend_from_slice((f.meta.payload_len as u16).to_le_bytes().as_slice());
                req_bytes.push(f.meta.device_addr);
                req_bytes.push(f.meta.extend_code);
                req_bytes.push(f.meta.func_code);

                req_bytes.extend_from_slice(f.meta.start_addr.to_le_bytes().as_slice());
                req_bytes.extend_from_slice((f.meta.except_data_len as u16).to_le_bytes().as_slice());
                let checksum = calc_crc8_itu(req_bytes.as_slice());
                req_bytes.push(checksum);
                Ok(req_bytes)
            }
            _ => {
                Err(CodecError::InvalidFrameType)
            }
        }
    }
}

#[async_trait]
impl FrameHandler<TactileAFrame, i32> for TactileAHandler {
    async fn on_frame(&mut self, frame: &TactileAFrame) -> anyhow::Result<Option<Vec<i32>>> {
        match frame {
            TactileAFrame::Rep(rep) => {
                let vals = TactileACodec::parse_data_frame(&rep.payload)?;
                Ok(Some(vals))
            }
            _ => Ok(None),
        }
    }
}

impl TactileACsvExporter {
    fn new(channels: usize) -> Self {
        TactileACsvExporter { channels }
    }
}

impl CsvExporter<TactileARepFrame> for TactileACsvExporter {
    type Error = CodecError;
    fn csv_header(&self, _recording: &Recording<TactileARepFrame>) -> Vec<String> {
        let mut header: Vec<String> = Vec::new();
        for i in 0..self.channels {
            header.push(format!("channel{}", i + 1));
        }

        header.push("dts".to_string());
        header.push("summary".to_string());
        header
    }

    fn csv_row(
        &self,
        item: &RecordedFrame<TactileARepFrame>,
    ) -> anyhow::Result<Vec<String>> {
        let packet = TactileADataPacket::try_from(&item.frame)?;
        let summary: i32 = packet.data.iter().sum();
        let mut row: Vec<String> = packet.data.iter().map(|x| x.to_string()).collect();
        row.push(packet.dts_ms.to_string());
        row.push(summary.to_string());
        Ok(row)
    }
}

impl CsvExporter<TactileAFrame> for TactileACsvExporter {
    type Error = CodecError;

    fn csv_header(&self, _recording: &Recording<TactileAFrame>) -> Vec<String> {
        let mut header: Vec<String> = Vec::new();
        for i in 0..self.channels {
            header.push(format!("channel{}", i + 1));
        }

        header.push("dts".to_string());
        header
    }

    fn csv_row(
        &self,
        item: &RecordedFrame<TactileAFrame>,
    ) -> anyhow::Result<Vec<String>> {
        let rep = match &item.frame {
            TactileAFrame::Rep(rep) => rep,
            TactileAFrame::Req(_) => return Err(anyhow!("request frame cannot be exported to csv row")),
        };

        let packet = TactileADataPacket::try_from(rep)?;
        let mut row: Vec<String> = packet.data.iter().map(|x| x.to_string()).collect();
        row.push(packet.dts_ms.to_string());
        Ok(row)
    }
}

impl TactileACsvImporter {
    pub fn new(_path: &str) -> TactileACsvImporter {
        Self {
            channels: 0,
            data_row: 0,
            packets: Vec::new(),
        }
    }

    fn parse_record(&mut self, record: StringRecord) -> anyhow::Result<TactileADataPacket> {
        if self.channels == 0 {
            return Err(anyhow!("csv header is missing channel columns"));
        }

        if record.len() < self.channels + 1 {
            return Err(anyhow!("csv row has insufficient columns"));
        }

        let mut data = Vec::with_capacity(self.channels);
        for index in 0..self.channels {
            let cell = record.get(index).ok_or_else(|| anyhow!("missing channel cell"))?;
            data.push(cell.parse::<i32>()?);
        }

        let dts_cell = record
            .get(self.channels)
            .ok_or_else(|| anyhow!("missing dts cell"))?;
        let dts_ms = dts_cell.parse::<u64>()?;

        Ok(TactileADataPacket {
            data: data,
            dts_ms: dts_ms,
        })
    }
}

impl CsvImporter<TactileADataPacket> for TactileACsvImporter {
    fn load<R: Read>(&mut self, reader: R) -> anyhow::Result<Vec<TactileADataPacket>> {
        let mut rdr = csv::Reader::from_reader(reader);
        let headers = rdr.headers()?.clone();
        self.channels = headers.len().saturating_sub(1);
        self.data_row = 0;
        self.packets.clear();

        for record in rdr.records() {
            let record = record?;
            let packet = self.parse_record(record)?;
            self.packets.push(packet);
            self.data_row += 1;
        }

        Ok(self.packets.clone())
    }
}

pub fn export_recording_csv<W>(recording: &Recording<TactileAFrame>, writer: W) -> anyhow::Result<()>
where
    W: std::io::Write,
{
    let channel_nb = recording
        .frames
        .iter()
        .find_map(|frame| match &frame.frame {
            TactileAFrame::Rep(rep) => Some(rep.payload.len() / 2),
            TactileAFrame::Req(_) => None,
        })
        .unwrap_or(0);

    let exporter = TactileACsvExporter::new(channel_nb);
    write_csv(recording, &exporter, writer)
}