sossolve.m

function [sos,info] = sossolve(sos,options)
% SOSSOLVE --- Solve a sum of squares program.
%
% SOSP = sossolve(SOSP)
%
% SOSP is the SOS program to be solved.
%
% Alternatively, SOSP = sossolve(SOSP,SOLVER_OPT) also defines the solver
% and/or the solver-specific options respectively by fields
%
% SOLVER_OPT.solver (name of the solver). This can be 'sedumi', 'sdpnal',
% 'sdpnalplus', 'csdp', 'cdcs', 'sdpt3', 'sdpa'.
% SOLVER_OPT.params (a structure containing solver-specific parameters)
%
% The default values for solvers is 'SeDuMi' with parameter ALG = 2, which
% uses the xz-linearization in the corrector and parameter tol =1e-9. See
% SeDuMi help files or user manual for more detail.
%
% Using a second output argument such as [SOSP,INFO] = sossolve(SOSP) will
% return in INFO numerous information concerning feasibility  and CPU time
% that is generated by the SDP solver.
%

% This file is part of SOSTOOLS - Sum of Squares Toolbox ver 4.00.
%
% Copyright (C)2002, 2004, 2013, 2016, 2018, 2021
%                                      A. Papachristodoulou (1), J. Anderson (1),
%                                      G. Valmorbida (2), S. Prajna (3),
%                                      P. Seiler (4), P. A. Parrilo (5),
%                                      M. Peet (6), D. Jagt (6)
% (1) Department of Engineering Science, University of Oxford, Oxford, U.K.
% (2) Laboratoire de Signaux et Systmes, CentraleSupelec, Gif sur Yvette,
%     91192, France
% (3) Control and Dynamical Systems - California Institute of Technology,
%     Pasadena, CA 91125, USA.
% (4) Aerospace and Engineering Mechanics Department, University of
%     Minnesota, Minneapolis, MN 55455-0153, USA.
% (5) Laboratory for Information and Decision Systems, M.I.T.,
%     Massachusetts, MA 02139-4307
% (6) Cybernetic Systems and Controls Laboratory, Arizona State University,
%     Tempe, AZ 85287-6106, USA.
%
% Send bug reports and feedback to: sostools@cds.caltech.edu
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program. If not, see <http://www.gnu.org/licenses/>.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Change log and developer notes

% 12/25/01   - SP
% 01/05/02   - SP - primal
% 01/07/02   - SP - objective
% aug/13     - JA,GV - CSDP,SDPNAL,SDPA solvers and SOS matrix decomposition
% 06/01/16   - JA - added interface to frlib facial reduction package by
%                 F Permenter and PP.
% 01/04/18   - AP - Added CDCS and SDPNALplus
% 6/27/2020  - MP, SS - Added mosek as an optional solver
% 09/11/2021 - AT - Created interface to sospsimplify
% 09/11/2021 - DJ - Added feasibility check after sospsimplify
% 09/25/2021 - AT - Added default parameters for sospsimlify. 
% 12/10/2021 - DJ - Added default tolerance for psimplify
%                   Also allow "options.simplify=0" as one of the options
% 02/14/2022 - DJ - Initial dpvar version of addextrasosvar.
% 02/25/2022 - DJ - Exit when monomials are empty in syms addextrasosvar
% 03/08/2022 - DJ - Set default value "feasextrasos=1"
% 03/09/2022 - DJ - Add check "isempty(K.s)" for sdpt3, sdpnal, sdpnalplus
% 08/14/2022 - DJ - Add check "phasevalue==pUNBD" and "==dUNBD" for sdpa.
% 09/03/2022 - DJ - Add check K.q==0 and K.r==0 in call to sdpa.
% 12/15/2022 - DJ - Remove 0x0 decision variables after psimplify.
% 02/07/2023 - DJ - Bugfix "addextrasosvar" for matrix-valued polynomials.
% 03/20/2023 - DJ - Add check for existence of solver on path.
% 10/31/2024 - DJ - Bugfix for SDPT3 post-processing (smallblkdim vs
%                                                           smblkdim)
% 05/18/2025 - DJ - Use Mosek as first choice for default SDP solver.

if (nargin==1)
    %Default options from old sossolve
    options.solver = 'none';
    options.params.tol = 1e-9;
    options.params.alg = 2;
elseif ((nargin==2) && ~isnumeric('options') )%2 arguments given,
    if ~isfield(options,'solver')
        options.solver = 'none';
    end
    if ~isfield(options,'params')
        options.params.tol = 1e-9;%default values for SeDuMi
        options.params.alg = 2;
    elseif ~isfield(options.params,'tol')
        options.params.tol = 1e-9;%default values for SeDuMi
    end
end

% Check if solver is appropriately specified.
solver_list = {'mosek'; 'sedumi'; 'sdpt3'; 'csdp'; 'sdpnal'; 'sdpnalplus'; 'sdpa'; 'cdcs'};     % 05/18/2025 - DJ
path_list = {'mosekopt'; 'sedumi'; 'sqlp'; 'csdp'; 'sdpnal'; 'sdpnalplus'; 'sdpam'; 'cdcs'};
if strcmp(options.solver,'none')
    % If no solver is specified, default to first one found on path.
    for k=1:length(solver_list)
        if exist(path_list{k},'file')
            options.solver = solver_list{k};
            break
        end
    end
    if strcmp(options.solver,'none')
        error('No SDP solver detected. Please make sure the desired solver is on the Matlab path.')
    end
