Implemented testing class in Debug package. Found Bug in pathfinder, debugging ongoing.