BQP/srcs/RobotNet10/RobotApp/Communication/CeresSharp.Test/ProblemTests.cs

using CeresSharp;
using CeresSharp.Enums;

namespace CeresSharp.Test;

[TestFixture]
public class ProblemTests
{
    [Test]
    public void CreateProblem_ShouldSucceed()
    {
        using var problem = new Problem();
        Assert.That(problem, Is.Not.Null);
        Assert.That(problem.NumParameterBlocks, Is.EqualTo(0));
        Assert.That(problem.NumResidualBlocks, Is.EqualTo(0));
    }

    [Test]
    public void AddParameterBlock_ShouldIncreaseCount()
    {
        using var problem = new Problem();
        var parameters = new double[] { 1.0, 2.0, 3.0 };
        
        problem.AddParameterBlock(parameters, parameters.Length);
        
        Assert.That(problem.NumParameterBlocks, Is.EqualTo(1));
    }

    [Test]
    public void AddMultipleParameterBlocks_ShouldIncreaseCount()
    {
        using var problem = new Problem();
        var params1 = new double[] { 1.0, 2.0 };
        var params2 = new double[] { 3.0, 4.0, 5.0 };
        
        problem.AddParameterBlock(params1, params1.Length);
        problem.AddParameterBlock(params2, params2.Length);
        
        Assert.That(problem.NumParameterBlocks, Is.EqualTo(2));
    }

    [Test]
    public void SetParameterBlockConstant_ShouldSucceed()
    {
        using var problem = new Problem();
        var parameters = new double[] { 1.0, 2.0 };
        problem.AddParameterBlock(parameters, parameters.Length);
        
        problem.SetParameterBlockConstant(parameters);
        
        // Should not throw
        Assert.Pass();
    }

    [Test]
    public void SetParameterBlockVariable_ShouldSucceed()
    {
        using var problem = new Problem();
        var parameters = new double[] { 1.0, 2.0 };
        problem.AddParameterBlock(parameters, parameters.Length);
        problem.SetParameterBlockConstant(parameters);
        
        problem.SetParameterBlockVariable(parameters);
        
        // Should not throw
        Assert.Pass();
    }

    [Test]
    public void SetParameterLowerBound_ShouldSucceed()
    {
        using var problem = new Problem();
        var parameters = new double[] { 1.0, 2.0 };
        problem.AddParameterBlock(parameters, parameters.Length);
        
        problem.SetParameterLowerBound(parameters, index: 0, lowerBound: 0.0);
        
        // Should not throw
        Assert.Pass();
    }

    [Test]
    public void SetParameterUpperBound_ShouldSucceed()
    {
        using var problem = new Problem();
        var parameters = new double[] { 1.0, 2.0 };
        problem.AddParameterBlock(parameters, parameters.Length);
        
        problem.SetParameterUpperBound(parameters, index: 0, upperBound: 10.0);
        
        // Should not throw
        Assert.Pass();
    }

    [Test]
    public void AddResidualBlock_ShouldIncreaseResidualCount()
    {
        using var problem = new Problem();
        var parameters = new double[] { 1.0 };
        problem.AddParameterBlock(parameters, parameters.Length);
        
        var costFunction = new AutoDiffCostFunction(
            (parameters, residuals) =>
            {
                residuals[0] = parameters[0][0] - 1.0;
                return true;
            },
            numResiduals: 1,
            parameterBlockSizes: new[] { 1 });
        
        var residualBlockId = problem.AddResidualBlock(
            costFunction,
            lossFunction: null,
            parameterBlocks: new[] { parameters });
        
        Assert.That(problem.NumResidualBlocks, Is.EqualTo(1));
        Assert.That(residualBlockId, Is.Not.EqualTo(IntPtr.Zero), "Residual block ID should not be zero");
    }

