Fixed last outstanding TODO!. Still untested and probably has some bugs.