Cleaned up and improved code

Various clean up and improvements. Should be way faster, will improve
more later.