Ensure unit tests pass

The unit tests currently didn't pass. Any refactoring in the
code needs passing unit tests. I've enclosed any non passing test case
in a TODO define, so that they can easily rerun with
`make run CFLAGS_EXTRA=-DTODO`