NodeElement _start, _goal, closestPossible;
    PriorityQueue frontier;
    bool searching = false;
    Dictionary<NodeElement, NodeElement> camefrom = new Dictionary<NodeElement, NodeElement>();
    Dictionary<NodeElement, double> costsofar = new Dictionary<NodeElement, double>();
    public IEnumerator MultiThreadAStar(NodeElement goal)
    {
        if (searching == false)
        {
            //Debug.LogError("new search");
           // double tim = Time.time;
            searching = true;
            SquareGrid graph = PathfindingMap.Grid();
            int count = 0;
            frontier = new PriorityQueue();
            frontier.Enqueue(curNode, 0);
            _start = curNode;
            _goal = goal;
            camefrom[_start] = _start;
            costsofar[_start] = 0;
            closestPossible = _start;
            while (frontier.Count > 0)
            {
                NodeElement current = frontier.Dequeue();
                if (current.Equals(goal))
                {
                    break;
                }

                foreach (var next in graph.Neighbors(current))
                {
                    double newCost = costsofar[current] + graph.Cost(current, next);
                    // double newCost = current.costSofar + graph.Cost(current, next);
                    if (!costsofar.ContainsKey(next)
                      //  || newCost < next.costSofar)
                      || newCost < costsofar[next])
                    {
                        count++;
                        if (count > 250)
                        {
                            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);
                        //next.cameFrom = current;
                        //next.costSofar = newCost;
                        camefrom[next] = current;
                        costsofar[next] = newCost;
                        if (h2 > h)
                        {
                            closestPossible = next;
                        }
                    }
                }
            }
           // Debug.LogError("finished search in " + (Time.time - tim) + " seconds");
            FindShortestPath();
            searching = false;
        }
    }
    public void FindShortestPath()
    {
        path.Clear();
        NodeElement next = _goal, start;
        if (!camefrom.ContainsKey(next))
        //if (next.cameFrom == null)
        {
            next = closestPossible;
            start = closestPossible;
        }
        else
        {
            start = _goal;
        }
        path.Add(next);
        for (int i = 0; i < camefrom.Count; i++)
        //  for (int i = 0; i < start.costSofar; i++)
        {
            next = camefrom[next];
            // next = next.cameFrom;
            path.Add(next);
            if (next == _start)
            {
                path.Reverse();
                break;
            }
        }/*
        for (int i = 0; i < frontier.elementstoremove.Count; i++)
        {

            NodeElement node = frontier.elementstoremove[i];
            node.passed = false;
            node.cameFrom = null;
            node.costSofar = 0;
        }*/
    }