acdream/tests/AcDream.Core.Tests/Physics/BSPStepUpTests.cs

using System;
using System.Collections.Generic;
using System.Numerics;
using AcDream.Core.Physics;
using DatReaderWriter.Types;
using Xunit;

namespace AcDream.Core.Tests.Physics;

/// <summary>
/// Conformance tests for BSP step-up (Path 5) and rooftop landing (Path 6) in
/// <see cref="BSPQuery.FindCollisions"/>.
///
/// <para>
/// Tests are organised in three groups corresponding to the three commits:
/// </para>
/// <list type="bullet">
///   <item><b>Group A — Baselines</b>: behaviours that should pass both before
///     and after the implementation (no-hit returns OK, fixture geometry checks).</item>
///   <item><b>Group B — Phase L.2.1 (Path 5 step-up)</b>: tests that are RED
///     because Path 5 wall-slides instead of stepping up.  L.2.1 flips these
///     GREEN.</item>
///   <item><b>Group C — Phase L.2.2 (Path 6 SetCollide)</b>: tests that are RED
///     because Path 6 wall-slides instead of setting the Collide flag.  L.2.2
///     flips these GREEN.</item>
/// </list>
///
/// <para>
/// Retail references:
///   BSPTREE::find_collisions Path 5 — acclient_2013_pseudo_c.txt:323849 /
///     ACE BSPTree.cs:192-196.
///   CTransition::step_up — acclient_2013_pseudo_c.txt:273099-273133 /
///     ACE Transition.cs:746-777.
///   BSPTREE::find_collisions Path 6 / SPHEREPATH::set_collide —
///     acclient_2013_pseudo_c.txt:323819 / ACE BSPTree.cs:210-219.
///   SPHEREPATH::set_collide — acclient_2013_pseudo_c.txt:321594-321607 /
///     ACE SpherePath.cs:279-286.
///   CTransition::transitional_insert Collide branch —
///     acclient_2013_pseudo_c.txt:273193-273239 / ACE Transition.cs:891-930.
/// </para>
/// </summary>
public class BSPStepUpTests
{
    // =========================================================================
    // Group A — Baselines (pass before AND after the implementation)
    // =========================================================================

    /// <summary>
    /// No BSP geometry → FindCollisions returns OK with no state changes.
    /// </summary>
    [Fact]
    public void A1_NullRoot_ReturnsOK()
    {
        var from = new Vector3(0f, 0f, BSPStepUpFixtures.SphereRadius);
        var to   = new Vector3(0.1f, 0f, BSPStepUpFixtures.SphereRadius);
        var t    = BSPStepUpFixtures.MakeGroundedTransition(from, to);

        var localSphere = new DatReaderWriter.Types.Sphere
        {
            Origin = to,
            Radius = BSPStepUpFixtures.SphereRadius,
        };

        var result = BSPQuery.FindCollisions(
            null,
            new Dictionary<ushort, ResolvedPolygon>(),
            t, localSphere, null,
            from, Vector3.UnitZ, 1.0f);

        Assert.Equal(TransitionState.OK, result);
    }

    /// <summary>
    /// Grounded mover far from the wall → no collision → OK.
    /// </summary>
    [Fact]
    public void A2_GroundedMover_NoWallNear_ReturnsOK()
    {
        var (root, resolved) = BSPStepUpFixtures.LowStep();

        // Moving in -X, away from the wall at x=0.5.
        var from = new Vector3(-1f, 0f, BSPStepUpFixtures.SphereRadius);
        var to   = new Vector3(-1.5f, 0f, BSPStepUpFixtures.SphereRadius);
        var t    = BSPStepUpFixtures.MakeGroundedTransition(from, to);

        var localSphere = new DatReaderWriter.Types.Sphere { Origin = to, Radius = BSPStepUpFixtures.SphereRadius };

        var result = BSPQuery.FindCollisions(
            root, resolved, t, localSphere, null,
            from, Vector3.UnitZ, 1.0f);

        Assert.Equal(TransitionState.OK, result);
    }

