Added some tests and minor improvements
Improved speed, added tests and benchmarks