using System;
using System.Collections.Generic;
using System.Numerics;

namespace AcDream.Core.Physics;

/// <summary>
/// Top-level physics resolver that combines <see cref="TerrainSurface"/> and
/// <see cref="CellSurface"/> to resolve entity movement with step-height
/// enforcement and outdoor/indoor cell transitions.
///
/// <para>
/// Landblocks are registered via <see cref="AddLandblock"/> with their
/// terrain, indoor cells, and world-space offsets. <see cref="Resolve"/>
/// takes a current position, the entity's current cell ID, a movement delta,
/// and a step-up height limit; it returns the validated new position, the
/// updated cell ID, and whether the entity is standing on a surface.
/// </para>
/// </summary>
public sealed class PhysicsEngine
{
    private readonly Dictionary<uint, LandblockPhysics> _landblocks = new();

    /// <summary>Number of registered landblocks (diagnostic).</summary>
    public int LandblockCount => _landblocks.Count;

    /// <summary>
    /// Cell-based spatial index for static object collision.
    /// Populated during landblock streaming; queried by the Transition system.
    /// </summary>
    public ShadowObjectRegistry ShadowObjects { get; } = new();

    /// <summary>
    /// Physics BSP cache shared with the streaming loader. Set once by the
    /// host (GameWindow) immediately after construction. The Transition system
    /// reads this during FindObjCollisions to perform narrow-phase BSP tests.
    /// </summary>
    public PhysicsDataCache? DataCache { get; set; }

    private sealed record LandblockPhysics(
        TerrainSurface Terrain,
        IReadOnlyList<CellSurface> Cells,
        IReadOnlyList<PortalPlane> Portals,
        float WorldOffsetX,
        float WorldOffsetY);

    /// <summary>
    /// Register a landblock with its terrain surface, indoor cells, portal
    /// planes, and world-space origin offset.
    /// </summary>
    public void AddLandblock(uint landblockId, TerrainSurface terrain,
        IReadOnlyList<CellSurface> cells, IReadOnlyList<PortalPlane> portals,
        float worldOffsetX, float worldOffsetY)
    {
        _landblocks[landblockId] = new LandblockPhysics(terrain, cells, portals, worldOffsetX, worldOffsetY);
    }

    /// <summary>
    /// Remove a previously registered landblock, including its shadow objects.
    /// </summary>
    public void RemoveLandblock(uint landblockId)
    {
        _landblocks.Remove(landblockId);
        ShadowObjects.RemoveLandblock(landblockId);
    }

    /// <summary>
    /// Find the landblock that contains the given world-space XY position and
    /// return its ID plus world-space origin offsets. Returns false when no
    /// registered landblock covers the position.
    /// Used by Transition.FindObjCollisions to build the shadow-object query.
    /// </summary>
    public bool TryGetLandblockContext(float worldX, float worldY,
        out uint landblockId, out float worldOffsetX, out float worldOffsetY)
    {
        foreach (var kvp in _landblocks)
        {
            var lb = kvp.Value;
            float localX = worldX - lb.WorldOffsetX;
            float localY = worldY - lb.WorldOffsetY;
            if (localX >= 0f && localX < 192f && localY >= 0f && localY < 192f)
            {
                landblockId  = kvp.Key;
                worldOffsetX = lb.WorldOffsetX;
                worldOffsetY = lb.WorldOffsetY;
                return true;
            }
        }
        landblockId  = 0;
        worldOffsetX = 0f;
        worldOffsetY = 0f;
        return false;
    }

    /// <summary>
    /// Sample the outdoor terrain Z at the given world-space XY position.
    /// Searches all registered landblocks; returns null if no landblock covers the position.
    /// Used by Transition.FindEnvCollisions for terrain collision resolution.
    /// </summary>
    public float? SampleTerrainZ(float worldX, float worldY)
    {
        foreach (var kvp in _landblocks)
        {
            var lb = kvp.Value;
            float localX = worldX - lb.WorldOffsetX;
            float localY = worldY - lb.WorldOffsetY;
            if (localX >= 0f && localX < 192f && localY >= 0f && localY < 192f)
                return lb.Terrain.SampleZ(localX, localY);
        }
        return null;
    }

