rover_nexus_core 0.2.0

// Copyright 2026 Rottinghaus Dynamics
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

//! Cap'n Proto message serialization for Zenoh transport
//!
//! This module provides the generated Cap'n Proto schema code and helper functions
//! for serializing/deserializing telemetry and command messages.

use crate::core_model::*;
use crate::internal_model::{AckStatus, AgentTelemetry, Heartbeat, HeartbeatAck, RobotAck};
use crate::spatial_types::{
    GeoPoint, GeoPose, LocalPoint, LocalPose, LocalPolygonData, LocalRoute, LocalShape, Pose,
    SpatialData, Wgs84PolygonData, Wgs84Route, Wgs84Shape,
};
use uuid::Uuid;

// The generated Cap'n Proto code is included at crate::messages_capnp
// We reference it directly here

use ::capnp::message::Builder;
use ::capnp::serialize;

use std::io::Cursor;

// Helper functions for Value enum serialization
fn write_value(builder: &mut crate::messages_capnp::value::Builder, value: &Value) {
    match value {
        Value::String(s) => builder.set_string(s),
        Value::Int(i) => builder.set_int(*i),
        Value::Number(n) => builder.set_number(*n),
        Value::Bool(b) => builder.set_bool(*b),
        Value::Enum(s) => builder.set_enum(s),
    }
}

fn read_value(reader: crate::messages_capnp::value::Reader) -> Result<Value, ::capnp::Error> {
    match reader.which()? {
        crate::messages_capnp::value::String(Ok(s)) => Ok(Value::String(s.to_str()?.to_string())),
        crate::messages_capnp::value::String(Err(e)) => Err(e),
        crate::messages_capnp::value::Int(i) => Ok(Value::Int(i)),
        crate::messages_capnp::value::Number(n) => Ok(Value::Number(n)),
        crate::messages_capnp::value::Bool(b) => Ok(Value::Bool(b)),
        crate::messages_capnp::value::Enum(Ok(s)) => Ok(Value::Enum(s.to_str()?.to_string())),
        crate::messages_capnp::value::Enum(Err(e)) => Err(e),
    }
}

// Helper functions for serde_json::Value serialization
fn write_json_value(
    builder: &mut crate::messages_capnp::value::Builder,
    value: &serde_json::Value,
) {
    match value {
        serde_json::Value::String(s) => builder.set_string(s),
        serde_json::Value::Number(n) => {
            if let Some(i) = n.as_i64() {
                builder.set_int(i as i32);
            } else if let Some(f) = n.as_f64() {
                builder.set_number(f);
            }
        }
        serde_json::Value::Bool(b) => builder.set_bool(*b),
        _ => builder.set_string(""),
    }
}

fn read_json_value(
    reader: crate::messages_capnp::value::Reader,
) -> Result<serde_json::Value, ::capnp::Error> {
    match reader.which()? {
        crate::messages_capnp::value::String(Ok(s)) => {
            Ok(serde_json::Value::String(s.to_str()?.to_string()))
        }
        crate::messages_capnp::value::String(Err(e)) => Err(e),
        crate::messages_capnp::value::Int(i) => {
            Ok(serde_json::Value::Number(serde_json::Number::from(i)))
        }
        crate::messages_capnp::value::Number(n) => serde_json::Number::from_f64(n)
            .map(serde_json::Value::Number)
            .ok_or_else(|| ::capnp::Error::failed(format!("Invalid float value: {}", n))),
        crate::messages_capnp::value::Bool(b) => Ok(serde_json::Value::Bool(b)),
        crate::messages_capnp::value::Enum(Ok(s)) => {
            Ok(serde_json::Value::String(s.to_str()?.to_string()))
        }
        crate::messages_capnp::value::Enum(Err(e)) => Err(e),
    }
}

/// Serialize a RobotUplinkMsg to Cap'n Proto bytes
pub fn serialize_robot_uplink_msg(msg: &RobotUplinkMsg) -> Result<Vec<u8>, ::capnp::Error> {
    let mut message = Builder::new_default();
    let uplink_builder = message.init_root::<crate::messages_capnp::robot_uplink_msg::Builder>();

    match msg {
        RobotUplinkMsg::MissionRunStatus(mission_run_status) => {
            let mut builder = uplink_builder.init_mission_run_status();
            write_mission_run_status(&mut builder, mission_run_status);
        }
        RobotUplinkMsg::GlobalMotionTelemetry(telem) => {
            let mut builder = uplink_builder.init_global_motion_telemetry();
            write_global_motion_telemetry(&mut builder, telem);
        }
        RobotUplinkMsg::LocalMotionTelemetry(telem) => {
            let mut builder = uplink_builder.init_local_motion_telemetry();
            write_local_motion_telemetry(&mut builder, telem);
        }
        RobotUplinkMsg::StatusTelemetry(telem) => {
            let mut builder = uplink_builder.init_status_telemetry();
            write_status_telemetry(&mut builder, telem);
        }
        RobotUplinkMsg::SensorTelemetry(telem) => {
            let mut builder = uplink_builder.init_sensor_telemetry();
            write_sensor_telemetry(&mut builder, telem);
        }
        RobotUplinkMsg::Capabilities(caps) => {
            let mut builder = uplink_builder.init_capabilities();
            write_capabilities(&mut builder, caps);
        }
        RobotUplinkMsg::AllowedCommands(allowed) => {
            let mut builder = uplink_builder.init_allowed_commands();
            write_allowed_commands(&mut builder, allowed);
        }
        RobotUplinkMsg::Fault(fault) => {
            let mut builder = uplink_builder.init_fault();
            write_fault(&mut builder, fault);
        }
        RobotUplinkMsg::Feature(update) => {
            let mut builder = uplink_builder.init_feature();
            write_reported_feature_update(&mut builder, update);
        }
        RobotUplinkMsg::Object(obj_op) => {
            let mut builder = uplink_builder.init_object();
            write_object_op(&mut builder, obj_op);
        }
        RobotUplinkMsg::SystemHealth(health) => {
            let mut builder = uplink_builder.init_system_health();
            write_system_health(&mut builder, health);
        }
        RobotUplinkMsg::UsageTelemetry(usage) => {
            let mut builder = uplink_builder.init_usage_telemetry();
            write_usage_telemetry(&mut builder, usage);
        }
        RobotUplinkMsg::ConsumableStatus(consumable) => {
            let mut builder = uplink_builder.init_consumable_status();
            write_consumable_status(&mut builder, consumable);
        }
        RobotUplinkMsg::CurrentSettings(settings) => {
            let mut settings_builder = uplink_builder.init_current_settings(settings.len() as u32);
            for (i, setting) in settings.iter().enumerate() {
                let mut setting_builder = settings_builder.reborrow().get(i as u32);
                setting_builder.set_key(&setting.key);
                let mut value_builder = setting_builder.init_value();
                write_value(&mut value_builder, &setting.value);
            }
        }
        RobotUplinkMsg::Message(msg) => {
            let mut builder = uplink_builder.init_message();
            write_message(&mut builder, msg);
        }
        RobotUplinkMsg::AgentInfo(info) => {
            let mut builder = uplink_builder.init_agent_info();
            write_agent_info(&mut builder, info);
        }
        RobotUplinkMsg::SpatialDirectiveStatus(status) => {
            let mut builder = uplink_builder.init_spatial_directive_status();
            write_spatial_directive_status(&mut builder, status);
        }
    }

    let mut buf = Vec::new();
    serialize::write_message(&mut buf, &message)?;
    Ok(buf)
}

/// Deserialize a RobotUplinkMsg from Cap'n Proto bytes
pub fn deserialize_robot_uplink_msg(data: &[u8]) -> Result<RobotUplinkMsg, ::capnp::Error> {
    let cursor = Cursor::new(data);
    let message_reader = serialize::read_message(cursor, ::capnp::message::ReaderOptions::new())?;
    let uplink_reader =
        message_reader.get_root::<crate::messages_capnp::robot_uplink_msg::Reader>()?;

    match uplink_reader.which()? {
        crate::messages_capnp::robot_uplink_msg::MissionRunStatus(Ok(reader)) => Ok(
            RobotUplinkMsg::MissionRunStatus(read_mission_run_status(reader)?),
        ),
        crate::messages_capnp::robot_uplink_msg::GlobalMotionTelemetry(Ok(reader)) => Ok(
            RobotUplinkMsg::GlobalMotionTelemetry(read_global_motion_telemetry(reader)?),
        ),
        crate::messages_capnp::robot_uplink_msg::LocalMotionTelemetry(Ok(reader)) => Ok(
            RobotUplinkMsg::LocalMotionTelemetry(read_local_motion_telemetry(reader)?),
        ),
        crate::messages_capnp::robot_uplink_msg::StatusTelemetry(Ok(reader)) => {
            Ok(RobotUplinkMsg::StatusTelemetry(read_status_telemetry(reader)?))
        }
        crate::messages_capnp::robot_uplink_msg::SensorTelemetry(Ok(reader)) => {
            Ok(RobotUplinkMsg::SensorTelemetry(read_sensor_telemetry(reader)?))
        }
        crate::messages_capnp::robot_uplink_msg::Capabilities(Ok(reader)) => {
            Ok(RobotUplinkMsg::Capabilities(read_capabilities(reader)?))
        }
        crate::messages_capnp::robot_uplink_msg::AllowedCommands(Ok(reader)) => {
            Ok(RobotUplinkMsg::AllowedCommands(read_allowed_commands(reader)?))
        }
        crate::messages_capnp::robot_uplink_msg::Fault(Ok(reader)) => {
            Ok(RobotUplinkMsg::Fault(read_fault(reader)?))
        }
        crate::messages_capnp::robot_uplink_msg::Feature(Ok(reader)) => {
            Ok(RobotUplinkMsg::Feature(read_reported_feature_update(reader)?))
        }
        crate::messages_capnp::robot_uplink_msg::Object(Ok(reader)) => {
            Ok(RobotUplinkMsg::Object(read_object_op(reader)?))
        }
        crate::messages_capnp::robot_uplink_msg::SystemHealth(Ok(reader)) => {
            Ok(RobotUplinkMsg::SystemHealth(read_system_health(reader)?))
        }
        crate::messages_capnp::robot_uplink_msg::UsageTelemetry(Ok(reader)) => {
            Ok(RobotUplinkMsg::UsageTelemetry(read_usage_telemetry(reader)?))
        }
        crate::messages_capnp::robot_uplink_msg::ConsumableStatus(Ok(reader)) => Ok(
            RobotUplinkMsg::ConsumableStatus(read_consumable_status(reader)?),
        ),
        crate::messages_capnp::robot_uplink_msg::CurrentSettings(Ok(settings_reader)) => {
            let mut settings = Vec::new();
            for i in 0..settings_reader.len() {
                let setting = settings_reader.get(i);
                settings.push(SettingUpdate {
                    key: setting.get_key()?.to_str()?.to_string(),
                    value: read_value(setting.get_value()?)?,
                });
            }
            Ok(RobotUplinkMsg::CurrentSettings(settings))
        }
        crate::messages_capnp::robot_uplink_msg::Message(Ok(reader)) => {
            Ok(RobotUplinkMsg::Message(read_message(reader)?))
        }
        crate::messages_capnp::robot_uplink_msg::AgentInfo(Ok(reader)) => {
            Ok(RobotUplinkMsg::AgentInfo(read_agent_info(reader)?))
        }
        crate::messages_capnp::robot_uplink_msg::SpatialDirectiveStatus(Ok(reader)) => Ok(
            RobotUplinkMsg::SpatialDirectiveStatus(read_spatial_directive_status(reader)?),
        ),
        _ => Err(::capnp::Error::failed(
            "Unsupported robot uplink message variant".to_string(),
        )),
    }
}

/// Serialize a RobotCommand to Cap'n Proto bytes
pub fn serialize_robot_command(cmd: &RobotCommand) -> Result<Vec<u8>, ::capnp::Error> {
    let mut message = Builder::new_default();
    let mut cmd_builder = message.init_root::<crate::messages_capnp::robot_command::Builder>();

    write_robot_command(&mut cmd_builder, cmd)?;

    let mut buf = Vec::new();
    serialize::write_message(&mut buf, &message)?;
    Ok(buf)
}

/// Deserialize a RobotCommand from Cap'n Proto bytes
pub fn deserialize_robot_command(data: &[u8]) -> Result<RobotCommand, ::capnp::Error> {
    let cursor = Cursor::new(data);
    let message_reader = serialize::read_message(cursor, ::capnp::message::ReaderOptions::new())?;
    let cmd_reader = message_reader.get_root::<crate::messages_capnp::robot_command::Reader>()?;

    read_robot_command_from_reader(cmd_reader)
}

/// Serialize a NexusCommand (server -> robot agent) to Cap'n Proto bytes.
pub fn serialize_nexus_command(cmd: &NexusCommand) -> Result<Vec<u8>, ::capnp::Error> {
    serialize_nexus_command_parts(cmd.id, &cmd.command)
}

/// Serialize a `(id, &RobotCommand)` pair to Cap'n Proto bytes
/// without materializing the wrapper struct. Lets the FM->Agent
/// dispatcher mint per-robot command_ids and serialize each
/// transmission against a borrowed RobotCommand, avoiding a clone
/// of the (potentially large) inner payload on the dispatch hot path.
pub fn serialize_nexus_command_parts(
    id: Uuid,
    command: &RobotCommand,
) -> Result<Vec<u8>, ::capnp::Error> {
    let mut message = Builder::new_default();
    let mut nc_builder = message.init_root::<crate::messages_capnp::nexus_command::Builder>();

    nc_builder.set_id(id.as_bytes());
    let mut inner_builder = nc_builder.reborrow().init_command();
    write_robot_command(&mut inner_builder, command)?;

    let mut buf = Vec::new();
    serialize::write_message(&mut buf, &message)?;
    Ok(buf)
}

/// Deserialize a NexusCommand (server -> robot agent) from Cap'n Proto bytes.
pub fn deserialize_nexus_command(data: &[u8]) -> Result<NexusCommand, ::capnp::Error> {
    let cursor = Cursor::new(data);
    let message_reader = serialize::read_message(cursor, ::capnp::message::ReaderOptions::new())?;
    let nc_reader = message_reader.get_root::<crate::messages_capnp::nexus_command::Reader>()?;

    let id = uuid_from_capnp(nc_reader.get_id()?)?;
    let command = read_robot_command_from_reader(nc_reader.get_command()?)?;
    Ok(NexusCommand { id, command })
}

/// Serialize a RobotAck (agent/robot -> server) to Cap'n Proto bytes.
pub fn serialize_robot_ack(ack: &RobotAck) -> Result<Vec<u8>, ::capnp::Error> {
    let mut message = Builder::new_default();
    let mut b = message.init_root::<crate::messages_capnp::robot_ack::Builder>();
    write_robot_ack(&mut b, ack);

    let mut buf = Vec::new();
    serialize::write_message(&mut buf, &message)?;
    Ok(buf)
}

/// Deserialize a RobotAck (agent/robot -> server) from Cap'n Proto bytes.
pub fn deserialize_robot_ack(data: &[u8]) -> Result<RobotAck, ::capnp::Error> {
    let cursor = Cursor::new(data);
    let message_reader = serialize::read_message(cursor, ::capnp::message::ReaderOptions::new())?;
    let r = message_reader.get_root::<crate::messages_capnp::robot_ack::Reader>()?;
    read_robot_ack(r)
}

/// Serialize an AgentTelemetry (agent -> server control-plane) to Cap'n Proto bytes.
pub fn serialize_agent_telemetry(msg: &AgentTelemetry) -> Result<Vec<u8>, ::capnp::Error> {
    let mut message = Builder::new_default();
    let at_builder = message.init_root::<crate::messages_capnp::agent_telemetry::Builder>();

    match msg {
        AgentTelemetry::Heartbeat(hb) => {
            let mut hb_builder = at_builder.init_heartbeat();
            hb_builder.set_seq(hb.seq);
            hb_builder.set_sent_time_ms(hb.sent_time_ms);
        }
        AgentTelemetry::RobotAck(ack) => {
            let mut ack_builder = at_builder.init_robot_ack();
            write_robot_ack(&mut ack_builder, ack);
        }
    }

    let mut buf = Vec::new();
    serialize::write_message(&mut buf, &message)?;
    Ok(buf)
}

/// Deserialize an AgentTelemetry (agent -> server control-plane) from Cap'n Proto bytes.
pub fn deserialize_agent_telemetry(data: &[u8]) -> Result<AgentTelemetry, ::capnp::Error> {
    let cursor = Cursor::new(data);
    let message_reader = serialize::read_message(cursor, ::capnp::message::ReaderOptions::new())?;
    let at_reader = message_reader.get_root::<crate::messages_capnp::agent_telemetry::Reader>()?;

    match at_reader.which()? {
        crate::messages_capnp::agent_telemetry::Heartbeat(Ok(hb)) => {
            Ok(AgentTelemetry::Heartbeat(Heartbeat {
                seq: hb.get_seq(),
                sent_time_ms: hb.get_sent_time_ms(),
            }))
        }
        crate::messages_capnp::agent_telemetry::RobotAck(Ok(ack)) => {
            Ok(AgentTelemetry::RobotAck(read_robot_ack(ack)?))
        }
        _ => Err(::capnp::Error::failed(
            "Unsupported agent telemetry variant".to_string(),
        )),
    }
}

/// Serialize a Heartbeat (robot -> server) to Cap'n Proto bytes.
pub fn serialize_heartbeat(msg: &Heartbeat) -> Result<Vec<u8>, ::capnp::Error> {
    let mut message = Builder::new_default();
    let mut b = message.init_root::<crate::messages_capnp::heartbeat::Builder>();
    b.set_seq(msg.seq);
    b.set_sent_time_ms(msg.sent_time_ms);

    let mut buf = Vec::new();
    serialize::write_message(&mut buf, &message)?;
    Ok(buf)
}

/// Deserialize a Heartbeat (robot -> server) from Cap'n Proto bytes.
pub fn deserialize_heartbeat(data: &[u8]) -> Result<Heartbeat, ::capnp::Error> {
    let cursor = Cursor::new(data);
    let message_reader = serialize::read_message(cursor, ::capnp::message::ReaderOptions::new())?;
    let r = message_reader.get_root::<crate::messages_capnp::heartbeat::Reader>()?;
    Ok(Heartbeat {
        seq: r.get_seq(),
        sent_time_ms: r.get_sent_time_ms(),
    })
}

/// Serialize a HeartbeatAck (server -> robot) to Cap'n Proto bytes.
pub fn serialize_heartbeat_ack(msg: &HeartbeatAck) -> Result<Vec<u8>, ::capnp::Error> {
    let mut message = Builder::new_default();
    let mut b = message.init_root::<crate::messages_capnp::heartbeat_ack::Builder>();
    b.set_seq(msg.seq);
    b.set_robot_sent_time_ms(msg.robot_sent_time_ms);
    b.set_server_recv_time_ms(msg.server_recv_time_ms);

    let mut buf = Vec::new();
    serialize::write_message(&mut buf, &message)?;
    Ok(buf)
}

/// Deserialize a HeartbeatAck (server -> robot) from Cap'n Proto bytes.
pub fn deserialize_heartbeat_ack(data: &[u8]) -> Result<HeartbeatAck, ::capnp::Error> {
    let cursor = Cursor::new(data);
    let message_reader = serialize::read_message(cursor, ::capnp::message::ReaderOptions::new())?;
    let r = message_reader.get_root::<crate::messages_capnp::heartbeat_ack::Reader>()?;
    Ok(HeartbeatAck {
        seq: r.get_seq(),
        robot_sent_time_ms: r.get_robot_sent_time_ms(),
        server_recv_time_ms: r.get_server_recv_time_ms(),
    })
}

// Helper functions for writing structs to Cap'n Proto builders

fn uuid_from_capnp(bytes: &[u8]) -> Result<Uuid, ::capnp::Error> {
    Uuid::from_slice(bytes)
        .map_err(|e| ::capnp::Error::failed(format!("Invalid UUID bytes (expected 16): {}", e)))
}

