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

namespace AcDream.Core.Tests.Physics;

/// <summary>
/// Unit tests for PhysicsBody — the C# port of CPhysicsObj's core simulation
/// from acclient.exe (FUN_005111d0, FUN_00511420, FUN_00511ec0, FUN_00511fa0,
/// FUN_00511de0, FUN_0050f940, FUN_00515020).
/// </summary>
public sealed class PhysicsBodyTests
{
    // ── helpers ──────────────────────────────────────────────────────────

    private static PhysicsBody MakeAirborne()
    {
        var body = new PhysicsBody
        {
            State = PhysicsStateFlags.Gravity | PhysicsStateFlags.ReportCollisions,
        };
        // Airborne: not in Contact, not OnWalkable
        body.TransientState = TransientStateFlags.Active;
        return body;
    }

    private static PhysicsBody MakeGrounded()
    {
        var body = new PhysicsBody
        {
            State = PhysicsStateFlags.Gravity | PhysicsStateFlags.ReportCollisions,
        };
        body.TransientState = TransientStateFlags.Contact | TransientStateFlags.OnWalkable | TransientStateFlags.Active;
        return body;
    }

    // ════════════════════════════════════════════════════════════════════
    // calc_acceleration
    // ════════════════════════════════════════════════════════════════════

    [Fact]
    public void calc_acceleration_airborne_gravity_sets_minus_9_8_on_z()
    {
        var body = MakeAirborne();
        body.calc_acceleration();

        Assert.Equal(0f, body.Acceleration.X);
        Assert.Equal(0f, body.Acceleration.Y);
        Assert.Equal(-9.8f, body.Acceleration.Z, precision: 6);
    }

    [Fact]
    public void calc_acceleration_grounded_zeros_acceleration_and_omega()
    {
        var body = MakeGrounded();
        body.Acceleration = new Vector3(1f, 2f, 3f);
        body.Omega = new Vector3(0.5f, 0.5f, 0.5f);
        body.calc_acceleration();

        Assert.Equal(Vector3.Zero, body.Acceleration);
        Assert.Equal(Vector3.Zero, body.Omega);
    }

    [Fact]
    public void calc_acceleration_no_gravity_flag_zeros_acceleration()
    {
        var body = new PhysicsBody
        {
            State = PhysicsStateFlags.None,  // no Gravity flag
            TransientState = TransientStateFlags.Active,
        };
        body.Acceleration = new Vector3(0f, 0f, -9.8f);
        body.calc_acceleration();

        Assert.Equal(Vector3.Zero, body.Acceleration);
    }

    [Fact]
    public void calc_acceleration_sledding_airborne_still_applies_gravity()
    {
        // Sledding but not grounded — gravity still applies
        var body = new PhysicsBody
        {
            State = PhysicsStateFlags.Gravity | PhysicsStateFlags.Sledding,
            TransientState = TransientStateFlags.Active,
        };
        body.calc_acceleration();

        Assert.Equal(-9.8f, body.Acceleration.Z, precision: 6);
    }

    // ════════════════════════════════════════════════════════════════════
    // UpdatePhysicsInternal — Euler integration
    // ════════════════════════════════════════════════════════════════════

    [Fact]
    public void UpdatePhysicsInternal_integrates_position_correctly_one_step()
    {
        // Analytical: x(t) = x0 + v0*t + 0.5*a*t²
        // With v0=(1,0,0), a=(0,0,-9.8), dt=0.1
        //   x = 0.1
        //   z = 0.5 * (-9.8) * 0.01 = -0.049
        var body = MakeAirborne();
        body.Velocity = new Vector3(1f, 0f, 0f);
        body.Acceleration = new Vector3(0f, 0f, -9.8f);

        body.UpdatePhysicsInternal(0.1f);

        Assert.Equal(0.1f, body.Position.X, precision: 5);
        Assert.Equal(0f,   body.Position.Y, precision: 5);
        // 0.5 * (-9.8) * 0.01 = -0.049
        Assert.Equal(-0.049f, body.Position.Z, precision: 4);
    }

    [Fact]
    public void UpdatePhysicsInternal_velocity_updated_by_acceleration_times_dt()
    {
        var body = MakeAirborne();
        body.Velocity = new Vector3(0f, 0f, 0f);
        body.Acceleration = new Vector3(0f, 0f, -9.8f);

        body.UpdatePhysicsInternal(0.5f);

        // velocity += accel * dt = (0, 0, -9.8 * 0.5) = (0, 0, -4.9)
        Assert.Equal(0f, body.Velocity.X, precision: 5);
        Assert.Equal(0f, body.Velocity.Y, precision: 5);
        Assert.Equal(-4.9f, body.Velocity.Z, precision: 4);
    }

