Slightly reduced the math
More bug fixes and unit tests
All unit test implemented!! yay!