Add working A* implementation

A* is now working as intended!! Next step, implement heuristics