using System.Numerics;
using DatReaderWriter.Enums;
using DatReaderWriter.Types;
using AcDream.Core.Physics;
using Xunit;

namespace AcDream.Core.Tests.Physics;

/// <summary>
/// Unit tests for <see cref="BSPQuery.SphereIntersectsPoly"/>.
///
/// Real BSP data requires dat files (integration-test territory), so these
/// tests use manually constructed BSP nodes and polygon/vertex data that
/// match the structure the dat reader would produce.
/// </summary>
public class BSPQueryTests
{
    // -----------------------------------------------------------------------
    // Helpers
    // -----------------------------------------------------------------------

    /// <summary>
    /// Build a <see cref="VertexArray"/> with four vertices forming a unit
    /// square in the XY-plane (Z = 0), ids 0-3.
    /// </summary>
    private static VertexArray UnitSquareVertexArray()
    {
        var va = new VertexArray();
        var positions = new[]
        {
            new Vector3(0f, 0f, 0f),
            new Vector3(1f, 0f, 0f),
            new Vector3(1f, 1f, 0f),
            new Vector3(0f, 1f, 0f),
        };
        for (ushort i = 0; i < positions.Length; i++)
        {
            var sv = new SWVertex { Origin = positions[i], Normal = Vector3.UnitZ };
            va.Vertices[i] = sv;
        }
        return va;
    }

    /// <summary>
    /// Build a <see cref="Polygon"/> referencing vertex ids 0-3 in order.
    /// </summary>
    private static Polygon UnitSquarePolygon() => new Polygon
    {
        SidesType = DatReaderWriter.Enums.CullMode.None,
        VertexIds = new List<short> { 0, 1, 2, 3 },
    };

    /// <summary>
    /// Build a leaf <see cref="PhysicsBSPNode"/> containing one polygon (id 0)
    /// with a bounding sphere that covers the unit square.
    /// </summary>
    private static PhysicsBSPNode LeafNode(Sphere bounds)
    {
        var node = new PhysicsBSPNode
        {
            Type = BSPNodeType.Leaf,
            BoundingSphere = bounds,
        };
        node.Polygons.Add(0);
        return node;
    }

    // -----------------------------------------------------------------------
    // Test 1: null node returns false without throwing
    // -----------------------------------------------------------------------

    [Fact]
    public void SphereIntersectsPoly_NullNode_ReturnsFalse()
    {
        var polygons = new Dictionary<ushort, Polygon>();
        var vertices = new VertexArray();

        bool hit = BSPQuery.SphereIntersectsPoly(
            null, polygons, vertices,
            new Vector3(0.5f, 0.5f, 0.1f), 0.2f,
            out _, out _);

        Assert.False(hit);
    }

    // -----------------------------------------------------------------------
    // Test 2: sphere far outside the bounding sphere is fast-rejected
    // -----------------------------------------------------------------------

    [Fact]
    public void SphereIntersectsPoly_MissesBoundingSphere_ReturnsFalse()
    {
        // Leaf node centred at origin with radius 1.
        var bounds = new Sphere { Origin = Vector3.Zero, Radius = 1f };
        var node = LeafNode(bounds);
        var polygons = new Dictionary<ushort, Polygon> { [0] = UnitSquarePolygon() };
        var vertices = UnitSquareVertexArray();

        // Sphere is 100 units away — broad phase must reject.
        bool hit = BSPQuery.SphereIntersectsPoly(
            node, polygons, vertices,
            new Vector3(100f, 100f, 100f), 0.5f,
            out _, out _);

        Assert.False(hit);
    }

    // -----------------------------------------------------------------------
    // Test 3: sphere resting just above the unit-square floor polygon hits
    // -----------------------------------------------------------------------

