Fixing some bugs, updating TODO and commenting code