Implemented Dijkstra's and Started old piece move