Cleanup and bug fixes

Fixes issue #17 
Added test script and various test directories
Shortened test files
Added some comments and misc cleanup.