fn write_ack_status(status: AckStatus) -> crate::messages_capnp::AckStatus {
    match status {
        AckStatus::Sent => crate::messages_capnp::AckStatus::Sent,
        AckStatus::AgentReceived => crate::messages_capnp::AckStatus::AgentReceived,
        AckStatus::AgentRejected => crate::messages_capnp::AckStatus::AgentRejected,
        AckStatus::RobotReceived => crate::messages_capnp::AckStatus::RobotReceived,
        AckStatus::RobotRejected => crate::messages_capnp::AckStatus::RobotRejected,
        AckStatus::RobotStateConfirmed => crate::messages_capnp::AckStatus::RobotStateConfirmed,
    }
}

fn read_ack_status(status: crate::messages_capnp::AckStatus) -> AckStatus {
    match status {
        crate::messages_capnp::AckStatus::Sent => AckStatus::Sent,
        crate::messages_capnp::AckStatus::AgentReceived => AckStatus::AgentReceived,
        crate::messages_capnp::AckStatus::AgentRejected => AckStatus::AgentRejected,
        crate::messages_capnp::AckStatus::RobotReceived => AckStatus::RobotReceived,
        crate::messages_capnp::AckStatus::RobotRejected => AckStatus::RobotRejected,
        crate::messages_capnp::AckStatus::RobotStateConfirmed => AckStatus::RobotStateConfirmed,
    }
}

fn write_robot_ack(builder: &mut crate::messages_capnp::robot_ack::Builder, ack: &RobotAck) {
    builder.set_command_id(ack.command_id.as_bytes());
    builder.set_status(write_ack_status(ack.status));
    builder.set_received_at_ms(ack.received_at_ms);
    if let Some(reason) = &ack.reason {
        builder.set_reason(reason);
        builder.set_has_reason(true);
    } else {
        builder.set_has_reason(false);
    }
}

fn read_robot_ack(
    reader: crate::messages_capnp::robot_ack::Reader,
) -> Result<RobotAck, ::capnp::Error> {
    Ok(RobotAck {
        command_id: uuid_from_capnp(reader.get_command_id()?)?,
        status: read_ack_status(reader.get_status()?),
        reason: if reader.get_has_reason() {
            Some(reader.get_reason()?.to_str()?.to_string())
        } else {
            None
        },
        received_at_ms: reader.get_received_at_ms(),
    })
}

fn write_robot_command(
    builder: &mut crate::messages_capnp::robot_command::Builder,
    cmd: &RobotCommand,
) -> Result<(), ::capnp::Error> {
    match cmd {
        RobotCommand::SetRobotMode(mode) => {
            builder.set_set_robot_mode(write_robot_mode(mode));
        }
        RobotCommand::Pause => {
            builder.set_pause(());
        }
        RobotCommand::Resume => {
            builder.set_resume(());
        }
        RobotCommand::AssignMission(mission_cmd) => {
            let mut mission_cmd_builder = builder.reborrow().init_assign_mission();
            write_mission_command(&mut mission_cmd_builder, mission_cmd);
        }
        RobotCommand::ControlMissionRun(control) => {
            let mut control_builder = builder.reborrow().init_control_mission_run();
            write_control_mission_run(&mut control_builder, control);
        }
        RobotCommand::InvokeService(call) => {
            let mut invoke_builder = builder.reborrow().init_invoke_service();
            write_service_call(&mut invoke_builder, call);
        }
        RobotCommand::UpdateSettings(settings) => {
            let mut settings_builder = builder
                .reborrow()
                .init_update_settings(settings.len() as u32);
            for (i, setting) in settings.iter().enumerate() {
                let mut setting_builder = settings_builder.reborrow().get(i as u32);
                setting_builder.set_key(&setting.key);
                let mut value_builder = setting_builder.init_value();
                write_value(&mut value_builder, &setting.value);
            }
        }
        RobotCommand::UpdateFeature(feature_op) => {
            let mut feature_builder = builder.reborrow().init_update_feature();
            write_feature_op(&mut feature_builder, feature_op);
        }
        RobotCommand::SpatialDirective(directive_op) => {
            let mut directive_builder = builder.reborrow().init_spatial_directive();
            write_spatial_directive_op(&mut directive_builder, directive_op);
        }
        RobotCommand::Object(obj_op) => {
            let mut obj_builder = builder.reborrow().init_object();
            write_object_op(&mut obj_builder, obj_op);
        }
        RobotCommand::NavigateTo(pose) => {
            let pose_builder = builder.reborrow().init_navigate_to();
            write_pose(pose_builder, pose);
        }
        RobotCommand::VelocityCmd(vel) => {
            let mut vel_builder = builder.reborrow().init_velocity_cmd();
            write_vel_twist(&mut vel_builder, vel);
        }
        RobotCommand::TeleopJoy(joy) => {
            let mut joy_builder = builder.reborrow().init_teleop_joy();
            write_teleop_joy(&mut joy_builder, joy);
        }
        RobotCommand::AgentTextRequest(req) => {
            let mut req_builder = builder.reborrow().init_agent_text_request();
            write_agent_text_request(&mut req_builder, req);
        }
        RobotCommand::SayText(req) => {
            let mut req_builder = builder.reborrow().init_say_text();
            write_say_text_request(&mut req_builder, req);
        }
        RobotCommand::MessageConfirmation(confirmation) => {
            let mut confirmation_builder = builder.reborrow().init_message_confirmation();
            write_message_confirmation(&mut confirmation_builder, confirmation);
        }
    }
    Ok(())
}

fn read_robot_command_from_reader(
    cmd_reader: crate::messages_capnp::robot_command::Reader,
) -> Result<RobotCommand, ::capnp::Error> {
    match cmd_reader.which()? {
        crate::messages_capnp::robot_command::SetRobotMode(Ok(reader)) => {
            Ok(RobotCommand::SetRobotMode(read_robot_mode(reader)?))
        }
        crate::messages_capnp::robot_command::Pause(()) => Ok(RobotCommand::Pause),
        crate::messages_capnp::robot_command::Resume(()) => Ok(RobotCommand::Resume),
        crate::messages_capnp::robot_command::AssignMission(Ok(reader)) => {
            Ok(RobotCommand::AssignMission(read_mission_command(reader)?))
        }
        crate::messages_capnp::robot_command::ControlMissionRun(Ok(reader)) => Ok(
            RobotCommand::ControlMissionRun(read_control_mission_run(reader)?),
        ),
        crate::messages_capnp::robot_command::InvokeService(Ok(reader)) => {
            Ok(RobotCommand::InvokeService(read_service_call(reader)?))
        }
        crate::messages_capnp::robot_command::UpdateSettings(Ok(settings_reader)) => {
            let mut settings = Vec::new();
            for i in 0..settings_reader.len() {
                let setting = settings_reader.get(i);
                settings.push(SettingUpdate {
                    key: setting.get_key()?.to_str()?.to_string(),
                    value: read_value(setting.get_value()?)?,
                });
            }
            Ok(RobotCommand::UpdateSettings(settings))
        }
        crate::messages_capnp::robot_command::UpdateFeature(Ok(reader)) => {
            Ok(RobotCommand::UpdateFeature(read_feature_op(reader)?))
        }
        crate::messages_capnp::robot_command::SpatialDirective(Ok(reader)) => Ok(
            RobotCommand::SpatialDirective(read_spatial_directive_op(reader)?),
        ),
        crate::messages_capnp::robot_command::Object(Ok(reader)) => {
            Ok(RobotCommand::Object(read_object_op(reader)?))
        }
        crate::messages_capnp::robot_command::NavigateTo(Ok(reader)) => {
            Ok(RobotCommand::NavigateTo(read_pose(reader)?))
        }
        crate::messages_capnp::robot_command::VelocityCmd(Ok(reader)) => {
            Ok(RobotCommand::VelocityCmd(read_vel_twist(reader)?))
        }
        crate::messages_capnp::robot_command::TeleopJoy(Ok(reader)) => {
            Ok(RobotCommand::TeleopJoy(read_teleop_joy(reader)?))
        }
        crate::messages_capnp::robot_command::AgentTextRequest(Ok(reader)) => {
            Ok(RobotCommand::AgentTextRequest(read_agent_text_request(reader)?))
        }
        crate::messages_capnp::robot_command::SayText(Ok(reader)) => {
            Ok(RobotCommand::SayText(read_say_text_request(reader)?))
        }
        crate::messages_capnp::robot_command::MessageConfirmation(Ok(reader)) => Ok(
            RobotCommand::MessageConfirmation(read_message_confirmation(reader)?),
        ),
        _ => Err(::capnp::Error::failed(
            "Unsupported robot command variant".to_string(),
        )),
    }
}

fn write_service_call(
    builder: &mut crate::messages_capnp::service_call::Builder,
    call: &ServiceCall,
) {
    builder.set_service_name(&call.service_name);
    if let Some(setting) = call.setting {
        builder.set_setting(setting);
        builder.set_has_setting(true);
    } else {
        builder.set_has_setting(false);
    }
}

fn read_service_call(
    reader: crate::messages_capnp::service_call::Reader,
) -> Result<ServiceCall, ::capnp::Error> {
    Ok(ServiceCall {
        service_name: reader.get_service_name()?.to_str()?.to_string(),
        setting: if reader.get_has_setting() {
            Some(reader.get_setting())
        } else {
            None
        },
    })
}

fn write_control_mission_run(
    builder: &mut crate::messages_capnp::control_mission_run::Builder,
    control: &ControlMissionRun,
) {
    builder.set_mission_id(&control.mission_id);
    builder.set_run_id(&control.run_id);
    builder.set_action(mission_run_action_to_capnp(control.action));
}

fn read_control_mission_run(
    reader: crate::messages_capnp::control_mission_run::Reader,
) -> Result<ControlMissionRun, ::capnp::Error> {
    Ok(ControlMissionRun {
        mission_id: reader.get_mission_id()?.to_str()?.to_string(),
        run_id: reader.get_run_id()?.to_str()?.to_string(),
        action: mission_run_action_from_capnp(reader.get_action()?),
    })
}

fn write_message_confirmation(
    builder: &mut crate::messages_capnp::message_confirmation::Builder,
    confirmation: &MessageConfirmation,
) {
    builder.set_confirmed(confirmation.confirmed);
    builder.set_confirmation_id(&confirmation.confirmation_id);
}

fn read_message_confirmation(
    reader: crate::messages_capnp::message_confirmation::Reader,
) -> Result<MessageConfirmation, ::capnp::Error> {
    Ok(MessageConfirmation {
        confirmed: reader.get_confirmed(),
        confirmation_id: reader.get_confirmation_id()?.to_str()?.to_string(),
    })
}

fn write_agent_text_request(
    builder: &mut crate::messages_capnp::agent_text_request::Builder,
    req: &AgentTextRequest,
) {
    builder.set_text(&req.text);
    builder.set_intent(agent_text_intent_to_capnp(&req.intent));
    builder.set_response_required(req.response_required);
}

fn read_agent_text_request(
    reader: crate::messages_capnp::agent_text_request::Reader,
) -> Result<AgentTextRequest, ::capnp::Error> {
    Ok(AgentTextRequest {
        text: reader.get_text()?.to_str()?.to_string(),
        intent: agent_text_intent_from_capnp(reader.get_intent()?),
        response_required: reader.get_response_required(),
    })
}

fn write_say_text_request(
    builder: &mut crate::messages_capnp::say_text_request::Builder,
    req: &SayTextRequest,
) {
    builder.set_text(&req.text);
    builder.set_priority(speech_priority_to_capnp(&req.priority));
}

fn read_say_text_request(
    reader: crate::messages_capnp::say_text_request::Reader,
) -> Result<SayTextRequest, ::capnp::Error> {
    Ok(SayTextRequest {
        text: reader.get_text()?.to_str()?.to_string(),
        priority: speech_priority_from_capnp(reader.get_priority()?),
    })
}

fn write_teleop_joy(builder: &mut crate::messages_capnp::teleop_joy::Builder, joy: &TeleopJoy) {
    builder.set_source_id(&joy.source_id);
    builder.set_session_id(&joy.session_id);
    builder.set_timestamp_ms(joy.timestamp_ms);
    let mut axes_builder = builder.reborrow().init_axes(joy.axes.len() as u32);
    for (i, axis) in joy.axes.iter().enumerate() {
        axes_builder.set(i as u32, *axis);
    }
    let mut buttons_builder = builder.reborrow().init_buttons(joy.buttons.len() as u32);
    for (i, button) in joy.buttons.iter().enumerate() {
        buttons_builder.set(i as u32, *button);
    }
}

fn read_teleop_joy(
    reader: crate::messages_capnp::teleop_joy::Reader,
) -> Result<TeleopJoy, ::capnp::Error> {
    let axes_reader = reader.get_axes()?;
    let mut axes = Vec::with_capacity(axes_reader.len() as usize);
    for i in 0..axes_reader.len() {
        axes.push(axes_reader.get(i));
    }
    let buttons_reader = reader.get_buttons()?;
    let mut buttons = Vec::with_capacity(buttons_reader.len() as usize);
    for i in 0..buttons_reader.len() {
        buttons.push(buttons_reader.get(i));
    }
    Ok(TeleopJoy {
        source_id: reader.get_source_id()?.to_str()?.to_string(),
        session_id: reader.get_session_id()?.to_str()?.to_string(),
        axes,
        buttons,
        timestamp_ms: reader.get_timestamp_ms(),
    })
}

fn write_mission_command(
    builder: &mut crate::messages_capnp::mission_command::Builder,
    cmd: &MissionCommand,
) {
    builder.set_mission_id(&cmd.mission_id);
    builder.set_run_id(&cmd.run_id);
    builder.set_name(&cmd.name);
    builder.set_user_id(&cmd.user_id);
    builder.set_detail(&cmd.detail);
    builder.set_scheduled_start_ms(cmd.scheduled_start_ms);
    if let Some(expected_end) = cmd.expected_end_time_ms {
        builder.set_expected_end_time_ms(expected_end);
        builder.set_has_expected_end_time_ms(true);
    } else {
        builder.set_has_expected_end_time_ms(false);
    }

    // Write mission_parameters
    let params = &cmd.mission_parameters;
    let mut params_builder = builder
        .reborrow()
        .init_mission_parameters(params.len() as u32);
    for (i, setting) in params.iter().enumerate() {
        let mut setting_builder = params_builder.reborrow().get(i as u32);
        setting_builder.set_key(&setting.key);
        let mut value_builder = setting_builder.init_value();
        write_value(&mut value_builder, &setting.value);
    }

    // Write capabilities
    let mut caps_builder = builder
        .reborrow()
        .init_capabilities(cmd.capabilities.len() as u32);
    for (i, cap) in cmd.capabilities.iter().enumerate() {
        let mut cap_builder = caps_builder.reborrow().get(i as u32);
        cap_builder.set_name(&cap.name);
        cap_builder.set_timing(capability_timing_to_capnp(&cap.timing));
        cap_builder.set_value(cap.value);
    }

    // Write optional feature (Feature)
    if let Some(ref feature) = cmd.feature {
        builder.set_has_feature(true);
        let mut feature_builder = builder.reborrow().init_feature();
        write_feature(&mut feature_builder, feature);
    } else {
        builder.set_has_feature(false);
    }
}

fn read_mission_command(
    reader: crate::messages_capnp::mission_command::Reader,
) -> Result<MissionCommand, ::capnp::Error> {
    // Read mission_parameters
    let params_reader = reader.get_mission_parameters()?;
    let mut mission_parameters = Vec::with_capacity(params_reader.len() as usize);
    for i in 0..params_reader.len() {
        let setting_reader = params_reader.get(i);
        mission_parameters.push(SettingUpdate {
            key: setting_reader.get_key()?.to_str()?.to_string(),
            value: read_value(setting_reader.get_value()?)?,
        });
    }

    // Read capabilities
    let caps_reader = reader.get_capabilities()?;
    let mut capabilities = Vec::with_capacity(caps_reader.len() as usize);
    for i in 0..caps_reader.len() {
        let cap_reader = caps_reader.get(i);
        capabilities.push(MissionCapability {
            name: cap_reader.get_name()?.to_str()?.to_string(),
            timing: capability_timing_from_capnp(cap_reader.get_timing()?),
            value: cap_reader.get_value(),
        });
    }

    // Read optional feature
    let feature = if reader.get_has_feature() {
        Some(read_feature(reader.get_feature()?)?)
    } else {
        None
    };

    Ok(MissionCommand {
        mission_id: reader.get_mission_id()?.to_str()?.to_string(),
        run_id: reader.get_run_id()?.to_str()?.to_string(),
        name: reader.get_name()?.to_str()?.to_string(),
        user_id: reader.get_user_id()?.to_str()?.to_string(),
        detail: reader.get_detail()?.to_str()?.to_string(),
        scheduled_start_ms: reader.get_scheduled_start_ms(),
        expected_end_time_ms: if reader.get_has_expected_end_time_ms() {
            Some(reader.get_expected_end_time_ms())
        } else {
            None
        },
        mission_parameters,
        capabilities,
        feature,
    })
}

pub fn write_spatial_directive(
    builder: &mut crate::messages_capnp::spatial_directive::Builder,
    directive: &SpatialDirective,
) {
    builder.set_directive_id(&directive.directive_id);
    builder.set_name(&directive.name);
    builder.set_start_time_ms(directive.start_time_ms);
    if let Some(expire) = directive.expire_time_ms {
        builder.set_expire_time_ms(expire);
        builder.set_has_expire_time_ms(true);
    } else {
        builder.set_has_expire_time_ms(false);
    }

    // Write zone_parameters
    let mut params_builder = builder
        .reborrow()
        .init_zone_parameters(directive.zone_parameters.len() as u32);
    for (i, setting) in directive.zone_parameters.iter().enumerate() {
        let mut setting_builder = params_builder.reborrow().get(i as u32);
        setting_builder.set_key(&setting.key);
        let mut value_builder = setting_builder.init_value();
        write_value(&mut value_builder, &setting.value);
    }

    // Write capabilities
    let mut caps_builder = builder
        .reborrow()
        .init_capabilities(directive.capabilities.len() as u32);
    for (i, cap) in directive.capabilities.iter().enumerate() {
        caps_builder.set(i as u32, cap);
    }

    // Write the spatial feature
    let mut spatial_builder = builder.reborrow().init_spatial();
    write_feature(&mut spatial_builder, &directive.spatial);
}

pub fn read_spatial_directive(
    reader: crate::messages_capnp::spatial_directive::Reader,
) -> Result<SpatialDirective, ::capnp::Error> {
    // Read zone_parameters
    let params_reader = reader.get_zone_parameters()?;
    let mut zone_parameters = Vec::with_capacity(params_reader.len() as usize);
    for i in 0..params_reader.len() {
        let setting_reader = params_reader.get(i);
        zone_parameters.push(SettingUpdate {
            key: setting_reader.get_key()?.to_str()?.to_string(),
            value: read_value(setting_reader.get_value()?)?,
        });
    }

    // Read capabilities
    let caps_reader = reader.get_capabilities()?;
    let mut capabilities = Vec::with_capacity(caps_reader.len() as usize);
    for i in 0..caps_reader.len() {
        capabilities.push(caps_reader.get(i)?.to_str()?.to_string());
    }

    Ok(SpatialDirective {
        directive_id: reader.get_directive_id()?.to_str()?.to_string(),
        name: reader.get_name()?.to_str()?.to_string(),
        start_time_ms: reader.get_start_time_ms(),
        expire_time_ms: if reader.get_has_expire_time_ms() {
            Some(reader.get_expire_time_ms())
        } else {
            None
        },
        zone_parameters,
        capabilities,
        spatial: read_feature(reader.get_spatial()?)?,
    })
}

fn write_asset_identity(
    builder: &mut crate::messages_capnp::asset_identity::Builder,
    identity: &AssetIdentity,
) {
    builder.set_id(&identity.id);
    builder.set_name(&identity.name);
    builder.set_rev(identity.rev);
    builder.set_hash(&identity.hash);
}

fn read_asset_identity(
    reader: crate::messages_capnp::asset_identity::Reader,
) -> Result<AssetIdentity, ::capnp::Error> {
    Ok(AssetIdentity {
        id: reader.get_id()?.to_str()?.to_string(),
        name: reader.get_name()?.to_str()?.to_string(),
        rev: reader.get_rev(),
        hash: reader.get_hash()?.to_str()?.to_string(),
    })
}

// Geometry helpers

fn write_geo_point(builder: &mut crate::messages_capnp::geo_point::Builder, pt: &GeoPoint) {
    builder.set_lon(pt.lon);
    builder.set_lat(pt.lat);
}

