Cleanup and fix test program