Code cleaned up. Updated README and TODO.