Code clean up and Unit Test

- Create Unit Test for functionalities
- Clean up code
- Add comments