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);
    }
}