Bug fixes

Fixed bugs, and TODOs in the code.