A working A* Algorithm. It doesn't prioritise the closest goal though.