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

namespace AcDream.Core.Tests.Physics;

/// <summary>
/// Phase L.1c (2026-04-28). Covers <see cref="RemoteMoveToDriver"/> — the
/// per-tick steering port of retail
/// <c>MoveToManager::HandleMoveToPosition</c> for server-controlled remote
/// creatures.
/// </summary>
public class RemoteMoveToDriverTests
{
    private const float Epsilon = 1e-3f;

    private static float Yaw(Quaternion q)
    {
        var fwd = Vector3.Transform(new Vector3(0, 1, 0), q);
        return MathF.Atan2(-fwd.X, fwd.Y);
    }

    [Fact]
    public void Drive_AlreadyAtTarget_ReportsArrived()
    {
        var bodyPos = new Vector3(10f, 20f, 0f);
        var bodyRot = Quaternion.Identity;
        var dest = new Vector3(10f, 20.3f, 0f);

        var result = RemoteMoveToDriver.Drive(
            bodyPos, bodyRot, dest,
            minDistance: 0.5f, distanceToObject: 0.6f,
            dt: 0.016f, moveTowards: true,
            out var newOrient);

        Assert.Equal(RemoteMoveToDriver.DriveResult.Arrived, result);
        Assert.Equal(bodyRot, newOrient); // orientation untouched
    }

    [Fact]
    public void Drive_AceMeleePacket_UsesDistanceToObjectAsArrival()
    {
        // ACE chase packet: MinDistance=0, DistanceToObject=0.6 (melee).
        // Body at 0.5m from target should ARRIVE — not keep oscillating
        // around the target the way it did pre-fix when only MinDistance
        // was the gate. This is the "monster keeps running in different
        // directions when it should be attacking" regression fix.
        var bodyPos = new Vector3(0f, 0f, 0f);
        var bodyRot = Quaternion.Identity;
        var dest = new Vector3(0f, 0.5f, 0f);

        var result = RemoteMoveToDriver.Drive(
            bodyPos, bodyRot, dest,
            minDistance: 0f, distanceToObject: 0.6f,
            dt: 0.016f, moveTowards: true,
            out _);

        Assert.Equal(RemoteMoveToDriver.DriveResult.Arrived, result);
    }

    [Fact]
    public void Drive_FleeArrival_UsesMinDistance()
    {
        // Flee branch (moveTowards=false): arrival when dist >= MinDistance.
        // Retail / ACE both use MinDistance for the flee-arrival threshold.
        var bodyPos = new Vector3(0f, 0f, 0f);
        var bodyRot = Quaternion.Identity;
        var dest = new Vector3(0f, 6f, 0f);

        var result = RemoteMoveToDriver.Drive(
            bodyPos, bodyRot, dest,
            minDistance: 5.0f, distanceToObject: 0.6f,
            dt: 0.016f, moveTowards: false,
            out _);

        Assert.Equal(RemoteMoveToDriver.DriveResult.Arrived, result);
    }

    [Fact]
    public void Drive_ChaseDoesNotArriveAtMinDistanceFloor()
    {
        // Regression: my earlier max(MinDistance, DistanceToObject) port
        // would have arrived here because dist (1.5) <= MinDistance (2.0).
        // Retail uses DistanceToObject for chase arrival, so a chase at
        // dist=1.5 with DistanceToObject=0.6 should still STEER, not arrive.
        var bodyPos = new Vector3(0f, 0f, 0f);
        var bodyRot = Quaternion.Identity;
        var dest = new Vector3(0f, 1.5f, 0f);

        var result = RemoteMoveToDriver.Drive(
            bodyPos, bodyRot, dest,
            minDistance: 2.0f, distanceToObject: 0.6f,
            dt: 0.016f, moveTowards: true,
            out _);

        Assert.Equal(RemoteMoveToDriver.DriveResult.Steering, result);
    }

    [Fact]
    public void Drive_ChasingButNotInRange_ReportsSteering()
    {
        var bodyPos = new Vector3(0f, 0f, 0f);
        var bodyRot = Quaternion.Identity;            // facing +Y
        var dest = new Vector3(0f, 50f, 0f);          // straight ahead

        var result = RemoteMoveToDriver.Drive(
            bodyPos, bodyRot, dest,
            minDistance: 0f, distanceToObject: 0f,
            dt: 0.016f, moveTowards: true,
            out var newOrient);

        Assert.Equal(RemoteMoveToDriver.DriveResult.Steering, result);
        // Already facing target → snap branch keeps yaw at 0.
        Assert.InRange(Yaw(newOrient), -Epsilon, Epsilon);
    }

