Skip to main content

mavlink_bindgen/
parser.rs

1use crc_any::CRCu16;
2use std::cmp::Ordering;
3use std::collections::btree_map::Entry;
4use std::collections::{BTreeMap, HashSet};
5use std::default::Default;
6use std::fs::File;
7use std::io::{BufReader, Write};
8use std::path::{Path, PathBuf};
9use std::str::FromStr;
10use std::sync::LazyLock;
11
12use regex::Regex;
13
14use quick_xml::{events::Event, Reader};
15
16use proc_macro2::{Ident, TokenStream};
17use quote::{format_ident, quote};
18
19#[cfg(feature = "serde")]
20use serde::{Deserialize, Serialize};
21
22use crate::error::BindGenError;
23use crate::util;
24
25static URL_REGEX: LazyLock<Regex> = LazyLock::new(|| {
26    Regex::new(concat!(
27        r"(https?://",                                               // url scheme
28        r"([-a-zA-Z0-9@:%._\+~#=]{2,256}\.)+",                       // one or more subdomains
29        r"[a-zA-Z]{2,63}",                                           // root domain
30        r"\b([-a-zA-Z0-9@:%_\+.~#?&/=]*[-a-zA-Z0-9@:%_\+~#?&/=])?)", // optional query or url fragments
31    ))
32    .expect("failed to build regex")
33});
34
35#[derive(Debug, PartialEq, Clone, Default)]
36#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
37pub struct MavProfile {
38    pub messages: BTreeMap<String, MavMessage>,
39    pub enums: BTreeMap<String, MavEnum>,
40    pub version: Option<u8>,
41    pub dialect: Option<u8>,
42}
43
44impl MavProfile {
45    fn add_message(&mut self, message: &MavMessage) {
46        match self.messages.entry(message.name.clone()) {
47            Entry::Occupied(entry) => {
48                assert!(
49                    entry.get() == message,
50                    "Message '{}' defined twice but definitions are different",
51                    message.name
52                );
53            }
54            Entry::Vacant(entry) => {
55                entry.insert(message.clone());
56            }
57        }
58    }
59
60    fn add_enum(&mut self, enm: &MavEnum) {
61        match self.enums.entry(enm.name.clone()) {
62            Entry::Occupied(entry) => {
63                entry.into_mut().try_combine(enm);
64            }
65            Entry::Vacant(entry) => {
66                entry.insert(enm.clone());
67            }
68        }
69    }
70
71    /// Go over all fields in the messages, and if you encounter an enum,
72    /// which is a bitmask, set the bitmask size based on field size
73    fn update_enums(mut self) -> Self {
74        for msg in self.messages.values_mut() {
75            for field in &mut msg.fields {
76                if let Some(enum_name) = &field.enumtype {
77                    // find the corresponding enum
78                    if let Some(enm) = self.enums.get_mut(enum_name) {
79                        // Handle legacy definition where bitmask is defined as display="bitmask"
80                        if field.display == Some("bitmask".to_string()) {
81                            enm.bitmask = true;
82                        }
83
84                        // it is a bitmask
85                        if enm.bitmask {
86                            enm.primitive = Some(field.mavtype.rust_primitive_type());
87
88                            // check if all enum values can be stored in the fields
89                            for entry in &enm.entries {
90                                assert!(
91                                    entry.value.unwrap_or_default() <= field.mavtype.max_int_value(),
92                                    "bitflag enum field {} of {} must be able to fit all possible values for {}",
93                                    field.name,
94                                    msg.name,
95                                    enum_name,
96                                );
97                            }
98
99                            // Fix fields in backwards manner
100                            if field.display.is_none() {
101                                field.display = Some("bitmask".to_string());
102                            }
103                        }
104                    }
105                }
106            }
107        }
108        self
109    }
110
111    //TODO verify this is no longer necessary since we're supporting both mavlink1 and mavlink2
112    //    ///If we are not using Mavlink v2, remove messages with id's > 254
113    //    fn update_messages(mut self) -> Self {
114    //        //println!("Updating messages");
115    //        let msgs = self.messages.into_iter().filter(
116    //            |x| x.id <= 254).collect::<Vec<MavMessage>>();
117    //        self.messages = msgs;
118    //        self
119    //    }
120
121    /// Simple header comment
122    #[inline(always)]
123    fn emit_comments(&self, dialect_name: &str) -> TokenStream {
124        let message = format!("MAVLink {dialect_name} dialect.");
125        quote!(
126            #![doc = #message]
127            #![doc = ""]
128            #![doc = "This file was automatically generated, do not edit."]
129        )
130    }
131
132    /// Emit rust messages
133    #[inline(always)]
134    fn emit_msgs(&self) -> Vec<TokenStream> {
135        self.messages
136            .values()
137            .map(|d| d.emit_rust(self.version.is_some()))
138            .collect()
139    }
140
141    /// Emit rust enums
142    #[inline(always)]
143    fn emit_enums(&self) -> Vec<TokenStream> {
144        self.enums.values().map(|d| d.emit_rust()).collect()
145    }
146
147    #[inline(always)]
148    fn emit_deprecations(&self) -> Vec<TokenStream> {
149        self.messages
150            .values()
151            .map(|msg| {
152                msg.deprecated
153                    .as_ref()
154                    .map(|d| d.emit_tokens())
155                    .unwrap_or_default()
156            })
157            .collect()
158    }
159
160    /// Get list of original message names
161    #[inline(always)]
162    fn emit_enum_names(&self) -> Vec<TokenStream> {
163        self.messages
164            .values()
165            .map(|msg| {
166                let name = format_ident!("{}", msg.name);
167                quote!(#name)
168            })
169            .collect()
170    }
171
172    /// Emit message names with "_DATA" at the end
173    #[inline(always)]
174    fn emit_struct_names(&self) -> Vec<TokenStream> {
175        self.messages
176            .values()
177            .map(|msg| msg.emit_struct_name())
178            .collect()
179    }
180
181    fn emit_rust(&self, dialect_name: &str) -> TokenStream {
182        //TODO verify that id_width of u8 is OK even in mavlink v1
183        let id_width = format_ident!("u32");
184
185        let comment = self.emit_comments(dialect_name);
186        let mav_minor_version = self.emit_minor_version();
187        let mav_dialect_number = self.emit_dialect_number();
188        let msgs = self.emit_msgs();
189        let deprecations = self.emit_deprecations();
190        let enum_names = self.emit_enum_names();
191        let struct_names = self.emit_struct_names();
192        let enums = self.emit_enums();
193
194        let variant_docs = self.emit_variant_description();
195
196        let mav_message =
197            self.emit_mav_message(&variant_docs, &deprecations, &enum_names, &struct_names);
198        let mav_message_all_ids = self.emit_mav_message_all_ids();
199        let mav_message_all_messages = self.emit_mav_message_all_messages();
200        let mav_message_parse = self.emit_mav_message_parse(&enum_names, &struct_names);
201        let mav_message_crc = self.emit_mav_message_crc(&id_width, &struct_names);
202        let mav_message_name = self.emit_mav_message_name(&enum_names, &struct_names);
203        let mav_message_id = self.emit_mav_message_id(&enum_names, &struct_names);
204        let mav_message_id_from_name = self.emit_mav_message_id_from_name(&struct_names);
205        let mav_message_default_from_id =
206            self.emit_mav_message_default_from_id(&enum_names, &struct_names);
207        let mav_message_random_from_id =
208            self.emit_mav_message_random_from_id(&enum_names, &struct_names);
209        let mav_message_serialize = self.emit_mav_message_serialize(&enum_names);
210        let mav_message_target_system_id = self.emit_mav_message_target_system_id();
211        let mav_message_target_component_id = self.emit_mav_message_target_component_id();
212
213        quote! {
214            #comment
215            #![allow(deprecated)]
216            #![allow(clippy::match_single_binding)]
217            #[allow(unused_imports)]
218            use num_derive::{FromPrimitive, ToPrimitive};
219            #[allow(unused_imports)]
220            use num_traits::{FromPrimitive, ToPrimitive};
221            #[allow(unused_imports)]
222            use bitflags::{bitflags, Flags};
223            #[allow(unused_imports)]
224            use mavlink_core::{MavlinkVersion, Message, MessageData, bytes::Bytes, bytes_mut::BytesMut, types::CharArray};
225
226            #[cfg(feature = "serde")]
227            use serde::{Serialize, Deserialize};
228
229            #[cfg(feature = "arbitrary")]
230            use arbitrary::Arbitrary;
231
232            #[cfg(feature = "ts")]
233            use ts_rs::TS;
234
235            #mav_minor_version
236            #mav_dialect_number
237
238            #(#enums)*
239
240            #(#msgs)*
241
242            #[derive(Clone, PartialEq, Debug)]
243            #mav_message
244
245            impl MavMessage {
246                #mav_message_all_ids
247                #mav_message_all_messages
248            }
249
250            impl Message for MavMessage {
251                #mav_message_parse
252                #mav_message_name
253                #mav_message_id
254                #mav_message_id_from_name
255                #mav_message_default_from_id
256                #mav_message_random_from_id
257                #mav_message_serialize
258                #mav_message_crc
259                #mav_message_target_system_id
260                #mav_message_target_component_id
261            }
262        }
263    }
264
265    #[inline(always)]
266    fn emit_mav_message(
267        &self,
268        docs: &[TokenStream],
269        deprecations: &[TokenStream],
270        enums: &[TokenStream],
271        structs: &[TokenStream],
272    ) -> TokenStream {
273        quote! {
274            #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
275            #[cfg_attr(feature = "serde", serde(tag = "type"))]
276            #[cfg_attr(feature = "arbitrary", derive(Arbitrary))]
277            #[cfg_attr(feature = "ts", derive(TS))]
278            #[cfg_attr(feature = "ts", ts(export))]
279            #[repr(u32)]
280            pub enum MavMessage {
281                #(#docs #deprecations #enums(#structs),)*
282            }
283        }
284    }
285
286    fn emit_variant_description(&self) -> Vec<TokenStream> {
287        self.messages
288            .values()
289            .map(|msg| {
290                let mut ts = TokenStream::new();
291
292                if let Some(doc) = msg.description.as_ref() {
293                    let doc = format!("{doc}{}", if doc.ends_with('.') { "" } else { "." });
294                    let doc = URL_REGEX.replace_all(&doc, "<$1>");
295                    ts.extend(quote!(#[doc = #doc]));
296
297                    // Leave a blank line before the message ID for readability.
298                    ts.extend(quote!(#[doc = ""]));
299                }
300
301                let id = format!("ID: {}", msg.id);
302                ts.extend(quote!(#[doc = #id]));
303
304                ts
305            })
306            .collect()
307    }
308
309    #[inline(always)]
310    fn emit_mav_message_all_ids(&self) -> TokenStream {
311        let mut message_ids = self.messages.values().map(|m| m.id).collect::<Vec<u32>>();
312        message_ids.sort();
313
314        quote!(
315            pub const fn all_ids() -> &'static [u32] {
316                &[#(#message_ids),*]
317            }
318        )
319    }
320
321    #[inline(always)]
322    fn emit_minor_version(&self) -> TokenStream {
323        if let Some(version) = self.version {
324            quote! (pub const MINOR_MAVLINK_VERSION: u8 = #version;)
325        } else {
326            TokenStream::default()
327        }
328    }
329
330    #[inline(always)]
331    fn emit_dialect_number(&self) -> TokenStream {
332        if let Some(dialect) = self.dialect {
333            quote! (pub const DIALECT_NUMBER: u8 = #dialect;)
334        } else {
335            TokenStream::default()
336        }
337    }
338
339    #[inline(always)]
340    fn emit_mav_message_parse(
341        &self,
342        enums: &[TokenStream],
343        structs: &[TokenStream],
344    ) -> TokenStream {
345        let id_width = format_ident!("u32");
346
347        quote! {
348            fn parse(version: MavlinkVersion, id: #id_width, payload: &[u8]) -> Result<Self, ::mavlink_core::error::ParserError> {
349                match id {
350                    #(#structs::ID => #structs::deser(version, payload).map(Self::#enums),)*
351                    _ => {
352                        Err(::mavlink_core::error::ParserError::UnknownMessage { id })
353                    },
354                }
355            }
356        }
357    }
358
359    #[inline(always)]
360    fn emit_mav_message_crc(&self, id_width: &Ident, structs: &[TokenStream]) -> TokenStream {
361        quote! {
362            fn extra_crc(id: #id_width) -> u8 {
363                match id {
364                    #(#structs::ID => #structs::EXTRA_CRC,)*
365                    _ => {
366                        0
367                    },
368                }
369            }
370        }
371    }
372
373    #[inline(always)]
374    fn emit_mav_message_name(&self, enums: &[TokenStream], structs: &[TokenStream]) -> TokenStream {
375        quote! {
376            fn message_name(&self) -> &'static str {
377                match self {
378                    #(Self::#enums(..) => #structs::NAME,)*
379                }
380            }
381        }
382    }
383
384    #[inline(always)]
385    fn emit_mav_message_id(&self, enums: &[TokenStream], structs: &[TokenStream]) -> TokenStream {
386        let id_width = format_ident!("u32");
387        quote! {
388            fn message_id(&self) -> #id_width {
389                match self {
390                    #(Self::#enums(..) => #structs::ID,)*
391                }
392            }
393        }
394    }
395
396    #[inline(always)]
397    fn emit_mav_message_id_from_name(&self, structs: &[TokenStream]) -> TokenStream {
398        quote! {
399            fn message_id_from_name(name: &str) -> Option<u32> {
400                match name {
401                    #(#structs::NAME => Some(#structs::ID),)*
402                    _ => {
403                        None
404                    }
405                }
406            }
407        }
408    }
409
410    #[inline(always)]
411    fn emit_mav_message_default_from_id(
412        &self,
413        enums: &[TokenStream],
414        structs: &[TokenStream],
415    ) -> TokenStream {
416        quote! {
417            fn default_message_from_id(id: u32) -> Option<Self> {
418                match id {
419                    #(#structs::ID => Some(Self::#enums(#structs::default())),)*
420                    _ => {
421                        None
422                    }
423                }
424            }
425        }
426    }
427
428    #[inline(always)]
429    fn emit_mav_message_random_from_id(
430        &self,
431        enums: &[TokenStream],
432        structs: &[TokenStream],
433    ) -> TokenStream {
434        quote! {
435            #[cfg(feature = "arbitrary")]
436            fn random_message_from_id<R: rand::RngCore>(id: u32, rng: &mut R) -> Option<Self> {
437                match id {
438                    #(#structs::ID => Some(Self::#enums(#structs::random(rng))),)*
439                    _ => None,
440                }
441            }
442        }
443    }
444
445    #[inline(always)]
446    fn emit_mav_message_serialize(&self, enums: &Vec<TokenStream>) -> TokenStream {
447        quote! {
448            fn ser(&self, version: MavlinkVersion, bytes: &mut [u8]) -> usize {
449                match self {
450                    #(Self::#enums(body) => body.ser(version, bytes),)*
451                }
452            }
453        }
454    }
455
456    #[inline(always)]
457    fn emit_mav_message_target_system_id(&self) -> TokenStream {
458        let arms: Vec<TokenStream> = self
459            .messages
460            .values()
461            .filter(|msg| msg.fields.iter().any(|f| f.name == "target_system"))
462            .map(|msg| {
463                let variant = format_ident!("{}", msg.name);
464                quote!(Self::#variant(inner) => Some(inner.target_system),)
465            })
466            .collect();
467
468        quote! {
469            fn target_system_id(&self) -> Option<u8> {
470                match self {
471                    #(#arms)*
472                    _ => None,
473                }
474            }
475        }
476    }
477
478    #[inline(always)]
479    fn emit_mav_message_target_component_id(&self) -> TokenStream {
480        let arms: Vec<TokenStream> = self
481            .messages
482            .values()
483            .filter(|msg| msg.fields.iter().any(|f| f.name == "target_component"))
484            .map(|msg| {
485                let variant = format_ident!("{}", msg.name);
486                quote!(Self::#variant(inner) => Some(inner.target_component),)
487            })
488            .collect();
489
490        quote! {
491            fn target_component_id(&self) -> Option<u8> {
492                match self {
493                    #(#arms)*
494                    _ => None,
495                }
496            }
497        }
498    }
499
500    #[inline(always)]
501    fn emit_mav_message_all_messages(&self) -> TokenStream {
502        let mut entries = self
503            .messages
504            .values()
505            .map(|msg| (msg.id, msg.emit_struct_name()))
506            .collect::<Vec<_>>();
507
508        entries.sort_by_key(|(id, _)| *id);
509
510        let pairs = entries
511            .into_iter()
512            .map(|(_, struct_name)| quote!((#struct_name::NAME, #struct_name::ID)))
513            .collect::<Vec<_>>();
514
515        quote! {
516            pub const fn all_messages() -> &'static [(&'static str, u32)] {
517                &[#(#pairs),*]
518            }
519        }
520    }
521}
522
523#[derive(Debug, PartialEq, Clone, Default)]
524#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
525pub struct MavEnum {
526    pub name: String,
527    pub description: Option<String>,
528    pub entries: Vec<MavEnumEntry>,
529    /// If contains Some, the string represents the primitive type (size) for bitflags.
530    /// If no fields use this enum, the bitmask is true, but primitive is None. In this case
531    /// regular enum is generated as primitive is unknown.
532    pub primitive: Option<String>,
533    pub bitmask: bool,
534    pub deprecated: Option<MavDeprecation>,
535}
536
537impl MavEnum {
538    fn try_combine(&mut self, enm: &Self) {
539        if self.name == enm.name {
540            for enum_entry in &enm.entries {
541                let found_entry = self.entries.iter().find(|elem| {
542                    elem.name == enum_entry.name && elem.value.unwrap() == enum_entry.value.unwrap()
543                });
544                match found_entry {
545                    Some(entry) => panic!("Enum entry {} already exists", entry.name),
546                    None => self.entries.push(enum_entry.clone()),
547                }
548            }
549        }
550    }
551
552    fn emit_defs(&self) -> Vec<TokenStream> {
553        let mut cnt = 0u64;
554        self.entries
555            .iter()
556            .map(|enum_entry| {
557                let name = format_ident!("{}", enum_entry.name.clone());
558                let value;
559
560                let deprecation = enum_entry.emit_deprecation();
561
562                let description = if let Some(description) = enum_entry.description.as_ref() {
563                    let description = URL_REGEX.replace_all(description, "<$1>");
564                    quote!(#[doc = #description])
565                } else {
566                    quote!()
567                };
568
569                let params_doc = enum_entry.emit_params();
570
571                if let Some(tmp_value) = enum_entry.value {
572                    cnt = cnt.max(tmp_value);
573                    let tmp = TokenStream::from_str(&tmp_value.to_string()).unwrap();
574                    value = quote!(#tmp);
575                } else {
576                    cnt += 1;
577                    value = quote!(#cnt);
578                }
579
580                if self.primitive.is_some() {
581                    quote! {
582                        #deprecation
583                        #description
584                        #params_doc
585                        const #name = #value;
586                    }
587                } else {
588                    quote! {
589                        #deprecation
590                        #description
591                        #params_doc
592                        #name = #value,
593                    }
594                }
595            })
596            .collect()
597    }
598
599    #[inline(always)]
600    fn emit_name(&self) -> TokenStream {
601        let name = format_ident!("{}", self.name);
602        quote!(#name)
603    }
604
605    #[inline(always)]
606    fn emit_const_default(&self) -> TokenStream {
607        let default = format_ident!("{}", self.entries[0].name);
608        quote!(pub const DEFAULT: Self = Self::#default;)
609    }
610
611    #[inline(always)]
612    fn emit_deprecation(&self) -> TokenStream {
613        self.deprecated
614            .as_ref()
615            .map(|d| d.emit_tokens())
616            .unwrap_or_default()
617    }
618
619    fn emit_rust(&self) -> TokenStream {
620        let defs = self.emit_defs();
621        let enum_name = self.emit_name();
622        let const_default = self.emit_const_default();
623
624        let deprecated = self.emit_deprecation();
625
626        let description = if let Some(description) = self.description.as_ref() {
627            let desc = URL_REGEX.replace_all(description, "<$1>");
628            quote!(#[doc = #desc])
629        } else {
630            quote!()
631        };
632
633        let mav_bool_impl = if self.name == "MavBool"
634            && self
635                .entries
636                .iter()
637                .any(|entry| entry.name == "MAV_BOOL_TRUE")
638        {
639            quote!(
640                pub fn as_bool(&self) -> bool {
641                    self.contains(Self::MAV_BOOL_TRUE)
642                }
643            )
644        } else {
645            quote!()
646        };
647
648        let enum_def;
649        if let Some(primitive) = self.primitive.clone() {
650            let primitive = format_ident!("{}", primitive);
651            enum_def = quote! {
652                bitflags!{
653                    #[cfg_attr(feature = "ts", derive(TS))]
654                    #[cfg_attr(feature = "ts", ts(export, type = "number"))]
655                    #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
656                    #[cfg_attr(feature = "arbitrary", derive(Arbitrary))]
657                    #[derive(Debug, Copy, Clone, PartialEq)]
658                    #deprecated
659                    #description
660                    pub struct #enum_name: #primitive {
661                        #(#defs)*
662                    }
663                }
664            };
665        } else {
666            enum_def = quote! {
667                #[cfg_attr(feature = "ts", derive(TS))]
668                #[cfg_attr(feature = "ts", ts(export))]
669                #[derive(Debug, Copy, Clone, PartialEq, FromPrimitive, ToPrimitive)]
670                #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
671                #[cfg_attr(feature = "serde", serde(tag = "type"))]
672                #[cfg_attr(feature = "arbitrary", derive(Arbitrary))]
673                #[repr(u32)]
674                #deprecated
675                #description
676                pub enum #enum_name {
677                    #(#defs)*
678                }
679            };
680        }
681
682        quote! {
683            #enum_def
684
685            impl #enum_name {
686                #const_default
687                #mav_bool_impl
688            }
689
690            impl Default for #enum_name {
691                fn default() -> Self {
692                    Self::DEFAULT
693                }
694            }
695        }
696    }
697}
698
699#[derive(Debug, PartialEq, Clone, Default)]
700#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
701pub struct MavEnumEntry {
702    pub value: Option<u64>,
703    pub name: String,
704    pub description: Option<String>,
705    pub params: Option<Vec<MavParam>>,
706    pub deprecated: Option<MavDeprecation>,
707}
708
709impl MavEnumEntry {
710    #[inline(always)]
711    fn emit_deprecation(&self) -> TokenStream {
712        self.deprecated
713            .as_ref()
714            .map(|d| d.emit_tokens())
715            .unwrap_or_default()
716    }
717
718    #[inline(always)]
719    fn emit_params(&self) -> TokenStream {
720        let Some(params) = &self.params else {
721            return quote!();
722        };
723        let any_value_range = params.iter().any(|p| {
724            p.min_value.is_some()
725                || p.max_value.is_some()
726                || p.increment.is_some()
727                || p.enum_used.is_some()
728                || (p.reserved && p.default.is_some())
729        });
730        let any_units = params.iter().any(|p| p.units.is_some());
731        let lines = params
732            .iter()
733            .map(|param| param.emit_doc_row(any_value_range, any_units));
734        let mut table_header = "| Parameter | Description |".to_string();
735        let mut table_hl = "| --------- | ----------- |".to_string();
736        if any_value_range {
737            table_header += " Values |";
738            table_hl += " ------ |";
739        }
740        if any_units {
741            table_header += " Units |";
742            table_hl += " ----- |";
743        }
744        quote! {
745            #[doc = ""]
746            #[doc = "# Parameters"]
747            #[doc = ""]
748            #[doc = #table_header]
749            #[doc = #table_hl]
750            #(#lines)*
751        }
752    }
753}
754
755#[derive(Debug, PartialEq, Clone, Default)]
756#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
757pub struct MavParam {
758    pub index: usize,
759    pub description: Option<String>,
760    pub label: Option<String>,
761    pub units: Option<String>,
762    pub enum_used: Option<String>,
763    pub increment: Option<f32>,
764    pub min_value: Option<f32>,
765    pub max_value: Option<f32>,
766    pub reserved: bool,
767    pub default: Option<f32>,
768}
769
770fn format_number_range(min: Option<f32>, max: Option<f32>, inc: Option<f32>) -> String {
771    match (min, max, inc) {
772        (Some(min), Some(max), Some(inc)) => {
773            if min + inc == max {
774                format!("{min}, {max}")
775            } else if min + 2. * inc == max {
776                format!("{}, {}, {}", min, min + inc, max)
777            } else {
778                format!("{}, {}, .. , {}", min, min + inc, max)
779            }
780        }
781        (Some(min), Some(max), None) => format!("{min} .. {max}"),
782        (Some(min), None, Some(inc)) => format!("{}, {}, ..", min, min + inc),
783        (None, Some(max), Some(inc)) => format!(".., {}, {}", max - inc, max),
784        (Some(min), None, None) => format!("&ge; {min}"),
785        (None, Some(max), None) => format!("&le; {max}"),
786        (None, None, Some(inc)) => format!("Multiples of {inc}"),
787        (None, None, None) => String::new(),
788    }
789}
790
791impl MavParam {
792    fn format_valid_values(&self) -> String {
793        if let (true, Some(default)) = (self.reserved, self.default) {
794            format!("Reserved (use {default})")
795        } else if let Some(enum_used) = &self.enum_used {
796            format!("[`{enum_used}`]")
797        } else {
798            format_number_range(self.min_value, self.max_value, self.increment)
799        }
800    }
801
802    fn emit_doc_row(&self, value_range_col: bool, units_col: bool) -> TokenStream {
803        let label = if let Some(label) = &self.label {
804            format!("{} ({})", self.index, label)
805        } else {
806            format!("{}", self.index)
807        };
808        let mut line = format!(
809            "| {label:10}| {:12}|",
810            self.description.as_deref().unwrap_or_default()
811        );
812        if value_range_col {
813            let range = self.format_valid_values();
814            line += &format!(" {range} |");
815        }
816        if units_col {
817            let units = self.units.clone().unwrap_or_default();
818            line += &format!(" {units} |");
819        }
820        quote! {#[doc = #line]}
821    }
822}
823
824#[derive(Debug, PartialEq, Clone, Default)]
825#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
826pub struct MavMessage {
827    pub id: u32,
828    pub name: String,
829    pub description: Option<String>,
830    pub fields: Vec<MavField>,
831    pub deprecated: Option<MavDeprecation>,
832}
833
834impl MavMessage {
835    /// Return Token of "MESSAGE_NAME_DATA
836    /// for mavlink struct data
837    fn emit_struct_name(&self) -> TokenStream {
838        let name = format_ident!("{}", format!("{}_DATA", self.name));
839        quote!(#name)
840    }
841
842    #[inline(always)]
843    fn emit_name_types(&self) -> (Vec<TokenStream>, usize) {
844        let mut encoded_payload_len: usize = 0;
845        let field_toks = self
846            .fields
847            .iter()
848            .map(|field| {
849                let nametype = field.emit_name_type();
850                encoded_payload_len += field.mavtype.len();
851
852                let description = field.emit_description();
853
854                // From MAVLink specification:
855                // If sent by an implementation that doesn't have the extensions fields
856                // then the recipient will see zero values for the extensions fields.
857                let serde_default = if field.is_extension {
858                    if field.enumtype.is_some() {
859                        quote!(#[cfg_attr(feature = "serde", serde(default))])
860                    } else {
861                        quote!(#[cfg_attr(feature = "serde", serde(default = "crate::RustDefault::rust_default"))])
862                    }
863                } else {
864                    quote!()
865                };
866
867                let serde_with_attr = if matches!(field.mavtype, MavType::Array(_, _)) {
868                    quote!(
869                        #[cfg_attr(feature = "serde", serde(with = "serde_arrays"))]
870                        #[cfg_attr(feature = "ts", ts(type = "Array<number>"))]
871                    )
872                } else if matches!(field.mavtype, MavType::CharArray(_)) {
873                    quote!(
874                        #[cfg_attr(feature = "ts", ts(type = "string"))]
875                    )
876                } else {
877                    quote!()
878                };
879
880                quote! {
881                    #description
882                    #serde_default
883                    #serde_with_attr
884                    #nametype
885                }
886            })
887            .collect::<Vec<TokenStream>>();
888        (field_toks, encoded_payload_len)
889    }
890
891    /// Generate description for the given message
892    #[inline(always)]
893    fn emit_description(&self) -> TokenStream {
894        let mut ts = TokenStream::new();
895        if let Some(doc) = self.description.as_ref() {
896            let doc = format!("{doc}{}", if doc.ends_with('.') { "" } else { "." });
897            // create hyperlinks
898            let doc = URL_REGEX.replace_all(&doc, "<$1>");
899            ts.extend(quote!(#[doc = #doc]));
900            // Leave a blank line before the message ID for readability.
901            ts.extend(quote!(#[doc = ""]));
902        }
903        let id = format!("ID: {}", self.id);
904        ts.extend(quote!(#[doc = #id]));
905        ts
906    }
907
908    #[inline(always)]
909    fn emit_serialize_vars(&self) -> TokenStream {
910        let (base_fields, ext_fields): (Vec<_>, Vec<_>) =
911            self.fields.iter().partition(|f| !f.is_extension);
912        let ser_vars = base_fields.iter().map(|f| f.rust_writer());
913        let ser_ext_vars = ext_fields.iter().map(|f| f.rust_writer());
914        quote! {
915            let mut __tmp = BytesMut::new(bytes);
916
917            // TODO: these lints are produced on a couple of cubepilot messages
918            // because they are generated as empty structs with no fields and
919            // therefore Self::ENCODED_LEN is 0. This itself is a bug because
920            // cubepilot.xml has unclosed tags in fields, which the parser has
921            // bad time handling. It should probably be fixed in both the parser
922            // and mavlink message definitions. However, until it's done, let's
923            // skip the lints.
924            #[allow(clippy::absurd_extreme_comparisons)]
925            #[allow(unused_comparisons)]
926            if __tmp.remaining() < Self::ENCODED_LEN {
927                panic!(
928                    "buffer is too small (need {} bytes, but got {})",
929                    Self::ENCODED_LEN,
930                    __tmp.remaining(),
931                )
932            }
933
934            #(#ser_vars)*
935            if matches!(version, MavlinkVersion::V2) {
936                #(#ser_ext_vars)*
937                let len = __tmp.len();
938                ::mavlink_core::utils::remove_trailing_zeroes(&bytes[..len])
939            } else {
940                __tmp.len()
941            }
942        }
943    }
944
945    #[inline(always)]
946    fn emit_deserialize_vars(&self) -> TokenStream {
947        let deser_vars = self
948            .fields
949            .iter()
950            .map(|f| f.rust_reader())
951            .collect::<Vec<TokenStream>>();
952
953        if deser_vars.is_empty() {
954            // struct has no fields
955            quote! {
956                Ok(Self::default())
957            }
958        } else {
959            quote! {
960                let avail_len = __input.len();
961
962                let mut payload_buf  = [0; Self::ENCODED_LEN];
963                let mut buf = if avail_len < Self::ENCODED_LEN {
964                    //copy available bytes into an oversized buffer filled with zeros
965                    payload_buf[0..avail_len].copy_from_slice(__input);
966                    Bytes::new(&payload_buf)
967                } else {
968                    // fast zero copy
969                    Bytes::new(__input)
970                };
971
972                let mut __struct = Self::default();
973                #(#deser_vars)*
974                Ok(__struct)
975            }
976        }
977    }
978
979    #[inline(always)]
980    fn emit_default_impl(&self) -> TokenStream {
981        let msg_name = self.emit_struct_name();
982        quote! {
983            impl Default for #msg_name {
984                fn default() -> Self {
985                    Self::DEFAULT.clone()
986                }
987            }
988        }
989    }
990
991    #[inline(always)]
992    fn emit_deprecation(&self) -> TokenStream {
993        self.deprecated
994            .as_ref()
995            .map(|d| d.emit_tokens())
996            .unwrap_or_default()
997    }
998
999    #[inline(always)]
1000    fn emit_const_default(&self, dialect_has_version: bool) -> TokenStream {
1001        let initializers = self
1002            .fields
1003            .iter()
1004            .map(|field| field.emit_default_initializer(dialect_has_version));
1005        quote!(pub const DEFAULT: Self = Self { #(#initializers)* };)
1006    }
1007
1008    fn emit_rust(&self, dialect_has_version: bool) -> TokenStream {
1009        let msg_name = self.emit_struct_name();
1010        let id = self.id;
1011        let name = self.name.clone();
1012        let extra_crc = extra_crc(self);
1013        let (name_types, payload_encoded_len) = self.emit_name_types();
1014        assert!(
1015            payload_encoded_len <= 255,
1016            "maximum payload length is 255 bytes"
1017        );
1018
1019        let deser_vars = self.emit_deserialize_vars();
1020        let serialize_vars = self.emit_serialize_vars();
1021        let const_default = self.emit_const_default(dialect_has_version);
1022        let default_impl = self.emit_default_impl();
1023
1024        let deprecation = self.emit_deprecation();
1025
1026        let description = self.emit_description();
1027
1028        quote! {
1029            #deprecation
1030            #description
1031            #[derive(Debug, Clone, PartialEq)]
1032            #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
1033            #[cfg_attr(feature = "arbitrary", derive(Arbitrary))]
1034            #[cfg_attr(feature = "ts", derive(TS))]
1035            #[cfg_attr(feature = "ts", ts(export))]
1036            pub struct #msg_name {
1037                #(#name_types)*
1038            }
1039
1040            impl #msg_name {
1041                pub const ENCODED_LEN: usize = #payload_encoded_len;
1042                #const_default
1043
1044                #[cfg(feature = "arbitrary")]
1045                pub fn random<R: rand::RngCore>(rng: &mut R) -> Self {
1046                    use arbitrary::{Unstructured, Arbitrary};
1047                    let mut buf = [0u8; 1024];
1048                    rng.fill_bytes(&mut buf);
1049                    let mut unstructured = Unstructured::new(&buf);
1050                    Self::arbitrary(&mut unstructured).unwrap_or_default()
1051                }
1052            }
1053
1054            #default_impl
1055
1056            impl MessageData for #msg_name {
1057                type Message = MavMessage;
1058
1059                const ID: u32 = #id;
1060                const NAME: &'static str = #name;
1061                const EXTRA_CRC: u8 = #extra_crc;
1062                const ENCODED_LEN: usize = #payload_encoded_len;
1063
1064                fn deser(_version: MavlinkVersion, __input: &[u8]) -> Result<Self, ::mavlink_core::error::ParserError> {
1065                    #deser_vars
1066                }
1067
1068                fn ser(&self, version: MavlinkVersion, bytes: &mut [u8]) -> usize {
1069                    #serialize_vars
1070                }
1071            }
1072        }
1073    }
1074
1075    /// Ensures that a message does not contain duplicate field names.
1076    ///
1077    /// Duplicate field names would generate invalid Rust structs.
1078    fn validate_unique_fields(&self) {
1079        let mut seen: HashSet<&str> = HashSet::new();
1080        for f in &self.fields {
1081            let name: &str = &f.name;
1082            assert!(
1083                seen.insert(name),
1084                "Duplicate field '{}' found in message '{}' while generating bindings",
1085                name,
1086                self.name
1087            );
1088        }
1089    }
1090
1091    /// Ensure that the fields count is at least one and no more than 64
1092    fn validate_field_count(&self) {
1093        assert!(
1094            !self.fields.is_empty(),
1095            "Message '{}' does not any fields",
1096            self.name
1097        );
1098        assert!(
1099            self.fields.len() <= 64,
1100            "Message '{}' has more then 64 fields",
1101            self.name
1102        );
1103    }
1104}
1105
1106#[derive(Debug, PartialEq, Clone, Default)]
1107#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
1108pub struct MavField {
1109    pub mavtype: MavType,
1110    pub name: String,
1111    pub description: Option<String>,
1112    pub enumtype: Option<String>,
1113    pub display: Option<String>,
1114    pub is_extension: bool,
1115}
1116
1117impl MavField {
1118    /// Emit rust name of a given field
1119    #[inline(always)]
1120    fn emit_name(&self) -> TokenStream {
1121        let name = format_ident!("{}", self.name);
1122        quote!(#name)
1123    }
1124
1125    /// Emit rust type of the field
1126    #[inline(always)]
1127    fn emit_type(&self) -> TokenStream {
1128        let mavtype;
1129        if matches!(self.mavtype, MavType::Array(_, _)) {
1130            let rt = TokenStream::from_str(&self.mavtype.rust_type()).unwrap();
1131            mavtype = quote!(#rt);
1132        } else if let Some(enumname) = &self.enumtype {
1133            let en = TokenStream::from_str(enumname).unwrap();
1134            mavtype = quote!(#en);
1135        } else {
1136            let rt = TokenStream::from_str(&self.mavtype.rust_type()).unwrap();
1137            mavtype = quote!(#rt);
1138        }
1139        mavtype
1140    }
1141
1142    /// Generate description for the given field
1143    #[inline(always)]
1144    fn emit_description(&self) -> TokenStream {
1145        let mut ts = TokenStream::new();
1146        if let Some(val) = self.description.as_ref() {
1147            let desc = URL_REGEX.replace_all(val, "<$1>");
1148            ts.extend(quote!(#[doc = #desc]));
1149        }
1150        ts
1151    }
1152
1153    /// Combine rust name and type of a given field
1154    #[inline(always)]
1155    fn emit_name_type(&self) -> TokenStream {
1156        let name = self.emit_name();
1157        let fieldtype = self.emit_type();
1158        quote!(pub #name: #fieldtype,)
1159    }
1160
1161    /// Emit writer
1162    fn rust_writer(&self) -> TokenStream {
1163        let mut name = "self.".to_string() + &self.name.clone();
1164        if self.enumtype.is_some() {
1165            // casts are not necessary for arrays, because they are currently
1166            // generated as primitive arrays
1167            if !matches!(self.mavtype, MavType::Array(_, _)) {
1168                if let Some(dsp) = &self.display {
1169                    // potentially a bitflag
1170                    if dsp == "bitmask" {
1171                        // it is a bitflag
1172                        name += ".bits() as ";
1173                        name += &self.mavtype.rust_type();
1174                    } else {
1175                        panic!("Display option not implemented");
1176                    }
1177                } else {
1178                    // an enum, have to use "*foo as u8" cast
1179                    name += " as ";
1180                    name += &self.mavtype.rust_type();
1181                }
1182            }
1183        }
1184        let ts = TokenStream::from_str(&name).unwrap();
1185        let name = quote!(#ts);
1186        let buf = format_ident!("__tmp");
1187        self.mavtype.rust_writer(&name, buf)
1188    }
1189
1190    /// Emit reader
1191    fn rust_reader(&self) -> TokenStream {
1192        let _name = TokenStream::from_str(&self.name).unwrap();
1193
1194        let name = quote!(__struct.#_name);
1195        let buf = format_ident!("buf");
1196        if let Some(enum_name) = &self.enumtype {
1197            // TODO: handle enum arrays properly, rather than just generating
1198            // primitive arrays
1199            if let MavType::Array(_t, _size) = &self.mavtype {
1200                return self.mavtype.rust_reader(&name, buf);
1201            }
1202            if let Some(dsp) = &self.display {
1203                if dsp == "bitmask" {
1204                    // bitflags
1205                    let tmp = self.mavtype.rust_reader(&quote!(let tmp), buf);
1206                    let enum_name_ident = format_ident!("{}", enum_name);
1207                    quote! {
1208                        #tmp
1209                        #name = #enum_name_ident::from_bits(tmp as <#enum_name_ident as Flags>::Bits)
1210                            .ok_or(::mavlink_core::error::ParserError::InvalidFlag { flag_type: #enum_name, value: tmp as u64 })?;
1211                    }
1212                } else {
1213                    panic!("Display option not implemented");
1214                }
1215            } else {
1216                // handle enum by FromPrimitive
1217                let tmp = self.mavtype.rust_reader(&quote!(let tmp), buf);
1218                let val = format_ident!("from_{}", &self.mavtype.rust_type());
1219                quote!(
1220                    #tmp
1221                    #name = FromPrimitive::#val(tmp)
1222                        .ok_or(::mavlink_core::error::ParserError::InvalidEnum { enum_type: #enum_name, value: tmp as u64 })?;
1223                )
1224            }
1225        } else {
1226            self.mavtype.rust_reader(&name, buf)
1227        }
1228    }
1229
1230    #[inline(always)]
1231    fn emit_default_initializer(&self, dialect_has_version: bool) -> TokenStream {
1232        let field = self.emit_name();
1233        // FIXME: Is this actually expected behaviour??
1234        if matches!(self.mavtype, MavType::Array(_, _)) {
1235            let default_value = self.mavtype.emit_default_value(dialect_has_version);
1236            quote!(#field: #default_value,)
1237        } else if let Some(enumname) = &self.enumtype {
1238            let ty = TokenStream::from_str(enumname).unwrap();
1239            quote!(#field: #ty::DEFAULT,)
1240        } else {
1241            let default_value = self.mavtype.emit_default_value(dialect_has_version);
1242            quote!(#field: #default_value,)
1243        }
1244    }
1245}
1246
1247#[derive(Debug, PartialEq, Clone, Default)]
1248#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
1249pub enum MavType {
1250    UInt8MavlinkVersion,
1251    #[default]
1252    UInt8,
1253    UInt16,
1254    UInt32,
1255    UInt64,
1256    Int8,
1257    Int16,
1258    Int32,
1259    Int64,
1260    Char,
1261    Float,
1262    Double,
1263    CharArray(usize),
1264    Array(Box<Self>, usize),
1265}
1266
1267impl MavType {
1268    fn parse_type(s: &str) -> Option<Self> {
1269        use self::MavType::*;
1270        match s {
1271            "uint8_t_mavlink_version" => Some(UInt8MavlinkVersion),
1272            "uint8_t" => Some(UInt8),
1273            "uint16_t" => Some(UInt16),
1274            "uint32_t" => Some(UInt32),
1275            "uint64_t" => Some(UInt64),
1276            "int8_t" => Some(Int8),
1277            "int16_t" => Some(Int16),
1278            "int32_t" => Some(Int32),
1279            "int64_t" => Some(Int64),
1280            "char" => Some(Char),
1281            "float" => Some(Float),
1282            "Double" => Some(Double),
1283            "double" => Some(Double),
1284            _ if s.starts_with("char[") => {
1285                let start = 4;
1286                let size = s[start + 1..(s.len() - 1)].parse::<usize>().ok()?;
1287                Some(CharArray(size))
1288            }
1289            _ if s.ends_with(']') => {
1290                let start = s.find('[')?;
1291                let size = s[start + 1..(s.len() - 1)].parse::<usize>().ok()?;
1292                let mtype = Self::parse_type(&s[0..start])?;
1293                Some(Array(Box::new(mtype), size))
1294            }
1295            _ => None,
1296        }
1297    }
1298
1299    /// Emit reader of a given type
1300    pub fn rust_reader(&self, val: &TokenStream, buf: Ident) -> TokenStream {
1301        use self::MavType::*;
1302        match self {
1303            Char => quote! {#val = #buf.get_u8()?;},
1304            UInt8 => quote! {#val = #buf.get_u8()?;},
1305            UInt16 => quote! {#val = #buf.get_u16_le()?;},
1306            UInt32 => quote! {#val = #buf.get_u32_le()?;},
1307            UInt64 => quote! {#val = #buf.get_u64_le()?;},
1308            UInt8MavlinkVersion => quote! {#val = #buf.get_u8()?;},
1309            Int8 => quote! {#val = #buf.get_i8()?;},
1310            Int16 => quote! {#val = #buf.get_i16_le()?;},
1311            Int32 => quote! {#val = #buf.get_i32_le()?;},
1312            Int64 => quote! {#val = #buf.get_i64_le()?;},
1313            Float => quote! {#val = #buf.get_f32_le()?;},
1314            Double => quote! {#val = #buf.get_f64_le()?;},
1315            CharArray(size) => {
1316                quote! {
1317                    let mut tmp = [0_u8; #size];
1318                    for v in &mut tmp {
1319                        *v = #buf.get_u8()?;
1320                    }
1321                    #val = CharArray::new(tmp);
1322                }
1323            }
1324            Array(t, _) => {
1325                let r = t.rust_reader(&quote!(let val), buf);
1326                quote! {
1327                    for v in &mut #val {
1328                        #r
1329                        *v = val;
1330                    }
1331                }
1332            }
1333        }
1334    }
1335
1336    /// Emit writer of a given type
1337    pub fn rust_writer(&self, val: &TokenStream, buf: Ident) -> TokenStream {
1338        use self::MavType::*;
1339        match self {
1340            UInt8MavlinkVersion => quote! {#buf.put_u8(#val);},
1341            UInt8 => quote! {#buf.put_u8(#val);},
1342            Char => quote! {#buf.put_u8(#val);},
1343            UInt16 => quote! {#buf.put_u16_le(#val);},
1344            UInt32 => quote! {#buf.put_u32_le(#val);},
1345            Int8 => quote! {#buf.put_i8(#val);},
1346            Int16 => quote! {#buf.put_i16_le(#val);},
1347            Int32 => quote! {#buf.put_i32_le(#val);},
1348            Float => quote! {#buf.put_f32_le(#val);},
1349            UInt64 => quote! {#buf.put_u64_le(#val);},
1350            Int64 => quote! {#buf.put_i64_le(#val);},
1351            Double => quote! {#buf.put_f64_le(#val);},
1352            CharArray(_) => {
1353                let w = Char.rust_writer(&quote!(*val), buf);
1354                quote! {
1355                    for val in &#val {
1356                        #w
1357                    }
1358                }
1359            }
1360            Array(t, _size) => {
1361                let w = t.rust_writer(&quote!(*val), buf);
1362                quote! {
1363                    for val in &#val {
1364                        #w
1365                    }
1366                }
1367            }
1368        }
1369    }
1370
1371    /// Size of a given Mavtype
1372    fn len(&self) -> usize {
1373        use self::MavType::*;
1374        match self {
1375            UInt8MavlinkVersion | UInt8 | Int8 | Char => 1,
1376            UInt16 | Int16 => 2,
1377            UInt32 | Int32 | Float => 4,
1378            UInt64 | Int64 | Double => 8,
1379            CharArray(size) => *size,
1380            Array(t, size) => t.len() * size,
1381        }
1382    }
1383
1384    fn max_int_value(&self) -> u64 {
1385        match self {
1386            Self::UInt8MavlinkVersion | Self::UInt8 => u8::MAX as u64,
1387            Self::UInt16 => u16::MAX as u64,
1388            Self::UInt32 => u32::MAX as u64,
1389            Self::UInt64 => u64::MAX,
1390            Self::Int8 | Self::Char | Self::CharArray(_) => i8::MAX as u64,
1391            Self::Int16 => i16::MAX as u64,
1392            Self::Int32 => i32::MAX as u64,
1393            Self::Int64 => i64::MAX as u64,
1394            // maximum precisly representable value minus 1 for float types
1395            Self::Float => (1 << f32::MANTISSA_DIGITS) - 1,
1396            Self::Double => (1 << f64::MANTISSA_DIGITS) - 1,
1397            Self::Array(mav_type, _) => mav_type.max_int_value(),
1398        }
1399    }
1400
1401    /// Used for ordering of types
1402    fn order_len(&self) -> usize {
1403        use self::MavType::*;
1404        match self {
1405            UInt8MavlinkVersion | UInt8 | Int8 | Char | CharArray(_) => 1,
1406            UInt16 | Int16 => 2,
1407            UInt32 | Int32 | Float => 4,
1408            UInt64 | Int64 | Double => 8,
1409            Array(t, _) => t.len(),
1410        }
1411    }
1412
1413    /// Used for crc calculation
1414    pub fn primitive_type(&self) -> String {
1415        use self::MavType::*;
1416        match self {
1417            UInt8MavlinkVersion => "uint8_t".into(),
1418            UInt8 => "uint8_t".into(),
1419            Int8 => "int8_t".into(),
1420            Char => "char".into(),
1421            UInt16 => "uint16_t".into(),
1422            Int16 => "int16_t".into(),
1423            UInt32 => "uint32_t".into(),
1424            Int32 => "int32_t".into(),
1425            Float => "float".into(),
1426            UInt64 => "uint64_t".into(),
1427            Int64 => "int64_t".into(),
1428            Double => "double".into(),
1429            CharArray(_) => "char".into(),
1430            Array(t, _) => t.primitive_type(),
1431        }
1432    }
1433
1434    /// Return rust equivalent of a given Mavtype
1435    /// Used for generating struct fields.
1436    pub fn rust_type(&self) -> String {
1437        use self::MavType::*;
1438        match self {
1439            UInt8 | UInt8MavlinkVersion => "u8".into(),
1440            Int8 => "i8".into(),
1441            Char => "u8".into(),
1442            UInt16 => "u16".into(),
1443            Int16 => "i16".into(),
1444            UInt32 => "u32".into(),
1445            Int32 => "i32".into(),
1446            Float => "f32".into(),
1447            UInt64 => "u64".into(),
1448            Int64 => "i64".into(),
1449            Double => "f64".into(),
1450            CharArray(size) => format!("CharArray<{size}>"),
1451            Array(t, size) => format!("[{};{}]", t.rust_type(), size),
1452        }
1453    }
1454
1455    pub fn emit_default_value(&self, dialect_has_version: bool) -> TokenStream {
1456        use self::MavType::*;
1457        match self {
1458            UInt8 => quote!(0_u8),
1459            UInt8MavlinkVersion => {
1460                if dialect_has_version {
1461                    quote!(MINOR_MAVLINK_VERSION)
1462                } else {
1463                    quote!(0_u8)
1464                }
1465            }
1466            Int8 => quote!(0_i8),
1467            Char => quote!(0_u8),
1468            UInt16 => quote!(0_u16),
1469            Int16 => quote!(0_i16),
1470            UInt32 => quote!(0_u32),
1471            Int32 => quote!(0_i32),
1472            Float => quote!(0.0_f32),
1473            UInt64 => quote!(0_u64),
1474            Int64 => quote!(0_i64),
1475            Double => quote!(0.0_f64),
1476            CharArray(size) => quote!(CharArray::new([0_u8; #size])),
1477            Array(ty, size) => {
1478                let default_value = ty.emit_default_value(dialect_has_version);
1479                quote!([#default_value; #size])
1480            }
1481        }
1482    }
1483
1484    /// Return rust equivalent of the primitive type of a MavType. The primitive
1485    /// type is the type itself for all except arrays, in which case it is the
1486    /// element type.
1487    pub fn rust_primitive_type(&self) -> String {
1488        use self::MavType::*;
1489        match self {
1490            Array(t, _) => t.rust_primitive_type(),
1491            _ => self.rust_type(),
1492        }
1493    }
1494
1495    /// Compare two MavTypes
1496    pub fn compare(&self, other: &Self) -> Ordering {
1497        let len = self.order_len();
1498        (-(len as isize)).cmp(&(-(other.order_len() as isize)))
1499    }
1500}
1501
1502#[derive(Debug, PartialEq, Eq, Clone, Default)]
1503#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
1504pub struct MavDeprecation {
1505    // YYYY-MM
1506    pub since: String,
1507    pub replaced_by: Option<String>,
1508    pub note: Option<String>,
1509}
1510
1511impl MavDeprecation {
1512    pub fn emit_tokens(&self) -> TokenStream {
1513        let since = &self.since;
1514        let note = match &self.note {
1515            Some(str) if str.is_empty() || str.ends_with(".") => str.clone(),
1516            Some(str) => format!("{str}."),
1517            None => String::new(),
1518        };
1519        let replaced_by = match &self.replaced_by {
1520            Some(str) if str.starts_with('`') => format!("See {str}"),
1521            Some(str) => format!("See `{str}`"),
1522            None => String::new(),
1523        };
1524        let message = format!("{note} {replaced_by} (Deprecated since {since})");
1525        quote!(#[deprecated = #message])
1526    }
1527}
1528
1529#[derive(Debug, PartialEq, Eq, Clone, Copy)]
1530#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
1531#[cfg_attr(feature = "serde", serde(tag = "type"))]
1532pub enum MavXmlElement {
1533    Version,
1534    Mavlink,
1535    Dialect,
1536    Include,
1537    Enums,
1538    Enum,
1539    Entry,
1540    Description,
1541    Param,
1542    Messages,
1543    Message,
1544    Field,
1545    Deprecated,
1546    Wip,
1547    Extensions,
1548}
1549
1550const fn identify_element(s: &[u8]) -> Option<MavXmlElement> {
1551    use self::MavXmlElement::*;
1552    match s {
1553        b"version" => Some(Version),
1554        b"mavlink" => Some(Mavlink),
1555        b"dialect" => Some(Dialect),
1556        b"include" => Some(Include),
1557        b"enums" => Some(Enums),
1558        b"enum" => Some(Enum),
1559        b"entry" => Some(Entry),
1560        b"description" => Some(Description),
1561        b"param" => Some(Param),
1562        b"messages" => Some(Messages),
1563        b"message" => Some(Message),
1564        b"field" => Some(Field),
1565        b"deprecated" => Some(Deprecated),
1566        b"wip" => Some(Wip),
1567        b"extensions" => Some(Extensions),
1568        _ => None,
1569    }
1570}
1571
1572fn is_valid_parent(p: Option<MavXmlElement>, s: MavXmlElement) -> bool {
1573    use self::MavXmlElement::*;
1574    match s {
1575        Version => p == Some(Mavlink),
1576        Mavlink => p.is_none(),
1577        Dialect => p == Some(Mavlink),
1578        Include => p == Some(Mavlink),
1579        Enums => p == Some(Mavlink),
1580        Enum => p == Some(Enums),
1581        Entry => p == Some(Enum),
1582        Description => p == Some(Entry) || p == Some(Message) || p == Some(Enum),
1583        Param => p == Some(Entry),
1584        Messages => p == Some(Mavlink),
1585        Message => p == Some(Messages),
1586        Field => p == Some(Message),
1587        Deprecated => p == Some(Entry) || p == Some(Message) || p == Some(Enum),
1588        Wip => p == Some(Entry) || p == Some(Message) || p == Some(Enum),
1589        Extensions => p == Some(Message),
1590    }
1591}
1592
1593pub fn parse_profile(
1594    definitions_dir: &Path,
1595    definition_file: &Path,
1596    parsed_files: &mut HashSet<PathBuf>,
1597) -> Result<MavProfile, BindGenError> {
1598    let in_path = Path::new(&definitions_dir).join(definition_file);
1599    parsed_files.insert(in_path.clone()); // Keep track of which files have been parsed
1600
1601    let mut stack: Vec<MavXmlElement> = vec![];
1602
1603    let mut text = None;
1604
1605    let mut profile = MavProfile::default();
1606    let mut field = MavField::default();
1607    let mut message = MavMessage::default();
1608    let mut mavenum = MavEnum::default();
1609    let mut entry = MavEnumEntry::default();
1610    let mut param_index: Option<usize> = None;
1611    let mut param_label: Option<String> = None;
1612    let mut param_units: Option<String> = None;
1613    let mut param_enum: Option<String> = None;
1614    let mut param_increment: Option<f32> = None;
1615    let mut param_min_value: Option<f32> = None;
1616    let mut param_max_value: Option<f32> = None;
1617    let mut param_reserved = false;
1618    let mut param_default: Option<f32> = None;
1619    let mut deprecated: Option<MavDeprecation> = None;
1620
1621    let mut xml_filter = MavXmlFilter::default();
1622    let mut events: Vec<Result<Event, quick_xml::Error>> = Vec::new();
1623    let file = File::open(&in_path).map_err(|e| BindGenError::CouldNotReadDefinitionFile {
1624        source: e,
1625        path: in_path.clone(),
1626    })?;
1627    let mut reader = Reader::from_reader(BufReader::new(file));
1628    reader.config_mut().trim_text(true);
1629    reader.config_mut().expand_empty_elements = true;
1630
1631    let mut buf = Vec::new();
1632    loop {
1633        match reader.read_event_into(&mut buf) {
1634            Ok(Event::Eof) => {
1635                events.push(Ok(Event::Eof));
1636                break;
1637            }
1638            Ok(event) => events.push(Ok(event.into_owned())),
1639            Err(why) => events.push(Err(why)),
1640        }
1641        buf.clear();
1642    }
1643    xml_filter.filter(&mut events);
1644    let mut is_in_extension = false;
1645    for e in events {
1646        match e {
1647            Ok(Event::Start(bytes)) => {
1648                let Some(id) = identify_element(bytes.name().into_inner()) else {
1649                    panic!(
1650                        "unexpected element {:?}",
1651                        String::from_utf8_lossy(bytes.name().into_inner())
1652                    );
1653                };
1654
1655                assert!(
1656                    is_valid_parent(stack.last().copied(), id),
1657                    "not valid parent {:?} of {id:?}",
1658                    stack.last(),
1659                );
1660
1661                match id {
1662                    MavXmlElement::Extensions => {
1663                        is_in_extension = true;
1664                    }
1665                    MavXmlElement::Message => {
1666                        message = MavMessage::default();
1667                    }
1668                    MavXmlElement::Field => {
1669                        field = MavField {
1670                            is_extension: is_in_extension,
1671                            ..Default::default()
1672                        };
1673                    }
1674                    MavXmlElement::Enum => {
1675                        mavenum = MavEnum::default();
1676                    }
1677                    MavXmlElement::Entry => {
1678                        if mavenum.entries.is_empty() {
1679                            mavenum.deprecated = deprecated;
1680                        }
1681                        deprecated = None;
1682                        entry = MavEnumEntry::default();
1683                    }
1684                    MavXmlElement::Param => {
1685                        param_index = None;
1686                        param_increment = None;
1687                        param_min_value = None;
1688                        param_max_value = None;
1689                        param_reserved = false;
1690                        param_default = None;
1691                    }
1692                    MavXmlElement::Deprecated => {
1693                        deprecated = Some(MavDeprecation {
1694                            replaced_by: None,
1695                            since: String::new(),
1696                            note: None,
1697                        });
1698                    }
1699                    _ => (),
1700                }
1701                stack.push(id);
1702
1703                for attr in bytes.attributes() {
1704                    let attr = attr.unwrap();
1705                    match stack.last() {
1706                        Some(&MavXmlElement::Enum) => {
1707                            if attr.key.into_inner() == b"name" {
1708                                mavenum.name = to_pascal_case(attr.value);
1709                                //mavenum.name = attr.value.clone();
1710                            } else if attr.key.into_inner() == b"bitmask" {
1711                                mavenum.bitmask = true;
1712                            }
1713                        }
1714                        Some(&MavXmlElement::Entry) => {
1715                            match attr.key.into_inner() {
1716                                b"name" => {
1717                                    entry.name = String::from_utf8_lossy(&attr.value).to_string();
1718                                }
1719                                b"value" => {
1720                                    let value = String::from_utf8_lossy(&attr.value);
1721                                    // Deal with hexadecimal numbers
1722                                    let (src, radix) = value
1723                                        .strip_prefix("0x")
1724                                        .map(|value| (value, 16))
1725                                        .unwrap_or((value.as_ref(), 10));
1726                                    entry.value = u64::from_str_radix(src, radix).ok();
1727                                }
1728                                _ => (),
1729                            }
1730                        }
1731                        Some(&MavXmlElement::Message) => {
1732                            match attr.key.into_inner() {
1733                                b"name" => {
1734                                    /*message.name = attr
1735                                    .value
1736                                    .clone()
1737                                    .split("_")
1738                                    .map(|x| x.to_lowercase())
1739                                    .map(|x| {
1740                                        let mut v: Vec<char> = x.chars().collect();
1741                                        v[0] = v[0].to_uppercase().nth(0).unwrap();
1742                                        v.into_iter().collect()
1743                                    })
1744                                    .collect::<Vec<String>>()
1745                                    .join("");
1746                                    */
1747                                    message.name = String::from_utf8_lossy(&attr.value).to_string();
1748                                }
1749                                b"id" => {
1750                                    message.id =
1751                                        String::from_utf8_lossy(&attr.value).parse().unwrap();
1752                                }
1753                                _ => (),
1754                            }
1755                        }
1756                        Some(&MavXmlElement::Field) => {
1757                            match attr.key.into_inner() {
1758                                b"name" => {
1759                                    let name = String::from_utf8_lossy(&attr.value);
1760                                    field.name = if name == "type" {
1761                                        "mavtype".to_string()
1762                                    } else {
1763                                        name.to_string()
1764                                    };
1765                                }
1766                                b"type" => {
1767                                    let r#type = String::from_utf8_lossy(&attr.value);
1768                                    field.mavtype = MavType::parse_type(&r#type).unwrap();
1769                                }
1770                                b"enum" => {
1771                                    field.enumtype = Some(to_pascal_case(&attr.value));
1772
1773                                    // Update field display if enum is a bitmask
1774                                    if let Some(e) =
1775                                        profile.enums.get(field.enumtype.as_ref().unwrap())
1776                                    {
1777                                        if e.bitmask {
1778                                            field.display = Some("bitmask".to_string());
1779                                        }
1780                                    }
1781                                }
1782                                b"display" => {
1783                                    field.display =
1784                                        Some(String::from_utf8_lossy(&attr.value).to_string());
1785                                }
1786                                _ => (),
1787                            }
1788                        }
1789                        Some(&MavXmlElement::Param) => {
1790                            if entry.params.is_none() {
1791                                entry.params = Some(vec![]);
1792                            }
1793                            match attr.key.into_inner() {
1794                                b"index" => {
1795                                    let value = String::from_utf8_lossy(&attr.value)
1796                                        .parse()
1797                                        .expect("failed to parse param index");
1798                                    assert!(
1799                                        (1..=7).contains(&value),
1800                                        "param index must be between 1 and 7"
1801                                    );
1802                                    param_index = Some(value);
1803                                }
1804                                b"label" => {
1805                                    param_label =
1806                                        std::str::from_utf8(&attr.value).ok().map(str::to_owned);
1807                                }
1808                                b"increment" => {
1809                                    param_increment = Some(
1810                                        String::from_utf8_lossy(&attr.value)
1811                                            .parse()
1812                                            .expect("failed to parse param increment"),
1813                                    );
1814                                }
1815                                b"minValue" => {
1816                                    param_min_value = Some(
1817                                        String::from_utf8_lossy(&attr.value)
1818                                            .parse()
1819                                            .expect("failed to parse param minValue"),
1820                                    );
1821                                }
1822                                b"maxValue" => {
1823                                    param_max_value = Some(
1824                                        String::from_utf8_lossy(&attr.value)
1825                                            .parse()
1826                                            .expect("failed to parse param maxValue"),
1827                                    );
1828                                }
1829                                b"units" => {
1830                                    param_units =
1831                                        std::str::from_utf8(&attr.value).ok().map(str::to_owned);
1832                                }
1833                                b"enum" => {
1834                                    param_enum =
1835                                        std::str::from_utf8(&attr.value).ok().map(to_pascal_case);
1836                                }
1837                                b"reserved" => {
1838                                    param_reserved = attr.value.as_ref() == b"true";
1839                                }
1840                                b"default" => {
1841                                    param_default = Some(
1842                                        String::from_utf8_lossy(&attr.value)
1843                                            .parse()
1844                                            .expect("failed to parse param maxValue"),
1845                                    );
1846                                }
1847                                _ => (),
1848                            }
1849                        }
1850                        Some(&MavXmlElement::Deprecated) => match attr.key.into_inner() {
1851                            b"since" => {
1852                                deprecated.as_mut().unwrap().since =
1853                                    String::from_utf8_lossy(&attr.value).to_string();
1854                            }
1855                            b"replaced_by" => {
1856                                let value = String::from_utf8_lossy(&attr.value);
1857                                deprecated.as_mut().unwrap().replaced_by = if value.is_empty() {
1858                                    None
1859                                } else {
1860                                    Some(value.to_string())
1861                                };
1862                            }
1863                            _ => (),
1864                        },
1865                        _ => (),
1866                    }
1867                }
1868            }
1869            Ok(Event::Text(bytes)) => {
1870                let s = String::from_utf8_lossy(&bytes);
1871
1872                use self::MavXmlElement::*;
1873                match (stack.last(), stack.get(stack.len() - 2)) {
1874                    (Some(&Description), Some(&Message))
1875                    | (Some(&Field), Some(&Message))
1876                    | (Some(&Description), Some(&Enum))
1877                    | (Some(&Description), Some(&Entry))
1878                    | (Some(&Include), Some(&Mavlink))
1879                    | (Some(&Version), Some(&Mavlink))
1880                    | (Some(&Dialect), Some(&Mavlink))
1881                    | (Some(&Param), Some(&Entry))
1882                    | (Some(Deprecated), _) => {
1883                        text = Some(text.map(|t| t + s.as_ref()).unwrap_or(s.to_string()));
1884                    }
1885                    data => {
1886                        panic!("unexpected text data {data:?} reading {s:?}");
1887                    }
1888                }
1889            }
1890            Ok(Event::GeneralRef(bytes)) => {
1891                let entity = String::from_utf8_lossy(&bytes);
1892                text = Some(
1893                    text.map(|t| format!("{t}&{entity};"))
1894                        .unwrap_or(format!("&{entity};")),
1895                );
1896            }
1897            Ok(Event::End(_)) => {
1898                match stack.last() {
1899                    Some(&MavXmlElement::Field) => {
1900                        field.description = text.map(|t| t.replace('\n', " "));
1901                        message.fields.push(field.clone());
1902                    }
1903                    Some(&MavXmlElement::Entry) => {
1904                        entry.deprecated = deprecated;
1905                        deprecated = None;
1906                        mavenum.entries.push(entry.clone());
1907                    }
1908                    Some(&MavXmlElement::Message) => {
1909                        message.deprecated = deprecated;
1910
1911                        deprecated = None;
1912                        is_in_extension = false;
1913                        // Follow mavlink ordering specification: https://mavlink.io/en/guide/serialization.html#field_reordering
1914                        let mut not_extension_fields = message.fields.clone();
1915                        let mut extension_fields = message.fields.clone();
1916
1917                        not_extension_fields.retain(|field| !field.is_extension);
1918                        extension_fields.retain(|field| field.is_extension);
1919
1920                        // Only not mavlink 1 fields need to be sorted
1921                        not_extension_fields.sort_by(|a, b| a.mavtype.compare(&b.mavtype));
1922
1923                        // Update msg fields and add the new message
1924                        let mut msg = message.clone();
1925                        msg.fields.clear();
1926                        msg.fields.extend(not_extension_fields);
1927                        msg.fields.extend(extension_fields);
1928
1929                        // Validate there are no duplicate field names
1930                        msg.validate_unique_fields();
1931                        // Validate field count must be between 1 and 64
1932                        msg.validate_field_count();
1933
1934                        profile.add_message(&msg);
1935                    }
1936                    Some(&MavXmlElement::Enum) => {
1937                        profile.add_enum(&mavenum);
1938                    }
1939                    Some(&MavXmlElement::Include) => {
1940                        let include =
1941                            PathBuf::from(text.map(|t| t.replace('\n', "")).unwrap_or_default());
1942                        let include_file = Path::new(&definitions_dir).join(include.clone());
1943                        if !parsed_files.contains(&include_file) {
1944                            let included_profile =
1945                                parse_profile(definitions_dir, &include, parsed_files)?;
1946                            for message in included_profile.messages.values() {
1947                                profile.add_message(message);
1948                            }
1949                            for enm in included_profile.enums.values() {
1950                                profile.add_enum(enm);
1951                            }
1952                            if profile.version.is_none() {
1953                                profile.version = included_profile.version;
1954                            }
1955                        }
1956                    }
1957                    Some(&MavXmlElement::Description) => match stack.get(stack.len() - 2) {
1958                        Some(&MavXmlElement::Message) => {
1959                            message.description = text.map(|t| t.replace('\n', " "));
1960                        }
1961                        Some(&MavXmlElement::Enum) => {
1962                            mavenum.description = text.map(|t| t.replace('\n', " "));
1963                        }
1964                        Some(&MavXmlElement::Entry) => {
1965                            entry.description = text.map(|t| t.replace('\n', " "));
1966                        }
1967                        _ => (),
1968                    },
1969                    Some(&MavXmlElement::Version) => {
1970                        if let Some(t) = text {
1971                            profile.version =
1972                                Some(t.parse().expect("Invalid minor version number format"));
1973                        }
1974                    }
1975                    Some(&MavXmlElement::Dialect) => {
1976                        if let Some(t) = text {
1977                            profile.dialect =
1978                                Some(t.parse().expect("Invalid dialect number format"));
1979                        }
1980                    }
1981                    Some(&MavXmlElement::Deprecated) => {
1982                        if let Some(t) = text {
1983                            deprecated.as_mut().unwrap().note = Some(t);
1984                        }
1985                    }
1986                    Some(&MavXmlElement::Param) => {
1987                        if let Some(params) = entry.params.as_mut() {
1988                            // Some messages can jump between values, like: 1, 2, 7
1989                            let param_index = param_index.expect("entry params must have an index");
1990                            while params.len() < param_index {
1991                                params.push(MavParam {
1992                                    index: params.len() + 1,
1993                                    description: None,
1994                                    ..Default::default()
1995                                });
1996                            }
1997                            if let Some((min, max)) = param_min_value.zip(param_max_value) {
1998                                assert!(
1999                                    min <= max,
2000                                    "param minValue must not be greater than maxValue"
2001                                );
2002                            }
2003                            params[param_index - 1] = MavParam {
2004                                index: param_index,
2005                                description: text.map(|t| t.replace('\n', " ")),
2006                                label: param_label,
2007                                units: param_units,
2008                                enum_used: param_enum,
2009                                increment: param_increment,
2010                                max_value: param_max_value,
2011                                min_value: param_min_value,
2012                                reserved: param_reserved,
2013                                default: param_default,
2014                            };
2015                            param_label = None;
2016                            param_units = None;
2017                            param_enum = None;
2018                        }
2019                    }
2020                    _ => (),
2021                }
2022                text = None;
2023                stack.pop();
2024                // println!("{}-{}", indent(depth), name);
2025            }
2026            Err(e) => {
2027                eprintln!("Error: {e}");
2028                break;
2029            }
2030            _ => {}
2031        }
2032    }
2033
2034    //let profile = profile.update_messages(); //TODO verify no longer needed
2035    Ok(profile.update_enums())
2036}
2037
2038/// Generate protobuf represenation of mavlink message set
2039/// Generate rust representation of mavlink message set with appropriate conversion methods
2040pub fn generate<W: Write>(
2041    definitions_dir: &Path,
2042    definition_file: &Path,
2043    output_rust: &mut W,
2044) -> Result<(), BindGenError> {
2045    let mut parsed_files: HashSet<PathBuf> = HashSet::new();
2046    let profile = parse_profile(definitions_dir, definition_file, &mut parsed_files)?;
2047
2048    let dialect_name = util::to_dialect_name(definition_file);
2049
2050    // rust file
2051    let rust_tokens = profile.emit_rust(&dialect_name);
2052    writeln!(output_rust, "{rust_tokens}").unwrap();
2053
2054    Ok(())
2055}
2056
2057/// CRC operates over names of the message and names of its fields
2058/// Hence we have to preserve the original uppercase names delimited with an underscore
2059/// For field names, we replace "type" with "mavtype" to make it rust compatible (this is
2060/// needed for generating sensible rust code), but for calculating crc function we have to
2061/// use the original name "type"
2062pub fn extra_crc(msg: &MavMessage) -> u8 {
2063    // calculate a 8-bit checksum of the key fields of a message, so we
2064    // can detect incompatible XML changes
2065    let mut crc = CRCu16::crc16mcrf4cc();
2066
2067    crc.digest(msg.name.as_bytes());
2068    crc.digest(b" ");
2069
2070    let mut f = msg.fields.clone();
2071    // only mavlink 1 fields should be part of the extra_crc
2072    f.retain(|f| !f.is_extension);
2073    f.sort_by(|a, b| a.mavtype.compare(&b.mavtype));
2074    for field in &f {
2075        crc.digest(field.mavtype.primitive_type().as_bytes());
2076        crc.digest(b" ");
2077        if field.name == "mavtype" {
2078            crc.digest(b"type");
2079        } else {
2080            crc.digest(field.name.as_bytes());
2081        }
2082        crc.digest(b" ");
2083        if let MavType::Array(_, size) | MavType::CharArray(size) = field.mavtype {
2084            crc.digest(&[size as u8]);
2085        }
2086    }
2087
2088    let crcval = crc.get_crc();
2089    ((crcval & 0xFF) ^ (crcval >> 8)) as u8
2090}
2091
2092#[cfg(not(feature = "emit-extensions"))]
2093struct ExtensionFilter {
2094    pub is_in: bool,
2095}
2096
2097struct MessageFilter {
2098    pub is_in: bool,
2099    pub messages: Vec<String>,
2100}
2101
2102impl MessageFilter {
2103    pub fn new() -> Self {
2104        Self {
2105            is_in: false,
2106            messages: vec![
2107                // device_cap_flags is u32, when enum is u16, which is not handled by the parser yet
2108                "STORM32_GIMBAL_MANAGER_INFORMATION".to_string(),
2109            ],
2110        }
2111    }
2112}
2113
2114struct MavXmlFilter {
2115    #[cfg(not(feature = "emit-extensions"))]
2116    extension_filter: ExtensionFilter,
2117    message_filter: MessageFilter,
2118}
2119
2120impl Default for MavXmlFilter {
2121    fn default() -> Self {
2122        Self {
2123            #[cfg(not(feature = "emit-extensions"))]
2124            extension_filter: ExtensionFilter { is_in: false },
2125            message_filter: MessageFilter::new(),
2126        }
2127    }
2128}
2129
2130impl MavXmlFilter {
2131    pub fn filter(&mut self, elements: &mut Vec<Result<Event, quick_xml::Error>>) {
2132        elements.retain(|x| self.filter_extension(x) && self.filter_messages(x));
2133    }
2134
2135    #[cfg(feature = "emit-extensions")]
2136    pub fn filter_extension(&mut self, _element: &Result<Event, quick_xml::Error>) -> bool {
2137        true
2138    }
2139
2140    /// Ignore extension fields
2141    #[cfg(not(feature = "emit-extensions"))]
2142    pub fn filter_extension(&mut self, element: &Result<Event, quick_xml::Error>) -> bool {
2143        match element {
2144            Ok(content) => {
2145                match content {
2146                    Event::Start(bytes) | Event::Empty(bytes) => {
2147                        let Some(id) = identify_element(bytes.name().into_inner()) else {
2148                            panic!(
2149                                "unexpected element {:?}",
2150                                String::from_utf8_lossy(bytes.name().into_inner())
2151                            );
2152                        };
2153                        if id == MavXmlElement::Extensions {
2154                            self.extension_filter.is_in = true;
2155                        }
2156                    }
2157                    Event::End(bytes) => {
2158                        let Some(id) = identify_element(bytes.name().into_inner()) else {
2159                            panic!(
2160                                "unexpected element {:?}",
2161                                String::from_utf8_lossy(bytes.name().into_inner())
2162                            );
2163                        };
2164
2165                        if id == MavXmlElement::Message {
2166                            self.extension_filter.is_in = false;
2167                        }
2168                    }
2169                    _ => {}
2170                }
2171                !self.extension_filter.is_in
2172            }
2173            Err(error) => panic!("Failed to filter XML: {error}"),
2174        }
2175    }
2176
2177    /// Filters messages by their name
2178    pub fn filter_messages(&mut self, element: &Result<Event, quick_xml::Error>) -> bool {
2179        match element {
2180            Ok(content) => {
2181                match content {
2182                    Event::Start(bytes) | Event::Empty(bytes) => {
2183                        let Some(id) = identify_element(bytes.name().into_inner()) else {
2184                            panic!(
2185                                "unexpected element {:?}",
2186                                String::from_utf8_lossy(bytes.name().into_inner())
2187                            );
2188                        };
2189                        if id == MavXmlElement::Message {
2190                            for attr in bytes.attributes() {
2191                                let attr = attr.unwrap();
2192                                if attr.key.into_inner() == b"name" {
2193                                    let value = String::from_utf8_lossy(&attr.value).into_owned();
2194                                    if self.message_filter.messages.contains(&value) {
2195                                        self.message_filter.is_in = true;
2196                                        return false;
2197                                    }
2198                                }
2199                            }
2200                        }
2201                    }
2202                    Event::End(bytes) => {
2203                        let Some(id) = identify_element(bytes.name().into_inner()) else {
2204                            panic!(
2205                                "unexpected element {:?}",
2206                                String::from_utf8_lossy(bytes.name().into_inner())
2207                            );
2208                        };
2209
2210                        if id == MavXmlElement::Message && self.message_filter.is_in {
2211                            self.message_filter.is_in = false;
2212                            return false;
2213                        }
2214                    }
2215                    _ => {}
2216                }
2217                !self.message_filter.is_in
2218            }
2219            Err(error) => panic!("Failed to filter XML: {error}"),
2220        }
2221    }
2222}
2223
2224#[inline(always)]
2225fn to_pascal_case(text: impl AsRef<[u8]>) -> String {
2226    let input = text.as_ref();
2227    let mut result = String::with_capacity(input.len());
2228    let mut capitalize = true;
2229
2230    for &b in input {
2231        if b == b'_' {
2232            capitalize = true;
2233            continue;
2234        }
2235
2236        if capitalize {
2237            result.push((b as char).to_ascii_uppercase());
2238            capitalize = false;
2239        } else {
2240            result.push((b as char).to_ascii_lowercase());
2241        }
2242    }
2243
2244    result
2245}
2246
2247#[cfg(test)]
2248mod tests {
2249    use super::*;
2250
2251    #[test]
2252    fn emits_target_id_match_arms() {
2253        // Build a minimal profile containing one message with target fields and one without
2254        let mut profile = MavProfile::default();
2255
2256        let msg_with_targets = MavMessage {
2257            id: 300,
2258            name: "COMMAND_INT".to_string(),
2259            description: None,
2260            fields: vec![
2261                MavField {
2262                    mavtype: MavType::UInt8,
2263                    name: "target_system".to_string(),
2264                    description: None,
2265                    enumtype: None,
2266                    display: None,
2267                    is_extension: false,
2268                },
2269                MavField {
2270                    mavtype: MavType::UInt8,
2271                    name: "target_component".to_string(),
2272                    description: None,
2273                    enumtype: None,
2274                    display: None,
2275                    is_extension: false,
2276                },
2277            ],
2278            deprecated: None,
2279        };
2280
2281        let msg_without_targets = MavMessage {
2282            id: 0,
2283            name: "HEARTBEAT".to_string(),
2284            description: None,
2285            fields: vec![MavField {
2286                mavtype: MavType::UInt32,
2287                name: "custom_mode".to_string(),
2288                description: None,
2289                enumtype: None,
2290                display: None,
2291                is_extension: false,
2292            }],
2293            deprecated: None,
2294        };
2295
2296        profile.add_message(&msg_with_targets);
2297        profile.add_message(&msg_without_targets);
2298
2299        let tokens = profile.emit_rust("common");
2300        let mut code = tokens.to_string();
2301        code.retain(|c| !c.is_whitespace());
2302
2303        // Check the code contains the target_system/component_id functions
2304        assert!(code.contains("fntarget_system_id(&self)->Option<u8>"));
2305        assert!(code.contains("fntarget_component_id(&self)->Option<u8>"));
2306
2307        // Check the generated impl contains arms referencing COMMAND_INT(inner).target_system/component
2308        assert!(code.contains("Self::COMMAND_INT(inner)=>Some(inner.target_system)"));
2309        assert!(code.contains("Self::COMMAND_INT(inner)=>Some(inner.target_component)"));
2310
2311        // Ensure a message without target fields returns None
2312        assert!(!code.contains("Self::HEARTBEAT(inner)=>Some(inner.target_system)"));
2313        assert!(!code.contains("Self::HEARTBEAT(inner)=>Some(inner.target_component)"));
2314    }
2315
2316    #[test]
2317    fn validate_unique_fields_allows_unique() {
2318        let msg = MavMessage {
2319            id: 1,
2320            name: "FOO".to_string(),
2321            description: None,
2322            fields: vec![
2323                MavField {
2324                    mavtype: MavType::UInt8,
2325                    name: "a".to_string(),
2326                    description: None,
2327                    enumtype: None,
2328                    display: None,
2329                    is_extension: false,
2330                },
2331                MavField {
2332                    mavtype: MavType::UInt16,
2333                    name: "b".to_string(),
2334                    description: None,
2335                    enumtype: None,
2336                    display: None,
2337                    is_extension: false,
2338                },
2339            ],
2340            deprecated: None,
2341        };
2342        // Should not panic
2343        msg.validate_unique_fields();
2344    }
2345
2346    #[test]
2347    #[should_panic(expected = "Duplicate field")]
2348    fn validate_unique_fields_panics_on_duplicate() {
2349        let msg = MavMessage {
2350            id: 2,
2351            name: "BAR".to_string(),
2352            description: None,
2353            fields: vec![
2354                MavField {
2355                    mavtype: MavType::UInt8,
2356                    name: "target_system".to_string(),
2357                    description: None,
2358                    enumtype: None,
2359                    display: None,
2360                    is_extension: false,
2361                },
2362                MavField {
2363                    mavtype: MavType::UInt8,
2364                    name: "target_system".to_string(),
2365                    description: None,
2366                    enumtype: None,
2367                    display: None,
2368                    is_extension: false,
2369                },
2370            ],
2371            deprecated: None,
2372        };
2373        // Should panic due to duplicate field names
2374        msg.validate_unique_fields();
2375    }
2376
2377    #[test]
2378    fn validate_field_count_ok() {
2379        let msg = MavMessage {
2380            id: 2,
2381            name: "FOO".to_string(),
2382            description: None,
2383            fields: vec![
2384                MavField {
2385                    mavtype: MavType::UInt8,
2386                    name: "a".to_string(),
2387                    description: None,
2388                    enumtype: None,
2389                    display: None,
2390                    is_extension: false,
2391                },
2392                MavField {
2393                    mavtype: MavType::UInt8,
2394                    name: "b".to_string(),
2395                    description: None,
2396                    enumtype: None,
2397                    display: None,
2398                    is_extension: false,
2399                },
2400            ],
2401            deprecated: None,
2402        };
2403        // Should not panic
2404        msg.validate_field_count();
2405    }
2406
2407    #[test]
2408    #[should_panic]
2409    fn validate_field_count_too_many() {
2410        let mut fields = vec![];
2411        for i in 0..65 {
2412            let field = MavField {
2413                mavtype: MavType::UInt8,
2414                name: format!("field_{i}"),
2415                description: None,
2416                enumtype: None,
2417                display: None,
2418                is_extension: false,
2419            };
2420            fields.push(field);
2421        }
2422        let msg = MavMessage {
2423            id: 2,
2424            name: "BAZ".to_string(),
2425            description: None,
2426            fields,
2427            deprecated: None,
2428        };
2429        // Should panic due to 65 fields
2430        msg.validate_field_count();
2431    }
2432
2433    #[test]
2434    #[should_panic]
2435    fn validate_field_count_empty() {
2436        let msg = MavMessage {
2437            id: 2,
2438            name: "BAM".to_string(),
2439            description: None,
2440            fields: vec![],
2441            deprecated: None,
2442        };
2443        // Should panic due to no fields
2444        msg.validate_field_count();
2445    }
2446
2447    #[test]
2448    fn test_fmt_mav_param_values() {
2449        let enum_param = MavParam {
2450            enum_used: Some("ENUM_NAME".to_string()),
2451            ..Default::default()
2452        };
2453        assert_eq!(enum_param.format_valid_values(), "[`ENUM_NAME`]");
2454
2455        let reserved_param = MavParam {
2456            reserved: true,
2457            default: Some(f32::NAN),
2458            ..Default::default()
2459        };
2460        assert_eq!(reserved_param.format_valid_values(), "Reserved (use NaN)");
2461
2462        let unrestricted_param = MavParam::default();
2463        assert_eq!(unrestricted_param.format_valid_values(), "");
2464
2465        let int_param = MavParam {
2466            increment: Some(1.0),
2467            ..Default::default()
2468        };
2469        assert_eq!(int_param.format_valid_values(), "Multiples of 1");
2470
2471        let pos_param = MavParam {
2472            min_value: Some(0.0),
2473            ..Default::default()
2474        };
2475        assert_eq!(pos_param.format_valid_values(), "&ge; 0");
2476
2477        let max_param = MavParam {
2478            max_value: Some(5.5),
2479            ..Default::default()
2480        };
2481        assert_eq!(max_param.format_valid_values(), "&le; 5.5");
2482
2483        let pos_int_param = MavParam {
2484            min_value: Some(0.0),
2485            increment: Some(1.0),
2486            ..Default::default()
2487        };
2488        assert_eq!(pos_int_param.format_valid_values(), "0, 1, ..");
2489
2490        let max_inc_param = MavParam {
2491            increment: Some(1.0),
2492            max_value: Some(360.0),
2493            ..Default::default()
2494        };
2495        assert_eq!(max_inc_param.format_valid_values(), ".., 359, 360");
2496
2497        let range_param = MavParam {
2498            min_value: Some(0.0),
2499            max_value: Some(10.0),
2500            ..Default::default()
2501        };
2502        assert_eq!(range_param.format_valid_values(), "0 .. 10");
2503
2504        let int_range_param = MavParam {
2505            min_value: Some(0.0),
2506            max_value: Some(10.0),
2507            increment: Some(1.0),
2508            ..Default::default()
2509        };
2510        assert_eq!(int_range_param.format_valid_values(), "0, 1, .. , 10");
2511
2512        let close_inc_range_param = MavParam {
2513            min_value: Some(-2.0),
2514            max_value: Some(2.0),
2515            increment: Some(2.0),
2516            ..Default::default()
2517        };
2518        assert_eq!(close_inc_range_param.format_valid_values(), "-2, 0, 2");
2519
2520        let bin_range_param = MavParam {
2521            min_value: Some(0.0),
2522            max_value: Some(1.0),
2523            increment: Some(1.0),
2524            ..Default::default()
2525        };
2526        assert_eq!(bin_range_param.format_valid_values(), "0, 1");
2527    }
2528
2529    #[test]
2530    fn test_emit_doc_row() {
2531        let param = MavParam {
2532            index: 3,
2533            label: Some("test param".to_string()),
2534            min_value: Some(0.0),
2535            units: Some("m/s".to_string()),
2536            ..Default::default()
2537        };
2538        // test with all variations of columns
2539        assert_eq!(
2540            param.emit_doc_row(false, false).to_string(),
2541            quote! {#[doc = "| 3 (test param)|             |"]}.to_string()
2542        );
2543        assert_eq!(
2544            param.emit_doc_row(false, true).to_string(),
2545            quote! {#[doc = "| 3 (test param)|             | m/s |"]}.to_string()
2546        );
2547        assert_eq!(
2548            param.emit_doc_row(true, false).to_string(),
2549            quote! {#[doc = "| 3 (test param)|             | &ge; 0 |"]}.to_string()
2550        );
2551        assert_eq!(
2552            param.emit_doc_row(true, true).to_string(),
2553            quote! {#[doc = "| 3 (test param)|             | &ge; 0 | m/s |"]}.to_string()
2554        );
2555
2556        let unlabeled_param = MavParam {
2557            index: 2,
2558            ..Default::default()
2559        };
2560        assert_eq!(
2561            unlabeled_param.emit_doc_row(false, false).to_string(),
2562            quote! {#[doc = "| 2         |             |"]}.to_string()
2563        );
2564    }
2565}