code cleanup and add unittests for CMP