nt_client 0.5.1

//! Google's `protobuf` support for `NetworkTables`.
//!
//! Note that because the library WPILibJ uses for Protocol Buffers, the packed bytes generated by
//! WPILibJ may not exactly match with the packed bytes generated by this crate. Parsing/unpacking
//! is still compatible both ways.

use protobuf::{Message, MessageFull, descriptor::FileDescriptorProto, reflect::MessageDescriptor};

use crate::{data::{DataType, NetworkTableData}};

include!(concat!(env!("OUT_DIR"), "/protobuf/mod.rs"));

/// Represents `protobuf` data that can be sent and received by a `NetworkTables` server.
#[derive(Debug, Clone, PartialEq, Eq)]
pub struct Protobuf<T>(pub T);

impl<T: ProtobufData> NetworkTableData for Protobuf<T> {
    fn data_type() -> DataType {
        DataType::Protobuf(T::proto_type_name())
    }

    fn from_value(value: rmpv::Value) -> Option<Self> {
        match value {
            rmpv::Value::Binary(bytes) => T::Proto::parse_from_bytes(&bytes).ok()
                .and_then(T::from_proto)
                .map(Self),
            _ => None,
        }
    }

    fn into_value(self) -> rmpv::Value {
        rmpv::Value::Binary(self.0.into_proto().write_to_bytes().expect("using protobuf 3"))
    }
}

/// Data that can be converted to and from generated `protobuf` messages.
pub trait ProtobufData {
    /// The generated `protobuf` type.
    type Proto: MessageFull;

    /// Converts self into data that can be sent and received by a `NetworkTables` server.
    fn into_protobuf_data(self) -> Protobuf<Self> where Self: Sized {
        Protobuf(self)
    }

    /// Returns the `protobuf` typename.
    ///
    /// This name will match the actual type name in WPILib.
    fn proto_type_name() -> String {
        Self::Proto::descriptor().full_name().to_owned()
    }

    /// This protobuf message's descriptor.
    fn message_descriptor() -> MessageDescriptor {
        MessageDescriptor::for_type::<Self::Proto>()
    }

    /// Converts from a `protobuf` message.
    fn from_proto(proto: Self::Proto) -> Option<Self> where Self: Sized;

    /// Converts into a `protobuf` message.
    fn into_proto(self) -> Self::Proto;
}

impl NetworkTableData for FileDescriptorProto {
    fn data_type() -> DataType {
        DataType::Protobuf(FileDescriptorProto::descriptor().name().to_owned())
    }

    fn from_value(value: rmpv::Value) -> Option<Self> {
        match value {
            rmpv::Value::Binary(bytes) => FileDescriptorProto::parse_from_bytes(&bytes).ok(),
            _ => None,
        }
    }

    fn into_value(self) -> rmpv::Value {
        rmpv::Value::Binary(self.write_to_bytes().expect("using protobuf 3"))
    }
}

#[cfg(test)]
mod tests {
    #[cfg(feature = "math")]
    use crate::math::*;

    use protobuf::Message;
    use tokio_test::assert_ok;

    use super::*;

