Algorithm Implemented

Fnally the algorithm seam its works!