    [Fact]
    public void SphereIntersectsPoly_HitsLeafPolygon()
    {
        // Bounding sphere covers the 1×1 unit-square leaf.
        var bounds = new Sphere { Origin = new Vector3(0.5f, 0.5f, 0f), Radius = 2f };
        var node = LeafNode(bounds);
        var polygons = new Dictionary<ushort, Polygon> { [0] = UnitSquarePolygon() };
        var vertices = UnitSquareVertexArray();

        // Sphere centred at (0.5, 0.5, 0.3) with radius 0.5 should touch Z=0 plane.
        bool hit = BSPQuery.SphereIntersectsPoly(
            node, polygons, vertices,
            new Vector3(0.5f, 0.5f, 0.3f), 0.5f,
            out ushort polyId, out Vector3 normal);

        Assert.True(hit);
        Assert.Equal(0, polyId);
        // Normal should point roughly upward (+Z).
        Assert.True(normal.Z > 0.9f, $"Expected Z-up normal, got {normal}");
    }

    // -----------------------------------------------------------------------
    // Test 4: sphere entirely above the polygon (no contact) returns false
    // -----------------------------------------------------------------------

    [Fact]
    public void SphereIntersectsPoly_SphereTooHigh_ReturnsFalse()
    {
        var bounds = new Sphere { Origin = new Vector3(0.5f, 0.5f, 0f), Radius = 2f };
        var node = LeafNode(bounds);
        var polygons = new Dictionary<ushort, Polygon> { [0] = UnitSquarePolygon() };
        var vertices = UnitSquareVertexArray();

        // Sphere centred 5 units above the floor with radius 0.3 → no contact.
        bool hit = BSPQuery.SphereIntersectsPoly(
            node, polygons, vertices,
            new Vector3(0.5f, 0.5f, 5f), 0.3f,
            out _, out _);

        Assert.False(hit);
    }

    // -----------------------------------------------------------------------
    // Test 5: internal node — sphere on positive side recurses pos subtree
    // -----------------------------------------------------------------------

    [Fact]
    public void SphereIntersectsPoly_InternalNode_PosSubtreeHit()
    {
        // Leaf on the positive side (Z > 0 half-space) contains the floor poly.
        var leafBounds = new Sphere { Origin = new Vector3(0.5f, 0.5f, 0f), Radius = 2f };
        var leafNode = LeafNode(leafBounds);

        var polygons = new Dictionary<ushort, Polygon> { [0] = UnitSquarePolygon() };
        var vertices = UnitSquareVertexArray();

        // Splitting plane: Z = 0, normal = +Z, D = 0.
        // Sphere at Z = 0.3 is on the positive side.
        var internalBounds = new Sphere { Origin = new Vector3(0.5f, 0.5f, 0f), Radius = 5f };
        var internalNode = new PhysicsBSPNode
        {
            Type = BSPNodeType.BPnn,   // has PosNode only (BPnn = pos + null-neg)
            SplittingPlane = new Plane(Vector3.UnitZ, 0f),
            BoundingSphere = internalBounds,
            PosNode = leafNode,
            NegNode = null,
        };

        bool hit = BSPQuery.SphereIntersectsPoly(
            internalNode, polygons, vertices,
            new Vector3(0.5f, 0.5f, 0.3f), 0.5f,
            out ushort polyId, out _);

        Assert.True(hit);
        Assert.Equal(0, polyId);
    }

    // -----------------------------------------------------------------------
    // Test 6: PhysicsDataCache — caches GfxObj with physics data
    // -----------------------------------------------------------------------

    [Fact]
    public void PhysicsDataCache_CachesGfxObjWithPhysics()
    {
        // We can't easily construct a real GfxObj (field-based dat type),
        // so this test verifies the SetupPhysics cache path which is more
        // easily instantiated.
        var cache = new PhysicsDataCache();
        Assert.Equal(0, cache.GfxObjCount);
        Assert.Equal(0, cache.SetupCount);

        // GetGfxObj for an unknown id should return null safely.
        Assert.Null(cache.GetGfxObj(0x01000001u));
        Assert.Null(cache.GetSetup(0x02000001u));
    }

    // =========================================================================
    // FindWalkableSphere — indoor walkable-plane finder (spec 2026-05-19)
    // =========================================================================

