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

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

namespace AcDream.Core.Tests.Physics;

/// <summary>
/// Regression test for A6.P3 slice 1 — Finding 2 (ContactPlane resynthesis blowup).
///
/// <para>
/// Pre-fix behavior: <see cref="Transition.FindEnvCollisions"/> enters the
/// indoor BSP branch every frame, calls <see cref="Transition.TryFindIndoorWalkablePlane"/>
/// on every OK-result frame, which calls <see cref="Transition.ValidateWalkable"/>
/// which calls <see cref="CollisionInfo.SetContactPlane"/> 1–2 times per frame.
/// Over 60 frames of flat-floor walking: ~60–120 additional ContactPlane writes.
/// </para>
///
/// <para>
/// Post-fix behavior: the contact plane is carried forward from LKCP (Last Known
/// ContactPlane) when already grounded on the same surface; synthesis fires at most
/// once on entry. Over 60 flat-floor frames: ≤5 additional CP writes.
/// </para>
///
/// <para>
/// Fixture pattern is a blend of <see cref="FindEnvCollisionsMultiCellTests"/>
/// (engine + DataCache + <c>RegisterCellStructForTest</c> + <c>AddLandblock</c> setup)
/// and <see cref="IndoorWalkablePlaneTests"/> (sphere radius 0.48f matching
/// <c>TryFindIndoorWalkablePlane</c>'s probe thresholds + <c>BuildCellWithFloor</c>
/// floor polygon pattern).
/// Tested cell ID 0xA9B40166 (low 16 bits = 0x0166 ≥ 0x0100 → indoor branch fires).
/// </para>
///
/// <para>
/// Retail oracle: A6.P2 findings doc
/// (<c>docs/research/2026-05-21-a6-cdb-capture-findings.md</c>), Finding 2.
/// </para>
/// </summary>
public class IndoorContactPlaneRetentionTests
{
    // ── Cell ID ───────────────────────────────────────────────────────────────
    // Low 16 bits = 0x0166 ≥ 0x0100 → indoor branch of FindEnvCollisions fires.
    // This is a real Holtburg inn cell from A6.P1 scen3 captures.
    private const uint IndoorCellId = 0xA9B40166u;

    // ── Sphere radius matching BSPStepUpFixtures convention ──────────────────
    private const float SphereRadius = 0.48f;

    // ── Maximum allowed additional CP writes in 60 frames (post-fix budget) ──
    private const int MaxAdditionalCpWrites = 5;

    // ── Number of simulated frames ────────────────────────────────────────────
    private const int SimulatedFrames = 60;

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

    /// <summary>
    /// Build a BSP leaf node that contains the given polygon ids.
    ///
    /// The bounding sphere must cover the test sphere position so that
    /// <c>NodeIntersects</c> passes and the BSP traversal reaches the leaf.
    /// We parameterise the center so the caller can align it with the test geometry.
    /// </summary>
    private static PhysicsBSPTree BuildLeafBsp(
        IEnumerable<ushort> polyIds,
        Vector3             bspCenter,
        float               bspRadius = 10f)
    {
        var node = new PhysicsBSPNode
        {
            Type = BSPNodeType.Leaf,
            BoundingSphere = new Sphere { Origin = bspCenter, Radius = bspRadius },
        };
        foreach (var id in polyIds)
            node.Polygons.Add(id);
        return new PhysicsBSPTree { Root = node };
    }