    /// <summary>
    /// Airborne mover well above the roof → no collision → OK.
    /// </summary>
    [Fact]
    public void A3_AirborneMover_AboveRoof_ReturnsOK()
    {
        var (root, resolved) = BSPStepUpFixtures.FlatRoof();

        // Mover at z=6 (well above the roof at z=3) with tiny downward step.
        float highZ = 6f;
        var from = new Vector3(0f, 0f, highZ + BSPStepUpFixtures.SphereRadius);
        var to   = new Vector3(0f, 0f, highZ + BSPStepUpFixtures.SphereRadius - 0.01f);
        var t    = BSPStepUpFixtures.MakeAirborneTransition(from, to);

        var localSphere = new DatReaderWriter.Types.Sphere { Origin = to, Radius = BSPStepUpFixtures.SphereRadius };

        var result = BSPQuery.FindCollisions(
            root, resolved, t, localSphere, null,
            from, Vector3.UnitZ, 1.0f);

        Assert.Equal(TransitionState.OK, result);
    }

    /// <summary>
    /// The slope fixture's polygon must have normal.Z below FloorZ (confirms
    /// the fixture geometry is set up correctly as a non-walkable surface).
    /// </summary>
    [Fact]
    public void A4_SlopedFixture_NormalBelowFloorZ()
    {
        var (_, resolved) = BSPStepUpFixtures.SlopedUnwalkable();
        var slope = resolved[BSPStepUpFixtures.SlopedUnwalkable_SlopeId];

        Assert.True(slope.Plane.Normal.Z < PhysicsGlobals.FloorZ,
            $"Slope normal.Z ({slope.Plane.Normal.Z:F4}) must be < FloorZ ({PhysicsGlobals.FloorZ:F4})");
        Assert.True(slope.Plane.Normal.Z > 0f,
            $"Slope normal.Z ({slope.Plane.Normal.Z:F4}) must be > 0 (upward-facing)");
    }

    /// <summary>
    /// Low-step upper-floor polygon has normal.Z >= FloorZ (it IS walkable).
    /// </summary>
    [Fact]
    public void A5_LowStepUpperFloor_NormalAboveFloorZ()
    {
        var (_, resolved) = BSPStepUpFixtures.LowStep();
        var upper = resolved[BSPStepUpFixtures.LowStep_UpperFloorId];

        Assert.True(upper.Plane.Normal.Z >= PhysicsGlobals.FloorZ,
            $"Upper floor normal.Z ({upper.Plane.Normal.Z:F4}) must be >= FloorZ ({PhysicsGlobals.FloorZ:F4})");
    }

    /// <summary>
    /// Roof polygon has normal.Z >= LandingZ (it can be landed on).
    /// </summary>
    [Fact]
    public void A6_FlatRoofPolygon_NormalAboveLandingZ()
    {
        var (_, resolved) = BSPStepUpFixtures.FlatRoof();
        var roof = resolved[BSPStepUpFixtures.FlatRoof_RoofId];

        Assert.True(roof.Plane.Normal.Z >= PhysicsGlobals.LandingZ,
            $"Roof normal.Z ({roof.Plane.Normal.Z:F4}) must be >= LandingZ ({PhysicsGlobals.LandingZ:F4})");
    }

    // =========================================================================
    // Group B — Phase L.2.1 (Path 5 step-up)
    //
    // RED before L.2.1, GREEN after.
    // Each test documents the CURRENT wrong behaviour and EXPECTED correct one.
    // =========================================================================

