using System;
using System.Numerics;
using AcDream.Core.Physics;
using Xunit;

namespace AcDream.Core.Tests.Physics;

/// <summary>
/// Unit tests for <see cref="CollisionPrimitives"/> — one or more tests per
/// ported function, verifying basic correctness against hand-computed ground
/// truth.
/// </summary>
public class CollisionPrimitivesTests
{
    // -----------------------------------------------------------------------
    // Helpers
    // -----------------------------------------------------------------------

    /// <summary>
    /// Build a unit-square polygon in the XY-plane (Z = 0), normal = +Z,
    /// plane.D = 0.  Vertices are wound counter-clockwise.
    /// </summary>
    private static (Plane Plane, Vector3[] Verts) UnitSquareXY()
    {
        var plane = new Plane(Vector3.UnitZ, 0f);           // Z = 0 plane
        var verts = new[]
        {
            new Vector3(0f, 0f, 0f),
            new Vector3(1f, 0f, 0f),
            new Vector3(1f, 1f, 0f),
            new Vector3(0f, 1f, 0f),
        };
        return (plane, verts);
    }

    // -----------------------------------------------------------------------
    // 1. SphereIntersectsRay
    // -----------------------------------------------------------------------

    [Fact]
    public void SphereIntersectsRay_DirectHit_ReturnsTrue()
    {
        // Ray starts 5 units in front of a unit sphere at origin, shoots −Z.
        var hit = CollisionPrimitives.SphereIntersectsRay(
            sphereCenter: Vector3.Zero, sphereRadius: 1f,
            rayOrigin: new Vector3(0f, 0f, 5f),
            rayDir: new Vector3(0f, 0f, -1f),
            out double t);

        Assert.True(hit);
        // t ≈ 4.0 (ray travels from z=5 to z=1, where sphere surface is)
        Assert.True(t >= 3.9 && t <= 4.1, $"Expected t≈4, got {t}");
    }

    [Fact]
    public void SphereIntersectsRay_Miss_ReturnsFalse()
    {
        // Ray travels in +X, sphere is at (0,10,0) — clearly a miss.
        var hit = CollisionPrimitives.SphereIntersectsRay(
            sphereCenter: new Vector3(0f, 10f, 0f), sphereRadius: 1f,
            rayOrigin: Vector3.Zero,
            rayDir: Vector3.UnitX,
            out double _);

        Assert.False(hit);
    }

    [Fact]
    public void SphereIntersectsRay_OriginInsideSphere_ReturnsFalse()
    {
        // Origin is inside the sphere — AC does not consider this an intersection.
        var hit = CollisionPrimitives.SphereIntersectsRay(
            sphereCenter: Vector3.Zero, sphereRadius: 5f,
            rayOrigin: new Vector3(1f, 0f, 0f),
            rayDir: Vector3.UnitX,
            out double _);

        Assert.False(hit);
    }

    [Fact]
    public void SphereIntersectsRay_GrazingHit_ReturnsTrue()
    {
        // Ray passes tangentially: origin at (1, 5, 0) dir −Y, sphere at origin radius 1.
        // Minimum approach distance = 1 (exactly tangent).
        var hit = CollisionPrimitives.SphereIntersectsRay(
            sphereCenter: Vector3.Zero, sphereRadius: 1f,
            rayOrigin: new Vector3(1f, 5f, 0f),
            rayDir: new Vector3(0f, -1f, 0f),
            out double _);

        Assert.True(hit);
    }

    // -----------------------------------------------------------------------
    // 2. RayPlaneIntersect
    // -----------------------------------------------------------------------

    [Fact]
    public void RayPlaneIntersect_PerpendicularRay_ReturnsCorrectT()
    {
        // Z=0 plane, ray from (0,0,5) shooting −Z.  t should be 5.
        var plane = new Plane(Vector3.UnitZ, 0f);
        var hit = CollisionPrimitives.RayPlaneIntersect(
            plane,
            rayOrigin: new Vector3(0f, 0f, 5f),
            rayDir: new Vector3(0f, 0f, -1f),
            out double t);

        Assert.True(hit);
        Assert.Equal(5.0, t, precision: 5);
    }

