Added beginings of unit testing

This needs much more comprehensive unit tests to really be useful

Must find a proper C unit test framework to use