Some fixes and create simple test program