A-star first implementation(Incomplete)