Cleanup and add commenting

First really working version.