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

public class AgentPath : MonoBehaviour {

    [SerializeField]
    private bool _ShowPath = false; // should display path when gizmos is on
    public bool ShowPath {
        get { return _ShowPath; }
        set { _ShowPath = value; }
    }

    [SerializeField]
    private bool _IsRandom = false; // should agent follow a random route around the points instead of linear path
    public bool IsRandom {
        get { return _IsRandom; }
        set { _IsRandom = value; }
    }

    [SerializeField]
    private bool _IsLooping = true; // Agent should loop round path
    public bool IsLooping
    {
        get { return _IsLooping; }
        set { _IsLooping = value; }
    }

    [SerializeField]
    private float _Radius = 2.0f; // distance at which to switch to next way point
    public float Radius
    {
        get { return _Radius; }
        set { _Radius = value; }
    }

    /// <summary>
    /// Used when IsLooping is set to false (not continuously looping around path) to indicate agent has reached the end of the path
    /// </summary>
    public bool IsFinished { get; private set; }

    [SerializeField]
    private List<Vector3> Waypoints = new List<Vector3>(); // the way points of the path route

    int currentIndex = 0; // index of current way point travelling towards
    int numWaypoints = 0; // the number of way points on the path

    /// <summary>
    /// Adds a new way point to the path's route
    /// </summary>
    /// <param name="wayPoint"></param>
    public void AddWayPoint(Vector3 wayPoint)
    {
        if (wayPoint != null)
        {
            Waypoints.Add(wayPoint);
            numWaypoints = Waypoints.Count;
        }
    }

    /// <summary>
    /// Removes the specified way point from the route
    /// </summary>
    /// <param name="wayPoint"></param>
    public void RemoveWayPoint(Vector3 wayPoint)
    {
        if (wayPoint != null && Waypoints != null)
        {
            Waypoints.Remove(wayPoint);
            numWaypoints = Waypoints.Count;
        }
    }

    /// <summary>
    /// Clears all way points
    /// </summary>
    public void ClearWaypoints()
    {
        Waypoints = new List<Vector3>();
    }

    // Use this for initialization
    void Start () {      
        numWaypoints = Waypoints.Count;

        if (numWaypoints > 0)
        {
            currentIndex = currentIndex + 1;
        }
	}
	
	// Update is called once per frame
	void Update () {
		
	}

    public bool IsAwayFromWaypoint (Vector3 position, float nearDistance)
    {
        // Get distance from position to current waypoint
        Vector3 distanceToWayPoint = (position - Waypoints[currentIndex]);

        // Calculate its square magnitude (square length of this vector)
        float sqrDistance = distanceToWayPoint.sqrMagnitude;

         Debug.Log("distanceToWayPoint: " + distanceToWayPoint + "   sqrDistance: " + sqrDistance + "   nearDistance: " + (nearDistance * nearDistance));

        /*
         * distanceToWayPoint: (-2.0, -1.8, 9.6)   sqrDistance: 99.9156   nearDistance: 100
         * distanceToWayPoint: (-2.0, -1.8, 9.6)   sqrDistance: 100.1136   nearDistance: 100
         * 
         */

        if (sqrDistance >= (nearDistance * nearDistance))
        {
            return true; // far away
        } else
        {
            return false; // close enough
        }
    }


    private void SetNextIndex()
    {
        // Check if using random path
        if (IsRandom)
        {
            currentIndex = GetRandomIndex();
        }
        else
        {
            if (currentIndex < numWaypoints - 1)
            {
                currentIndex++;
            }
            else if (IsLooping)
            {
                currentIndex = 0;
            }
            else
            {
                // Debug.Log("Finished!");
                IsFinished = true;
            }
        }
    }

    public Vector3 GetDirection(Vector3 position)
    {
        Vector3 direction = Vector3.zero;

        // Get distance to next way point

        // Get the current waypoint the agent should/is targeting
        Vector3 target = Waypoints[currentIndex];

        // Get distance from position to current waypoint
        Vector3 distanceToWaypoint = position - target;

        // Calculate its square magnitude (square length of this vector)
        float sqrDistance = (position - target).sqrMagnitude;

        // Check if distance is within the radius range to move to the next waypoint in the path
        // If it is, get the index of next waypoint to target
        if (sqrDistance < Radius * Radius)
        {
            SetNextIndex();
        }

        if (IsFinished) //currentIndex >= numWaypoints && !IsLooping
            return direction;

        // Get current position at same height of the current waypoint 
        Vector3 currentPosition = new Vector3(position.x, Waypoints[currentIndex].y, position.z);

        // Calculate the change in direction between the current waypoint and the current position
        direction = (Waypoints[currentIndex] - currentPosition).normalized;

        //Debug.Log(waypointDistance + "   " + Radius + "    " + position + "    "  + direction);
        return direction;
    }

    private int GetRandomIndex()
    {
        // Generate a random number. Random returns a float, you need to convert to integer as indices can only be ints.
        int randomIndex = Mathf.FloorToInt(Random.value * numWaypoints);

        // If number of waypoints is greater than one, make sure random number generated is not the same as the last one or agent will be stuck at same spot
        // Keep trying till you get a different one.
        if (numWaypoints > 1)
        {
            // check that it has not just generated the same index as the last one
            // if yes, continue trying for a different random index
            while (currentIndex == randomIndex)
            {
                randomIndex = Mathf.FloorToInt(Random.value * numWaypoints);
            }
        }
        return randomIndex;
    }

    public Vector3 GetDirectionToTarget(Vector3 position, Vector3 target)
    {
        Vector3 currentPosition = new Vector3(position.x, Waypoints[currentIndex].y, position.z);
        Vector3 targetPosition = new Vector3(target.x, Waypoints[currentIndex].y, target.z);
        Vector3 direction = (targetPosition - currentPosition).normalized;
        //Debug.Log("Direction to target: " + direction + "   " + (targetPosition - currentPosition));
        return direction;
    }

    public void Restart()
    {
        currentIndex = 0;
        IsFinished = false;
    }

    void OnDrawGizmos()
    {
        if (!ShowPath) return;
        
        
        for (int i = 0; i < numWaypoints; i++)
        {
            if (i + 1 < numWaypoints)
            {
                Debug.DrawLine(Waypoints[i], Waypoints[i + 1], Color.red);
            }
        }
    }
}
