Basic A* implementation for pathfinding demo