Add homemade version of the A* path planning