Rewritten code and added comments

Rewritten code and added comments, still some todo's to be done.