Finished the algorithms but not tested yet