First Draft finished

All functions has been finished as first draft. Further debugging or
changing of comment is needed