whisker.rs

use core::str::FromStr;

use arrayvec::{ArrayString, ArrayVec};
use half::f16;

pub enum Whisker {
    Identification(Identification),
    Timestamp(Timestamp),
    Gps(Gps),
    Comment(Comment),
    Route(Route),
    Destination(Destination),
}

#[derive(Debug, PartialEq, Eq)]
pub struct Identification {
    pub callsign: ArrayString<254>,
    pub ssid: u8,
}

impl Identification {
    pub fn new(call: &str, ssid: u8) -> Option<Self> {
        let callsign = ArrayString::from(call).ok()?;

        Some(Self { callsign, ssid })
    }

    /// Returns none if there is not enough space in the buffer
    pub fn encode<'a>(&self, buf: &'a mut [u8]) -> Option<&'a [u8]> {
        // safe to unwrap because we know the length is <= 254
        let len: u8 = self.callsign.as_bytes().len().try_into().unwrap();
        *buf.get_mut(0)? = len + 1;

        let mut len = 1;
        for (i, b) in self.callsign.as_bytes().iter().enumerate() {
            *buf.get_mut(i + 1)? = *b;
            len += 1
        }

        *buf.get_mut(len)? = self.ssid;
        len += 1;

        Some(&buf[0..len])
    }

    pub fn decode(data: &[u8]) -> Option<Self> {
        let len = (*data.first()?).into();

        let callsign = core::str::from_utf8(data.get(1..len)?).ok()?;
        let callsign = ArrayString::from(callsign).ok()?;
        let ssid = *data.get(len)?;

        Some(Self { callsign, ssid })
    }
}

#[derive(Debug, PartialEq, Eq)]
pub struct Timestamp(u64);

impl Timestamp {
    /// Returns none if the given unix timestamp doesn't fit in 5 bytes
    pub fn new(unix_time: u64) -> Option<Self> {
        // must fit in 5 bytes
        if unix_time > (1 << (5 * 8)) - 1 {
            return None;
        }

        Some(Self(unix_time))
    }

    pub fn unix_time(&self) -> u64 {
        self.0
    }

    /// Returns none if there is not enough space in the buffer
    pub fn encode<'a>(&self, buf: &'a mut [u8]) -> Option<&'a [u8]> {
        if buf.len() < 6 {
            return None;
        }

        buf[0] = 5;
        buf[1..6].copy_from_slice(&self.0.to_le_bytes()[0..5]);

        Some(&buf[0..6])
    }

    pub fn decode(data: &[u8]) -> Option<Self> {
        if data.len() < 6 {
            return None;
        }
        if data[0] != 5 {
            return None;
        }

        let mut extended_data = [0; 8];
        extended_data[0..5].copy_from_slice(&data[1..6]);

        Some(Self(u64::from_le_bytes(extended_data)))
    }
}

#[derive(Debug, PartialEq)]
pub struct Gps {
    latitude: i32,
    longitude: i32,
    pub altitude: f16,
    pub max_error: u8,
    heading: u8,
    pub speed: f16,
}

impl Gps {
    /// Constructs a new GPS whisker.
    ///
    /// latitude: degrees
    /// longitude: degrees
    /// altitude: meters
    /// max_error: maximum error distance from specified lat/lon/alt, meters
    /// heading: direction relative to north, degrees
    /// speed: meters per second
    pub fn new(
        latitude: f64,
        longitude: f64,
        altitude: f16,
        max_error: u8,
        heading: f64,
        speed: f16,
    ) -> Self {
        let latitude = latitude.clamp(-89.999, 89.999);
        let latitude = (latitude * ((1u32 << 31) as f64) / 90.0) as i32;

        let longitude = longitude.clamp(-179.999, 179.999);
        let longitude = (longitude * ((1u32 << 31) as f64) / 180.0) as i32;

        let heading = heading.clamp(0.0, 359.999);
        let heading = (heading * 128.0 / 360.0) as u8;

        Self {
            latitude,
            longitude,
            altitude,
            max_error,
            heading,
            speed,
        }
    }