fn read_geo_point(
    reader: crate::messages_capnp::geo_point::Reader,
) -> Result<GeoPoint, ::capnp::Error> {
    Ok(GeoPoint {
        lon: reader.get_lon(),
        lat: reader.get_lat(),
    })
}

fn write_local_point(builder: &mut crate::messages_capnp::local_point::Builder, pt: &LocalPoint) {
    builder.set_x(pt.x);
    builder.set_y(pt.y);
}

fn read_local_point(
    reader: crate::messages_capnp::local_point::Reader,
) -> Result<LocalPoint, ::capnp::Error> {
    Ok(LocalPoint {
        x: reader.get_x(),
        y: reader.get_y(),
    })
}

fn write_geo_pose(builder: &mut crate::messages_capnp::geo_pose::Builder, pose: &GeoPose) {
    builder.set_lon_deg(pose.lon_deg);
    builder.set_lat_deg(pose.lat_deg);
    builder.set_altitude_m(pose.altitude_m);
    builder.set_heading_deg(pose.heading_deg);
}

fn read_geo_pose(reader: crate::messages_capnp::geo_pose::Reader) -> Result<GeoPose, ::capnp::Error> {
    Ok(GeoPose {
        lon_deg: reader.get_lon_deg(),
        lat_deg: reader.get_lat_deg(),
        altitude_m: reader.get_altitude_m(),
        heading_deg: reader.get_heading_deg(),
    })
}

fn write_local_pose(builder: &mut crate::messages_capnp::local_pose::Builder, pose: &LocalPose) {
    builder.set_x(pose.x);
    builder.set_y(pose.y);
    builder.set_altitude_m(pose.altitude_m);
    builder.set_heading_rad(pose.heading_rad);
}

fn read_local_pose(
    reader: crate::messages_capnp::local_pose::Reader,
) -> Result<LocalPose, ::capnp::Error> {
    Ok(LocalPose {
        x: reader.get_x(),
        y: reader.get_y(),
        altitude_m: reader.get_altitude_m(),
        heading_rad: reader.get_heading_rad(),
    })
}

fn write_pose(builder: crate::messages_capnp::pose::Builder, pose: &Pose) {
    match pose {
        Pose::GeoPose(gp) => {
            let mut gp_builder = builder.init_geo_pose();
            write_geo_pose(&mut gp_builder, gp);
        }
        Pose::LocalPose(lp) => {
            let mut lp_builder = builder.init_local_pose();
            write_local_pose(&mut lp_builder, lp);
        }
    }
}

fn read_pose(reader: crate::messages_capnp::pose::Reader) -> Result<Pose, ::capnp::Error> {
    match reader.which()? {
        crate::messages_capnp::pose::GeoPose(Ok(gp)) => Ok(Pose::GeoPose(read_geo_pose(gp)?)),
        crate::messages_capnp::pose::LocalPose(Ok(lp)) => Ok(Pose::LocalPose(read_local_pose(lp)?)),
        _ => Err(::capnp::Error::failed("Invalid pose variant".to_string())),
    }
}

fn write_vel_twist(builder: &mut crate::messages_capnp::vel_twist::Builder, vel: &VelTwist) {
    builder.set_forward_mps(vel.forward_mps);
    builder.set_angular_radps(vel.angular_radps);
}

fn read_vel_twist(
    reader: crate::messages_capnp::vel_twist::Reader,
) -> Result<VelTwist, ::capnp::Error> {
    Ok(VelTwist {
        forward_mps: reader.get_forward_mps(),
        angular_radps: reader.get_angular_radps(),
    })
}

fn write_trajectory(builder: &mut crate::messages_capnp::trajectory::Builder, traj: &Trajectory) {
    builder.set_speed_mps(traj.speed_mps);
    builder.set_course_deg(traj.course_deg);
    builder.set_vertical_rate_mps(traj.vertical_rate_mps);
}

fn read_trajectory(
    reader: crate::messages_capnp::trajectory::Reader,
) -> Result<Trajectory, ::capnp::Error> {
    Ok(Trajectory {
        speed_mps: reader.get_speed_mps(),
        course_deg: reader.get_course_deg(),
        vertical_rate_mps: reader.get_vertical_rate_mps(),
    })
}

pub fn write_feature(builder: &mut crate::messages_capnp::feature::Builder, feature: &Feature) {
    let mut identity_builder = builder.reborrow().init_identity();
    write_asset_identity(&mut identity_builder, &feature.identity);
    builder.set_layer_role(layer_role_to_capnp(feature.layer_role));
    builder.set_scope(scope_type_to_capnp(feature.scope));
    builder.set_expires_at_ms(feature.expires_at_ms);
    builder.set_min_z_m(feature.min_z_m);
    builder.set_max_z_m(feature.max_z_m);
    builder.set_level_id(feature.level_id);

    let data_builder = builder.reborrow().init_data();
    write_spatial_data(data_builder, &feature.data);
}

pub fn read_feature(
    reader: crate::messages_capnp::feature::Reader,
) -> Result<Feature, ::capnp::Error> {
    Ok(Feature {
        identity: read_asset_identity(reader.get_identity()?)?,
        layer_role: layer_role_from_capnp(reader.get_layer_role()?),
        scope: scope_type_from_capnp(reader.get_scope()?),
        data: read_spatial_data(reader.get_data()?)?,
        expires_at_ms: reader.get_expires_at_ms(),
        min_z_m: reader.get_min_z_m(),
        max_z_m: reader.get_max_z_m(),
        level_id: reader.get_level_id(),
    })
}

fn write_reported_feature(
    builder: &mut crate::messages_capnp::reported_feature::Builder,
    feature: &ReportedFeature,
) {
    builder.set_name(&feature.name);
    builder.set_layer_role(layer_role_to_capnp(feature.layer_role));
    builder.set_scope(scope_type_to_capnp(feature.scope));
    builder.set_expires_at_ms(feature.expires_at_ms);
    if let Some(min_z) = feature.min_z_m {
        builder.set_min_z_m(min_z);
        builder.set_has_min_z_m(true);
    } else {
        builder.set_has_min_z_m(false);
    }
    if let Some(max_z) = feature.max_z_m {
        builder.set_max_z_m(max_z);
        builder.set_has_max_z_m(true);
    } else {
        builder.set_has_max_z_m(false);
    }
    if let Some(level_id) = feature.level_id {
        builder.set_level_id(level_id);
        builder.set_has_level_id(true);
    } else {
        builder.set_has_level_id(false);
    }

    let data_builder = builder.reborrow().init_data();
    write_spatial_data(data_builder, &feature.data);
}

fn read_reported_feature(
    reader: crate::messages_capnp::reported_feature::Reader,
) -> Result<ReportedFeature, ::capnp::Error> {
    Ok(ReportedFeature {
        name: reader.get_name()?.to_str()?.to_string(),
        layer_role: layer_role_from_capnp(reader.get_layer_role()?),
        scope: scope_type_from_capnp(reader.get_scope()?),
        data: read_spatial_data(reader.get_data()?)?,
        expires_at_ms: reader.get_expires_at_ms(),
        min_z_m: if reader.get_has_min_z_m() {
            Some(reader.get_min_z_m())
        } else {
            None
        },
        max_z_m: if reader.get_has_max_z_m() {
            Some(reader.get_max_z_m())
        } else {
            None
        },
        level_id: if reader.get_has_level_id() {
            Some(reader.get_level_id())
        } else {
            None
        },
    })
}

fn write_spatial_data(builder: crate::messages_capnp::spatial_data::Builder, data: &SpatialData) {
    match data {
        SpatialData::Wgs84Shape(shape) => {
            write_wgs84_shape(builder.init_wgs84_shape(), shape);
        }
        SpatialData::Wgs84Route(route) => {
            write_wgs84_route(builder.init_wgs84_route(), route);
        }
        SpatialData::LocalShape(shape) => {
            write_local_shape(builder.init_local_shape(), shape);
        }
        SpatialData::LocalRoute(route) => {
            write_local_route(builder.init_local_route(), route);
        }
    }
}

fn read_spatial_data(
    reader: crate::messages_capnp::spatial_data::Reader,
) -> Result<SpatialData, ::capnp::Error> {
    match reader.which()? {
        crate::messages_capnp::spatial_data::Wgs84Shape(Ok(r)) => {
            Ok(SpatialData::Wgs84Shape(read_wgs84_shape(r)?))
        }
        crate::messages_capnp::spatial_data::Wgs84Route(Ok(r)) => {
            Ok(SpatialData::Wgs84Route(read_wgs84_route(r)?))
        }
        crate::messages_capnp::spatial_data::LocalShape(Ok(r)) => {
            Ok(SpatialData::LocalShape(read_local_shape(r)?))
        }
        crate::messages_capnp::spatial_data::LocalRoute(Ok(r)) => {
            Ok(SpatialData::LocalRoute(read_local_route(r)?))
        }
        _ => Err(::capnp::Error::failed(
            "Invalid spatial data variant".to_string(),
        )),
    }
}

fn write_wgs84_polygon_data(
    builder: &mut crate::messages_capnp::wgs84_polygon_data::Builder,
    poly: &Wgs84PolygonData,
) {
    let mut ext_builder = builder.reborrow().init_exterior(poly.exterior.len() as u32);
    for (i, pt) in poly.exterior.iter().enumerate() {
        let mut pt_builder = ext_builder.reborrow().get(i as u32);
        write_geo_point(&mut pt_builder, pt);
    }
    let mut holes_builder = builder.reborrow().init_holes(poly.holes.len() as u32);
    for (i, hole) in poly.holes.iter().enumerate() {
        let mut hole_builder = holes_builder.reborrow().init(i as u32, hole.len() as u32);
        for (j, pt) in hole.iter().enumerate() {
            let mut pt_builder = hole_builder.reborrow().get(j as u32);
            write_geo_point(&mut pt_builder, pt);
        }
    }
}

fn read_wgs84_polygon_data(
    reader: crate::messages_capnp::wgs84_polygon_data::Reader,
) -> Result<Wgs84PolygonData, ::capnp::Error> {
    let ext_reader = reader.get_exterior()?;
    let mut exterior = Vec::with_capacity(ext_reader.len() as usize);
    for i in 0..ext_reader.len() {
        exterior.push(read_geo_point(ext_reader.get(i))?);
    }
    let holes_reader = reader.get_holes()?;
    let mut holes = Vec::with_capacity(holes_reader.len() as usize);
    for i in 0..holes_reader.len() {
        let hole = holes_reader.get(i)?;
        let mut hole_pts = Vec::with_capacity(hole.len() as usize);
        for j in 0..hole.len() {
            hole_pts.push(read_geo_point(hole.get(j))?);
        }
        holes.push(hole_pts);
    }
    Ok(Wgs84PolygonData { exterior, holes })
}

fn write_local_polygon_data(
    builder: &mut crate::messages_capnp::local_polygon_data::Builder,
    poly: &LocalPolygonData,
) {
    let mut ext_builder = builder.reborrow().init_exterior(poly.exterior.len() as u32);
    for (i, pt) in poly.exterior.iter().enumerate() {
        let mut pt_builder = ext_builder.reborrow().get(i as u32);
        write_local_point(&mut pt_builder, pt);
    }
    let mut holes_builder = builder.reborrow().init_holes(poly.holes.len() as u32);
    for (i, hole) in poly.holes.iter().enumerate() {
        let mut hole_builder = holes_builder.reborrow().init(i as u32, hole.len() as u32);
        for (j, pt) in hole.iter().enumerate() {
            let mut pt_builder = hole_builder.reborrow().get(j as u32);
            write_local_point(&mut pt_builder, pt);
        }
    }
}

fn read_local_polygon_data(
    reader: crate::messages_capnp::local_polygon_data::Reader,
) -> Result<LocalPolygonData, ::capnp::Error> {
    let ext_reader = reader.get_exterior()?;
    let mut exterior = Vec::with_capacity(ext_reader.len() as usize);
    for i in 0..ext_reader.len() {
        exterior.push(read_local_point(ext_reader.get(i))?);
    }
    let holes_reader = reader.get_holes()?;
    let mut holes = Vec::with_capacity(holes_reader.len() as usize);
    for i in 0..holes_reader.len() {
        let hole = holes_reader.get(i)?;
        let mut hole_pts = Vec::with_capacity(hole.len() as usize);
        for j in 0..hole.len() {
            hole_pts.push(read_local_point(hole.get(j))?);
        }
        holes.push(hole_pts);
    }
    Ok(LocalPolygonData { exterior, holes })
}

fn write_wgs84_shape(builder: crate::messages_capnp::wgs84_shape::Builder, shape: &Wgs84Shape) {
    match shape {
        Wgs84Shape::Point(pt) => {
            let mut pt_builder = builder.init_point();
            write_geo_point(&mut pt_builder, pt);
        }
        Wgs84Shape::MultiPoint(pts) => {
            let mut list_builder = builder.init_multi_point(pts.len() as u32);
            for (i, pt) in pts.iter().enumerate() {
                let mut pt_builder = list_builder.reborrow().get(i as u32);
                write_geo_point(&mut pt_builder, pt);
            }
        }
        Wgs84Shape::LineString(pts) => {
            let mut list_builder = builder.init_line_string(pts.len() as u32);
            for (i, pt) in pts.iter().enumerate() {
                let mut pt_builder = list_builder.reborrow().get(i as u32);
                write_geo_point(&mut pt_builder, pt);
            }
        }
        Wgs84Shape::MultiLineString(lines) => {
            let mut list_builder = builder.init_multi_line_string(lines.len() as u32);
            for (i, line) in lines.iter().enumerate() {
                let mut line_builder = list_builder.reborrow().init(i as u32, line.len() as u32);
                for (j, pt) in line.iter().enumerate() {
                    let mut pt_builder = line_builder.reborrow().get(j as u32);
                    write_geo_point(&mut pt_builder, pt);
                }
            }
        }
        Wgs84Shape::Polygon(poly) => {
            let mut poly_builder = builder.init_polygon();
            write_wgs84_polygon_data(&mut poly_builder, poly);
        }
        Wgs84Shape::MultiPolygon(polygons) => {
            let mut list_builder = builder.init_multi_polygon(polygons.len() as u32);
            for (i, poly) in polygons.iter().enumerate() {
                let mut poly_builder = list_builder.reborrow().get(i as u32);
                write_wgs84_polygon_data(&mut poly_builder, poly);
            }
        }
    }
}

fn read_wgs84_shape(
    reader: crate::messages_capnp::wgs84_shape::Reader,
) -> Result<Wgs84Shape, ::capnp::Error> {
    match reader.which()? {
        crate::messages_capnp::wgs84_shape::Point(Ok(pt)) => {
            Ok(Wgs84Shape::Point(read_geo_point(pt)?))
        }
        crate::messages_capnp::wgs84_shape::MultiPoint(Ok(pts)) => {
            let mut result = Vec::with_capacity(pts.len() as usize);
            for i in 0..pts.len() {
                result.push(read_geo_point(pts.get(i))?);
            }
            Ok(Wgs84Shape::MultiPoint(result))
        }
        crate::messages_capnp::wgs84_shape::LineString(Ok(pts)) => {
            let mut result = Vec::with_capacity(pts.len() as usize);
            for i in 0..pts.len() {
                result.push(read_geo_point(pts.get(i))?);
            }
            Ok(Wgs84Shape::LineString(result))
        }
        crate::messages_capnp::wgs84_shape::MultiLineString(Ok(lines)) => {
            let mut result = Vec::with_capacity(lines.len() as usize);
            for i in 0..lines.len() {
                let line = lines.get(i)?;
                let mut line_pts = Vec::with_capacity(line.len() as usize);
                for j in 0..line.len() {
                    line_pts.push(read_geo_point(line.get(j))?);
                }
                result.push(line_pts);
            }
            Ok(Wgs84Shape::MultiLineString(result))
        }
        crate::messages_capnp::wgs84_shape::Polygon(Ok(poly)) => {
            Ok(Wgs84Shape::Polygon(read_wgs84_polygon_data(poly)?))
        }
        crate::messages_capnp::wgs84_shape::MultiPolygon(Ok(polys)) => {
            let mut result = Vec::with_capacity(polys.len() as usize);
            for i in 0..polys.len() {
                result.push(read_wgs84_polygon_data(polys.get(i))?);
            }
            Ok(Wgs84Shape::MultiPolygon(result))
        }
        _ => Err(::capnp::Error::failed(
            "Invalid wgs84 shape variant".to_string(),
        )),
    }
}

fn write_local_shape(builder: crate::messages_capnp::local_shape::Builder, shape: &LocalShape) {
    match shape {
        LocalShape::Point(pt) => {
            let mut pt_builder = builder.init_point();
            write_local_point(&mut pt_builder, pt);
        }
        LocalShape::MultiPoint(pts) => {
            let mut list_builder = builder.init_multi_point(pts.len() as u32);
            for (i, pt) in pts.iter().enumerate() {
                let mut pt_builder = list_builder.reborrow().get(i as u32);
                write_local_point(&mut pt_builder, pt);
            }
        }
        LocalShape::LineString(pts) => {
            let mut list_builder = builder.init_line_string(pts.len() as u32);
            for (i, pt) in pts.iter().enumerate() {
                let mut pt_builder = list_builder.reborrow().get(i as u32);
                write_local_point(&mut pt_builder, pt);
            }
        }
        LocalShape::MultiLineString(lines) => {
            let mut list_builder = builder.init_multi_line_string(lines.len() as u32);
            for (i, line) in lines.iter().enumerate() {
                let mut line_builder = list_builder.reborrow().init(i as u32, line.len() as u32);
                for (j, pt) in line.iter().enumerate() {
                    let mut pt_builder = line_builder.reborrow().get(j as u32);
                    write_local_point(&mut pt_builder, pt);
                }
            }
        }
        LocalShape::Polygon(poly) => {
            let mut poly_builder = builder.init_polygon();
            write_local_polygon_data(&mut poly_builder, poly);
        }
        LocalShape::MultiPolygon(polygons) => {
            let mut list_builder = builder.init_multi_polygon(polygons.len() as u32);
            for (i, poly) in polygons.iter().enumerate() {
                let mut poly_builder = list_builder.reborrow().get(i as u32);
                write_local_polygon_data(&mut poly_builder, poly);
            }
        }
    }
}

fn read_local_shape(
    reader: crate::messages_capnp::local_shape::Reader,
) -> Result<LocalShape, ::capnp::Error> {
    match reader.which()? {
        crate::messages_capnp::local_shape::Point(Ok(pt)) => {
            Ok(LocalShape::Point(read_local_point(pt)?))
        }
        crate::messages_capnp::local_shape::MultiPoint(Ok(pts)) => {
            let mut result = Vec::with_capacity(pts.len() as usize);
            for i in 0..pts.len() {
                result.push(read_local_point(pts.get(i))?);
            }
            Ok(LocalShape::MultiPoint(result))
        }
        crate::messages_capnp::local_shape::LineString(Ok(pts)) => {
            let mut result = Vec::with_capacity(pts.len() as usize);
            for i in 0..pts.len() {
                result.push(read_local_point(pts.get(i))?);
            }
            Ok(LocalShape::LineString(result))
        }
        crate::messages_capnp::local_shape::MultiLineString(Ok(lines)) => {
            let mut result = Vec::with_capacity(lines.len() as usize);
            for i in 0..lines.len() {
                let line = lines.get(i)?;
                let mut line_pts = Vec::with_capacity(line.len() as usize);
                for j in 0..line.len() {
                    line_pts.push(read_local_point(line.get(j))?);
                }
                result.push(line_pts);
            }
            Ok(LocalShape::MultiLineString(result))
        }
        crate::messages_capnp::local_shape::Polygon(Ok(poly)) => {
            Ok(LocalShape::Polygon(read_local_polygon_data(poly)?))
        }
        crate::messages_capnp::local_shape::MultiPolygon(Ok(polys)) => {
            let mut result = Vec::with_capacity(polys.len() as usize);
            for i in 0..polys.len() {
                result.push(read_local_polygon_data(polys.get(i))?);
            }
            Ok(LocalShape::MultiPolygon(result))
        }
        _ => Err(::capnp::Error::failed(
            "Invalid local shape variant".to_string(),
        )),
    }
}

