Cleaned up some code.

Added structure and cleaned up debug stuff.