Added buildpath function (A* algorithm)