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