Bug fixes and unit tests for integrator
More tests/bug fixes