AStar algorithm implementation, No left turn solver implemented