using CeresSharp;

namespace CeresSharp.Test;

[TestFixture]
public class ManifoldTests
{
    [Test]
    public void QuaternionManifold_ShouldCreate()
    {
        using var manifold = new QuaternionManifold();
        Assert.That(manifold, Is.Not.Null);
    }

    [Test]
    public void QuaternionManifold_WithProblem_ShouldWork()
    {
        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 SphereManifold_ShouldCreate()
    {
        using var manifold = new SphereManifold(dimension: 3);
        Assert.That(manifold, Is.Not.Null);
    }

    [Test]
    public void SphereManifold_WithProblem_ShouldWork()
    {
        using var problem = new Problem();
        var point = new double[] { 1.0, 0.0, 0.0 }; // Unit vector
        problem.AddParameterBlock(point, point.Length);
        
        using var manifold = new SphereManifold(dimension: 3);
        problem.SetManifold(point, manifold);
        
        // Should not throw
        Assert.Pass();
    }

    [Test]
    public void LineManifold_ShouldCreate()
    {
        using var manifold = new LineManifold(dimension: 3);
        Assert.That(manifold, Is.Not.Null);
    }

    [Test]
    public void EuclideanManifold_ShouldCreate()
    {
        using var manifold = new EuclideanManifold(dimension: 3);
        Assert.That(manifold, Is.Not.Null);
    }

    [Test]
    public void SubsetManifold_ShouldCreate()
    {
        var constantSubset = new int[] { 0, 2 }; // Fix indices 0 and 2
        using var manifold = new SubsetManifold(constantSubset, ambientSize: 4);
        Assert.That(manifold, Is.Not.Null);
    }

    [Test]
    public void SubsetManifold_WithProblem_ShouldWork()
    {
        using var problem = new Problem();
        var parameters = new double[] { 1.0, 2.0, 3.0, 4.0 };
        problem.AddParameterBlock(parameters, parameters.Length);
        
        var constantSubset = new int[] { 0, 2 };
        using var manifold = new SubsetManifold(constantSubset, ambientSize: 4);
        problem.SetManifold(parameters, manifold);
        
        // Should not throw
        Assert.Pass();
    }

    [Test]
    public void ProductManifold_ShouldCreate()
    {
        using var quaternionManifold = new QuaternionManifold();
        using var euclideanManifold = new EuclideanManifold(dimension: 3);
        
        var manifolds = new Manifold[] { quaternionManifold, euclideanManifold };
        using var productManifold = new ProductManifold(manifolds);
        
        Assert.That(productManifold, Is.Not.Null);
    }

    [Test]
    public void ProductManifold_WithProblem_ShouldWork()
    {
        using var problem = new Problem();
        // 4 for quaternion + 3 for translation = 7
        var pose = new double[] { 0.0, 0.0, 0.0, 1.0, 1.0, 2.0, 3.0 };
        problem.AddParameterBlock(pose, pose.Length);
        
        using var quaternionManifold = new QuaternionManifold();
        using var euclideanManifold = new EuclideanManifold(dimension: 3);
        var manifolds = new Manifold[] { quaternionManifold, euclideanManifold };
        
        using var productManifold = new ProductManifold(manifolds);
        problem.SetManifold(pose, productManifold);
        
        // Should not throw
        Assert.Pass();
    }

    [Test]
    public void MultipleManifolds_ShouldWork()
    {
        using var problem = new Problem();
        
        var quaternion1 = new double[] { 0.0, 0.0, 0.0, 1.0 };
        var quaternion2 = new double[] { 0.0, 0.0, 0.0, 1.0 };
        problem.AddParameterBlock(quaternion1, quaternion1.Length);
        problem.AddParameterBlock(quaternion2, quaternion2.Length);
        
        using var manifold1 = new QuaternionManifold();
        using var manifold2 = new QuaternionManifold();
        
        problem.SetManifold(quaternion1, manifold1);
        problem.SetManifold(quaternion2, manifold2);
        
        // Should not throw
        Assert.Pass();
    }
}