using UnityEngine;
#if !(UNITY_5_1 || UNITY_5_2 || UNITY_5_3 || UNITY_5_4)
using UnityEngine.AI;
#endif
using BehaviorDesigner.Runtime;
using BehaviorDesigner.Runtime.Tasks;
using BehaviorDesigner.Runtime.Tasks.Unity.Timeline;

namespace Valheim
{
    public class PlayerFollow : NavMeshGroupMovement2
    {
        public SharedFloat neighborDistance = 10;
        public SharedFloat leaderBehindDistance = 2;
        public SharedFloat separationDistance = 2;
        public SharedFloat aheadDistance = 2;
        public SharedFloat moveDistance = 2;
        public SharedGameObject leader = null;
        public SharedBool hasInit = false;

        // component cache
        private Transform leaderTransform;
        // private NavMeshAgent leaderAgent;
        private Rigidbody leaderRigidbody;

        private Vector3[] lastTargetPositions;
        private bool[] hasMoveds;
        
        private Vector3[] aroundOffsets;
        public SharedFloat followRadius = 2f; // 默认离玩家 4 米，你可以在 Inspector 里调节

        // The agents will always be following the leader so always return running
        public override TaskStatus OnUpdate()
        {
            // 初始化
            if (!hasInit.Value && leader.Value != null && agents.Value.Count > 0)
            {
                hasInit.Value = true;
                //leaderTransform = leader.Value.GetComponent<Player>().leaderPoint;
                leaderTransform = leader.Value.transform;
                // leaderAgent = leader.Value.GetComponent<NavMeshAgent>();
                leaderRigidbody = leader.Value.GetComponent<Rigidbody>();
                hasMoveds = new bool[agents.Value.Count];
                lastTargetPositions = new Vector3[agents.Value.Count];
                navMeshAgents = new NavMeshAgent[agents.Value.Count];
                transforms = new Transform[agents.Value.Count];

                for (int i = 0; i < agents.Value.Count; i++)
                {
                    hasMoveds[i] = false;
                    lastTargetPositions[i] = LeaderBehindPosition() + Vector3.one * (moveDistance.Value + 1);
                    transforms[i] = agents.Value[i].transform;
                    navMeshAgents[i] = agents.Value[i].GetComponent<NavMeshAgent>();
                    navMeshAgents[i].speed = agents.Value[i].GetComponent<Pet>().speed;
                    navMeshAgents[i].angularSpeed = angularSpeed.Value;
                    navMeshAgents[i].isStopped = false;
                }
                
                aroundOffsets = new Vector3[agents.Value.Count];
                float radius = followRadius.Value;
                for (int i = 0; i < agents.Value.Count; i++)
                {
                    float angle = 2 * Mathf.PI * i / agents.Value.Count;
                    aroundOffsets[i] = new Vector3(
                        Mathf.Cos(angle) * radius,
                        0,
                        Mathf.Sin(angle) * radius
                    );
                }
            }

            // 有leader与跟随者才进行更新
            if (leader.Value != null && agents.Value.Count > 0)
            {   
                Vector3 behindPosition = LeaderBehindPosition();
                if (leader.Value == null || agents.Value.Count == 0) return TaskStatus.Running;
                // 1. 每帧重新取玩家中心点
                Vector3 center = leader.Value.transform.position;
                // Debug.Log("x-" + behindPosition.x + " y-" + behindPosition.y + " z-" + behindPosition.z);

                // Determine a destination for each agent
                for (int i = 0; i < agents.Value.Count; ++i)
                {
                    var pet = agents.Value[i].GetComponent<Pet>();
                    if (pet != null && (pet.state == PetState.Terminal || pet.order == OrderType.Attack|| pet.state== PetState.CurWin))
                        continue;

                    // 基础环绕位置（始终保持在 followRadius 半径处）
                    Vector3 targetPosition = center + aroundOffsets[i];

                    //（可选）再加一点分离，避免宠物互相重叠
                    targetPosition += DetermineSeparation(i) * 0.5f; 

                    // 发起移动
                    if ((targetPosition - lastTargetPositions[i]).sqrMagnitude
                        >= moveDistance.Value * moveDistance.Value)
                    {
                        SetDestination(i, targetPosition);
                        lastTargetPositions[i] = targetPosition;
                        hasMoveds[i] = true;
                    }
                    else if (hasMoveds[i] &&
                             (targetPosition - transforms[i].position).magnitude < moveDistance.Value)
                    {
                        navMeshAgents[i].isStopped = true;
                        hasMoveds[i] = false;
                        lastTargetPositions[i] = targetPosition;
                    }
                }
            }
            return TaskStatus.Running;
        }

        // 获得leader后方的一个位置，这个根据leader的移动速度来计算
        private Vector3 LeaderBehindPosition()
        {
            // The behind position is the normalized inverse of the leader's velocity multiplied by the leaderBehindDistance
            // return leaderTransform.position + (-leaderAgent.velocity).normalized * leaderBehindDistance.Value + (-leaderTransform.forward) * leaderBehindDistance.Value;
            //return leaderTransform.position + (-leaderRigidbody.velocity).normalized * leaderBehindDistance.Value;
            return leaderTransform.position;
        }

        // Determine the separation between the current agent and all of the other agents also following the leader
        private Vector3 DetermineSeparation(int agentIndex)
        {
            var separation = Vector3.zero;
            int neighborCount = 0;
            var agentTransform = transforms[agentIndex];
            // Loop through each agent to determine the separation
            for (int i = 0; i < agents.Value.Count; ++i)
            {
                // The agent can't compare against itself
                if (agentIndex != i)
                {
                    // Only determine the parameters if the other agent is its neighbor
                    if (Vector3.SqrMagnitude(transforms[i].position - agentTransform.position) < neighborDistance.Value)
                    {
                        // This agent is the neighbor of the original agent so add the separation
                        separation += transforms[i].position - agentTransform.position;
                        neighborCount++;
                    }
                }
            }

            // Don't move if there are no neighbors
            if (neighborCount == 0)
            {
                return Vector3.zero;
            }

            // Normalize the value
            return ((separation / neighborCount) * -1).normalized * separationDistance.Value;
        }

        // Use the dot product to determine if the leader is looking at the current agent
        public bool LeaderLookingAtAgent(int agentIndex)
        {
            return Vector3.Dot(leaderTransform.forward, transforms[agentIndex].forward) < -0.5f;
        }

        // Reset the public variables
        public override void OnReset()
        {
            base.OnReset();

            neighborDistance = 10;
            leaderBehindDistance = 2;
            separationDistance = 2;
            aheadDistance = 2;
            leader = null;
        }
    }
}