    pub fn latitude(&self) -> f64 {
        (self.latitude as f64) / (1u32 << 31) as f64 * 90.0
    }

    pub fn longitude(&self) -> f64 {
        (self.longitude as f64) / (1u32 << 31) as f64 * 180.0
    }

    pub fn heading(&self) -> f64 {
        self.heading as f64 / 128.0 * 360.0
    }

    pub fn encode<'a>(&self, buf: &'a mut [u8]) -> Option<&'a [u8]> {
        let buf = buf.get_mut(0..15)?;
        buf[0] = 14;
        buf[1..5].copy_from_slice(&self.latitude.to_le_bytes());
        buf[5..9].copy_from_slice(&self.longitude.to_le_bytes());
        buf[9..11].copy_from_slice(&self.altitude.to_le_bytes());
        buf[11] = self.max_error;
        buf[12] = self.heading;
        buf[13..15].copy_from_slice(&self.speed.to_le_bytes());

        Some(buf)
    }

    pub fn decode(data: &[u8]) -> Option<Self> {
        let data = data.get(0..15)?;
        if data[0] != 14 {
            return None;
        }
        let latitude = i32::from_le_bytes(data[1..5].try_into().unwrap());
        let longitude = i32::from_le_bytes(data[5..9].try_into().unwrap());
        let altitude = f16::from_le_bytes(data[9..11].try_into().unwrap());
        let max_error = data[11];
        let heading = data[12];
        let speed = f16::from_le_bytes(data[13..15].try_into().unwrap());

        Some(Self {
            latitude,
            longitude,
            altitude,
            max_error,
            heading,
            speed,
        })
    }
}

pub struct Comment(ArrayVec<u8, 255>);

impl Comment {
    pub fn new(data: &[u8]) -> Option<Self> {
        let mut av = ArrayVec::new();
        av.try_extend_from_slice(data).ok()?;

        Some(Self(av))
    }

    pub fn encode<'a>(&self, buf: &'a mut [u8]) -> Option<&'a [u8]> {
        let len = self.0.len();

        // length must be <= 255, so this is safe
        *buf.get_mut(0)? = len.try_into().unwrap();
        buf.get_mut(1..(len + 1))?.copy_from_slice(&self.0);

        Some(&buf[0..(len + 1)])
    }

    pub fn decode(data: &[u8]) -> Option<Self> {
        let len: usize = (*data.first()?).into();
        let content = data.get(1..(len + 1))?;

        Self::new(content)
    }
}

#[derive(Debug, PartialEq, Eq)]
pub struct Route {
    pub max_hops: u8,
    path: ArrayVec<u8, 254>,
    has_future: bool,
}

impl Route {
    pub fn new(max_hops: u8) -> Self {
        let path = ArrayVec::new();

        Self {
            max_hops,
            path,
            has_future: false,
        }
    }

    /// Returns `None` if there isn't enough space for the callsign
    /// Returns `None` if attempting to pushed is_used=true after an existing is_used=false
    /// is_future=false when this callsign has digipeated
    /// is_future=true when this callsign is expected to be part of the route, but isn't yet
    /// see the specs for more information
    pub fn push_callsign(&mut self, callsign: &str, ssid: u8, is_future: bool) -> Option<()> {
        let len = callsign.as_bytes().len() + 2;
        let free_space = self.path.capacity() - self.path.len();

        if len > free_space {
            return None;
        }

        self.has_future = self.has_future || is_future;
        if self.has_future && !is_future {
            return None;
        }

        // safe to unwrap since we already did a length check
        self.path
            .try_extend_from_slice(callsign.as_bytes())
            .unwrap();
        self.path.push(if is_future { 0xFD } else { 0xFF });
        self.path.push(ssid);

        Some(())
    }

    /// Returns `None` if there isn't enough space
    pub fn push_internet(&mut self) -> Option<()> {
        let free_space = self.path.capacity() - self.path.len();

        if free_space < 1 {
            return None;
        }

        self.path.push(0xFE);

        Some(())
    }