fn write_wgs84_route(builder: crate::messages_capnp::wgs84_route::Builder, route: &Wgs84Route) {
    match route {
        Wgs84Route::Waypoint(pose) => {
            let mut pose_builder = builder.init_waypoint();
            write_geo_pose(&mut pose_builder, pose);
        }
        Wgs84Route::MultiWaypoint(poses) => {
            let mut list_builder = builder.init_multi_waypoint(poses.len() as u32);
            for (i, pose) in poses.iter().enumerate() {
                let mut pose_builder = list_builder.reborrow().get(i as u32);
                write_geo_pose(&mut pose_builder, pose);
            }
        }
        Wgs84Route::Path(poses) => {
            let mut list_builder = builder.init_path(poses.len() as u32);
            for (i, pose) in poses.iter().enumerate() {
                let mut pose_builder = list_builder.reborrow().get(i as u32);
                write_geo_pose(&mut pose_builder, pose);
            }
        }
        Wgs84Route::MultiPath(paths) => {
            let mut list_builder = builder.init_multi_path(paths.len() as u32);
            for (i, path) in paths.iter().enumerate() {
                let mut path_builder = list_builder.reborrow().init(i as u32, path.len() as u32);
                for (j, pose) in path.iter().enumerate() {
                    let mut pose_builder = path_builder.reborrow().get(j as u32);
                    write_geo_pose(&mut pose_builder, pose);
                }
            }
        }
    }
}

fn read_wgs84_route(
    reader: crate::messages_capnp::wgs84_route::Reader,
) -> Result<Wgs84Route, ::capnp::Error> {
    match reader.which()? {
        crate::messages_capnp::wgs84_route::Waypoint(Ok(pose)) => {
            Ok(Wgs84Route::Waypoint(read_geo_pose(pose)?))
        }
        crate::messages_capnp::wgs84_route::MultiWaypoint(Ok(poses)) => {
            let mut result = Vec::with_capacity(poses.len() as usize);
            for i in 0..poses.len() {
                result.push(read_geo_pose(poses.get(i))?);
            }
            Ok(Wgs84Route::MultiWaypoint(result))
        }
        crate::messages_capnp::wgs84_route::Path(Ok(poses)) => {
            let mut result = Vec::with_capacity(poses.len() as usize);
            for i in 0..poses.len() {
                result.push(read_geo_pose(poses.get(i))?);
            }
            Ok(Wgs84Route::Path(result))
        }
        crate::messages_capnp::wgs84_route::MultiPath(Ok(paths)) => {
            let mut result = Vec::with_capacity(paths.len() as usize);
            for i in 0..paths.len() {
                let path = paths.get(i)?;
                let mut path_poses = Vec::with_capacity(path.len() as usize);
                for j in 0..path.len() {
                    path_poses.push(read_geo_pose(path.get(j))?);
                }
                result.push(path_poses);
            }
            Ok(Wgs84Route::MultiPath(result))
        }
        _ => Err(::capnp::Error::failed(
            "Invalid wgs84 route variant".to_string(),
        )),
    }
}

fn write_local_route(builder: crate::messages_capnp::local_route::Builder, route: &LocalRoute) {
    match route {
        LocalRoute::Waypoint(pose) => {
            let mut pose_builder = builder.init_waypoint();
            write_local_pose(&mut pose_builder, pose);
        }
        LocalRoute::MultiWaypoint(poses) => {
            let mut list_builder = builder.init_multi_waypoint(poses.len() as u32);
            for (i, pose) in poses.iter().enumerate() {
                let mut pose_builder = list_builder.reborrow().get(i as u32);
                write_local_pose(&mut pose_builder, pose);
            }
        }
        LocalRoute::Path(poses) => {
            let mut list_builder = builder.init_path(poses.len() as u32);
            for (i, pose) in poses.iter().enumerate() {
                let mut pose_builder = list_builder.reborrow().get(i as u32);
                write_local_pose(&mut pose_builder, pose);
            }
        }
        LocalRoute::MultiPath(paths) => {
            let mut list_builder = builder.init_multi_path(paths.len() as u32);
            for (i, path) in paths.iter().enumerate() {
                let mut path_builder = list_builder.reborrow().init(i as u32, path.len() as u32);
                for (j, pose) in path.iter().enumerate() {
                    let mut pose_builder = path_builder.reborrow().get(j as u32);
                    write_local_pose(&mut pose_builder, pose);
                }
            }
        }
    }
}

fn read_local_route(
    reader: crate::messages_capnp::local_route::Reader,
) -> Result<LocalRoute, ::capnp::Error> {
    match reader.which()? {
        crate::messages_capnp::local_route::Waypoint(Ok(pose)) => {
            Ok(LocalRoute::Waypoint(read_local_pose(pose)?))
        }
        crate::messages_capnp::local_route::MultiWaypoint(Ok(poses)) => {
            let mut result = Vec::with_capacity(poses.len() as usize);
            for i in 0..poses.len() {
                result.push(read_local_pose(poses.get(i))?);
            }
            Ok(LocalRoute::MultiWaypoint(result))
        }
        crate::messages_capnp::local_route::Path(Ok(poses)) => {
            let mut result = Vec::with_capacity(poses.len() as usize);
            for i in 0..poses.len() {
                result.push(read_local_pose(poses.get(i))?);
            }
            Ok(LocalRoute::Path(result))
        }
        crate::messages_capnp::local_route::MultiPath(Ok(paths)) => {
            let mut result = Vec::with_capacity(paths.len() as usize);
            for i in 0..paths.len() {
                let path = paths.get(i)?;
                let mut path_poses = Vec::with_capacity(path.len() as usize);
                for j in 0..path.len() {
                    path_poses.push(read_local_pose(path.get(j))?);
                }
                result.push(path_poses);
            }
            Ok(LocalRoute::MultiPath(result))
        }
        _ => Err(::capnp::Error::failed(
            "Invalid local route variant".to_string(),
        )),
    }
}

fn write_object(builder: &mut crate::messages_capnp::object::Builder, obj: &Object) {
    let mut identity_builder = builder.reborrow().init_identity();
    write_asset_identity(&mut identity_builder, &obj.identity);
    builder.set_unix_time_ms(obj.unix_time_ms);
    builder.set_expires_at_ms(obj.expires_at_ms);
    builder.set_scope(scope_type_to_capnp(obj.scope));
    builder.set_object_type(object_type_to_capnp(obj.object_type));
    builder.set_radius_m(obj.radius_m);

    let pose_builder = builder.reborrow().init_pose();
    write_pose(pose_builder, &obj.pose);

    let mut trajectory_builder = builder.reborrow().init_trajectory();
    write_trajectory(&mut trajectory_builder, &obj.trajectory);
}

fn read_object(reader: crate::messages_capnp::object::Reader) -> Result<Object, ::capnp::Error> {
    Ok(Object {
        identity: read_asset_identity(reader.get_identity()?)?,
        unix_time_ms: reader.get_unix_time_ms(),
        expires_at_ms: reader.get_expires_at_ms(),
        scope: scope_type_from_capnp(reader.get_scope()?),
        object_type: object_type_from_capnp(reader.get_object_type()?),
        pose: read_pose(reader.get_pose()?)?,
        trajectory: read_trajectory(reader.get_trajectory()?)?,
        radius_m: reader.get_radius_m(),
    })
}

// Op type write functions

fn write_feature_op(
    builder: &mut crate::messages_capnp::feature_op::Builder,
    feature_op: &FeatureOp,
) {
    match feature_op {
        FeatureOp::Upsert(feature) => {
            let mut feature_builder = builder.reborrow().init_upsert();
            write_feature(&mut feature_builder, feature);
        }
        FeatureOp::Delete { id } => {
            builder.set_delete(id);
        }
    }
}

fn write_spatial_directive_op(
    builder: &mut crate::messages_capnp::spatial_directive_op::Builder,
    op: &SpatialDirectiveOp,
) {
    match op {
        SpatialDirectiveOp::Upsert(directive) => {
            let mut directive_builder = builder.reborrow().init_upsert();
            write_spatial_directive(&mut directive_builder, directive);
        }
        SpatialDirectiveOp::Delete { id } => {
            builder.set_delete(id);
        }
    }
}

fn write_reported_feature_update(
    builder: &mut crate::messages_capnp::reported_feature_update::Builder,
    update: &ReportedFeatureUpdate,
) {
    builder.set_resource_producer_id(&update.resource_producer_id);
    builder.set_timestamp_ms(update.timestamp_ms);
    let mut op_builder = builder.reborrow().init_op();
    write_reported_feature_op(&mut op_builder, &update.op);
}

fn write_reported_feature_op(
    builder: &mut crate::messages_capnp::reported_feature_op::Builder,
    feature_op: &ReportedFeatureOp,
) {
    match feature_op {
        ReportedFeatureOp::Upsert(feature) => {
            let mut feature_builder = builder.reborrow().init_upsert();
            write_reported_feature(&mut feature_builder, feature);
        }
        ReportedFeatureOp::Clear => {
            builder.set_clear(());
        }
    }
}

fn write_object_op(builder: &mut crate::messages_capnp::object_op::Builder, obj_op: &ObjectOp) {
    match obj_op {
        ObjectOp::Upsert(obj) => {
            let mut obj_builder = builder.reborrow().init_upsert();
            write_object(&mut obj_builder, obj);
        }
        ObjectOp::Delete { id } => {
            builder.set_delete(id);
        }
    }
}

// Op type read functions

fn read_feature_op(
    reader: crate::messages_capnp::feature_op::Reader,
) -> Result<FeatureOp, ::capnp::Error> {
    match reader.which()? {
        crate::messages_capnp::feature_op::Upsert(Ok(feature_reader)) => {
            Ok(FeatureOp::Upsert(read_feature(feature_reader)?))
        }
        crate::messages_capnp::feature_op::Delete(Ok(id)) => Ok(FeatureOp::Delete {
            id: id.to_str()?.to_string(),
        }),
        _ => Err(::capnp::Error::failed(
            "Invalid feature op variant".to_string(),
        )),
    }
}

fn read_spatial_directive_op(
    reader: crate::messages_capnp::spatial_directive_op::Reader,
) -> Result<SpatialDirectiveOp, ::capnp::Error> {
    match reader.which()? {
        crate::messages_capnp::spatial_directive_op::Upsert(Ok(directive_reader)) => Ok(
            SpatialDirectiveOp::Upsert(read_spatial_directive(directive_reader)?),
        ),
        crate::messages_capnp::spatial_directive_op::Delete(Ok(id)) => {
            Ok(SpatialDirectiveOp::Delete {
                id: id.to_str()?.to_string(),
            })
        }
        _ => Err(::capnp::Error::failed(
            "Invalid spatial directive op variant".to_string(),
        )),
    }
}

fn read_reported_feature_update(
    reader: crate::messages_capnp::reported_feature_update::Reader,
) -> Result<ReportedFeatureUpdate, ::capnp::Error> {
    Ok(ReportedFeatureUpdate {
        resource_producer_id: reader.get_resource_producer_id()?.to_str()?.to_string(),
        timestamp_ms: reader.get_timestamp_ms(),
        op: read_reported_feature_op(reader.get_op()?)?,
    })
}

fn read_reported_feature_op(
    reader: crate::messages_capnp::reported_feature_op::Reader,
) -> Result<ReportedFeatureOp, ::capnp::Error> {
    match reader.which()? {
        crate::messages_capnp::reported_feature_op::Upsert(Ok(feature_reader)) => {
            Ok(ReportedFeatureOp::Upsert(read_reported_feature(feature_reader)?))
        }
        crate::messages_capnp::reported_feature_op::Clear(()) => Ok(ReportedFeatureOp::Clear),
        _ => Err(::capnp::Error::failed(
            "Invalid reported feature op variant".to_string(),
        )),
    }
}

fn read_object_op(
    reader: crate::messages_capnp::object_op::Reader,
) -> Result<ObjectOp, ::capnp::Error> {
    match reader.which()? {
        crate::messages_capnp::object_op::Upsert(Ok(obj_reader)) => {
            Ok(ObjectOp::Upsert(read_object(obj_reader)?))
        }
        crate::messages_capnp::object_op::Delete(Ok(id)) => Ok(ObjectOp::Delete {
            id: id.to_str()?.to_string(),
        }),
        _ => Err(::capnp::Error::failed(
            "Invalid object op variant".to_string(),
        )),
    }
}

fn write_global_motion_telemetry(
    builder: &mut crate::messages_capnp::global_motion_telemetry::Builder,
    telem: &GlobalMotionTelemetry,
) {
    builder.set_unix_time_ms(telem.unix_time_ms);
    builder.set_gps_source(write_gps_source(telem.gps_source));

    if let Some(pose) = &telem.pose {
        builder.set_has_pose(true);
        let mut pose_builder = builder.reborrow().init_pose();
        write_geo_pose(&mut pose_builder, pose);
    } else {
        builder.set_has_pose(false);
    }

    if let Some(vel) = &telem.velocity {
        builder.set_has_velocity(true);
        let mut vel_builder = builder.reborrow().init_velocity();
        write_vel_twist(&mut vel_builder, vel);
    } else {
        builder.set_has_velocity(false);
    }

    if let Some(fix_type) = telem.gps_fix {
        builder.set_has_gps_fix_type(true);
        builder.set_gps_fix_type(gps_fix_type_to_capnp(fix_type));
    } else {
        builder.set_has_gps_fix_type(false);
    }
}

fn read_global_motion_telemetry(
    reader: crate::messages_capnp::global_motion_telemetry::Reader,
) -> Result<GlobalMotionTelemetry, ::capnp::Error> {
    Ok(GlobalMotionTelemetry {
        unix_time_ms: reader.get_unix_time_ms(),
        pose: if reader.get_has_pose() {
            Some(read_geo_pose(reader.get_pose()?)?)
        } else {
            None
        },
        velocity: if reader.get_has_velocity() {
            Some(read_vel_twist(reader.get_velocity()?)?)
        } else {
            None
        },
        gps_fix: if reader.get_has_gps_fix_type() {
            Some(gps_fix_type_from_capnp(reader.get_gps_fix_type()?))
        } else {
            None
        },
        gps_source: read_gps_source(reader.get_gps_source()?),
    })
}

fn write_local_motion_telemetry(
    builder: &mut crate::messages_capnp::local_motion_telemetry::Builder,
    telem: &LocalMotionTelemetry,
) {
    builder.set_unix_time_ms(telem.unix_time_ms);

    let mut pose_builder = builder.reborrow().init_pose();
    write_local_pose(&mut pose_builder, &telem.pose);

    let mut vel_builder = builder.reborrow().init_velocity();
    write_vel_twist(&mut vel_builder, &telem.velocity);

    if let Some(accuracy) = telem.accuracy_m {
        builder.set_accuracy_m(accuracy);
        builder.set_has_accuracy_m(true);
    } else {
        builder.set_has_accuracy_m(false);
    }

    if let Some(ref frame) = telem.frame {
        builder.set_frame(frame);
        builder.set_has_frame(true);
    } else {
        builder.set_has_frame(false);
    }
}

fn read_local_motion_telemetry(
    reader: crate::messages_capnp::local_motion_telemetry::Reader,
) -> Result<LocalMotionTelemetry, ::capnp::Error> {
    Ok(LocalMotionTelemetry {
        unix_time_ms: reader.get_unix_time_ms(),
        pose: read_local_pose(reader.get_pose()?)?,
        velocity: read_vel_twist(reader.get_velocity()?)?,
        accuracy_m: if reader.get_has_accuracy_m() {
            Some(reader.get_accuracy_m())
        } else {
            None
        },
        frame: if reader.get_has_frame() {
            Some(reader.get_frame()?.to_str()?.to_string())
        } else {
            None
        },
    })
}

fn write_status_telemetry(
    builder: &mut crate::messages_capnp::status_telemetry::Builder,
    telem: &StatusTelemetry,
) {
    builder.set_unix_time_ms(telem.unix_time_ms);
    builder.set_faulted(telem.faulted);
    builder.set_mission_active(telem.accepting_missions);
    builder.set_status(&telem.status);

    let mut estop_builder = builder.reborrow().init_estop();
    write_estop(&mut estop_builder, &telem.estop);

    if let Some(battery) = &telem.battery {
        builder.set_has_battery(true);
        let mut battery_builder = builder.reborrow().init_battery();
        write_battery_status(&mut battery_builder, battery);
    } else {
        builder.set_has_battery(false);
    }

    if let Some(fuel) = &telem.fuel {
        builder.set_has_fuel(true);
        let mut fuel_builder = builder.reborrow().init_fuel();
        write_fuel_status(&mut fuel_builder, fuel);
    } else {
        builder.set_has_fuel(false);
    }

    if let Some(range) = telem.range_remaining_m {
        builder.set_has_range_remaining(true);
        builder.set_range_remaining(range);
    } else {
        builder.set_has_range_remaining(false);
    }

    if let Some(runtime) = telem.runtime_remaining_s {
        builder.set_has_runtime_remaining(true);
        builder.set_runtime_remaining(runtime);
    } else {
        builder.set_has_runtime_remaining(false);
    }

    if let Some(mode) = &telem.mode {
        builder.set_has_mode(true);
        let mode_capnp = write_robot_mode(mode);
        builder.set_mode(mode_capnp);
    } else {
        builder.set_has_mode(false);
    }
}

fn read_status_telemetry(
    reader: crate::messages_capnp::status_telemetry::Reader,
) -> Result<StatusTelemetry, ::capnp::Error> {
    Ok(StatusTelemetry {
        unix_time_ms: reader.get_unix_time_ms(),
        battery: if reader.get_has_battery() {
            Some(read_battery_status(reader.get_battery()?)?)
        } else {
            None
        },
        fuel: if reader.get_has_fuel() {
            Some(read_fuel_status(reader.get_fuel()?)?)
        } else {
            None
        },
        range_remaining_m: if reader.get_has_range_remaining() {
            Some(reader.get_range_remaining())
        } else {
            None
        },
        runtime_remaining_s: if reader.get_has_runtime_remaining() {
            Some(reader.get_runtime_remaining())
        } else {
            None
        },
        mode: if reader.get_has_mode() {
            Some(read_robot_mode(reader.get_mode()?)?)
        } else {
            None
        },
        estop: read_estop(reader.get_estop()?)?,
        faulted: reader.get_faulted(),
        accepting_missions: reader.get_mission_active(),
        status: reader.get_status()?.to_str()?.to_string(),
    })
}

fn write_estop(builder: &mut crate::messages_capnp::e_stop::Builder, estop: &EStop) {
    builder.set_unix_time_ms(estop.unix_time_ms);
    builder.set_active(estop.active);
    let mut ids_builder = builder.reborrow().init_ids(estop.ids.len() as u32);
    for (i, id) in estop.ids.iter().enumerate() {
        ids_builder.set(i as u32, id);
    }
}

fn read_estop(reader: crate::messages_capnp::e_stop::Reader) -> Result<EStop, ::capnp::Error> {
    let ids_reader = reader.get_ids()?;
    let mut ids = Vec::with_capacity(ids_reader.len() as usize);
    for i in 0..ids_reader.len() {
        ids.push(ids_reader.get(i)?.to_str()?.to_string());
    }

    Ok(EStop {
        unix_time_ms: reader.get_unix_time_ms(),
        active: reader.get_active(),
        ids,
    })
}

fn write_battery_status(
    builder: &mut crate::messages_capnp::battery_status::Builder,
    battery: &BatteryStatus,
) {
    builder.set_soc_pct(battery.soc_pct);
    builder.set_power_supply_status(power_supply_status_to_capnp(battery.power_supply_status));
    builder.set_power_supply_health(power_supply_health_to_capnp(battery.power_supply_health));
    builder.set_power_supply_technology(power_supply_technology_to_capnp(
        battery.power_supply_technology,
    ));

    if let Some(v) = battery.voltage_v {
        builder.set_has_voltage_v(true);
        builder.set_voltage_v(v);
    } else {
        builder.set_has_voltage_v(false);
    }

    if let Some(c) = battery.current_a {
        builder.set_has_current_a(true);
        builder.set_current_a(c);
    } else {
        builder.set_has_current_a(false);
    }

    if let Some(ch) = battery.charge_ah {
        builder.set_has_charge_ah(true);
        builder.set_charge_ah(ch);
    } else {
        builder.set_has_charge_ah(false);
    }

    if let Some(cap) = battery.capacity_ah {
        builder.set_has_capacity_ah(true);
        builder.set_capacity_ah(cap);
    } else {
        builder.set_has_capacity_ah(false);
    }

    if let Some(dcap) = battery.design_capacity_ah {
        builder.set_has_design_capacity_ah(true);
        builder.set_design_capacity_ah(dcap);
    } else {
        builder.set_has_design_capacity_ah(false);
    }

    if let Some(temp) = battery.temperature {
        builder.set_has_temperature(true);
        builder.set_temperature(temp);
    } else {
        builder.set_has_temperature(false);
    }
}

