Update control mode switch logic

* Decoupled timeouts
* thread safety
* minor optimizations

 Untested!