#40 Code reorganization: Cleanup + TODOs