Added unit tests, improved performance, fixed