using System;
using System.Collections.Generic;
using UnityEngine;

namespace FishROV.AvoidanceBenchmark
{
    public sealed class Rvo2Adapter : IAvoidanceAdapter
    {
        private const float TimeHorizon = 2.5f;
        private const float TimeHorizonObstacles = 2.5f;
        private const float NeighbourDistanceMultiplier = 8f;

        private readonly Dictionary<BenchmarkAgent, int> agentIds = new Dictionary<BenchmarkAgent, int>(512);
        private AvoidanceBenchmarkConfig config;
        private RVO.Simulator simulator;

        public string Name => "RVO2 本地避障";
        public bool IsAvailable => simulator != null;
        public string Status => config != null && config.DimensionMode == AvoidanceDimensionMode.XYZ3D
            ? $"RVO2-CS 已直接接入，当前 {agentIds.Count} 个对象；RVO2-CS 是 2D 算法，3D 压测会投影到 XZ 平面。"
            : $"RVO2-CS 已直接接入，当前 {agentIds.Count} 个对象。";

        public void Initialize(AvoidanceBenchmarkConfig config)
        {
            this.config = config.Clone();
            agentIds.Clear();
            simulator = RVO.Simulator.Instance;
            simulator.Clear();
            simulator.NumWorkers = Math.Max(1, Environment.ProcessorCount - 1);
            simulator.TimeStep = Time.fixedDeltaTime;
        }

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

            var neighbourDistance = Mathf.Max(spawnInfo.Radius * NeighbourDistanceMultiplier, spawnInfo.Radius * 2f);
            var id = simulator.AddAgent(
                ToRvoVector(spawnInfo.Position),
                neighbourDistance,
                Math.Max(1, config.MaxNeighbours),
                TimeHorizon,
                TimeHorizonObstacles,
                Mathf.Max(0.01f, spawnInfo.Radius),
                Mathf.Max(0.01f, spawnInfo.MaxSpeed),
                new RVO.Vector2(0f, 0f));

            if (id >= 0)
            {
                agentIds[agent] = id;
            }

            return agent;
        }

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

            simulator.SetAgentPosition(id, ToRvoVector(agent.transform.position));
            simulator.SetAgentPrefVelocity(id, ToRvoVector(Vector3.ClampMagnitude(preferredVelocity, agent.MaxSpeed)));
        }

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

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

            simulator.TimeStep = Mathf.Max(0.001f, deltaTime);
            simulator.DoStep();

            foreach (var pair in agentIds)
            {
                pair.Key.ApplyVelocity(FromRvoVector(simulator.GetAgentVelocity(pair.Value), pair.Key.MaxSpeed));
            }
        }

        public void Dispose()
        {
            agentIds.Clear();
            simulator?.Clear();
            simulator = null;
        }

        private static RVO.Vector2 ToRvoVector(Vector3 value)
        {
            return new RVO.Vector2(value.x, value.z);
        }

        private static Vector3 FromRvoVector(RVO.Vector2 value, float maxSpeed)
        {
            return Vector3.ClampMagnitude(new Vector3(value.X, 0f, value.Y), maxSpeed);
        }
    }
}