    #[test]
    #[cfg(feature = "math")]
    fn test_armfeedforward() {
        let bytes = [
            0x09, 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0x02,
            0x40, 0x11, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
            0x59, 0x40, 0x19, 0x29, 0x5C, 0x8F, 0xC2, 0xF5,
            0xA8, 0x20, 0x40, 0x21, 0xCD, 0xCC, 0xCC, 0xCC,
            0xCC, 0xCC, 0x14, 0x40, 0x29, 0x9A, 0x99, 0x99,
            0x99, 0x99, 0x99, 0xF1, 0x3F,
        ];

        let data = ArmFeedforward {
            k_a: 5.2,
            k_s: 2.3,
            k_g: 100.0,
            d_t: 1.1,
            k_v: 8.33,
        };
        assert_eq!(ArmFeedforward::proto_type_name(), "wpi.proto.ProtobufArmFeedforward");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_chassisspeeds() {
        let bytes = [
            0x09, 0xCD, 0xCC, 0xCC, 0xCC, 0xCC, 0x4C, 0x34,
            0x40, 0x11, 0x9A, 0x99, 0x99, 0x99, 0x99, 0x99,
            0x20, 0x40, 0x19, 0xB8, 0x1E, 0x85, 0xEB, 0x51,
            0x38, 0x26, 0x40,
        ];

        let data = ChassisSpeeds {
            velocity_y: 8.3,
            velocity_x: 20.3,
            omega: 11.11,
        };
        assert_eq!(ChassisSpeeds::proto_type_name(), "wpi.proto.ProtobufChassisSpeeds");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_cubichermitespline() {
        let bytes = [
            0x09, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x08,
            0x40, 0x09, 0xB8, 0x1E, 0x85, 0xEB, 0x51, 0x38,
            0x22, 0x40, 0x11, 0x00, 0x00, 0x00, 0x00, 0x00,
            0x80, 0x40, 0xC0, 0x11, 0x33, 0x33, 0x33, 0x33,
            0x33, 0x43, 0x87, 0x40, 0x19, 0x9A, 0x99, 0x99,
            0x99, 0x99, 0x99, 0x01, 0x40, 0x19, 0xAE, 0x47,
            0xE1, 0x7A, 0x14, 0xAE, 0xFF, 0xBF, 0x21, 0xCD,
            0xCC, 0xCC, 0xCC, 0xCC, 0x0C, 0x56, 0x40, 0x21,
            0x5C, 0x8F, 0xC2, 0xF5, 0x28, 0x9C, 0x46, 0x40,
        ];

        let data = CubicHermiteSpline {
            x_initial_control_vector: [3.0, 9.11],
            y_initial_control_vector: [2.2, -1.98],
            x_final_control_vector: [-33.0, 744.4],
            y_final_control_vector: [88.2, 45.22],
        };
        assert_eq!(CubicHermiteSpline::proto_type_name(), "wpi.proto.ProtobufCubicHermiteSpline");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_dcmotor() {
        let bytes = [
            0x09, 0x9A, 0x99, 0x99, 0x99, 0x99, 0x99, 0x28,
            0x40, 0x11, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
            0xF0, 0x3F, 0x19, 0x9A, 0x99, 0x99, 0x99, 0x99,
            0x99, 0x01, 0x40, 0x21, 0xD7, 0xA3, 0x70, 0x3D,
            0x0A, 0xD7, 0x1D, 0x40, 0x29, 0x00, 0x00, 0x00,
            0x00, 0x00, 0xD0, 0x74, 0x40,
        ];

        let data = DCMotor {
            free_speed: 333.0,
            stall_torque: 1.0,
            stall_current: 2.2,
            nominal_voltage: 12.3,
            free_current: 7.46,
        };
        assert_eq!(DCMotor::proto_type_name(), "wpi.proto.ProtobufDCMotor");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_differentialdrivefeedforward() {
        let bytes = [
            0x09, 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0x24,
            0x40, 0x11, 0x71, 0x3D, 0x0A, 0xD7, 0xA3, 0x70,
            0x32, 0x40, 0x19, 0xCD, 0xCC, 0xCC, 0xCC, 0xCC,
            0xCC, 0x1C, 0x40, 0x21, 0x00, 0x00, 0x00, 0x00,
            0x00, 0x00, 0x10, 0x40,
        ];

        let data = DifferentialDriveFeedforward {
            velocity_linear: 10.2,
            acceleration_linear: 18.44,
            acceleration_angular: 4.0,
            velocity_angular: 7.2,
        };
        assert_eq!(DifferentialDriveFeedforward::proto_type_name(), "wpi.proto.ProtobufDifferentialDriveFeedforward");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_differentialdrivekinematics() {
        let bytes = [
            0x09, 0x9A, 0x99, 0x99, 0x99, 0x99, 0x99, 0x09,
            0x40,
        ];

        let data = DifferentialDriveKinematics {
            track_width: 3.2,
        };
        assert_eq!(DifferentialDriveKinematics::proto_type_name(), "wpi.proto.ProtobufDifferentialDriveKinematics");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_differentialdrivewheelpositions() {
        let bytes = [
            0x09, 0xCD, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0x14,
            0x40, 0x11, 0x71, 0x3D, 0x0A, 0xD7, 0xA3, 0x70,
            0x14, 0x40,
        ];

        let data = DifferentialDriveWheelPositions {
            left: 5.2,
            right: 5.11,
        };
        assert_eq!(DifferentialDriveWheelPositions::proto_type_name(), "wpi.proto.ProtobufDifferentialDriveWheelPositions");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_differentialdrivewheelspeeds() {
        let bytes = [
            0x09, 0x9A, 0x99, 0x99, 0x99, 0x99, 0x59, 0x8F,
            0x40, 0x11, 0x00, 0x00, 0x00, 0x00, 0x00, 0x50,
            0x7A, 0xC0,
        ];

        let data = DifferentialDriveWheelSpeeds {
            left: 1003.2,
            right: -421.0,
        };
        assert_eq!(DifferentialDriveWheelSpeeds::proto_type_name(), "wpi.proto.ProtobufDifferentialDriveWheelSpeeds");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_differentialdrivewheelvoltages() {
        let bytes = [
            0x09, 0x9A, 0x99, 0x99, 0x99, 0x99, 0x99, 0x27,
            0x40, 0x11, 0xCD, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC,
            0x1C, 0xC0,
        ];

        let data = DifferentialDriveWheelVoltages {
            left: 11.8,
            right: -7.2,
        };
        assert_eq!(DifferentialDriveWheelVoltages::proto_type_name(), "wpi.proto.ProtobufDifferentialDriveWheelVoltages");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_elevatorfeedforward() {
        let bytes = [
            0x09, 0xCD, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0x1C,
            0x40, 0x11, 0xCD, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC,
            0xF4, 0x3F, 0x19, 0x52, 0xB8, 0x1E, 0x85, 0xEB,
            0x51, 0x19, 0x40, 0x21, 0x66, 0x66, 0x66, 0x66,
            0x66, 0x66, 0x1C, 0x40, 0x29, 0x9A, 0x99, 0x99,
            0x99, 0x99, 0x99, 0xC9, 0x3F,
        ];

        let data = ElevatorFeedforward {
            k_a: 7.1,
            k_s: 7.2,
            k_g: 1.3,
            d_t: 0.2,
            k_v: 6.33,
        };
        assert_eq!(ElevatorFeedforward::proto_type_name(), "wpi.proto.ProtobufElevatorFeedforward");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_ellipse2d() {
        let bytes = [
            0x11, 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0x20,
            0x40, 0x19, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33,
            0x0B, 0x40, 0x0A, 0x1F, 0x0A, 0x12, 0x09, 0x9A,
            0x99, 0x99, 0x99, 0x99, 0x99, 0x20, 0x40, 0x11,
            0x9A, 0x99, 0x99, 0x99, 0x99, 0x99, 0x01, 0x40,
            0x12, 0x09, 0x09, 0x66, 0x66, 0x66, 0x66, 0x66,
            0x86, 0x54, 0x40,
        ];

        let data = Ellipse2d {
            x_semi_axis: 8.2,
            y_semi_axis: 3.4,
            center: Pose2d { translation: Translation2d { x: 8.3, y: 2.2 }, rotation: Rotation2d { value: 82.1 } },
        };
        assert_eq!(Ellipse2d::proto_type_name(), "wpi.proto.ProtobufEllipse2d");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_linearsystem() {
        let bytes = [
            0x08, 0x03, 0x10, 0x05, 0x18, 0x02, 0x22, 0x55,
            0x08, 0x03, 0x10, 0x03, 0x19, 0x66, 0x66, 0x66,
            0x66, 0x66, 0x66, 0x02, 0x40, 0x19, 0x9A, 0x99,
            0x99, 0x99, 0x99, 0x99, 0xF1, 0x3F, 0x19, 0x66,
            0x66, 0x66, 0x66, 0x66, 0x66, 0x12, 0x40, 0x19,
            0xCD, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0x00, 0x40,
            0x19, 0xCD, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0x18,
            0x40, 0x19, 0xCD, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC,
            0x1A, 0x40, 0x19, 0x33, 0x33, 0x33, 0x33, 0x33,
            0x33, 0xF3, 0x3F, 0x19, 0x9A, 0x99, 0x99, 0x99,
            0x99, 0x99, 0x09, 0x40, 0x19, 0x66, 0x66, 0x66,
            0x66, 0x66, 0x66, 0x43, 0x40, 0x2A, 0x8B, 0x01,
            0x08, 0x03, 0x10, 0x05, 0x19, 0x9A, 0x99, 0x99,
            0x99, 0x99, 0x19, 0x48, 0xC0, 0x19, 0x9A, 0x99,
            0x99, 0x99, 0x99, 0x19, 0x43, 0x40, 0x19, 0x00,
            0x00, 0x00, 0x00, 0x00, 0xC0, 0x5B, 0xC0, 0x19,
            0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0x20, 0x40,
            0x19, 0x66, 0x66, 0x66, 0x66, 0x66, 0xC6, 0x5B,
            0x40, 0x19, 0x00, 0x00, 0x00, 0x00, 0x00, 0xE0,
            0x72, 0x40, 0x19, 0x9A, 0x99, 0x99, 0x99, 0x99,
            0x99, 0x28, 0x40, 0x19, 0x9A, 0x99, 0x99, 0x99,
            0x99, 0x19, 0x4D, 0x40, 0x19, 0x9A, 0x99, 0x99,
            0x99, 0x99, 0x99, 0xF1, 0x3F, 0x19, 0x33, 0x33,
            0x33, 0x33, 0x33, 0x33, 0x15, 0x40, 0x19, 0x66,
            0x66, 0x66, 0x66, 0x66, 0xC6, 0x50, 0x40, 0x19,
            0x9A, 0x99, 0x99, 0x99, 0x99, 0x89, 0x7D, 0x40,
            0x19, 0x00, 0x00, 0x00, 0x00, 0x00, 0x28, 0x8A,
            0x40, 0x19, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
            0x52, 0x40, 0x19, 0x00, 0x00, 0x00, 0x00, 0x00,
            0x00, 0x46, 0x40, 0x32, 0x3A, 0x08, 0x02, 0x10,
            0x03, 0x19, 0xCD, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC,
            0x00, 0x40, 0x19, 0xCD, 0xCC, 0xCC, 0xCC, 0xCC,
            0xE4, 0x81, 0x40, 0x19, 0x66, 0x66, 0x66, 0x66,
            0x66, 0x66, 0x0A, 0x40, 0x19, 0x00, 0x00, 0x00,
            0x00, 0x00, 0x00, 0x20, 0x40, 0x19, 0x00, 0x00,
            0x00, 0x00, 0x00, 0x00, 0x26, 0xC0, 0x19, 0x9A,
            0x99, 0x99, 0x99, 0x99, 0x99, 0x09, 0x40, 0x3A,
            0x5E, 0x08, 0x02, 0x10, 0x05, 0x19, 0x9A, 0x99,
            0x99, 0x99, 0x99, 0x19, 0x3A, 0x40, 0x19, 0x66,
            0x66, 0x66, 0x66, 0x66, 0x66, 0x16, 0x40, 0x19,
            0xCD, 0xCC, 0xCC, 0xCC, 0xCC, 0xAC, 0x53, 0x40,
            0x19, 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0x1C,
            0x40, 0x19, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40,
            0x46, 0x40, 0x19, 0x00, 0x00, 0x00, 0x00, 0x00,
            0x00, 0x22, 0x40, 0x19, 0x00, 0x00, 0x00, 0x00,
            0x00, 0x00, 0x26, 0x40, 0x19, 0x66, 0x66, 0x66,
            0x66, 0x66, 0xB6, 0x71, 0xC0, 0x19, 0x00, 0x00,
            0x00, 0x00, 0x00, 0x00, 0x56, 0xC0, 0x19, 0x71,
            0x3D, 0x0A, 0xD7, 0xA3, 0x70, 0x20, 0x40,
        ];

        let data = LinearSystem::<3, 5, 2> {
            d: Matrix { data: [[26.1, 5.6, 78.7, 7.1, 44.5], [9.0, 11.0, -283.4, -88.0, 8.22]] },
            c: Matrix { data: [[2.1, 572.6, 3.3], [8.0, -11.0, 3.2]] },
            b: Matrix { data: [[-48.2, 38.2, -111.0, 8.2, 111.1], [302.0, 12.3, 58.2, 1.1, 5.3], [67.1, 472.6, 837.0, 72.0, 44.0]] },
            a: Matrix { data: [[2.3, 1.1, 4.6], [2.1, 6.2, 6.7], [1.2, 3.2, 38.8]] },
        };
        assert_eq!(LinearSystem::<3, 5, 2>::proto_type_name(), "wpi.proto.ProtobufLinearSystem");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_matrix() {
        let bytes = [
            0x08, 0x02, 0x10, 0x03, 0x19, 0x66, 0x66, 0x66,
            0x66, 0x66, 0x66, 0x20, 0xC0, 0x19, 0xAE, 0x47,
            0xE1, 0x7A, 0x14, 0xAE, 0x16, 0x40, 0x19, 0x9A,
            0x99, 0x99, 0x99, 0x99, 0x19, 0x48, 0x40, 0x19,
            0xCD, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0x10, 0x40,
            0x19, 0x71, 0x3D, 0x0A, 0xD7, 0xA3, 0x70, 0x14,
            0x40, 0x19, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
            0x23, 0x40,
        ];

        let data = Matrix::<2, 3> {
            data: [[-8.2, 5.67, 48.2], [4.2, 5.11, 9.5]],
        };
        assert_eq!(Matrix::<2, 3>::proto_type_name(), "wpi.proto.ProtobufMatrix");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_mecanumdrivekinematics() {
        let bytes = [
            0x0A, 0x12, 0x09, 0xCD, 0xCC, 0xCC, 0xCC, 0xCC,
            0xCC, 0x00, 0xC0, 0x11, 0x00, 0x00, 0x00, 0x00,
            0x00, 0x00, 0x10, 0x40, 0x12, 0x12, 0x09, 0xCD,
            0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0x10, 0x40, 0x11,
            0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x40,
            0x1A, 0x12, 0x09, 0xCD, 0xCC, 0xCC, 0xCC, 0xCC,
            0xCC, 0x00, 0xC0, 0x11, 0x66, 0x66, 0x66, 0x66,
            0x66, 0x66, 0x20, 0xC0, 0x22, 0x12, 0x09, 0xCD,
            0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0x10, 0x40, 0x11,
            0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0x20, 0xC0,
        ];

        let data = MecanumDriveKinematics {
            front_right: Translation2d { x: 4.2, y: 4.0 },
            front_left: Translation2d { x: -2.1, y: 4.0 },
            rear_left: Translation2d { x: -2.1, y: -8.2 },
            rear_right: Translation2d { x: 4.2, y: -8.2 },
        };
        assert_eq!(MecanumDriveKinematics::proto_type_name(), "wpi.proto.ProtobufMecanumDriveKinematics");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_mecanumdrivewheelpositions() {
        let bytes = [
            0x09, 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0x20,
            0x40, 0x11, 0x33, 0x33, 0x33, 0x33, 0x33, 0xF3,
            0x68, 0x40, 0x19, 0x00, 0x00, 0x00, 0x00, 0x00,
            0x00, 0x22, 0x40, 0x21, 0x33, 0x33, 0x33, 0x33,
            0x33, 0x33, 0x36, 0xC0,
        ];

        let data = MecanumDriveWheelPositions {
            front_right: 199.6,
            front_left: 8.2,
            rear_left: 9.0,
            rear_right: -22.2,
        };
        assert_eq!(MecanumDriveWheelPositions::proto_type_name(), "wpi.proto.ProtobufMecanumDriveWheelPositions");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_mecanumdrivewheelspeeds() {
        let bytes = [
            0x09, 0xCD, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0x18,
            0x40, 0x11, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
            0x12, 0x40, 0x19, 0x00, 0x00, 0x00, 0x00, 0x00,
            0x88, 0x8B, 0x40, 0x21, 0xE1, 0x7A, 0x14, 0xAE,
            0x47, 0xA1, 0x55, 0xC0,
        ];

        let data = MecanumDriveWheelSpeeds {
            front_right: 4.5,
            front_left: 6.2,
            rear_left: 881.0,
            rear_right: -86.52,
        };
        assert_eq!(MecanumDriveWheelSpeeds::proto_type_name(), "wpi.proto.ProtobufMecanumDriveWheelSpeeds");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_pose2d() {
        let bytes = [
            0x0A, 0x12, 0x09, 0xCD, 0xCC, 0xCC, 0xCC, 0xCC,
            0x3C, 0x91, 0xC0, 0x11, 0x66, 0x66, 0x66, 0x66,
            0x66, 0xEC, 0x81, 0xC0, 0x12, 0x09, 0x09, 0xC3,
            0xF5, 0x28, 0x5C, 0x8F, 0xC2, 0x1D, 0x40,
        ];

        let data = Pose2d {
            rotation: Rotation2d { value: 7.44 },
            translation: Translation2d { x: -1103.2, y: -573.55 },
        };
        assert_eq!(Pose2d::proto_type_name(), "wpi.proto.ProtobufPose2d");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_pose3d() {
        let bytes = [
            0x0A, 0x1B, 0x09, 0x7B, 0x14, 0xAE, 0x47, 0xE1,
            0x7A, 0x0C, 0x40, 0x11, 0x33, 0x33, 0x33, 0x33,
            0x33, 0x33, 0x1B, 0x40, 0x19, 0x9A, 0x99, 0x99,
            0x99, 0x99, 0x99, 0x11, 0x40, 0x12, 0x26, 0x0A,
            0x24, 0x09, 0x55, 0xF7, 0x96, 0x9A, 0x56, 0xC2,
            0xD0, 0x3F, 0x11, 0xC7, 0x49, 0xC9, 0x78, 0x73,
            0x58, 0xB6, 0x3F, 0x19, 0xC7, 0x49, 0xC9, 0x78,
            0x73, 0x58, 0xA6, 0xBF, 0x21, 0x72, 0xC5, 0x14,
            0xC6, 0x9E, 0xB9, 0xEE, 0x3F,
        ];

        let data = Pose3d {
            rotation: Rotation3d { quaternion: Quaternion { w: 0.26186146828319085, x: 0.08728715609439695, y: -0.04364357804719848, z: 0.9601587170383665 } },
            translation: Translation3d { x: 3.56, y: 6.8, z: 4.4 },
        };
        assert_eq!(Pose3d::proto_type_name(), "wpi.proto.ProtobufPose3d");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_quaternion() {
        let bytes = [
            0x09, 0xCD, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0x23,
            0x40, 0x11, 0xCD, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC,
            0x00, 0x40, 0x19, 0x66, 0x66, 0x66, 0x66, 0x66,
            0x66, 0x16, 0x40, 0x21, 0x66, 0x66, 0x66, 0x66,
            0x66, 0xA6, 0x4B, 0x40,
        ];

        let data = Quaternion {
            w: 9.9,
            z: 55.3,
            y: 5.6,
            x: 2.1,
        };
        assert_eq!(Quaternion::proto_type_name(), "wpi.proto.ProtobufQuaternion");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_quintichermitespline() {
        let bytes = [
            0x09, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x15,
            0x40, 0x09, 0x71, 0x3D, 0x0A, 0xD7, 0xA3, 0x70,
            0x10, 0x40, 0x09, 0x85, 0xEB, 0x51, 0xB8, 0x1E,
            0x05, 0x22, 0x40, 0x11, 0x7B, 0x14, 0xAE, 0x47,
            0xE1, 0x7A, 0x84, 0x3F, 0x11, 0x66, 0x66, 0x66,
            0x66, 0x66, 0x66, 0x20, 0x40, 0x11, 0xCD, 0xCC,
            0xCC, 0xCC, 0xCC, 0x0C, 0x46, 0x40, 0x19, 0xA4,
            0x70, 0x3D, 0x0A, 0xD7, 0xA3, 0x1A, 0x40, 0x19,
            0xCD, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0x10, 0x40,
            0x19, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x1F,
            0x40, 0x21, 0x9A, 0x99, 0x99, 0x99, 0x99, 0x99,
            0x26, 0x40, 0x21, 0x00, 0x00, 0x00, 0x00, 0x00,
            0x00, 0x21, 0x40, 0x21, 0x66, 0x66, 0x66, 0x66,
            0x66, 0x66, 0x12, 0x40,
        ];

        let data = QuinticHermiteSpline {
            x_final: [0.01, 8.2, 44.1],
            y_initial: [6.66, 4.2, 7.8],
            x_initial: [5.3, 4.11, 9.01],
            y_final: [11.3, 8.5, 4.6],
        };
        assert_eq!(QuinticHermiteSpline::proto_type_name(), "wpi.proto.ProtobufQuinticHermiteSpline");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_rectangle2d() {
        let bytes = [
            0x11, 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0x20,
            0x40, 0x19, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33,
            0x23, 0x40, 0x0A, 0x1F, 0x0A, 0x12, 0x09, 0xCD,
            0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0x21, 0x40, 0x11,
            0xCD, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0x2C, 0x40,
            0x12, 0x09, 0x09, 0x00, 0x00, 0x00, 0x00, 0x00,
            0x00, 0x00, 0x00,
        ];

        let data = Rectangle2d {
            x_width: 8.2,
            y_width: 9.6,
            center: Pose2d { translation: Translation2d { x: 8.9, y: 14.4 }, rotation: Rotation2d { value: 0.0 } },
        };
        assert_eq!(Rectangle2d::proto_type_name(), "wpi.proto.ProtobufRectangle2d");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_rotation2d() {
        let bytes = [
            0x09, 0xE1, 0x7A, 0x14, 0xAE, 0x47, 0xE1, 0x20,
            0x40,
        ];

        let data = Rotation2d {
            value: 8.44,
        };
        assert_eq!(Rotation2d::proto_type_name(), "wpi.proto.ProtobufRotation2d");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_rotation3d() {
        let bytes = [
            0x0A, 0x24, 0x09, 0xF7, 0xC6, 0x57, 0x88, 0x7B,
            0xA7, 0xC8, 0x3F, 0x11, 0xFC, 0x93, 0xE4, 0x36,
            0xE8, 0x3F, 0xDC, 0x3F, 0x19, 0x50, 0x8C, 0x8F,
            0xF3, 0xC7, 0x57, 0xE6, 0x3F, 0x21, 0xCA, 0x58,
            0xBC, 0xED, 0x24, 0xF3, 0xE0, 0x3F,
        ];

        let data = Rotation3d {
            quaternion: Quaternion { w: 0.19261116177909063, x: 0.44140057907708274, y: 0.6982154614492035, z: 0.5296806948924992 },
        };
        assert_eq!(Rotation3d::proto_type_name(), "wpi.proto.ProtobufRotation3d");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_simplemotorfeedforward() {
        let bytes = [
            0x09, 0x9A, 0x99, 0x99, 0x99, 0x99, 0x99, 0xF1,
            0x3F, 0x11, 0x1F, 0x85, 0xEB, 0x51, 0xB8, 0x1E,
            0xE5, 0x3F, 0x19, 0xCD, 0xCC, 0xCC, 0xCC, 0xCC,
            0x4C, 0x32, 0x40, 0x21, 0x7B, 0x14, 0xAE, 0x47,
            0xE1, 0x7A, 0xD4, 0x3F,
        ];

        let data = SimpleMotorFeedforward {
            k_s: 1.1,
            k_a: 18.3,
            k_v: 0.66,
            d_t: 0.32,
        };
        assert_eq!(SimpleMotorFeedforward::proto_type_name(), "wpi.proto.ProtobufSimpleMotorFeedforward");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_swervedrivekinematics() {
        let bytes = [
            0x0A, 0x12, 0x09, 0x33, 0x33, 0x33, 0x33, 0x33,
            0x33, 0x11, 0x40, 0x11, 0x66, 0x66, 0x66, 0x66,
            0x66, 0x66, 0x1A, 0x40, 0x0A, 0x12, 0x09, 0x00,
            0x00, 0x00, 0x00, 0x00, 0x00, 0x14, 0xC0, 0x11,
            0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0x1A, 0x40,
            0x0A, 0x12, 0x09, 0x33, 0x33, 0x33, 0x33, 0x33,
            0x33, 0x11, 0x40, 0x11, 0xCD, 0xCC, 0xCC, 0xCC,
            0xCC, 0xCC, 0x23, 0xC0, 0x0A, 0x12, 0x09, 0x00,
            0x00, 0x00, 0x00, 0x00, 0x00, 0x14, 0xC0, 0x11,
            0xCD, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0x23, 0xC0,
        ];

        let data = SwerveDriveKinematics::<4> {
            modules: [Translation2d { x: 4.3, y: 6.6 }, Translation2d { x: -5.0, y: 6.6 }, Translation2d { x: 4.3, y: -9.9 }, Translation2d { x: -5.0, y: -9.9 }],
        };
        assert_eq!(SwerveDriveKinematics::<4>::proto_type_name(), "wpi.proto.ProtobufSwerveDriveKinematics");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_swervemoduleposition() {
        let bytes = [
            0x09, 0xCD, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0x00,
            0x40, 0x12, 0x09, 0x09, 0x48, 0xE1, 0x7A, 0x14,
            0xAE, 0x47, 0xE9, 0x3F,
        ];

        let data = SwerveModulePosition {
            angle: Rotation2d { value: 0.79 },
            distance: 2.1,
        };
        assert_eq!(SwerveModulePosition::proto_type_name(), "wpi.proto.ProtobufSwerveModulePosition");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_swervemodulestate() {
        let bytes = [
            0x09, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x16,
            0x40, 0x12, 0x09, 0x09, 0x6F, 0x12, 0x83, 0xC0,
            0xCA, 0x21, 0x09, 0x40,
        ];

        let data = SwerveModuleState {
            speed: 5.5,
            #[allow(clippy::approx_constant)]
            angle: Rotation2d { value: 3.1415 },
        };
        assert_eq!(SwerveModuleState::proto_type_name(), "wpi.proto.ProtobufSwerveModuleState");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_trajectory() {
        let bytes = [
            0x12, 0x45, 0x09, 0x66, 0x66, 0x66, 0x66, 0x66,
            0x66, 0x02, 0x40, 0x11, 0x9A, 0x99, 0x99, 0x99,
            0x99, 0x99, 0x40, 0x40, 0x19, 0x00, 0x00, 0x00,
            0x00, 0x00, 0x00, 0x25, 0xC0, 0x29, 0x66, 0x66,
            0x66, 0x66, 0x66, 0x86, 0x53, 0x40, 0x22, 0x1F,
            0x0A, 0x12, 0x09, 0x66, 0x66, 0x66, 0x66, 0x66,
            0x66, 0x14, 0x40, 0x11, 0xE1, 0x7A, 0x14, 0xAE,
            0x47, 0xE1, 0x22, 0x40, 0x12, 0x09, 0x09, 0x39,
            0xB4, 0xC8, 0x76, 0xBE, 0x9F, 0xE2, 0x3F, 0x12,
            0x45, 0x09, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33,
            0x20, 0x40, 0x11, 0xA4, 0x70, 0x3D, 0x0A, 0xD7,
            0xA3, 0x16, 0xC0, 0x19, 0xCD, 0xCC, 0xCC, 0xCC,
            0xCC, 0x78, 0x84, 0x40, 0x29, 0x66, 0x66, 0x66,
            0x66, 0x66, 0x66, 0x45, 0x40, 0x22, 0x1F, 0x0A,
            0x12, 0x09, 0xE1, 0x7A, 0x14, 0xAE, 0x47, 0xE1,
            0xFA, 0x3F, 0x11, 0xCD, 0xCC, 0xCC, 0xCC, 0xCC,
            0xCC, 0xE4, 0xBF, 0x12, 0x09, 0x09, 0xB6, 0xF3,
            0xFD, 0xD4, 0x78, 0xE9, 0xFA, 0x3F, 0x12, 0x45,
            0x09, 0x14, 0xAE, 0x47, 0xE1, 0x7A, 0x94, 0x2C,
            0xC0, 0x11, 0x9A, 0x99, 0x99, 0x99, 0x99, 0x99,
            0xE1, 0x3F, 0x19, 0xE1, 0x7A, 0x14, 0xAE, 0x47,
            0xE1, 0xFA, 0x3F, 0x29, 0x48, 0xE1, 0x7A, 0x14,
            0xAE, 0x47, 0xF5, 0xBF, 0x22, 0x1F, 0x0A, 0x12,
            0x09, 0xF6, 0x28, 0x5C, 0x8F, 0xC2, 0x15, 0x48,
            0xC0, 0x11, 0x6A, 0xBC, 0x74, 0x93, 0x18, 0x04,
            0xF0, 0x3F, 0x12, 0x09, 0x09, 0x46, 0xB6, 0xF3,
            0xFD, 0xD4, 0x78, 0xED, 0xBF,
        ];

        let data = Trajectory {
            states: vec![
                TrajectoryState {
                    time: 2.3,
                    velocity: 33.2,
                    acceleration: -10.5,
                    pose: Pose2d {
                        translation: Translation2d { x: 5.1, y: 9.44 },
                        rotation: Rotation2d { value: 0.582 },
                    },
                    curvature: 78.1,
                },
                TrajectoryState {
                    time: 8.1,
                    velocity: -5.66,
                    acceleration: 655.1,
                    pose: Pose2d {
                        translation: Translation2d { x: 1.68, y: -0.65 },
                        rotation: Rotation2d { value: 1.682 },
                    },
                    curvature: 42.8,
                },
                TrajectoryState {
                    time: -14.29,
                    velocity: 0.55,
                    acceleration: 1.68,
                    pose: Pose2d {
                        translation: Translation2d { x: -48.17, y: 1.001 },
                        rotation: Rotation2d { value: -0.921 },
                    },
                    curvature: -1.33,
                },
            ],
        };
        assert_eq!(Trajectory::proto_type_name(), "wpi.proto.ProtobufTrajectory");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_trajectorystate() {
        let bytes = [
            0x09, 0x66, 0x66, 0x66, 0x66, 0x66, 0x06, 0x53,
            0x40, 0x11, 0xA4, 0x70, 0x3D, 0x0A, 0xD7, 0xA3,
            0xF0, 0x3F, 0x19, 0x00, 0x00, 0x00, 0x00, 0x00,
            0x00, 0x14, 0x40, 0x29, 0x8F, 0xC2, 0xF5, 0x28,
            0x5C, 0x8F, 0x15, 0xC0, 0x22, 0x1F, 0x0A, 0x12,
            0x09, 0xCD, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0x08,
            0x40, 0x11, 0xE1, 0x7A, 0x14, 0xAE, 0x47, 0xE1,
            0x22, 0xC0, 0x12, 0x09, 0x09, 0xD5, 0x78, 0xE9,
            0x26, 0x31, 0x08, 0x08, 0x40,
        ];

        let data = TrajectoryState {
            pose: Pose2d { translation: Translation2d { x: 3.1, y: -9.44 }, rotation: Rotation2d { value: 3.004 } },
            acceleration: 5.0,
            velocity: 1.04,
            curvature: -5.39,
            time: 76.1,
        };
        assert_eq!(TrajectoryState::proto_type_name(), "wpi.proto.ProtobufTrajectoryState");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_transform2d() {
        let bytes = [
            0x0A, 0x12, 0x09, 0xC3, 0xF5, 0x28, 0x5C, 0x8F,
            0xC2, 0x09, 0x40, 0x11, 0xCD, 0xCC, 0xCC, 0xCC,
            0xCC, 0xCC, 0x19, 0x40, 0x12, 0x09, 0x09, 0x71,
            0x3D, 0x0A, 0xD7, 0xA3, 0x70, 0x22, 0x40,
        ];

        let data = Transform2d {
            rotation: Rotation2d { value: 9.22 },
            translation: Translation2d { x: 3.22, y: 6.45 },
        };
        assert_eq!(Transform2d::proto_type_name(), "wpi.proto.ProtobufTransform2d");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_transform3d() {
        let bytes = [
            0x0A, 0x1B, 0x09, 0x33, 0x33, 0x33, 0x33, 0x33,
            0x33, 0x03, 0xC0, 0x11, 0x7B, 0x14, 0xAE, 0x47,
            0xE1, 0x7A, 0x17, 0x40, 0x19, 0x33, 0x33, 0x33,
            0x33, 0x33, 0x33, 0x22, 0x40, 0x12, 0x26, 0x0A,
            0x24, 0x09, 0xC3, 0x48, 0x2E, 0x27, 0xAC, 0xB7,
            0xE5, 0x3F, 0x11, 0x65, 0xE9, 0x61, 0x02, 0x39,
            0xD4, 0xD3, 0x3F, 0x19, 0xB7, 0x8B, 0x0A, 0x68,
            0xDA, 0x44, 0xE5, 0xBF, 0x21, 0x15, 0x19, 0xC8,
            0x94, 0xF2, 0xC5, 0xA4, 0x3F,
        ];

        let data = Transform3d {
            rotation: Rotation3d { quaternion: Quaternion { w: 0.6786709561586338, x: 0.3098280452028546, y: -0.6646549255423143, z: 0.04057272020513573 } },
            translation: Translation3d { x: -2.4, y: 5.87, z: 9.1 },
        };
        assert_eq!(Transform3d::proto_type_name(), "wpi.proto.ProtobufTransform3d");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_translation2d() {
        let bytes = [
            0x09, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x20,
            0x40, 0x11, 0x0A, 0xD7, 0xA3, 0x70, 0x3D, 0x0A,
            0xF7, 0x3F,
        ];

        let data = Translation2d {
            y: 1.44,
            x: 8.1,
        };
        assert_eq!(Translation2d::proto_type_name(), "wpi.proto.ProtobufTranslation2d");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_translation3d() {
        let bytes = [
            0x09, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x22,
            0x40, 0x11, 0x9A, 0x99, 0x99, 0x99, 0x99, 0x99,
            0x1B, 0x40, 0x19, 0xF6, 0x28, 0x5C, 0x8F, 0xC2,
            0xF5, 0x10, 0x40,
        ];

        let data = Translation3d {
            z: 4.24,
            y: 6.9,
            x: 9.1,
        };
        assert_eq!(Translation3d::proto_type_name(), "wpi.proto.ProtobufTranslation3d");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_twist2d() {
        let bytes = [
            0x09, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10,
            0x40, 0x11, 0x48, 0xE1, 0x7A, 0x14, 0xAE, 0x47,
            0xF5, 0x3F, 0x19, 0xCD, 0xCC, 0xCC, 0xCC, 0xCC,
            0xAC, 0x52, 0x40,
        ];

        let data = Twist2d {
            dy: 1.33,
            dx: 4.0,
            dtheta: 74.7,
        };
        assert_eq!(Twist2d::proto_type_name(), "wpi.proto.ProtobufTwist2d");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_twist3d() {
        let bytes = [
            0x09, 0x9A, 0x99, 0x99, 0x99, 0x99, 0x99, 0xD9,
            0x3F, 0x11, 0x66, 0x66, 0x66, 0x66, 0x66, 0x66,
            0x12, 0x40, 0x19, 0x71, 0x3D, 0x0A, 0xD7, 0xA3,
            0x70, 0x20, 0x40, 0x21, 0x9A, 0x99, 0x99, 0x99,
            0x99, 0x99, 0x15, 0x40, 0x29, 0x9A, 0x99, 0x99,
            0x99, 0x99, 0x99, 0xE9, 0x3F, 0x31, 0x66, 0x66,
            0x66, 0x66, 0x66, 0x66, 0xF6, 0x3F,
        ];

        let data = Twist3d {
            rz: 1.4,
            ry: 0.8,
            rx: 5.4,
            dz: 8.22,
            dy: 4.6,
            dx: 0.4,
        };
        assert_eq!(Twist3d::proto_type_name(), "wpi.proto.ProtobufTwist3d");
        test_protobuf(data, &bytes);
    }

    #[test]
    #[cfg(feature = "math")]
    fn test_vector() {
        let bytes = [
            0x09, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x12,
            0x40, 0x09, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33,
            0xE3, 0xBF, 0x09, 0xF6, 0x28, 0x5C, 0x8F, 0xC2,
            0xF5, 0x1F, 0x40,
        ];

        let data = Vector::<3> {
            rows: [4.5, -0.6, 7.99],
        };
        assert_eq!(Vector::<3>::proto_type_name(), "wpi.proto.ProtobufVector");
        test_protobuf(data, &bytes);
    }

    fn test_protobuf<P>(p: P, matches: &[u8])
    where P: ProtobufData + std::clone::Clone + std::fmt::Debug + std::cmp::PartialEq
    {
        // don't test that the written bytes are the same, Java's quickbuf writes them differently
        let proto = assert_ok!(P::Proto::parse_from_bytes(matches));
        let data = P::from_proto(proto).unwrap();
        assert_eq!(data, p);
    }
}