    [Fact]
    public void Drive_TargetSlightlyOffAxis_SnapsWithinTolerance()
    {
        // Body facing +Y; target at (1, 10, 0) — that's a small angle
        // (about 5.7°), well within the 20° snap tolerance.
        var bodyPos = Vector3.Zero;
        var bodyRot = Quaternion.Identity;
        var dest = new Vector3(1f, 10f, 0f);

        var result = RemoteMoveToDriver.Drive(
            bodyPos, bodyRot, dest,
            minDistance: 0f, distanceToObject: 0f,
            dt: 0.016f, moveTowards: true,
            out var newOrient);

        Assert.Equal(RemoteMoveToDriver.DriveResult.Steering, result);
        // Snap should land us pointing at (1, 10): yaw = atan2(-1, 10) ≈ -0.0997 rad.
        float expectedYaw = MathF.Atan2(-1f, 10f);
        Assert.InRange(Yaw(newOrient), expectedYaw - Epsilon, expectedYaw + Epsilon);

        // Verify orientation actually transforms +Y onto the (1,10) line.
        var worldFwd = Vector3.Transform(new Vector3(0, 1, 0), newOrient);
        Assert.InRange(worldFwd.X / worldFwd.Y, 0.1f - 1e-3f, 0.1f + 1e-3f);
    }

    [Fact]
    public void Drive_TargetBeyondTolerance_RotatesByLimitedStep()
    {
        // Body facing +Y; target at (-10, 0) — that's 90° to the left
        // (well beyond the 20° snap tolerance), so we turn by at most
        // TurnRateRadPerSec * dt this tick rather than snapping.
        var bodyPos = Vector3.Zero;
        var bodyRot = Quaternion.Identity;            // yaw = 0
        var dest = new Vector3(-10f, 0f, 0f);          // yaw = +π/2 (left)
        const float dt = 0.1f;

        var result = RemoteMoveToDriver.Drive(
            bodyPos, bodyRot, dest,
            minDistance: 0f, distanceToObject: 0f,
            dt: dt, moveTowards: true,
            out var newOrient);

        Assert.Equal(RemoteMoveToDriver.DriveResult.Steering, result);
        float expectedStep = RemoteMoveToDriver.TurnRateRadPerSec * dt;
        // We should turn LEFT (positive yaw) toward the target.
        Assert.InRange(Yaw(newOrient), expectedStep - Epsilon, expectedStep + Epsilon);
    }

    [Fact]
    public void Drive_TargetBehind_TurnsRightOrLeftViaShortestPath()
    {
        // Body facing +Y; target directly behind at (0, -10, 0).
        // |delta| = π, equally close either way; the implementation
        // picks one (sign depends on float wobble) — just assert
        // we made progress (yaw changed by exactly TurnRate * dt).
        var bodyPos = Vector3.Zero;
        var bodyRot = Quaternion.Identity;
        var dest = new Vector3(0f, -10f, 0f);
        const float dt = 0.1f;

        var result = RemoteMoveToDriver.Drive(
            bodyPos, bodyRot, dest,
            minDistance: 0f, distanceToObject: 0f,
            dt: dt, moveTowards: true,
            out var newOrient);

        Assert.Equal(RemoteMoveToDriver.DriveResult.Steering, result);
        float expectedStep = RemoteMoveToDriver.TurnRateRadPerSec * dt;
        Assert.InRange(MathF.Abs(Yaw(newOrient)), expectedStep - Epsilon, expectedStep + Epsilon);
    }

    [Fact]
    public void Drive_PreservesOrientationAtArrival()
    {
        var bodyPos = new Vector3(5f, 5f, 0f);
        var bodyRot = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, 1.234f);
        var dest = new Vector3(5.01f, 5.01f, 0f);

        var result = RemoteMoveToDriver.Drive(
            bodyPos, bodyRot, dest,
            minDistance: 0.5f, distanceToObject: 0.6f,
            dt: 0.016f, moveTowards: true,
            out var newOrient);

