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

namespace AcDream.Core.Tests.Physics;

// ─────────────────────────────────────────────────────────────────────────────
// PositionManagerTests — 6 tests covering ComputeOffset.
//
// Mirrors retail CPhysicsObj::UpdateObjectInternal (acclient @ 0x00513730).
// Pure-function combiner: animation root motion (seqVel × dt, rotated by
// body orientation) + InterpolationManager.AdjustOffset correction.
// ─────────────────────────────────────────────────────────────────────────────

public sealed class PositionManagerTests
{
    // ── helpers ───────────────────────────────────────────────────────────────

    private static PositionManager Make() => new();

    private static InterpolationManager EmptyInterp() => new();

    // =========================================================================
    // Test 1: stationary remote — both sources zero, no motion
    // =========================================================================

    [Fact]
    public void ComputeOffset_StationaryRemote_BothSourcesZero_NoMotion()
    {
        var pm    = Make();
        var interp = EmptyInterp();

        Vector3 offset = pm.ComputeOffset(
            dt: 0.1,
            currentBodyPosition: Vector3.Zero,
            seqVel: Vector3.Zero,
            ori: Quaternion.Identity,
            interp: interp,
            maxSpeed: 4f);

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

    // =========================================================================
    // Test 2: animation only, identity orientation, forward velocity
    // =========================================================================

    [Fact]
    public void ComputeOffset_AnimationOnly_Forward_BodyAdvances()
    {
        var pm     = Make();
        var interp = EmptyInterp();

        // seqVel = (0, 4, 0), dt = 0.1 → rootMotion = (0, 0.4, 0)
        Vector3 offset = pm.ComputeOffset(
            dt: 0.1,
            currentBodyPosition: Vector3.Zero,
            seqVel: new Vector3(0f, 4f, 0f),
            ori: Quaternion.Identity,
            interp: interp,
            maxSpeed: 0f);

        Assert.Equal(0f,   offset.X, precision: 4);
        Assert.Equal(0.4f, offset.Y, precision: 4);
        Assert.Equal(0f,   offset.Z, precision: 4);
    }

    // =========================================================================
    // Test 3: animation only, 180° yaw around Z — body moves south (-Y)
    // =========================================================================

    [Fact]
    public void ComputeOffset_AnimationOnly_OrientedSouth_BodyMovesSouth()
    {
        var pm     = Make();
        var interp = EmptyInterp();

        // 180° around Z flips +Y → -Y
        Quaternion ori = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, MathF.PI);

        Vector3 offset = pm.ComputeOffset(
            dt: 0.1,
            currentBodyPosition: Vector3.Zero,
            seqVel: new Vector3(0f, 4f, 0f),
            ori: ori,
            interp: interp,
            maxSpeed: 0f);

        Assert.Equal(0f,    offset.X, precision: 4);
        Assert.Equal(-0.4f, offset.Y, precision: 4);
    }

    // =========================================================================
    // Test 4: interp only, no animation — body chases queue
    // =========================================================================

    [Fact]
    public void ComputeOffset_InterpOnly_NoAnimation_BodyChasesQueue()
    {
        var pm     = Make();
        var interp = new InterpolationManager();

        // Enqueue target 1m ahead on +X; body starts at origin
        interp.Enqueue(new Vector3(1f, 0f, 0f), heading: 0f, isMovingTo: false);

        // Expected catch-up: catchUpSpeed = maxSpeed × 2 = 4 × 2 = 8 m/s
        // step = 8 × 0.1 = 0.8m (< dist = 1m so no overshoot clamp)
        Vector3 offset = pm.ComputeOffset(
            dt: 0.1,
            currentBodyPosition: Vector3.Zero,
            seqVel: Vector3.Zero,
            ori: Quaternion.Identity,
            interp: interp,
            maxSpeed: 4f);

        Assert.Equal(0.8f, offset.X, precision: 3);
        Assert.Equal(0f,   offset.Y, precision: 3);
        Assert.Equal(0f,   offset.Z, precision: 3);
    }

    // =========================================================================
    // Test 5: both sources active — combined delta
    // =========================================================================

