Add first outline of control algorithm