namespace RobotNet10.Shared.Geometry;

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

    /// <summary>
    /// Covariance matrix (row-major order)
    /// The orientation parameters use a fixed-axis representation.
    /// In order, the parameters are:
    /// (vx, vy, vz, wx, wy, wz)
    /// </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 TwistWithCovariance()
    {
        Twist = new Twist();
        Covariance = new double[CovarianceSize];
    }

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