Add test code and Makefile