fn read_battery_status(
    reader: crate::messages_capnp::battery_status::Reader,
) -> Result<BatteryStatus, ::capnp::Error> {
    Ok(BatteryStatus {
        soc_pct: reader.get_soc_pct(),
        voltage_v: if reader.get_has_voltage_v() {
            Some(reader.get_voltage_v())
        } else {
            None
        },
        current_a: if reader.get_has_current_a() {
            Some(reader.get_current_a())
        } else {
            None
        },
        charge_ah: if reader.get_has_charge_ah() {
            Some(reader.get_charge_ah())
        } else {
            None
        },
        capacity_ah: if reader.get_has_capacity_ah() {
            Some(reader.get_capacity_ah())
        } else {
            None
        },
        design_capacity_ah: if reader.get_has_design_capacity_ah() {
            Some(reader.get_design_capacity_ah())
        } else {
            None
        },
        temperature: if reader.get_has_temperature() {
            Some(reader.get_temperature())
        } else {
            None
        },
        power_supply_status: power_supply_status_from_capnp(reader.get_power_supply_status()?),
        power_supply_health: power_supply_health_from_capnp(reader.get_power_supply_health()?),
        power_supply_technology: power_supply_technology_from_capnp(
            reader.get_power_supply_technology()?,
        ),
    })
}

fn write_fuel_status(builder: &mut crate::messages_capnp::fuel_status::Builder, fuel: &FuelStatus) {
    builder.set_level_pct(fuel.level_pct);
    if let Some(vol) = fuel.volume_remaining {
        builder.set_volume_remaining(vol);
        builder.set_has_volume_remaining(true);
    } else {
        builder.set_has_volume_remaining(false);
    }
    if let Some(cap) = fuel.capacity {
        builder.set_capacity(cap);
        builder.set_has_capacity(true);
    } else {
        builder.set_has_capacity(false);
    }
    if let Some(ref unit) = fuel.volume_unit {
        builder.set_volume_unit(unit);
        builder.set_has_volume_unit(true);
    } else {
        builder.set_has_volume_unit(false);
    }
    if let Some(eff) = fuel.efficiency {
        builder.set_efficiency(eff);
        builder.set_has_efficiency(true);
    } else {
        builder.set_has_efficiency(false);
    }
    if let Some(ref unit) = fuel.efficiency_unit {
        builder.set_efficiency_unit(unit);
        builder.set_has_efficiency_unit(true);
    } else {
        builder.set_has_efficiency_unit(false);
    }
    builder.set_is_refueling(fuel.is_refueling);
}

fn read_fuel_status(
    reader: crate::messages_capnp::fuel_status::Reader,
) -> Result<FuelStatus, ::capnp::Error> {
    Ok(FuelStatus {
        level_pct: reader.get_level_pct(),
        volume_remaining: if reader.get_has_volume_remaining() {
            Some(reader.get_volume_remaining())
        } else {
            None
        },
        capacity: if reader.get_has_capacity() {
            Some(reader.get_capacity())
        } else {
            None
        },
        volume_unit: if reader.get_has_volume_unit() {
            Some(reader.get_volume_unit()?.to_str()?.to_string())
        } else {
            None
        },
        efficiency: if reader.get_has_efficiency() {
            Some(reader.get_efficiency())
        } else {
            None
        },
        efficiency_unit: if reader.get_has_efficiency_unit() {
            Some(reader.get_efficiency_unit()?.to_str()?.to_string())
        } else {
            None
        },
        is_refueling: reader.get_is_refueling(),
    })
}

fn write_sensor_telemetry(
    builder: &mut crate::messages_capnp::sensor_telemetry::Builder,
    extras: &SensorTelemetry,
) {
    builder.set_unix_time_ms(extras.unix_time_ms);
    let mut kv_builder = builder.reborrow().init_kv(extras.kv.len() as u32);
    for (i, sensor_reading) in extras.kv.iter().enumerate() {
        let mut entry_builder = kv_builder.reborrow().get(i as u32);
        entry_builder.set_key(&sensor_reading.key);
        let mut value_builder = entry_builder.reborrow().init_value();
        write_value(&mut value_builder, &sensor_reading.value);
        if let Some(ts) = sensor_reading.unix_time_ms {
            entry_builder.set_unix_time_ms(ts);
            entry_builder.set_has_unix_time_ms(true);
        } else {
            entry_builder.set_has_unix_time_ms(false);
        }
    }
}

fn read_sensor_telemetry(
    reader: crate::messages_capnp::sensor_telemetry::Reader,
) -> Result<SensorTelemetry, ::capnp::Error> {
    let kv_reader = reader.get_kv()?;
    let mut kv = Vec::with_capacity(kv_reader.len() as usize);

    for i in 0..kv_reader.len() {
        let entry = kv_reader.get(i);
        let key = entry.get_key()?.to_str()?.to_string();
        let value = read_value(entry.get_value()?)?;
        let timestamp = if entry.get_has_unix_time_ms() {
            Some(entry.get_unix_time_ms())
        } else {
            None
        };
        kv.push(SensorReading {
            key,
            value,
            unix_time_ms: timestamp,
        });
    }

    Ok(SensorTelemetry {
        unix_time_ms: reader.get_unix_time_ms(),
        kv,
    })
}

fn write_fault(builder: &mut crate::messages_capnp::fault::Builder, fault: &Fault) {
    builder.set_unix_time_ms(fault.unix_time_ms);
    builder.set_fault_id(&fault.fault_id);
    builder.set_source(&fault.source);
    builder.set_severity(write_fault_severity(fault.severity));
    builder.set_active(fault.active);
    builder.set_category(&fault.category);
    builder.set_description(&fault.description);
    builder.set_suggested_action(&fault.suggested_action);
}

fn read_fault(reader: crate::messages_capnp::fault::Reader) -> Result<Fault, ::capnp::Error> {
    Ok(Fault {
        unix_time_ms: reader.get_unix_time_ms(),
        fault_id: reader.get_fault_id()?.to_str()?.to_string(),
        source: reader.get_source()?.to_str()?.to_string(),
        severity: read_fault_severity(reader.get_severity()?),
        active: reader.get_active(),
        category: reader.get_category()?.to_str()?.to_string(),
        description: reader.get_description()?.to_str()?.to_string(),
        suggested_action: reader.get_suggested_action()?.to_str()?.to_string(),
    })
}

fn write_mission_run_status(
    builder: &mut crate::messages_capnp::mission_run_status::Builder,
    status: &MissionRunStatus,
) {
    builder.set_unix_time_ms(status.unix_time_ms);
    builder.set_mission_id(&status.mission_id);
    builder.set_run_id(&status.run_id);
    builder.set_status(mission_status_to_capnp(status.status));

    if let Some(ref msg) = status.status_message {
        builder.set_status_message(msg);
        builder.set_has_status_message(true);
    } else {
        builder.set_has_status_message(false);
    }

    if let Some(progress) = status.progress_x100 {
        builder.set_progress_x100(progress);
        builder.set_has_progress_x100(true);
    } else {
        builder.set_has_progress_x100(false);
    }

    if let Some(ref path_id) = status.current_path_id {
        builder.set_current_path_id(path_id);
        builder.set_has_current_path_id(true);
    } else {
        builder.set_has_current_path_id(false);
    }

    if let Some(ref target) = status.current_target {
        builder.set_has_current_target(true);
        let mut pose_builder = builder.reborrow().init_current_target();
        write_geo_pose(&mut pose_builder, target);
    } else {
        builder.set_has_current_target(false);
    }

    if let Some(step) = status.current_step {
        builder.set_current_step(step);
        builder.set_has_current_step(true);
    } else {
        builder.set_has_current_step(false);
    }

    if let Some(total) = status.total_steps {
        builder.set_total_steps(total);
        builder.set_has_total_steps(true);
    } else {
        builder.set_has_total_steps(false);
    }

    if let Some(time_started) = status.time_started_ms {
        builder.set_time_started_ms(time_started);
        builder.set_has_time_started_ms(true);
    } else {
        builder.set_has_time_started_ms(false);
    }

    if let Some(expected_end) = status.expected_end_time_ms {
        builder.set_expected_end_time_ms(expected_end);
        builder.set_has_expected_end_time_ms(true);
    } else {
        builder.set_has_expected_end_time_ms(false);
    }

    if let Some(time_completed) = status.time_completed_ms {
        builder.set_time_completed_ms(time_completed);
        builder.set_has_time_completed_ms(true);
    } else {
        builder.set_has_time_completed_ms(false);
    }

    if let Some(ref name) = status.name {
        builder.set_name(name);
        builder.set_has_name(true);
    } else {
        builder.set_has_name(false);
    }
}

fn read_mission_run_status(
    reader: crate::messages_capnp::mission_run_status::Reader,
) -> Result<MissionRunStatus, ::capnp::Error> {
    Ok(MissionRunStatus {
        unix_time_ms: reader.get_unix_time_ms(),
        mission_id: reader.get_mission_id()?.to_str()?.to_string(),
        run_id: reader.get_run_id()?.to_str()?.to_string(),
        status: mission_status_from_capnp(reader.get_status()?),
        status_message: if reader.get_has_status_message() {
            Some(reader.get_status_message()?.to_str()?.to_string())
        } else {
            None
        },
        progress_x100: if reader.get_has_progress_x100() {
            Some(reader.get_progress_x100())
        } else {
            None
        },
        current_path_id: if reader.get_has_current_path_id() {
            Some(reader.get_current_path_id()?.to_str()?.to_string())
        } else {
            None
        },
        current_target: if reader.get_has_current_target() {
            Some(read_geo_pose(reader.get_current_target()?)?)
        } else {
            None
        },
        current_step: if reader.get_has_current_step() {
            Some(reader.get_current_step())
        } else {
            None
        },
        total_steps: if reader.get_has_total_steps() {
            Some(reader.get_total_steps())
        } else {
            None
        },
        time_started_ms: if reader.get_has_time_started_ms() {
            Some(reader.get_time_started_ms())
        } else {
            None
        },
        expected_end_time_ms: if reader.get_has_expected_end_time_ms() {
            Some(reader.get_expected_end_time_ms())
        } else {
            None
        },
        time_completed_ms: if reader.get_has_time_completed_ms() {
            Some(reader.get_time_completed_ms())
        } else {
            None
        },
        name: if reader.get_has_name() {
            Some(reader.get_name()?.to_str()?.to_string())
        } else {
            None
        },
    })
}

fn write_system_health(
    builder: &mut crate::messages_capnp::system_health::Builder,
    health: &SystemHealth,
) {
    builder.set_device_id(&health.device_id);
    builder.set_unix_time_ms(health.unix_time_ms);
    builder.set_cpu_pct(health.cpu_pct);
    builder.set_mem_pct(health.mem_pct);
    builder.set_disk_pct(health.disk_pct);
    builder.set_cpu_temp(health.cpu_temp);
    builder.set_signal_strength(health.signal_strength);
    builder.set_signal_quality(health.signal_quality);
}

fn read_system_health(
    reader: crate::messages_capnp::system_health::Reader,
) -> Result<SystemHealth, ::capnp::Error> {
    Ok(SystemHealth {
        unix_time_ms: reader.get_unix_time_ms(),
        device_id: reader.get_device_id()?.to_str()?.to_string(),
        cpu_pct: reader.get_cpu_pct(),
        mem_pct: reader.get_mem_pct(),
        disk_pct: reader.get_disk_pct(),
        cpu_temp: reader.get_cpu_temp(),
        signal_strength: reader.get_signal_strength(),
        signal_quality: reader.get_signal_quality(),
    })
}

fn write_usage_telemetry(
    builder: &mut crate::messages_capnp::usage_telemetry::Builder,
    usage: &UsageTelemetry,
) {
    builder.set_unix_time_ms(usage.unix_time_ms);
    builder.set_total_distance_m(usage.total_distance_m);
    builder.set_auto_distance_m(usage.auto_distance_m);
    builder.set_manual_distance_m(usage.manual_distance_m);
    builder.set_uptime_total_s(usage.uptime_total_s);
    builder.set_drive_time_total_s(usage.drive_time_total_s);
    builder.set_auto_time_total_s(usage.auto_time_total_s);
    builder.set_mission_count_total(usage.mission_count_total);
    builder.set_charge_cycles_total(usage.charge_cycles_total);
    builder.set_reboot_count(usage.reboot_count);
    builder.set_last_reboot_unix_time_ms(usage.last_reboot_unix_time_ms);
}

fn read_usage_telemetry(
    reader: crate::messages_capnp::usage_telemetry::Reader,
) -> Result<UsageTelemetry, ::capnp::Error> {
    Ok(UsageTelemetry {
        unix_time_ms: reader.get_unix_time_ms(),
        total_distance_m: reader.get_total_distance_m(),
        auto_distance_m: reader.get_auto_distance_m(),
        manual_distance_m: reader.get_manual_distance_m(),
        uptime_total_s: reader.get_uptime_total_s(),
        drive_time_total_s: reader.get_drive_time_total_s(),
        auto_time_total_s: reader.get_auto_time_total_s(),
        mission_count_total: reader.get_mission_count_total(),
        charge_cycles_total: reader.get_charge_cycles_total(),
        reboot_count: reader.get_reboot_count(),
        last_reboot_unix_time_ms: reader.get_last_reboot_unix_time_ms(),
    })
}

fn write_consumable_status(
    builder: &mut crate::messages_capnp::consumable_status::Builder,
    consumable: &ConsumableStatus,
) {
    builder.set_unix_time_ms(consumable.unix_time_ms);
    builder.set_kind(&consumable.kind);
    builder.set_tank_id(&consumable.tank_id);
    builder.set_level_pct(consumable.level_pct);
    builder.set_amount_remaining(consumable.amount_remaining);
    builder.set_unit(&consumable.unit);
    builder.set_is_critical(consumable.is_critical);
}

fn read_consumable_status(
    reader: crate::messages_capnp::consumable_status::Reader,
) -> Result<ConsumableStatus, ::capnp::Error> {
    Ok(ConsumableStatus {
        unix_time_ms: reader.get_unix_time_ms(),
        kind: reader.get_kind()?.to_str()?.to_string(),
        tank_id: reader.get_tank_id()?.to_str()?.to_string(),
        level_pct: reader.get_level_pct(),
        amount_remaining: reader.get_amount_remaining(),
        unit: reader.get_unit()?.to_str()?.to_string(),
        is_critical: reader.get_is_critical(),
    })
}

fn write_message(builder: &mut crate::messages_capnp::message::Builder, msg: &Message) {
    builder.set_unix_time_ms(msg.unix_time_ms);
    builder.set_level(message_level_to_capnp(&msg.level));
    builder.set_message(&msg.message);
    builder.set_needs_confirmation(msg.needs_confirmation);
    builder.set_confirmation_id(&msg.confirmation_id);
}

fn read_message(reader: crate::messages_capnp::message::Reader) -> Result<Message, ::capnp::Error> {
    Ok(Message {
        unix_time_ms: reader.get_unix_time_ms(),
        level: message_level_from_capnp(reader.get_level()?),
        message: reader.get_message()?.to_str()?.to_string(),
        needs_confirmation: reader.get_needs_confirmation(),
        confirmation_id: reader.get_confirmation_id()?.to_str()?.to_string(),
    })
}

fn write_spatial_directive_status(
    builder: &mut crate::messages_capnp::spatial_directive_status::Builder,
    status: &SpatialDirectiveStatus,
) {
    builder.set_directive_id(&status.directive_id);
    builder.set_last_eval_time_ms(status.last_eval_time_ms);
    builder.set_active(status.active);
    builder.set_inside_zone(status.inside_zone);
    if let Some(distance) = status.distance_to_zone_m {
        builder.set_distance_to_zone_m(distance);
        builder.set_has_distance_to_zone_m(true);
    } else {
        builder.set_has_distance_to_zone_m(false);
    }
}

fn read_spatial_directive_status(
    reader: crate::messages_capnp::spatial_directive_status::Reader,
) -> Result<SpatialDirectiveStatus, ::capnp::Error> {
    Ok(SpatialDirectiveStatus {
        directive_id: reader.get_directive_id()?.to_str()?.to_string(),
        last_eval_time_ms: reader.get_last_eval_time_ms(),
        active: reader.get_active(),
        inside_zone: reader.get_inside_zone(),
        distance_to_zone_m: if reader.get_has_distance_to_zone_m() {
            Some(reader.get_distance_to_zone_m())
        } else {
            None
        },
    })
}

fn write_agent_info(builder: &mut crate::messages_capnp::agent_info::Builder, info: &AgentInfo) {
    builder.set_agent_version(&info.agent_version);
    if let Some(ref sw_version) = info.software_version {
        builder.set_software_version(sw_version);
        builder.set_has_software_version(true);
    } else {
        builder.set_has_software_version(false);
    }
    if let Some(ref os_info) = info.os_info {
        builder.set_os_info(os_info);
        builder.set_has_os_info(true);
    } else {
        builder.set_has_os_info(false);
    }
    if let Some(uptime) = info.uptime_s {
        builder.set_uptime_s(uptime);
        builder.set_has_uptime_s(true);
    } else {
        builder.set_has_uptime_s(false);
    }
}

fn read_agent_info(
    reader: crate::messages_capnp::agent_info::Reader,
) -> Result<AgentInfo, ::capnp::Error> {
    Ok(AgentInfo {
        agent_version: reader.get_agent_version()?.to_str()?.to_string(),
        software_version: if reader.get_has_software_version() {
            Some(reader.get_software_version()?.to_str()?.to_string())
        } else {
            None
        },
        os_info: if reader.get_has_os_info() {
            Some(reader.get_os_info()?.to_str()?.to_string())
        } else {
            None
        },
        uptime_s: if reader.get_has_uptime_s() {
            Some(reader.get_uptime_s())
        } else {
            None
        },
    })
}

fn write_capabilities(
    builder: &mut crate::messages_capnp::capabilities::Builder,
    caps: &Capabilities,
) {
    // Write services
    let mut services_builder = builder.reborrow().init_services(caps.services.len() as u32);
    for (i, service) in caps.services.iter().enumerate() {
        let mut service_builder = services_builder.reborrow().get(i as u32);
        service_builder.set_name(&service.name);
        service_builder.set_description(&service.description);
        service_builder.set_capability_type(capability_type_to_capnp(&service.capability_type));
        if let Some(ref msg) = service.confirmation_message {
            service_builder.set_confirmation_message(msg);
            service_builder.set_has_confirmation_message(true);
        } else {
            service_builder.set_confirmation_message("");
            service_builder.set_has_confirmation_message(false);
        }
        service_builder.set_state_key(service.state_key.as_deref().unwrap_or(""));
        service_builder.set_requires_role(role_to_capnp(&service.requires_role));
        service_builder.set_danger_level(danger_level_to_capnp(&service.danger_level));
    }

    // Write sensors (SensorDescriptor)
    let mut sensors_builder = builder.reborrow().init_sensors(caps.sensors.len() as u32);
    for (i, sensor) in caps.sensors.iter().enumerate() {
        let mut sensor_builder = sensors_builder.reborrow().get(i as u32);
        sensor_builder.set_key(&sensor.key);
        sensor_builder.set_label(&sensor.label);
        sensor_builder.set_value_type(value_type_to_capnp(&sensor.value_type));
        sensor_builder.set_unit(sensor.unit.as_deref().unwrap_or(""));
        if let Some(ref range) = sensor.range {
            sensor_builder.set_range_min(range.min);
            sensor_builder.set_range_max(range.max);
            sensor_builder.set_has_range(true);
        } else {
            sensor_builder.set_has_range(false);
        }
        sensor_builder.set_display_hint(display_hint_to_capnp(&sensor.display_hint));
    }

    // Write settings (SettingDescriptor)
    let mut settings_builder = builder.reborrow().init_settings(caps.settings.len() as u32);
    for (i, setting) in caps.settings.iter().enumerate() {
        let mut setting_builder = settings_builder.reborrow().get(i as u32);
        setting_builder.set_key(&setting.key);
        setting_builder.set_label(&setting.label);
        setting_builder.set_value_type(value_type_to_capnp(&setting.value_type));
        let mut default_builder = setting_builder.reborrow().init_default();
        write_json_value(&mut default_builder, &setting.default);
        if let Some(ref range) = setting.range {
            let mut range_builder = setting_builder.reborrow().init_range();
            range_builder.set_min(range.min);
            range_builder.set_max(range.max);
            setting_builder.set_has_range(true);
        } else {
            setting_builder.set_has_range(false);
        }
        if let Some(step) = setting.step {
            setting_builder.set_step(step);
            setting_builder.set_has_step(true);
        } else {
            setting_builder.set_has_step(false);
        }
    }

    // Write resource producers
    let mut producers_builder = builder
        .reborrow()
        .init_resource_producers(caps.resource_producers.len() as u32);
    for (i, producer) in caps.resource_producers.iter().enumerate() {
        let mut producer_builder = producers_builder.reborrow().get(i as u32);
        write_resource_producer_descriptor(&mut producer_builder, producer);
    }
}

