Code: Bug fixes and minor refactoring

@zhangcong2711, modified your code with my changes. See the changes.