    [Fact]
    public void UpdatePhysicsInternal_multiple_frames_accumulates_correctly()
    {
        // Free-fall from rest under gravity for N frames of dt each.
        // Analytical z(t) = 0.5 * g * t²  where g = -9.8
        // After 10 frames of 0.1 s each (total t=1.0 s):
        //   z = 0.5 * (-9.8) * 1.0 = -4.9
        // The Euler integrator accumulates small truncation error, so allow 2% tolerance.
        var body = MakeAirborne();
        body.Velocity = Vector3.Zero;
        body.Acceleration = new Vector3(0f, 0f, PhysicsBody.Gravity);

        const int frames = 10;
        const float dt = 0.1f;
        for (int i = 0; i < frames; i++)
            body.UpdatePhysicsInternal(dt);

        float expected = 0.5f * PhysicsBody.Gravity * (frames * dt) * (frames * dt);
        Assert.True(MathF.Abs(body.Position.Z - expected) < 0.15f,
            $"Expected z ≈ {expected:F4}, got {body.Position.Z:F4}");
    }

    [Fact]
    public void UpdatePhysicsInternal_zero_velocity_clears_active_flag_when_grounded()
    {
        var body = MakeGrounded();
        body.Velocity = Vector3.Zero;
        body.TransientState |= TransientStateFlags.Active;

        body.UpdatePhysicsInternal(0.1f);

        Assert.False(body.IsActive);
    }

    // ════════════════════════════════════════════════════════════════════
    // set_velocity — velocity clamping
    // ════════════════════════════════════════════════════════════════════

    [Fact]
    public void set_velocity_below_max_stores_velocity_unchanged()
    {
        var body = new PhysicsBody();
        var v = new Vector3(10f, 5f, 2f);
        body.set_velocity(v);

        Assert.Equal(v, body.Velocity);
    }

    [Fact]
    public void set_velocity_above_max_clamps_to_MaxVelocity_magnitude()
    {
        var body = new PhysicsBody();
        // velocity with magnitude > 50
        var v = new Vector3(100f, 0f, 0f);
        body.set_velocity(v);

        Assert.True(body.Velocity.Length() <= PhysicsBody.MaxVelocity + 1e-4f,
            $"Velocity magnitude {body.Velocity.Length()} exceeds MaxVelocity {PhysicsBody.MaxVelocity}");
        Assert.Equal(PhysicsBody.MaxVelocity, body.Velocity.Length(), precision: 4);
    }

    [Fact]
    public void set_velocity_diagonal_above_max_clamps_and_preserves_direction()
    {
        var body = new PhysicsBody();
        var dir = Vector3.Normalize(new Vector3(3f, 4f, 0f)); // unit vector
        var v = dir * 80f;  // magnitude = 80 > 50
        body.set_velocity(v);

        Assert.Equal(PhysicsBody.MaxVelocity, body.Velocity.Length(), precision: 3);
        // Direction should be preserved
        var resultDir = Vector3.Normalize(body.Velocity);
        Assert.Equal(dir.X, resultDir.X, precision: 4);
        Assert.Equal(dir.Y, resultDir.Y, precision: 4);
    }

    [Fact]
    public void set_velocity_sets_active_flag()
    {
        var body = new PhysicsBody();
        body.TransientState = TransientStateFlags.None;
        body.set_velocity(new Vector3(1f, 0f, 0f));

        Assert.True(body.IsActive);
    }

    [Fact]
    public void set_velocity_exactly_at_max_is_not_clamped()
    {
        var body = new PhysicsBody();
        var v = new Vector3(PhysicsBody.MaxVelocity, 0f, 0f);
        body.set_velocity(v);

        Assert.Equal(v.X, body.Velocity.X, precision: 4);
        Assert.Equal(0f,  body.Velocity.Y, precision: 4);
        Assert.Equal(0f,  body.Velocity.Z, precision: 4);
    }

    // ════════════════════════════════════════════════════════════════════
    // set_local_velocity — body→world transform
    // ════════════════════════════════════════════════════════════════════

    [Fact]
    public void set_local_velocity_identity_orientation_passes_through()
    {
        var body = new PhysicsBody { Orientation = Quaternion.Identity };
        body.set_local_velocity(new Vector3(1f, 0f, 0f));

        Assert.Equal(1f, body.Velocity.X, precision: 5);
        Assert.Equal(0f, body.Velocity.Y, precision: 5);
        Assert.Equal(0f, body.Velocity.Z, precision: 5);
    }

