//#define ASTARDEBUG
using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using Pathfinding;
using Pathfinding.RVO;

/** AI for 2D top down games.
 * This is one of the default movement scripts which comes with the A* Pathfinding Project.
 * It is in no way required by the rest of the system, so feel free to write your own. But I hope this script will make it easier
 * to set up movement for the characters in your game.
 * \n
 * This script will try to follow a target transform, and at regular intervals the path to that target will be recalculated.
 *
 * \section variables Quick overview of the variables
 * In the inspector in Unity, you will see a bunch of variables. You can view detailed information further down, but here's a quick overview.\n
 * The #repathRate determines how often it will search for new paths, if you have fast moving targets, you might want to set it to a lower value.\n
 * The #target variable is where the AI will try to move, it can be a point on the ground where the player has clicked in an RTS for example.
 * Or it can be the player object in a zombie game.\n
 * The speed is self-explanatory, so is turningSpeed, however #slowdownDistance might require some explanation.
 * It is the approximate distance from the target where the AI will start to slow down. Note that this doesn't only affect the end point of the path
 * but also any intermediate points, so be sure to set #forwardLook and #pickNextWaypointDist to a higher value than this.\n
 * #pickNextWaypointDist is simply determines within what range it will switch to target the next waypoint in the path.\n
 * #forwardLook will try to calculate an interpolated target point on the current segment in the path so that it has a distance of #forwardLook from the AI\n
 * Below is an image illustrating several variables as well as some internal ones, but which are relevant for understanding how it works.
 * Note that the #forwardLook range will not match up exactly with the target point practically, even though that's the goal.
 * \shadowimage{aipath_variables.png}
 * This script can use a Rigidbody2D for movement or it can simply use transform.position.
 * If it finds a Rigidbody2D attached to the same GameObject, it will start to use it automatically.
 *
 * \ingroup movementscripts
 */
[RequireComponent(typeof(Seeker))]
[AddComponentMenu("Pathfinding/AI/AIPath (2D top down)")]
public class AIPath2DTopDown : AIPathBase {

    /** Cached Rigidbody component */
    protected Rigidbody2D rigid;

    /** Initializes reference variables.
     * If you override this function you should in most cases call base.Awake () at the start of it.
      * */
    protected override void Awake () {
        base.Awake ();

        //Cache some other components (not all are necessarily there)
        rigid = GetComponent<Rigidbody2D>();
    }

    public override Vector3 GetFeetPosition () {
        return tr.position;
    }

    public virtual void Update () {

        if (!canMove) { return; }

        Vector2 dir = CalculateVelocity (GetFeetPosition());

        //Rotate towards targetDirection (filled in by CalculateVelocity)
        RotateTowards (targetDirection);

        if (rigid != null) {
            //rigid.AddForce (dir, ForceMode2D.Impulse);
            rigid.MovePosition (rigid.position + dir*Time.deltaTime);
        } else {
            transform.Translate (dir*Time.deltaTime, Space.World);
        }
    }

    /** Point to where the AI is heading.
      * Filled in by #CalculateVelocity */
    protected Vector2 targetPoint;
    /** Relative direction to where the AI is heading.
     * Filled in by #CalculateVelocity */
    protected Vector2 targetDirection;

