Add unittesting, make the program better for test
Add test.sh for easily testing