Replace goto control flow with state machine control flow in id_main

Replace goto control flow with state machine control flow in id_main #607