I150/srcs/RobotNet10/RobotApp/Communication/CeresSharp.Test/CostFunctionTests.cs

using CeresSharp;
using CeresSharp.Enums;

namespace CeresSharp.Test;

[TestFixture]
public class CostFunctionTests
{
    [Test]
    public void AutoDiffCostFunction_SimpleLinear_ShouldWork()
    {
        var costFunction = new AutoDiffCostFunction(
            (parameters, residuals) =>
            {
                // Simple linear: residual = x - 1.0
                residuals[0] = parameters[0][0] - 1.0;
                return true;
            },
            numResiduals: 1,
            parameterBlockSizes: new[] { 1 });
        
        Assert.That(costFunction, Is.Not.Null);
    }

    [Test]
    public void AutoDiffCostFunction_MultipleParameterBlocks_ShouldWork()
    {
        var costFunction = new AutoDiffCostFunction(
            (parameters, residuals) =>
            {
                // residual = x[0] * y[0] - 2.0
                residuals[0] = parameters[0][0] * parameters[1][0] - 2.0;
                return true;
            },
            numResiduals: 1,
            parameterBlockSizes: new[] { 1, 1 });
        
        Assert.That(costFunction, Is.Not.Null);
    }

    [Test]
    public void AutoDiffCostFunction_MultipleResiduals_ShouldWork()
    {
        var costFunction = new AutoDiffCostFunction(
            (parameters, residuals) =>
            {
                // 2 residuals: [x - 1, x - 2]
                residuals[0] = parameters[0][0] - 1.0;
                residuals[1] = parameters[0][0] - 2.0;
                return true;
            },
            numResiduals: 2,
            parameterBlockSizes: new[] { 1 });
        
        Assert.That(costFunction, Is.Not.Null);
    }

    [Test]
    public void DynamicAutoDiffCostFunction_ShouldWork()
    {
        var costFunction = new DynamicAutoDiffCostFunction(
            (parameters, residuals) =>
            {
                residuals[0] = parameters[0][0] - 1.0;
                return true;
            },
            numResiduals: 1,
            parameterBlockSizes: new[] { 1 });
        
        Assert.That(costFunction, Is.Not.Null);
    }

    [Test]
    public void DynamicAutoDiffCostFunction_MultipleBlocks_ShouldWork()
    {
        var costFunction = new DynamicAutoDiffCostFunction(
            (parameters, residuals) =>
            {
                residuals[0] = parameters[0][0] + parameters[1][0] - 3.0;
                return true;
            },
            numResiduals: 1,
            parameterBlockSizes: new[] { 1, 2 });
        
        Assert.That(costFunction, Is.Not.Null);
    }

    [Test]
    public void NumericDiffCostFunction_Central_ShouldWork()
    {
        var costFunction = new NumericDiffCostFunction(
            (parameters, residuals) =>
            {
                residuals[0] = parameters[0][0] * parameters[0][0] - 4.0;
                return true;
            },
            method: NumericDiffMethod.Central,
            numResiduals: 1,
            parameterBlockSizes: new[] { 1 });
        
        Assert.That(costFunction, Is.Not.Null);
    }

    [Test]
    public void NumericDiffCostFunction_Forward_ShouldWork()
    {
        var costFunction = new NumericDiffCostFunction(
            (parameters, residuals) =>
            {
                residuals[0] = parameters[0][0] - 1.0;
                return true;
            },
            method: NumericDiffMethod.Forward,
            numResiduals: 1,
            parameterBlockSizes: new[] { 1 });
        
        Assert.That(costFunction, Is.Not.Null);
    }

    [Test]
    public void NumericDiffCostFunction_Ridders_ShouldWork()
    {
        var costFunction = new NumericDiffCostFunction(
            (parameters, residuals) =>
            {
                residuals[0] = parameters[0][0] - 1.0;
                return true;
            },
            method: NumericDiffMethod.Ridders,
            numResiduals: 1,
            parameterBlockSizes: new[] { 1 });
        
        Assert.That(costFunction, Is.Not.Null);
    }

    [Test]
    public void DynamicNumericDiffCostFunction_ShouldWork()
    {
        var costFunction = new DynamicNumericDiffCostFunction(
            (parameters, residuals) =>
            {
                residuals[0] = parameters[0][0] - 1.0;
                return true;
            },
            method: NumericDiffMethod.Central,
            numResiduals: 1,
            parameterBlockSizes: new[] { 1 });
        
        Assert.That(costFunction, Is.Not.Null);
    }

    [Test]
    public void CostFunction_WithComplexCalculation_ShouldWork()
    {
        var costFunction = new AutoDiffCostFunction(
            (parameters, residuals) =>
            {
                var x = parameters[0][0];
                var y = parameters[0][1];
                // residual = x^2 + y^2 - 1 (circle constraint)
                residuals[0] = x * x + y * y - 1.0;
                return true;
            },
            numResiduals: 1,
            parameterBlockSizes: new[] { 2 });
        
        Assert.That(costFunction, Is.Not.Null);
    }

    [Test]
    public void CostFunction_ReturnFalse_ShouldIndicateFailure()
    {
        var costFunction = new AutoDiffCostFunction(
            (parameters, residuals) =>
            {
                // Simulate failure condition
                if (parameters[0][0] < 0)
                {
                    return false;
                }
                residuals[0] = parameters[0][0] - 1.0;
                return true;
            },
            numResiduals: 1,
            parameterBlockSizes: new[] { 1 });
        
        Assert.That(costFunction, Is.Not.Null);
    }
}