    /// <summary>
    /// Build a CellPhysics with a single large upward-facing floor polygon
    /// (a 20×20 square in the XY plane at local/world Z = <paramref name="floorZ"/>),
    /// identity transforms, and a BSP leaf whose bounding sphere is centered at
    /// <paramref name="bspCenterZ"/> so that <c>NodeIntersects</c> always passes.
    ///
    /// The floor is large enough (±10 in both X and Y) that any test position
    /// within 10m of the origin in XY is always over the floor — no XY misses.
    ///
    /// Using identity transforms means local space == world space, which keeps
    /// the geometry traceable without transform arithmetic.
    ///
    /// IMPORTANT — sphere center computation:
    /// <see cref="SpherePath.InitPath"/> places <c>LocalSphere[0].Origin = (0,0,sphereRadius)</c>.
    /// After <c>SetCheckPos(worldPos, cellId)</c>, the global sphere center is
    /// <c>worldPos + (0,0,sphereRadius)</c>. The BSP bounding sphere must be
    /// centered near this sphere center (NOT at worldPos) so that NodeIntersects
    /// passes during <c>TryFindIndoorWalkablePlane → FindWalkableSphere → FindWalkableInternal</c>.
    /// </summary>
    private static CellPhysics BuildCellWithFloor(float floorZ, float bspCenterZ)
    {
        // 20×20 upward-facing floor at local Z = floorZ.
        var verts = new[]
        {
            new Vector3(-10f, -10f, floorZ),
            new Vector3( 10f, -10f, floorZ),
            new Vector3( 10f,  10f, floorZ),
            new Vector3(-10f,  10f, floorZ),
        };
        var normal = Vector3.UnitZ;         // straight up
        float d    = -floorZ;              // N·p + D = 0  →  D = -floorZ

        var floorPoly = new ResolvedPolygon
        {
            Vertices  = verts,
            Plane     = new Plane(normal, d),
            NumPoints = 4,
            SidesType = CullMode.None,
        };

        // BSP leaf bounding sphere centered at the actual sphere center
        // (worldPos + (0,0,sphereRadius)). Radius 10 covers the 20×20 floor
        // polygon and the test sphere.
        var bspCenter = new Vector3(0f, 0f, bspCenterZ);

        return new CellPhysics
        {
            BSP                   = BuildLeafBsp(new ushort[] { 0 }, bspCenter, bspRadius: 10f),
            WorldTransform        = Matrix4x4.Identity,
            InverseWorldTransform = Matrix4x4.Identity,
            Resolved              = new Dictionary<ushort, ResolvedPolygon> { [0] = floorPoly },
            // CellBSP intentionally left null: ResolveCellId sees
            // indoorCell.CellBSP?.Root == null → returns the indoor id unchanged
            // (the "can't verify; trust FindCellList" branch), so the cell ID
            // stays indoor throughout every loop iteration.
            CellBSP = null,
        };
    }

    /// <summary>
    /// Build a minimal PhysicsEngine with a flat landblock registered so that
    /// FindEnvCollisions's AddLandblock / SampleTerrainWalkable / ResolveCellId
    /// paths all have a coherent landblock context if they ever reach outdoors.
    /// </summary>
    private static PhysicsEngine BuildEngine(uint indoorCellId, CellPhysics cell)
    {
        var engine = new PhysicsEngine();
        engine.DataCache = new PhysicsDataCache();

        // Flat terrain height table: all zeroes.
        var heights = new byte[81];
        Array.Fill(heights, (byte)0);
        var ht = new float[256];
        for (int i = 0; i < 256; i++) ht[i] = (float)i;

        // Use the high 16 bits of IndoorCellId as the landblock prefix.
        // The landblock key is constructed with 0xFFFF suffix (the "all cells"
        // landblock sentinel used throughout the test suite).
        uint landblockKey = (indoorCellId & 0xFFFF0000u) | 0xFFFFu;
        engine.AddLandblock(landblockKey,
            new TerrainSurface(heights, ht),
            Array.Empty<CellSurface>(),
            Array.Empty<PortalPlane>(),
            worldOffsetX: 0f, worldOffsetY: 0f);

        engine.DataCache.RegisterCellStructForTest(indoorCellId, cell);
        return engine;
    }

    /// <summary>
    /// Build a grounded Transition positioned on the floor at
    /// <paramref name="worldPos"/> and headed to <paramref name="worldTarget"/>.
    ///
    /// Sets Contact + OnWalkable (grounded mover), seeds LastKnownContactPlane
    /// with the floor plane, and calls <see cref="CollisionInfo.SetContactPlane"/>
    /// once so the CP is considered "already established."
    ///
    /// The initial SetContactPlane call is factored into <paramref name="seededWrites"/>
    /// (which the test snapshots before the loop).
    /// </summary>
    private static Transition BuildGroundedTransition(
        Vector3 worldPos,
        Vector3 worldTarget,
        uint    cellId,
        Plane   floorPlane)
    {
        var t = new Transition();
        t.SpherePath.InitPath(worldPos, worldTarget, cellId, SphereRadius);
        // InitPath above already set CheckPos = begin; no second SetCheckPos
        // needed because this test doesn't move the sphere from begin to a
        // different destination (unlike multi-cell tests that walk a path).

        // Grounded state — mirrors BSPStepUpFixtures.MakeGroundedTransition.
        t.ObjectInfo.State          = ObjectInfoState.Contact | ObjectInfoState.OnWalkable;
        t.ObjectInfo.StepUpHeight   = 0.04f;
        t.ObjectInfo.StepDownHeight = 0.04f;
        t.ObjectInfo.StepDown       = true;

        // Seed LastKnownContactPlane — the resolver's "we were on this floor last frame" memory.
        t.CollisionInfo.LastKnownContactPlane      = floorPlane;
        t.CollisionInfo.LastKnownContactPlaneValid = true;

        // Seed ContactPlane so ValidateWalkable sees us as already grounded.
        // This call increments ContactPlaneWriteCount to 1 (the "seeded" count).
        t.CollisionInfo.SetContactPlane(floorPlane, cellId, isWater: false);

        return t;
    }

