Add unit test framework to CMakeLists.txt. Fix a couple of bugs found by unit tests. Change-Id: I2092a7f0570fae0f19f9e083c4837ccefcc4ca1a