Refactor and add debug code