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

namespace RobotNet10.Shared.Sensor;

/// <summary>
/// MagneticField message (sensor_msgs/MagneticField)
/// Measurement of the Magnetic Field vector at a specific location
/// </summary>
public struct MagneticField
{
    /// <summary>
    /// Header with timestamp and frame ID
    /// </summary>
    public Header Header { get; set; }

    /// <summary>
    /// Magnetic field vector in Tesla
    /// </summary>
    public RobotNet10.Shared.Numbers.Vector3 MagneticFieldVector { get; set; }

    /// <summary>
    /// Covariance matrix (row-major about x, y, z axes)
    /// Size: 9 (3x3 matrix)
    /// </summary>
    public double[] MagneticFieldCovariance { get; set; }

    /// <summary>
    /// Size of magnetic field covariance matrix (3x3 = 9 elements)
    /// </summary>
    public const int MagneticFieldCovarianceSize = 9;

    /// <summary>
    /// Default constructor
    /// </summary>
    public MagneticField()
    {
        Header = new Header();
        MagneticFieldVector = new RobotNet10.Shared.Numbers.Vector3();
        MagneticFieldCovariance = new double[MagneticFieldCovarianceSize];
    }

    /// <summary>
    /// Constructor with parameters
    /// </summary>
    public MagneticField(Header header, RobotNet10.Shared.Numbers.Vector3 magneticFieldVector, double[] magneticFieldCovariance)
    {
        Header = header;
        MagneticFieldVector = magneticFieldVector;
        
        if (magneticFieldCovariance == null || magneticFieldCovariance.Length != MagneticFieldCovarianceSize)
        {
            throw new ArgumentException($"Magnetic field covariance array must have {MagneticFieldCovarianceSize} elements", nameof(magneticFieldCovariance));
        }
        MagneticFieldCovariance = magneticFieldCovariance;
    }
}