nmeaparser.rs

use core::str::FromStr;
use nom::bytes::streaming::{take, take_until, take_while_m_n};
use nom::character::complete::alpha0;
use nom::character::streaming::digit1;
use nom::character::{is_digit, is_hex_digit};
use nom::combinator::map;
use nom::number::streaming::float;
use nom::{
    alt, branch::alt, bytes::streaming::tag, named, number::streaming::double, tag, IResult,
};
use time::Time;

#[derive(Debug, PartialEq)]
pub enum LatitudeDirection {
    North,
    South,
}

#[derive(Debug, PartialEq)]
pub enum LongitudeDirection {
    East,
    West,
}

#[derive(Debug, PartialEq)]
pub struct Latitude {
    lat: f64,
    dir: LatitudeDirection,
}

#[derive(Debug, PartialEq)]
pub struct Longitude {
    lon: f64,
    dir: LongitudeDirection,
}

#[derive(Debug, PartialEq)]
pub struct GnsFixData {
    lat: Latitude,
    lon: Longitude,
    utc: Time,
    satellites: u8,
    hdop: f32,
}

#[derive(Debug, PartialEq)]
pub(crate) enum NmeaMessage {
    GnsFixData(GnsFixData),
}

fn digitn(n: usize) -> impl Fn(&[u8]) -> IResult<&[u8], &[u8]> {
    move |s| Ok(take_while_m_n(n, n, |char| is_digit(char))(s)?)
}

fn parse_gns(start: &[u8]) -> nom::IResult<&[u8], GnsFixData, ()> {
    let (s, _) = tag("$GNGNS,")(start)?;
    let (s, hours) = digitn(2)(s)?;
    let (s, minutes) = digitn(2)(s)?;
    let (s, seconds) = digitn(2)(s)?;
    let (s, _) = tag(".")(s)?;
    let (s, milliseconds) = digitn(2)(s)?;
    let (s, _) = tag(",")(s)?;
    let (s, lat) = double(s)?;
    let (s, _) = tag(",")(s)?;
    let (s, lat_dir) = alt((
        map(tag("N"), |_| LatitudeDirection::North),
        map(tag("S"), |_| LatitudeDirection::South),
    ))(s)?;
    let (s, _) = tag(",")(s)?;
    let (s, lon) = double(s)?;
    let (s, _) = tag(",")(s)?;
    let (s, lon_dir) = alt((
        map(tag("E"), |_| LongitudeDirection::East),
        map(tag("W"), |_| LongitudeDirection::West),
    ))(s)?;
    let (s, _) = tag(",")(s)?;
    let (s, _mode) = alpha0(s)?;
    let (s, _) = tag(",")(s)?;
    let (s, satellites) = map(digit1, |num| {
        u8::from_str(unsafe { core::str::from_utf8_unchecked(num) }).unwrap()
    })(s)?;
    let (s, _) = tag(",")(s)?;
    let (s, hdop) = float(s)?;

    let (s, _) = take_until("*")(s)?;
    let (s, _) = tag("*")(s)?;
    let (s, included_crc) = map(take_while_m_n(2, 2, |s| is_hex_digit(s)), |val| {
        u8::from_str_radix(unsafe { core::str::from_utf8_unchecked(val) }, 16).unwrap()
    })(s)?;
    let (crc_start, _) = tag("$")(start)?;
    let (_, crc_bytes) = take_until("*")(crc_start)?;

    let mut calculated_crc = 0;
    for n in crc_bytes {
        calculated_crc ^= n;
    }

    if included_crc != calculated_crc {
        return Err(nom::Err::Failure(());
    }

    assert_eq!(included_crc, calculated_crc);

    Ok((
        s,
        GnsFixData {
            lat: Latitude { lat, dir: lat_dir },
            lon: Longitude { lon, dir: lon_dir },
            utc: Time::try_from_hms_milli(
                // This is safe because nom makes sure these are digits
                u8::from_str(unsafe { core::str::from_utf8_unchecked(hours) }).unwrap(),
                u8::from_str(unsafe { core::str::from_utf8_unchecked(minutes) }).unwrap(),
                u8::from_str(unsafe { core::str::from_utf8_unchecked(seconds) }).unwrap(),
                u16::from_str(unsafe { core::str::from_utf8_unchecked(milliseconds) }).unwrap(),
            )
            .unwrap(),
            satellites,
            hdop,
        },
    ))
}

pub(crate) fn parse(s: &[u8]) -> nom::IResult<&[u8], NmeaMessage, ()> {
    nom::combinator::map(
        |s| {
            let (s, res) = parse_gns(s)?;
            Ok((s, NmeaMessage::GnsFixData(res)))
        },
        |v| v,
    )(s)
    // alt((
    //     nom::combinator::map(|s| {
    //         let (s, res) = parse_gns(s)?;
    //         Ok((s, NmeaMessage::GnsFixData(res)))
    //     }, |v| v),
    //     nom::combinator::map(|s| {
    //         let (s, res) = parse_gns(s)?;
    //         Ok((s, NmeaMessage::GnsFixData(res)))
    //     }, |v| v),
    // ))(s)
}

#[cfg(test)]
mod tests {
    use time::Time;

    #[test]
    fn parses_gns() {
        use super::{
            GnsFixData, Latitude, LatitudeDirection, Longitude, LongitudeDirection, NmeaMessage,
        };
        assert_eq!(
            super::parse(b"$GNGNS,112257.00,3844.24011,N,00908.43828,W,AN,03,10.5,,,,*49\r\n"),
            Ok((
                &b""[..],
                NmeaMessage::GnsFixData(super::GnsFixData {
                    lat: Latitude {
                        lat: 3844.24011,
                        dir: LatitudeDirection::North
                    },
                    lon: Longitude {
                        lon: 00908.43828,
                        dir: LongitudeDirection::West
                    },
                    utc: Time::try_from_hms_milli(11, 22, 57, 00).unwrap(),
                    satellites: 3,
                    hdop: 10.5
                })
            ))
        )
    }
}