Added new algorithm functions.
Finalized new algorithm