Implement A Star algorithm
WIP Add specs