    /// <summary>
    /// Build a single-leaf BSP rooted at one node containing two horizontal
    /// walkable polygons (id 0 at Z=lowerZ, id 1 at Z=upperZ), both covering
    /// the unit square X[0..1] × Y[0..1]. Bounding sphere is sized to enclose
    /// both polys.
    /// </summary>
    private static (PhysicsBSPNode root, Dictionary<ushort, ResolvedPolygon> resolved)
        BuildTwoFloorsBsp(float lowerZ, float upperZ)
    {
        var center = new Vector3(0.5f, 0.5f, (lowerZ + upperZ) * 0.5f);
        float halfHeight = MathF.Abs(upperZ - lowerZ) * 0.5f + 1.0f;
        float radius = MathF.Sqrt(0.5f * 0.5f + 0.5f * 0.5f + halfHeight * halfHeight);

        var root = new PhysicsBSPNode
        {
            Type = BSPNodeType.Leaf,
            BoundingSphere = new Sphere { Origin = center, Radius = radius },
        };
        root.Polygons.Add(0);
        root.Polygons.Add(1);

        Vector3[] lowerVerts =
        {
            new Vector3(0f, 0f, lowerZ),
            new Vector3(1f, 0f, lowerZ),
            new Vector3(1f, 1f, lowerZ),
            new Vector3(0f, 1f, lowerZ),
        };
        Vector3[] upperVerts =
        {
            new Vector3(0f, 0f, upperZ),
            new Vector3(1f, 0f, upperZ),
            new Vector3(1f, 1f, upperZ),
            new Vector3(0f, 1f, upperZ),
        };

        var resolved = new Dictionary<ushort, ResolvedPolygon>
        {
            [0] = new ResolvedPolygon
            {
                Vertices = lowerVerts,
                Plane = new Plane(Vector3.UnitZ, -lowerZ),
                NumPoints = 4,
                SidesType = CullMode.None,
            },
            [1] = new ResolvedPolygon
            {
                Vertices = upperVerts,
                Plane = new Plane(Vector3.UnitZ, -upperZ),
                NumPoints = 4,
                SidesType = CullMode.None,
            },
        };

        return (root, resolved);
    }

    /// <summary>
    /// Build a Transition with WalkableAllowance set to FloorZ — what the
    /// indoor walkable-plane synthesis uses.
    /// </summary>
    private static Transition BuildFloorZTransition()
    {
        var transition = new Transition();
        transition.SpherePath.WalkableAllowance = PhysicsGlobals.FloorZ;
        transition.SpherePath.WalkInterp = 1.0f;
        return transition;
    }

    [Fact]
    public void FindWalkableSphere_TwoFloors_FootBetween_PicksLowerFloor()
    {
        // Two floors at Z=0 and Z=3. Foot sphere center at Z=0.4 (radius 0.48).
        // The sphere overlaps the lower floor (|center.Z - 0| = 0.4 < radius 0.48),
        // so find_walkable can resolve a rest position against it.
        // Upper floor at Z=3 is out of range (dist=2.6 >> radius 0.48).
        // Expect: pick lower floor (id 0).
        var (root, resolved) = BuildTwoFloorsBsp(lowerZ: 0f, upperZ: 3f);
        var transition = BuildFloorZTransition();

        var sphere = new Sphere { Origin = new Vector3(0.5f, 0.5f, 0.4f), Radius = 0.48f };

        bool found = BSPQuery.FindWalkableSphere(
            root, resolved, transition,
            sphere,
            probeDistance: 0.5f,
            up: Vector3.UnitZ,
            out var hitPoly,
            out var hitPolyId,
            out var adjustedCenter);

        Assert.True(found);
        Assert.Equal((ushort)0, hitPolyId);
        Assert.NotNull(hitPoly);
        Assert.Equal(1f, hitPoly!.Plane.Normal.Z, precision: 3);  // horizontal floor: normal.Z = 1
    }

