Denso/srcs/RobotNet10/RobotApp/Communication/CeresSharp.Test/AutoDiffManifoldTests.cs

using CeresSharp;
using CeresSharp.Enums;

namespace CeresSharp.Test;

[TestFixture]
public class AutoDiffManifoldTests
{
    [Test]
    public void AutoDiffManifold_ShouldCreate()
    {
        // Euclidean manifold: Plus = x + delta, Minus = y - x
        using var manifold = new AutoDiffManifold(
            ambientSize: 3,
            tangentSize: 3,
            plus: (x, delta, xPlusDelta) =>
            {
                for (int i = 0; i < 3; i++)
                    xPlusDelta[i] = x[i] + delta[i];
                return true;
            },
            minus: (y, x, yMinusX) =>
            {
                for (int i = 0; i < 3; i++)
                    yMinusX[i] = y[i] - x[i];
                return true;
            });

        Assert.That(manifold, Is.Not.Null);
        Assert.That(manifold.AmbientSize, Is.EqualTo(3));
        Assert.That(manifold.TangentSize, Is.EqualTo(3));
    }

    [Test]
    public void AutoDiffManifold_Plus_ShouldWork()
    {
        // Test Plus operation: x + delta → x_plus_delta
        using var manifold = new AutoDiffManifold(
            ambientSize: 3,
            tangentSize: 3,
            plus: (x, delta, xPlusDelta) =>
            {
                for (int i = 0; i < 3; i++)
                    xPlusDelta[i] = x[i] + delta[i];
                return true;
            },
            minus: (y, x, yMinusX) =>
            {
                for (int i = 0; i < 3; i++)
                    yMinusX[i] = y[i] - x[i];
                return true;
            });

        // Test via Problem integration
        using var problem = new Problem();
        var parameters = new double[] { 1.0, 2.0, 3.0 };
        problem.AddParameterBlock(parameters, parameters.Length);
        problem.SetManifold(parameters, manifold);

        // Verify manifold was set
        Assert.That(problem.HasManifold(parameters), Is.True);
        Assert.That(problem.GetParameterBlockTangentSize(parameters), Is.EqualTo(3));
    }

    [Test]
    public void AutoDiffManifold_Minus_ShouldWork()
    {
        // Test Minus operation: y - x → y_minus_x
        using var manifold = new AutoDiffManifold(
            ambientSize: 3,
            tangentSize: 3,
            plus: (x, delta, xPlusDelta) =>
            {
                for (int i = 0; i < 3; i++)
                    xPlusDelta[i] = x[i] + delta[i];
                return true;
            },
            minus: (y, x, yMinusX) =>
            {
                for (int i = 0; i < 3; i++)
                    yMinusX[i] = y[i] - x[i];
                return true;
            });

        // Test via Problem integration
        using var problem = new Problem();
        var parameters = new double[] { 1.0, 2.0, 3.0 };
        problem.AddParameterBlock(parameters, parameters.Length);
        problem.SetManifold(parameters, manifold);

        // Verify manifold dimensions
        Assert.That(manifold.AmbientSize, Is.EqualTo(3));
        Assert.That(manifold.TangentSize, Is.EqualTo(3));
    }

    [Test]
    public void AutoDiffManifold_WithProblem_ShouldWork()
    {
        // Based on C test: Euclidean manifold integration
        using var problem = new Problem();
        var parameters = new double[] { 1.0, 2.0, 3.0 };
        problem.AddParameterBlock(parameters, parameters.Length);

        using var manifold = new AutoDiffManifold(
            ambientSize: 3,
            tangentSize: 3,
            plus: (x, delta, xPlusDelta) =>
            {
                for (int i = 0; i < 3; i++)
                    xPlusDelta[i] = x[i] + delta[i];
                return true;
            },
            minus: (y, x, yMinusX) =>
            {
                for (int i = 0; i < 3; i++)
                    yMinusX[i] = y[i] - x[i];
                return true;
            });

        problem.SetManifold(parameters, manifold);

        // Verify manifold was set
        Assert.That(problem.HasManifold(parameters), Is.True);
        Assert.That(problem.GetParameterBlockTangentSize(parameters), Is.EqualTo(3));
        Assert.That(problem.GetParameterBlockSize(parameters), Is.EqualTo(3));
    }

    [Test]
    public void AutoDiffManifold_InvalidSizes_ShouldThrow()
    {
        // Test invalid ambient size
        Assert.Throws<ArgumentException>(() =>
        {
            new AutoDiffManifold(
                ambientSize: 0,
                tangentSize: 3,
                plus: (x, delta, xPlusDelta) => true,
                minus: (y, x, yMinusX) => true);
        });

        // Test invalid tangent size
        Assert.Throws<ArgumentException>(() =>
        {
            new AutoDiffManifold(
                ambientSize: 3,
                tangentSize: 0,
                plus: (x, delta, xPlusDelta) => true,
                minus: (y, x, yMinusX) => true);
        });

        // Test tangent size > ambient size
        Assert.Throws<ArgumentException>(() =>
        {
            new AutoDiffManifold(
                ambientSize: 3,
                tangentSize: 4,
                plus: (x, delta, xPlusDelta) => true,
                minus: (y, x, yMinusX) => true);
        });
    }

