1use std::{collections::HashMap, convert::{TryFrom, TryInto}, f32::consts::PI};
2
3use itertools::izip;
4use nom::{
5 error::{ErrorKind, ParseError},
6 multi::fold_many0,
7 IResult,
8};
9use num_rational::Ratio;
10
11use super::predictor::{AnyIPredictor, AnyPPredictor, FieldPredictor};
12use crate::{
13 frame::{
14 header::{parse_header, Frame},
15 FieldEncoding, RawFieldEncoding,
16 },
17 stream::predictor::AnyGPredictor,
18};
19
20#[derive(Debug, Clone)]
21pub struct Header {
22 product: String,
23 data_version: String,
24 firmware_type: Option<String>,
25 firmware_revision: Option<String>,
26 firmware_date: Option<String>,
27 board_information: Option<String>,
28 log_start_datetime: Option<String>,
29 craft_name: Option<String>,
30 i_interval: i16,
31 p_interval: Ratio<u16>,
32 p_ratio: u16,
33 pub gyro_scale: f32,
34 pub raw_gyro_scale: f32,
35 pub loop_time: u32,
36
37 pub other_headers: HashMap<String, String>,
38
39 pub ip_fields: HashMap<String, IPField>,
40 pub s_fields: HashMap<String, SlowField>,
41 pub g_fields: HashMap<String, GNSSField>,
42 pub h_fields: HashMap<String, GNSSHomeField>,
43
44 pub ip_fields_in_order: Vec<IPField>,
45 pub s_fields_in_order: Vec<SlowField>,
46 pub g_fields_in_order: Vec<GNSSField>,
47
48 pub(crate) i_field_encodings: Vec<FieldEncoding>,
49 pub(crate) i_field_predictors: Vec<AnyIPredictor>,
50 pub(crate) p_field_encodings: Vec<FieldEncoding>,
51 pub(crate) p_field_predictors: Vec<AnyPPredictor>,
52 pub(crate) s_field_encodings: Vec<FieldEncoding>,
53 pub(crate) g_field_encodings: Vec<FieldEncoding>,
54 pub(crate) g_field_predictors: Vec<AnyGPredictor>,
55 pub(crate) h_field_encodings: Vec<FieldEncoding>,
56 pub(crate) h_field_predictors: Vec<AnyPPredictor>,
57}
58
59#[derive(Debug)]
60pub enum HeaderBuildError {
61 MissingHeader(&'static str),
62 }
64
65impl AsRef<str> for HeaderBuildError {
66 fn as_ref(&self) -> &str {
67 match self {
68 Self::MissingHeader(r) => r,
69 }
70 }
71}
72
73impl TryFrom<HeaderBuilder> for Header {
74 type Error = HeaderBuildError;
75
76 fn try_from(builder: HeaderBuilder) -> Result<Self, Self::Error> {
77 let product = builder
78 .product
79 .ok_or(HeaderBuildError::MissingHeader("Product"))?;
80 let data_version = builder
81 .data_version
82 .ok_or(HeaderBuildError::MissingHeader("Data version"))?;
83 let i_interval = builder
84 .i_interval
85 .ok_or(HeaderBuildError::MissingHeader("I interval"))?;
86 let p_interval = builder
87 .p_interval
88 .ok_or(HeaderBuildError::MissingHeader("P interval"))?;
89 let p_ratio = builder.p_ratio.unwrap_or(1);
90 let gyro_scale = builder
91 .gyro_scale
92 .ok_or(HeaderBuildError::MissingHeader("gyro_scale"))?;
93 let loop_time = builder
94 .loop_time
95 .ok_or(HeaderBuildError::MissingHeader("looptime"))?;
96
97 let mut ip_fields = HashMap::with_capacity(builder.i_field_names.len());
98 let mut ip_fields_in_order = Vec::with_capacity(builder.i_field_names.len());
99 let mut i_field_encodings = Vec::with_capacity(builder.i_field_names.len());
100 let mut p_field_encodings = Vec::with_capacity(builder.i_field_names.len());
101 let mut i_field_predictors = Vec::with_capacity(builder.i_field_names.len());
102 let mut p_field_predictors = Vec::with_capacity(builder.i_field_names.len());
103
104 fn add_encoding(encodings: &mut Vec<FieldEncoding>, new_encoding: RawFieldEncoding) {
105 let new_encoding = match new_encoding {
106 RawFieldEncoding::Tag8_8SVB => {
107 if let Some(FieldEncoding::Tag8_8SVB(n_fields)) = encodings.last_mut() {
108 if *n_fields != 8 {
109 *n_fields += 1;
110 return;
111 }
112 }
113 FieldEncoding::Tag8_8SVB(1)
114 }
115 RawFieldEncoding::Tag2_3S32 => {
116 if let Some(FieldEncoding::Tag2_3S32(n_fields)) = encodings.last_mut() {
117 if *n_fields != 3 {
118 *n_fields += 1;
119 return;
120 }
121 }
122 FieldEncoding::Tag2_3S32(1)
123 }
124 RawFieldEncoding::Tag2_3SVariable => {
125 if let Some(FieldEncoding::Tag2_3SVariable(n_fields)) = encodings.last_mut() {
126 if *n_fields != 3 {
127 *n_fields += 1;
128 return;
129 }
130 }
131 FieldEncoding::Tag2_3SVariable(1)
132 }
133 RawFieldEncoding::Tag8_4S16 => {
134 if let Some(FieldEncoding::Tag8_4S16(n_fields)) = encodings.last_mut() {
135 if *n_fields != 4 {
136 *n_fields += 1;
137 return;
138 }
139 }
140 FieldEncoding::Tag8_4S16(1)
141 }
142 RawFieldEncoding::Null => FieldEncoding::Null,
143 RawFieldEncoding::Negative14BitVB => FieldEncoding::Negative14BitVB,
144 RawFieldEncoding::SignedVB => FieldEncoding::SignedVB,
145 RawFieldEncoding::UnsignedVB => FieldEncoding::UnsignedVB,
146 };
147 encodings.push(new_encoding);
148 }
149
150 for (ix, (name, signedness, i_encoding, p_encoding)) in izip!(
151 builder.i_field_names,
152 builder.i_field_signedness,
153 builder.i_field_encoding,
154 builder.p_field_encoding
155 )
156 .enumerate()
157 {
158 add_encoding(&mut i_field_encodings, i_encoding);
159 add_encoding(&mut p_field_encodings, p_encoding);
160
161 let field = IPField {
162 name: name.clone(),
163 ix,
164 signed: signedness,
165 };
166 ip_fields.insert(name, field.clone());
167 ip_fields_in_order.push(field);
168 }
169
170 for (ix, i_predictor) in builder.i_field_predictors.iter().copied().enumerate() {
171 i_field_predictors.push(AnyIPredictor::new(
172 i_predictor,
173 &builder.other_headers,
174 &ip_fields,
175 ix,
176 ));
177 }
178
179 for (ix, p_predictor) in builder.p_field_predictors.iter().copied().enumerate() {
180 p_field_predictors.push(AnyPPredictor::new(p_predictor, p_interval, ix));
181 }
182
183 let mut s_fields = HashMap::with_capacity(builder.s_field_names.len());
184 let mut s_field_encodings = Vec::with_capacity(builder.s_field_names.len());
185 let mut s_fields_in_order = Vec::with_capacity(builder.s_field_names.len());
186 for (ix, (name, signedness, encoding, predictor)) in izip!(
187 builder.s_field_names,
188 builder.s_field_signedness,
189 builder.s_field_encoding,
190 builder.s_field_predictors
191 )
192 .enumerate()
193 {
194 add_encoding(&mut s_field_encodings, encoding);
195 let field = SlowField {
196 name,
197 ix,
198 predictor,
199 signed: signedness,
200 };
201 s_fields.insert(field.name.clone(), field.clone());
202 s_fields_in_order.push(field);
203 }
204
205 let mut g_fields = HashMap::with_capacity(builder.g_field_names.len());
206 let mut g_field_encodings = Vec::with_capacity(builder.g_field_names.len());
207 let mut g_field_predictors = Vec::with_capacity(builder.g_field_names.len());
208 let mut g_fields_in_order = Vec::with_capacity(builder.g_field_names.len());
209
210 for (ix, (name, signedness, encoding, predictor)) in izip!(
211 builder.g_field_names,
212 builder.g_field_signedness,
213 builder.g_field_encoding,
214 builder.g_field_predictors
215 )
216 .enumerate()
217 {
218 add_encoding(&mut g_field_encodings, encoding);
219 let mut name_chars = name.chars();
220 let sub_ix = if let Some(_) = name_chars.find(|&c| c == '[') {
221 name_chars
222 .next()
223 .and_then(|c| c.to_digit(10))
224 .unwrap_or(0u32) as usize
225 } else {
226 0
227 };
228
229 g_field_predictors.push(AnyGPredictor::new(predictor, ix, sub_ix, &ip_fields));
230
231 let field = GNSSField {
232 name,
233 ix,
234 predictor,
235 signed: signedness,
236 };
237 g_fields.insert(field.name.clone(), field.clone());
238 g_fields_in_order.push(field);
239 }
240
241 let mut h_fields = HashMap::with_capacity(builder.h_field_names.len());
242 let mut h_field_encodings = Vec::with_capacity(builder.h_field_names.len());
243 let mut h_field_predictors = Vec::with_capacity(builder.h_field_names.len());
244 for (ix, (name, signedness, encoding, predictor)) in izip!(
245 builder.h_field_names,
246 builder.h_field_signedness,
247 builder.h_field_encoding,
248 builder.h_field_predictors
249 )
250 .enumerate()
251 {
252 add_encoding(&mut h_field_encodings, encoding);
253 assert_eq!(predictor, FieldPredictor::None);
254 h_field_predictors.push(AnyPPredictor::none(ix));
255
256 h_fields.insert(
257 name.clone(),
258 GNSSHomeField {
259 name,
260 ix,
261 predictor: predictor,
262 signed: signedness,
263 },
264 );
265 }
266
267 Ok(Header {
268 product,
269 data_version,
270 firmware_type: builder.firmware_type,
271 firmware_revision: builder.firmware_revision,
272 firmware_date: builder.firmware_date,
273 board_information: builder.board_information,
274 log_start_datetime: builder.log_start_datetime,
275 craft_name: builder.craft_name,
276 i_interval,
277 p_interval,
278 p_ratio,
279 other_headers: builder.other_headers,
280 ip_fields,
281 s_fields,
282 ip_fields_in_order,
283 s_fields_in_order,
284 g_fields_in_order,
285 i_field_encodings,
286 i_field_predictors,
287 p_field_encodings,
288 p_field_predictors,
289 s_field_encodings,
290 g_fields,
291 h_fields,
292 g_field_encodings,
293 g_field_predictors,
294 h_field_encodings,
295 h_field_predictors,
296 gyro_scale: gyro_scale * (PI / 180.0) * 0.000001,
297 raw_gyro_scale: gyro_scale,
298 loop_time,
299 })
300 }
301}
302
303#[derive(Clone, Debug, Default)]
304struct HeaderBuilder {
305 product: Option<String>,
306 data_version: Option<String>,
307 firmware_type: Option<String>,
308 firmware_revision: Option<String>,
309 firmware_date: Option<String>,
310 board_information: Option<String>,
311 log_start_datetime: Option<String>,
312 craft_name: Option<String>,
313 i_interval: Option<i16>,
314 p_interval: Option<Ratio<u16>>,
315 p_ratio: Option<u16>,
316 gyro_scale: Option<f32>,
317 loop_time: Option<u32>,
318
319 other_headers: HashMap<String, String>,
320
321 i_field_names: Vec<String>,
322 i_field_signedness: Vec<bool>,
323 i_field_encoding: Vec<RawFieldEncoding>,
324 i_field_predictors: Vec<FieldPredictor>,
325 p_field_encoding: Vec<RawFieldEncoding>,
326 p_field_predictors: Vec<FieldPredictor>,
327
328 s_field_names: Vec<String>,
329 s_field_signedness: Vec<bool>,
330 s_field_encoding: Vec<RawFieldEncoding>,
331 s_field_predictors: Vec<FieldPredictor>,
332
333 g_field_names: Vec<String>,
334 g_field_signedness: Vec<bool>,
335 g_field_encoding: Vec<RawFieldEncoding>,
336 g_field_predictors: Vec<FieldPredictor>,
337
338 h_field_names: Vec<String>,
339 h_field_signedness: Vec<bool>,
340 h_field_encoding: Vec<RawFieldEncoding>,
341 h_field_predictors: Vec<FieldPredictor>,
342}
343
344#[derive(Clone, Debug)]
345pub struct IPField {
346 pub name: String,
347 pub ix: usize,
348 pub signed: bool,
349}
350
351#[derive(Clone, Debug)]
352pub struct SlowField {
353 pub name: String,
354 ix: usize,
355 signed: bool,
356 predictor: FieldPredictor,
357}
358
359#[derive(Clone, Debug)]
360pub struct GNSSField {
361 pub name: String,
362 ix: usize,
363 signed: bool,
364 predictor: FieldPredictor,
365}
366
367#[derive(Clone, Debug)]
368pub struct GNSSHomeField {
369 name: String,
370 ix: usize,
371 signed: bool,
372 predictor: FieldPredictor,
373}
374
375#[derive(Debug)]
376pub enum ParseHeadersError<I> {
377 HeaderBuildError(HeaderBuildError),
378 Nom(I, ErrorKind),
379}
380
381impl<I> ParseError<I> for ParseHeadersError<I> {
382 fn from_error_kind(input: I, kind: ErrorKind) -> Self {
383 ParseHeadersError::Nom(input, kind)
384 }
385
386 fn append(_: I, _: ErrorKind, other: Self) -> Self {
387 other
388 }
389}
390
391impl<I> From<nom::error::Error<I>> for ParseHeadersError<I> {
392 fn from(err: nom::error::Error<I>) -> Self {
393 Self::Nom(err.input, err.code)
394 }
395}
396
397pub fn parse_headers(input: &[u8]) -> IResult<&[u8], Header, ParseHeadersError<&[u8]>> {
398 let (input, header) = fold_many0(
399 parse_header,
400 HeaderBuilder::default(),
401 |mut header, header_frame| {
402 match header_frame {
403 Frame::Product(product) => header.product = Some(product.to_owned()),
404 Frame::DataVersion(version) => header.data_version = Some(version.to_owned()),
405 Frame::IInterval(i_interval) => header.i_interval = Some(i_interval),
406 Frame::FieldIName(i_field_names) => {
407 header.i_field_names =
408 i_field_names.into_iter().map(ToOwned::to_owned).collect()
409 }
410 Frame::FieldIPredictor(i_field_predictors) => {
411 header.i_field_predictors = i_field_predictors
412 }
413 Frame::FieldISignedness(i_field_signedness) => {
414 header.i_field_signedness = i_field_signedness
415 }
416 Frame::FieldIEncoding(i_field_encoding) => {
417 header.i_field_encoding = i_field_encoding
418 }
419 Frame::PInterval(p_interval) => header.p_interval = Some(p_interval),
420 Frame::PRatio(p_ratio) => header.p_ratio = Some(p_ratio),
421 Frame::FieldPPredictor(p_field_predictors) => {
422 header.p_field_predictors = p_field_predictors
423 }
424 Frame::FieldPEncoding(p_field_encoding) => {
425 header.p_field_encoding = p_field_encoding
426 }
427 Frame::FieldSName(s_field_names) => {
428 header.s_field_names =
429 s_field_names.into_iter().map(ToOwned::to_owned).collect()
430 }
431 Frame::FieldSPredictor(s_field_predictors) => {
432 header.s_field_predictors = s_field_predictors
433 }
434 Frame::FieldSSignedness(s_field_signedness) => {
435 header.s_field_signedness = s_field_signedness
436 }
437 Frame::FieldSEncoding(s_field_encoding) => {
438 header.s_field_encoding = s_field_encoding
439 }
440 Frame::FieldGName(g_field_names) => {
441 header.g_field_names =
442 g_field_names.into_iter().map(ToOwned::to_owned).collect()
443 }
444 Frame::FieldGPredictor(g_field_predictors) => {
445 header.g_field_predictors = g_field_predictors
446 }
447 Frame::FieldGSignedness(g_field_signedness) => {
448 header.g_field_signedness = g_field_signedness
449 }
450 Frame::FieldGEncoding(g_field_encoding) => {
451 header.g_field_encoding = g_field_encoding
452 }
453 Frame::FieldHName(h_field_names) => {
454 header.h_field_names =
455 h_field_names.into_iter().map(ToOwned::to_owned).collect()
456 }
457 Frame::FieldHPredictor(h_field_predictors) => {
458 header.h_field_predictors = h_field_predictors
459 }
460 Frame::FieldHSignedness(h_field_signedness) => {
461 header.h_field_signedness = h_field_signedness
462 }
463 Frame::FieldHEncoding(h_field_encoding) => {
464 header.h_field_encoding = h_field_encoding
465 }
466 Frame::GyroScale(gyro_scale) => {
467 header.gyro_scale = Some(gyro_scale)
468 }
469 Frame::LoopTime(loop_time) => {
470 header.loop_time = Some(loop_time)
471 }
472 Frame::UnkownHeader(name, value) => {
473 header.other_headers.insert(name.into(), value.into());
474 }
475 _ => {}
476 };
477 header
478 },
479 )(input)
480 .map_err(nom::Err::convert)?;
481
482 let header = header
483 .try_into()
484 .map_err(|err| nom::Err::Failure(ParseHeadersError::HeaderBuildError(err)))?;
485 Ok((input, header))
486}