Restructure, added comment and better timing

- Restructered the code for better reading
- Added more comments
- Better timing
- Removed bug in README.md