    /// <summary>
    /// Grounded mover (Contact + OnWalkable) walking toward the low step (25 cm):
    /// should step up onto the upper floor, not slide sideways.
    ///
    /// <para>
    /// Current (wrong): Path 5 applies wall-slide → CurPos.X stays left of wall;
    /// Z stays at floor level.
    /// </para>
    /// <para>
    /// Expected after L.2.1: Path 5 calls StepUp → DoStepDown finds upper floor
    /// → sphere lifts to z ≥ 0.25 + SphereRadius and X advances past the wall.
    /// </para>
    ///
    /// <para>Retail: BSPTREE::step_sphere_up / CTransition::step_up
    /// acclient_2013_pseudo_c.txt:323849, 273099.</para>
    /// </summary>
    [Fact]
    public void B1_GroundedMover_LowStep_StepsUp()
    {
        var (root, resolved) = BSPStepUpFixtures.LowStep();
        const float stepUpHeight = 0.30f;  // larger than step (0.25), so step-up succeeds

        // CurPos (foot position) starts at z=0 (on the terrain / BSP floor at z=0).
        // The sphere center is at CurPos + (0, 0, SphereRadius) = (x, 0, 0.2).
        // lowPoint = sphere_center - (0,0,r) = (x, 0, 0) → on terrain → contact.
        var from = new Vector3(0.1f, 0f, 0f);
        // to.X = 0.6 → offset = (0.5, 0, 0), 3 sub-steps of 0.1667 each.
        // Step 2: CurPos ≈ (0.433, 0, 0), sphere center x ≈ 0.433.
        // Wall: dist = 0.5 - 0.433 = 0.067 < rad = 0.198 → HIT Path 5 ✓
        var to   = new Vector3(0.6f, 0f, 0f);   // foot stays at z=0, crosses wall at x=0.5

        var t      = BSPStepUpFixtures.MakeGroundedTransition(from, to, stepUpHeight);
        // terrainZ=0f: terrain at z=0 keeps the step-down probe grounded between
        // steps, preserving Contact/OnWalkable across the sub-step boundary.
        var engine = MakeTestEngine(root, resolved, terrainZ: 0f);

        bool ok = t.FindTransitionalPosition(engine);

        // After step-up, the character's foot (CurPos.Z) must be at or above the
        // upper floor (z=0.25). CurPos stores the foot origin; the sphere center is
        // CurPos.Z + SphereRadius.  The lower bound is the upper-floor Z minus a
        // small epsilon to tolerate floating-point rounding in AdjustSphereToPlane.
        float expectedMinZ = 0.25f - PhysicsGlobals.EPSILON * 10f;
        Assert.True(t.SpherePath.CurPos.Z >= expectedMinZ,
            $"Expected Z >= {expectedMinZ:F4} (stepped up to upper floor at z=0.25), " +
            $"got CurPos.Z = {t.SpherePath.CurPos.Z:F4}. " +
            "Path 5 must call StepUp (L.2.1) instead of wall-sliding.");
    }

    /// <summary>
    /// Grounded mover walking into the too-tall wall (5 m) should NOT step up —
    /// the wall is taller than StepUpHeight.
    ///
    /// <para>
    /// Expected: StepUp is called, DoStepDown finds no walkable surface within
    /// 0.04 m (no upper floor exists), StepUpSlide applies → mover stays
    /// left of the wall.
    /// </para>
    ///
    /// <para>Retail: SPHEREPATH::step_up_slide
    /// ACE SpherePath.cs:309-316.</para>
    /// </summary>
    [Fact]
    public void B2_GroundedMover_TallWall_BlockedOrSlides()
    {
        var (root, resolved) = BSPStepUpFixtures.TallWall();
        const float stepUpHeight = 0.04f;  // default — cannot scale 5 m wall

        // Foot at z=0 (on terrain). Same reasoning as B1.
        var from = new Vector3(0.1f, 0f, 0f);
        var to   = new Vector3(0.6f, 0f, 0f);

        var t      = BSPStepUpFixtures.MakeGroundedTransition(from, to, stepUpHeight);
        // terrainZ=0f: keep grounded between steps (same as B1).
        var engine = MakeTestEngine(root, resolved, terrainZ: 0f);

        t.FindTransitionalPosition(engine);

        // The mover should NOT have crossed the wall at x=0.5.
        float wallFace = 0.5f - BSPStepUpFixtures.SphereRadius;
        Assert.True(t.SpherePath.CurPos.X <= wallFace + PhysicsGlobals.EPSILON * 20f,
            $"Expected mover blocked before wall (x <= {wallFace:F3}), " +
            $"got CurPos.X = {t.SpherePath.CurPos.X:F4}");
    }