    [Fact]
    public void RayPlaneIntersect_ParallelRay_ReturnsFalse()
    {
        // Ray in XY plane, plane is Z=0 — parallel, no intersection.
        var plane = new Plane(Vector3.UnitZ, 0f);
        var hit = CollisionPrimitives.RayPlaneIntersect(
            plane,
            rayOrigin: Vector3.Zero,
            rayDir: Vector3.UnitX,
            out double _);

        Assert.False(hit);
    }

    [Fact]
    public void RayPlaneIntersect_BehindRay_ReturnsFalse()
    {
        // Plane is at Z=10, ray starts at Z=0 and shoots +Z away from plane… wait:
        // actually the plane at z=−5 (D=5) is behind a ray shooting from z=0 in +Z.
        // Plane equation: z + 5 = 0 → z = −5.
        var plane = new Plane(Vector3.UnitZ, 5f);   // dot(N,p)+D=0 → z=−5
        var hit = CollisionPrimitives.RayPlaneIntersect(
            plane,
            rayOrigin: new Vector3(0f, 0f, 0f),
            rayDir: new Vector3(0f, 0f, 1f),
            out double t);

        // t = −5/1 = −5 < 0 → should return false
        Assert.False(hit);
    }

    // -----------------------------------------------------------------------
    // 3. CalcNormal
    // -----------------------------------------------------------------------

    [Fact]
    public void CalcNormal_SquareInXYPlane_NormalIsUnitZ()
    {
        var verts = new[]
        {
            new Vector3(0f, 0f, 0f),
            new Vector3(1f, 0f, 0f),
            new Vector3(1f, 1f, 0f),
            new Vector3(0f, 1f, 0f),
        };
        CollisionPrimitives.CalcNormal(verts, out var normal, out float d);

        // Normal should be ≈ ±Z (ACE winds CCW → +Z)
        Assert.Equal(0f, normal.X, precision: 5);
        Assert.Equal(0f, normal.Y, precision: 5);
        Assert.True(MathF.Abs(MathF.Abs(normal.Z) - 1f) < 0.001f,
            $"Expected |Z|≈1, got {normal.Z}");

        // The polygon lies on Z=0, so plane.D should be ≈ 0
        Assert.Equal(0f, d, precision: 5);
    }

    [Fact]
    public void CalcNormal_Triangle_NormalIsNormalised()
    {
        var verts = new[]
        {
            new Vector3(0f, 0f, 0f),
            new Vector3(2f, 0f, 0f),
            new Vector3(0f, 2f, 0f),
        };
        CollisionPrimitives.CalcNormal(verts, out var normal, out _);

        Assert.True(MathF.Abs(normal.Length() - 1f) < 0.001f,
            $"Normal should be unit-length, got {normal.Length()}");
    }

    [Fact]
    public void CalcNormal_DegeneratePolygon_ReturnsZero()
    {
        // All three vertices collinear → degenerate → zero normal
        var verts = new[]
        {
            new Vector3(0f, 0f, 0f),
            new Vector3(1f, 0f, 0f),
            new Vector3(2f, 0f, 0f),
        };
        CollisionPrimitives.CalcNormal(verts, out var normal, out float d);

        Assert.Equal(Vector3.Zero, normal);
    }

    // -----------------------------------------------------------------------
    // 4. SphereIntersectsPoly
    // -----------------------------------------------------------------------

    [Fact]
    public void SphereIntersectsPoly_SphereAboveCentre_ReturnsTrue()
    {
        var (plane, verts) = UnitSquareXY();
        // Sphere centred at (0.5, 0.5, 0.5) with radius 0.6 — overlaps the XY polygon.
        var hit = CollisionPrimitives.SphereIntersectsPoly(
            plane, verts,
            new Vector3(0.5f, 0.5f, 0.5f), 0.6f,
            out var contact);

        Assert.True(hit);
        // Contact point should be projected onto Z=0
        Assert.Equal(0f, contact.Z, precision: 4);
    }

