Fixed a bug in routine to add matrix entries when rebuilding is required, added more rigorous tests