Fix merge error
Refactor code for better readability
Migrate to debug macro instead of calling printf directly