Refactored and reimplemented algorithms

The algorithms NEED to be tested, it's the first time a implement them and i'm not a good implementator XD