Added simple testing framework adapted from lest
Added some tests and already fixed bugs exposed by tests
Added some math tests