    pub fn iter(&'_ self) -> RouteIter<'_> {
        RouteIter::new(self)
    }

    pub fn encode<'a>(&self, buf: &'a mut [u8]) -> Option<&'a [u8]> {
        let packet_len = self.path.len() + 1;
        let buf = buf.get_mut(0..(packet_len + 1))?;
        buf[0] = packet_len.try_into().unwrap();
        buf[1] = self.max_hops;
        buf[2..(packet_len + 1)].copy_from_slice(&self.path);

        Some(buf)
    }

    // TODO reject packets that have a 0xFF anywhere after an FD
    pub fn decode(data: &[u8]) -> Option<Self> {
        let len: usize = (*data.first()?).into();
        let data = data.get(1..(len + 1))?;

        let max_hops = data[0];

        let mut path = ArrayVec::new();
        path.try_extend_from_slice(&data[1..]).unwrap();

        let has_future = data[1..].iter().any(|x| *x == 0xFD);

        Some(Self {
            max_hops,
            path,
            has_future,
        })
    }
}

#[derive(Debug, Eq, PartialEq)]
pub enum RouteNode<'a> {
    Internet,
    Identity(&'a str, u8, bool),
}

#[derive(Debug)]
pub struct RouteIter<'a> {
    route: &'a Route,
    i: usize,
}

impl<'a> RouteIter<'a> {
    fn new(route: &'a Route) -> Self {
        Self { route, i: 0 }
    }
}

impl<'a> Iterator for RouteIter<'a> {
    type Item = RouteNode<'a>;

    fn next(&mut self) -> Option<Self::Item> {
        if self.i == self.route.path.len() {
            return None;
        }

        let i_start = self.i;
        self.i += 1;

        if self.route.path[i_start] == 0xFE {
            return Some(RouteNode::Internet);
        }

        while self.route.path[self.i] != 0xFD && self.route.path[self.i] != 0xFF {
            self.i += 1;
        }

        let callsign = core::str::from_utf8(&self.route.path[i_start..self.i]).unwrap();
        let is_future = self.route.path[self.i] == 0xFD;
        self.i += 1;
        let ssid = self.route.path[self.i];
        self.i += 1;

        Some(RouteNode::Identity(callsign, ssid, is_future))
    }
}

#[derive(Debug, PartialEq, Eq)]
pub struct Destination {
    ack: u8,
    callsign: ArrayString<253>,
    ssid: u8,
}

impl Destination {
    /// Returns none if the ack number is > 127
    /// Returns none if the dest callsign is too long
    /// Returns none is is_ack is true and ack_num is 0
    pub fn new(is_ack: bool, ack_num: u8, dest_callsign: &str, dest_ssid: u8) -> Option<Self> {
        if ack_num > 127 {
            return None;
        }
        if is_ack && ack_num == 0 {
            return None;
        }
        let ack = if is_ack { (1 << 7) | ack_num } else { ack_num };
        let callsign = ArrayString::from_str(dest_callsign).ok()?;
        let ssid = dest_ssid;

        Some(Self {
            ack,
            callsign,
            ssid,
        })
    }

    pub fn is_ack(&self) -> bool {
        self.ack & (1 << 7) > 0
    }

    pub fn ack_num(&self) -> u8 {
        self.ack
    }

    pub fn callsign(&self) -> &str {
        self.callsign.as_str()
    }

    pub fn ssid(&self) -> u8 {
        self.ssid
    }