    /// <summary>
    /// Direct Path 5 invocation: Contact mover sphere just overlapping the low
    /// wall should NOT return Slid after L.2.1.
    ///
    /// <para>
    /// Current: returns Slid (wall-slide).
    /// Expected after L.2.1: returns OK (step-up succeeded) with Z lifted.
    /// </para>
    /// </summary>
    [Fact]
    public void B3_Path5_DirectCall_ContactHitsLowWall_NotSlid()
    {
        var (root, resolved) = BSPStepUpFixtures.LowStep();

        // Sphere center overlaps the wall (x=0.5) by half-radius.
        float r   = BSPStepUpFixtures.SphereRadius;
        var checkPos = new Vector3(0.5f - r * 0.5f, 0f, r);
        var currPos  = new Vector3(0.1f, 0f, r);

        var t = new Transition();
        t.SpherePath.InitPath(currPos, checkPos, 0xA9B40001u, r);
        t.SpherePath.SetCheckPos(checkPos, 0xA9B40001u);
        t.ObjectInfo.State          = ObjectInfoState.Contact | ObjectInfoState.OnWalkable;
        t.ObjectInfo.StepUpHeight   = 0.30f;
        t.ObjectInfo.StepDownHeight = 0.04f;
        t.CollisionInfo.LastKnownContactPlane      = new Plane(Vector3.UnitZ, 0f);
        t.CollisionInfo.LastKnownContactPlaneValid = true;

        var localSphere = new DatReaderWriter.Types.Sphere { Origin = checkPos, Radius = r };

        // Pass engine so Path 5 can call DoStepUp → DoStepDown (L.2.1).
        // Without engine the fallback wall-slide would return Slid.
        var engine = MakeTestEngine(root, resolved);

        var result = BSPQuery.FindCollisions(
            root, resolved, t, localSphere, null,
            currPos, Vector3.UnitZ, 1.0f, Quaternion.Identity, engine);

        // After L.2.1 this assertion flips from failing (Slid) to passing.
        Assert.NotEqual(TransitionState.Slid, result);
    }

    // =========================================================================
    // Group C — Phase L.2.2 (Path 6 SetCollide)
    //
    // RED before L.2.2, GREEN after.
    // =========================================================================

    /// <summary>
    /// Airborne mover hitting the flat roof from above should set Collide flag
    /// and return Adjusted (not Slid with wall-slide offset).
    ///
    /// <para>
    /// Current (wrong): Path 6 computes a wall-slide offset and returns Slid.
    /// </para>
    /// <para>
    /// Expected after L.2.2: Path 6 calls path.SetCollide(worldNormal), sets
    /// WalkableAllowance = LandingZ, returns Adjusted.
    /// </para>
    ///
    /// <para>Retail: SPHEREPATH::set_collide
    /// acclient_2013_pseudo_c.txt:321594 / ACE BSPTree.cs:210-219.</para>
    /// </summary>
    [Fact]
    public void C1_Path6_AirborneMoverHitsRoof_SetsCollideFlagAndAdjusted()
    {
        var (root, resolved) = BSPStepUpFixtures.FlatRoof();

        // Sphere center just penetrating the roof polygon (z=3) from above.
        float r   = BSPStepUpFixtures.SphereRadius;
        var checkPos = new Vector3(0f, 0f, 3f + r * 0.5f);   // half-radius above roof
        var currPos  = new Vector3(0f, 0f, 3f + r + 0.1f);   // clearly above

        var t = new Transition();
        t.SpherePath.InitPath(currPos, checkPos, 0xA9B40001u, r);
        t.SpherePath.SetCheckPos(checkPos, 0xA9B40001u);
        t.ObjectInfo.State = ObjectInfoState.None;  // airborne — no Contact

        var localSphere = new DatReaderWriter.Types.Sphere { Origin = checkPos, Radius = r };

        var result = BSPQuery.FindCollisions(
            root, resolved, t, localSphere, null,
            currPos, Vector3.UnitZ, 1.0f);

        // After L.2.2: result = Adjusted, Collide = true, WalkableAllowance = LandingZ.
        // Currently: result = Slid (wall-slide path).
        Assert.Equal(TransitionState.Adjusted, result);
        Assert.True(t.SpherePath.Collide,
            "Expected SpherePath.Collide = true after Path 6 hit (L.2.2)");
        Assert.Equal(PhysicsGlobals.LandingZ, t.SpherePath.WalkableAllowance,
            precision: 5);
    }

