Initial code pass.

- Appears to work. Need to run through some test cases
- Need to comment better
Commented code