Wrote an A-star search system
Started on actual PF element.