Algorithm fix

Fixed some bugs and merged with the latest code. Now all of the framework is done! Should be easy to add the other algorithms.