#ifndef ASTAR_H
#define ASTAR_H

#include <iostream>
#include <vector>
#include <algorithm>
#include <limits>
#include <queue>
#include <unordered_map>
#include <cassert>

#include "../../grid/Grid.h"


template <typename T> class AStar {

public:

#define LE_MAX 500000

	//dijkstra with priority queue O(E log V)
    template <typename Access>
    static float get(const T* source, const T* destination, Access acc) {

		// track distances from the source to each other node
		//std::unordered_map<const T*, float> distance;

		float distance[LE_MAX];


		// track the previous node for each node along the path
		//std::unordered_map<const T*, const T*> parent;
		const T* parent[LE_MAX];

		// all nodes
        //const std::vector<T>& nodes = acc.getAllNodes();

		// priority queue to check which node is to-be-processed next
		std::priority_queue<std::pair<T*,float>, std::vector<std::pair<const T*,float>>, Comparator2> Q;

		// start with infinite distance
//		for(const auto& node : nodes){
//			distance[node.getIdx()] = std::numeric_limits<float>::max();
//		}
        std::fill_n(distance, LE_MAX, std::numeric_limits<float>::max());


//        std::cout << (std::string)*source << std::endl;
//        std::cout << (std::string)*destination << std::endl;
//        int iter = 0;

		// start at the source
		distance[source->getIdx()] = 0.0f;
		Q.push(std::make_pair(source,distance[source->getIdx()]));

		// proceed until there are now new nodes to follow
		while(!Q.empty()) {

//            ++iter;

			// fetch the next-nearest node from the queue
			const T* u = Q.top().first;

			// and check whether we reached the destination
			if (u == destination) {break;}

			// remove from the Queue
			Q.pop();

			// process all neighbors for the current element
			for( const T& v : acc.getNeighbors(*u)) {

				// weight (distance) between the current node and its neighbor
				//const float w = ((Point3)v - (Point3)*u).length();
				const float w = acc.getWeightBetween(v, *u);

				// found a better route?
				if (distance[v.getIdx()] > distance[u->getIdx()] + w) {
					distance[v.getIdx()] = distance[u->getIdx()] + w;
					parent[v.getIdx()] = u;
					Q.push(std::make_pair(&v, distance[v.getIdx()] + acc.getHeuristic(v, *destination)));		// SOURCE OR DEST?!
				}

			}
		}


//        std::cout << iter << std::endl;

//		// construct the path
//		std::vector<const T*> path;
//		const T* p = destination;
//		path.push_back(destination);

//		// until we reached the source-node
//		while (p!=source) {
//			if (p) {
//				p = parent[p->getIdx()];
//				path.push_back(p);
//			} else {
//				return std::vector<const T*>(); //if no path could be found, just return an empty vector.
//			}
//		}

		// done
        return distance[destination->getIdx()];

	}


//	template <typename Access> static std::vector<const T*> getShortestPathAStar(const T* src, const T* dst, Access acc){

//		std::vector<const T*> shortestPath;

//		//here we could do some preprocessing. e.g. area of interest of nodes


//		// call aStar
//		shortestPath = aStar(src, dst, acc);

//		return shortestPath;
//	}

	class Comparator2 {
	public:
		int operator() ( const std::pair<const T*,float>& p1, const std::pair<const T*,float>& p2){
			return p1.second > p2.second;
		}
	};

};

#endif // ASTAR_H