    [Fact]
    public void set_local_velocity_90_degree_yaw_rotates_forward_to_right()
    {
        // A 90° CCW rotation around Z maps +X in local space to +Y in world space.
        var body = new PhysicsBody
        {
            Orientation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, MathF.PI / 2f)
        };
        body.set_local_velocity(new Vector3(1f, 0f, 0f));

        // After 90° yaw: local +X becomes world +Y (approximately)
        Assert.True(MathF.Abs(body.Velocity.X) < 1e-4f, $"Expected Vx≈0, got {body.Velocity.X}");
        Assert.True(MathF.Abs(body.Velocity.Y - 1f) < 1e-4f, $"Expected Vy≈1, got {body.Velocity.Y}");
        Assert.True(MathF.Abs(body.Velocity.Z) < 1e-4f, $"Expected Vz≈0, got {body.Velocity.Z}");
    }

    [Fact]
    public void set_local_velocity_180_degree_yaw_reverses_horizontal_forward()
    {
        var body = new PhysicsBody
        {
            Orientation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, MathF.PI)
        };
        body.set_local_velocity(new Vector3(1f, 0f, 0f));

        Assert.True(MathF.Abs(body.Velocity.X + 1f) < 1e-4f, $"Expected Vx≈-1, got {body.Velocity.X}");
        Assert.True(MathF.Abs(body.Velocity.Y) < 1e-4f, $"Expected Vy≈0, got {body.Velocity.Y}");
    }

    [Fact]
    public void set_local_velocity_magnitude_preserved_after_rotation()
    {
        var body = new PhysicsBody
        {
            Orientation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, 1.23f)
        };
        var localVel = new Vector3(3f, 4f, 0f);
        body.set_local_velocity(localVel);

        Assert.Equal(localVel.Length(), body.Velocity.Length(), precision: 4);
    }

    // ════════════════════════════════════════════════════════════════════
    // set_on_walkable
    // ════════════════════════════════════════════════════════════════════

    [Fact]
    public void set_on_walkable_true_sets_OnWalkable_flag()
    {
        var body = MakeAirborne();
        body.set_on_walkable(true);

        Assert.True(body.OnWalkable);
    }

    [Fact]
    public void set_on_walkable_false_clears_OnWalkable_flag()
    {
        var body = MakeGrounded();
        body.set_on_walkable(false);

        Assert.False(body.OnWalkable);
    }

    [Fact]
    public void set_on_walkable_true_also_calls_calc_acceleration_zeroing_accel()
    {
        // When Contact + OnWalkable (non-sledding): acceleration should be zeroed.
        var body = new PhysicsBody
        {
            State = PhysicsStateFlags.Gravity | PhysicsStateFlags.ReportCollisions,
            TransientState = TransientStateFlags.Contact,
            Acceleration = new Vector3(0f, 0f, -9.8f),
        };
        body.set_on_walkable(true);

        Assert.Equal(Vector3.Zero, body.Acceleration);
    }

    [Fact]
    public void set_on_walkable_false_allows_gravity_to_apply()
    {
        var body = MakeGrounded();
        body.set_on_walkable(false);

        // After clearing OnWalkable, calc_acceleration should apply gravity.
        Assert.Equal(-9.8f, body.Acceleration.Z, precision: 6);
    }

    // ════════════════════════════════════════════════════════════════════
    // calc_friction
    // ════════════════════════════════════════════════════════════════════

    [Fact]
    public void calc_friction_not_on_walkable_does_nothing()
    {
        var body = MakeAirborne();
        body.Velocity = new Vector3(5f, 0f, 0f);
        var before = body.Velocity;
        body.calc_friction(0.1f, body.Velocity.LengthSquared());

        Assert.Equal(before, body.Velocity);
    }

    [Fact]
    public void calc_friction_velocity_parallel_to_ground_reduces_magnitude()
    {
        // Ground normal = +Z, velocity is horizontal (no inward component),
        // but if we tilt slightly downward (dot < 0) friction fires.
        var body = MakeGrounded();
        body.GroundNormal = Vector3.UnitZ;
        // Give a small downward Z component so dot(normal, vel) < 0
        body.Velocity = new Vector3(5f, 0f, -0.1f);
        float mag2 = body.Velocity.LengthSquared();

        body.calc_friction(0.1f, mag2);

        // Speed should be reduced by friction
        Assert.True(body.Velocity.Length() < new Vector3(5f, 0f, 0f).Length(),
            "Friction should reduce velocity magnitude");
    }

    [Fact]
    public void calc_friction_velocity_moving_away_from_normal_no_change()
    {
        // dot(GroundNormal=(0,0,1), velocity=(5,0,1)) = 1 > 0 → no friction
        var body = MakeGrounded();
        body.GroundNormal = Vector3.UnitZ;
        body.Velocity = new Vector3(5f, 0f, 1f);  // moving up = away from ground
        var before = body.Velocity;
        float mag2 = body.Velocity.LengthSquared();

        body.calc_friction(0.1f, mag2);

        Assert.Equal(before, body.Velocity);
    }

    [Fact]
    public void calc_friction_zero_friction_coefficient_no_reduction()
    {
        var body = MakeGrounded();
        body.GroundNormal = Vector3.UnitZ;
        body.Velocity = new Vector3(5f, 0f, -0.01f);
        body.Friction = 0f;  // frictionless surface
        float mag2 = body.Velocity.LengthSquared();

        body.calc_friction(0.1f, mag2);

        // After removing normal component, velocity magnitude should be ≈ 5 (horizontal)
        // With friction=0, pow(1-0, dt)=1, so velocity unchanged beyond normal removal
        Assert.True(body.Velocity.Length() > 4.9f,
            $"Zero friction: speed {body.Velocity.Length()} should stay near 5");
    }

    [Fact]
    public void calc_friction_removes_normal_component_from_velocity()
    {
        // Velocity = (1, 0, -1), GroundNormal = (0, 0, 1)
        // dot = -1 → velocity -= (-1) * (0,0,1) = velocity + (0,0,1) → (1, 0, 0)
        var body = MakeGrounded();
        body.GroundNormal = Vector3.UnitZ;
        body.Friction = 0f;  // no friction to isolate normal-removal behavior
        body.Velocity = new Vector3(1f, 0f, -1f);
        float mag2 = body.Velocity.LengthSquared();

        body.calc_friction(1.0f, mag2);

        // After normal removal the Z component should be zero (or very small).
        Assert.True(MathF.Abs(body.Velocity.Z) < 1e-4f,
            $"Normal component should be removed; Vz = {body.Velocity.Z}");
        Assert.Equal(1f, body.Velocity.X, precision: 4);
    }

    // ════════════════════════════════════════════════════════════════════
    // update_object — per-frame driver
    // ════════════════════════════════════════════════════════════════════

    [Fact]
    public void update_object_dt_below_min_quantum_does_not_advance()
    {
        var body = MakeAirborne();
        body.Velocity = new Vector3(1f, 0f, 0f);
        body.Acceleration = Vector3.Zero;
        body.LastUpdateTime = 0.0;

        // Advance by less than MinQuantum — should be a no-op
        body.update_object(PhysicsBody.MinQuantum * 0.5);

        Assert.Equal(Vector3.Zero, body.Position);
    }

    [Fact]
    public void update_object_dt_above_huge_quantum_consumes_time_without_simulating()
    {
        var body = MakeAirborne();
        body.Velocity = new Vector3(1f, 0f, 0f);
        body.Acceleration = Vector3.Zero;
        body.LastUpdateTime = 0.0;

        body.update_object(PhysicsBody.HugeQuantum + 0.5);

        // Time consumed but no physics step — position unchanged
        Assert.Equal(Vector3.Zero, body.Position);
        Assert.Equal(PhysicsBody.HugeQuantum + 0.5, body.LastUpdateTime, precision: 10);
    }

    [Fact]
    public void update_object_advances_position_over_valid_dt()
    {
        var body = MakeAirborne();
        // No friction or gravity interference — just pure horizontal velocity
        body.State = PhysicsStateFlags.None;  // no gravity
        body.Velocity = new Vector3(10f, 0f, 0f);
        body.LastUpdateTime = 0.0;

        double dt = 0.1;
        body.update_object(dt);

        // x ≈ 10 * 0.1 = 1.0  (ignoring sub-step rounding)
        Assert.True(body.Position.X > 0f, "Position should have advanced");
    }

    [Fact]
    public void update_object_updates_LastUpdateTime()
    {
        var body = MakeAirborne();
        body.LastUpdateTime = 0.0;
        body.State = PhysicsStateFlags.None;

        double t = 0.05;
        body.update_object(t);

        Assert.Equal(t, body.LastUpdateTime, precision: 10);
    }

    [Fact]
    public void update_object_gravity_free_fall_accumulates_downward_velocity()
    {
        var body = MakeAirborne();
        // Let it fall for one valid quantum
        body.LastUpdateTime = 0.0;
        double dt = PhysicsBody.MinQuantum * 2; // > MinQuantum but < HugeQuantum

        body.update_object(dt);

        // After one step velocity should be negative Z
        Assert.True(body.Velocity.Z < 0f,
            $"Gravity should produce negative Z velocity; got {body.Velocity.Z}");
    }
}