From 7f6adcfbc30a7f0dcf7924bd226a34aa48479293 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Wed, 15 Jun 2022 01:29:28 +0200
Subject: [PATCH] started writer function

---
 src/sp/ecss.rs |  2 +-
 src/sp/mod.rs  | 16 ++++++++++++--
 src/sp/tc.rs   | 60 +++++++++++++++++++++++++++++++++++++-------------
 3 files changed, 60 insertions(+), 18 deletions(-)

diff --git a/src/sp/ecss.rs b/src/sp/ecss.rs
index e8c161c..9a193c7 100644
--- a/src/sp/ecss.rs
+++ b/src/sp/ecss.rs
@@ -7,7 +7,7 @@ pub trait PusPacket: CcsdsPacket {
     fn ack_flags(&self) -> u8;
 
     fn user_data(&self) -> Option<&[u8]>;
-    fn crc16(&self) -> u16;
+    fn crc16(&self) -> Option<u16>;
     /// Verify that the packet is valid. PUS packets have a CRC16 checksum to do this
     fn verify(&mut self) -> bool;
 }
diff --git a/src/sp/mod.rs b/src/sp/mod.rs
index bef0d2d..ea677f0 100644
--- a/src/sp/mod.rs
+++ b/src/sp/mod.rs
@@ -4,6 +4,14 @@ pub mod ecss;
 pub mod tc;
 pub mod tm;
 
+#[derive(Debug, Copy, Clone, PartialEq)]
+pub enum PacketError {
+    /// The passed slice is too small. Returns the required size of the failed size chgeck
+    ToBytesSliceTooSmall(usize),
+    /// The [zerocopy] library failed to write to bytes
+    ToBytesZeroCopyError,
+}
+
 #[derive(Serialize, Deserialize, Debug, PartialEq, Copy, Clone)]
 pub enum PacketType {
     Tm = 0,
@@ -355,9 +363,13 @@ pub mod zc {
             }
         }
 
-        pub fn from_slice(slice: impl AsRef<[u8]>) -> Option<Self> {
+        pub fn from_bytes(slice: impl AsRef<[u8]>) -> Option<Self> {
             SpHeader::read_from(slice.as_ref())
         }
+
+        pub fn to_bytes(&self, mut slice: impl AsMut<[u8]>) -> Option<()> {
+            self.write_to(slice.as_mut())
+        }
     }
 
     impl CcsdsPacket for SpHeader {
@@ -515,7 +527,7 @@ mod tests {
         assert_eq!(slice[4], 0x00);
         assert_eq!(slice[5], 0x00);
 
-        let sp_header = zc::SpHeader::from_slice(slice);
+        let sp_header = zc::SpHeader::from_bytes(slice);
         assert!(sp_header.is_some());
         let sp_header = sp_header.unwrap();
         println!("Header: {:?}", sp_header);
diff --git a/src/sp/tc.rs b/src/sp/tc.rs
index 22cdd34..0975e1c 100644
--- a/src/sp/tc.rs
+++ b/src/sp/tc.rs
@@ -11,11 +11,25 @@ pub enum PusVersion {
 
 pub const CRC_CCITT_FALSE: Crc<u16> = Crc::<u16>::new(&CRC_16_IBM_3740);
 
+pub mod zc {
+    use zerocopy::{AsBytes, FromBytes, NetworkEndian, Unaligned, U16};
+
+    #[derive(FromBytes, AsBytes, Unaligned)]
+    #[repr(C)]
+    pub struct PusTcDataFieldHeader {
+        version_ack: u8,
+        service: u8,
+        subservice: u8,
+        source_id: U16<NetworkEndian>,
+    }
+}
+
 pub mod srd {
+    use crate::sp;
     use crate::sp::ecss::PusPacket;
     use crate::sp::srd::SpHeader;
     use crate::sp::tc::{PusVersion, CRC_CCITT_FALSE};
-    use crate::sp::{CcsdsPacket, PacketId, PacketSequenceCtrl};
+    use crate::sp::{CcsdsPacket, PacketError, PacketId, PacketSequenceCtrl, PacketType};
     use serde::{Deserialize, Serialize};
 
     #[derive(PartialEq, Copy, Clone, Serialize, Deserialize)]
@@ -39,29 +53,43 @@ pub mod srd {
         }
     }
 
+    #[derive(PartialEq, Copy, Clone, Serialize, Deserialize)]
     pub struct PusTc<'slice> {
         pub sph: SpHeader,
         pub data_field_header: PusTcDataFieldHeader,
         raw_data: Option<&'slice [u8]>,
         app_data: Option<&'slice [u8]>,
-        crc16: u16,
+        crc16: Option<u16>,
     }
 
     impl<'slice> PusTc<'slice> {
         pub fn new(
-            sph: &SpHeader,
+            sph: &mut SpHeader,
             service: u8,
             subservice: u8,
             app_data: Option<&'slice [u8]>,
         ) -> Self {
+            sph.packet_id.ptype = PacketType::Tc;
             PusTc {
-                sph: sph.clone(),
+                sph: *sph,
                 raw_data: None,
                 app_data,
                 data_field_header: PusTcDataFieldHeader::new(service, subservice, 0b1111),
-                crc16: 0,
+                crc16: None,
             }
         }
+
+        pub fn write(&mut self, mut slice: impl AsMut<[u8]>) -> Result<(), PacketError> {
+            let sph_zc = sp::zc::SpHeader::from(self.sph);
+            if slice.as_mut().len() < 6 {
+                return Err(PacketError::ToBytesSliceTooSmall(6));
+            }
+            sph_zc
+                .to_bytes(slice)
+                .ok_or(PacketError::ToBytesZeroCopyError)?;
+            // TODO: Finish impl
+            Ok(())
+        }
     }
     impl CcsdsPacket for PusTc<'_> {
         fn version(&self) -> u8 {
@@ -102,7 +130,7 @@ pub mod srd {
             self.app_data
         }
 
-        fn crc16(&self) -> u16 {
+        fn crc16(&self) -> Option<u16> {
             self.crc16
         }
 
@@ -120,15 +148,17 @@ pub mod srd {
     }
 }
 
-pub mod zc {
-    use zerocopy::{AsBytes, FromBytes, NetworkEndian, Unaligned, U16};
+#[cfg(test)]
+mod tests {
+    use crate::sp::srd::SpHeader;
+    use crate::sp::tc::srd::PusTc;
+    use postcard::to_stdvec;
 
-    #[derive(FromBytes, AsBytes, Unaligned)]
-    #[repr(C)]
-    pub struct PusTcDataFieldHeader {
-        version_ack: u8,
-        service: u8,
-        subservice: u8,
-        source_id: U16<NetworkEndian>,
+    #[test]
+    fn test_tc() {
+        let mut sph = SpHeader::tc(0x42, 12).unwrap();
+        let pus_tc = PusTc::new(&mut sph, 1, 1, None);
+        let out = to_stdvec(&pus_tc).unwrap();
+        println!("Vector {:#04x?} with length {}", out, out.len());
     }
 }