A*- heuristic 1 is done, needs more testing .. basic testing passed