Updated TODO.
Code refactoring.
Started implementing block world test case.
Refactored and cleaned code.