Add test suite and optimize some stuff