Done all basic A* path finding function and visualization