Cleaned up code

> Better function names
> Better comments
> More assertions
> Minor bug fixes