    [Test]
    public void AddResidualBlock_WithLossFunction_ShouldSucceed()
    {
        using var problem = new Problem();
        var parameters = new double[] { 1.0 };
        problem.AddParameterBlock(parameters, parameters.Length);
        
        var costFunction = new AutoDiffCostFunction(
            (parameters, residuals) =>
            {
                residuals[0] = parameters[0][0] - 1.0;
                return true;
            },
            numResiduals: 1,
            parameterBlockSizes: new[] { 1 });
        
        using var loss = new HuberLoss(1.0);
        var residualBlockId = problem.AddResidualBlock(
            costFunction,
            loss,
            parameterBlocks: new[] { parameters });
        
        Assert.That(problem.NumResidualBlocks, Is.EqualTo(1));
    }

    [Test]
    public void RemoveResidualBlock_ShouldDecreaseCount()
    {
        using var problem = new Problem();
        var parameters = new double[] { 1.0 };
        problem.AddParameterBlock(parameters, parameters.Length);
        
        var costFunction = new AutoDiffCostFunction(
            (parameters, residuals) =>
            {
                residuals[0] = parameters[0][0] - 1.0;
                return true;
            },
            numResiduals: 1,
            parameterBlockSizes: new[] { 1 });
        
        var residualBlockId = problem.AddResidualBlock(
            costFunction,
            lossFunction: null,
            parameterBlocks: new[] { parameters });
        
        Assert.That(problem.NumResidualBlocks, Is.EqualTo(1));
        
        problem.RemoveResidualBlock(residualBlockId);
        
        Assert.That(problem.NumResidualBlocks, Is.EqualTo(0));
    }

    [Test]
    public void SetManifold_ShouldSucceed()
    {
        using var problem = new Problem();
        var quaternion = new double[] { 0.0, 0.0, 0.0, 1.0 }; // qx, qy, qz, qw
        problem.AddParameterBlock(quaternion, quaternion.Length);
        
        using var manifold = new QuaternionManifold();
        problem.SetManifold(quaternion, manifold);
        
        // Should not throw
        Assert.Pass();
    }

    [Test]
    public void SetManifold_ThenRemove_ShouldSucceed()
    {
        using var problem = new Problem();
        var quaternion = new double[] { 0.0, 0.0, 0.0, 1.0 };
        problem.AddParameterBlock(quaternion, quaternion.Length);
        
        using var manifold = new QuaternionManifold();
        problem.SetManifold(quaternion, manifold);
        
        // Note: SetManifold doesn't accept null - manifold removal is not directly supported
        // The manifold will be removed when parameter block is removed or problem is disposed
        
        // Should not throw
        Assert.Pass();
    }


    [Test]
    public void IsParameterBlockConstant_ShouldReturnFalse_WhenVariable()
    {
        using var problem = new Problem();
        var parameters = new double[] { 1.0, 2.0 };
        problem.AddParameterBlock(parameters, parameters.Length);
        
        var isConstant = problem.IsParameterBlockConstant(parameters);
        
        Assert.That(isConstant, Is.False);
    }

    [Test]
    public void IsParameterBlockConstant_ShouldReturnTrue_WhenConstant()
    {
        using var problem = new Problem();
        var parameters = new double[] { 1.0, 2.0 };
        problem.AddParameterBlock(parameters, parameters.Length);
        problem.SetParameterBlockConstant(parameters);
        
        var isConstant = problem.IsParameterBlockConstant(parameters);
        
        Assert.That(isConstant, Is.True);
    }

    [Test]
    public void HasParameterBlock_ShouldReturnTrue_WhenExists()
    {
        using var problem = new Problem();
        var parameters = new double[] { 1.0, 2.0 };
        problem.AddParameterBlock(parameters, parameters.Length);
        
        var hasBlock = problem.HasParameterBlock(parameters);
        
        Assert.That(hasBlock, Is.True);
    }

    [Test]
    public void HasParameterBlock_ShouldReturnFalse_WhenNotExists()
    {
        using var problem = new Problem();
        var parameters = new double[] { 1.0, 2.0 };
        var fakeParams = new double[] { 99.0, 99.0 };
        
        problem.AddParameterBlock(parameters, parameters.Length);
        
        var hasBlock = problem.HasParameterBlock(fakeParams);
        
        Assert.That(hasBlock, Is.False);
    }

