Implemented algorithm. Needs to be tested.