using RobotNet10.Shared;
using RobotNet10.Shared.Geometry;

namespace RobotNet10.Shared.Sensor;

/// <summary>
/// Odometry message (nav_msgs/Odometry)
/// This represents an estimate of a position and velocity in free space.
/// The pose in this message should be specified in the coordinate frame given by header.frame_id.
/// The twist in this message should be specified in the coordinate frame given by the child_frame_id.
/// </summary>
public struct Odometry
{
    /// <summary>
    /// Header with timestamp and frame ID (parent frame, typically "odom")
    /// </summary>
    public Header Header { get; set; }

    /// <summary>
    /// Child frame ID (typically "base_link" or "base_footprint")
    /// </summary>
    public string ChildFrameId { get; set; }

    /// <summary>
    /// Pose with covariance (PoseWithCovariance)
    /// </summary>
    public PoseWithCovariance Pose { get; set; }

    /// <summary>
    /// Twist with covariance (TwistWithCovariance)
    /// </summary>
    public TwistWithCovariance Twist { get; set; }

    /// <summary>
    /// Default constructor
    /// </summary>
    public Odometry()
    {
        Header = new Header();
        ChildFrameId = string.Empty;
        Pose = new PoseWithCovariance();
        Twist = new TwistWithCovariance();
    }

    /// <summary>
    /// Constructor with parameters
    /// </summary>
    public Odometry(Header header, string childFrameId, PoseWithCovariance pose, TwistWithCovariance twist)
    {
        Header = header;
        ChildFrameId = childFrameId;
        Pose = pose;
        Twist = twist;
    }
}