fn write_resource_producer_descriptor(
    builder: &mut crate::messages_capnp::resource_producer_descriptor::Builder,
    producer: &ResourceProducerDescriptor,
) {
    builder.set_resource_producer_id(&producer.resource_producer_id);
    builder.set_label(&producer.label);
    if let Some(ref description) = producer.description {
        builder.set_description(description);
        builder.set_has_description(true);
    } else {
        builder.set_has_description(false);
    }
    builder.set_layer_role(layer_role_to_capnp(producer.layer_role));
    builder.set_geometry_type(geometry_type_to_capnp(&producer.geometry_type));
}

fn read_resource_producer_descriptor(
    reader: crate::messages_capnp::resource_producer_descriptor::Reader,
) -> Result<ResourceProducerDescriptor, ::capnp::Error> {
    Ok(ResourceProducerDescriptor {
        resource_producer_id: reader.get_resource_producer_id()?.to_str()?.to_string(),
        label: reader.get_label()?.to_str()?.to_string(),
        description: if reader.get_has_description() {
            Some(reader.get_description()?.to_str()?.to_string())
        } else {
            None
        },
        layer_role: layer_role_from_capnp(reader.get_layer_role()?),
        geometry_type: geometry_type_from_capnp(reader.get_geometry_type()?),
    })
}

fn read_capabilities(
    reader: crate::messages_capnp::capabilities::Reader,
) -> Result<Capabilities, ::capnp::Error> {
    // Read services
    let services_reader = reader.get_services()?;
    let mut services = Vec::with_capacity(services_reader.len() as usize);
    for i in 0..services_reader.len() {
        let service = services_reader.get(i);
        let confirmation_message = if service.get_has_confirmation_message() {
            let msg = service.get_confirmation_message()?.to_str()?;
            if msg.is_empty() {
                None
            } else {
                Some(msg.to_string())
            }
        } else {
            None
        };
        let state_key_str = service.get_state_key()?.to_str()?;
        let state_key = if state_key_str.is_empty() {
            None
        } else {
            Some(state_key_str.to_string())
        };

        services.push(ServiceCapability {
            name: service.get_name()?.to_str()?.to_string(),
            description: service.get_description()?.to_str()?.to_string(),
            capability_type: capability_type_from_capnp(service.get_capability_type()?),
            confirmation_message,
            state_key,
            requires_role: role_from_capnp(service.get_requires_role()?),
            danger_level: danger_level_from_capnp(service.get_danger_level()?),
        });
    }

    // Read sensors (SensorDescriptor)
    let sensors_reader = reader.get_sensors()?;
    let mut sensors = Vec::with_capacity(sensors_reader.len() as usize);
    for i in 0..sensors_reader.len() {
        let sensor = sensors_reader.get(i);
        let unit_str = sensor.get_unit()?.to_str()?;
        sensors.push(SensorDescriptor {
            key: sensor.get_key()?.to_str()?.to_string(),
            label: sensor.get_label()?.to_str()?.to_string(),
            value_type: value_type_from_capnp(sensor.get_value_type()?),
            unit: if unit_str.is_empty() {
                None
            } else {
                Some(unit_str.to_string())
            },
            range: if sensor.get_has_range() {
                Some(Range {
                    min: sensor.get_range_min(),
                    max: sensor.get_range_max(),
                })
            } else {
                None
            },
            display_hint: display_hint_from_capnp(sensor.get_display_hint()?),
        });
    }

    // Read settings (SettingDescriptor)
    let settings_reader = reader.get_settings()?;
    let mut settings = Vec::with_capacity(settings_reader.len() as usize);
    for i in 0..settings_reader.len() {
        let setting = settings_reader.get(i);
        let range = if setting.get_has_range() {
            let r = setting.get_range()?;
            Some(Range {
                min: r.get_min(),
                max: r.get_max(),
            })
        } else {
            None
        };
        let step = if setting.get_has_step() {
            Some(setting.get_step())
        } else {
            None
        };
        settings.push(SettingDescriptor {
            key: setting.get_key()?.to_str()?.to_string(),
            label: setting.get_label()?.to_str()?.to_string(),
            value_type: value_type_from_capnp(setting.get_value_type()?),
            default: read_json_value(setting.get_default()?)?,
            range,
            step,
        });
    }

    // Read resource producers
    let producers_reader = reader.get_resource_producers()?;
    let mut resource_producers = Vec::with_capacity(producers_reader.len() as usize);
    for i in 0..producers_reader.len() {
        resource_producers.push(read_resource_producer_descriptor(producers_reader.get(i))?);
    }

    Ok(Capabilities {
        services,
        sensors,
        settings,
        resource_producers,
    })
}

fn write_allowed_commands(
    builder: &mut crate::messages_capnp::allowed_commands::Builder,
    allowed: &AllowedCommands,
) {
    builder.set_set_mode(allowed.set_mode);
    builder.set_assign_mission(allowed.assign_mission);
    builder.set_control_mission_run(allowed.control_mission_run);
    builder.set_invoke_service(allowed.invoke_service);
    builder.set_update_settings(allowed.update_settings);
    builder.set_update_feature(allowed.update_feature);
    builder.set_spatial_directive(allowed.spatial_directive);
    builder.set_object(allowed.object);
    builder.set_go_to(allowed.go_to);
    builder.set_velocity_cmd(allowed.velocity_cmd);
    builder.set_teleop_joy(allowed.teleop_joy);
    builder.set_agent_text_request(allowed.agent_text_request);
    builder.set_say_text_request(allowed.say_text_request);
    builder.set_message_confirmation(allowed.message_confirmation);
}

fn read_allowed_commands(
    reader: crate::messages_capnp::allowed_commands::Reader,
) -> Result<AllowedCommands, ::capnp::Error> {
    Ok(AllowedCommands {
        set_mode: reader.get_set_mode(),
        assign_mission: reader.get_assign_mission(),
        control_mission_run: reader.get_control_mission_run(),
        invoke_service: reader.get_invoke_service(),
        update_settings: reader.get_update_settings(),
        update_feature: reader.get_update_feature(),
        spatial_directive: reader.get_spatial_directive(),
        object: reader.get_object(),
        go_to: reader.get_go_to(),
        velocity_cmd: reader.get_velocity_cmd(),
        teleop_joy: reader.get_teleop_joy(),
        agent_text_request: reader.get_agent_text_request(),
        say_text_request: reader.get_say_text_request(),
        message_confirmation: reader.get_message_confirmation(),
    })
}

// Enum conversion helpers

fn write_robot_mode(mode: &RobotMode) -> crate::messages_capnp::RobotMode {
    match mode {
        RobotMode::Manual => crate::messages_capnp::RobotMode::Manual,
        RobotMode::Auto => crate::messages_capnp::RobotMode::Auto,
        RobotMode::Teleop => crate::messages_capnp::RobotMode::Teleop,
        RobotMode::Disabled => crate::messages_capnp::RobotMode::Disabled,
        RobotMode::Maintenance => crate::messages_capnp::RobotMode::Maintenance,
    }
}

fn read_robot_mode(reader: crate::messages_capnp::RobotMode) -> Result<RobotMode, ::capnp::Error> {
    match reader {
        crate::messages_capnp::RobotMode::Manual => Ok(RobotMode::Manual),
        crate::messages_capnp::RobotMode::Auto => Ok(RobotMode::Auto),
        crate::messages_capnp::RobotMode::Teleop => Ok(RobotMode::Teleop),
        crate::messages_capnp::RobotMode::Disabled => Ok(RobotMode::Disabled),
        crate::messages_capnp::RobotMode::Maintenance => Ok(RobotMode::Maintenance),
    }
}

fn write_gps_source(src: GpsSource) -> crate::messages_capnp::GpsSource {
    match src {
        GpsSource::Real => crate::messages_capnp::GpsSource::Real,
        GpsSource::Simulated => crate::messages_capnp::GpsSource::Simulated,
        GpsSource::Estimated => crate::messages_capnp::GpsSource::Estimated,
    }
}

fn read_gps_source(src: crate::messages_capnp::GpsSource) -> GpsSource {
    match src {
        crate::messages_capnp::GpsSource::Real => GpsSource::Real,
        crate::messages_capnp::GpsSource::Simulated => GpsSource::Simulated,
        crate::messages_capnp::GpsSource::Estimated => GpsSource::Estimated,
    }
}

fn write_fault_severity(sev: FaultSeverity) -> crate::messages_capnp::FaultSeverity {
    match sev {
        FaultSeverity::Info => crate::messages_capnp::FaultSeverity::Info,
        FaultSeverity::Minor => crate::messages_capnp::FaultSeverity::Minor,
        FaultSeverity::Major => crate::messages_capnp::FaultSeverity::Major,
        FaultSeverity::Critical => crate::messages_capnp::FaultSeverity::Critical,
    }
}

fn read_fault_severity(sev: crate::messages_capnp::FaultSeverity) -> FaultSeverity {
    match sev {
        crate::messages_capnp::FaultSeverity::Info => FaultSeverity::Info,
        crate::messages_capnp::FaultSeverity::Minor => FaultSeverity::Minor,
        crate::messages_capnp::FaultSeverity::Major => FaultSeverity::Major,
        crate::messages_capnp::FaultSeverity::Critical => FaultSeverity::Critical,
    }
}

fn gps_fix_type_to_capnp(fix_type: GpsFixType) -> crate::messages_capnp::GpsFixType {
    match fix_type {
        GpsFixType::Unknown => crate::messages_capnp::GpsFixType::Unknown,
        GpsFixType::Invalid => crate::messages_capnp::GpsFixType::Invalid,
        GpsFixType::GPS => crate::messages_capnp::GpsFixType::Gps,
        GpsFixType::DGPS => crate::messages_capnp::GpsFixType::Dgps,
        GpsFixType::SBAS => crate::messages_capnp::GpsFixType::Sbas,
        GpsFixType::FloatRTK => crate::messages_capnp::GpsFixType::FloatRtk,
        GpsFixType::FixedRTK => crate::messages_capnp::GpsFixType::FixedRtk,
        GpsFixType::PPS => crate::messages_capnp::GpsFixType::Pps,
        GpsFixType::GBAS => crate::messages_capnp::GpsFixType::Gbas,
    }
}

fn gps_fix_type_from_capnp(fix_type: crate::messages_capnp::GpsFixType) -> GpsFixType {
    match fix_type {
        crate::messages_capnp::GpsFixType::Unknown => GpsFixType::Unknown,
        crate::messages_capnp::GpsFixType::Invalid => GpsFixType::Invalid,
        crate::messages_capnp::GpsFixType::Gps => GpsFixType::GPS,
        crate::messages_capnp::GpsFixType::Dgps => GpsFixType::DGPS,
        crate::messages_capnp::GpsFixType::Sbas => GpsFixType::SBAS,
        crate::messages_capnp::GpsFixType::FloatRtk => GpsFixType::FloatRTK,
        crate::messages_capnp::GpsFixType::FixedRtk => GpsFixType::FixedRTK,
        crate::messages_capnp::GpsFixType::Pps => GpsFixType::PPS,
        crate::messages_capnp::GpsFixType::Gbas => GpsFixType::GBAS,
    }
}

fn power_supply_status_to_capnp(
    status: PowerSupplyStatus,
) -> crate::messages_capnp::PowerSupplyStatus {
    match status {
        PowerSupplyStatus::Unknown => crate::messages_capnp::PowerSupplyStatus::Unknown,
        PowerSupplyStatus::Charging => crate::messages_capnp::PowerSupplyStatus::Charging,
        PowerSupplyStatus::Discharging => crate::messages_capnp::PowerSupplyStatus::Discharging,
        PowerSupplyStatus::NotCharging => crate::messages_capnp::PowerSupplyStatus::NotCharging,
        PowerSupplyStatus::Full => crate::messages_capnp::PowerSupplyStatus::Full,
    }
}

fn power_supply_status_from_capnp(
    status: crate::messages_capnp::PowerSupplyStatus,
) -> PowerSupplyStatus {
    match status {
        crate::messages_capnp::PowerSupplyStatus::Unknown => PowerSupplyStatus::Unknown,
        crate::messages_capnp::PowerSupplyStatus::Charging => PowerSupplyStatus::Charging,
        crate::messages_capnp::PowerSupplyStatus::Discharging => PowerSupplyStatus::Discharging,
        crate::messages_capnp::PowerSupplyStatus::NotCharging => PowerSupplyStatus::NotCharging,
        crate::messages_capnp::PowerSupplyStatus::Full => PowerSupplyStatus::Full,
    }
}

fn power_supply_health_to_capnp(
    health: PowerSupplyHealth,
) -> crate::messages_capnp::PowerSupplyHealth {
    match health {
        PowerSupplyHealth::Unknown => crate::messages_capnp::PowerSupplyHealth::Unknown,
        PowerSupplyHealth::Good => crate::messages_capnp::PowerSupplyHealth::Good,
        PowerSupplyHealth::Overheat => crate::messages_capnp::PowerSupplyHealth::Overheat,
        PowerSupplyHealth::Dead => crate::messages_capnp::PowerSupplyHealth::Dead,
        PowerSupplyHealth::Overvoltage => crate::messages_capnp::PowerSupplyHealth::Overvoltage,
        PowerSupplyHealth::UnspecifiedFailure => {
            crate::messages_capnp::PowerSupplyHealth::UnspecifiedFailure
        }
        PowerSupplyHealth::Cold => crate::messages_capnp::PowerSupplyHealth::Cold,
        PowerSupplyHealth::WatchdogTimerExpire => {
            crate::messages_capnp::PowerSupplyHealth::WatchdogTimerExpire
        }
        PowerSupplyHealth::SafetyTimerExpire => {
            crate::messages_capnp::PowerSupplyHealth::SafetyTimerExpire
        }
    }
}

fn power_supply_health_from_capnp(
    health: crate::messages_capnp::PowerSupplyHealth,
) -> PowerSupplyHealth {
    match health {
        crate::messages_capnp::PowerSupplyHealth::Unknown => PowerSupplyHealth::Unknown,
        crate::messages_capnp::PowerSupplyHealth::Good => PowerSupplyHealth::Good,
        crate::messages_capnp::PowerSupplyHealth::Overheat => PowerSupplyHealth::Overheat,
        crate::messages_capnp::PowerSupplyHealth::Dead => PowerSupplyHealth::Dead,
        crate::messages_capnp::PowerSupplyHealth::Overvoltage => PowerSupplyHealth::Overvoltage,
        crate::messages_capnp::PowerSupplyHealth::UnspecifiedFailure => {
            PowerSupplyHealth::UnspecifiedFailure
        }
        crate::messages_capnp::PowerSupplyHealth::Cold => PowerSupplyHealth::Cold,
        crate::messages_capnp::PowerSupplyHealth::WatchdogTimerExpire => {
            PowerSupplyHealth::WatchdogTimerExpire
        }
        crate::messages_capnp::PowerSupplyHealth::SafetyTimerExpire => {
            PowerSupplyHealth::SafetyTimerExpire
        }
    }
}

fn power_supply_technology_to_capnp(
    tech: PowerSupplyTechnology,
) -> crate::messages_capnp::PowerSupplyTechnology {
    match tech {
        PowerSupplyTechnology::Unknown => crate::messages_capnp::PowerSupplyTechnology::Unknown,
        PowerSupplyTechnology::NiMH => crate::messages_capnp::PowerSupplyTechnology::NiMH,
        PowerSupplyTechnology::LiIon => crate::messages_capnp::PowerSupplyTechnology::LiIon,
        PowerSupplyTechnology::LiPo => crate::messages_capnp::PowerSupplyTechnology::LiPo,
        PowerSupplyTechnology::LiFe => crate::messages_capnp::PowerSupplyTechnology::LiFe,
        PowerSupplyTechnology::NiCd => crate::messages_capnp::PowerSupplyTechnology::NiCd,
        PowerSupplyTechnology::LiMn => crate::messages_capnp::PowerSupplyTechnology::LiMn,
    }
}

fn power_supply_technology_from_capnp(
    tech: crate::messages_capnp::PowerSupplyTechnology,
) -> PowerSupplyTechnology {
    match tech {
        crate::messages_capnp::PowerSupplyTechnology::Unknown => PowerSupplyTechnology::Unknown,
        crate::messages_capnp::PowerSupplyTechnology::NiMH => PowerSupplyTechnology::NiMH,
        crate::messages_capnp::PowerSupplyTechnology::LiIon => PowerSupplyTechnology::LiIon,
        crate::messages_capnp::PowerSupplyTechnology::LiPo => PowerSupplyTechnology::LiPo,
        crate::messages_capnp::PowerSupplyTechnology::LiFe => PowerSupplyTechnology::LiFe,
        crate::messages_capnp::PowerSupplyTechnology::NiCd => PowerSupplyTechnology::NiCd,
        crate::messages_capnp::PowerSupplyTechnology::LiMn => PowerSupplyTechnology::LiMn,
    }
}

fn mission_status_to_capnp(status: MissionStatus) -> crate::messages_capnp::MissionStatus {
    match status {
        MissionStatus::Unknown => crate::messages_capnp::MissionStatus::Unknown,
        MissionStatus::Ready => crate::messages_capnp::MissionStatus::Ready,
        MissionStatus::Starting => crate::messages_capnp::MissionStatus::Starting,
        MissionStatus::Running => crate::messages_capnp::MissionStatus::Running,
        MissionStatus::Paused => crate::messages_capnp::MissionStatus::Paused,
        MissionStatus::Blocked => crate::messages_capnp::MissionStatus::Blocked,
        MissionStatus::Aborted => crate::messages_capnp::MissionStatus::Aborted,
        MissionStatus::Completed => crate::messages_capnp::MissionStatus::Complete,
        MissionStatus::Error => crate::messages_capnp::MissionStatus::Error,
    }
}

fn mission_status_from_capnp(status: crate::messages_capnp::MissionStatus) -> MissionStatus {
    match status {
        crate::messages_capnp::MissionStatus::Unknown => MissionStatus::Unknown,
        crate::messages_capnp::MissionStatus::Ready => MissionStatus::Ready,
        crate::messages_capnp::MissionStatus::Starting => MissionStatus::Starting,
        crate::messages_capnp::MissionStatus::Running => MissionStatus::Running,
        crate::messages_capnp::MissionStatus::Paused => MissionStatus::Paused,
        crate::messages_capnp::MissionStatus::Blocked => MissionStatus::Blocked,
        crate::messages_capnp::MissionStatus::Aborted => MissionStatus::Aborted,
        crate::messages_capnp::MissionStatus::Complete => MissionStatus::Completed,
        crate::messages_capnp::MissionStatus::Error => MissionStatus::Error,
    }
}

fn mission_run_action_to_capnp(
    action: MissionRunAction,
) -> crate::messages_capnp::MissionRunAction {
    match action {
        MissionRunAction::Pause => crate::messages_capnp::MissionRunAction::Pause,
        MissionRunAction::Resume => crate::messages_capnp::MissionRunAction::Resume,
        MissionRunAction::Cancel => crate::messages_capnp::MissionRunAction::Cancel,
        MissionRunAction::Abort => crate::messages_capnp::MissionRunAction::Abort,
    }
}

fn mission_run_action_from_capnp(
    action: crate::messages_capnp::MissionRunAction,
) -> MissionRunAction {
    match action {
        crate::messages_capnp::MissionRunAction::Pause => MissionRunAction::Pause,
        crate::messages_capnp::MissionRunAction::Resume => MissionRunAction::Resume,
        crate::messages_capnp::MissionRunAction::Cancel => MissionRunAction::Cancel,
        crate::messages_capnp::MissionRunAction::Abort => MissionRunAction::Abort,
    }
}

