Reorganized and rewrote.

Tried to make it look like I know what I'm doing. Also added a couple
JUnit tests.