diff --git a/nav/a-star/AStar.h b/nav/a-star/AStar.h
index 486baf3..edf4ec0 100644
--- a/nav/a-star/AStar.h
+++ b/nav/a-star/AStar.h
@@ -16,41 +16,47 @@ template <typename T> class AStar {
 
 public:
 
-
+#define LE_MAX 500000
 
 	//dijkstra with priority queue O(E log V)
-	template <typename Access> static std::vector<const T*> get(const T* source, const T* destination, Access acc) {
+    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;
+		//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;
+		//std::unordered_map<const T*, const T*> parent;
+		const T* parent[LE_MAX];
 
 		// all nodes
-		const std::vector<T>& nodes = acc.getAllNodes();
+        //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] = std::numeric_limits<float>::max();
-		}
+//		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] = 0.0f;
-		Q.push(std::make_pair(source,distance[source]));
-
-                int iter = 0;
-                //std::cout << (std::string)*source << std::endl;
-                //std::cout << (std::string)*destination << std::endl;
+		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;
+//            ++iter;
 
 			// fetch the next-nearest node from the queue
 			const T* u = Q.top().first;
@@ -69,35 +75,35 @@ public:
 				const float w = acc.getWeightBetween(v, *u);
 
 				// found a better route?
-				if (distance[&v] > distance[u] + w) {
-					distance[&v] = distance[u] + w;
-					parent[&v] = u;
-                    Q.push(std::make_pair(&v, distance[&v] + acc.getHeuristic(v, *destination)));
+				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;
 
+//        std::cout << iter << std::endl;
 
-		// construct the path
-		std::vector<const T*> path;
-		const T* p = destination;
-		path.push_back(destination);
+//		// 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];
-				path.push_back(p);
-			} else {
-				return std::vector<const T*>(); //if no path could be found, just return an empty vector.
-			}
-		}
+//		// 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 path;
+        return distance[destination->getIdx()];
 
 	}