Implemented the FSM

Lots of little changes etc