    /// <summary>
    /// Full integration: airborne mover drops onto the 3 m flat roof.
    ///
    /// <para>
    /// After L.2.2: TransitionalInsert sees Collide flag, re-tests as Placement,
    /// finds walkable polygon at z=3, sets ContactPlane with normal.Z ≈ 1.
    /// </para>
    /// <para>
    /// Current: mover slides sideways off the roof (never lands).
    /// Expected after L.2.2: ContactPlane is set with Normal.Z >= LandingZ.
    /// </para>
    /// </summary>
    [Fact]
    public void C2_AirborneMover_LandsOnFlatRoof_ContactPlaneSet()
    {
        var (root, resolved) = BSPStepUpFixtures.FlatRoof();

        float roofZ = 3f;
        float r     = BSPStepUpFixtures.SphereRadius;
        // CurPos = foot position. Sphere center = CurPos + (0,0,r).
        // from: foot at z = roofZ - r + 0.3f → sphere center at roofZ + 0.3 = 3.3 (above roof)
        // to:   foot at z = roofZ - r - 0.05f → sphere center at roofZ - 0.05 = 2.95 (into roof by 0.05)
        // Roof polygon at z=roofZ, normal=+Z: dist = sphere_center.z - roofZ.
        // At to: dist = -0.05; |dist| = 0.05 < rad=0.198 → roof hit ✓
        var from = new Vector3(0f, 0f, roofZ - r + 0.3f);
        var to   = new Vector3(0f, 0f, roofZ - r - 0.05f);   // sphere bottom at z ≈ 2.95 (into roof)

        var t      = BSPStepUpFixtures.MakeAirborneTransition(from, to);
        // terrainZ=-50f: airborne mover — terrain must not interfere with roof landing.
        var engine = MakeTestEngine(root, resolved, terrainZ: -50f);

        t.FindTransitionalPosition(engine);

        // After L.2.2: at least one of ContactPlane / LastKnownContactPlane is set.
        bool planeSet = t.CollisionInfo.ContactPlaneValid
                     || t.CollisionInfo.LastKnownContactPlaneValid;

        Assert.True(planeSet,
            "Expected a contact plane after landing on roof (L.2.2). " +
            "Currently Path 6 wall-slides and never sets ContactPlane.");

        if (planeSet)
        {
            var plane = t.CollisionInfo.ContactPlaneValid
                ? t.CollisionInfo.ContactPlane
                : t.CollisionInfo.LastKnownContactPlane;

            Assert.True(plane.Normal.Z >= PhysicsGlobals.LandingZ,
                $"Contact plane normal.Z ({plane.Normal.Z:F4}) must be >= LandingZ ({PhysicsGlobals.LandingZ:F4})");
        }
    }

