Implemented a hopefully working version of the A* Algorithm