    pub fn encode<'a>(&self, buf: &'a mut [u8]) -> Option<&'a [u8]> {
        let n = self.callsign.len() + 2;
        *buf.get_mut(0)? = n.try_into().unwrap();
        *buf.get_mut(1)? = self.ack;
        buf.get_mut(2..n)?.copy_from_slice(self.callsign.as_bytes());
        *buf.get_mut(n)? = self.ssid;

        Some(&buf[0..(n + 1)])
    }

    pub fn decode(data: &[u8]) -> Option<Self> {
        let n = data.first()?;
        let call_length: usize = n.checked_sub(2)?.into();

        let ack = *data.get(1)?;
        if ack == 0b10000000 {
            // is_ack is true and # is 0
            return None;
        }

        let callsign = core::str::from_utf8(data.get(2..(call_length + 2))?).ok()?;
        let callsign = ArrayString::from_str(callsign).ok()?;

        let ssid = *data.get(call_length + 2)?;

        Some(Self {
            ack,
            callsign,
            ssid,
        })
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn ident_e2e() {
        let call = "VE9ABCDEFGZZ4839-???";
        let ssid = 17;

        let ident = Identification::new(call, ssid).unwrap();
        let mut buf = [0; 256];

        let encoded = ident.encode(&mut buf).unwrap();
        let decoded = Identification::decode(encoded).unwrap();
        assert_eq!(ident, decoded);
        assert_eq!(call, &decoded.callsign);
        assert_eq!(ssid, decoded.ssid);
    }

    #[test]
    fn new_timestamp() {
        let t = 34894;
        assert_eq!(t, Timestamp::new(t).unwrap().unix_time());

        let t = 1 << (5 * 8);
        assert_eq!(None, Timestamp::new(t));
    }

    #[test]
    fn timestamp_e2e() {
        let t = 34894;
        let timestamp = Timestamp::new(t).unwrap();

        let mut buf = [0; 256];
        let encoded = timestamp.encode(&mut buf).unwrap();
        let decoded = Timestamp::decode(encoded).unwrap();
        assert_eq!(timestamp, decoded);
        assert_eq!(t, timestamp.unix_time());
    }

    #[test]
    fn new_gps() {
        let gps = Gps::new(
            90.0,
            -180.0,
            f16::from_f32(45.02),
            24,
            123.45,
            f16::from_f32(12.3),
        );

        assert_eq!(89.99899999704212, gps.latitude());
        assert_eq!(-179.9989999551326, gps.longitude());
        assert_eq!(120.9375, gps.heading());
    }

    #[test]
    fn gps_max_heading() {
        let gps = Gps::new(
            4.0,
            23.45,
            f16::from_f32(45.02),
            24,
            360.0,
            f16::from_f32(12.3),
        );

        assert_eq!(357.1875, gps.heading());
    }

    #[test]
    fn gps_e2e() {
        let gps = Gps::new(
            90.0,
            -180.0,
            f16::from_f32(45.02),
            24,
            123.45,
            f16::from_f32(12.3),
        );

        let mut buf = [0; 256];
        let encoded = gps.encode(&mut buf).unwrap();
        let decoded = Gps::decode(encoded).unwrap();
        assert_eq!(gps, decoded);
    }

    #[test]
    fn comment_e2e() {
        let data = b"Hello world! This is an example comment";
        let comment = Comment::new(data).unwrap();
        let mut buf = [0; 256];
        let encoded = comment.encode(&mut buf).unwrap();
        let decoded = Comment::decode(encoded).unwrap();
        let decoded2 = Comment::decode(&buf).unwrap();
        assert_eq!(data[..], decoded.0[..]);
        assert_eq!(data[..], decoded2.0[..]);
    }

    #[test]
    fn route_push_and_iter() {
        let mut route = Route::new(34);
        route.push_callsign("VE2XYZ", 23, false).unwrap();
        route.push_internet().unwrap();
        route.push_internet().unwrap();
        route.push_internet().unwrap();
        route.push_callsign("VE9AAAAA", 94, true).unwrap();
        assert!(route.push_callsign("VE9AAAAA", 94, false).is_none());

        // too long
        assert!(route
            .push_callsign(
                "lsdfjslkdfjlksdjflksfjsdklfjsdklfjsdklfjsdklfjsklfsef;jklsdfjkl;sdf;klsdf;klsjdfJSDJFSKL:DFJDSL:KFskldfj;slkdfjsdkl;fjdskl;fjsdfl;kjsdfl;ksdjfkl;ssdfl;kjsdfl;ksdjf;sdklsd;lfkjsdlfk;jsdl;fkjsd;klfjsd;fljsf;oidfjgwper0tujdfgndfjkl;gjnergjol;kehfgo;dijge;oghdfkl;gjdfkl;gjdeior;lgjedr;ioghjdorighndeklo;grjiop[",
                20,
                true,
            ).is_none());

        route
            .push_callsign("This is the last callsign", 0, true)
            .unwrap();
        route.push_internet().unwrap();

        let mut iter = route.iter();
        assert_eq!(
            RouteNode::Identity("VE2XYZ", 23, false),
            iter.next().unwrap()
        );
        assert_eq!(RouteNode::Internet, iter.next().unwrap());
        assert_eq!(RouteNode::Internet, iter.next().unwrap());
        assert_eq!(RouteNode::Internet, iter.next().unwrap());
        assert_eq!(
            RouteNode::Identity("VE9AAAAA", 94, true),
            iter.next().unwrap()
        );
        assert_eq!(
            RouteNode::Identity("This is the last callsign", 0, true),
            iter.next().unwrap()
        );
        assert_eq!(RouteNode::Internet, iter.next().unwrap());
        assert_eq!(None, iter.next());

        assert_eq!(34, route.max_hops);
    }

    #[test]
    fn route_e2e() {
        let mut route = Route::new(34);
        route.push_callsign("VE2XYZ", 23, false).unwrap();
        route.push_internet().unwrap();
        route.push_internet().unwrap();
        route.push_internet().unwrap();
        route.push_callsign("VE9AAAAA", 94, true).unwrap();
        route
            .push_callsign("This is the last callsign", 0, true)
            .unwrap();
        route.push_internet().unwrap();

        let mut buf = [0; 256];
        let encoded = route.encode(&mut buf).unwrap();
        let decoded = Route::decode(encoded).unwrap();
        assert_eq!(route, decoded);
    }

    // verify examples in the standard doc
    #[test]
    fn route_doc_examples() {
        let mut ex1 = Route::new(4);
        ex1.push_callsign("VE1ABC", 0, false);
        ex1.push_callsign("VE2DEF", 234, false);
        ex1.push_callsign("VE3XYZ", 14, false);

        let mut buf = [0; 256];
        let encoded = ex1.encode(&mut buf).unwrap();
        assert_eq!(
            &[
                0x19, 0x04, 0x56, 0x45, 0x31, 0x41, 0x42, 0x43, 0xFF, 0x00, 0x56, 0x45, 0x32, 0x44,
                0x45, 0x46, 0xFF, 0xEA, 0x56, 0x45, 0x33, 0x58, 0x59, 0x5A, 0xFF, 0x0E
            ],
            &encoded
        );

        let mut ex2 = Route::new(3);
        ex2.push_callsign("VE1ABC", 0, false);
        ex2.push_internet();
        ex2.push_callsign("VE2DEF", 234, false);
        ex2.push_internet();
        ex2.push_callsign("VE3XYZ", 14, false);

        let encoded = ex2.encode(&mut buf).unwrap();
        assert_eq!(
            &[
                0x1B, 0x03, 0x56, 0x45, 0x31, 0x41, 0x42, 0x43, 0xFF, 0x00, 0xFE, 0x56, 0x45, 0x32,
                0x44, 0x45, 0x46, 0xFF, 0xEA, 0xFE, 0x56, 0x45, 0x33, 0x58, 0x59, 0x5A, 0xFF, 0x0E
            ],
            &encoded
        );

        let mut ex3 = Route::new(0);
        ex3.push_callsign("VE1ABC", 0, false);
        ex3.push_internet();
        ex3.push_internet();

        let encoded = ex3.encode(&mut buf).unwrap();
        assert_eq!(
            &[0x0B, 0x00, 0x56, 0x45, 0x31, 0x41, 0x42, 0x43, 0xFF, 0x00, 0xFE, 0xFE],
            &encoded
        );
    }

    #[test]
    fn dest_e2e() {
        assert_eq!(None, Destination::new(true, 0, "dest", 36));
        assert_eq!(None, Destination::new(false, 128, "abc", 0));
        assert!(Destination::new(false, 0, "anc", 8).is_some());

        let dest = Destination::new(true, 64, "mrow", 31).unwrap();
        let mut buf = [0; 256];
        let encoded = dest.encode(&mut buf).unwrap();
        let decoded = Destination::decode(encoded).unwrap();
        assert_eq!(dest, decoded);
    }
}