Adding test routines and minor bugfix