    [Fact]
    public void ComputeOffset_BothActive_Combined()
    {
        var pm     = Make();
        var interp = new InterpolationManager();

        // Enqueue target 1m ahead on +X
        interp.Enqueue(new Vector3(1f, 0f, 0f), heading: 0f, isMovingTo: false);

        // rootMotion = (0, 4, 0) × 0.1 = (0, 0.4, 0)
        // correction ≈ (0.8, 0, 0)
        // combined ≈ (0.8, 0.4, 0)
        Vector3 offset = pm.ComputeOffset(
            dt: 0.1,
            currentBodyPosition: Vector3.Zero,
            seqVel: new Vector3(0f, 4f, 0f),
            ori: Quaternion.Identity,
            interp: interp,
            maxSpeed: 4f);

        Assert.Equal(0.8f, offset.X, precision: 3);
        Assert.Equal(0.4f, offset.Y, precision: 3);
        Assert.Equal(0f,   offset.Z, precision: 3);
    }

    // =========================================================================
    // Test 6: local-to-world rotation — +90° yaw around Z
    // =========================================================================

    [Fact]
    public void ComputeOffset_LocalToWorldRotation_Yaw90()
    {
        var pm     = Make();
        var interp = EmptyInterp();

        // +90° CCW around Z in right-handed coordinates:
        //   body-local +Y → world -X
        Quaternion ori = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, MathF.PI / 2f);

        // seqVel = (0, 1, 0), dt = 1 → rootMotionLocal = (0, 1, 0)
        // after Transform by ori → (-1, 0, 0) approximately
        Vector3 offset = pm.ComputeOffset(
            dt: 1.0,
            currentBodyPosition: Vector3.Zero,
            seqVel: new Vector3(0f, 1f, 0f),
            ori: ori,
            interp: interp,
            maxSpeed: 0f);

        Assert.Equal(-1f, offset.X, precision: 4);
        Assert.Equal(0f,  offset.Y, precision: 4);
        Assert.Equal(0f,  offset.Z, precision: 4);
    }

    // =========================================================================
    // Test 7: slope projection — anim root motion gains Z proportional to slope
    //
    // Lock-the-fix for the "remote running on a slope shows ~5 Hz Z staircase"
    // bug: the queue-empty fallback was returning a flat (Z=0) world motion
    // because animation cycles bake Z=0 in body-local. Projecting onto the
    // local terrain plane gives the motion a Z component matching slope angle
    // × forward speed.
    // =========================================================================

    [Fact]
    public void ComputeOffset_SeqVelFallback_SlopedTerrainNormal_ProjectsZOntoSlope()
    {
        var pm     = Make();
        var interp = EmptyInterp();   // queue empty → fallback path runs

        // Slope tilted 30° eastward (+X is downhill). Plane normal points
        // up-and-east-of-vertical: (sin 30°, 0, cos 30°) = (0.5, 0, 0.866).
        Vector3 N = Vector3.Normalize(new Vector3(0.5f, 0f, MathF.Sqrt(3f) / 2f));

        // Body running due east at 4 m/s, dt = 1s → rootMotionWorld initially
        // (4, 0, 0). After projection onto the plane:
        //   into = dot((4,0,0), (0.5,0,0.866)) = 2.0
        //   result = (4,0,0) - (0.5,0,0.866) * 2.0 = (3.0, 0, -1.732)
        // i.e. body moves east AND descends ~1.73m for the second.
        Vector3 offset = pm.ComputeOffset(
            dt: 1.0,
            currentBodyPosition: Vector3.Zero,
            seqVel: new Vector3(4f, 0f, 0f),
            ori: Quaternion.Identity,
            interp: interp,
            maxSpeed: 0f,
            terrainNormal: N);

        Assert.Equal( 3.000f, offset.X, precision: 3);
        Assert.Equal( 0.000f, offset.Y, precision: 3);
        Assert.Equal(-1.732f, offset.Z, precision: 3);
    }

    [Fact]
    public void ComputeOffset_SeqVelFallback_FlatTerrainNormal_NoZChange()
    {
        var pm     = Make();
        var interp = EmptyInterp();

        // Flat ground: normal = +Z. Projection should be a no-op.
        Vector3 offset = pm.ComputeOffset(
            dt: 0.1,
            currentBodyPosition: Vector3.Zero,
            seqVel: new Vector3(0f, 4f, 0f),
            ori: Quaternion.Identity,
            interp: interp,
            maxSpeed: 0f,
            terrainNormal: Vector3.UnitZ);

        Assert.Equal(0f,   offset.X, precision: 4);
        Assert.Equal(0.4f, offset.Y, precision: 4);
        Assert.Equal(0f,   offset.Z, precision: 4);
    }
}