first version..  Not yet implement reducing path length
Astar initial version