Implemented most of the algorithm