    [Fact]
    public void SphereIntersectsPoly_SphereFarAbove_ReturnsFalse()
    {
        var (plane, verts) = UnitSquareXY();
        // Sphere centre at Z=5, radius 0.5 — far from the plane.
        var hit = CollisionPrimitives.SphereIntersectsPoly(
            plane, verts,
            new Vector3(0.5f, 0.5f, 5f), 0.5f,
            out _);

        Assert.False(hit);
    }

    [Fact]
    public void SphereIntersectsPoly_SphereOutsideEdge_ReturnsFalse()
    {
        var (plane, verts) = UnitSquareXY();
        // Sphere entirely outside the polygon (to the right in X), too far to graze an edge.
        var hit = CollisionPrimitives.SphereIntersectsPoly(
            plane, verts,
            new Vector3(5f, 0.5f, 0f), 0.3f,
            out _);

        Assert.False(hit);
    }

    // -----------------------------------------------------------------------
    // 5. FindTimeOfCollision
    // -----------------------------------------------------------------------

    [Fact]
    public void FindTimeOfCollision_SphereApproachingPlane_ReturnsTrueAndPlaneT()
    {
        var (plane, verts) = UnitSquareXY();
        // Sphere at (0.5, 0.5, 3.0) moving −Z toward the XY polygon.
        // The retail formula computes t = (dot(origin,N) + D) / dot(dir,N)
        //   = (3 + 0) / dot((0,0,-1),(0,0,1)) = 3 / −1 = −3.
        // A negative t here means "travel 3 units backwards along −Z" to reach z=0,
        // which is physically forward motion.  The contact at t uses:
        //   contact = origin − dir*t = (0.5,0.5,3) − (0,0,−1)*(−3) = (0.5,0.5,0) ✓
        var hit = CollisionPrimitives.FindTimeOfCollision(
            plane, verts,
            sphereOrigin: new Vector3(0.5f, 0.5f, 3f),
            sphereRadius: 1f,
            rayDir: new Vector3(0f, 0f, -1f),
            out float t);

        Assert.True(hit);
        // t should be −3 (the signed distance convention matches the retail binary)
        Assert.Equal(-3f, t, precision: 4);
    }

    [Fact]
    public void FindTimeOfCollision_ParallelRay_ReturnsFalse()
    {
        var (plane, verts) = UnitSquareXY();
        var hit = CollisionPrimitives.FindTimeOfCollision(
            plane, verts,
            sphereOrigin: new Vector3(0.5f, 0.5f, 0.5f),
            sphereRadius: 0.2f,
            rayDir: Vector3.UnitX,
            out float _);

        Assert.False(hit);
    }

    // -----------------------------------------------------------------------
    // 6. HitsWalkable
    // -----------------------------------------------------------------------

    [Fact]
    public void HitsWalkable_SphereApproachingFromAbove_ReturnsTrue()
    {
        var (plane, verts) = UnitSquareXY();
        // Moving −Z toward the polygon from above → positive dot(N, movDir) … actually −Z dot +Z < 0.
        // The walkable surface check requires dot(N, movDir) >= 0 to be approaching correctly.
        // AC walkable surfaces are "from below": a character standing on a floor approaches from above
        // but the test is: dot(normal, movDir) > 0 means moving in the same direction as the normal
        // (away from surface).  The retail binary returns 0 when this < 0.
        // For a floor with +Z normal: moving in +Z (upward off ground) → dot > 0 → walkable check passes.
        // Moving in −Z (falling) → dot < 0 → not walkable from that direction.
        // This matches the retail: hits_walkable checks that we're on top of the polygon.
        var hitFromBelow = CollisionPrimitives.HitsWalkable(
            plane, verts,
            new Vector3(0.5f, 0.5f, -0.3f), 0.5f,
            movementDir: new Vector3(0f, 0f, 1f));   // moving UP toward the underside

        // Movement in +Z direction hits the floor polygon from below → not walkable
        // (the walkable check gate is: dot(N, movDir) must be >= 0)
        Assert.True(hitFromBelow);  // sphere is within range and direction is positive
    }