fn role_to_capnp(role: &Role) -> crate::messages_capnp::Role {
    match role {
        Role::Viewer => crate::messages_capnp::Role::Viewer,
        Role::Operator => crate::messages_capnp::Role::Operator,
        Role::Admin => crate::messages_capnp::Role::Admin,
        Role::Owner => crate::messages_capnp::Role::Owner,
    }
}

fn role_from_capnp(role: crate::messages_capnp::Role) -> Role {
    match role {
        crate::messages_capnp::Role::Viewer => Role::Viewer,
        crate::messages_capnp::Role::Operator => Role::Operator,
        crate::messages_capnp::Role::Admin => Role::Admin,
        crate::messages_capnp::Role::Owner => Role::Owner,
    }
}

fn display_hint_to_capnp(hint: &DisplayHint) -> crate::messages_capnp::DisplayHint {
    match hint {
        DisplayHint::None => crate::messages_capnp::DisplayHint::None,
        DisplayHint::OnOff => crate::messages_capnp::DisplayHint::OnOff,
        DisplayHint::Gauge => crate::messages_capnp::DisplayHint::Gauge,
        DisplayHint::Text => crate::messages_capnp::DisplayHint::Text,
        DisplayHint::Online => crate::messages_capnp::DisplayHint::Online,
        DisplayHint::WarningLight => crate::messages_capnp::DisplayHint::WarningLight,
        DisplayHint::Temperature => crate::messages_capnp::DisplayHint::Temperature,
        DisplayHint::Progress => crate::messages_capnp::DisplayHint::Progress,
    }
}

fn display_hint_from_capnp(hint: crate::messages_capnp::DisplayHint) -> DisplayHint {
    match hint {
        crate::messages_capnp::DisplayHint::None => DisplayHint::None,
        crate::messages_capnp::DisplayHint::OnOff => DisplayHint::OnOff,
        crate::messages_capnp::DisplayHint::Gauge => DisplayHint::Gauge,
        crate::messages_capnp::DisplayHint::Text => DisplayHint::Text,
        crate::messages_capnp::DisplayHint::Online => DisplayHint::Online,
        crate::messages_capnp::DisplayHint::WarningLight => DisplayHint::WarningLight,
        crate::messages_capnp::DisplayHint::Temperature => DisplayHint::Temperature,
        crate::messages_capnp::DisplayHint::Progress => DisplayHint::Progress,
    }
}

fn capability_type_to_capnp(ct: &CapabilityType) -> crate::messages_capnp::CapabilityType {
    match ct {
        CapabilityType::Trigger => crate::messages_capnp::CapabilityType::Trigger,
        CapabilityType::SetBool => crate::messages_capnp::CapabilityType::SetBool,
        CapabilityType::MissionType => crate::messages_capnp::CapabilityType::MissionType,
    }
}

fn capability_type_from_capnp(ct: crate::messages_capnp::CapabilityType) -> CapabilityType {
    match ct {
        crate::messages_capnp::CapabilityType::Trigger => CapabilityType::Trigger,
        crate::messages_capnp::CapabilityType::SetBool => CapabilityType::SetBool,
        crate::messages_capnp::CapabilityType::MissionType => CapabilityType::MissionType,
    }
}

fn danger_level_to_capnp(dl: &DangerLevel) -> crate::messages_capnp::DangerLevel {
    match dl {
        DangerLevel::Normal => crate::messages_capnp::DangerLevel::Normal,
        DangerLevel::Warning => crate::messages_capnp::DangerLevel::Warning,
        DangerLevel::Critical => crate::messages_capnp::DangerLevel::Critical,
    }
}

fn danger_level_from_capnp(dl: crate::messages_capnp::DangerLevel) -> DangerLevel {
    match dl {
        crate::messages_capnp::DangerLevel::Normal => DangerLevel::Normal,
        crate::messages_capnp::DangerLevel::Warning => DangerLevel::Warning,
        crate::messages_capnp::DangerLevel::Critical => DangerLevel::Critical,
    }
}

fn value_type_to_capnp(vt: &ValueType) -> crate::messages_capnp::ValueType {
    match vt {
        ValueType::String => crate::messages_capnp::ValueType::String,
        ValueType::Int => crate::messages_capnp::ValueType::Int,
        ValueType::Number => crate::messages_capnp::ValueType::Number,
        ValueType::Bool => crate::messages_capnp::ValueType::Bool,
        ValueType::Enum { .. } => crate::messages_capnp::ValueType::Enum,
    }
}

fn value_type_from_capnp(vt: crate::messages_capnp::ValueType) -> ValueType {
    match vt {
        crate::messages_capnp::ValueType::String => ValueType::String,
        crate::messages_capnp::ValueType::Int => ValueType::Int,
        crate::messages_capnp::ValueType::Number => ValueType::Number,
        crate::messages_capnp::ValueType::Bool => ValueType::Bool,
        crate::messages_capnp::ValueType::Enum => ValueType::Enum { options: vec![] },
    }
}

fn message_level_to_capnp(level: &MessageLevel) -> crate::messages_capnp::MessageLevel {
    match level {
        MessageLevel::Info => crate::messages_capnp::MessageLevel::Info,
        MessageLevel::Warn => crate::messages_capnp::MessageLevel::Warn,
        MessageLevel::Error => crate::messages_capnp::MessageLevel::Error,
    }
}

fn message_level_from_capnp(level: crate::messages_capnp::MessageLevel) -> MessageLevel {
    match level {
        crate::messages_capnp::MessageLevel::Info => MessageLevel::Info,
        crate::messages_capnp::MessageLevel::Warn => MessageLevel::Warn,
        crate::messages_capnp::MessageLevel::Error => MessageLevel::Error,
    }
}

fn agent_text_intent_to_capnp(
    intent: &AgentTextIntent,
) -> crate::messages_capnp::AgentTextIntent {
    match intent {
        AgentTextIntent::ObserveAndReport => {
            crate::messages_capnp::AgentTextIntent::ObserveAndReport
        }
        AgentTextIntent::AnswerQuestion => crate::messages_capnp::AgentTextIntent::AnswerQuestion,
        AgentTextIntent::OperatorInstruction => {
            crate::messages_capnp::AgentTextIntent::OperatorInstruction
        }
        AgentTextIntent::TroubleshootingRequest => {
            crate::messages_capnp::AgentTextIntent::TroubleshootingRequest
        }
        AgentTextIntent::GeneralMessage => crate::messages_capnp::AgentTextIntent::GeneralMessage,
    }
}

fn agent_text_intent_from_capnp(
    intent: crate::messages_capnp::AgentTextIntent,
) -> AgentTextIntent {
    match intent {
        crate::messages_capnp::AgentTextIntent::ObserveAndReport => {
            AgentTextIntent::ObserveAndReport
        }
        crate::messages_capnp::AgentTextIntent::AnswerQuestion => AgentTextIntent::AnswerQuestion,
        crate::messages_capnp::AgentTextIntent::OperatorInstruction => {
            AgentTextIntent::OperatorInstruction
        }
        crate::messages_capnp::AgentTextIntent::TroubleshootingRequest => {
            AgentTextIntent::TroubleshootingRequest
        }
        crate::messages_capnp::AgentTextIntent::GeneralMessage => AgentTextIntent::GeneralMessage,
    }
}

fn speech_priority_to_capnp(priority: &SpeechPriority) -> crate::messages_capnp::SpeechPriority {
    match priority {
        SpeechPriority::Low => crate::messages_capnp::SpeechPriority::Low,
        SpeechPriority::Next => crate::messages_capnp::SpeechPriority::Next,
        SpeechPriority::Interrupt => crate::messages_capnp::SpeechPriority::Interrupt,
    }
}

fn speech_priority_from_capnp(priority: crate::messages_capnp::SpeechPriority) -> SpeechPriority {
    match priority {
        crate::messages_capnp::SpeechPriority::Low => SpeechPriority::Low,
        crate::messages_capnp::SpeechPriority::Next => SpeechPriority::Next,
        crate::messages_capnp::SpeechPriority::Interrupt => SpeechPriority::Interrupt,
    }
}

fn layer_role_to_capnp(role: LayerRole) -> crate::messages_capnp::LayerRole {
    match role {
        // Intent / user-defined
        LayerRole::AllowedArea => crate::messages_capnp::LayerRole::AllowedArea,
        LayerRole::Preferred => crate::messages_capnp::LayerRole::Preferred,
        LayerRole::Keepout => crate::messages_capnp::LayerRole::Keepout,
        LayerRole::SoftExclusion => crate::messages_capnp::LayerRole::SoftExclusion,
        LayerRole::WorkArea => crate::messages_capnp::LayerRole::WorkArea,
        LayerRole::Target => crate::messages_capnp::LayerRole::Target,
        LayerRole::Queue => crate::messages_capnp::LayerRole::Queue,
        LayerRole::ControlZone => crate::messages_capnp::LayerRole::ControlZone,
        LayerRole::TriggerZone => crate::messages_capnp::LayerRole::TriggerZone,
        // State / system-generated
        LayerRole::Coverage => crate::messages_capnp::LayerRole::Coverage,
        LayerRole::Occupied => crate::messages_capnp::LayerRole::Occupied,
        LayerRole::Reserved => crate::messages_capnp::LayerRole::Reserved,
        LayerRole::Consumed => crate::messages_capnp::LayerRole::Consumed,
        LayerRole::Hazard => crate::messages_capnp::LayerRole::Hazard,
        LayerRole::Blocked => crate::messages_capnp::LayerRole::Blocked,
        LayerRole::PlannedPath => crate::messages_capnp::LayerRole::PlannedPath,
    }
}

fn layer_role_from_capnp(role: crate::messages_capnp::LayerRole) -> LayerRole {
    match role {
        // Intent / user-defined
        crate::messages_capnp::LayerRole::AllowedArea => LayerRole::AllowedArea,
        crate::messages_capnp::LayerRole::Preferred => LayerRole::Preferred,
        crate::messages_capnp::LayerRole::Keepout => LayerRole::Keepout,
        crate::messages_capnp::LayerRole::SoftExclusion => LayerRole::SoftExclusion,
        crate::messages_capnp::LayerRole::WorkArea => LayerRole::WorkArea,
        crate::messages_capnp::LayerRole::Target => LayerRole::Target,
        crate::messages_capnp::LayerRole::Queue => LayerRole::Queue,
        crate::messages_capnp::LayerRole::ControlZone => LayerRole::ControlZone,
        crate::messages_capnp::LayerRole::TriggerZone => LayerRole::TriggerZone,
        // Unused placeholder — fall back to AllowedArea
        crate::messages_capnp::LayerRole::Unused9 => LayerRole::AllowedArea,
        // State / system-generated
        crate::messages_capnp::LayerRole::Coverage => LayerRole::Coverage,
        crate::messages_capnp::LayerRole::Occupied => LayerRole::Occupied,
        crate::messages_capnp::LayerRole::Reserved => LayerRole::Reserved,
        crate::messages_capnp::LayerRole::Consumed => LayerRole::Consumed,
        crate::messages_capnp::LayerRole::Hazard => LayerRole::Hazard,
        crate::messages_capnp::LayerRole::Blocked => LayerRole::Blocked,
        crate::messages_capnp::LayerRole::PlannedPath => LayerRole::PlannedPath,
    }
}

fn geometry_type_to_capnp(gt: &GeometryType) -> crate::messages_capnp::GeometryType {
    match gt {
        GeometryType::Point => crate::messages_capnp::GeometryType::Point,
        GeometryType::LineString => crate::messages_capnp::GeometryType::LineString,
        GeometryType::Polygon => crate::messages_capnp::GeometryType::Polygon,
        GeometryType::MultiPoint => crate::messages_capnp::GeometryType::MultiPoint,
        GeometryType::MultiLineString => crate::messages_capnp::GeometryType::MultiLineString,
        GeometryType::MultiPolygon => crate::messages_capnp::GeometryType::MultiPolygon,
    }
}

fn geometry_type_from_capnp(gt: crate::messages_capnp::GeometryType) -> GeometryType {
    match gt {
        crate::messages_capnp::GeometryType::Point => GeometryType::Point,
        crate::messages_capnp::GeometryType::LineString => GeometryType::LineString,
        crate::messages_capnp::GeometryType::Polygon => GeometryType::Polygon,
        crate::messages_capnp::GeometryType::MultiPoint => GeometryType::MultiPoint,
        crate::messages_capnp::GeometryType::MultiLineString => GeometryType::MultiLineString,
        crate::messages_capnp::GeometryType::MultiPolygon => GeometryType::MultiPolygon,
    }
}

fn scope_type_to_capnp(scope: ScopeType) -> crate::messages_capnp::ScopeType {
    match scope {
        ScopeType::Robot => crate::messages_capnp::ScopeType::Robot,
        ScopeType::Fleet => crate::messages_capnp::ScopeType::Fleet,
        ScopeType::World => crate::messages_capnp::ScopeType::World,
        ScopeType::User => crate::messages_capnp::ScopeType::User,
    }
}

fn scope_type_from_capnp(scope: crate::messages_capnp::ScopeType) -> ScopeType {
    match scope {
        crate::messages_capnp::ScopeType::Robot => ScopeType::Robot,
        crate::messages_capnp::ScopeType::Fleet => ScopeType::Fleet,
        crate::messages_capnp::ScopeType::World => ScopeType::World,
        crate::messages_capnp::ScopeType::User => ScopeType::User,
    }
}

fn object_type_to_capnp(obj_type: ObjectType) -> crate::messages_capnp::ObjectType {
    match obj_type {
        ObjectType::Unknown => crate::messages_capnp::ObjectType::Unknown,
        ObjectType::Person => crate::messages_capnp::ObjectType::Person,
        ObjectType::Animal => crate::messages_capnp::ObjectType::Animal,
        ObjectType::Vehicle => crate::messages_capnp::ObjectType::Vehicle,
        ObjectType::Robot => crate::messages_capnp::ObjectType::Robot,
        ObjectType::Structure => crate::messages_capnp::ObjectType::Structure,
        ObjectType::Equipment => crate::messages_capnp::ObjectType::Equipment,
        ObjectType::Obstacle => crate::messages_capnp::ObjectType::Obstacle,
        ObjectType::Debris => crate::messages_capnp::ObjectType::Debris,
        ObjectType::Hazard => crate::messages_capnp::ObjectType::Hazard,
        ObjectType::Marker => crate::messages_capnp::ObjectType::Marker,
        ObjectType::PointOfInterest => crate::messages_capnp::ObjectType::PointOfInterest,
    }
}

fn object_type_from_capnp(obj_type: crate::messages_capnp::ObjectType) -> ObjectType {
    match obj_type {
        crate::messages_capnp::ObjectType::Unknown => ObjectType::Unknown,
        crate::messages_capnp::ObjectType::Person => ObjectType::Person,
        crate::messages_capnp::ObjectType::Animal => ObjectType::Animal,
        crate::messages_capnp::ObjectType::Vehicle => ObjectType::Vehicle,
        crate::messages_capnp::ObjectType::Robot => ObjectType::Robot,
        crate::messages_capnp::ObjectType::Structure => ObjectType::Structure,
        crate::messages_capnp::ObjectType::Equipment => ObjectType::Equipment,
        crate::messages_capnp::ObjectType::Obstacle => ObjectType::Obstacle,
        crate::messages_capnp::ObjectType::Debris => ObjectType::Debris,
        crate::messages_capnp::ObjectType::Hazard => ObjectType::Hazard,
        crate::messages_capnp::ObjectType::Marker => ObjectType::Marker,
        crate::messages_capnp::ObjectType::PointOfInterest => ObjectType::PointOfInterest,
    }
}

fn capability_timing_to_capnp(
    timing: &CapabilityTiming,
) -> crate::messages_capnp::CapabilityTiming {
    match timing {
        CapabilityTiming::ImmediateTrigger => {
            crate::messages_capnp::CapabilityTiming::ImmediateTrigger
        }
        CapabilityTiming::AtTargetTrigger => {
            crate::messages_capnp::CapabilityTiming::AtTargetTrigger
        }
        CapabilityTiming::DuringStep => crate::messages_capnp::CapabilityTiming::DuringStep,
        CapabilityTiming::DuringFeature => crate::messages_capnp::CapabilityTiming::DuringFeature,
        CapabilityTiming::FinalTrigger => crate::messages_capnp::CapabilityTiming::FinalTrigger,
    }
}

