Rewite the code by using C99 initialization and remove some useless code
Rearrange the code