Cleaned up and improved code

Various clean up and improvements. Should be way faster, will improve
more later.
Merge pull request #1 from robottom/master

Cleaned up and improved code