fn capability_timing_from_capnp(
    timing: crate::messages_capnp::CapabilityTiming,
) -> CapabilityTiming {
    match timing {
        crate::messages_capnp::CapabilityTiming::ImmediateTrigger => {
            CapabilityTiming::ImmediateTrigger
        }
        crate::messages_capnp::CapabilityTiming::AtTargetTrigger => {
            CapabilityTiming::AtTargetTrigger
        }
        crate::messages_capnp::CapabilityTiming::DuringStep => CapabilityTiming::DuringStep,
        crate::messages_capnp::CapabilityTiming::DuringFeature => CapabilityTiming::DuringFeature,
        crate::messages_capnp::CapabilityTiming::FinalTrigger => CapabilityTiming::FinalTrigger,
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::spatial_types::{
        GeoPoint, GeoPose, LocalPoint, LocalPose, LocalRoute, LocalShape, Pose, SpatialData,
        Wgs84PolygonData, Wgs84Shape,
    };

    fn sample_wgs84_feature(id: &str) -> Feature {
        Feature {
            identity: AssetIdentity {
                id: id.to_string(),
                name: "Test Feature".to_string(),
                rev: 1,
                hash: "abc123".to_string(),
            },
            layer_role: LayerRole::WorkArea,
            scope: ScopeType::Fleet,
            data: SpatialData::Wgs84Shape(Wgs84Shape::Polygon(Wgs84PolygonData {
                exterior: vec![
                    GeoPoint {
                        lon: -122.4194,
                        lat: 37.7749,
                    },
                    GeoPoint {
                        lon: -122.4195,
                        lat: 37.7750,
                    },
                    GeoPoint {
                        lon: -122.4196,
                        lat: 37.7749,
                    },
                ],
                holes: vec![],
            })),
            expires_at_ms: 1_700_000_000_000,
            min_z_m: 0.0,
            max_z_m: 100.0,
            level_id: 0,
        }
    }

    fn sample_local_feature(id: &str) -> Feature {
        Feature {
            identity: AssetIdentity {
                id: id.to_string(),
                name: "Local Feature".to_string(),
                rev: 2,
                hash: String::new(),
            },
            layer_role: LayerRole::Target,
            scope: ScopeType::Robot,
            data: SpatialData::LocalRoute(LocalRoute::Path(vec![
                LocalPose {
                    x: 1.0,
                    y: 2.0,
                    altitude_m: 0.0,
                    heading_rad: 0.5,
                },
                LocalPose {
                    x: 3.0,
                    y: 4.0,
                    altitude_m: 0.0,
                    heading_rad: 1.0,
                },
            ])),
            expires_at_ms: 0,
            min_z_m: 0.0,
            max_z_m: 0.0,
            level_id: 0,
        }
    }

    #[test]
    fn test_global_motion_telemetry_roundtrip() {
        let original = GlobalMotionTelemetry {
            unix_time_ms: 1234567890,
            pose: Some(GeoPose {
                lon_deg: -122.4194,
                lat_deg: 37.7749,
                altitude_m: 10.0,
                heading_deg: 45.0,
            }),
            velocity: Some(VelTwist {
                forward_mps: 1.5,
                angular_radps: 0.3,
            }),
            gps_fix: Some(GpsFixType::GBAS),
            gps_source: GpsSource::Real,
        };

        let uplink = RobotUplinkMsg::GlobalMotionTelemetry(original.clone());
        let serialized = serialize_robot_uplink_msg(&uplink).expect("Failed to serialize");
        let deserialized =
            deserialize_robot_uplink_msg(&serialized).expect("Failed to deserialize");

        match deserialized {
            RobotUplinkMsg::GlobalMotionTelemetry(motion) => assert_eq!(motion, original),
            _ => panic!("Expected GlobalMotionTelemetry variant"),
        }
    }

    #[test]
    fn test_local_motion_telemetry_roundtrip() {
        for (accuracy_m, frame) in [
            (Some(0.5_f32), Some("odom".to_string())),
            (None, None),
        ] {
            let original = LocalMotionTelemetry {
                unix_time_ms: 42,
                pose: LocalPose {
                    x: 1.0,
                    y: 2.0,
                    altitude_m: 0.5,
                    heading_rad: 1.2,
                },
                velocity: VelTwist {
                    forward_mps: 0.8,
                    angular_radps: -0.1,
                },
                accuracy_m,
                frame,
            };

            let uplink = RobotUplinkMsg::LocalMotionTelemetry(original.clone());
            let serialized = serialize_robot_uplink_msg(&uplink).expect("Failed to serialize");
            let deserialized =
                deserialize_robot_uplink_msg(&serialized).expect("Failed to deserialize");

            match deserialized {
                RobotUplinkMsg::LocalMotionTelemetry(motion) => assert_eq!(motion, original),
                _ => panic!("Expected LocalMotionTelemetry variant"),
            }
        }
    }

    #[test]
    fn test_capabilities_with_settings_roundtrip() {
        use crate::core_model::{
            Capabilities, CapabilityType, DangerLevel, DisplayHint, GeometryType, LayerRole, Range,
            ResourceProducerDescriptor, Role, SensorDescriptor, ServiceCapability,
            SettingDescriptor, ValueType,
        };

        let original = Capabilities {
            services: vec![ServiceCapability {
                name: "test_service".to_string(),
                description: "Test Service".to_string(),
                capability_type: CapabilityType::Trigger,
                confirmation_message: Some("Are you sure?".to_string()),
                state_key: None,
                requires_role: Role::Operator,
                danger_level: DangerLevel::Normal,
            }],
            sensors: vec![SensorDescriptor {
                key: "temp".to_string(),
                label: "Temperature".to_string(),
                value_type: ValueType::Number,
                unit: Some("°C".to_string()),
                range: Some(Range {
                    min: 0.0,
                    max: 100.0,
                }),
                display_hint: DisplayHint::Temperature,
            }],
            settings: vec![
                SettingDescriptor {
                    key: "max_speed".to_string(),
                    label: "Maximum Speed".to_string(),
                    value_type: ValueType::Number,
                    default: serde_json::json!(5.0),
                    range: Some(Range {
                        min: 0.0,
                        max: 10.0,
                    }),
                    step: Some(0.5),
                },
                SettingDescriptor {
                    key: "auto_mode".to_string(),
                    label: "Auto Mode".to_string(),
                    value_type: ValueType::Bool,
                    default: serde_json::json!(true),
                    range: None,
                    step: None,
                },
                SettingDescriptor {
                    key: "mode_select".to_string(),
                    label: "Mode Selection".to_string(),
                    value_type: ValueType::Enum {
                        options: vec!["slow".to_string(), "medium".to_string(), "fast".to_string()],
                    },
                    default: serde_json::json!("medium"),
                    range: None,
                    step: None,
                },
            ],
            resource_producers: vec![ResourceProducerDescriptor {
                resource_producer_id: "coverage".to_string(),
                label: "Mowed Area".to_string(),
                description: Some("Area the robot has mowed".to_string()),
                layer_role: LayerRole::Coverage,
                geometry_type: GeometryType::MultiPolygon,
            }],
        };

        let uplink = RobotUplinkMsg::Capabilities(original.clone());
        let serialized = serialize_robot_uplink_msg(&uplink).expect("Failed to serialize");
        let deserialized =
            deserialize_robot_uplink_msg(&serialized).expect("Failed to deserialize");

        match deserialized {
            RobotUplinkMsg::Capabilities(caps) => {
                assert_eq!(caps.services.len(), 1);
                assert_eq!(caps.sensors.len(), 1);
                assert_eq!(caps.settings.len(), 3);
                assert_eq!(caps.resource_producers.len(), 1);
                assert_eq!(caps.resource_producers[0].resource_producer_id, "coverage");
                assert_eq!(caps.resource_producers[0].label, "Mowed Area");
                assert_eq!(
                    caps.resource_producers[0].description.as_deref(),
                    Some("Area the robot has mowed")
                );

                let setting0 = &caps.settings[0];
                assert_eq!(setting0.key, "max_speed");
                assert!(matches!(setting0.value_type, ValueType::Number));
                assert_eq!(setting0.default, serde_json::json!(5.0));
                assert_eq!(setting0.range.as_ref().unwrap().max, 10.0);
                assert_eq!(setting0.step, Some(0.5));

                let setting1 = &caps.settings[1];
                assert_eq!(setting1.key, "auto_mode");
                assert!(matches!(setting1.value_type, ValueType::Bool));
                assert!(setting1.range.is_none());

                let setting2 = &caps.settings[2];
                assert!(matches!(setting2.value_type, ValueType::Enum { .. }));
                assert!(setting2.default.is_string());
            }
            _ => panic!("Expected Capabilities variant"),
        }
    }

    #[test]
    fn test_robot_command_set_mode_roundtrip() {
        let original = RobotCommand::SetRobotMode(RobotMode::Maintenance);
        let serialized = serialize_robot_command(&original).expect("Failed to serialize");
        let deserialized = deserialize_robot_command(&serialized).expect("Failed to deserialize");
        assert_eq!(original, deserialized);
    }

    #[test]
    fn test_robot_command_pause_resume_roundtrip() {
        for original in [RobotCommand::Pause, RobotCommand::Resume] {
            let serialized = serialize_robot_command(&original).expect("Failed to serialize");
            let deserialized =
                deserialize_robot_command(&serialized).expect("Failed to deserialize");
            assert_eq!(original, deserialized);
        }
    }

    #[test]
    fn test_robot_command_velocity_roundtrip() {
        let original = RobotCommand::VelocityCmd(VelTwist {
            forward_mps: 2.0,
            angular_radps: 0.5,
        });
        let serialized = serialize_robot_command(&original).expect("Failed to serialize");
        let deserialized = deserialize_robot_command(&serialized).expect("Failed to deserialize");
        assert_eq!(original, deserialized);
    }

    #[test]
    fn test_robot_command_navigate_to_roundtrip() {
        for pose in [
            Pose::GeoPose(GeoPose {
                lon_deg: -122.0,
                lat_deg: 37.0,
                altitude_m: 10.0,
                heading_deg: 90.0,
            }),
            Pose::LocalPose(LocalPose {
                x: 1.5,
                y: -2.5,
                altitude_m: 0.0,
                heading_rad: 0.25,
            }),
        ] {
            let original = RobotCommand::NavigateTo(pose);
            let serialized = serialize_robot_command(&original).expect("Failed to serialize");
            let deserialized =
                deserialize_robot_command(&serialized).expect("Failed to deserialize");
            assert_eq!(original, deserialized);
        }
    }

    #[test]
    fn test_robot_command_invoke_service_roundtrip() {
        for setting in [Some(true), None] {
            let original = RobotCommand::InvokeService(ServiceCall {
                service_name: "start_mowing".to_string(),
                setting,
            });
            let serialized = serialize_robot_command(&original).expect("Failed to serialize");
            let deserialized =
                deserialize_robot_command(&serialized).expect("Failed to deserialize");
            assert_eq!(original, deserialized);
        }
    }

    #[test]
    fn test_robot_command_control_mission_run_roundtrip() {
        let original = RobotCommand::ControlMissionRun(ControlMissionRun {
            mission_id: "m-1".to_string(),
            run_id: "r-1".to_string(),
            action: MissionRunAction::Abort,
        });
        let serialized = serialize_robot_command(&original).expect("Failed to serialize");
        let deserialized = deserialize_robot_command(&serialized).expect("Failed to deserialize");
        assert_eq!(original, deserialized);
    }

    #[test]
    fn test_robot_command_update_settings_roundtrip() {
        let original = RobotCommand::UpdateSettings(vec![
            SettingUpdate {
                key: "speed".to_string(),
                value: Value::Number(1.5),
            },
            SettingUpdate {
                key: "name".to_string(),
                value: Value::String("bot".to_string()),
            },
        ]);
        let serialized = serialize_robot_command(&original).expect("Failed to serialize");
        let deserialized = deserialize_robot_command(&serialized).expect("Failed to deserialize");
        assert_eq!(original, deserialized);
    }

    #[test]
    fn test_robot_command_update_feature_wgs84_roundtrip() {
        let original =
            RobotCommand::UpdateFeature(FeatureOp::Upsert(sample_wgs84_feature("feat-1")));
        let serialized = serialize_robot_command(&original).expect("Failed to serialize");
        let deserialized = deserialize_robot_command(&serialized).expect("Failed to deserialize");
        assert_eq!(original, deserialized);
    }

    #[test]
    fn test_robot_command_update_feature_local_roundtrip() {
        let original =
            RobotCommand::UpdateFeature(FeatureOp::Upsert(sample_local_feature("feat-2")));
        let serialized = serialize_robot_command(&original).expect("Failed to serialize");
        let deserialized = deserialize_robot_command(&serialized).expect("Failed to deserialize");
        assert_eq!(original, deserialized);
    }

    #[test]
    fn test_robot_command_object_roundtrip() {
        let original = RobotCommand::Object(ObjectOp::Upsert(Object {
            identity: AssetIdentity {
                id: "o-1".to_string(),
                name: "obstacle".to_string(),
                rev: 0,
                hash: String::new(),
            },
            unix_time_ms: 100,
            expires_at_ms: 200,
            scope: ScopeType::World,
            object_type: ObjectType::Vehicle,
            pose: Pose::GeoPose(GeoPose {
                lon_deg: -1.0,
                lat_deg: 2.0,
                altitude_m: 3.0,
                heading_deg: 4.0,
            }),
            trajectory: Trajectory {
                speed_mps: 1.0,
                course_deg: 90.0,
                vertical_rate_mps: 0.0,
            },
            radius_m: 0.5,
        }));
        let serialized = serialize_robot_command(&original).expect("Failed to serialize");
        let deserialized = deserialize_robot_command(&serialized).expect("Failed to deserialize");
        assert_eq!(original, deserialized);
    }

    #[test]
    fn test_robot_command_agent_text_request_roundtrip() {
        let original = RobotCommand::AgentTextRequest(AgentTextRequest {
            text: "go check the north field".to_string(),
            intent: AgentTextIntent::OperatorInstruction,
            response_required: true,
        });
        let serialized = serialize_robot_command(&original).expect("Failed to serialize");
        let deserialized = deserialize_robot_command(&serialized).expect("Failed to deserialize");
        assert_eq!(original, deserialized);
    }

    #[test]
    fn test_robot_command_say_text_roundtrip() {
        let original = RobotCommand::SayText(SayTextRequest {
            text: "hello".to_string(),
            priority: SpeechPriority::Interrupt,
        });
        let serialized = serialize_robot_command(&original).expect("Failed to serialize");
        let deserialized = deserialize_robot_command(&serialized).expect("Failed to deserialize");
        assert_eq!(original, deserialized);
    }

    #[test]
    fn test_robot_command_message_confirmation_roundtrip() {
        let original = RobotCommand::MessageConfirmation(MessageConfirmation {
            confirmed: true,
            confirmation_id: "c-1".to_string(),
        });
        let serialized = serialize_robot_command(&original).expect("Failed to serialize");
        let deserialized = deserialize_robot_command(&serialized).expect("Failed to deserialize");
        assert_eq!(original, deserialized);
    }

    #[test]
    fn test_robot_command_teleop_joy_roundtrip() {
        let original = RobotCommand::TeleopJoy(TeleopJoy {
            source_id: "webrtc-ui".to_string(),
            session_id: "session-42".to_string(),
            axes: vec![0.0, -0.75, 1.0, 0.25],
            buttons: vec![true, false, false, true, false],
            timestamp_ms: 1_700_000_000_123,
        });
        let serialized = serialize_robot_command(&original).expect("Failed to serialize");
        let deserialized = deserialize_robot_command(&serialized).expect("Failed to deserialize");
        assert_eq!(original, deserialized);
    }

    #[test]
    fn test_reported_feature_uplink_roundtrip() {
        // Cover both the Upsert (with optional z fields present and absent) and
        // Clear ops.
        let upsert = ReportedFeatureUpdate {
            resource_producer_id: "coverage".to_string(),
            timestamp_ms: 1_700_000_000_000,
            op: ReportedFeatureOp::Upsert(ReportedFeature {
                name: "Mowed Area".to_string(),
                layer_role: LayerRole::Coverage,
                scope: ScopeType::Fleet,
                data: SpatialData::LocalShape(LocalShape::Point(LocalPoint { x: 5.0, y: 6.0 })),
                expires_at_ms: 123,
                min_z_m: Some(0.0),
                max_z_m: None,
                level_id: Some(2),
            }),
        };
        let clear = ReportedFeatureUpdate {
            resource_producer_id: "coverage".to_string(),
            timestamp_ms: 1_700_000_000_001,
            op: ReportedFeatureOp::Clear,
        };

        for original in [upsert, clear] {
            let uplink = RobotUplinkMsg::Feature(original.clone());
            let serialized = serialize_robot_uplink_msg(&uplink).expect("Failed to serialize");
            let deserialized =
                deserialize_robot_uplink_msg(&serialized).expect("Failed to deserialize");

            match deserialized {
                RobotUplinkMsg::Feature(update) => assert_eq!(update, original),
                _ => panic!("Expected Feature variant"),
            }
        }
    }

    #[test]
    fn test_allowed_commands_roundtrip() {
        let original = AllowedCommands {
            set_mode: true,
            assign_mission: false,
            control_mission_run: true,
            invoke_service: false,
            update_settings: true,
            update_feature: false,
            spatial_directive: true,
            object: false,
            go_to: true,
            velocity_cmd: false,
            teleop_joy: true,
            agent_text_request: false,
            say_text_request: true,
            message_confirmation: false,
        };

        let uplink = RobotUplinkMsg::AllowedCommands(original.clone());
        let serialized = serialize_robot_uplink_msg(&uplink).expect("Failed to serialize");
        let deserialized =
            deserialize_robot_uplink_msg(&serialized).expect("Failed to deserialize");

        match deserialized {
            RobotUplinkMsg::AllowedCommands(allowed) => assert_eq!(allowed, original),
            _ => panic!("Expected AllowedCommands variant"),
        }
    }

    #[test]
    fn test_message_uplink_roundtrip() {
        let original = Message {
            unix_time_ms: 1,
            level: MessageLevel::Warn,
            message: "hi".to_string(),
            needs_confirmation: true,
            confirmation_id: "c-9".to_string(),
        };

        let uplink = RobotUplinkMsg::Message(original.clone());
        let serialized = serialize_robot_uplink_msg(&uplink).expect("Failed to serialize");
        let deserialized =
            deserialize_robot_uplink_msg(&serialized).expect("Failed to deserialize");

        match deserialized {
            RobotUplinkMsg::Message(m) => assert_eq!(m, original),
            _ => panic!("Expected Message variant"),
        }
    }

    #[test]
    fn test_consumable_status_roundtrip() {
        let original = ConsumableStatus {
            unix_time_ms: 1234567890,
            kind: "herbicide".to_string(),
            tank_id: "main".to_string(),
            level_pct: 65.0,
            amount_remaining: 32.5,
            unit: "liters".to_string(),
            is_critical: false,
        };

        let uplink = RobotUplinkMsg::ConsumableStatus(original.clone());
        let serialized = serialize_robot_uplink_msg(&uplink).expect("Failed to serialize");
        let deserialized =
            deserialize_robot_uplink_msg(&serialized).expect("Failed to deserialize");

        match deserialized {
            RobotUplinkMsg::ConsumableStatus(consumable) => {
                assert_eq!(consumable.unix_time_ms, original.unix_time_ms);
                assert_eq!(consumable.kind, original.kind);
                assert_eq!(consumable.tank_id, original.tank_id);
                assert_eq!(consumable.level_pct, original.level_pct);
                assert_eq!(consumable.amount_remaining, original.amount_remaining);
                assert_eq!(consumable.unit, original.unit);
                assert_eq!(consumable.is_critical, original.is_critical);
            }
            _ => panic!("Expected ConsumableStatus variant"),
        }
    }

    #[test]
    fn test_spatial_directive_status_roundtrip() {
        for distance in [Some(12.5_f32), None] {
            let original = SpatialDirectiveStatus {
                directive_id: "directive-abc".to_string(),
                last_eval_time_ms: 1234567890,
                active: true,
                inside_zone: false,
                distance_to_zone_m: distance,
            };

            let uplink = RobotUplinkMsg::SpatialDirectiveStatus(original.clone());
            let serialized = serialize_robot_uplink_msg(&uplink).expect("Failed to serialize");
            let deserialized =
                deserialize_robot_uplink_msg(&serialized).expect("Failed to deserialize");

            match deserialized {
                RobotUplinkMsg::SpatialDirectiveStatus(status) => assert_eq!(status, original),
                _ => panic!("Expected SpatialDirectiveStatus variant"),
            }
        }
    }

    #[test]
    fn test_nexus_command_roundtrip() {
        let original = NexusCommand {
            id: Uuid::from_u128(0x0123_4567_89ab_cdef_fedc_ba98_7654_3210),
            command: RobotCommand::VelocityCmd(VelTwist {
                forward_mps: 2.5,
                angular_radps: 0.3,
            }),
        };

        let serialized = serialize_nexus_command(&original).expect("Failed to serialize");
        let deserialized = deserialize_nexus_command(&serialized).expect("Failed to deserialize");

        assert_eq!(deserialized, original);
    }

    #[test]
    fn test_robot_ack_roundtrip_with_reason() {
        let original = RobotAck {
            command_id: Uuid::from_u128(0xdead_beef_cafe_babe_0123_4567_89ab_cdef),
            status: AckStatus::AgentRejected,
            reason: Some("unknown service".to_string()),
            received_at_ms: 1_700_000_000_000_000_000,
        };

        let serialized = serialize_robot_ack(&original).expect("Failed to serialize");
        let deserialized = deserialize_robot_ack(&serialized).expect("Failed to deserialize");

        assert_eq!(deserialized, original);
    }

    #[test]
    fn test_robot_ack_roundtrip_no_reason() {
        let original = RobotAck {
            command_id: Uuid::from_u128(0x1111_2222_3333_4444_5555_6666_7777_8888),
            status: AckStatus::RobotStateConfirmed,
            reason: None,
            received_at_ms: 42,
        };

        let serialized = serialize_robot_ack(&original).expect("Failed to serialize");
        let deserialized = deserialize_robot_ack(&serialized).expect("Failed to deserialize");

        assert_eq!(deserialized, original);
    }

    #[test]
    fn test_agent_telemetry_heartbeat_roundtrip() {
        let original = AgentTelemetry::Heartbeat(Heartbeat {
            seq: 99,
            sent_time_ms: 1_700_000_000_000_000_000,
        });

        let serialized = serialize_agent_telemetry(&original).expect("Failed to serialize");
        let deserialized = deserialize_agent_telemetry(&serialized).expect("Failed to deserialize");

        assert_eq!(deserialized, original);
    }

    #[test]
    fn test_agent_telemetry_robot_ack_roundtrip() {
        let original = AgentTelemetry::RobotAck(RobotAck {
            command_id: Uuid::from_u128(0xaaaa_bbbb_cccc_dddd_eeee_ffff_0000_1111),
            status: AckStatus::AgentReceived,
            reason: None,
            received_at_ms: 1234,
        });

        let serialized = serialize_agent_telemetry(&original).expect("Failed to serialize");
        let deserialized = deserialize_agent_telemetry(&serialized).expect("Failed to deserialize");

        assert_eq!(deserialized, original);
    }

    #[test]
    fn test_heartbeat_roundtrip() {
        let original = Heartbeat {
            seq: 12345,
            sent_time_ms: 1_700_000_000_000_000_000,
        };

        let serialized = serialize_heartbeat(&original).expect("Failed to serialize");
        let deserialized = deserialize_heartbeat(&serialized).expect("Failed to deserialize");

        assert_eq!(deserialized, original);
    }

    #[test]
    fn test_heartbeat_ack_roundtrip() {
        let original = HeartbeatAck {
            seq: 12345,
            robot_sent_time_ms: 1_700_000_000_000_000_000,
            server_recv_time_ms: 1_700_000_000_001_500_000,
        };

        let serialized = serialize_heartbeat_ack(&original).expect("Failed to serialize");
        let deserialized = deserialize_heartbeat_ack(&serialized).expect("Failed to deserialize");

        assert_eq!(deserialized, original);
    }
}