function [zopt, fopt, status] = mpcCustomOptimization(xQP,degrees,isQP,rseq,...
    old_u,vseq,utarget,Ac,Bc,maxiter,isquad,H,f,isnlcon,...
    yoff,uoff,voff,uscale,yscale,vscale,Jm,SuJm,Su1,Sx,Hv)
% Gateway function to custom (nonlinear) cost function and constraints

% Author:  A. Bemporad
% Copyright 2017 The MathWorks, Inc.

%% collect info
nmv = numel(old_u);
ny = numel(yoff);
p = size(rseq,1)/ny;
m = (degrees-isQP)/nmv;     % degrees = TotalFreeMoves + isQP by definition
nv = numel(vseq)/(p+1);     % the last column of vseq is DX Offset
%% process y and u references and measured disturbance
rseq = reshape(rseq,ny,p)';     % convert to p-by-ny
vseq = reshape(vseq,nv,p+1)';   % convert to p+1-by-nv
utarget = reshape(utarget,nmv,p)';  % convert to p-by-nmv
%% adjust offset/scaling to get absolute units of references
rseq = (rseq+ones(p,1)*(yoff')).*(ones(p,1)*yscale');
utarget = (utarget+ones(p,1)*(uoff')).*(ones(p,1)*uscale');
% vseq adjustment is done later inside "mpcCustomFcnMap" called by wrappers
%% set "fmincon" options
opts = optimoptions('fmincon');
opts.Display = 'off';
opts.Algorithm = 'sqp';
opts.MaxIterations = maxiter; % honor maximum iteration
%% Set "SpecifyObjectiveGradient" based on what custom cost function returns
% if custom cost function is not used, gradient is always used by built-in.
flag = true;
if  ~isquad
    % Makes dummy function evaluation to test Jacobians
    [~,dfdy,dfdu,dfddu,dfdslack] = mpcCustomCostFcn(rseq,rseq,utarget,utarget,zeros(p,nmv),vseq(:,1:end-1),0);
    if isempty(dfdy) || isempty(dfdu) || isempty(dfddu) || isempty(dfdslack)
        flag = false;
    end
end
opts.SpecifyObjectiveGradient = flag;
%% Set "SpecifyConstraintGradient" based on what custom constraint function returns
if isnlcon
    [C,Ceq,dCdy,dCdu,dCddu,dCdslack,dCeqdy,dCeqdu,dCeqddu,dCeqdslack] = mpcCustomConstraintFcn(rseq,rseq,utarget,utarget,zeros(nmv,1),vseq(:,1:end-1),0);
    flag = true;
    if ~isempty(C) && (isempty(dCdy) || isempty(dCdu) || isempty(dCddu) || isempty(dCdslack))
        flag = false;
    end
    if ~isempty(Ceq) && (isempty(dCeqdy) || isempty(dCeqdu) || isempty(dCeqddu) || isempty(dCeqdslack))
        flag = false;
    end
    if isempty(C) && isempty(Ceq)
        flag = false;
    end
    opts.SpecifyConstraintGradient=flag;
end
%% compute I2Jm
if ~isquad || isnlcon
    I2Jm = kron(tril(ones(p)),eye(nmv))*Jm;
end
%% define cost function 
if isquad
    % use build-in quad cost function
    fun = @(z) mpcCustomCostQuadFcn(z,H,f);
else
    fun = @(z) mpcCustomCostFcnWrapper(z,rseq,old_u,vseq,utarget,xQP,...
          I2Jm,SuJm,Su1,Sx,Hv,nmv,ny,m,p,yoff,uoff,voff,uscale,yscale,vscale);
end
%% define constraint function 
if isnlcon
    con = @(z) mpcCustomConstraintFcnWrapper(z,rseq,old_u,vseq,utarget,xQP,...
          I2Jm,SuJm,Su1,Sx,Hv,nmv,ny,m,p,yoff,uoff,voff,uscale,yscale,vscale);
else
    con = [];
end
%% initialize outputs
x0 = zeros(degrees,1);
Aeq = [];
Beq = [];
LB = [];
UB = [];
%% run optimization
[zopt,fopt,flag] = fmincon(fun,x0,-Ac,-Bc,Aeq,Beq,LB,UB,con,opts);
%% process exit flag (SQP: 2/1/0/-1/-2/-3)
switch flag
    case 2          % suboptimal: solver cannot move x while no constraint violation
        status = 0; 
    case 1          % optimal (no iteration information available)
        status = 1; 
    case 0
        status = 0; % maximum iteration exceeded
    case -2
        status= -1; % infeasible
    otherwise
        status= -2;
end

