First Steps A* pathfinding
Algorithm working but not implementet yet