Finished the algorithm and fixed some bugs