partially completed
Main body of work on the alg done
Some minor comments added
Merge pull request #6 from xLeachimx/implementAlg

Implement alg