Adding some tests and bench. Correcting few bugs.