Added code to detect when ICP alignment thread is finished and to proceed to the next state