A* Algorithm and other small updates and fixes.

[OpenSauce]

Hello!

Closes #2

In this I've added an A* method with the relevant heuristics. I've also included the update of the original function to use a map for faster lookup. If there's anything you want changing or adding let me know.