Working generic FSM. Tested with CppUTest, needs to be tested on target.