BQP/srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/Optimization/OptimizationHelpers.cs

using CartographerSharp.Transform;
using  RobotNet10.Shared.Numbers;

namespace CartographerSharp.Mapping.Internal.Optimization;

/// <summary>
/// Helper utilities for optimization problems.
/// Provides common operations for pose parameter conversion and angle normalization.
/// </summary>
public static class OptimizationHelpers
{
    /// <summary>
    /// Normalizes angle difference to [-pi, pi].
    /// Uses modulo-based approach for efficiency with large angles.
    /// </summary>
    /// <param name="angle">The angle to normalize.</param>
    /// <returns>Normalized angle in [-pi, pi].</returns>
    public static double NormalizeAngleDifference(double angle)
    {
        // Use modulo for efficiency - handles large angles in O(1)
        const double twoPi = 2.0 * Math.PI;
        angle = angle % twoPi;
        if (angle > Math.PI)
            angle -= twoPi;
        else if (angle < -Math.PI)
            angle += twoPi;
        return angle;
    }

    /// <summary>
    /// Converts Rigid2d pose to parameter array [x, y, theta].
    /// </summary>
    /// <param name="pose">The 2D pose.</param>
    /// <returns>Parameter array [x, y, theta].</returns>
    public static double[] Rigid2dToParameters(Rigid2d pose) => [ pose.Translation.X, pose.Translation.Y, pose.Rotation ];

    /// <summary>
    /// Converts parameter array [x, y, theta] to Rigid2d pose.
    /// </summary>
    /// <param name="parameters">Parameter array [x, y, theta].</param>
    /// <returns>The 2D pose.</returns>
    public static Rigid2d ParametersToRigid2d(double[] parameters)
    {
        if (parameters == null || parameters.Length < 3)
            throw new ArgumentException("Parameters array must have at least 3 elements", nameof(parameters));

        return new Rigid2d(
            new Vector2(parameters[0], parameters[1]),
            parameters[2]
        );
    }

    /// <summary>
    /// Converts Rigid3d pose to parameter arrays (rotation and translation).
    /// </summary>
    /// <param name="pose">The 3D pose.</param>
    /// <returns>Tuple of (rotation[4], translation[3]).</returns>
    public static (double[] rotation, double[] translation) Rigid3dToParameters(Rigid3d pose)
    {
        var rotation = new double[4]
        {
            pose.Rotation.W,
            pose.Rotation.X,
            pose.Rotation.Y,
            pose.Rotation.Z
        };
        var translation = new double[3]
        {
            pose.Translation.X,
            pose.Translation.Y,
            pose.Translation.Z
        };
        return (rotation, translation);
    }

    /// <summary>
    /// Converts parameter arrays to Rigid3d pose.
    /// </summary>
    /// <param name="rotation">Rotation parameters [w, x, y, z].</param>
    /// <param name="translation">Translation parameters [x, y, z].</param>
    /// <returns>The 3D pose.</returns>
    public static Rigid3d ParametersToRigid3d(double[] rotation, double[] translation)
    {
        if (rotation == null || rotation.Length < 4)
            throw new ArgumentException("Rotation array must have at least 4 elements", nameof(rotation));
        if (translation == null || translation.Length < 3)
            throw new ArgumentException("Translation array must have at least 3 elements", nameof(translation));

        // Convert from [w, x, y, z] to (x, y, z, w) for System.Numerics.Quaternion
        return new Rigid3d(
            new Vector3(translation[0], translation[1], translation[2]),
            new Quaternion(rotation[1], rotation[2], rotation[3], rotation[0])
        );
    }

    /// <summary>
    /// Converts Quaternion to parameter array [w, x, y, z].
    /// </summary>
    /// <param name="quaternion">The quaternion.</param>
    /// <returns>Parameter array [w, x, y, z].</returns>
    public static double[] QuaternionToParameters(Quaternion quaternion) => [ quaternion.W, quaternion.X, quaternion.Y, quaternion.Z ];

    /// <summary>
    /// Converts parameter array [w, x, y, z] to System.Numerics.Quaternion (x, y, z, w).
    /// Match C++: Eigen::Quaternion<T> uses (w, x, y, z) format.
    /// System.Numerics.Quaternion uses (x, y, z, w) format.
    /// </summary>
    /// <param name="parameters">Parameter array [w, x, y, z].</param>
    /// <returns>The quaternion.</returns>
    public static Quaternion ParametersToQuaternion(double[] parameters)
    {
        if (parameters == null || parameters.Length < 4)
            throw new ArgumentException("Parameters array must have at least 4 elements", nameof(parameters));

        // Convert from [w, x, y, z] to (x, y, z, w)
        return new Quaternion(
            parameters[1], // x
            parameters[2], // y
            parameters[3], // z
            parameters[0]  // w
        );
    }

    /// <summary>
    /// Converts Vector3 to parameter array [x, y, z].
    /// </summary>
    /// <param name="vector">The vector.</param>
    /// <returns>Parameter array [x, y, z].</returns>
    public static double[] Vector3ToParameters(Vector3 vector) => [ vector.X, vector.Y, vector.Z ];

    /// <summary>
    /// Converts parameter array [x, y, z] to Vector3.
    /// </summary>
    /// <param name="parameters">Parameter array [x, y, z].</param>
    /// <returns>The vector.</returns>
    public static Vector3 ParametersToVector3(double[] parameters)
    {
        if (parameters == null || parameters.Length < 3)
            throw new ArgumentException("Parameters array must have at least 3 elements", nameof(parameters));

        return new Vector3(
            parameters[0],
            parameters[1],
            parameters[2]
        );
    }

    /// <summary>
    /// Computes interpolation parameter for time-based interpolation.
    /// </summary>
    /// <param name="observationTime">The observation time.</param>
    /// <param name="prevTime">The previous node time.</param>
    /// <param name="nextTime">The next node time.</param>
    /// <returns>Interpolation parameter in [0, 1].</returns>
    public static double ComputeInterpolationParameter(long observationTime, long prevTime, long nextTime)
    {
        var timeDiff = nextTime - prevTime;
        if (timeDiff == 0)
            return 0.0;
        // Cast to double to avoid integer division
        return (double)(observationTime - prevTime) / timeDiff;
    }
}