Improvements and comments

Optimization, clean up, commenting etc.