First pass at real astar pathing