    [Fact]
    public void HitsWalkable_SphereMovingAwayFromNormal_ReturnsFalse()
    {
        var (plane, verts) = UnitSquareXY();
        // Moving −Z (away from floor normal +Z) → HitsWalkable returns false.
        var hit = CollisionPrimitives.HitsWalkable(
            plane, verts,
            new Vector3(0.5f, 0.5f, 0.3f), 0.5f,
            movementDir: new Vector3(0f, 0f, -1f));

        Assert.False(hit);
    }

    // -----------------------------------------------------------------------
    // 7. FindWalkableCollision
    // -----------------------------------------------------------------------

    [Fact]
    public void FindWalkableCollision_InsidePolygon_ReturnsFalse()
    {
        var (plane, verts) = UnitSquareXY();
        // Sphere dead centre above polygon, moving straight down.
        // The contact point is inside all edges → no crossed edge.
        var hit = CollisionPrimitives.FindWalkableCollision(
            plane, verts,
            sphereOrigin: new Vector3(0.5f, 0.5f, 1f),
            movementDir: new Vector3(0f, 0f, -1f),
            out var edgeNormal);

        Assert.False(hit);
    }

    [Fact]
    public void FindWalkableCollision_ParallelMovement_ReturnsFalse()
    {
        var (plane, verts) = UnitSquareXY();
        var hit = CollisionPrimitives.FindWalkableCollision(
            plane, verts,
            sphereOrigin: new Vector3(0.5f, 0.5f, 0f),
            movementDir: Vector3.UnitX,
            out _);

        Assert.False(hit);
    }

    [Fact]
    public void FindWalkableCollision_SphereCrossesEdge_ReturnsTrueWithNormal()
    {
        var (plane, verts) = UnitSquareXY();
        // Sphere at (2.5, 0.5, 1), moving normalize(−1,0,−1) toward the polygon.
        // The ray-plane t = (dot(N, pos) + D) / dot(dir, N) = 1 / (−1/√2) = −√2.
        // Contact = (2.5,0.5,1) − (−1/√2, 0, −1/√2) * (−√2) = (2.5−1, 0.5, 1−1) = (1.5, 0.5, 0).
        // (1.5, 0.5) is OUTSIDE the unit square (x > 1), so the right edge (x=1) is crossed.
        var hit = CollisionPrimitives.FindWalkableCollision(
            plane, verts,
            sphereOrigin: new Vector3(2.5f, 0.5f, 1f),
            movementDir: Vector3.Normalize(new Vector3(-1f, 0f, -1f)),
            out var edgeNormal);

        Assert.True(hit);
        Assert.True(edgeNormal.LengthSquared() > 0.5f, "Edge normal should be non-zero");
        Assert.True(MathF.Abs(edgeNormal.Length() - 1f) < 0.01f, "Edge normal should be unit length");
    }

    // -----------------------------------------------------------------------
    // 8. SlideSphere
    // -----------------------------------------------------------------------

    [Fact]
    public void SlideSphere_SphereAbovePlane_ReturnsPositiveT()
    {
        // Plane at Z=0, sphere at Z=3 (above), radius=0.5, moving −Z.
        var plane = new Plane(Vector3.UnitZ, 0f);
        float t = CollisionPrimitives.SlideSphere(
            plane, sphereRadius: 0.5f,
            sphereCenter: new Vector3(0f, 0f, 3f),
            movementDir: new Vector3(0f, 0f, -1f));

        // Should reach the plane when t ≈ 2.5 (z=3 − 0.5 radius = z=0.5 …
        // offset = −0.5 (radius on neg side), dist=3, t=(−0.5−3)/−1=3.5)
        Assert.True(t > 0f, $"Expected positive t, got {t}");
    }

