Fix hierarchical state machine bug and add unit test