using UnityEngine;
using System.Collections;
using System.Collections.Generic;
public class Seeker : MonoBehaviour
{
    public List<NodeElement> path = new List<NodeElement>();
    public NodeElement curNode;
    public NodeElement LastDestination;
    Transform tr;
    public NodeElement LastPathElement()
    {
        return path[path.Count - 1];
    }

    protected virtual void Start()
    {
        tr = transform;
        curNode = PathfindingMap.VecToNode(Mathf.RoundToInt(tr.position.x), Mathf.RoundToInt(tr.position.z));
        path.Add(curNode);
        LastDestination = curNode;
        curNode.occupied = this;
    }
    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 && searching == false;
    }
    protected virtual void RecalculatePath() { }
    protected virtual void LateUpdate()
    {
        Rotate();
        if (path.Count > 1)
        {
            Vector3 pos = path[1].pos();
            transform.position = Vector3.Lerp(transform.position, pos, Time.deltaTime * Speed);
            if (Vector3.Distance(transform.position, pos) < 0.4f)
            {
                path[0].occupied = null;
                path.RemoveAt(0);
                path[0].occupied = this;
                curNode = path[0];
            }
            anim.SetFloat("Speed", 1);
        }
        else
        {
            if (path.Count == 0)
            {
                path.Add(curNode);
            }
            path[0].occupied = this;
            anim.SetFloat("Speed", 0);
        }

    }

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

        int x = Mathf.RoundToInt(_pos.x);
        int y = Mathf.RoundToInt(_pos.z);
        SetDestination(PathfindingMap.VecToNode(x, y), recalculating);
    }
    List<NodeElement> _path = new List<NodeElement>();
    public void SetDestination(NodeElement goal, bool recalculating = false)
    {
        if (goal != null)
            if (SquareGrid.InBounds(goal))
            {
                if (goal != curNode && (goal != LastDestination || recalculating == true) && searching == false)
                {
                    LastDestination = goal;
                    goal = FindNearestReachable(goal,false);
                    StartCoroutine(MultiThreadAStar(goal));
                }
            }
    }
    protected List<NodeElement> searchedNodes = new List<NodeElement>();
    public NodeElement FindNearestReachable(NodeElement nod, bool recalculating)
    {
        searchedNodes = new List<NodeElement>();
        if (recalculating==true?nod.Passable():nod.Passable(this))
        {
            return nod;
        }
        else
        {
            searchedNodes.Add(nod);
            NodeElement cur = nod;
            int i = 0;
            do
            {
                for (int q = 0; q < SquareGrid.DIRS.Length; q++)
                {
                    NodeElement node = PathfindingMap.VecToNode(cur.x + SquareGrid.DIRS[q].x, cur.y + SquareGrid.DIRS[q].y);
                    if (!searchedNodes.Contains(node))
                    {
                        i++;
                        searchedNodes.Add(node);
                        if (node.Passable(this))
                        {
                            return node;
                        }
                    }
                }
                cur = searchedNodes[i];
            }
            while (cur != null);
            return null;
        }
    }
    static public double Heuristic(NodeElement a, NodeElement b)
    {
        return Mathf.Abs(a.x - b.x) + Mathf.Abs(a.y - b.y);
    }
    public NodeElement _start, _goal, closestPossible;
    PriorityQueue frontier;
    protected bool searching = false;
    public static int MaxNodesPerSearch = 250;
    public Dictionary<NodeElement, NodeElement> camefrom = new Dictionary<NodeElement, NodeElement>();
    public Dictionary<NodeElement, double> costsofar = new Dictionary<NodeElement, double>();
    protected bool unreachableTarget = false;
    static int count = 0;
    public IEnumerator MultiThreadAStar(NodeElement goal)
    {
        searching = true;
        SquareGrid graph = PathfindingMap.grid();
        frontier = new PriorityQueue();
        frontier.Enqueue(curNode, 0);
        _start = curNode;
        _goal = goal;
        camefrom.Clear();
        costsofar.Clear();
        camefrom[_start] = _start;
        costsofar[_start] = 0;
        closestPossible = _start;
        count = 0;
        while (frontier.Count > 0)
        {
            NodeElement current = frontier.Dequeue();
            if (current.Equals(goal))
            {
                break;
            }
            Debug.DrawLine(current.pos(), current.pos() + Vector3.up * 3, Color.blue, 1);
            foreach (var next in SquareGrid.Neighbors(current, this))
            {
                double newCost = costsofar[current] + graph.Cost(current, next);
                if (!costsofar.ContainsKey(next)
                  || newCost < costsofar[next])
                {
                    count++;
                    if (count > MaxNodesPerSearch)
                    {
                        yield return null;
                        count = 0;
                    }
                    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;
                    if (h2 > h)
                    {
                        closestPossible = next;
                    }
                }
            }
        }
        FindShortestPath();
        searching = false;

    }
    public void FindShortestPath()
    {
        path.Clear();
        NodeElement next = _goal;
        if (!camefrom.ContainsKey(next))
        {
            next = closestPossible;
        }
        path.Add(next);
        for (int i = 0; i < camefrom.Count; i++)
        {
            next = camefrom[next];
            path.Add(next);
            if (next == _start)
            {
                path.Reverse();
                break;
            }
        }
    }
}