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_RetailMinDistanceWins_WhenLargerThanDistanceToObject()
    {
        // Hypothetical retail packet: MinDistance=2.0 (set explicitly),
        // DistanceToObject=0.6 (default). Arrival should fire at 2 m
        // because retail's algorithm uses MinDistance and it's the larger
        // of the two.
        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.Arrived, 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 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);
    }
}