implemented the algorithm. Some need of refactoring though