    [Test]
    public void AutoDiffManifold_NullCallbacks_ShouldThrow()
    {
        // Test null Plus callback
        Assert.Throws<ArgumentNullException>(() =>
        {
            new AutoDiffManifold(
                ambientSize: 3,
                tangentSize: 3,
                plus: null!,
                minus: (y, x, yMinusX) => true);
        });

        // Test null Minus callback
        Assert.Throws<ArgumentNullException>(() =>
        {
            new AutoDiffManifold(
                ambientSize: 3,
                tangentSize: 3,
                plus: (x, delta, xPlusDelta) => true,
                minus: null!);
        });
    }

    [Test]
    public void AutoDiffManifold_Dispose_ShouldNotCrash()
    {
        var manifold = new AutoDiffManifold(
            ambientSize: 3,
            tangentSize: 3,
            plus: (x, delta, xPlusDelta) =>
            {
                for (int i = 0; i < 3; i++)
                    xPlusDelta[i] = x[i] + delta[i];
                return true;
            },
            minus: (y, x, yMinusX) =>
            {
                for (int i = 0; i < 3; i++)
                    yMinusX[i] = y[i] - x[i];
                return true;
            });

        // Dispose should not crash
        manifold.Dispose();

        // Verify disposed
        Assert.Pass("Dispose completed without crash");
    }

    [Test]
    public void AutoDiffManifold_UsingStatement_ShouldWork()
    {
        // Test with using statement
        using var manifold = new AutoDiffManifold(
            ambientSize: 3,
            tangentSize: 3,
            plus: (x, delta, xPlusDelta) =>
            {
                for (int i = 0; i < 3; i++)
                    xPlusDelta[i] = x[i] + delta[i];
                return true;
            },
            minus: (y, x, yMinusX) =>
            {
                for (int i = 0; i < 3; i++)
                    yMinusX[i] = y[i] - x[i];
                return true;
            });

        // Should not throw
        Assert.That(manifold, Is.Not.Null);
    }

    [Test]
    public void AutoDiffManifold_DifferentSizes_ShouldWork()
    {
        // Test with different ambient and tangent sizes (e.g., quaternion-like: 4D ambient, 3D tangent)
        using var manifold = new AutoDiffManifold(
            ambientSize: 4,
            tangentSize: 3,
            plus: (x, delta, xPlusDelta) =>
            {
                // Simple example: just copy x and add delta to first 3 elements
                for (int i = 0; i < 4; i++)
                    xPlusDelta[i] = x[i];
                for (int i = 0; i < 3; i++)
                    xPlusDelta[i] += delta[i];
                return true;
            },
            minus: (y, x, yMinusX) =>
            {
                // Simple example: difference of first 3 elements
                for (int i = 0; i < 3; i++)
                    yMinusX[i] = y[i] - x[i];
                return true;
            });

        Assert.That(manifold.AmbientSize, Is.EqualTo(4));
        Assert.That(manifold.TangentSize, Is.EqualTo(3));
    }

    [Test]
    public void AutoDiffManifold_WithCostFunction_ShouldWork()
    {
        // Test AutoDiffManifold with a cost function (integration test)
        using var problem = new Problem();
        var parameters = new double[] { 1.0, 2.0, 3.0 };
        problem.AddParameterBlock(parameters, parameters.Length);

        using var manifold = new AutoDiffManifold(
            ambientSize: 3,
            tangentSize: 3,
            plus: (x, delta, xPlusDelta) =>
            {
                for (int i = 0; i < 3; i++)
                    xPlusDelta[i] = x[i] + delta[i];
                return true;
            },
            minus: (y, x, yMinusX) =>
            {
                for (int i = 0; i < 3; i++)
                    yMinusX[i] = y[i] - x[i];
                return true;
            });

        problem.SetManifold(parameters, manifold);

        // Add a cost function
        var costFunction = new AutoDiffCostFunction(
            (parameters, residuals) =>
            {
                // Simple cost: minimize ||parameters||^2
                residuals[0] = parameters[0][0] * parameters[0][0] +
                              parameters[0][1] * parameters[0][1] +
                              parameters[0][2] * parameters[0][2];
                return true;
            },
            numResiduals: 1,
            parameterBlockSizes: new[] { 3 });

        problem.AddResidualBlock(costFunction, lossFunction: null,
            parameterBlocks: new[] { parameters });

        // Verify problem setup
        Assert.That(problem.NumResidualBlocks, Is.EqualTo(1));
        Assert.That(problem.HasManifold(parameters), Is.True);
    }
}