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

public class Seeker : Actor
{
    //is the pathfinding thread running
    bool searchingPath = false;
    //the path
    public List<NodeElement> path = new List<NodeElement>();
    //the node we're currently standing on
    public NodeElement curNode;
    //the last node we were trying to reach
    public NodeElement LastDestination;
    public NodeElement LastPathElement()
    {
        return path[path.Count - 1];
    }

    public Animator anim;
    public float Speed = 4;

    public Quaternion DesiredRotation;
    protected virtual void Rotate()
    {
        if (path.Count > 1)
        {
            DesiredRotation = Quaternion.LookRotation(path[1].pos() - curNode.pos());
            transform.rotation = Quaternion.Slerp(transform.rotation, DesiredRotation, Time.deltaTime * 4);

        }
    }
    public bool StoppedMoving()
    {
        return path.Count <= 1;
    }
    float recalculateCooldown = 0;
    //gets called every frame
    protected void RecalculatePath()
    {
        //recalculate path if someone is infront of us
        if (path.Count > 1)
        {
            if (path[1].occupied != null && recalculateCooldown > 1)
            {
                recalculateCooldown = 0;
                SetDestination(LastDestination, true);
            }
            else
            {
                recalculateCooldown += Time.deltaTime;
            }
        }
        //or recalculate the path if we somehow ended up standing on someone else's node, no clue how to avoid having to do this
        else
        {
            if (curNode.occupied != this && recalculateCooldown > 1)
            {
                recalculateCooldown = 0;
                SetDestination(curNode, true);
            }
            else
            {
                recalculateCooldown += Time.deltaTime;
            }
        }
        //redo the path if it was initially too short
        if (recalculating)
        {
            recalculateTimer += Time.deltaTime;
            if (recalculateTimer > 0.5f)
            {
                recalculateTimer = 0;
                recalculating = false;
                SetDestination(LastDestination, true);
            }
        }
    }
    public override void LateUpdate()
    {
        base.LateUpdate();
        //draw the path in the editor
        if (path.Count > 1)
        {
            for (int i = 0; i < path.Count - 1; i++)
            {
                Debug.DrawLine(path[i].pos(), path[i + 1].pos(), Color.blue);
            }
        }
        //check for path reculculation needs
        RecalculatePath();

        // Rotate();
        //if we're not currently remaking the path,  we can use it
        if (!searchingPath)
        {
            //if it's more than the current node, move to the next one
            if (path.Count > 1)
            {
                Vector3 pos = path[1].pos();
                transform.position = Vector3.Slerp(transform.position, pos, 0.04f * Speed);
                if (Vector3.Distance(transform.position, pos) < 0.1f)
                {
                    path[0].occupied = null;
                    path.RemoveAt(0);
                    path[0].occupied = this;
                    curNode = path[0];
                }
                //  anim.SetFloat("Speed", 1);
            }
        }
    }

    public void SetDestination(Vector3 _pos, bool recalculatingp = false)
    {

        int x = Mathf.RoundToInt(_pos.x);
        int y = Mathf.RoundToInt(_pos.z);
        SetDestination(PathfindingMap.VecToNode(x, y), recalculatingp);
    }
    Thread PathFinder;
    public void SetDestination(NodeElement goal, bool recalculatingp = false)
    {
        if (goal != null)
            if (SquareGrid.InBounds(goal))
            {
                //if we're not doing the same pathfinding check as before, or if we're recalculating it
                if ((goal != curNode && goal != LastDestination) || recalculatingp == true)
                {
                    LastDestination = goal;
                    goal = SquareGrid.FindNearestReachable(this, goal, recalculatingp);
                    searchingPath = true;
                    if (PathFinder != null)
                        PathFinder.Abort();
                    PathFinder = new Thread(() => MultiThreadAStar(goal));
                    PathFinder.Start();
                }
            }
    }

    static public double Heuristic(NodeElement a, NodeElement b)
    {
        float x = Mathf.Abs(a.x - b.x);
        float y = Mathf.Abs(a.y - b.y);
        //add 0.1f to make diagonal paths less likely
        return x + y + (x == y ? 0.1f : 0);
    }
    public NodeElement _start, _goal, closestPossible;
    PriorityQueue frontier;
    //which node came from which node
    public Dictionary<NodeElement, NodeElement> camefrom = new Dictionary<NodeElement, NodeElement>();
    //how much it cost to reach the given node starting from the current node
    public Dictionary<NodeElement, double> costsofar = new Dictionary<NodeElement, double>();
    protected bool unreachableTarget = false;
    //gets called as a new thread
    public void MultiThreadAStar(NodeElement goal)
    {
        path.Clear();
        //reference to the map
        SquareGrid graph = PathfindingMap.grid();
        //frontier/priority que
        frontier = new PriorityQueue();
        //add the current node as the start of the frontier
        frontier.Enqueue(curNode, 0);
        _start = curNode;
        _goal = goal;
        camefrom.Clear();
        costsofar.Clear();
        //first node came from itself, cost 0
        camefrom[_start] = _start;
        costsofar[_start] = 0;
        closestPossible = _start;
        //do this until the frontier becomes empty (or we reach the goal)
        while (frontier.Count > 0)
        {
            //current node is the one closest to the goal
            NodeElement current = frontier.Dequeue();
            if (current.Equals(goal))
            {
                break;
            }
            //regular A* neighbors, in other words everyone surrounding the current node
            foreach (var next in SquareGrid.Neighbors(current, this))
            {
                //ignore it if it's occupied by another unit
                if (next.occupied == null)
                {
                    //add it to the optimal paths if it isn't in the frontier, or it's cost is lower than the current one
                    double newCost = costsofar[current] + graph.Cost(current, next);
                    if (!costsofar.ContainsKey(next)
                      || newCost < costsofar[next])
                    {
                        double h = Heuristic(next, goal);
                        double h2 = Heuristic(closestPossible, goal);
                        double priority = newCost + Heuristic(next, goal);
                        frontier.Enqueue(next, priority);
                        camefrom[next] = current;
                        costsofar[next] = newCost;
                        //save the closest node to the goal, in case we can't reach the goal
                        if (h2 > h)
                        {
                            closestPossible = next;
                        }
                    }
                }
            }
        }
        //reconstruct the path
        NodeElement _next = _goal;
        //end of the path is the closest node to the goal
        if (!camefrom.ContainsKey(_next))
        {
            _next = closestPossible;
        }
        if (camefrom[_next] == _start)
        {
            path.Add(_next);
        }
        for (int i = 0; i < camefrom.Count; i++)
        {
            _next = camefrom[_next];
            path.Add(_next);
            if (_next == _start)
            {
                path.Reverse();
                break;
            }
        }
        //if the path is too short, the unit was probably surrounded and couldn't move, restart the path in a second or so
        if (path.Count <= 2 && Vector3.Distance(curNode.pos(), LastDestination.pos()) > 2.5f)
        {
            recalculating = true;
            recalculateTimer = 0;
        }
        searchingPath = false;
    }
    bool recalculating = false;
    float recalculateTimer = 0;
}