namespace RobotNet10.Shared.Geometry;

/// <summary>
/// AccelWithCovariance message (geometry_msgs/AccelWithCovariance)
/// An Accel with associated covariance matrix
/// </summary>
public struct AccelWithCovariance
{
    /// <summary>
    /// Acceleration
    /// </summary>
    public Accel Accel { get; set; }

    /// <summary>
    /// Covariance matrix (row-major order)
    /// The orientation parameters use a fixed-axis representation.
    /// In order, the parameters are:
    /// (ax, ay, az, 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 AccelWithCovariance()
    {
        Accel = new Accel();
        Covariance = new double[CovarianceSize];
    }

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