using RobotNet10.Shared.Geometry;

namespace RobotNet10.Shared.Sensor;

/// <summary>
/// Imu message (sensor_msgs/Imu)
/// Contains data from an IMU (Inertial Measurement Unit)
/// 
/// Accelerations should be in m/s^2 (not g's), and rotational velocity should be in rad/sec
/// 
/// If the covariance of the measurement is known, it should be filled in (if all you know is the 
/// variance of each measurement, e.g. from the datasheet, just put those along the diagonal)
/// A covariance matrix of all zeros will be interpreted as "covariance unknown", and to use the
/// data a covariance will have to be assumed or gotten from some other source
/// 
/// If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an orientation 
/// estimate), please set element 0 of the associated covariance matrix to -1
/// If you are interpreting this message, please check for a value of -1 in the first element of each
/// covariance matrix, and ignore the associated estimate.
/// </summary>
public struct Imu
{
    /// <summary>
    /// Header with timestamp and frame ID
    /// </summary>
    public Header Header { get; set; }

    /// <summary>
    /// Orientation quaternion (geometry_msgs/Quaternion)
    /// </summary>
    public Quaternion Orientation { get; set; }

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

    /// <summary>
    /// Size of orientation covariance matrix (3x3 = 9 elements)
    /// </summary>
    public const int OrientationCovarianceSize = 9;

    /// <summary>
    /// Angular velocity (geometry_msgs/Vector3)
    /// </summary>
    public Vector3 AngularVelocity { get; set; }

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

    /// <summary>
    /// Size of angular velocity covariance matrix (3x3 = 9 elements)
    /// </summary>
    public const int AngularVelocityCovarianceSize = 9;

    /// <summary>
    /// Linear acceleration (geometry_msgs/Vector3)
    /// </summary>
    public Vector3 LinearAcceleration { get; set; }

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

    /// <summary>
    /// Size of linear acceleration covariance matrix (3x3 = 9 elements)
    /// </summary>
    public const int LinearAccelerationCovarianceSize = 9;

    /// <summary>
    /// Default constructor
    /// </summary>
    public Imu()
    {
        Header = new Header();
        Orientation = new Quaternion();
        OrientationCovariance = new double[OrientationCovarianceSize];
        AngularVelocity = new Vector3();
        AngularVelocityCovariance = new double[AngularVelocityCovarianceSize];
        LinearAcceleration = new Vector3();
        LinearAccelerationCovariance = new double[LinearAccelerationCovarianceSize];
    }

    /// <summary>
    /// Constructor with parameters
    /// </summary>
    public Imu(
        Header header,
        Quaternion orientation,
        double[] orientationCovariance,
        Vector3 angularVelocity,
        double[] angularVelocityCovariance,
        Vector3 linearAcceleration,
        double[] linearAccelerationCovariance)
    {
        Header = header;
        Orientation = orientation;
        
        if (orientationCovariance == null || orientationCovariance.Length != OrientationCovarianceSize)
        {
            throw new ArgumentException($"Orientation covariance array must have {OrientationCovarianceSize} elements", nameof(orientationCovariance));
        }
        OrientationCovariance = orientationCovariance;

        AngularVelocity = angularVelocity;
        
        if (angularVelocityCovariance == null || angularVelocityCovariance.Length != AngularVelocityCovarianceSize)
        {
            throw new ArgumentException($"Angular velocity covariance array must have {AngularVelocityCovarianceSize} elements", nameof(angularVelocityCovariance));
        }
        AngularVelocityCovariance = angularVelocityCovariance;

        LinearAcceleration = linearAcceleration;
        
        if (linearAccelerationCovariance == null || linearAccelerationCovariance.Length != LinearAccelerationCovarianceSize)
        {
            throw new ArgumentException($"Linear acceleration covariance array must have {LinearAccelerationCovarianceSize} elements", nameof(linearAccelerationCovariance));
        }
        LinearAccelerationCovariance = linearAccelerationCovariance;
    }
}