Wrote test code. Bugs: Dosn't really test as accuratly as I would like.
Work in progress.