Fixed some issues from previous changes.

Now works as expected. Time to implement new algorithm.