        Assert.Equal(RemoteMoveToDriver.DriveResult.Arrived, result);
        // Caller would zero velocity; orientation should be untouched
        // so the body settles facing whatever direction it was already.
        Assert.Equal(bodyRot, newOrient);
    }

    [Fact]
    public void ClampApproachVelocity_NoOverShoot_LandsExactlyAtThreshold()
    {
        // Body 1 m from destination, running at 4 m/s, dt = 0.1 s.
        // Naive advance = 0.4 m → would end at 0.6 m from dest, exactly
        // on the threshold. With threshold=0.6 and remaining=0.4, the
        // clamp should let the full velocity through (advance == remaining).
        var bodyPos = new Vector3(0f, 0f, 0f);
        var dest = new Vector3(0f, 1f, 0f);
        var vel = new Vector3(0f, 4f, 0f);

        var clamped = RemoteMoveToDriver.ClampApproachVelocity(
            bodyPos, vel, dest, arrivalThreshold: 0.6f, dt: 0.1f, moveTowards: true);

        // Within float-precision: 4 m/s × 0.1 s = 0.4 m, exactly the
        // remaining distance. The clamp may apply a 0.99999×-style
        // tiny scale due to FP rounding — accept anything ≥ 99.9% of
        // the input as "no meaningful overshoot prevention applied."
        Assert.InRange(clamped.Y, 4f * 0.999f, 4f);
        Assert.Equal(0f, clamped.X);
        Assert.Equal(0f, clamped.Z);
    }

    [Fact]
    public void ClampApproachVelocity_WouldOverShoot_ScalesDownToExactLanding()
    {
        // Body 1 m from destination, running at 4 m/s, dt = 0.2 s.
        // Naive advance = 0.8 m → would overshoot 0.6 m threshold by 0.4 m.
        // remaining = 0.4 m, advance = 0.8 m → scale = 0.5.
        // Velocity should be halved → 2 m/s.
        var bodyPos = new Vector3(0f, 0f, 0f);
        var dest = new Vector3(0f, 1f, 0f);
        var vel = new Vector3(0f, 4f, 0f);

        var clamped = RemoteMoveToDriver.ClampApproachVelocity(
            bodyPos, vel, dest, arrivalThreshold: 0.6f, dt: 0.2f, moveTowards: true);

        Assert.InRange(clamped.Y, 2f - Epsilon, 2f + Epsilon);
        Assert.Equal(0f, clamped.X);
    }

    [Fact]
    public void ClampApproachVelocity_AlreadyAtThreshold_ZeroesHorizontal()
    {
        // Body exactly 0.6 m from dest with threshold 0.6 → remaining ≈ 0.
        // Any horizontal velocity would overshoot; clamp must zero it.
        var bodyPos = new Vector3(0f, 0f, 0f);
        var dest = new Vector3(0f, 0.6f, 0f);
        var vel = new Vector3(0f, 4f, 0.5f); // some Z to confirm Z is preserved

        var clamped = RemoteMoveToDriver.ClampApproachVelocity(
            bodyPos, vel, dest, arrivalThreshold: 0.6f, dt: 0.016f, moveTowards: true);

        Assert.Equal(0f, clamped.X);
        Assert.Equal(0f, clamped.Y);
        Assert.Equal(0.5f, clamped.Z); // gravity / Z handling unaffected
    }

    [Fact]
    public void ClampApproachVelocity_FleeBranch_NoOp()
    {
        // moveTowards=false (flee): no overshoot risk, return velocity unchanged.
        var bodyPos = Vector3.Zero;
        var dest = new Vector3(0f, 1f, 0f);
        var vel = new Vector3(0f, -4f, 0f);

        var clamped = RemoteMoveToDriver.ClampApproachVelocity(
            bodyPos, vel, dest, arrivalThreshold: 5f, dt: 0.5f, moveTowards: false);

        Assert.Equal(vel, clamped);
    }

    [Fact]
    public void OriginToWorld_AppliesLandblockGridShift()
    {
        // Cell ID 0xA8B4000E → landblock x=0xA8, y=0xB4. With live center
        // at (0xA9, 0xB4), that's one landblock west and zero north,
        // so origin (10, 20, 0) inside that landblock should map to
        // (10 - 192, 20 + 0, 0) = (-182, 20, 0) in render-world space.
        var w = RemoteMoveToDriver.OriginToWorld(
            originCellId: 0xA8B4000Eu,
            originX: 10f, originY: 20f, originZ: 0f,
            liveCenterLandblockX: 0xA9, liveCenterLandblockY: 0xB4);

        Assert.Equal(-182f, w.X);
        Assert.Equal(20f, w.Y);
        Assert.Equal(0f, w.Z);
    }
}