[MAJOR] Partial rewrite of a couple of files.
Fix errors in rewrite
Merge pull request #38 from wei2912/rewrite

Partial rewrite of a couple of files