1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158

mod def;

use alloc::vec::Vec;

pub use self::def::*;
use super::Unit;
use crate::filter::AppliedFilter;
use crate::units::prelude::*;
use crate::utils::as_i32;
use crate::{units, Headers};

/// Data parsed from a GPS frame.
#[derive(Debug, Clone)]
pub struct GpsFrame<'data, 'headers, 'parser> {
    headers: &'headers Headers<'data>,
    raw: RawGpsFrame,
    filter: &'parser AppliedFilter,
}

impl super::seal::Sealed for GpsFrame<'_, '_, '_> {}

impl super::Frame for GpsFrame<'_, '_, '_> {
    type Value = GpsValue;

    #[inline]
    fn len(&self) -> usize {
        self.filter.len()
    }

    fn get_raw(&self, index: usize) -> Option<u32> {
        let index = self.filter.get(index)?;
        Some(self.raw.values[index])
    }

    fn get(&self, index: usize) -> Option<Self::Value> {
        let frame_def = self.headers.gps_frame_def().unwrap();
        let index = self.filter.get(index)?;

        let def = &frame_def.fields[index];
        let raw = self.raw.values[index];

        let value = match def.unit {
            GpsUnit::Coordinate => {
                assert!(def.signed);
                let value = as_i32(raw);

                GpsValue::Coordinate(f64::from(value) / 10000000.)
            }
            GpsUnit::Altitude => {
                let altitude = if def.signed {
                    as_i32(raw).into()
                } else {
                    raw.into()
                };

                GpsValue::Altitude(Length::new::<meter>(altitude))
            }
            GpsUnit::Velocity => {
                assert!(!def.signed);
                GpsValue::Velocity(units::new::velocity(raw))
            }
            GpsUnit::Heading => {
                assert!(!def.signed);
                GpsValue::Heading(f64::from(raw) / 10.)
            }
            GpsUnit::Unitless => GpsValue::new_unitless(raw, def.signed),
        };

        Some(value)
    }
}

impl<'data, 'headers, 'parser> GpsFrame<'data, 'headers, 'parser> {
    pub(crate) fn new(
        headers: &'headers Headers<'data>,
        raw: RawGpsFrame,
        filter: &'parser AppliedFilter,
    ) -> Self {
        Self {
            headers,
            raw,
            filter,
        }
    }

    /// Returns the parsed time since power on.
    pub fn time(&self) -> Time {
        units::new::time(self.raw.time)
    }

    /// Returns the raw microsecond counter since power on.
    ///
    /// **Note:** This does not currently handle overflow of the transmitted
    /// 32bit counter.
    pub fn time_raw(&self) -> u64 {
        self.raw.time
    }
}

#[derive(Debug, Clone)]
pub(crate) struct RawGpsFrame {
    pub(crate) time: u64,
    pub(crate) values: Vec<u32>,
}

#[derive(Debug, Clone, Copy, PartialEq)]
pub enum GpsValue {
    Coordinate(f64),
    Altitude(Length),
    Velocity(Velocity),
    Heading(f64),
    Unsigned(u32),
    Signed(i32),
}

impl GpsValue {
    const fn new_unitless(value: u32, signed: bool) -> Self {
        if signed {
            Self::Signed(as_i32(value))
        } else {
            Self::Unsigned(value)
        }
    }
}

impl From<GpsValue> for super::Value {
    fn from(value: GpsValue) -> Self {
        match value {
            GpsValue::Coordinate(c) => Self::GpsCoordinate(c),
            GpsValue::Altitude(a) => Self::Altitude(a),
            GpsValue::Velocity(v) => Self::Velocity(v),
            GpsValue::Heading(h) => Self::GpsHeading(h),
            GpsValue::Unsigned(x) => Self::Unsigned(x),
            GpsValue::Signed(x) => Self::Signed(x),
        }
    }
}

#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
pub enum GpsUnit {
    Coordinate,
    Altitude,
    Velocity,
    Heading,
    Unitless,
}

impl From<GpsUnit> for Unit {
    fn from(unit: GpsUnit) -> Self {
        match unit {
            GpsUnit::Coordinate => Self::GpsCoordinate,
            GpsUnit::Altitude => Self::Altitude,
            GpsUnit::Velocity => Self::Velocity,
            GpsUnit::Heading => Self::GpsHeading,
            GpsUnit::Unitless => Self::Unitless,
        }
    }
}