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;
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; Self::ENCODED_LEN];
960                    payload_buf[0..avail_len].copy_from_slice(__input);
961                    Bytes::new(&payload_buf)
962                } else {
963                    // fast zero copy
964                    Bytes::new(__input)
965                };
966
967                let mut __struct = Self::default();
968                #(#deser_vars)*
969                Ok(__struct)
970            }
971        }
972    }
973
974    #[inline(always)]
975    fn emit_default_impl(&self) -> TokenStream {
976        let msg_name = self.emit_struct_name();
977        quote! {
978            impl Default for #msg_name {
979                fn default() -> Self {
980                    Self::DEFAULT.clone()
981                }
982            }
983        }
984    }
985
986    #[inline(always)]
987    fn emit_deprecation(&self) -> TokenStream {
988        self.deprecated
989            .as_ref()
990            .map(|d| d.emit_tokens())
991            .unwrap_or_default()
992    }
993
994    #[inline(always)]
995    fn emit_const_default(&self, dialect_has_version: bool) -> TokenStream {
996        let initializers = self
997            .fields
998            .iter()
999            .map(|field| field.emit_default_initializer(dialect_has_version));
1000        quote!(pub const DEFAULT: Self = Self { #(#initializers)* };)
1001    }
1002
1003    fn emit_rust(&self, dialect_has_version: bool) -> TokenStream {
1004        let msg_name = self.emit_struct_name();
1005        let id = self.id;
1006        let name = self.name.clone();
1007        let extra_crc = extra_crc(self);
1008        let (name_types, payload_encoded_len) = self.emit_name_types();
1009        assert!(
1010            (1..=255).contains(&payload_encoded_len),
1011            "payload length must be between 1 and 255 bytes"
1012        );
1013
1014        let deser_vars = self.emit_deserialize_vars();
1015        let serialize_vars = self.emit_serialize_vars();
1016        let const_default = self.emit_const_default(dialect_has_version);
1017        let default_impl = self.emit_default_impl();
1018
1019        let deprecation = self.emit_deprecation();
1020
1021        let description = self.emit_description();
1022
1023        quote! {
1024            #deprecation
1025            #description
1026            #[derive(Debug, Clone, PartialEq)]
1027            #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
1028            #[cfg_attr(feature = "arbitrary", derive(Arbitrary))]
1029            #[cfg_attr(feature = "ts-rs", derive(TS))]
1030            #[cfg_attr(feature = "ts-rs", ts(export))]
1031            pub struct #msg_name {
1032                #(#name_types)*
1033            }
1034
1035            impl #msg_name {
1036                pub const ENCODED_LEN: usize = #payload_encoded_len;
1037                #const_default
1038
1039                #[cfg(feature = "arbitrary")]
1040                pub fn random<R: rand::RngCore>(rng: &mut R) -> Self {
1041                    use arbitrary::{Unstructured, Arbitrary};
1042                    let mut buf = [0u8; 1024];
1043                    rng.fill_bytes(&mut buf);
1044                    let mut unstructured = Unstructured::new(&buf);
1045                    Self::arbitrary(&mut unstructured).unwrap_or_default()
1046                }
1047            }
1048
1049            #default_impl
1050
1051            impl MessageData for #msg_name {
1052                type Message = MavMessage;
1053
1054                const ID: u32 = #id;
1055                const NAME: &'static str = #name;
1056                const EXTRA_CRC: u8 = #extra_crc;
1057                const ENCODED_LEN: usize = #payload_encoded_len;
1058
1059                fn deser(_version: MavlinkVersion, __input: &[u8]) -> Result<Self, ::mavlink_core::error::ParserError> {
1060                    #deser_vars
1061                }
1062
1063                fn ser(&self, version: MavlinkVersion, bytes: &mut [u8]) -> usize {
1064                    #serialize_vars
1065                }
1066            }
1067        }
1068    }
1069
1070    /// Ensures that a message does not contain duplicate field names.
1071    ///
1072    /// Duplicate field names would generate invalid Rust structs.
1073    fn validate_unique_fields(&self) {
1074        let mut seen: HashSet<&str> = HashSet::new();
1075        for f in &self.fields {
1076            let name: &str = &f.name;
1077            assert!(
1078                seen.insert(name),
1079                "Duplicate field '{}' found in message '{}' while generating bindings",
1080                name,
1081                self.name
1082            );
1083        }
1084    }
1085
1086    /// Ensure that the fields count is at least one and no more than 64
1087    fn validate_field_count(&self) {
1088        assert!(
1089            !self.fields.is_empty(),
1090            "Message '{}' does not any fields",
1091            self.name
1092        );
1093        assert!(
1094            self.fields.len() <= 64,
1095            "Message '{}' has more then 64 fields",
1096            self.name
1097        );
1098    }
1099}
1100
1101#[derive(Debug, PartialEq, Clone, Default)]
1102#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
1103pub struct MavField {
1104    pub mavtype: MavType,
1105    pub name: String,
1106    pub description: Option<String>,
1107    pub enumtype: Option<String>,
1108    pub display: Option<String>,
1109    pub is_extension: bool,
1110}
1111
1112impl MavField {
1113    /// Emit rust name of a given field
1114    #[inline(always)]
1115    fn emit_name(&self) -> TokenStream {
1116        let name = format_ident!("{}", self.name);
1117        quote!(#name)
1118    }
1119
1120    /// Emit rust type of the field
1121    #[inline(always)]
1122    fn emit_type(&self) -> TokenStream {
1123        let mavtype;
1124        if matches!(self.mavtype, MavType::Array(_, _)) {
1125            let rt = TokenStream::from_str(&self.mavtype.rust_type()).unwrap();
1126            mavtype = quote!(#rt);
1127        } else if let Some(enumname) = &self.enumtype {
1128            let en = TokenStream::from_str(enumname).unwrap();
1129            mavtype = quote!(#en);
1130        } else {
1131            let rt = TokenStream::from_str(&self.mavtype.rust_type()).unwrap();
1132            mavtype = quote!(#rt);
1133        }
1134        mavtype
1135    }
1136
1137    /// Generate description for the given field
1138    #[inline(always)]
1139    fn emit_description(&self) -> TokenStream {
1140        let mut ts = TokenStream::new();
1141        if let Some(val) = self.description.as_ref() {
1142            let desc = URL_REGEX.replace_all(val, "<$1>");
1143            ts.extend(quote!(#[doc = #desc]));
1144        }
1145        ts
1146    }
1147
1148    /// Combine rust name and type of a given field
1149    #[inline(always)]
1150    fn emit_name_type(&self) -> TokenStream {
1151        let name = self.emit_name();
1152        let fieldtype = self.emit_type();
1153        quote!(pub #name: #fieldtype,)
1154    }
1155
1156    /// Emit writer
1157    fn rust_writer(&self) -> TokenStream {
1158        let mut name = "self.".to_string() + &self.name.clone();
1159        if self.enumtype.is_some() {
1160            // casts are not necessary for arrays, because they are currently
1161            // generated as primitive arrays
1162            if !matches!(self.mavtype, MavType::Array(_, _)) {
1163                if let Some(dsp) = &self.display {
1164                    // potentially a bitflag
1165                    if dsp == "bitmask" {
1166                        // it is a bitflag
1167                        name += ".bits() as ";
1168                        name += &self.mavtype.rust_type();
1169                    } else {
1170                        panic!("Display option not implemented");
1171                    }
1172                } else {
1173                    // an enum, have to use "*foo as u8" cast
1174                    name += " as ";
1175                    name += &self.mavtype.rust_type();
1176                }
1177            }
1178        }
1179        let ts = TokenStream::from_str(&name).unwrap();
1180        let name = quote!(#ts);
1181        let buf = format_ident!("__tmp");
1182        self.mavtype.rust_writer(&name, buf)
1183    }
1184
1185    /// Emit reader
1186    fn rust_reader(&self) -> TokenStream {
1187        let _name = TokenStream::from_str(&self.name).unwrap();
1188
1189        let name = quote!(__struct.#_name);
1190        let buf = format_ident!("buf");
1191        if let Some(enum_name) = &self.enumtype {
1192            // TODO: handle enum arrays properly, rather than just generating
1193            // primitive arrays
1194            if let MavType::Array(_t, _size) = &self.mavtype {
1195                return self.mavtype.rust_reader(&name, buf);
1196            }
1197            if let Some(dsp) = &self.display {
1198                if dsp == "bitmask" {
1199                    // bitflags
1200                    let tmp = self.mavtype.rust_reader(&quote!(let tmp), buf);
1201                    let enum_name_ident = format_ident!("{}", enum_name);
1202                    quote! {
1203                        #tmp
1204                        // Keep unknown bits for forward compatibility.
1205                        #name = #enum_name_ident::from_bits_retain(tmp as <#enum_name_ident as Flags>::Bits);
1206                    }
1207                } else {
1208                    panic!("Display option not implemented");
1209                }
1210            } else {
1211                // handle enum by FromPrimitive
1212                let tmp = self.mavtype.rust_reader(&quote!(let tmp), buf);
1213                let val = format_ident!("from_{}", &self.mavtype.rust_type());
1214                quote!(
1215                    #tmp
1216                    #name = FromPrimitive::#val(tmp)
1217                        .ok_or(::mavlink_core::error::ParserError::InvalidEnum { enum_type: #enum_name, value: tmp as u64 })?;
1218                )
1219            }
1220        } else {
1221            self.mavtype.rust_reader(&name, buf)
1222        }
1223    }
1224
1225    #[inline(always)]
1226    fn emit_default_initializer(&self, dialect_has_version: bool) -> TokenStream {
1227        let field = self.emit_name();
1228        // FIXME: Is this actually expected behaviour??
1229        if matches!(self.mavtype, MavType::Array(_, _)) {
1230            let default_value = self.mavtype.emit_default_value(dialect_has_version);
1231            quote!(#field: #default_value,)
1232        } else if let Some(enumname) = &self.enumtype {
1233            let ty = TokenStream::from_str(enumname).unwrap();
1234            quote!(#field: #ty::DEFAULT,)
1235        } else {
1236            let default_value = self.mavtype.emit_default_value(dialect_has_version);
1237            quote!(#field: #default_value,)
1238        }
1239    }
1240}
1241
1242#[derive(Debug, PartialEq, Clone, Default)]
1243#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
1244pub enum MavType {
1245    UInt8MavlinkVersion,
1246    #[default]
1247    UInt8,
1248    UInt16,
1249    UInt32,
1250    UInt64,
1251    Int8,
1252    Int16,
1253    Int32,
1254    Int64,
1255    Char,
1256    Float,
1257    Double,
1258    CharArray(usize),
1259    Array(Box<Self>, usize),
1260}
1261
1262impl MavType {
1263    fn parse_type(s: &str) -> Option<Self> {
1264        use self::MavType::*;
1265        match s {
1266            "uint8_t_mavlink_version" => Some(UInt8MavlinkVersion),
1267            "uint8_t" => Some(UInt8),
1268            "uint16_t" => Some(UInt16),
1269            "uint32_t" => Some(UInt32),
1270            "uint64_t" => Some(UInt64),
1271            "int8_t" => Some(Int8),
1272            "int16_t" => Some(Int16),
1273            "int32_t" => Some(Int32),
1274            "int64_t" => Some(Int64),
1275            "char" => Some(Char),
1276            "float" => Some(Float),
1277            "Double" => Some(Double),
1278            "double" => Some(Double),
1279            _ if s.starts_with("char[") => {
1280                let start = 4;
1281                let size = s[start + 1..(s.len() - 1)].parse::<usize>().ok()?;
1282                Some(CharArray(size))
1283            }
1284            _ if s.ends_with(']') => {
1285                let start = s.find('[')?;
1286                let size = s[start + 1..(s.len() - 1)].parse::<usize>().ok()?;
1287                let mtype = Self::parse_type(&s[0..start])?;
1288                Some(Array(Box::new(mtype), size))
1289            }
1290            _ => None,
1291        }
1292    }
1293
1294    /// Emit reader of a given type
1295    pub fn rust_reader(&self, val: &TokenStream, buf: Ident) -> TokenStream {
1296        use self::MavType::*;
1297        match self {
1298            Char => quote! {#val = #buf.get_u8()?;},
1299            UInt8 => quote! {#val = #buf.get_u8()?;},
1300            UInt16 => quote! {#val = #buf.get_u16_le()?;},
1301            UInt32 => quote! {#val = #buf.get_u32_le()?;},
1302            UInt64 => quote! {#val = #buf.get_u64_le()?;},
1303            UInt8MavlinkVersion => quote! {#val = #buf.get_u8()?;},
1304            Int8 => quote! {#val = #buf.get_i8()?;},
1305            Int16 => quote! {#val = #buf.get_i16_le()?;},
1306            Int32 => quote! {#val = #buf.get_i32_le()?;},
1307            Int64 => quote! {#val = #buf.get_i64_le()?;},
1308            Float => quote! {#val = #buf.get_f32_le()?;},
1309            Double => quote! {#val = #buf.get_f64_le()?;},
1310            CharArray(size) => {
1311                quote! {
1312                    let mut tmp = [0_u8; #size];
1313                    for v in &mut tmp {
1314                        *v = #buf.get_u8()?;
1315                    }
1316                    #val = CharArray::new(tmp);
1317                }
1318            }
1319            Array(t, _) => {
1320                let r = t.rust_reader(&quote!(let val), buf);
1321                quote! {
1322                    for v in &mut #val {
1323                        #r
1324                        *v = val;
1325                    }
1326                }
1327            }
1328        }
1329    }
1330
1331    /// Emit writer of a given type
1332    pub fn rust_writer(&self, val: &TokenStream, buf: Ident) -> TokenStream {
1333        use self::MavType::*;
1334        match self {
1335            UInt8MavlinkVersion => quote! {#buf.put_u8(#val);},
1336            UInt8 => quote! {#buf.put_u8(#val);},
1337            Char => quote! {#buf.put_u8(#val);},
1338            UInt16 => quote! {#buf.put_u16_le(#val);},
1339            UInt32 => quote! {#buf.put_u32_le(#val);},
1340            Int8 => quote! {#buf.put_i8(#val);},
1341            Int16 => quote! {#buf.put_i16_le(#val);},
1342            Int32 => quote! {#buf.put_i32_le(#val);},
1343            Float => quote! {#buf.put_f32_le(#val);},
1344            UInt64 => quote! {#buf.put_u64_le(#val);},
1345            Int64 => quote! {#buf.put_i64_le(#val);},
1346            Double => quote! {#buf.put_f64_le(#val);},
1347            CharArray(_) => {
1348                let w = Char.rust_writer(&quote!(*val), buf);
1349                quote! {
1350                    for val in &#val {
1351                        #w
1352                    }
1353                }
1354            }
1355            Array(t, _size) => {
1356                let w = t.rust_writer(&quote!(*val), buf);
1357                quote! {
1358                    for val in &#val {
1359                        #w
1360                    }
1361                }
1362            }
1363        }
1364    }
1365
1366    /// Size of a given Mavtype
1367    fn len(&self) -> usize {
1368        use self::MavType::*;
1369        match self {
1370            UInt8MavlinkVersion | UInt8 | Int8 | Char => 1,
1371            UInt16 | Int16 => 2,
1372            UInt32 | Int32 | Float => 4,
1373            UInt64 | Int64 | Double => 8,
1374            CharArray(size) => *size,
1375            Array(t, size) => t.len() * size,
1376        }
1377    }
1378
1379    fn max_int_value(&self) -> u64 {
1380        match self {
1381            Self::UInt8MavlinkVersion | Self::UInt8 => u8::MAX as u64,
1382            Self::UInt16 => u16::MAX as u64,
1383            Self::UInt32 => u32::MAX as u64,
1384            Self::UInt64 => u64::MAX,
1385            Self::Int8 | Self::Char | Self::CharArray(_) => i8::MAX as u64,
1386            Self::Int16 => i16::MAX as u64,
1387            Self::Int32 => i32::MAX as u64,
1388            Self::Int64 => i64::MAX as u64,
1389            // maximum precisly representable value minus 1 for float types
1390            Self::Float => (1 << f32::MANTISSA_DIGITS) - 1,
1391            Self::Double => (1 << f64::MANTISSA_DIGITS) - 1,
1392            Self::Array(mav_type, _) => mav_type.max_int_value(),
1393        }
1394    }
1395
1396    /// Used for ordering of types
1397    fn order_len(&self) -> usize {
1398        use self::MavType::*;
1399        match self {
1400            UInt8MavlinkVersion | UInt8 | Int8 | Char | CharArray(_) => 1,
1401            UInt16 | Int16 => 2,
1402            UInt32 | Int32 | Float => 4,
1403            UInt64 | Int64 | Double => 8,
1404            Array(t, _) => t.len(),
1405        }
1406    }
1407
1408    /// Used for crc calculation
1409    pub fn primitive_type(&self) -> String {
1410        use self::MavType::*;
1411        match self {
1412            UInt8MavlinkVersion => "uint8_t".into(),
1413            UInt8 => "uint8_t".into(),
1414            Int8 => "int8_t".into(),
1415            Char => "char".into(),
1416            UInt16 => "uint16_t".into(),
1417            Int16 => "int16_t".into(),
1418            UInt32 => "uint32_t".into(),
1419            Int32 => "int32_t".into(),
1420            Float => "float".into(),
1421            UInt64 => "uint64_t".into(),
1422            Int64 => "int64_t".into(),
1423            Double => "double".into(),
1424            CharArray(_) => "char".into(),
1425            Array(t, _) => t.primitive_type(),
1426        }
1427    }
1428
1429    /// Return rust equivalent of a given Mavtype
1430    /// Used for generating struct fields.
1431    pub fn rust_type(&self) -> String {
1432        use self::MavType::*;
1433        match self {
1434            UInt8 | UInt8MavlinkVersion => "u8".into(),
1435            Int8 => "i8".into(),
1436            Char => "u8".into(),
1437            UInt16 => "u16".into(),
1438            Int16 => "i16".into(),
1439            UInt32 => "u32".into(),
1440            Int32 => "i32".into(),
1441            Float => "f32".into(),
1442            UInt64 => "u64".into(),
1443            Int64 => "i64".into(),
1444            Double => "f64".into(),
1445            CharArray(size) => format!("CharArray<{size}>"),
1446            Array(t, size) => format!("[{};{}]", t.rust_type(), size),
1447        }
1448    }
1449
1450    pub fn emit_default_value(&self, dialect_has_version: bool) -> TokenStream {
1451        use self::MavType::*;
1452        match self {
1453            UInt8 => quote!(0_u8),
1454            UInt8MavlinkVersion => {
1455                if dialect_has_version {
1456                    quote!(MINOR_MAVLINK_VERSION)
1457                } else {
1458                    quote!(0_u8)
1459                }
1460            }
1461            Int8 => quote!(0_i8),
1462            Char => quote!(0_u8),
1463            UInt16 => quote!(0_u16),
1464            Int16 => quote!(0_i16),
1465            UInt32 => quote!(0_u32),
1466            Int32 => quote!(0_i32),
1467            Float => quote!(0.0_f32),
1468            UInt64 => quote!(0_u64),
1469            Int64 => quote!(0_i64),
1470            Double => quote!(0.0_f64),
1471            CharArray(size) => quote!(CharArray::new([0_u8; #size])),
1472            Array(ty, size) => {
1473                let default_value = ty.emit_default_value(dialect_has_version);
1474                quote!([#default_value; #size])
1475            }
1476        }
1477    }
1478
1479    /// Return rust equivalent of the primitive type of a MavType. The primitive
1480    /// type is the type itself for all except arrays, in which case it is the
1481    /// element type.
1482    pub fn rust_primitive_type(&self) -> String {
1483        use self::MavType::*;
1484        match self {
1485            Array(t, _) => t.rust_primitive_type(),
1486            _ => self.rust_type(),
1487        }
1488    }
1489
1490    /// Compare two MavTypes
1491    pub fn compare(&self, other: &Self) -> Ordering {
1492        let len = self.order_len();
1493        (-(len as isize)).cmp(&(-(other.order_len() as isize)))
1494    }
1495}
1496
1497#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
1498#[derive(Debug, PartialEq, Eq, Clone, Default)]
1499pub enum MavDeprecationType {
1500    #[default]
1501    Deprecated,
1502    Superseded,
1503}
1504
1505impl Display for MavDeprecationType {
1506    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
1507        match self {
1508            Self::Deprecated => f.write_str("Deprecated"),
1509            Self::Superseded => f.write_str("Superseded"),
1510        }
1511    }
1512}
1513
1514#[derive(Debug, PartialEq, Eq, Clone, Default)]
1515#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
1516pub struct MavDeprecation {
1517    // YYYY-MM
1518    pub since: String,
1519    pub replaced_by: Option<String>,
1520    pub deprecation_type: MavDeprecationType,
1521    pub note: Option<String>,
1522}
1523
1524impl MavDeprecation {
1525    pub fn emit_tokens(&self) -> TokenStream {
1526        let since = &self.since;
1527        let note = match &self.note {
1528            Some(str) if str.is_empty() || str.ends_with(".") => str.clone(),
1529            Some(str) => format!("{str}."),
1530            None => String::new(),
1531        };
1532        let replaced_by = match &self.replaced_by {
1533            Some(str) if str.starts_with('`') => format!("See {str}"),
1534            Some(str) => format!("See `{str}`"),
1535            None => String::new(),
1536        };
1537        let message = format!(
1538            "{note} {replaced_by} ({} since {since})",
1539            self.deprecation_type
1540        );
1541        quote!(#[deprecated = #message])
1542    }
1543}
1544
1545#[derive(Debug, PartialEq, Eq, Clone, Copy)]
1546#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
1547#[cfg_attr(feature = "serde", serde(tag = "type"))]
1548pub enum MavXmlElement {
1549    Version,
1550    Mavlink,
1551    Dialect,
1552    Include,
1553    Enums,
1554    Enum,
1555    Entry,
1556    Description,
1557    Param,
1558    Messages,
1559    Message,
1560    Field,
1561    Deprecated,
1562    Wip,
1563    Extensions,
1564    Superseded,
1565}
1566
1567const fn identify_element(s: &[u8]) -> Option<MavXmlElement> {
1568    use self::MavXmlElement::*;
1569    match s {
1570        b"version" => Some(Version),
1571        b"mavlink" => Some(Mavlink),
1572        b"dialect" => Some(Dialect),
1573        b"include" => Some(Include),
1574        b"enums" => Some(Enums),
1575        b"enum" => Some(Enum),
1576        b"entry" => Some(Entry),
1577        b"description" => Some(Description),
1578        b"param" => Some(Param),
1579        b"messages" => Some(Messages),
1580        b"message" => Some(Message),
1581        b"field" => Some(Field),
1582        b"deprecated" => Some(Deprecated),
1583        b"wip" => Some(Wip),
1584        b"extensions" => Some(Extensions),
1585        b"superseded" => Some(Superseded),
1586        _ => None,
1587    }
1588}
1589
1590fn is_valid_parent(p: Option<MavXmlElement>, s: MavXmlElement) -> bool {
1591    use self::MavXmlElement::*;
1592    match s {
1593        Version => p == Some(Mavlink),
1594        Mavlink => p.is_none(),
1595        Dialect => p == Some(Mavlink),
1596        Include => p == Some(Mavlink),
1597        Enums => p == Some(Mavlink),
1598        Enum => p == Some(Enums),
1599        Entry => p == Some(Enum),
1600        Description => p == Some(Entry) || p == Some(Message) || p == Some(Enum),
1601        Param => p == Some(Entry),
1602        Messages => p == Some(Mavlink),
1603        Message => p == Some(Messages),
1604        Field => p == Some(Message),
1605        Deprecated => p == Some(Entry) || p == Some(Message) || p == Some(Enum),
1606        Wip => p == Some(Entry) || p == Some(Message) || p == Some(Enum),
1607        Extensions => p == Some(Message),
1608        Superseded => p == Some(Entry) || p == Some(Message) || p == Some(Enum),
1609    }
1610}
1611
1612pub fn parse_profile(
1613    definitions_dir: &Path,
1614    definition_file: &Path,
1615    parsed_files: &mut HashSet<PathBuf>,
1616) -> Result<MavProfile, BindGenError> {
1617    let in_path = Path::new(&definitions_dir).join(definition_file);
1618    parsed_files.insert(in_path.clone()); // Keep track of which files have been parsed
1619
1620    let mut stack: Vec<MavXmlElement> = vec![];
1621
1622    let mut text = None;
1623
1624    let mut profile = MavProfile::default();
1625    let mut field = MavField::default();
1626    let mut message = MavMessage::default();
1627    let mut mavenum = MavEnum::default();
1628    let mut entry = MavEnumEntry::default();
1629    let mut param_index: Option<usize> = None;
1630    let mut param_label: Option<String> = None;
1631    let mut param_units: Option<String> = None;
1632    let mut param_enum: Option<String> = None;
1633    let mut param_increment: Option<f32> = None;
1634    let mut param_min_value: Option<f32> = None;
1635    let mut param_max_value: Option<f32> = None;
1636    let mut param_reserved = false;
1637    let mut param_default: Option<f32> = None;
1638    let mut deprecated: Option<MavDeprecation> = None;
1639
1640    let mut xml_filter = MavXmlFilter::default();
1641    let mut events: Vec<Result<Event, quick_xml::Error>> = Vec::new();
1642    let xml = std::fs::read_to_string(&in_path).map_err(|e| {
1643        BindGenError::CouldNotReadDefinitionFile {
1644            source: e,
1645            path: in_path.clone(),
1646        }
1647    })?;
1648    let mut reader = Reader::from_str(&xml);
1649    reader.config_mut().trim_text(true);
1650    reader.config_mut().expand_empty_elements = true;
1651
1652    loop {
1653        match reader.read_event() {
1654            Ok(Event::Eof) => {
1655                events.push(Ok(Event::Eof));
1656                break;
1657            }
1658            Ok(event) => events.push(Ok(event.into_owned())),
1659            Err(why) => events.push(Err(why)),
1660        }
1661    }
1662    xml_filter.filter(&mut events);
1663    let mut is_in_extension = false;
1664    for e in events {
1665        match e {
1666            Ok(Event::Start(bytes)) => {
1667                let Some(id) = identify_element(bytes.name().into_inner()) else {
1668                    panic!(
1669                        "unexpected element {:?}",
1670                        String::from_utf8_lossy(bytes.name().into_inner())
1671                    );
1672                };
1673
1674                assert!(
1675                    is_valid_parent(stack.last().copied(), id),
1676                    "not valid parent {:?} of {id:?}",
1677                    stack.last(),
1678                );
1679
1680                match id {
1681                    MavXmlElement::Extensions => {
1682                        is_in_extension = true;
1683                    }
1684                    MavXmlElement::Message => {
1685                        message = MavMessage::default();
1686                    }
1687                    MavXmlElement::Field => {
1688                        field = MavField {
1689                            is_extension: is_in_extension,
1690                            ..Default::default()
1691                        };
1692                    }
1693                    MavXmlElement::Enum => {
1694                        mavenum = MavEnum::default();
1695                    }
1696                    MavXmlElement::Entry => {
1697                        if mavenum.entries.is_empty() {
1698                            mavenum.deprecated = deprecated;
1699                        }
1700                        deprecated = None;
1701                        entry = MavEnumEntry::default();
1702                    }
1703                    MavXmlElement::Param => {
1704                        param_index = None;
1705                        param_increment = None;
1706                        param_min_value = None;
1707                        param_max_value = None;
1708                        param_reserved = false;
1709                        param_default = None;
1710                    }
1711                    MavXmlElement::Deprecated => {
1712                        deprecated = Some(MavDeprecation {
1713                            replaced_by: None,
1714                            since: String::new(),
1715                            deprecation_type: MavDeprecationType::Deprecated,
1716                            note: None,
1717                        });
1718                    }
1719                    MavXmlElement::Superseded => {
1720                        deprecated = Some(MavDeprecation {
1721                            replaced_by: Some(String::new()),
1722                            since: String::new(),
1723                            deprecation_type: MavDeprecationType::Superseded,
1724                            note: None,
1725                        });
1726                    }
1727                    _ => (),
1728                }
1729                stack.push(id);
1730
1731                for attr in bytes.attributes() {
1732                    let attr = attr.unwrap();
1733                    match stack.last() {
1734                        Some(&MavXmlElement::Enum) => {
1735                            if attr.key.into_inner() == b"name" {
1736                                mavenum.name = to_pascal_case(attr.value);
1737                                //mavenum.name = attr.value.clone();
1738                            } else if attr.key.into_inner() == b"bitmask" {
1739                                mavenum.bitmask = true;
1740                            }
1741                        }
1742                        Some(&MavXmlElement::Entry) => {
1743                            match attr.key.into_inner() {
1744                                b"name" => {
1745                                    entry.name = String::from_utf8_lossy(&attr.value).to_string();
1746                                }
1747                                b"value" => {
1748                                    let value = String::from_utf8_lossy(&attr.value);
1749                                    // Deal with hexadecimal numbers
1750                                    let (src, radix) = value
1751                                        .strip_prefix("0x")
1752                                        .map(|value| (value, 16))
1753                                        .unwrap_or((value.as_ref(), 10));
1754                                    entry.value = u64::from_str_radix(src, radix).ok();
1755                                }
1756                                _ => (),
1757                            }
1758                        }
1759                        Some(&MavXmlElement::Message) => {
1760                            match attr.key.into_inner() {
1761                                b"name" => {
1762                                    /*message.name = attr
1763                                    .value
1764                                    .clone()
1765                                    .split("_")
1766                                    .map(|x| x.to_lowercase())
1767                                    .map(|x| {
1768                                        let mut v: Vec<char> = x.chars().collect();
1769                                        v[0] = v[0].to_uppercase().nth(0).unwrap();
1770                                        v.into_iter().collect()
1771                                    })
1772                                    .collect::<Vec<String>>()
1773                                    .join("");
1774                                    */
1775                                    message.name = String::from_utf8_lossy(&attr.value).to_string();
1776                                }
1777                                b"id" => {
1778                                    message.id =
1779                                        String::from_utf8_lossy(&attr.value).parse().unwrap();
1780                                }
1781                                _ => (),
1782                            }
1783                        }
1784                        Some(&MavXmlElement::Field) => {
1785                            match attr.key.into_inner() {
1786                                b"name" => {
1787                                    let name = String::from_utf8_lossy(&attr.value);
1788                                    field.name = if name == "type" {
1789                                        "mavtype".to_string()
1790                                    } else {
1791                                        name.to_string()
1792                                    };
1793                                }
1794                                b"type" => {
1795                                    let r#type = String::from_utf8_lossy(&attr.value);
1796                                    field.mavtype = MavType::parse_type(&r#type).unwrap();
1797                                }
1798                                b"enum" => {
1799                                    field.enumtype = Some(to_pascal_case(&attr.value));
1800
1801                                    // Update field display if enum is a bitmask
1802                                    if let Some(e) =
1803                                        profile.enums.get(field.enumtype.as_ref().unwrap())
1804                                    {
1805                                        if e.bitmask {
1806                                            field.display = Some("bitmask".to_string());
1807                                        }
1808                                    }
1809                                }
1810                                b"display" => {
1811                                    field.display =
1812                                        Some(String::from_utf8_lossy(&attr.value).to_string());
1813                                }
1814                                _ => (),
1815                            }
1816                        }
1817                        Some(&MavXmlElement::Param) => {
1818                            if entry.params.is_none() {
1819                                entry.params = Some(vec![]);
1820                            }
1821                            match attr.key.into_inner() {
1822                                b"index" => {
1823                                    let value = String::from_utf8_lossy(&attr.value)
1824                                        .parse()
1825                                        .expect("failed to parse param index");
1826                                    assert!(
1827                                        (1..=7).contains(&value),
1828                                        "param index must be between 1 and 7"
1829                                    );
1830                                    param_index = Some(value);
1831                                }
1832                                b"label" => {
1833                                    param_label =
1834                                        std::str::from_utf8(&attr.value).ok().map(str::to_owned);
1835                                }
1836                                b"increment" => {
1837                                    param_increment = Some(
1838                                        String::from_utf8_lossy(&attr.value)
1839                                            .parse()
1840                                            .expect("failed to parse param increment"),
1841                                    );
1842                                }
1843                                b"minValue" => {
1844                                    param_min_value = Some(
1845                                        String::from_utf8_lossy(&attr.value)
1846                                            .parse()
1847                                            .expect("failed to parse param minValue"),
1848                                    );
1849                                }
1850                                b"maxValue" => {
1851                                    param_max_value = Some(
1852                                        String::from_utf8_lossy(&attr.value)
1853                                            .parse()
1854                                            .expect("failed to parse param maxValue"),
1855                                    );
1856                                }
1857                                b"units" => {
1858                                    param_units =
1859                                        std::str::from_utf8(&attr.value).ok().map(str::to_owned);
1860                                }
1861                                b"enum" => {
1862                                    param_enum =
1863                                        std::str::from_utf8(&attr.value).ok().map(to_pascal_case);
1864                                }
1865                                b"reserved" => {
1866                                    param_reserved = attr.value.as_ref() == b"true";
1867                                }
1868                                b"default" => {
1869                                    param_default = Some(
1870                                        String::from_utf8_lossy(&attr.value)
1871                                            .parse()
1872                                            .expect("failed to parse param maxValue"),
1873                                    );
1874                                }
1875                                _ => (),
1876                            }
1877                        }
1878                        Some(&MavXmlElement::Deprecated) => match attr.key.into_inner() {
1879                            b"since" => {
1880                                deprecated.as_mut().unwrap().since =
1881                                    String::from_utf8_lossy(&attr.value).to_string();
1882                            }
1883                            b"replaced_by" => {
1884                                let value = String::from_utf8_lossy(&attr.value);
1885                                deprecated.as_mut().unwrap().replaced_by = if value.is_empty() {
1886                                    None
1887                                } else {
1888                                    Some(value.to_string())
1889                                };
1890                            }
1891                            _ => (),
1892                        },
1893                        Some(&MavXmlElement::Superseded) => match attr.key.into_inner() {
1894                            b"since" => {
1895                                deprecated.as_mut().unwrap().since =
1896                                    String::from_utf8_lossy(&attr.value).to_string();
1897                            }
1898                            b"replaced_by" => {
1899                                deprecated.as_mut().unwrap().replaced_by =
1900                                    Some(String::from_utf8_lossy(&attr.value).to_string());
1901                            }
1902                            _ => (),
1903                        },
1904                        _ => (),
1905                    }
1906                }
1907            }
1908            Ok(Event::Text(bytes)) => {
1909                let s = String::from_utf8_lossy(&bytes);
1910
1911                use self::MavXmlElement::*;
1912                match (stack.last(), stack.get(stack.len() - 2)) {
1913                    (Some(&Description), Some(&Message))
1914                    | (Some(&Field), Some(&Message))
1915                    | (Some(&Description), Some(&Enum))
1916                    | (Some(&Description), Some(&Entry))
1917                    | (Some(&Include), Some(&Mavlink))
1918                    | (Some(&Version), Some(&Mavlink))
1919                    | (Some(&Dialect), Some(&Mavlink))
1920                    | (Some(&Param), Some(&Entry))
1921                    | (Some(Deprecated), _)
1922                    | (Some(Superseded), _) => {
1923                        text = Some(text.map(|t| t + s.as_ref()).unwrap_or(s.to_string()));
1924                    }
1925                    data => {
1926                        panic!("unexpected text data {data:?} reading {s:?}");
1927                    }
1928                }
1929            }
1930            Ok(Event::GeneralRef(bytes)) => {
1931                let entity = String::from_utf8_lossy(&bytes);
1932                text = Some(
1933                    text.map(|t| format!("{t}&{entity};"))
1934                        .unwrap_or(format!("&{entity};")),
1935                );
1936            }
1937            Ok(Event::End(_)) => {
1938                match stack.last() {
1939                    Some(&MavXmlElement::Field) => {
1940                        field.description = text.map(|t| t.replace('\n', " "));
1941                        message.fields.push(field.clone());
1942                    }
1943                    Some(&MavXmlElement::Entry) => {
1944                        entry.deprecated = deprecated;
1945                        deprecated = None;
1946                        mavenum.entries.push(entry.clone());
1947                    }
1948                    Some(&MavXmlElement::Message) => {
1949                        message.deprecated = deprecated;
1950
1951                        deprecated = None;
1952                        is_in_extension = false;
1953                        // Follow mavlink ordering specification: https://mavlink.io/en/guide/serialization.html#field_reordering
1954                        let mut not_extension_fields = message.fields.clone();
1955                        let mut extension_fields = message.fields.clone();
1956
1957                        not_extension_fields.retain(|field| !field.is_extension);
1958                        extension_fields.retain(|field| field.is_extension);
1959
1960                        // Only not mavlink 1 fields need to be sorted
1961                        not_extension_fields.sort_by(|a, b| a.mavtype.compare(&b.mavtype));
1962
1963                        // Update msg fields and add the new message
1964                        let mut msg = message.clone();
1965                        msg.fields.clear();
1966                        msg.fields.extend(not_extension_fields);
1967                        msg.fields.extend(extension_fields);
1968
1969                        // Validate there are no duplicate field names
1970                        msg.validate_unique_fields();
1971                        // Validate field count must be between 1 and 64
1972                        msg.validate_field_count();
1973
1974                        profile.add_message(&msg);
1975                    }
1976                    Some(&MavXmlElement::Enum) => {
1977                        profile.add_enum(&mavenum);
1978                    }
1979                    Some(&MavXmlElement::Include) => {
1980                        let include =
1981                            PathBuf::from(text.map(|t| t.replace('\n', "")).unwrap_or_default());
1982                        let include_file = Path::new(&definitions_dir).join(include.clone());
1983                        if !parsed_files.contains(&include_file) {
1984                            let included_profile =
1985                                parse_profile(definitions_dir, &include, parsed_files)?;
1986                            for message in included_profile.messages.values() {
1987                                profile.add_message(message);
1988                            }
1989                            for enm in included_profile.enums.values() {
1990                                profile.add_enum(enm);
1991                            }
1992                            if profile.version.is_none() {
1993                                profile.version = included_profile.version;
1994                            }
1995                        }
1996                    }
1997                    Some(&MavXmlElement::Description) => match stack.get(stack.len() - 2) {
1998                        Some(&MavXmlElement::Message) => {
1999                            message.description = text.map(|t| t.replace('\n', " "));
2000                        }
2001                        Some(&MavXmlElement::Enum) => {
2002                            mavenum.description = text.map(|t| t.replace('\n', " "));
2003                        }
2004                        Some(&MavXmlElement::Entry) => {
2005                            entry.description = text.map(|t| t.replace('\n', " "));
2006                        }
2007                        _ => (),
2008                    },
2009                    Some(&MavXmlElement::Version) => {
2010                        if let Some(t) = text {
2011                            profile.version =
2012                                Some(t.parse().expect("Invalid minor version number format"));
2013                        }
2014                    }
2015                    Some(&MavXmlElement::Dialect) => {
2016                        if let Some(t) = text {
2017                            profile.dialect =
2018                                Some(t.parse().expect("Invalid dialect number format"));
2019                        }
2020                    }
2021                    Some(&MavXmlElement::Deprecated) | Some(&MavXmlElement::Superseded) => {
2022                        if let Some(t) = text {
2023                            deprecated.as_mut().unwrap().note = Some(t);
2024                        }
2025                    }
2026                    Some(&MavXmlElement::Param) => {
2027                        if let Some(params) = entry.params.as_mut() {
2028                            // Some messages can jump between values, like: 1, 2, 7
2029                            let param_index = param_index.expect("entry params must have an index");
2030                            while params.len() < param_index {
2031                                params.push(MavParam {
2032                                    index: params.len() + 1,
2033                                    description: None,
2034                                    ..Default::default()
2035                                });
2036                            }
2037                            if let Some((min, max)) = param_min_value.zip(param_max_value) {
2038                                assert!(
2039                                    min <= max,
2040                                    "param minValue must not be greater than maxValue"
2041                                );
2042                            }
2043                            params[param_index - 1] = MavParam {
2044                                index: param_index,
2045                                description: text.map(|t| t.replace('\n', " ")),
2046                                label: param_label,
2047                                units: param_units,
2048                                enum_used: param_enum,
2049                                increment: param_increment,
2050                                max_value: param_max_value,
2051                                min_value: param_min_value,
2052                                reserved: param_reserved,
2053                                default: param_default,
2054                            };
2055                            param_label = None;
2056                            param_units = None;
2057                            param_enum = None;
2058                        }
2059                    }
2060                    _ => (),
2061                }
2062                text = None;
2063                stack.pop();
2064                // println!("{}-{}", indent(depth), name);
2065            }
2066            Err(e) => {
2067                eprintln!("Error: {e}");
2068                break;
2069            }
2070            _ => {}
2071        }
2072    }
2073
2074    Ok(profile.update_enums())
2075}
2076
2077/// Generate protobuf represenation of mavlink message set
2078/// Generate rust representation of mavlink message set with appropriate conversion methods
2079pub fn generate<W: Write>(
2080    definitions_dir: &Path,
2081    definition_file: &Path,
2082    output_rust: &mut W,
2083) -> Result<(), BindGenError> {
2084    let mut parsed_files: HashSet<PathBuf> = HashSet::new();
2085    let profile = parse_profile(definitions_dir, definition_file, &mut parsed_files)?;
2086
2087    let dialect_name = util::to_dialect_name(definition_file);
2088
2089    // rust file
2090    let rust_tokens = profile.emit_rust(&dialect_name);
2091    writeln!(output_rust, "{rust_tokens}").unwrap();
2092
2093    Ok(())
2094}
2095
2096/// CRC operates over names of the message and names of its fields
2097/// Hence we have to preserve the original uppercase names delimited with an underscore
2098/// For field names, we replace "type" with "mavtype" to make it rust compatible (this is
2099/// needed for generating sensible rust code), but for calculating crc function we have to
2100/// use the original name "type"
2101pub fn extra_crc(msg: &MavMessage) -> u8 {
2102    // calculate a 8-bit checksum of the key fields of a message, so we
2103    // can detect incompatible XML changes
2104    let mut crc = CRCu16::crc16mcrf4cc();
2105
2106    crc.digest(msg.name.as_bytes());
2107    crc.digest(b" ");
2108
2109    let mut f = msg.fields.clone();
2110    // only mavlink 1 fields should be part of the extra_crc
2111    f.retain(|f| !f.is_extension);
2112    f.sort_by(|a, b| a.mavtype.compare(&b.mavtype));
2113    for field in &f {
2114        crc.digest(field.mavtype.primitive_type().as_bytes());
2115        crc.digest(b" ");
2116        if field.name == "mavtype" {
2117            crc.digest(b"type");
2118        } else {
2119            crc.digest(field.name.as_bytes());
2120        }
2121        crc.digest(b" ");
2122        if let MavType::Array(_, size) | MavType::CharArray(size) = field.mavtype {
2123            crc.digest(&[size as u8]);
2124        }
2125    }
2126
2127    let crcval = crc.get_crc();
2128    ((crcval & 0xFF) ^ (crcval >> 8)) as u8
2129}
2130
2131#[cfg(not(feature = "mav2-message-extensions"))]
2132struct ExtensionFilter {
2133    pub is_in: bool,
2134}
2135
2136struct MessageFilter {
2137    pub is_in: bool,
2138    pub messages: Vec<String>,
2139}
2140
2141impl MessageFilter {
2142    pub fn new() -> Self {
2143        Self {
2144            is_in: false,
2145            messages: vec![],
2146        }
2147    }
2148}
2149
2150struct MavXmlFilter {
2151    #[cfg(not(feature = "mav2-message-extensions"))]
2152    extension_filter: ExtensionFilter,
2153    message_filter: MessageFilter,
2154}
2155
2156impl Default for MavXmlFilter {
2157    fn default() -> Self {
2158        Self {
2159            #[cfg(not(feature = "mav2-message-extensions"))]
2160            extension_filter: ExtensionFilter { is_in: false },
2161            message_filter: MessageFilter::new(),
2162        }
2163    }
2164}
2165
2166impl MavXmlFilter {
2167    pub fn filter(&mut self, elements: &mut Vec<Result<Event, quick_xml::Error>>) {
2168        elements.retain(|x| self.filter_extension(x) && self.filter_messages(x));
2169    }
2170
2171    #[cfg(feature = "mav2-message-extensions")]
2172    pub fn filter_extension(&mut self, _element: &Result<Event, quick_xml::Error>) -> bool {
2173        true
2174    }
2175
2176    /// Ignore extension fields
2177    #[cfg(not(feature = "mav2-message-extensions"))]
2178    pub fn filter_extension(&mut self, element: &Result<Event, quick_xml::Error>) -> bool {
2179        match element {
2180            Ok(content) => {
2181                match content {
2182                    Event::Start(bytes) | Event::Empty(bytes) => {
2183                        let Some(id) = identify_element(bytes.name().into_inner()) else {
2184                            panic!(
2185                                "unexpected element {:?}",
2186                                String::from_utf8_lossy(bytes.name().into_inner())
2187                            );
2188                        };
2189                        if id == MavXmlElement::Extensions {
2190                            self.extension_filter.is_in = true;
2191                        }
2192                    }
2193                    Event::End(bytes) => {
2194                        let Some(id) = identify_element(bytes.name().into_inner()) else {
2195                            panic!(
2196                                "unexpected element {:?}",
2197                                String::from_utf8_lossy(bytes.name().into_inner())
2198                            );
2199                        };
2200
2201                        if id == MavXmlElement::Message {
2202                            self.extension_filter.is_in = false;
2203                        }
2204                    }
2205                    _ => {}
2206                }
2207                !self.extension_filter.is_in
2208            }
2209            Err(error) => panic!("Failed to filter XML: {error}"),
2210        }
2211    }
2212
2213    /// Filters messages by their name
2214    pub fn filter_messages(&mut self, element: &Result<Event, quick_xml::Error>) -> bool {
2215        match element {
2216            Ok(content) => {
2217                match content {
2218                    Event::Start(bytes) | Event::Empty(bytes) => {
2219                        let Some(id) = identify_element(bytes.name().into_inner()) else {
2220                            panic!(
2221                                "unexpected element {:?}",
2222                                String::from_utf8_lossy(bytes.name().into_inner())
2223                            );
2224                        };
2225                        if id == MavXmlElement::Message {
2226                            for attr in bytes.attributes() {
2227                                let attr = attr.unwrap();
2228                                if attr.key.into_inner() == b"name" {
2229                                    let value = String::from_utf8_lossy(&attr.value).into_owned();
2230                                    if self.message_filter.messages.contains(&value) {
2231                                        self.message_filter.is_in = true;
2232                                        return false;
2233                                    }
2234                                }
2235                            }
2236                        }
2237                    }
2238                    Event::End(bytes) => {
2239                        let Some(id) = identify_element(bytes.name().into_inner()) else {
2240                            panic!(
2241                                "unexpected element {:?}",
2242                                String::from_utf8_lossy(bytes.name().into_inner())
2243                            );
2244                        };
2245
2246                        if id == MavXmlElement::Message && self.message_filter.is_in {
2247                            self.message_filter.is_in = false;
2248                            return false;
2249                        }
2250                    }
2251                    _ => {}
2252                }
2253                !self.message_filter.is_in
2254            }
2255            Err(error) => panic!("Failed to filter XML: {error}"),
2256        }
2257    }
2258}
2259
2260#[inline(always)]
2261fn to_pascal_case(text: impl AsRef<[u8]>) -> String {
2262    let input = text.as_ref();
2263    let mut result = String::with_capacity(input.len());
2264    let mut capitalize = true;
2265
2266    for &b in input {
2267        if b == b'_' {
2268            capitalize = true;
2269            continue;
2270        }
2271
2272        if capitalize {
2273            result.push((b as char).to_ascii_uppercase());
2274            capitalize = false;
2275        } else {
2276            result.push((b as char).to_ascii_lowercase());
2277        }
2278    }
2279
2280    result
2281}
2282
2283#[cfg(test)]
2284mod tests {
2285    use super::*;
2286
2287    #[test]
2288    fn emits_target_id_match_arms() {
2289        // Build a minimal profile containing one message with target fields and one without
2290        let mut profile = MavProfile::default();
2291
2292        let msg_with_targets = MavMessage {
2293            id: 300,
2294            name: "COMMAND_INT".to_string(),
2295            description: None,
2296            fields: vec![
2297                MavField {
2298                    mavtype: MavType::UInt8,
2299                    name: "target_system".to_string(),
2300                    description: None,
2301                    enumtype: None,
2302                    display: None,
2303                    is_extension: false,
2304                },
2305                MavField {
2306                    mavtype: MavType::UInt8,
2307                    name: "target_component".to_string(),
2308                    description: None,
2309                    enumtype: None,
2310                    display: None,
2311                    is_extension: false,
2312                },
2313            ],
2314            deprecated: None,
2315        };
2316
2317        let msg_without_targets = MavMessage {
2318            id: 0,
2319            name: "HEARTBEAT".to_string(),
2320            description: None,
2321            fields: vec![MavField {
2322                mavtype: MavType::UInt32,
2323                name: "custom_mode".to_string(),
2324                description: None,
2325                enumtype: None,
2326                display: None,
2327                is_extension: false,
2328            }],
2329            deprecated: None,
2330        };
2331
2332        profile.add_message(&msg_with_targets);
2333        profile.add_message(&msg_without_targets);
2334
2335        let tokens = profile.emit_rust("common");
2336        let mut code = tokens.to_string();
2337        code.retain(|c| !c.is_whitespace());
2338
2339        // Check the code contains the target_system/component_id functions
2340        assert!(code.contains("fntarget_system_id(&self)->Option<u8>"));
2341        assert!(code.contains("fntarget_component_id(&self)->Option<u8>"));
2342
2343        // Check the generated impl contains arms referencing COMMAND_INT(inner).target_system/component
2344        assert!(code.contains("Self::COMMAND_INT(inner)=>Some(inner.target_system)"));
2345        assert!(code.contains("Self::COMMAND_INT(inner)=>Some(inner.target_component)"));
2346
2347        // Ensure a message without target fields returns None
2348        assert!(!code.contains("Self::HEARTBEAT(inner)=>Some(inner.target_system)"));
2349        assert!(!code.contains("Self::HEARTBEAT(inner)=>Some(inner.target_component)"));
2350    }
2351
2352    #[test]
2353    fn validate_unique_fields_allows_unique() {
2354        let msg = MavMessage {
2355            id: 1,
2356            name: "FOO".to_string(),
2357            description: None,
2358            fields: vec![
2359                MavField {
2360                    mavtype: MavType::UInt8,
2361                    name: "a".to_string(),
2362                    description: None,
2363                    enumtype: None,
2364                    display: None,
2365                    is_extension: false,
2366                },
2367                MavField {
2368                    mavtype: MavType::UInt16,
2369                    name: "b".to_string(),
2370                    description: None,
2371                    enumtype: None,
2372                    display: None,
2373                    is_extension: false,
2374                },
2375            ],
2376            deprecated: None,
2377        };
2378        // Should not panic
2379        msg.validate_unique_fields();
2380    }
2381
2382    #[test]
2383    #[should_panic(expected = "Duplicate field")]
2384    fn validate_unique_fields_panics_on_duplicate() {
2385        let msg = MavMessage {
2386            id: 2,
2387            name: "BAR".to_string(),
2388            description: None,
2389            fields: vec![
2390                MavField {
2391                    mavtype: MavType::UInt8,
2392                    name: "target_system".to_string(),
2393                    description: None,
2394                    enumtype: None,
2395                    display: None,
2396                    is_extension: false,
2397                },
2398                MavField {
2399                    mavtype: MavType::UInt8,
2400                    name: "target_system".to_string(),
2401                    description: None,
2402                    enumtype: None,
2403                    display: None,
2404                    is_extension: false,
2405                },
2406            ],
2407            deprecated: None,
2408        };
2409        // Should panic due to duplicate field names
2410        msg.validate_unique_fields();
2411    }
2412
2413    #[test]
2414    fn validate_field_count_ok() {
2415        let msg = MavMessage {
2416            id: 2,
2417            name: "FOO".to_string(),
2418            description: None,
2419            fields: vec![
2420                MavField {
2421                    mavtype: MavType::UInt8,
2422                    name: "a".to_string(),
2423                    description: None,
2424                    enumtype: None,
2425                    display: None,
2426                    is_extension: false,
2427                },
2428                MavField {
2429                    mavtype: MavType::UInt8,
2430                    name: "b".to_string(),
2431                    description: None,
2432                    enumtype: None,
2433                    display: None,
2434                    is_extension: false,
2435                },
2436            ],
2437            deprecated: None,
2438        };
2439        // Should not panic
2440        msg.validate_field_count();
2441    }
2442
2443    #[test]
2444    #[should_panic]
2445    fn validate_field_count_too_many() {
2446        let mut fields = vec![];
2447        for i in 0..65 {
2448            let field = MavField {
2449                mavtype: MavType::UInt8,
2450                name: format!("field_{i}"),
2451                description: None,
2452                enumtype: None,
2453                display: None,
2454                is_extension: false,
2455            };
2456            fields.push(field);
2457        }
2458        let msg = MavMessage {
2459            id: 2,
2460            name: "BAZ".to_string(),
2461            description: None,
2462            fields,
2463            deprecated: None,
2464        };
2465        // Should panic due to 65 fields
2466        msg.validate_field_count();
2467    }
2468
2469    #[test]
2470    #[should_panic]
2471    fn validate_field_count_empty() {
2472        let msg = MavMessage {
2473            id: 2,
2474            name: "BAM".to_string(),
2475            description: None,
2476            fields: vec![],
2477            deprecated: None,
2478        };
2479        // Should panic due to no fields
2480        msg.validate_field_count();
2481    }
2482
2483    #[test]
2484    fn test_fmt_mav_param_values() {
2485        let enum_param = MavParam {
2486            enum_used: Some("ENUM_NAME".to_string()),
2487            ..Default::default()
2488        };
2489        assert_eq!(enum_param.format_valid_values(), "[`ENUM_NAME`]");
2490
2491        let reserved_param = MavParam {
2492            reserved: true,
2493            default: Some(f32::NAN),
2494            ..Default::default()
2495        };
2496        assert_eq!(reserved_param.format_valid_values(), "Reserved (use NaN)");
2497
2498        let unrestricted_param = MavParam::default();
2499        assert_eq!(unrestricted_param.format_valid_values(), "");
2500
2501        let int_param = MavParam {
2502            increment: Some(1.0),
2503            ..Default::default()
2504        };
2505        assert_eq!(int_param.format_valid_values(), "Multiples of 1");
2506
2507        let pos_param = MavParam {
2508            min_value: Some(0.0),
2509            ..Default::default()
2510        };
2511        assert_eq!(pos_param.format_valid_values(), "&ge; 0");
2512
2513        let max_param = MavParam {
2514            max_value: Some(5.5),
2515            ..Default::default()
2516        };
2517        assert_eq!(max_param.format_valid_values(), "&le; 5.5");
2518
2519        let pos_int_param = MavParam {
2520            min_value: Some(0.0),
2521            increment: Some(1.0),
2522            ..Default::default()
2523        };
2524        assert_eq!(pos_int_param.format_valid_values(), "0, 1, ..");
2525
2526        let max_inc_param = MavParam {
2527            increment: Some(1.0),
2528            max_value: Some(360.0),
2529            ..Default::default()
2530        };
2531        assert_eq!(max_inc_param.format_valid_values(), ".., 359, 360");
2532
2533        let range_param = MavParam {
2534            min_value: Some(0.0),
2535            max_value: Some(10.0),
2536            ..Default::default()
2537        };
2538        assert_eq!(range_param.format_valid_values(), "0 .. 10");
2539
2540        let int_range_param = MavParam {
2541            min_value: Some(0.0),
2542            max_value: Some(10.0),
2543            increment: Some(1.0),
2544            ..Default::default()
2545        };
2546        assert_eq!(int_range_param.format_valid_values(), "0, 1, .. , 10");
2547
2548        let close_inc_range_param = MavParam {
2549            min_value: Some(-2.0),
2550            max_value: Some(2.0),
2551            increment: Some(2.0),
2552            ..Default::default()
2553        };
2554        assert_eq!(close_inc_range_param.format_valid_values(), "-2, 0, 2");
2555
2556        let bin_range_param = MavParam {
2557            min_value: Some(0.0),
2558            max_value: Some(1.0),
2559            increment: Some(1.0),
2560            ..Default::default()
2561        };
2562        assert_eq!(bin_range_param.format_valid_values(), "0, 1");
2563    }
2564
2565    #[test]
2566    fn test_emit_doc_row() {
2567        let param = MavParam {
2568            index: 3,
2569            label: Some("test param".to_string()),
2570            min_value: Some(0.0),
2571            units: Some("m/s".to_string()),
2572            ..Default::default()
2573        };
2574        // test with all variations of columns
2575        assert_eq!(
2576            param.emit_doc_row(false, false).to_string(),
2577            quote! {#[doc = "| 3 (test param)|             |"]}.to_string()
2578        );
2579        assert_eq!(
2580            param.emit_doc_row(false, true).to_string(),
2581            quote! {#[doc = "| 3 (test param)|             | m/s |"]}.to_string()
2582        );
2583        assert_eq!(
2584            param.emit_doc_row(true, false).to_string(),
2585            quote! {#[doc = "| 3 (test param)|             | &ge; 0 |"]}.to_string()
2586        );
2587        assert_eq!(
2588            param.emit_doc_row(true, true).to_string(),
2589            quote! {#[doc = "| 3 (test param)|             | &ge; 0 | m/s |"]}.to_string()
2590        );
2591
2592        let unlabeled_param = MavParam {
2593            index: 2,
2594            ..Default::default()
2595        };
2596        assert_eq!(
2597            unlabeled_param.emit_doc_row(false, false).to_string(),
2598            quote! {#[doc = "| 2         |             |"]}.to_string()
2599        );
2600    }
2601}