using System.Collections.Generic;
using Pathfinding.RVO;
using Pathfinding.Util;
using Unity.Collections;
using Unity.Jobs;
using UnityEngine;

namespace FishROV.AvoidanceBenchmark
{
    public sealed class AstarProRvoVelocityLikeAdapter : IAvoidanceAdapter
    {
        private const float TimeHorizon = 2.5f;
        private const float ObstacleTimeHorizon = 2.5f;
        private const float BoundaryPadding = 0.15f;

        private readonly Dictionary<BenchmarkAgent, IAgent> agents = new Dictionary<BenchmarkAgent, IAgent>(512);
        private readonly Dictionary<BenchmarkAgent, float> verticalVelocities = new Dictionary<BenchmarkAgent, float>(512);
        private SimulatorBurst simulator;
        private AvoidanceBenchmarkConfig config;

        public string Name => "A* Pro RVO VelocityLike";
        public bool IsAvailable => simulator != null;
        public string Status => config != null && config.DimensionMode == AvoidanceDimensionMode.XYZ3D
            ? $"A* Pathfinding Project Pro RVO velocity-like adapter active, {agents.Count} agents; RVO still solves on XZ plane."
            : $"A* Pathfinding Project Pro RVO velocity-like adapter active, {agents.Count} agents.";

        public void Initialize(AvoidanceBenchmarkConfig config)
        {
            this.config = config.Clone();
            agents.Clear();
            verticalVelocities.Clear();
            simulator = new SimulatorBurst(MovementPlane.XZ)
            {
                HardCollisions = true,
                SymmetryBreakingBias = 0.1f,
                UseNavmeshAsObstacle = false
            };
        }

        public BenchmarkAgent CreateAgent(BenchmarkAgentSpawnInfo spawnInfo, Transform parent)
        {
            var agent = BenchmarkAgent.CreatePrimitive(spawnInfo, parent);
            if (simulator == null)
            {
                return agent;
            }

            var rvoAgent = simulator.AddAgent(spawnInfo.Position);
            rvoAgent.MovementPlane = SimpleMovementPlane.XZPlane;
            rvoAgent.Radius = Mathf.Max(0.01f, spawnInfo.Radius);
            rvoAgent.Height = Mathf.Max(0.01f, spawnInfo.Height);
            rvoAgent.AgentTimeHorizon = TimeHorizon;
            rvoAgent.ObstacleTimeHorizon = ObstacleTimeHorizon;
            rvoAgent.MaxNeighbours = Mathf.Max(1, config.MaxNeighbours);
            rvoAgent.Priority = Mathf.Clamp01(spawnInfo.Priority <= 0f ? 0.5f : spawnInfo.Priority);
            rvoAgent.Layer = RVOLayer.DefaultAgent;
            rvoAgent.CollidesWith = (RVOLayer)(-1);
            rvoAgent.Position = spawnInfo.Position;
            rvoAgent.SetTarget(spawnInfo.Position, 0f, agent.MaxSpeed, NoEndOfPath());
            agents[agent] = rvoAgent;
            verticalVelocities[agent] = 0f;
            return agent;
        }

        public void SetPreferredVelocity(BenchmarkAgent agent, Vector3 preferredVelocity)
        {
            if (simulator == null || !agents.TryGetValue(agent, out var rvoAgent))
            {
                agent.ApplyVelocity(preferredVelocity);
                return;
            }

            var clamped = Vector3.ClampMagnitude(preferredVelocity, agent.MaxSpeed);
            var horizontal = new Vector3(clamped.x, 0f, clamped.z);
            var speed = Mathf.Min(agent.MaxSpeed, horizontal.magnitude);
            var position = agent.transform.position;
            verticalVelocities[agent] = config.DimensionMode == AvoidanceDimensionMode.XYZ3D ? clamped.y : 0f;
            rvoAgent.Position = position;

            if (speed <= 0.001f)
            {
                rvoAgent.SetTarget(position, 0f, agent.MaxSpeed, NoEndOfPath());
                return;
            }

            var target = position + horizontal;
            rvoAgent.SetTarget(target, speed, speed, NoEndOfPath());
        }

        public void SetTarget(BenchmarkAgent agent, Vector3 target)
        {
            agent.Target = target;
        }

        public void Tick(float deltaTime)
        {
            if (simulator == null || deltaTime <= 0f)
            {
                return;
            }

            simulator.Update(default(JobHandle), Mathf.Max(0.001f, deltaTime), false, Allocator.TempJob).Complete();

            foreach (var pair in agents)
            {
                var agent = pair.Key;
                var rvoAgent = pair.Value;
                var direction = rvoAgent.CalculatedTargetPoint - agent.transform.position;
                direction.y = 0f;

                var speed = Mathf.Min(agent.MaxSpeed, rvoAgent.CalculatedSpeed);
                var velocity = direction.sqrMagnitude > 0.0001f
                    ? direction.normalized * speed
                    : Vector3.zero;
                velocity = KeepVelocityInsideArena(agent.transform.position, velocity);
                if (verticalVelocities.TryGetValue(agent, out var verticalVelocity))
                {
                    velocity.y = verticalVelocity;
                }
                agent.ApplyVelocity(velocity);
            }
        }

        public void Dispose()
        {
            agents.Clear();
            verticalVelocities.Clear();
            simulator?.OnDestroy();
            simulator = null;
            config = null;
        }

        private static Vector3 NoEndOfPath()
        {
            return new Vector3(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity);
        }

        private Vector3 KeepVelocityInsideArena(Vector3 position, Vector3 velocity)
        {
            if (config == null)
            {
                return velocity;
            }

            var flatPosition = new Vector2(position.x, position.z);
            var flatVelocity = new Vector2(velocity.x, velocity.z);
            var radius = Mathf.Max(0.01f, config.ArenaRadius - BoundaryPadding);
            if (flatPosition.magnitude < radius || flatVelocity.sqrMagnitude <= 0.0001f)
            {
                return velocity;
            }

            var outward = flatPosition.normalized;
            var outwardSpeed = Vector2.Dot(flatVelocity, outward);
            if (outwardSpeed <= 0f)
            {
                return velocity;
            }

            flatVelocity -= outward * outwardSpeed;
            velocity.x = flatVelocity.x;
            velocity.z = flatVelocity.y;
            return velocity;
        }
    }
}