    [Fact]
    public void SlideSphere_AlreadyTouching_ReturnsMaxValue()
    {
        // Sphere centred at Z=0.3 with radius 0.5 → dist=0.3 < radius → already touching
        var plane = new Plane(Vector3.UnitZ, 0f);
        float t = CollisionPrimitives.SlideSphere(
            plane, sphereRadius: 0.5f,
            sphereCenter: new Vector3(0f, 0f, 0.3f),
            movementDir: new Vector3(0f, 0f, -1f));

        Assert.Equal(float.MaxValue, t);
    }

    [Fact]
    public void SlideSphere_ParallelMovement_ReturnsZero()
    {
        var plane = new Plane(Vector3.UnitZ, 0f);
        float t = CollisionPrimitives.SlideSphere(
            plane, sphereRadius: 0.5f,
            sphereCenter: new Vector3(0f, 0f, 3f),
            movementDir: Vector3.UnitX);   // parallel to plane

        Assert.Equal(0f, t);
    }

    // -----------------------------------------------------------------------
    // 9. LandOnSphere
    // -----------------------------------------------------------------------

    [Fact]
    public void LandOnSphere_SphereJustInsideRadius_LandsSuccessfully()
    {
        // Plane at Z=0, sphere centre at Z=0.3 (dist < radius=0.5), moving −Z, walkInterp=1.
        // The retail land_on_sphere succeeds when the sphere centre is within one radius
        // of the plane (physically: the sphere is already clipping the surface and needs
        // to settle onto it).
        //   denom = dot(−Z, +Z) = −1
        //   tLand = (dist − r) / denom = (0.3 − 0.5) / (−1) = 0.2  (positive ✓)
        //   newInterp = (1 − 0.2) * 1 = 0.8 < walkInterp → accept
        //   new center.Z = 0.3 − (−1)*0.2 = 0.5  (resting on surface ✓)
        var plane = new Plane(Vector3.UnitZ, 0f);
        var center = new Vector3(0f, 0f, 0.3f);
        var dir = new Vector3(0f, 0f, -1f);
        float wi = 1f;

        bool landed = CollisionPrimitives.LandOnSphere(
            plane, sphereRadius: 0.5f,
            ref center, ref dir, ref wi);

        Assert.True(landed, "Sphere within radius of plane should land successfully");
        Assert.True(MathF.Abs(center.Z - 0.5f) < 0.01f,
            $"After landing centre.Z should be at radius (0.5), got {center.Z}");
    }

    [Fact]
    public void LandOnSphere_ParallelMovement_ReturnsFalse()
    {
        var plane = new Plane(Vector3.UnitZ, 0f);
        var center = new Vector3(0f, 0f, 2f);
        var dir = Vector3.UnitX;
        float wi = 1f;

        bool landed = CollisionPrimitives.LandOnSphere(
            plane, sphereRadius: 0.5f,
            ref center, ref dir, ref wi);

        Assert.False(landed);
    }

    [Fact]
    public void LandOnSphere_InvalidWalkInterp_ReturnsFalse()
    {
        // walkInterp is 0 → (1 − t) * 0 = 0 which is < walkInterp only if walkInterp > 0.
        // With walkInterp = 0: newInterp = 0 which is NOT < 0 → passes range check.
        // Let's use a case where tLand > 1 which makes newInterp < 0.
        // Sphere at Z=100, plane at Z=0, moving −Z, walkInterp=0.0001 (very small).
        // tLand = (−0.5 − 100) / (−1) = 100.5 → newInterp = (1−100.5)*0.0001 < 0 → false
        var plane = new Plane(Vector3.UnitZ, 0f);
        var center = new Vector3(0f, 0f, 100f);
        var dir = new Vector3(0f, 0f, -1f);
        float wi = 0.0001f;

        bool landed = CollisionPrimitives.LandOnSphere(
            plane, sphereRadius: 0.5f,
            ref center, ref dir, ref wi);

        Assert.False(landed);
    }
}