Model Predictive Control Code