Cleanup code a bit
Update TODO