Bug fixes, test and Nico's function