diff --git a/geo/Heading.h b/geo/Heading.h
index 73acb43..172e124 100644
--- a/geo/Heading.h
+++ b/geo/Heading.h
@@ -46,6 +46,11 @@ public:
 		return (Heading(*this) += _rad);
 	}
 
+        Heading& operator = (const float _rad) {
+            rad = _rad;
+            return *this;
+        }
+
 	/** compare two headings */
 	bool operator == (const Heading other) const {return rad == other.rad;}
 	bool operator != (const Heading other) const {return rad != other.rad;}
diff --git a/grid/walk/GridWalkPathControl.h b/grid/walk/GridWalkPathControl.h
index bf5bb21..4980780 100644
--- a/grid/walk/GridWalkPathControl.h
+++ b/grid/walk/GridWalkPathControl.h
@@ -64,7 +64,7 @@ public:
 	GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, float distance_m, float headChange_rad) {
 
 		// proportional change of the heading
-		static 	Distribution::Normal<float> dHead(1, 0.01);
+        static 	Distribution::Normal<float> dHead(1, 0.01);
 
 		// proportional change of the to-be-walked distance
 		static 	Distribution::Normal<float> dWalk(1, 0.10);
@@ -97,7 +97,7 @@ private:
 		const float diff = possibleHead.getDiffHalfRAD(head);
 
 //		// compare this heading with the requested one
-		const double angleProb = Distribution::Normal<float>::getProbability(0, Angle::degToRad(25), diff);
+        const double angleProb = Distribution::Normal<float>::getProbability(0, Angle::degToRad(25), diff);
 //		const double angleProb = (diff <= Angle::degToRad(15)) ? 1 : 0.1;		// favor best 3 angles equally
 
 		// nodes own importance
diff --git a/nav/a-star/AStar.h b/nav/a-star/AStar.h
index 131d18f..486baf3 100644
--- a/nav/a-star/AStar.h
+++ b/nav/a-star/AStar.h
@@ -43,9 +43,15 @@ public:
 		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;
+
 		// 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;
 
@@ -66,12 +72,14 @@ public:
 				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, *source)));
+                    Q.push(std::make_pair(&v, distance[&v] + acc.getHeuristic(v, *destination)));
 				}
 
 			}
 		}
 
+                //std::cout << iter << std::endl;
+
 
 		// construct the path
 		std::vector<const T*> path;