    [Fact]
    public void FindWalkableSphere_OnlyUpperFloor_FootAbove_PicksUpperFloor()
    {
        // Two floors at Z=0 and Z=3. Foot sphere center at Z=3.4 (radius 0.48).
        // The sphere overlaps the upper floor (|center.Z - 3| = 0.4 < radius 0.48).
        // Lower floor at Z=0 is out of range (dist=3.4 >> radius 0.48).
        // Expect: pick the upper floor (id 1).
        var (root, resolved) = BuildTwoFloorsBsp(lowerZ: 0f, upperZ: 3f);
        var transition = BuildFloorZTransition();

        var sphere = new Sphere { Origin = new Vector3(0.5f, 0.5f, 3.4f), Radius = 0.48f };

        bool found = BSPQuery.FindWalkableSphere(
            root, resolved, transition,
            sphere,
            probeDistance: 0.5f,
            up: Vector3.UnitZ,
            out var hitPoly,
            out var hitPolyId,
            out _);

        Assert.True(found);
        Assert.Equal((ushort)1, hitPolyId);
        Assert.NotNull(hitPoly);
    }

    [Fact]
    public void FindWalkableSphere_NoWalkableInProbeRange_ReturnsFalse()
    {
        // Two floors at Z=0 and Z=3. Foot at Z=10 with radius 0.48 — out of
        // sphere-overlap range for both polygons (|10-0|=10 >> 0.48, |10-3|=7 >> 0.48).
        // find_walkable requires the sphere to overlap the polygon plane; neither
        // floor is within overlap range, so no hit is found.
        var (root, resolved) = BuildTwoFloorsBsp(lowerZ: 0f, upperZ: 3f);
        var transition = BuildFloorZTransition();

        var sphere = new Sphere { Origin = new Vector3(0.5f, 0.5f, 10f), Radius = 0.48f };

        bool found = BSPQuery.FindWalkableSphere(
            root, resolved, transition,
            sphere,
            probeDistance: 0.5f,
            up: Vector3.UnitZ,
            out var hitPoly,
            out var hitPolyId,
            out var adjustedCenter);

        Assert.False(found);
        Assert.Null(hitPoly);
        Assert.Equal((ushort)0, hitPolyId);
        Assert.Equal(sphere.Origin, adjustedCenter);
    }

    [Fact]
    public void FindWalkableSphere_SteepPoly_RejectedByWalkableAllowance()
    {
        // One polygon with a steep normal (Z component ≈ 0.5 < FloorZ ≈ 0.6642).
        // WalkableHitsSphere checks dp = dot(up, normal) > WalkableAllowance;
        // dp = 0.5 <= FloorZ → not walkable. Sphere overlaps the plane so
        // PolygonHitsSpherePrecise would pass, but the walkability gate fires first.
        var center = new Vector3(0.5f, 0.5f, 0f);
        var root = new PhysicsBSPNode
        {
            Type = BSPNodeType.Leaf,
            BoundingSphere = new Sphere { Origin = center, Radius = 2f },
        };
        root.Polygons.Add(0);

        // Plane tilted: normal has Z = 0.5 (60° slope). Build from two orthogonal verts.
        var steepNormal = Vector3.Normalize(new Vector3(0f, MathF.Sqrt(0.75f), 0.5f));
        // Vertices lying on the plane through the origin.
        float rise = MathF.Sqrt(0.75f) / 0.5f;   // how much Y-displacement equals 1 unit Z-rise
        Vector3[] verts =
        {
            new Vector3(0f, 0f,    0f),
            new Vector3(1f, 0f,    0f),
            new Vector3(1f, 1f,   rise),
            new Vector3(0f, 1f,   rise),
        };
        var resolved = new Dictionary<ushort, ResolvedPolygon>
        {
            [0] = new ResolvedPolygon
            {
                Vertices = verts,
                Plane = new Plane(steepNormal, -Vector3.Dot(steepNormal, verts[0])),
                NumPoints = 4,
                SidesType = CullMode.None,
            },
        };

        var transition = BuildFloorZTransition();
        // Sphere overlapping the tilted plane at the origin side.
        var sphere = new Sphere { Origin = new Vector3(0.5f, 0.5f, 0.3f), Radius = 0.48f };

        bool found = BSPQuery.FindWalkableSphere(
            root, resolved, transition,
            sphere,
            probeDistance: 0.5f,
            up: Vector3.UnitZ,
            out _,
            out _,
            out _);

        Assert.False(found);
    }
}