Re-enabled RTOS and added basic state machine framework