    // =========================================================================
    // Tests
    // =========================================================================

    /// <summary>
    /// Sixty frames of indoor flat-floor walking must not produce more than
    /// <see cref="MaxAdditionalCpWrites"/> additional ContactPlane writes
    /// beyond the initial seeding write.
    ///
    /// <para>
    /// Pre-fix (broken): TryFindIndoorWalkablePlane fires + ValidateWalkable
    /// calls SetContactPlane 1–2×/frame → ~60–120 additional writes → FAIL.
    /// Post-fix: LKCP is restored early; synthesis skipped for already-grounded
    /// movers → ≤5 additional writes → PASS.
    /// </para>
    /// </summary>
    [Fact]
    public void IndoorFlatFloorWalking_60Frames_ProducesAtMost5ExtraCpWrites()
    {
        // ── Arrange ───────────────────────────────────────────────────────────
        // Post-fix grounded model:
        //
        //   SpherePath.InitPath sets LocalSphere[0].Origin = (0,0,sphereRadius).
        //   After SetCheckPos(worldPos), the global sphere CENTER is at
        //     worldPos + (0,0,SphereRadius) = (0,0,SphereRadius).
        //
        //   A correctly-grounded sphere has its bottom exactly at the floor:
        //     sphereBottom = sphereCenter.Z - SphereRadius
        //                  = worldPosZ + SphereRadius - SphereRadius
        //                  = worldPosZ
        //
        //   With worldPosZ = 0 (= floorZ), the bottom just touches the floor.
        //   SphereIntersectsPolyInternal uses a strict penetration check, so a
        //   sphere touching-but-not-penetrating does NOT count as a hit.
        //   Path 5 (Contact grounded) returns OK with no CP write.
        //
        //   Pre-fix: the sphere was positioned 5 cm BELOW the floor so that
        //   TryFindIndoorWalkablePlane → ValidateWalkable would fire every frame.
        //   Post-fix: synthesis is gone; the sphere must be at its natural
        //   grounded position (bottom at floorZ) so that BSP Path 5 finds no
        //   penetration and returns OK immediately — zero additional CP writes.
        const float floorZ       = 0f;
        const float worldPosZ    = floorZ;               // sphere bottom exactly at floor
        const float sphereCenterZ = worldPosZ + SphereRadius; // = 0.48

        var floorPlane = new Plane(Vector3.UnitZ, -floorZ);   // N·p + D = 0 → D = 0
        var worldPos   = new Vector3(0f, 0f, worldPosZ);

        // BSP bounding sphere centered at the sphere center so NodeIntersects
        // passes during the BSP traversal.
        var cell   = BuildCellWithFloor(floorZ, bspCenterZ: sphereCenterZ);
        var engine = BuildEngine(IndoorCellId, cell);
        var t      = BuildGroundedTransition(
            worldPos,
            worldPos + new Vector3(0.001f, 0f, 0f),  // tiny horizontal target
            IndoorCellId,
            floorPlane);

        // Snapshot the write count right after seeding (should be exactly 1).
        int seededWrites = t.CollisionInfo.ContactPlaneWriteCount;
        Assert.Equal(1, seededWrites);

        // ── Act — 60 frames of flat-floor walking ─────────────────────────────
        // Each iteration: nudge CheckPos a tiny bit forward in X (stays on the
        // same floor at the grounded Z), then call FindEnvCollisions as the
        // indoor branch would call it each physics frame.
        for (int frame = 0; frame < SimulatedFrames; frame++)
        {
            // Advance position by 1 mm forward — same Z (grounded on floor).
            var newPos = new Vector3(frame * 0.001f, 0f, worldPosZ);
            t.SpherePath.SetCheckPos(newPos, IndoorCellId);

            // Simulate FindEnvCollisions as the physics loop calls it.
            t.FindEnvCollisions(engine);
        }

        // ── Assert ────────────────────────────────────────────────────────────
        int totalWrites      = t.CollisionInfo.ContactPlaneWriteCount;
        int additionalWrites = totalWrites - seededWrites;

        Assert.True(
            additionalWrites <= MaxAdditionalCpWrites,
            $"Expected ≤{MaxAdditionalCpWrites} additional CP writes across " +
            $"{SimulatedFrames} flat-floor frames, got {additionalWrites}. " +
            "Finding 2 fix not complete.");
    }
}