using UnityEngine;
using System.Collections;
using Pathfinding;
using Pathfinding.RVO;

/** Starcraft 2-like avoidance.
 * The main idea for this script is to
 * - Reduce the local avoidance priority for agents that have reached their destination once.
 * - Make agents stop if there is a high density of units around its destination.
 *
 * 'High density' is defined as:
 * Take the circle with the center at the AI's destination and a radius such that the AI's current position
 * is touching its border. Let 'A' be the area of that circle. Further let 'a' be the total area of all
 * individual agents inside that circle.
 * The agent should stop if a > A*0.6 or something like that. I.e if the agents inside the circle cover
 * over 60% of the surface of the circle. The 60% figure can be modified.
 */
public class SC2Avoidance : MonoBehaviour {
    IAstarAI ai;
    RVOController rvo;

    void Awake () {
        ai = GetComponent<IAstarAI>();
        rvo = GetComponent<RVOController>();
    }

    float timer1 = 0;
    Vector3 prevDestination;
    bool reachedCurrentDestination;

    bool ShouldStop () {
        var agents = rvo.simulator.GetAgents();
        var thisAgent = rvo.rvoAgent;
        var radius = (ai.destination - ai.position).magnitude;
        var accArea = 0f;

        foreach (var agent in agents) {
            var distToDestination = (rvo.To2D(ai.destination) - agent.Position).magnitude;
            if (distToDestination < radius + agent.radius * 0.75f) {
                accArea += agent.Radius*agent.Radius*Mathf.PI;
            }
        }

        bool result = accArea > radius*radius*Mathf.PI*0.60f;
        Pathfinding.Util.Draw.Debug.CircleXZ(ai.destination, radius, result ? Color.green : Color.red);
        return result;
    }

    void Update () {
        if (ai.destination != prevDestination) {
            timer1 = float.PositiveInfinity;
            reachedCurrentDestination = false;
        }
        prevDestination = ai.destination;

        if (ai.targetReached || ShouldStop()) {
            if (!ai.pathPending) {
                timer1 = 0f;
                reachedCurrentDestination = true;
            }
            ai.isStopped = true;
            rvo.priority = Mathf.Lerp(rvo.priority, 0.05f, Time.deltaTime * 2);
        } else {
            timer1 += Time.deltaTime;

            if (timer1 > 3) {
                ai.isStopped = false;
                if (reachedCurrentDestination) {
                    rvo.priority = Mathf.Lerp(rvo.priority, 0.1f, Time.deltaTime * 2);
                } else {
                    rvo.priority = Mathf.Lerp(rvo.priority, 0.5f, Time.deltaTime * 4);
                }
            }
        }
    }
}