function [zopt, f, status, iA] = mpc_solveQP(xQP,nCon,degrees,isQP,Kx,Kr,...
    rseq,Ku1,old_u,Kv,vseq,Kut,utarget,Linv,Hinv,Ac,Bc,iA,...
    CustomSolver,CustomSolverCodeGen,H,UseSuboptimalSolution,...
    QPAlgorithm,ASOptions,IPOptions,ADOptions,MIQPOptions,isHyb,Mdis,Ndis,Vdis,uoff,numdis,maxdis)
% Internal Use (by mpcloop_engine and mpcblock_optimizer)
%
% Analytical solution: when MPC is unconstrained or when (MPC is constrained && MaxIter is 0)
%
% Freezing: when status<0 (infeasible QP) or (status=0 && not suboptimal)

%   Authors: R. Chen, A. Bemporad
%   Copyright 1986-2013 The MathWorks, Inc.

%#codegen
coder.extrinsic('mpcCustomSolver');
ZERO = zeros('like',xQP);
ONE = ones('like',xQP);
ZERO32 = zeros('int32');
ONE32 = ones('int32');
ZERO16 = zeros('int16');
ONE16 = ones('int16');
TotalFreeMoves = degrees-int32(isQP);

%% Compute f
% Formula: f=(xQP'*Kx+rseq'*Kr+old_u'*Ku1+vseq'*Kv+utarget'*Kut)'=Kx'*xQP+Kr'*rseq+Ku1'*old_u+Kv'*vseq+Kut'*utarget
f = zeros(degrees,1,'like',xQP);
for i = 1:TotalFreeMoves
    f(i) = Kx(:,i)'*xQP+Kr(:,i)'*rseq+Ku1(:,i)'*old_u+Kv(:,i)'*vseq+Kut(:,i)'*utarget;
end

%% Compute zopt
% Unconstrained MPC or max iterations is 0: analytical solution
if (~isQP && ~isHyb) || (QPAlgorithm==int32(1) && ASOptions.MaxIterations == ZERO32) || (QPAlgorithm==int32(3) && IPOptions.MaxIterations == ZERO32) 
    zopt = zeros(degrees,1,'like',xQP); 
    for i = 1:degrees
        for j = 1:degrees
            zopt(i) = zopt(i)-Hinv(i,j)*f(j);
        end
    end
    status = ONE; % iA not changed
else % Constrained, must solve QP (iA is logical) or MIQP
    if isHyb
        % Here H,Hinv,Ac,Linv have been already changed to new coordinates
        vlb = -Inf(degrees,1,'like',xQP);
        vub = Inf(degrees,1,'like',xQP);
        Aeq = zeros(0,degrees,'like',xQP);
        beq = zeros(0,1,'like',xQP);
        fw = zeros(degrees,1,'like',xQP);
        dvar = zeros(degrees,1,'int16');
        ubar = zeros(degrees,1,'like',xQP);
        % Compute ubar and adjusts discrete values by subtracting scaled input-offset.
        % Alternative: optimize directly u (with offset) by keeping Vdis as it is 
        % and define uold=old_u+uoff.
        dvar(1:TotalFreeMoves) = int16(Ndis);
        nu = numel(old_u)*ONE16;
        h = ZERO16;
        for i=1:int16(TotalFreeMoves)
            di = dvar(i);
            if di>ZERO16
                % jth MV is discrete
                j = rem(i,nu);
                if j==0
                    j = nu;
                end
                for t = 1:di
                    Vdis(h+t)=Vdis(h+t)-uoff(j);
                end
                ubar(i) = old_u(j); % ubar(i) remains zero for continuous inputs
            end
            h = h + di;
        end
        fw(1:TotalFreeMoves) = linsolve(Mdis',f(1:TotalFreeMoves),struct('UT',true))-H(1:TotalFreeMoves,1:TotalFreeMoves)*ubar(1:TotalFreeMoves);
        Bcw = Bc+Ac(:,1:TotalFreeMoves)*ubar(1:TotalFreeMoves);
        if isQP
            % We exploit block-diagonal structure of Hessian and
            % optimization-vector transformation
            fw(degrees) = f(degrees);
        end
        maxstack = int16(MIQPOptions.MaxPendingNodes);
        maxiter = int16(MIQPOptions.MaxIterations);
        feastol = MIQPOptions.ConstraintTolerance;
        discfeastol = MIQPOptions.DiscreteConstraintTolerance;
        V0 = Inf(1,1,'like',xQP);
        rounding = MIQPOptions.RoundingAtRootNode;
        [wopt,status,numiter] = miqpkwik(...
            H,fw,-Ac,-Bcw,Aeq,beq,vlb,vub,dvar,Vdis,Linv,Hinv,...
            maxstack,maxiter,feastol,discfeastol,V0,rounding,int16(numdis),int16(maxdis));
        zopt=zeros(degrees,1,'like',xQP);
        zopt(1:TotalFreeMoves) = linsolve(Mdis,wopt(1:TotalFreeMoves)-ubar(1:TotalFreeMoves),struct('LT',true)); %iA is not used
        if int32(status)==ZERO32
            status = numiter;
        end
        if numiter==maxiter
            status = ZERO;
        end
        % status:
        %   >0 optimal solution found in "status" number of iterations
        %    0 maxiter reached
        %   -1 infeasible
        %   -2 QP relaxation is feasible but some discrete variables are not discrete
        %   -3 discrete feasible solution found, but stack exceeded.
        %   -4 stack exceeded, no discrete feasible solution found.
        %   -5 stack exceeded, feasible QP relaxation found.
        if (int32(status) == -ONE32 || int32(status) == -2*ONE32 || int32(status) == -4*ONE32 || int32(status) == -5*ONE32 || (int32(status) == ZERO32 && ~UseSuboptimalSolution))
            zopt = zeros(degrees,1,'like',xQP);
        end % if status == -3 then keeps discrete feasible solution found
    elseif (CustomSolverCodeGen)
        % Call wrapper to solve QP in EML code.
        x0 = getX0(Ac,Bc,nCon,degrees,ZERO);
        [zopt,status] = mpcCustomSolverCodeGen(H,f,Ac,Bc,x0);  % iA not changed
    elseif (CustomSolver)
        % Call wrapper to solve QP.  
        x0 = getX0(Ac,Bc,nCon,degrees,ZERO);
        [zopt,status] = mpcCustomSolver(H,f,Ac,Bc,x0);  % iA not changed
    elseif QPAlgorithm == int32(1) % ACTIVE
        [zopt,~,statusINT,iA] = qpkwik(Linv,Hinv,f,Ac,Bc,iA,int32(ASOptions.MaxIterations),int32(nCon),int32(degrees),ZERO32,ONE*ASOptions.ConstraintTolerance);
        status = cast(statusINT,'like',zopt);
    elseif QPAlgorithm == int32(2) % ADMM
        Aeq=zeros(0,degrees,'like',xQP);
        beq=zeros(0,1,'like',xQP);
        z0 = zeros(degrees,1,'like',xQP);
        [zopt,statusINT] = mpc.internal.solver.qpadmm(H,f,-Ac,-Bc,Aeq,beq,[],[],z0,ADOptions);
        status = cast(statusINT,'like',zopt);
    else    % INterios
        Aeq=zeros(0,degrees,'like',xQP);
        beq=zeros(0,1,'like',xQP);
        z0 = zeros(degrees,1,'like',xQP);
        % must use -Ac and -Bc here
        [zopt,statusINT] = qpipmw(H,f,-Ac,-Bc,Aeq,beq,z0,IPOptions);
        status = cast(statusINT,'like',zopt);
    end
    if ~isHyb && (int32(status) < ZERO32 || (int32(status) == ZERO32 && ~UseSuboptimalSolution))
        % QP infeasible or max iterations (if no suboptimal), freeze MV
        zopt = zeros(degrees,1,'like',xQP);
    end
end

function x0 = getX0(Ac,Bc,nCon,degrees,ZERO)
%#codegen
% compute an initial guess under the assumption that the inputs
% from the previous control instant don't change.  This might result in
% a positive initial slack.    
x0 = zeros(degrees,1,'like',ZERO);
for i = 1:nCon
    if Ac(i,degrees)*x0(degrees) < Bc(i) && Ac(i,degrees) ~= ZERO
        x0(degrees) = Bc(i)/Ac(i,degrees);
    end
end
