A* and Code cleanup: Path still needs simplification