    /// <summary>
    /// Sample the per-point water depth at the given world-space XY
    /// (meters by which the character is allowed to sink below the
    /// contact plane — 0.9 on fully-flooded water cells, 0.45 on
    /// partial-water near a water corner, 0.1 on non-water corners of
    /// partial-water cells, 0 on dry cells). Matches ACE
    /// <c>ObjCell.get_water_depth</c>. Used by
    /// <see cref="Transition"/> to visually submerge characters in water
    /// without needing a separate water surface mesh.
    /// </summary>
    public float SampleWaterDepth(float worldX, float worldY)
    {
        foreach (var kvp in _landblocks)
        {
            var lb = kvp.Value;
            float localX = worldX - lb.WorldOffsetX;
            float localY = worldY - lb.WorldOffsetY;
            if (localX >= 0f && localX < 192f && localY >= 0f && localY < 192f)
                return lb.Terrain.SampleWaterDepth(localX, localY);
        }
        return 0f;
    }

    /// <summary>
    /// Sample the outdoor terrain plane (Z + sloped normal) at the given
    /// world-space XY position. The returned <see cref="System.Numerics.Plane"/>
    /// has the true terrain-triangle normal (NOT a flat <c>(0,0,1)</c>), and
    /// its <c>D</c> is set so the plane passes through the sampled point. Used
    /// by <see cref="Transition"/> to build a CORRECT contact plane — a flat
    /// plane breaks slope tracking because <c>AdjustOffset</c>'s projection
    /// onto a flat plane cannot impart the Z component that horizontal
    /// velocity needs to follow the slope.
    /// </summary>
    public System.Numerics.Plane? SampleTerrainPlane(float worldX, float worldY)
    {
        foreach (var kvp in _landblocks)
        {
            var lb = kvp.Value;
            float localX = worldX - lb.WorldOffsetX;
            float localY = worldY - lb.WorldOffsetY;
            if (localX >= 0f && localX < 192f && localY >= 0f && localY < 192f)
            {
                var (z, normal) = lb.Terrain.SampleSurface(localX, localY);
                // System.Numerics.Plane convention: dot(Normal, P) + D == 0
                // for points P on the plane. Pick P = (worldX, worldY, z).
                float d = -(normal.X * worldX + normal.Y * worldY + normal.Z * z);
                return new System.Numerics.Plane(normal, d);
            }
        }
        return null;
    }

