Prototyped BFS but way points from start to end were not recorded properly. The way points were recorded regardless of whether they will lead to a solution or not.