bool Astar::new_search(  SearchState& start,
                         SearchState& goal,
                         std::vector<SearchState>& solution){

    double weight=1.0;

    if(start==goal) //calls GridNode's (derived) implementation
        std::cout<<" ";

    //initialize costs
    //(...)
    PriorityQueue<SearchState, double> frontier;
    frontier.put(start, 0);

    came_from[start] = start;
    cost_so_far[start] = 0;

    size_t expandedStates=0;
    while (!frontier.empty()) {


      SearchState current = frontier.get();

      if (current == goal) {//calls SearchState's(base) implementation
        goal.setGCost(current.getGCost());
        break;
      }

      std::vector<SearchState> exploredStates = explorer->neighbors(current);
      for (SearchState next : exploredStates) {

        double new_cost = next.getGCost();
        if (cost_so_far.find(next) == cost_so_far.end()//if not in cost_so_far
            || new_cost < cost_so_far[next]) {
          cost_so_far[next] = new_cost;
          double priority = new_cost + weight*(next.getHCost());
          frontier.put(next, priority);
          came_from[next] = current;
        }
      }
      expandedStates++;
    }
    solution = reconstruct_path(start, goal, came_from);
    printSolutionStates(solution);
    std::cout<<"Expanded States: "<<expandedStates<<std::endl;
    std::cout<<"Number of States in Solutiion: "<<solution.size()<<std::endl;
    std::cout<<"Effeciency: "<<(double)solution.size()/expandedStates<<std::endl;
    return true;
}

template<typename T, typename priority_t>
struct PriorityQueue {
  typedef std::pair<priority_t, T> PQElement;
  std::priority_queue<PQElement, std::vector<PQElement>,
                 std::greater<PQElement>> elements;

  inline bool empty() const {
     return elements.empty();
  }

  inline void put(T item, priority_t priority) {
    elements.emplace(priority, item);
  }

  T get() {
    T best_item = elements.top().second;
    elements.pop();
    return best_item;
  }
};