    /// <summary>
    /// Resolve an entity's movement from <paramref name="currentPos"/> by
    /// applying <paramref name="delta"/> (XY only) and computing the correct Z
    /// from the terrain or indoor cell floor beneath the candidate position.
    ///
    /// <para>
    /// Step-height enforcement rejects horizontal movement when the upward Z
    /// change exceeds <paramref name="stepUpHeight"/>. Downhill movement is
    /// always accepted. Returns <see cref="ResolveResult.IsOnGround"/> false
    /// when no loaded landblock covers the candidate position.
    /// </para>
    /// </summary>
    public ResolveResult Resolve(Vector3 currentPos, uint cellId, Vector3 delta, float stepUpHeight)
    {
        var candidatePos = currentPos + new Vector3(delta.X, delta.Y, 0f);

        // Find the landblock this candidate position falls in.
        LandblockPhysics? physics = null;
        foreach (var kvp in _landblocks)
        {
            var lb = kvp.Value;
            float localX = candidatePos.X - lb.WorldOffsetX;
            float localY = candidatePos.Y - lb.WorldOffsetY;
            if (localX >= 0 && localX < 192f && localY >= 0 && localY < 192f)
            {
                physics = lb;
                break;
            }
        }

        if (physics is null)
            return new ResolveResult(candidatePos, cellId, IsOnGround: false);

        float localCandX = candidatePos.X - physics.WorldOffsetX;
        float localCandY = candidatePos.Y - physics.WorldOffsetY;

        // Check if the candidate position falls on any indoor cell floor.
        // Pick the cell whose floor Z is closest to the entity's current Z.
        CellSurface? bestCell = null;
        float? bestCellZ = null;
        float bestZDist = float.MaxValue;

        foreach (var cell in physics.Cells)
        {
            float? floorZ = cell.SampleFloorZ(candidatePos.X, candidatePos.Y);
            if (floorZ is not null)
            {
                float dist = MathF.Abs(floorZ.Value - currentPos.Z);
                if (dist < bestZDist)
                {
                    bestCell = cell;
                    bestCellZ = floorZ;
                    bestZDist = dist;
                }
            }
        }

        // Determine target surface Z and cell.
        float terrainZ = physics.Terrain.SampleZ(localCandX, localCandY);
        float targetZ;
        uint targetCellId;

        // Only the low 16 bits of cellId carry the cell index. Outdoor
        // cells are 0x0001–0x0040; indoor (EnvCell) cells are 0x0100+.
        // The full 32-bit cellId includes the landblock prefix in the
        // high 16 bits (e.g., 0xA9B40001), so we MUST mask before
        // comparing. Without the mask, every cell looks "indoor" because
        // 0xA9B40001 >= 0x0100 → the engine always takes the "stay
        // indoors" path and snaps Z to an EnvCell floor 28m below.
        bool currentlyIndoor = (cellId & 0xFFFFu) >= 0x0100;

        if (currentlyIndoor)
        {
            // Check whether the player crosses a portal belonging to the current cell.
            uint currentCellIndex = cellId & 0xFFFFu;
            PortalPlane? crossedPortal = null;
            foreach (var portal in physics.Portals)
            {
                // Only portals owned by the current cell are relevant when indoors.
                if ((portal.OwnerCellId & 0xFFFFu) != currentCellIndex) continue;
                if (portal.IsCrossing(currentPos, candidatePos))
                {
                    crossedPortal = portal;
                    break;
                }
            }

            if (crossedPortal is not null)
            {
                if (crossedPortal.Value.TargetCellId == 0xFFFFu)
                {
                    // Indoor → Outdoor exit.
                    targetZ = terrainZ;
                    targetCellId = physics.Terrain.ComputeOutdoorCellId(localCandX, localCandY);
                }
                else
                {
                    // Indoor → Indoor (room to room).
                    uint nextCellIndex = crossedPortal.Value.TargetCellId & 0xFFFFu;
                    CellSurface? nextCell = null;
                    foreach (var c in physics.Cells)
                    {
                        if ((c.CellId & 0xFFFFu) == nextCellIndex) { nextCell = c; break; }
                    }
                    float? nextFloorZ = nextCell?.SampleFloorZ(candidatePos.X, candidatePos.Y);
                    targetZ = nextFloorZ ?? terrainZ;
                    targetCellId = nextCellIndex;
                }
            }
            else if (bestCellZ is not null)
            {
                // Staying in the same indoor cell.
                targetZ = bestCellZ.Value;
                targetCellId = bestCell!.CellId & 0xFFFFu;
            }
            else
            {
                // No cell floor found and no portal crossed — fall back to outdoor.
                targetZ = terrainZ;
                targetCellId = physics.Terrain.ComputeOutdoorCellId(localCandX, localCandY);
            }
        }
        else
        {
            // Outdoor player: check for a portal crossing into an indoor cell.
            // Outside-facing portals have TargetCellId == 0xFFFF (they face the
            // outdoor world); crossing one from the outdoor side enters the OwnerCellId.
            PortalPlane? crossedPortal = null;
            foreach (var portal in physics.Portals)
            {
                if (portal.TargetCellId != 0xFFFFu) continue; // only outside-facing portals
                if (portal.IsCrossing(currentPos, candidatePos))
                {
                    crossedPortal = portal;
                    break;
                }
            }

            if (crossedPortal is not null)
            {
                // Outdoor → Indoor: enter the OwnerCellId IF the target cell
                // actually contains the candidate position. Without CellBSP,
                // we verify by checking that SampleFloorZ returns non-null
                // (position is within the cell's floor polygon bounds) AND the
                // floor Z is close to the player's current Z (not a basement
                // 30m below). This prevents the wall-bounce bug where portal
                // planes on upper floors captured outdoor positions.
                uint enterCellIndex = crossedPortal.Value.OwnerCellId & 0xFFFFu;
                CellSurface? enterCell = null;
                foreach (var c in physics.Cells)
                {
                    if ((c.CellId & 0xFFFFu) == enterCellIndex) { enterCell = c; break; }
                }
                float? enterFloorZ = enterCell?.SampleFloorZ(candidatePos.X, candidatePos.Y);

                // Validate: floor must exist AND be within step height of current Z.
                // This rejects transitions to basements, upper floors, and cells
                // whose floor polygon doesn't actually cover this position.
                bool validTransition = enterFloorZ is not null
                    && MathF.Abs(enterFloorZ.Value - currentPos.Z) < stepUpHeight + 2f;

                if (validTransition)
                {
                    targetZ = enterFloorZ!.Value;
                    targetCellId = enterCellIndex;
                }
                else
                {
                    // Portal crossed but target cell doesn't contain us — stay outdoor.
                    targetZ = terrainZ;
                    targetCellId = physics.Terrain.ComputeOutdoorCellId(localCandX, localCandY);
                }
            }
            else
            {
                // Stay outdoors on terrain.
                targetZ = terrainZ;
                targetCellId = physics.Terrain.ComputeOutdoorCellId(localCandX, localCandY);
            }
        }

        // Step-height enforcement: block upward movement that exceeds the limit.
        float zDelta = targetZ - currentPos.Z;
        if (zDelta > stepUpHeight)
        {
            // Too steep to step up — reject horizontal movement.
            return new ResolveResult(currentPos, cellId, IsOnGround: true);
        }

        return new ResolveResult(
            new Vector3(candidatePos.X, candidatePos.Y, targetZ),
            targetCellId,
            IsOnGround: true);
    }