else
    % Otherwise, check that the desired solver is indeed on Matlab path.
    [is_solver,solver_num] = ismember(options.solver,solver_list);
    if ~is_solver
        error(['The desired SDP solver ''',options.solver,''' is not supported by SOSTOOLS. Please pass one of: ',...
                '''sedumi'', ''mosek'', ''sdpt3'', ''csdp'', ''sdpnal'', ''sdpnalplus'', ''sdpa'', ''cdcs''.'])
    elseif ~exist(path_list{solver_num},'file')
        error(['The desired SDP solver ''',solver_list{solver_num},''' could not be found. Please make sure the function ''',path_list{solver_num},''' is on the Matlab path.'])
    end
end


% AT (09/25/2021) Default options for sospsimplify
if isfield(options,'simplify')
    if (ischar(options.simplify) && (strcmpi(options.simplify,'on') || strcmpi(options.simplify,'true') || strcmpi(options.simplify,'1') || strcmpi(options.simplify,'simplify'))) ...
            || (islogical(options.simplify) && options.simplify) ...
                || (isnumeric(options.simplify) && options.simplify == 1)
        options.simplify = true;
    elseif (ischar(options.simplify) && (strcmpi(options.simplify,'off') || strcmpi(options.simplify,'false') || strcmpi(options.simplify,'0'))) ...
            || (islogical(options.simplify) && ~options.simplify) ...
                || (isnumeric(options.simplify) && options.simplify == 0)
        options.simplify = false;
    else
        warning("options.simplify should be set to either true or false");
        warning("Set simplify off by default");
        
        % sospsimplify is disabled if the user used an unapproved input
        options.simplify = false;
    end
else
    % sospsimplify off by default
    options.simplify = false;
end

%whenever nargin>=2 options are overwritten
if (nargin==3)
    error('Current SOSTOOLS version does not support call to sossolve with 3 arguments, see manual.');
end

if ~isempty(sos.solinfo.x)
    error('The SOS program is already solved.');
end

% Adding slack variables to inequalities
sos.extravar.idx{1} = sos.var.idx{sos.var.num+1};
% SOS variables
feasextrasos = 1;   % Will be set to 0 if constraints are immediately found infeasible
I = [find(strcmp(sos.expr.type,'ineq')), find(strcmp(sos.expr.type,'sparse')), find(strcmp(sos.expr.type,'sparsemultipartite'))];
if ~isempty(I)
    [sos,feasextrasos] = addextrasosvar(sos,I);
end
% SOS variables type II (restricted on interval)
I = find(strcmp(sos.expr.type,'posint'));
if ~isempty(I)
    sos = addextrasosvar2(sos,I);
end

% Processing all expressions


Atf = []; bf = [];
for i = 1:sos.expr.num
    Atf = [Atf, sos.expr.At{i}];
    bf = [bf; sos.expr.b{i}];
end;

% Processing all variables
[At,b,K,RR] = processvars(sos,Atf,bf);

% Objective function
c = sparse(size(At,1),1);

%% Added by PAP, for compatibility with MATLAB 6.5
if isempty(sos.objective)
    sos.objective = zeros(size(c(1:sos.var.idx{end}-1)));
end
%% End added stuff

c(1:sos.var.idx{end}-1) = c(1:sos.var.idx{end}-1) + sos.objective;       % 01/07/02
c = RR'*c;

pars = options.params;
% Set tolerance for psimplify
if isfield(options,'simplify_tol')    
    ptol = options.simplify_tol;
else
    ptol = max(pars.tol*1e-3,1e-12);         
end

% AT - created interface to sospsimplify
feassosp = 1; %09/25/21 the default value, it is used for sospsimplify
if options.simplify

    fprintf('Running simplification process:\n')
    if isfield(options,'frlib')
        disp('Warning: SOSTOOLS does not support use of both "psimplify" and "frlib" to simplify program; proceeding with only "psimplify".')
    end

    At_full = At';   c_full = c; %Need duplicate copy if applying facial reduction as reduced matrices overwrite these
    b_full = b;     K_full = K;
    size_At_full = size(At_full);
    % Some initial parameters for sospsimplify
    Nsosvarc = length(K.s);
    dv2x = 1:size_At_full(2);
    K_full.l = 0;

    Zmonom = cell(1,Nsosvarc);
    for i= 1:Nsosvarc
        Zmonom{i} = (1:K.s(i))';
    end
    %A,b,K -reduced matrices
    [A,b,K,~,dv2x,~,feassosp,~,removed_rows] = sospsimplify(At_full,b_full,K_full,Zmonom, dv2x,Nsosvarc, ptol);

    fprintf('Old A size: %d  %d\n', size(At));
    fprintf('New A size: %d  %d\n', size(A'));
    % 	[prg_primal] = frlib_pre(options.frlib,At',b,c,K); %interface with frlib
    At = A';  %reduced SDP matrices
    %b = b;
    chosen_idx = (dv2x ~= 0);
    c = c(chosen_idx);
    K.s(K.s==0) = [];   % Get rid of 0x0 variables, DJ - 12/15/22
    size_AT_solved = size(At);
    %if size(At,2)~=length(b) | length(b) > length(c)
    %    error('Error simplifying the problem, it may be infeasible. Try running without ''simplify''.')
    %end

% Perform facial reduction using FP's algorithm (JA 5/1/16)
elseif isfield(options,'frlib')
    At_full = At;   c_full = c; %Need duplicate copy if applying facial reduction as reduced matrices overwrite these
    b_full = b;     K_full = K;
    size_At_full = size(At_full);
    
    [prg_primal] = frlib_pre(options.frlib,At',b,c,K); %interface with frlib
    At = prg_primal.A';  %reduced SDP matrices
    b = prg_primal.b;
    c = prg_primal.c';
    K = prg_primal.K;
    size_AT_solved = size(At);

    fprintf('Old A size: %d  %d\n', size_At_full);
    fprintf('New A size: %d  %d\n', size_AT_solved);
    
    if size(At,2)~=length(b) | length(b) > length(c)
        error('Error simplifying the problem, it may be infeasible. Try running without ''frlib''.')
    end
else
    size_At_full = size(At);
end

% Check for trivially infeasible constraints b=0 with nonzero b             % DJ, 08/12/23
if feassosp && any((sum(abs(At)>ptol,1)==0)' & (abs(b)>ptol*max(abs(b))))
    feassosp=0;
end

% AT - 9/28/2021
if feassosp==0 || feasextrasos==0 % if the sospsimplify or addextrasosvar return infeasible solution.
    % If the problem is clearly infeasible, sedumi can return an error.
    % Return no solution if the problem is clearly infeasible from
    % sospsimplify or addextrasosvar.
    fprintf(2,'\n Warning: Primal program infeasible, no solution produced.\n\n')
    info.iter = 0;
    info.feasratio = -1;
    info.pinf = 1;
    info.dinf = 1;
    info.numerr = 0;
    info.timing = 0;
    info.cpusec = 0; 
    sos.solinfo.info = info;
    sos.solinfo.solverOptions = options;

    % % Set the solution to NaN;
    % if isempty(sos.extravar.idx)
    %     nx = sos.var.idx{end}-1;
    % else
    %     nx = sos.extravar.idx{end}-1;
    % end
    % ny = 0;
    % for j=1:numel(sos.extravar.T)
    %     ny = ny+size(sos.extravar.T{j},2);
    % end
    % sos.solinfo.x = nan(nx,1);
    % sos.solinfo.RRx = nan(nx,1);
    % sos.solinfo.y = nan(ny,1);
    % sos.solinfo.RRy = nan(nx,1);
    return
end

if strcmp(lower(options.solver),'sedumi')
    % SeDuMi in action
    size_At = size(At);
    disp(['Size: ' num2str(size_At)]);
    disp([' ']);
    
    [x,y,info] = sedumi(At,b,c,K,pars);
    if ~isfield(info,'pinf')
        info.pinf=0;
    end
    if ~isfield(info,'dinf')
        info.dinf=0;
    end
    if ~isfield(info,'numerr')
        info.numerr=0;
    end
    %size_AT_solved = size(At);
    %     if isfield(options,'ReducePrimal') && size_AT_solved(1) < size_At_full(1) %frlib applied and reduction constructed
    %        dim_b = length(b_full);
    %        [x,y] = frlib_post(prg_primal,x,y,dim_b);
    %        At = At_full;
    %        b = b_full;
    %        c = c_full;
    %        K = K_full;
    %     end
elseif strcmp(lower(options.solver),'mosek')
    % Converting to mosek compatible format
    size_At = size(At);
    disp(['Size: ' num2str(size_At)]);
    disp([' ']);
    prob = Sedumi2Mosek(At',b,c,K);
    [~,res] = mosekopt('minimize info',prob);
    [x,Y] = MosekSol2SedumiSol(K,res);
    y=Y(1:size(At,2));
    info=[];
    info.cpusec = res.info.MSK_DINF_OPTIMIZER_TIME; %OK
    info.iter = res.info.MSK_IINF_INTPNT_ITER;   %OK
    info.feasratio = res.info.MSK_DINF_INTPNT_OPT_STATUS;
    if contains(res.sol.itr.prosta,'INFEASIBLE') && contains(res.sol.itr.prosta,'DUAL')
        info.dinf = 1;
    else
        info.dinf = 0;
    end
    if contains(res.sol.itr.prosta,'INFEASIBLE') && contains(res.sol.itr.prosta,'PRIM')
        info.pinf = 1;
    else
        info.pinf = 0;
    end
    if contains(res.sol.itr.prosta,'UNKNOWN')
        info.numerr = 2;
    elseif contains(res.sol.itr.solsta,'UNKNOWN')
        info.numerr = 1;
    else
        info.numerr = 0;
    end
elseif strcmp(lower(options.solver),'cdcs')
    % CDCS in action
    size_At = size(At);
    disp(['Size: ' num2str(size_At)]);
    disp([' ']);
    params.maxIter = 50000;
    %params.solver = 'sos';
    %params.relTol = 1e-5;
    [x,y,z,info] = cdcs(At,b,c,K,params);
    info.pinf = 0;
    info.dinf = 0;
    info.numerr = 0;
    if info.problem == 1
        info.pinf = 1;
    elseif info.problem == 2
        info.dinf = 1;
    elseif info.problem == 4
        info.numerr = 1;
    end
elseif strcmp(lower(options.solver),'sdpt3')
    % SDPT3 in action
    smallblkdim = 60;
    save sostoolsdata_forSDPT3 At b c K smallblkdim;
    [blk,At2,C2,b2] = read_sedumi('sostoolsdata_forSDPT3.mat');
    delete sostoolsdata_forSDPT3.mat;
    [obj,X,y,Z,infoSDPT] = sqlp(blk,At2,C2,b2,pars);
    %size_AT_solved = size(At);
    x = zeros(length(c),1);
    cellidx = 1;
    if K.f ~= 0
        x(1:K.f) = X{1}(:);
        cellidx = 2;
    end;
    if ~(isempty(K.s) || K.s(1)==0)   % DJ, 03/09/2022  (do we need the K.s(1)~=0 check?)
        idxX = 1;
        idx = K.f+1;
        % smblkdim = 100;   % DJ, 10/31/2024  (is there a reason we had smblkdim~=smallblkdim?)
        deblkidx = find(K.s > smallblkdim);
        spblkidx = find(K.s <= smallblkdim);
        blknnz = [0 cumsum(K.s.*K.s)];
        for i = deblkidx
            dummy = X{cellidx};
            x(idx+blknnz(i):idx+blknnz(i+1)-1) = dummy(:);
            cellidx = cellidx+1;
        end;
        for i = spblkidx
            dummy = X{cellidx}(idxX:idxX+K.s(i)-1,idxX:idxX+K.s(i)-1);
            x(idx+blknnz(i):idx+blknnz(i+1)-1) = dummy(:);
            idxX = idxX+K.s(i);
        end;
    end;
    info.cpusec = infoSDPT.cputime;
    info.iter = infoSDPT.iter;
    if infoSDPT.termcode == 1
        info.pinf = 1;
    else
        info.pinf = (infoSDPT.pinfeas>0.1);
    end;
    if infoSDPT.termcode == 2
        info.dinf = 1;
    else
        info.dinf = (infoSDPT.dinfeas>0.1);
    end;
    if infoSDPT.termcode<= 0
        info.numerr = infoSDPT.termcode;
    else
        info.numerr = 0;
    end;
elseif strcmp(lower(options.solver),'csdp') %6/6/13 JA CSDP interface
    %CSDP in action
    if exist('solver_options.params')
        pars = options.params;
    else
        pars.objtol = 1e-9;
        pars.printlevel = 1;
    end
    if (isfield(K,'f')) %Convert free vars to non-negative LP vars
        n_free = K.f;
        [A,b,c,K] = convertf(At,b,c,K); %K.f set to zero
        At = A';
    end
    c = full(c);
    [x,y,z,info_csdp] = csdp(At,b,c,K,pars);  %JA updated handling of info flag
    c = sparse(c);
    % 7/6/13 JA Remove extra entries from x corresponding to LP vars
    if (isfield(K,'f')) %Convert free vars to non-negative LP vars
        index = [n_free+1:2*n_free];
        x(1:n_free) = x(1:n_free)-x(index);
        x(index) = [];
        At(index,:)=[];
        c(index) = [];
    end
    switch info_csdp
        case {0,3}
            info.pinf = 0;
            info.dinf = 0;
        case 1
            info.pinf = 1;
            info.dinf = 0;
        case 2
            info.pinf = 0;
            info.dinf = 1;
        otherwise
            info.pinf = 1;
            info.dinf = 1;
    end
elseif strcmp(lower(options.solver),'sdpnal') %6/11/13 JA SDPNAL interface
    % SDPNAL in action
    save sostoolsdata_forSDPNAL At b c K;
    [blk,At2,C2,b2] = read_sedumi('sostoolsdata_forSDPNAL.mat');
    delete sostoolsdata_forSDPNAL.mat;
    pars.maxiter = 100;
    try
        [obj,X,y,Z,infonal,runhist] = sdpnal(blk,At2,C2,b2,pars); %run history not returned;
    catch
        [obj,X,y,Z,infonal,runhist] = sdpnal(blk,At2,C2,b2,pars); %run history not returned;
        info = [];
    end
    x = zeros(length(c),1);
    cellidx = 1;
    if K.f ~= 0
        x(1:K.f) = X{1}(:);
        cellidx = 2;
    end;
    if ~(isempty(K.s) || K.s(1)==0)   % DJ, 03/09/2022  (do we need the K.s(1)~=0 check?)
        idxX = 1;
        idx = K.f+1;
        smblkdim = 100;
        deblkidx = find(K.s > smblkdim);
        spblkidx = find(K.s <= smblkdim);
        blknnz = [0 cumsum(K.s.*K.s)];
        for i = deblkidx
            dummy = X{cellidx};
            x(idx+blknnz(i):idx+blknnz(i+1)-1) = dummy(:);
            cellidx = cellidx+1;
        end;
        for i = spblkidx
            dummy = X{cellidx}(idxX:idxX+K.s(i)-1,idxX:idxX+K.s(i)-1);
            x(idx+blknnz(i):idx+blknnz(i+1)-1) = dummy(:);
            idxX = idxX+K.s(i);
        end;
    end;
    info.iter = infonal.iter;
    info.pinf = (infonal.pinfeas>0.1);
    info.dinf = (infonal.dinfeas>0.1);
    info.msg = infonal.msg;
elseif strcmp(lower(options.solver),'sdpnalplus') %6/11/13 JA SDPNALPLUS interface
    % SDPNALPLUS in action
    smallblkdim = 50;
    save sostoolsdata_forSDPNAL At b c K smallblkdim;
    [blk,At2,C2,b2] = read_sedumi('sostoolsdata_forSDPNAL.mat');
    delete sostoolsdata_forSDPNAL.mat;
    pars.maxiter = 100;
    try
        [obj,X,s,y,S,Z,y2,v,info,runhist] = sdpnalplus(blk,At2,C2,b2,[],[],[],[],[],pars); %run history not returned;
    catch
        [obj,X,s,y,S,Z,y2,v,info,runhist] = sdpnalplus(blk,At2,C2,b2,[],[],[],[],[],pars); %run history not returned;
        info = [];
    end
    x = zeros(length(c),1);
    cellidx = 1;
    if K.f ~= 0
        x(1:K.f) = X{1}(:);
        cellidx = 2;
    end;
    if ~(isempty(K.s) || K.s(1)==0)   % DJ, 03/09/2022  (do we need the K.s(1)~=0 check?)
        idxX = 1;
        idx = K.f+1;
        smblkdim = 100;
        deblkidx = find(K.s > smblkdim);
        spblkidx = find(K.s <= smblkdim);
        blknnz = [0 cumsum(K.s.*K.s)];
        for i = deblkidx
            dummy = X{cellidx};
            x(idx+blknnz(i):idx+blknnz(i+1)-1) = dummy(:);
            cellidx = cellidx+1;
        end;
        for i = spblkidx
            dummy = X{cellidx}(idxX:idxX+K.s(i)-1,idxX:idxX+K.s(i)-1);
            x(idx+blknnz(i):idx+blknnz(i+1)-1) = dummy(:);
            idxX = idxX+K.s(i);
        end;
    end;
    if ~isempty(info)
        if info.etaRp>1e-6||info.etaRd>1e-6
            info.dinf=1;
            info.pinf=1;
        else
            info.dinf=0;
            info.pinf=0;
        end;
    else
        info.dinf=1;
        info.pinf=1;
    end
elseif strcmp(lower(options.solver),'sdpa')
    % SDPA in action
    disp(['Size: ' num2str(size(At))]);
    disp([' ']);
    if isfield(K,'q') && isequal(K.q,0)
        K = rmfield(K,'q');
    end
    if isfield(K,'r') && isequal(K.r,0)
        K = rmfield(K,'r');
    end
        
    [x,y,info]=sedumiwrap(At',b,c,K,[],pars);
    
    if strcmp(info.phasevalue,'pdOPT')|| strcmp(info.phasevalue,'pdFEAS')%primal and dual optimal or feasible
        info.dinf=0;
        info.pinf=0;
    elseif strcmp(info.phasevalue,'pdINF') %primal and dual infeasible
        info.dinf=1;
        info.pinf=1;
    elseif strcmp(info.phasevalue,'pINF_dFEAS') % primal infeasible, dual infeasible
        info.dinf=0;
        info.pinf=1;
    elseif strcmp(info.phasevalue,'pFEAS_dINF') % dual infesaible, primal feasible
        info.dinf=1;
        info.pinf=0;
    elseif strcmp(info.phasevalue,'pUNBD')  % primal problem unbounded
        info.dinf=1;
        info.pinf=0;
        fprintf('\n The primal problem is likely unbounded.\n')
    elseif strcmp(info.phasevalue,'dUNBD')  % dual problem unbounded
        info.dinf=0;
        info.pinf=1;
        fprintf('\n The dual problem is likely unbounded.\n') 
    elseif strcmp(info.phasevalue,'noINFO') || strcmp(info.phasevalue,'pFEAS') || strcmp(info.phasevalue,'dFEAS')% max. iterations exceeded no idea if feasible
        if info.primalError < 1e-6 || strcmp(info.phasevalue,'pFEAS')
            info.pinf = 0;
        else
            info.pinf = 1;
        end
        if info.dualError < 1e-6 || strcmp(info.phasevalue,'dFEAS')
            info.dinf = 0;
        else
            info.dinf = 1;
        end
    end;
end;

% DJ - Avoid multiplication with empty array if infeasible
if info.pinf && isempty(x)  
    fprintf(2,'\n Warning: Primal program infeasible, no solution produced.\n')
    info.iter = 0;
    info.feasratio = -1;
    info.pinf = 1;
    info.dinf = 1;
    info.numerr = 0;
    info.timing = 0;
    info.cpusec = 0; 
    sos.solinfo.info = info;
    sos.solinfo.solverOptions = options;
    return
end

% AT - added interface to sospsimplify (9/11/2021)
% post-processing
if lower(options.simplify)==1 | (strcmp(lower(options.simplify),'on') | strcmp(lower(options.simplify),'1')  | strcmp(lower(options.simplify),'simplify'))
    %if isfield(prog1_sosp.solinfo.info,'pinf')&&prog1_sosp.solinfo.info.pinf==0
    %if isfield(prog1_sosp.solinfo.info,'pinf')&&prog1_sosp.solinfo.info.pinf==0
    At = At_full';  %Restore original matrices
    b = b_full;
    c = c_full;
    K = K_full;
    xx = zeros(size(At_full, 2), 1); % if we remove it using sospsimplify it must be 0

    xx(chosen_idx) = x;    % restore the solution
    yy = zeros(size(At_full, 1), 1);    % restore dual multipliers
    row_idx = ones(size(At_full, 1), 1);    
    row_idx(removed_rows) = 0;    
    yy(row_idx == 1) = y;% restore dual multipliers
    y = yy;
    x = xx;
    %end
    %end    %JA frlib post-process
elseif isfield(options,'frlib') && size_AT_solved(1) < size_At_full(1) %frlib applied and reduction constructed
    dim_b = length(b_full);
    [x,y] = frlib_post(prg_primal,x,y,dim_b);
    At = At_full;  %Restore original matrices
    b = b_full;
    c = c_full;
    K = K_full;
end

disp([' ']);
disp(['Residual norm: ' num2str(norm(At'*x-b))]);
disp([' ']);
sos.solinfo.x = x;
sos.solinfo.y = y;
sos.solinfo.RRx = RR*x;
sos.solinfo.RRy = RR*(c-At*y);    % inv(RR') = RR
sos.solinfo.info = info;
sos.solinfo.solverOptions = options;
disp(info)


%return;

% Constructing the (primal and dual) solution vectors and matrices
% If you want to have them, comment/delete the return command above.
% In the future version, these primal and dual solutions will be computed only
% when they are needed. We don't want to store redundant info.

for i = 1:sos.var.num
    switch sos.var.type{i}
        case 'poly'
            sos.solinfo.var.primal{i} = sos.solinfo.RRx(sos.var.idx{i}:sos.var.idx{i+1}-1);
            sos.solinfo.var.dual{i} = sos.solinfo.RRy(sos.var.idx{i}:sos.var.idx{i+1}-1);
        case 'sos'
            primaltemp = sos.solinfo.RRx(sos.var.idx{i}:sos.var.idx{i+1}-1);
            dualtemp = sos.solinfo.RRy(sos.var.idx{i}:sos.var.idx{i+1}-1);
            sos.solinfo.var.primal{i} = reshape(primaltemp,sqrt(length(primaltemp)),sqrt(length(primaltemp)));
            sos.solinfo.var.dual{i} = reshape(dualtemp,sqrt(length(dualtemp)),sqrt(length(dualtemp)));
    end;
end;

for i = 1:sos.extravar.num
    primaltemp = sos.solinfo.RRx(sos.extravar.idx{i}:sos.extravar.idx{i+1}-1);
    dualtemp = sos.solinfo.RRy(sos.extravar.idx{i}:sos.extravar.idx{i+1}-1);
    sos.solinfo.extravar.primal{i} = reshape(primaltemp,sqrt(length(primaltemp)),sqrt(length(primaltemp)));
    sos.solinfo.extravar.dual{i} = reshape(dualtemp,sqrt(length(dualtemp)),sqrt(length(dualtemp)));
end;

sos.solinfo.decvar.primal = sos.solinfo.RRx(1:sos.var.idx{1}-1);
sos.solinfo.decvar.dual = sos.solinfo.RRy(1:sos.var.idx{1}-1);



% ====================================================================================
function [sos,feasextrasos] = addextrasosvar(sos,I)
% Adding slack SOS variables to inequalities
feasextrasos = 1;   % Assume problem is feasible

if ~isfield(sos,'symvartable')
    % Code for dpvar case (how to distinguish pvar and dpvar case?)
    for i = I
    % % % Enforce inequality constraint F(x)=S(x) where S(x) is PSD for all x
        
        % % Extract information about the LHS function F(x)
        Zin = sos.expr.Z{i};
        [nmons,nvars] = size(Zin);
        ncons = size(sos.expr.b{i},1);
        if mod(ncons,nmons)~=0
            error('Error in the implementation, size of b should be divisible by the number of monomials')
        else
            Fdim = sqrt(ncons/nmons);
        end
        if Fdim~=round(Fdim)
            error('Constraints of type ''ineq'' can only be imposed on square matrices')
        end
        
        
        % % Build monomial vector for RHS function S(x)
        % Initialize as all monomials between min/2 and max/2
        maxdeg = full(max(sum(Zin,2)));     % maximum total degree of the monomials
        mindeg = full(min(sum(Zin,2)));     % minimum total degree of the monomials (for matrixvars, this will be at least 2)
        Z = monomials(nvars,(floor(mindeg/2):ceil(maxdeg/2))); 
        
        % Then, discard unnecessary monomials
        maxdegree = sparse(max(Zin,[],1)/2);    % row of max degrees in each variable
        mindegree = sparse(min(Zin,[],1)/2);    % row of min degrees in each variable
        Zdummy1 = bsxfun(@minus,maxdegree,Z);   % maxdegree monomial minus each monomial
        Zdummy2 = bsxfun(@minus,Z,mindegree);   % each monomial minus mindegree monomial
        [I,~] = find([Zdummy1 Zdummy2]<0);      % rows which contain negative terms
        IND = setdiff(1:size(Z,1),I,'stable');  % rows not listed in I
        if isempty(IND)
            fprintf(['\n Warning: Inequality constraint ',num2str(i),...
                ' in your sos program structure corresponds to a polynomial that is not sum-of-squares.\n'])
            feasextrasos = 0;   % Indicate the problem is not feasible.
            return
        else
            Z = Z(IND,:);                       % discard all monomials rows listed in I
        end
        
        % If requested, further optimize Z
        if strcmp(sos.expr.type{i},'sparse')
            Z2 = sos.expr.Z{i}/2;
            Z = inconvhull(full(Z),full(Z2));
            Z = makesparse(Z);
            %disp(['Optimized again : ',num2str(size(Z,1))]);
        end
        % and sparse_multipartite, also just copied from sym version:
        if strcmp(sos.expr.type{i},'sparsemultipartite')
            Z2 = sos.expr.Z{i}/2;           % lots of fractional degrees
            info2 = sos.expr.multipart{i};  % the vectors of independent variables
            sizeinfo2m = length(info2);
            vecindex = [];
            for indm = 1:sizeinfo2m % for each set of independent variables (first true ind, then matrix)
                sizeinfo2n(indm) = length(info2{indm}); % number of variables in cell
                for indn = 1:sizeinfo2n(indm) % scroll through the matrix variables,

                    % PJS 9/12/13: Update code to handle polynomial objects
                    var = info2{indm}(indn);
                    cvartable = char(sos.varmat.vartable);

                    if ispvar(var)
                        % Convert to string representation
                        var = var.varname;
                    end
                    varcheckindex = find(strcmp(var,sos.vartable));
                    if ~isempty(varcheckindex)
                        vecindex{indm}(indn) = varcheckindex;
                    else
                        vecindex{indm}(indn) = length(info2{1}) + find(strcmp(var,cvartable));
                    end
                        
                    % PJS 9/12/13: Original Code to handle polynomial objects
                    %vecindex{indm}(indn) = find(strcmp(info2{indm}(indn).varname,sos.vartable));

                end
            end
            Zmp = sparsemultipart(full(Z),full(Z2),vecindex);
            Zmp = makesparse(Zmp);
            if ~isempty(Zmp)        % Fix in case result is empty, but might not be appropriate...
                Z = Zmp;
            end
        end
        nmons_Z = size(Z,1);
        
        
        % % Add variables associated to RHS S(x) to sos program
        % Add the monomials
        sos.extravar.num = sos.extravar.num + 1;
        var = sos.extravar.num;
        sos.extravar.Z{var} = makesparse(Z);
        [T,ZZ] = getconstraint(Z);      % Z'QZ = vec(Q)'T'ZZ 
        sos.extravar.ZZ{var} = ZZ;
        sos.extravar.T{var} = T';
        
        % Add the decision variables
        sos.extravar.idx{var+1} = sos.extravar.idx{var}+(nmons_Z*Fdim)^2; % new decision variables associated with this constraint
        for j = 1:sos.expr.num
            sos.expr.At{j} = [sos.expr.At{j}; sparse(size(T,2)*Fdim^2,size(sos.expr.At{j},2))];
            % we're not exploiting symmetry, but I'm not sure that would be possible...
        end
        
        
        % % Finally, implement the actual constraint F(x) = S(x)
        if Fdim==1
            % If F(x) is scalar, S(x) = Z(x)'*C2*Z(x) = c2'*T'*ZZ(x), thus:
            % F(x)-S(x) = (b'-c1'*At)*Zin(x) - c2'*T'*ZZ(x) = 0
            
            [R1,R2,Znew] = findcommonZ(Zin,ZZ); 
            % F(x)-S(x) = (b'-c1'*At)*R1*Znew - c2'*T'*R2*Znew
            %           = ([b*R1;0]' - [c1;c2]'*[At*R1;T'*R2]) * Znew
        
            if isempty(sos.expr.At{i})  % In case Zin is empty? Will this happen?
                sos.expr.At{i} = sparse(size(sos.expr.At{i},1),size(R1,1));
            end
            sos.expr.Z{i} = Znew;               % Update the monomials
            sos.expr.b{i} = R1'*sos.expr.b{i};  % Adjust b to match new monomials
            sos.expr.At{i} = sos.expr.At{i}*R1; % Adjust At to match new monomials
            
            lidx = sos.extravar.idx{var};
            uidx = sos.extravar.idx{var+1}-1;
            sos.expr.At{i}(lidx:uidx,:) = T'*R2; % Add contribution of S(x)
            
        else
            % If F(x) is not scalar, S(x) = sum_ij Cij*[Z(x)]_i*[Z(x)]_j,
            % where Cij is of size Fdim^2.
            
            [R1,R2,Znew] = findcommonZ(Zin,ZZ);
            sos.expr.Z{i} = Znew;               % Update the monomials  
            
            nmons_new = size(Znew,1);
            Zindx = T'*R2*(1:nmons_new)';   % Z'QZ = vec(Q)'(T'*R2)'Znew = vec(Q)^T*Znew(Zindx)
            
            % Initialize new At and b
            Atnew = sparse([],[],[],size(sos.expr.At{i},1),Fdim^2*size(Znew,1),nnz(sos.expr.At{i})+(nmons_Z*Fdim)^2);
            bnew = sparse([],[],[],Fdim^2*size(Znew,1),1,nnz(sos.expr.b{i}));
            
            for j=1:Fdim^2
            % For each of the matrix elements j = sub2ind(k,l), do:
                indx_old = (j-1)*nmons+1:j*nmons;           % Old columns associated to element j
                indx_new = (j-1)*nmons_new+1:j*nmons_new;   % New columns associated to element j
                
                bnew(indx_new) = R1'*sos.expr.b{i}(indx_old);       % Adjust b to match new monomials
                Atnew(:,indx_new) = sos.expr.At{i}(:,indx_old)*R1;  % Adjust At to match new monomials
                
                % What coefficients of S are associated to element j?
                [rindx,cindx] = ind2sub([Fdim,Fdim],j);     % Matrix element k,l
                Crindx = (rindx-1)*nmons_Z + (1:nmons_Z);   % Row indices of associated coefficients of S
                Ccindx = (cindx-1)*nmons_Z + (1:nmons_Z);   % Col indices of associated coefficients of S
                Cindx = reshape(Crindx' + Fdim*nmons_Z*(Ccindx-1),[],1); % Linear indices of coefficients

                % Pair each monomial with the appropriate coefficient
                rindx = sos.extravar.idx{var} + Cindx - 1;  % Rows in At associated to coefficients of S
                cindx = (j-1)*nmons_new + Zindx;            % Columns in At associated to coefficients of S
                Cindx = sub2ind(size(Atnew),rindx,cindx);   % Linear indices of coefficients
                
                % Finally, enforce the constraint M_kl(x) = S_kl(x)
                Atnew(Cindx) = 1;
                
            end
            sos.expr.At{i} = Atnew;     % Update the values of At and b
            sos.expr.b{i} = bnew;
   
        end     
    end
        
else
    % syms and pvar case, though pvar case does not get routed here!
    for i = I
        numstates = size(sos.expr.Z{i},2);%GV&JA 6/12/2013 % number of ind variables
        % Creating extra variables
        maxdeg = full(max(sum(sos.expr.Z{i},2))); % maximum total degree of the monomials
        mindeg = full(min(sum(sos.expr.Z{i},2))); % minimum total degree of the monomials (for matrixvars, this will be at least 2)
        Z = monomials(numstates,[floor(mindeg/2):ceil(maxdeg/2)]); % start with all monomials between min/2 and max/2
        %disp(['Original : ',num2str(size(Z,1))]);
        
        % Discarding unnecessary monomials
        maxdegree = sparse(max(sos.expr.Z{i},[],1)/2); % row of max degrees in each variable
        mindegree = sparse(min(sos.expr.Z{i},[],1)/2); % row of min degrees in each variable
        
        Zdummy1 = bsxfun(@minus,maxdegree,Z); % maxdegree monomial minus each monomial
        Zdummy2 = bsxfun(@minus,Z,mindegree); % each monomial minus mindegree monomial
        [I,~] = find([Zdummy1 Zdummy2]<0); % rows which contain negative terms
        IND = setdiff(1:size(Z,1),I,'stable'); % rows not listed in I
        if isempty(IND) % 02/21/2022 - DJ: Add check in case problem is infeasible
            fprintf(['\n Warning: Inequality constraint ',num2str(i),...
                ' in your sos program structure corresponds to a polynomial that is not sum-of-squares.\n'])
            feasextrasos = 0;   % Indicate the problem is not feasible.
            return
        else
            Z = Z(IND,:);                       % discard all monomials rows listed in I
        end
        % Z is now the monomial vector in the quadratic representation Z^T Q Z
        % as opposed to expr.Z which is all the monomials in the expression
        
        %GV 27/06/2014 - replaced the code below by the above, where the indexes
        %are used to update the matrix of monomials. Matrices maxdegree and
        %mindegree were set to be sparse.
        
        %     Iout = [];indI = 0;%GV 27/06/2014 checking correctness
        %     Z = monomials(numstates,[floor(mindeg/2):ceil(maxdeg/2)]);%GV 27/06/2014 checking correctness
        %     j = 1;
        %     while (j <= size(Z,1))
        %         indI = indI+1;%GV 27/06/2014 checking correctness
        %         Zdummy1 = maxdegree-Z(j,:);
        %         Zdummy2 = Z(j,:)-mindegree;
        %         idx = find([Zdummy1, Zdummy2]<0);
        %         if ~isempty(idx)
        %             Iout = [Iout; indI];%GV 27/06/2014 checking correctness
        %             Z = [Z(1:j-1,:); Z(j+1:end,:)];
        %         else
        %             j = j+1;
        %         end;
        %     end;
        %     sparse(unique(I,'legacy')-Iout)%GV 27/06/2014 checking correctness
        
        
        %disp(['Optimized : ',num2str(size(Z,1))]);
        
        % Convex hull algorithm
        if strcmp(sos.expr.type{i},'sparse')
            Z2 = sos.expr.Z{i}/2;
            Z = inconvhull(full(Z),full(Z2));
            Z = makesparse(Z);
            %disp(['Optimized again : ',num2str(size(Z,1))]);
        end;
        
        if strcmp(sos.expr.type{i},'sparsemultipartite')
            Z2 = sos.expr.Z{i}/2; % lots of fractional degrees
            info2 = sos.expr.multipart{i};%the vectors of independent variables
            sizeinfo2m = length(info2);
            vecindex = [];
            for indm = 1:sizeinfo2m%for each set of independent variables (first true ind, then matrix)
                sizeinfo2n(indm) = length(info2{indm}); % number of variables in cell
                for indn = 1:sizeinfo2n(indm) %scroll through the matrix variables,
                    
                    if isfield(sos,'symvartable')%
                        varcheckindex = find(info2{indm}(indn)==sos.symvartable);
                        if ~isempty(varcheckindex)
                            vecindex{indm}(indn) = varcheckindex;
                        else
                            vecindex{indm}(indn) = length(info2{1})+find(info2{indm}(indn)==sos.varmat.symvartable);%GV&JA 6/12/2013
                        end
                        
                    else
                        % PJS 9/12/13: Update code to handle polynomial objects
                        var = info2{indm}(indn);
                        cvartable = char(sos.varmat.vartable);
                        
                        if ispvar(var)
                            % Convert to string representation
                            var = var.varname;
                        end
                        varcheckindex = find(strcmp(var,sos.vartable));
                        if ~isempty(varcheckindex)
                            vecindex{indm}(indn) = varcheckindex;
                        else
                            vecindex{indm}(indn) = length(info2{1}) + find(strcmp(var,cvartable));
                        end
                        
                        % PJS 9/12/13: Original Code to handle polynomial objects
                        %vecindex{indm}(indn) = find(strcmp(info2{indm}(indn).varname,sos.vartable));
                    end;
                end
            end
            Z = sparsemultipart(full(Z),full(Z2),vecindex);
            Z = makesparse(Z);
        end;
        % Z in the quadratic representation has now been updated.
        
        
        dimp = size(sos.expr.b{i},2); % detecting whether Mineq is active
        
        % Adding slack variables
        sos.extravar.num = sos.extravar.num + 1;
        var = sos.extravar.num;
        sos.extravar.Z{var} = makesparse(Z);
        [T,ZZ] = getconstraint(Z);
        sos.extravar.ZZ{var} = ZZ;
        sos.extravar.T{var} = T'; %Z'QZ=vec(Q)T'ZZ (note the transpose to extravar.T)
        %sos.extravar.idx{var+1} = sos.extravar.idx{var}+size(Z,1)^2;%GVcomment the next slack variable starts in the column i+dim(Z)^2 - the elements of the vectorized square matrix
        
        
        sos.extravar.idx{var+1} = sos.extravar.idx{var}+(size(Z,1)*dimp)^2; % new decision variables associated with this constraint
        for j = 1:sos.expr.num
            sos.expr.At{j} = [sos.expr.At{j}; ...  %padding all the At{i} to make room for the new variables
                sparse(size(sos.extravar.T{var},1)*dimp^2,size(sos.expr.At{j},2))];
        end
        
        ZZ = flipud(ZZ);
        T = flipud(T);
        
        Zcheck = sos.expr.Z{i};
        %this is for the matrix case
        
        
        if dimp==1
            % JFS 6/3/2003: Ensure correct size: % Why? compensating for
            % monomials incorrectly eliminated for sparse or multipartite case?
            pc.Z = sos.extravar.ZZ{var};
            pc.F = -speye(size(pc.Z,1));
            [R1,R2,newZ] = findcommonZ(sos.expr.Z{i},pc.Z);
            % JFS 6/3/2003: Ensure correct size:
            
            if isempty(sos.expr.At{i})
                sos.expr.At{i} = sparse(size(sos.expr.At{i},1),size(R1,1));
            end
            %------------
            sos.expr.At{i} = sos.expr.At{i}*R1;
            lidx = sos.extravar.idx{var};
            uidx = sos.extravar.idx{var+1}-1;
            sos.expr.At{i}(lidx:uidx,:) = sos.expr.At{i}(lidx:uidx,:) - sos.extravar.T{var}*pc.F*R2; % why is expr.At in here? Should be zero...
            sos.expr.b{i} = R1'*sos.expr.b{i};
            sos.expr.Z{i} = newZ;
            
        else
            ZZ = flipud(ZZ); %why? Only matrix case?
            T = flipud(T);  % only matrix case?
            Zcheck = sos.expr.Z{i}; % Zcheck is total list of monomials in the expression
            %this is for the matrix case
            % This is the code for the Mineq option
            [R1,R2,Znew] = findcommonZ(Zcheck,ZZ);
            
            R1 = fliplr(R1);
            R2 = fliplr(R2);
            Znew = flipud(Znew);
            
            R1sum = sum(R1,1);
            T = R2'*T;
            
            ii = 1;
            sig_ZZ = size(ZZ,1);
            sig_Z = size(Z,1);
            sig_Znew = size(Znew,1);
            
            Tf = sparse(dimp^2*sig_Znew,(dimp*sig_Z)^2);
            Sv = sparse(sig_Znew*dimp^2,1);
            for j = 1:sig_Znew
                Mt0 = sparse(dimp,dimp*sig_Z^2);
                for k = 1:sig_Z
                    Mt0(:, (dimp*sig_Z)*(k-1)+1:(dimp*sig_Z)*k) = kron(eye(dimp),T(j,(sig_Z)*(k-1)+1:(sig_Z)*k));
                end
                Tf((j-1)*dimp^2+1:j*dimp^2,:) = kron(eye(dimp),Mt0);
                
                if R1sum(j)==1
                    Sv((j-1)*dimp^2+1:j*dimp^2)= reshape(sos.expr.b{i}( dimp*(ii-1)+1:dimp*ii,:)',dimp^2,1);
                    if ii<size(Zcheck,1)
                        ii = ii+1;
                    end
                else
                    sos.expr.At{i} = [ sos.expr.At{i}(:,1:(j-1)*dimp^2) sparse(size(sos.expr.At{i},1),dimp^2) sos.expr.At{i}(:,(j-1)*dimp^2+1:end)];
                end
            end
            
            lidx = sos.extravar.idx{var};
            uidx = sos.extravar.idx{var+1}-1;
            sos.expr.At{i}(lidx:uidx,:) =  Tf';
            sos.expr.b{i} =  Sv;
            
        end
    end

end

% ====================================================================================
function sos = addextrasosvar2(sos,I)
% Adding slack SOS variables type II
%
numstates = size(sos.expr.Z{1},2);
for i = I
    % Creating extra variable
    maxdeg = full(max(sum(sos.expr.Z{i},2)));
    mindeg = full(min(sum(sos.expr.Z{i},2)));
    Z = monomials(numstates,[floor(mindeg/2):ceil(maxdeg/2)]);
    
    % Discarding unnecessary monomials
    maxdegree = max(sos.expr.Z{i},[],1)/2;
    mindegree = min(sos.expr.Z{i},[],1)/2;
    j = 1;
    while (j <= size(Z,1))
        Zdummy1 = maxdegree-Z(j,:);
        Zdummy2 = Z(j,:)-mindegree;
        idx = find([Zdummy1, Zdummy2]<0);
        if ~isempty(idx)
            Z = [Z(1:j-1,:); Z(j+1:end,:)];
        else
            j = j+1;
        end;
    end;
    
    % Add the variables
    % assumes the symbolic expression has the form f(x)\ge 0 for x\ge 0?
    for k = 0:1 % add two sos constraints
        
        sos.extravar.num = sos.extravar.num + 1;
        var = sos.extravar.num;
        sos.extravar.Z{sos.extravar.num} = makesparse(Z);
        [T,ZZ] = getconstraint(Z);
        sos.extravar.ZZ{var} = ZZ;
        sos.extravar.T{var} = T';
        sos.extravar.idx{var+1} = sos.extravar.idx{var}+size(Z,1)^2;
        for j = 1:sos.expr.num
            sos.expr.At{j} = [sos.expr.At{j}; ...
                sparse(size(sos.extravar.T{var},1),size(sos.expr.At{j},2))];
        end;
        
        % Modifying expression
        degoffset = [k sparse(1,numstates-1)];
        pc.Z = sos.extravar.ZZ{var} + degoffset(ones(size(sos.extravar.ZZ{var},1),1),:);
        pc.F = -speye(size(pc.Z,1));
        [R1,R2,newZ] = findcommonZ(sos.expr.Z{i},pc.Z);
        % JFS 6/3/2003: Ensure correct size:
        if isempty(sos.expr.At{i})
            sos.expr.At{i} = sparse(size(sos.expr.At{i},1),size(R1,1));
        end
        %------------
        sos.expr.At{i} = sos.expr.At{i}*R1;
        lidx = sos.extravar.idx{var};
        uidx = sos.extravar.idx{var+1}-1;
        sos.expr.At{i}(lidx:uidx,:) = sos.expr.At{i}(lidx:uidx,:) - sos.extravar.T{var}*pc.F*R2;
        sos.expr.b{i} = R1'*sos.expr.b{i};
        sos.expr.Z{i} = newZ;
        
        Z = Z(find(Z<maxdegree));    % Discard the unnecessary monomial for the second variable
        
    end;
    
end;

% ====================================================================================
function [At,b,K,RR] = processvars(sos,Atf,bf)
% Processing all variables

% Decision variables
K.s = [];
K.f = sos.var.idx{1}-1;
RR = speye(K.f);


% Polynomial and SOS variables
for i = 1:sos.var.num
    switch sos.var.type{i}
        case 'poly'
            sizeX = sos.var.idx{i+1}-sos.var.idx{i};
            startidx = sos.var.idx{i};
            RR = spantiblkdiag(RR,speye(sizeX));
            K.f = sizeX+K.f;
        case 'sos'
            sizeX = sqrt(sos.var.idx{i+1}-sos.var.idx{i});
            startidx = sos.var.idx{i};
            RR = spblkdiag(RR,speye(sizeX^2));
            K.s = [K.s sizeX];
    end;
end;

for i = 1:sos.extravar.num
    sizeX = sqrt(sos.extravar.idx{i+1}-sos.extravar.idx{i});
    startidx = sos.extravar.idx{i};
    RR = spblkdiag(RR,speye(sizeX^2));
    K.s = [K.s sizeX];
end;

At = RR'*Atf;

b = bf;