Fixed control logic.

Not bug free, but control is now close to correct.