70% done. Added A-star search