    [Test]
    public void GetParameterBlockSize_ShouldReturnCorrectSize()
    {
        using var problem = new Problem();
        var parameters = new double[] { 1.0, 2.0, 3.0 };
        problem.AddParameterBlock(parameters, parameters.Length);
        
        var size = problem.GetParameterBlockSize(parameters);
        
        Assert.That(size, Is.EqualTo(3));
    }

    [Test]
    public void GetParameterBlockSize_ShouldReturnMinusOne_WhenNotExists()
    {
        using var problem = new Problem();
        var fakeParams = new double[] { 99.0, 99.0 };
        
        var size = problem.GetParameterBlockSize(fakeParams);
        
        Assert.That(size, Is.EqualTo(-1));
    }

    [Test]
    public void HasManifold_ShouldReturnFalse_WhenNoManifold()
    {
        using var problem = new Problem();
        var parameters = new double[] { 1.0, 2.0, 3.0, 4.0 };
        problem.AddParameterBlock(parameters, parameters.Length);
        
        var hasManifold = problem.HasManifold(parameters);
        
        Assert.That(hasManifold, Is.False);
    }

    [Test]
    public void HasManifold_ShouldReturnTrue_WhenManifoldSet()
    {
        using var problem = new Problem();
        var quaternion = new double[] { 0.0, 0.0, 0.0, 1.0 };
        problem.AddParameterBlock(quaternion, quaternion.Length);
        
        using var manifold = new QuaternionManifold();
        problem.SetManifold(quaternion, manifold);
        
        var hasManifold = problem.HasManifold(quaternion);
        
        Assert.That(hasManifold, Is.True);
    }

    [Test]
    public void GetManifoldHandle_ShouldReturnZero_WhenNoManifold()
    {
        using var problem = new Problem();
        var parameters = new double[] { 1.0, 2.0 };
        problem.AddParameterBlock(parameters, parameters.Length);
        
        var handle = problem.GetManifoldHandle(parameters);
        
        Assert.That(handle, Is.EqualTo(IntPtr.Zero));
    }

    [Test]
    public void GetManifoldHandle_ShouldReturnNonZero_WhenManifoldSet()
    {
        using var problem = new Problem();
        var quaternion = new double[] { 0.0, 0.0, 0.0, 1.0 };
        problem.AddParameterBlock(quaternion, quaternion.Length);
        
        using var manifold = new QuaternionManifold();
        problem.SetManifold(quaternion, manifold);
        
        var handle = problem.GetManifoldHandle(quaternion);
        
        Assert.That(handle, Is.Not.EqualTo(IntPtr.Zero));
    }

    [Test]
    public void GetParameterBlockTangentSize_ShouldReturnTangentSize_WithManifold()
    {
        using var problem = new Problem();
        var quaternion = new double[] { 0.0, 0.0, 0.0, 1.0 }; // 4D ambient
        problem.AddParameterBlock(quaternion, quaternion.Length);
        
        using var manifold = new QuaternionManifold(); // 3D tangent
        problem.SetManifold(quaternion, manifold);
        
        var tangentSize = problem.GetParameterBlockTangentSize(quaternion);
        
        Assert.That(tangentSize, Is.EqualTo(3)); // Quaternion: 4D ambient → 3D tangent
    }

    [Test]
    public void GetParameterBlockTangentSize_ShouldReturnAmbientSize_WithoutManifold()
    {
        using var problem = new Problem();
        var parameters = new double[] { 1.0, 2.0, 3.0 }; // 3D
        problem.AddParameterBlock(parameters, parameters.Length);
        
        // No manifold set, so tangent size should equal ambient size
        var tangentSize = problem.GetParameterBlockTangentSize(parameters);
        
        Assert.That(tangentSize, Is.EqualTo(3)); // Without manifold, tangent = ambient
    }
}