Refactored code, added some debugging help

... and broke the algo :) Now on to fixing it.