Mostly refactored, some bugfixes, removed debug prints, Pep8ified.