commiting astar algorithm... almost complete ish