Implemented simple A* path finding