    /** Calculates desired velocity.
     * Finds the target path segment and returns the forward direction, scaled with speed.
     * A whole bunch of restrictions on the velocity is applied to make sure it doesn't overshoot, does not look too far ahead,
     * and slows down when close to the target.
     * /see speed
     * /see endReachedDistance
     * /see slowdownDistance
     * /see CalculateTargetPoint
     * /see targetPoint
     * /see targetDirection
     * /see currentWaypointIndex
     */
    protected Vector2 CalculateVelocity (Vector2 currentPosition) {
        if (path == null || path.vectorPath == null || path.vectorPath.Count == 0) return Vector3.zero;

        List<Vector3> vPath = path.vectorPath;

        // Just for the rest of the code to work, if there is only one waypoint in the path
        // add another one
        if (vPath.Count == 1) {
            vPath.Insert (0,currentPosition);
        }

        // Make sure we stay inside valid ranges
        currentWaypointIndex = Mathf.Clamp (currentWaypointIndex, 1, vPath.Count - 1);

        // Possibly pick the next segment
        while (true) {
            if (currentWaypointIndex < vPath.Count-1) {
                //There is a "next path segment"
                Vector2 nextWaypointDir = (Vector2)vPath[currentWaypointIndex] - currentPosition;
                float dist = nextWaypointDir.sqrMagnitude;
                    //Mathfx.DistancePointSegmentStrict (vPath[currentWaypointIndex+1],vPath[currentWaypointIndex+2],currentPosition);
                if (dist < pickNextWaypointDist*pickNextWaypointDist) {
                    lastFoundWaypointPosition = currentPosition;
                    lastFoundWaypointTime = Time.time;
                    currentWaypointIndex++;
                } else {
                    break;
                }
            } else {
                break;
            }
        }

        // Calculate the point we should move towards
        Vector2 targetPosition = CalculateTargetPoint (currentPosition,vPath[currentWaypointIndex-1] , vPath[currentWaypointIndex]);

        // Vector to the target position
        Vector2 dir = targetPosition-currentPosition;
        float targetDist = dir.magnitude;

        float slowdown = Mathf.Clamp01 (targetDist / slowdownDistance);

        this.targetDirection = dir;
        this.targetPoint = targetPosition;

        if (currentWaypointIndex == vPath.Count-1 && targetDist <= endReachedDistance) {
            if (!targetReached) { targetReached = true; OnTargetReached (); }

            //Send a move request, this ensures gravity is applied
            return Vector3.zero;
        }

        // The Y axis is forward in 2D space
        Vector2 forward = -tr.up;
        float dot = Vector2.Dot (dir.normalized,forward);
        float sp = speed * Mathf.Max (dot,minMoveScale) * slowdown;

#if ASTARDEBUG
        Debug.DrawLine (vPath[currentWaypointIndex-1] , vPath[currentWaypointIndex],Color.black);
        Debug.DrawLine (GetFeetPosition(),targetPosition,Color.red);
        Debug.DrawRay (targetPosition,Vector3.up, Color.red);
        Debug.DrawRay (GetFeetPosition(),dir,Color.yellow);
        Debug.DrawRay (GetFeetPosition(),forward*sp,Color.cyan);
#endif

        // Make sure we don't overshoot the target
        // when the framerate is low
        if (Time.deltaTime  > 0) {
            sp = Mathf.Clamp (sp,0,targetDist/(Time.deltaTime*2));
        }
        return forward*sp;
    }

    /** Rotates in the specified direction.
     * Rotates around the Y-axis.
     * \see turningSpeed
     */
    protected void RotateTowards (Vector2 dir) {

        if (dir == Vector2.zero) return;

        // Figure out the angle so that the Y axis faces dir
        float angle = Mathf.Atan2 (dir.x, -dir.y)*Mathf.Rad2Deg;

        Vector3 rot = tr.eulerAngles;
        rot.z = Mathf.LerpAngle (rot.z, angle, turningSpeed * Time.deltaTime);
        tr.eulerAngles = rot;
    }

    /** Calculates target point from the current line segment.
     * \param p Current position
     * \param a Line segment start
     * \param b Line segment end
     * The returned point will lie somewhere on the line segment.
     * \see #forwardLook
     * \todo This function uses .magnitude quite a lot, can it be optimized?
     */
    protected Vector2 CalculateTargetPoint (Vector2 p, Vector2 a, Vector2 b) {

        float magn = (a-b).magnitude;
        if (magn == 0) return a;

        float closest = AstarMath.Clamp01 (AstarMath.NearestPointFactor (a, b, p));
        Vector2 point = (b-a)*closest + a;
        float distance = (point-p).magnitude;

        float lookAhead = Mathf.Clamp (forwardLook - distance, 0.0F, forwardLook);

        float offset = lookAhead / magn;
        offset = Mathf.Clamp (offset+closest,0.0F,1.0F);
        return (b-a)*offset + a;
    }
}