Reorganize codes, add debug function