Refactoring and getting the algorithm to work

Issues currently with list iterator, needs a lot of work!