namespace RobotNet10.Shared.Geometry;

/// <summary>
/// PoseWithCovariance message (geometry_msgs/PoseWithCovariance)
/// A Pose with associated covariance matrix
/// </summary>
public struct PoseWithCovariance
{
    /// <summary>
    /// Pose
    /// </summary>
    public Pose Pose { get; set; }

    /// <summary>
    /// Covariance matrix (row-major order)
    /// The orientation parameters use a fixed-axis representation.
    /// In order, the parameters are:
    /// (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
    /// </summary>
    public double[] Covariance { get; set; }

    /// <summary>
    /// Size of covariance matrix (6x6 = 36 elements)
    /// </summary>
    public const int CovarianceSize = 36;

    /// <summary>
    /// Default constructor
    /// </summary>
    public PoseWithCovariance()
    {
        Pose = new Pose();
        Covariance = new double[CovarianceSize];
    }

    /// <summary>
    /// Constructor with parameters
    /// </summary>
    public PoseWithCovariance(Pose pose, double[] covariance)
    {
        Pose = pose;
        if (covariance == null || covariance.Length != CovarianceSize)
        {
            throw new ArgumentException($"Covariance array must have {CovarianceSize} elements", nameof(covariance));
        }
        Covariance = covariance;
    }
}