FSM Test coverage, code cleanup and bugfixes