    /// <summary>
    /// Resolve movement using the CTransition sphere-sweep system.
    /// Subdivides movement into sphere-radius steps, tests terrain collision
    /// at each step, handles step-down for ground contact.
    /// Falls back to the simple <see cref="Resolve"/> if the transition fails.
    ///
    /// <para>
    /// <paramref name="body"/> is optional but highly recommended for movement
    /// that runs across multiple frames. When provided, the previous frame's
    /// contact plane is copied INTO the transition's CollisionInfo (mirroring
    /// retail's <c>PhysicsObj.get_object_info → InitContactPlane</c> at
    /// <c>PhysicsObj.cs:2598-2604</c>). That seed is critical for slope
    /// tracking: <c>AdjustOffset</c> projects the Euler offset onto the plane
    /// so horizontal velocity acquires the correct Z component for the slope,
    /// preventing the character from floating on downhill runs where the
    /// per-frame descent exceeds the 4 cm step-down budget.
    /// </para>
    ///
    /// <para>
    /// On return, the plane discovered during this call is written BACK to
    /// <paramref name="body"/>, so the next frame's transition starts with
    /// an up-to-date plane seed. Callers without a persistent body (tests,
    /// one-shot movements) can pass <c>null</c> and accept the first-frame
    /// hiccup.
    /// </para>
    /// </summary>
    public ResolveResult ResolveWithTransition(
        Vector3 currentPos, Vector3 targetPos, uint cellId,
        float sphereRadius, float sphereHeight,
        float stepUpHeight, float stepDownHeight,
        bool isOnGround,
        PhysicsBody? body = null)
    {
        var transition = new Transition();
        transition.ObjectInfo.StepUpHeight = stepUpHeight;
        transition.ObjectInfo.StepDownHeight = stepDownHeight;
        transition.ObjectInfo.StepDown = true;

        if (isOnGround)
            transition.ObjectInfo.State |= ObjectInfoState.Contact | ObjectInfoState.OnWalkable;

        // K-fix7 (2026-04-26): only seed the contact plane when the body
        // is actually grounded. Pre-seeding while AIRBORNE caused
        // AdjustOffset's "Have a contact plane / Moving away from plane"
        // branch to fire on every jump step — which calls
        // Plane::snap_to_plane on the offset and ZEROES the Z component,
        // killing all upward jump motion (the body's Z velocity stayed
        // ~9 m/s but Position.Z never advanced because every step's
        // offset got snapped flat). Retail's CTransition::init at
        // 0x509dd0 (named-retail line 271954) explicitly clears
        // contact_plane_valid = 0 at the start of every transition
        // resolve, then ValidateWalkable re-establishes it during the
        // sweep when the sphere bottom is within EPSILON of the terrain
        // plane — so for grounded motion the plane is set fresh every
        // resolve, and for airborne motion no plane interferes.
        //
        // We KEEP the seeding when isOnGround for slope-walking
        // continuity (the original concern that motivated the seed) —
        // walking up a hill needs the previous step's slope to project
        // movement properly. Airborne / jumping must start with no
        // plane so AdjustOffset preserves Z.
        if (isOnGround && body is not null && body.ContactPlaneValid)
        {
            transition.CollisionInfo.SetContactPlane(
                body.ContactPlane,
                body.ContactPlaneCellId,
                body.ContactPlaneIsWater);
        }

        transition.SpherePath.InitPath(currentPos, targetPos, cellId, sphereRadius, sphereHeight);

        bool ok = transition.FindTransitionalPosition(this);

        var sp = transition.SpherePath;
        var ci = transition.CollisionInfo;

        // Persist the resulting contact plane state back to the body so the
        // next frame's transition can seed from it. Uses LastKnownContactPlane
        // when current is invalid (e.g., airborne this frame), matching retail.
        if (body is not null)
        {
            if (ci.ContactPlaneValid)
            {
                body.ContactPlaneValid = true;
                body.ContactPlane = ci.ContactPlane;
                body.ContactPlaneCellId = ci.ContactPlaneCellId;
                body.ContactPlaneIsWater = ci.ContactPlaneIsWater;
            }
            else if (ci.LastKnownContactPlaneValid)
            {
                body.ContactPlaneValid = true;
                body.ContactPlane = ci.LastKnownContactPlane;
                body.ContactPlaneCellId = ci.LastKnownContactPlaneCellId;
                body.ContactPlaneIsWater = ci.LastKnownContactPlaneIsWater;
            }
            else
            {
                body.ContactPlaneValid = false;
            }
        }

        if (ok)
        {
            bool onGround = ci.ContactPlaneValid
                || transition.ObjectInfo.State.HasFlag(ObjectInfoState.OnWalkable);

            return new ResolveResult(sp.CheckPos, sp.CheckCellId, onGround);
        }

        // Transition failed (e.g., stuck in corner, too many steps).
        // Use whatever position the transition reached (partial movement)
        // instead of falling back to the no-collision Resolve.
        // If CheckPos hasn't moved from CurPos, the player stays put —
        // this is correct behavior when completely blocked.
        bool partialOnGround = ci.ContactPlaneValid
            || transition.ObjectInfo.State.HasFlag(ObjectInfoState.OnWalkable)
            || isOnGround;

        return new ResolveResult(sp.CheckPos, sp.CheckCellId != 0 ? sp.CheckCellId : cellId, partialOnGround);
    }
}