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>BSPQuery.FindCollisions → BSP traversal → NodeIntersects</c>
    /// (the indoor cell's primary BSP query in <see cref="Transition.FindEnvCollisions"/>).
    /// </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 ───────────────────────────────────────────────────────────
        // Geometry: sphere center 5 cm BELOW the floor so that the synthesis
        // path's distance guard passes and TryFindIndoorWalkablePlane would
        // actually find the floor polygon if synthesis were re-introduced.
        //
        //   SpherePath.InitPath sets LocalSphere[0].Origin = (0,0,SphereRadius).
        //   After SetCheckPos(worldPos), the global sphere CENTER is at
        //     worldPos + (0,0,SphereRadius) = (worldPosX, 0, -0.05 + 0.48 = 0.43).
        //
        //   PolygonHitsSpherePrecise distance guard (BSPQuery.cs line ~117):
        //     dist = Dot(normal=(0,0,1), center=(x,0,0.43)) + D(=0) = 0.43
        //     rad  = SphereRadius - EPSILON = 0.48 - 0.002 = 0.478
        //     |dist| = 0.43 < 0.478 → guard passes → polygon is tested → FOUND.
        //   So TryFindIndoorWalkablePlane WOULD call ValidateWalkable → CP write
        //   if the synthesis call were present. With the strip in place it is never
        //   called → ≤5 additional writes → PASS.
        //
        //   With the post-5f7722a setup (worldPosZ = floorZ = 0):
        //     dist = 0.48, rad = 0.478, |dist| = 0.48 > 0.478 → guard FIRES →
        //     TryFindIndoorWalkablePlane returns false even WITH synthesis code.
        //   That setup was not a regression sentinel; this one is.
        //
        //   Path 5 (BSP Contact branch, BSPQuery.cs ~line 1732):
        //   The loop advances only in X, so movement = (0.001, 0, 0).
        //   PosHitsSphere culls hits where Dot(movement, normal) >= 0.
        //   Dot((0.001,0,0), (0,0,1)) = 0 >= 0 → floor polygon is ALWAYS
        //   rejected by the front-face cull, even though the sphere center is
        //   below the floor. Path 5 exits OK with no CP write regardless of
        //   whether the Contact flag is set. Leaving Contact set keeps this
        //   test on the realistic grounded-mover path.
        const float floorZ        = 0f;
        const float worldPosZ     = floorZ - 0.05f;          // 5 cm below floor
        const float sphereCenterZ = worldPosZ + SphereRadius; // = 0.43

        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 (sphere center 5 cm below 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.");
    }
}