    /// <summary>
    /// Airborne mover descending toward a steep slope (normal.Z &lt; FloorZ):
    /// Path 6 should still set the Collide flag (it fires for any polygon hit,
    /// walkable or not).
    ///
    /// <para>Retail: set_collide fires unconditionally when sphere_intersects_poly
    /// hits; the walkable check happens later in the Collide-flag handler.</para>
    /// </summary>
    [Fact]
    public void C3_Path6_AirborneMoverHitsSteepSlope_SetsCollide()
    {
        var (root, resolved) = BSPStepUpFixtures.SlopedUnwalkable();

        float r   = BSPStepUpFixtures.SphereRadius;
        // Approach the slope mid-face from above.
        var checkPos = new Vector3(0.5f, 0f, 1.0f + r * 0.5f);
        var currPos  = new Vector3(0.5f, 0f, 1.0f + r + 0.1f);

        var t = new Transition();
        t.SpherePath.InitPath(currPos, checkPos, 0xA9B40001u, r);
        t.SpherePath.SetCheckPos(checkPos, 0xA9B40001u);
        t.ObjectInfo.State = ObjectInfoState.None;  // airborne

        var localSphere = new DatReaderWriter.Types.Sphere { Origin = checkPos, Radius = r };

        var result = BSPQuery.FindCollisions(
            root, resolved, t, localSphere, null,
            currPos, Vector3.UnitZ, 1.0f);

        // After L.2.2: Collide flag set, Adjusted returned.
        // Currently: Slid (wall-slide).
        Assert.Equal(TransitionState.Adjusted, result);
        Assert.True(t.SpherePath.Collide,
            "Expected Collide flag set when airborne sphere hits slope (L.2.2)");
    }

    // =========================================================================
    // Helpers
    // =========================================================================

    /// <summary>
    /// Build a <see cref="PhysicsEngine"/> that serves one synthetic BSP object.
    /// <paramref name="terrainZ"/> sets every terrain sample to the given height.
    /// Use 0f for grounded tests (terrain flush with the BSP floor at z=0, so the
    /// step-down probe finds ground and keeps Contact/OnWalkable set between steps).
    /// Use -50f for tests where terrain must never interfere (airborne / roof landing).
    /// </summary>
    private static PhysicsEngine MakeTestEngine(
        PhysicsBSPNode                       root,
        Dictionary<ushort, ResolvedPolygon>  resolved,
        Vector3?                             objectPosition = null,
        float                                terrainZ       = 0f)
    {
        const uint LandblockId    = 0xA9B4FFFFu;
        const uint SyntheticGfxId = 0xDEADBEEFu;

        var heights   = new byte[81];  // all zero → uses index 0 from heightTable
        var heightTab = new float[256];
        for (int i = 0; i < 256; i++) heightTab[i] = terrainZ;

        var engine = new PhysicsEngine();
        engine.AddLandblock(
            LandblockId,
            new TerrainSurface(heights, heightTab),
            Array.Empty<CellSurface>(),
            Array.Empty<PortalPlane>(),
            worldOffsetX: 0f, worldOffsetY: 0f);

        // Register the BSP physics into the data cache.
        var cache  = new PhysicsDataCache();
        var bspTree = new DatReaderWriter.Types.PhysicsBSPTree { Root = root };
        var physics = new GfxObjPhysics
        {
            BSP             = bspTree,
            PhysicsPolygons = new Dictionary<ushort, DatReaderWriter.Types.Polygon>(),
            Vertices        = new DatReaderWriter.Types.VertexArray(),
            Resolved        = resolved,
            BoundingSphere  = new DatReaderWriter.Types.Sphere { Origin = Vector3.Zero, Radius = 15f },
        };
        cache.RegisterGfxObjForTest(SyntheticGfxId, physics);
        engine.DataCache = cache;

        // Register the object in the shadow registry so FindObjCollisions picks it up.
        Vector3 pos = objectPosition ?? Vector3.Zero;
        engine.ShadowObjects.Register(
            entityId:      SyntheticGfxId,
            gfxObjId:      SyntheticGfxId,
            worldPos:      pos,
            rotation:      Quaternion.Identity,
            radius:        15f,
            worldOffsetX:  0f,
            worldOffsetY:  0f,
            landblockId:   LandblockId,
            collisionType: ShadowCollisionType.BSP,
            scale:         1.0f);

        return engine;
    }
}