Generic A* implementation (UNFINISHED)