function  [rankFR, rankFRV, rankFRvanilla, rankFRVvanilla, model] = checkRankFR(model, printLevel)
% Calculates the rank of `[F R]` and `[F; R]`, when restricted to certain
% rows and columns
%
% USAGE:
%
%    [rankFR, rankFRV, rankFRvanilla, rankFRVvanilla, model] = checkRankFR(model, printLevel)
%
% INPUTS:
%    model:             model structure
%    printLevel:        verbose level
%
% OUTPUTS:
%    rankFR:            rank of [`F R`], when using only `FRrows`
%    rankFRV:           rank of [`F; R`], when using only `FRVcols`
%    rankFRvanilla:     rank of [`F R`], when using all rows
%    rankFRVvanilla:    rank of [`F; R`], when using all cols
%    model:             structure with fields:
%
%                         * .FRrows - `m x 1` boolean of rows of [`F R`] that are nonzero,
%                           unique upto positive scaling and part of the
%                           maximal conservation vector
%                         * .FRVcols - `n x 1` boolean of cols of [`F; R`] that are nonzero,
%                           unique upto positive scaling and part of the
%                           maximal conservation vector
%                         * .FRirows - `m x 1` boolean of rows of [`F R`] that are independent
%                         * .FRdrows - `m x 1` boolean of rows of [`F R`] that are dependent
%                         * .FRwrows - `m x 1` boolean of independent rows of [`F R`] that
%                           have dependent rows amongst `model.FRdrows`
%                         * .FRVdcols - `n x 1` boolean of cols of [`F; R`] that are dependent
%                         * model.SConsistentMetBool - `m x 1` boolean vector indicating metabolites involved
%                           in the maximal consistent vector
%                         * .SConsistentRxnBool - `n x 1` boolean vector indicating metabolites involved
%                           in the maximal consistent vector
%                         * .FRnonZeroBool - `m x 1` boolean vector indicating metabolites involved
%                           in at least one internal reaction
%                         * .FRuniqueBool - `m x 1` boolean vector indicating metabolites with
%                           reaction stoichiometry unique upto scalar multiplication
%                         * .SIntRxnBool - `n x 1` boolean vector indicating the non exchange reactions
%                         * .FRVnonZeroBool - `n x 1` boolean vector indicating non exchange reactions
%                           with at least one metabolite involved
%                         * .FRVuniqueBool - `n x 1` boolean vector indicating non exchange reactions
%                           with stoichiometry unique upto scalar multiplication
%                         * .connectedRowsFRBool - `m x 1` boolean vector indicating metabolites in connected rows of [`F R`]
%                         * .connectedRowsFRVBool - `n x 1` boolean vector indicating complexes in connected columns of [`F; R`]
%                         * .V - :math:`S V = 0`; :math:`1^T |V| > 1` for all flux consistent reactions

if ~exist('printLevel','var')
    printLevel=1;
end

%scaling of stoichiometric matrix may be necessary when badly scaled, off
%by default
scaling='none';
%scaling='gmscal';
%scaling='minimumCoefficient';
switch scaling
    case 'minimumCoefficient'
        %rescale stoichiometric coefficients
        A=abs(model.S);
        minCoefficient=min(min(A(model.S~=0)));
        model.S=model.S*(1000/minCoefficient);
        model.lb=model.lb*(1000/minCoefficient);
        model.ub=model.ub*(1000/minCoefficient);
    case 'gmscal'
        %geometric mean scaling
        [cscale,rscale] = gmscal(model.S,0,0.9);

        % model.S =diag(rscale)*model.S*diag(cscale);
        % model.b =diag(rscale)*model.b;
        % model.lb=diag(cscale)*model.lb;
        % model.ub=diag(cscale)*model.ub;
        % model.c =diag(cscale)*model.c;

        % only scale columns
        model.S =model.S*diag(cscale);
        model.b =model.b;
        model.lb=diag(cscale)*model.lb;
        model.ub=diag(cscale)*model.ub;
        model.c =diag(cscale)*model.c;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%vanilla forward and reverse half stoichiometric matrices
F       = -model.S;
F(F<0)  =    0;
R       =  model.S;
R(R<0)  =    0;

%vanilla ranks
[rankFRvanilla,~,~]      = getRankLUSOL([F R]);
[rankFRVvanilla,~,~]     = getRankLUSOL([F;R]);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[nMet,nRxn]=size(model.S);

if ~isfield(model,'SIntRxnBool')  || ~isfield(model,'SIntMetBool')
    %finds the reactions in the model which export/import from the model
    %boundary i.e. mass unbalanced reactions
    %e.g. Exchange reactions
    %     Demand reactions
    %     Sink reactions
    model = findSExRxnInd(model);
end
model.SIntRxnBool_findSExRxnInd=model.SIntRxnBool;

%mass and charge balance
if isfield(model,'metFormulas')
    [massImbalance,imBalancedMass,imBalancedCharge,imBalancedRxnBool,Elements,missingFormulaeBool,balancedMetBool]...
        = checkMassChargeBalance(model,printLevel);
    model.balancedRxnBool=~imBalancedRxnBool;
    model.balancedMetBool=balancedMetBool;
    model.Elements=Elements;
    model.missingFormulaeBool=missingFormulaeBool;

    if nnz(model.missingFormulaeBool)<size(model.S,1)
        %assumes that all mass imbalanced reations are exchange reactions
        model.SIntRxnBool = model.SIntRxnBool & model.balancedRxnBool;
        model.SIntMetBool = model.SIntMetBool & model.balancedMetBool;
    end
end

%stoichiometric consistency
if ~isfield(model,'SConsistentMetBool') || ~isfield(model,'SConsistentRxnBool')
    %finds the metabolites in the model that are stoichiometrically
    %consistent

    % method.interface='solveCobraLP';
    % method.solver='mosek';
    % method.param.MSK_IPAR_OPTIMIZER='MSK_OPTIMIZER_DUAL_SIMPLEX';
    method.interface='solveCobraLP';
    method.solver='gurobi';
    method.param.Method=1;

    [inform,m,model]=checkStoichiometricConsistency(model,printLevel-1,method);

    %assumes that all stoichiometrically inconsistent reactions are exchange reactions
    model.SIntRxnBool = model.SIntRxnBool & model.SConsistentRxnBool;
    model.SIntMetBool = model.SIntMetBool & model.SConsistentMetBool;
end

if printLevel>0
    fprintf('%u%s\n',nnz(~model.SIntRxnBool),' exchange reactions (imbalanced elementally, or involves a stoichiometrically inconsistent molecular species)')
    for i=1:nRxn
        if ~model.SIntRxnBool(i) && model.SIntRxnBool_findSExRxnInd(i)
            fprintf('%s%s\n',model.rxns{i},': deemed an exchange reaction')
        end
    end
    fprintf('---------\n')
end

if any(~model.SIntMetBool) & printLevel>0
    fprintf('%u%s\n',nnz(~model.SIntMetBool),' rows of S only involved in exchange reactions')
    for i=1:nMet
        if ~model.SIntMetBool(i)
            fprintf('%s%s\n',model.mets{i}',': deemed a molecular species only involved in exchange reactions')
        end
    end
    fprintf('---------\n')
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%metBool1=(model.SConsistentMetBool | ~model.SIntMetBool); %incorrect to
%include mets that are only exchanged
metBool1=model.SConsistentMetBool;
rxnBool1=(model.SConsistentRxnBool | ~model.SIntRxnBool);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%find rows that are not all zero when a subset of reactions omitted
A1=[F(:,rxnBool1) R(:,rxnBool1)];
model.FRnonZeroRowBool1 = any(A1,2);

%find cols that are not all zero when a subset of metabolites omitted
A1=[F(metBool1,:); R(metBool1,:)];
model.FRnonZeroColBool1 = any(A1,1)';

%only report for the consistent rows
if any(~model.FRnonZeroRowBool1 & metBool1) & printLevel>0
    fprintf('%u%s\n',nnz(~model.FRnonZeroRowBool1 & metBool1),' zero rows of [F,R]')
    for i=1:nMet
        if ~model.FRnonZeroRowBool1(i) && metBool1(i)
            fprintf('%s%s\n',model.mets{i},': is a zero row of [F,R]')
        end
    end
    fprintf('\n')
end

%only report for the consistent cols
if any(~model.FRnonZeroColBool1 & rxnBool1) && 0
    fprintf('%u%s\n',nnz(~model.FRnonZeroColBool1 & rxnBool1),' zero cols of consistent [F;R]')
    for i=1:nRxn
        if ~model.FRnonZeroColBool1(i) && rxnBool1(i)
            fprintf('%s%s\n',model.rxns{i},': is a zero col of consistent [F;R]')
        end
    end
    fprintf('\n')
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
A=[F,R];%depends on the direction of reaction

%detect the rows of A that are identical upto scalar multiplication
%divide each row by the sum of each row.
sumA2            = sum(A,2);
sumA2(sumA2==0) = 1;
normalA2          = diag(1./sumA2)*A;

%get unique rows, but do not change the order
% [C,IA,IC] = unique(A,'rows') also returns index vectors IA and IC such
% that C = A(IA,:) and A = C(IC,:).
[uniqueRowsA,IA,IC] = unique(normalA2,'rows','stable');
model.FRuniqueRowBool=zeros(nMet,1);
model.FRuniqueRowBool(IA)=1;

if any(~model.FRuniqueRowBool & metBool1) & printLevel>0
    fprintf('%u%s\n',nnz(~model.FRuniqueRowBool & metBool1),' non-unique rows of stoich consistent [F,R]')
    for i=1:nMet
        if ~model.FRuniqueRowBool(i) && metBool1(i)
            fprintf('%s%s\n',model.mets{i},': not unique row of stoich consistent [F,R]')
        end
    end
    fprintf('\n')
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
A=F+R;%invariant to direction of reaction

%detect the cols of A that are identical upto scalar multiplication
%divide each col by the sum of each row.
sumA1          = sum(A,1);
sumA1(sumA1==0)  = 1;
normalA1          = A*diag(1./sumA1);

%get unique cols, but do not change the order
% [C,IA,IC] = unique(A,'rows') also returns index vectors IA and IC such
% that C = A(IA,:) and A = C(IC,:).
[uniqueColsA,IA,IC] = unique(normalA1','rows','stable');
model.FRuniqueColBool=zeros(nRxn,1);
model.FRuniqueColBool(IA)=1;

if any(~model.FRuniqueColBool & rxnBool1) & printLevel>0
    fprintf('%u%s\n',nnz(~model.FRuniqueColBool & rxnBool1),' non-unique cols of stoich consistent [F;R]')
    for i=1:nRxn
        if ~model.FRuniqueColBool(i) && rxnBool1(i)
            fprintf('%s%s\n',model.rxns{i},': not unique col of stoich consistent [F;R]')
        end
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if printLevel>0
    fprintf('\n%s\n','Diagnostics on size of boolean vectors for rows:')
    fprintf('%s%u\n','SIntMetBool:                 ',nnz(model.SIntMetBool))
    fprintf('%s%u\n','SConsistentMetBool:          ',nnz(model.SConsistentMetBool))
    fprintf('%s%u\n','FRnonZeroRowBool1:               ',nnz(model.FRnonZeroRowBool1))
    fprintf('%s%u\n','FRuniqueRowBool:               ',nnz(model.FRuniqueRowBool))

    fprintf('\n%s\n','Diagnostics on size of boolean vectors for cols:')
    fprintf('%s%u\n','SIntRxnBool:                 ',nnz(model.SIntRxnBool))
    fprintf('%s%u\n','SConsistentRxnBool:          ',nnz(model.SConsistentRxnBool))
    fprintf('%s%u\n','FRnonzeroColBool:               ',nnz(model.FRnonZeroColBool1))
    fprintf('%s%u\n','FRuniqueColBool:               ',nnz(model.FRuniqueColBool))
    fprintf('\n')
end

if ~isempty(find(strcmp(model.mets(~model.SConsistentMetBool),'h[c]')))
    warning('h[c] is one of the inconsistent molecules')
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
metBool2=model.SConsistentMetBool & model.FRuniqueRowBool & model.FRnonZeroRowBool1;

% rxnBool2=(model.SConsistentRxnBool | ~model.SIntRxnBool);
rxnBool2=(model.SConsistentRxnBool | ~model.SIntRxnBool) & model.FRuniqueColBool;

%the uniqueness check has to be done before flux consistency since some
%models have a forward and backward reaction in separately, but these are
%the same reaction when both considered reversible.

%from the unique exchange reactions, and the subset of the unique non-exchange
%reactions that form a matrix with stoichiometrically consistent rows,
%check for reactions that are also flux consistent, with all reactions
%reversible
modelRev.S = model.S(metBool2,rxnBool2);

%infinite bounds may be necessary when stoichiometric matrix is badly
%scaled, in that case the minimum flux required (epsilon) is set to 1, the
%default lower and upper bounds are {-1000, 1000}, with epsilon getCobraSolverParams('LP', 'feasTol')*100,
%as that is two orders of magnitude higher than the typical feasibility
%tolerance for volation of mass balance constraints.
fluxConsistencyBounds='aThousand';
switch fluxConsistencyBounds
    case 'aThousand'
        %use all reactions reversible
        modelRev.lb=-1000*ones(size(modelRev.S,2),1);
        modelRev.ub= 1000*ones(size(modelRev.S,2),1);
        epsilon = getCobraSolverParams('LP', 'feasTol')*100;
    case 'infinity'
        %use all reactions reversible
        modelRev.lb=-inf*ones(size(modelRev.S,2),1);
        modelRev.ub= inf*ones(size(modelRev.S,2),1);
        epsilon = 1;
    case 'reconstruction'
        %use reconstruction reactions
        modelRev.lb=model.lb(rxnBool2);
        modelRev.ub=model.ub(rxnBool2);
        epsilon = getCobraSolverParams('LP', 'feasTol')*100;
end
modelRev.mets=model.mets(metBool2);
modelRev.rxns=model.rxns(rxnBool2);

%fast consistency check code from Nikos Vlassis et al
modeFlag=1;
[indFluxConsist,~,V0]=fastcc(modelRev,epsilon,printLevel-1,modeFlag);
modelRev.fluxConsistentRxnBool=false(size(modelRev.S,2),1);
modelRev.fluxConsistentRxnBool(indFluxConsist)=1;

%build the vector the same size as the original S
model.fluxConsistentRxnBool=false(nRxn,1);
%assign the boolean state to the original size model
model.fluxConsistentRxnBool(rxnBool2)=modelRev.fluxConsistentRxnBool;

%pad out V0 to correspond to the original size of S
model.V=sparse(nRxn,size(V0,2));
model.V(rxnBool2,:)=V0;

%check to make sure booleans are correct
if nnz(model.SConsistentRxnBool & ~model.SIntRxnBool)~=0
    error('No exchange reaction should be stoichiometrically consistent')
end

%check to make sure that V in nullspace of S.
tmp=norm(full(model.S(metBool2,model.SConsistentRxnBool & model.SIntRxnBool)*model.V(model.SConsistentRxnBool & model.SIntRxnBool,:)...
       + model.S(metBool2,~model.SIntRxnBool)*model.V(~model.SIntRxnBool,:)));
%two extra digits spare
if tmp>100*epsilon
    disp(tmp)
    error('Not flux consistent')
end
%create a stoichiometric matrix of perpetireactions only for
%stoichiometrically and flux consistent part
model.P=sparse(nMet,size(V0,2));
model.P(metBool2,:)=model.S(metBool2,~model.SIntRxnBool)*model.V(~model.SIntRxnBool,:);

% model.fluxConsistentRxnBool=true(nRxn,1);

%rows corresponding to flux consistent reactions
model.fluxConsistentMetBool = sum(model.S(:,model.fluxConsistentRxnBool)~=0,2)~=0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%eliminate the metabolites and reactions that are stoichiometrically
%inconsistent or flux inconsistent from further consideration, but keep the
%flux consistent exchange reactions, also eliminate scalar multiples
metBool3=model.SConsistentMetBool & model.fluxConsistentMetBool & model.FRuniqueRowBool & model.FRnonZeroRowBool1;
rxnBool3=(model.SConsistentRxnBool | ~model.SIntRxnBool)  & model.FRuniqueColBool & model.fluxConsistentRxnBool;

%find rows that are not all zero when a subset of reactions omitted
A3=[F(:,rxnBool3) R(:,rxnBool3)];
model.FRnonZeroRowBool = any(A3,2);

%find cols that are not all zero when a subset of metabolites omitted
A3=[F(metBool3,:); R(metBool3,:)];
model.FRnonZeroColBool = any(A3,1)';

%only report for the latest subset of rows
if any(~model.FRnonZeroRowBool & metBool3) & printLevel>0
    fprintf('%u%s\n',nnz(~model.FRnonZeroRowBool & metBool3),' zero rows of [F,R]')
    for i=1:nMet
        if ~model.FRnonZeroRowBool(i) && metBool3(i)
            fprintf('%s%s\n',model.mets{i},': is a zero row of [F,R]')
        end
    end
    fprintf('\n')
end

%only report for the latest subset of cols
if any(~model.FRnonZeroColBool & rxnBool3) && 0
    fprintf('%u%s\n',nnz(~model.FRnonZeroColBool & rxnBool3),' zero cols of consistent [F;R]')
    for i=1:nRxn
        if ~model.FRnonZeroColBool(i) && rxnBool3(i)
            fprintf('%s%s\n',model.rxns{i},': is a zero col of consistent [F;R]')
        end
    end
    fprintf('\n')
end

%pause(eps)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% A4=F(:,rxnBool3)+R(:,rxnBool3);%invariant to direction of reaction
%
% %detect the cols of A that are identical upto scalar multiplication
% %divide each col by the sum of each row.
% sumA4             = sum(A4,1);
% sumA4(sumA4==0)   = 1;
% normalA4          = A4*diag(1./sumA4);
%
% %get unique cols, but do not change the order
% % [C,IA,IC] = unique(A,'rows') also returns index vectors IA and IC such
% % that C = A(IA,:) and A = C(IC,:).
% [uniqueColsA,IA,IC] = unique(normalA4','rows','stable');
% model.FRuniqueColBool2=zeros(nRxn,1);
% model.FRuniqueColBool2(IA)=1;
%
% if any(~model.FRuniqueColBool2 & rxnBool3)
%     fprintf('%u%s\n',nnz(~model.FRuniqueColBool2 & rxnBool3),' non-unique cols of consistent [F;R]')
%     for i=1:nRxn
%         if ~model.FRuniqueColBool(i) && rxnBool1(i)
%             fprintf('%s%s\n',model.rxns{i},': not unique col of consistent [F;R]')
%         end
%     end
% end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% The largest connected component - not necessary, flux consistency takes
% care of this connected rows of [F,R] and columns of [F;R]
% [model.connectedRowsFRBool,model.connectedColsFRVBool]=connectedFR(F,R);
% [~,maxInd]=max(sum(model.connectedRowsFRBool,1));
% model.largestConnectedRowsFRBool=model.connectedRowsFRBool(:,maxInd);
% [~,maxInd]=max(sum(model.connectedColsFRVBool,1));
% model.largestConnectedColsFRVBool=model.connectedColsFRVBool(:,maxInd);

%bypass test for connectedness
model.largestConnectedRowsFRBool=true(nMet,1);
model.largestConnectedColsFRVBool=true(nRxn,1);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%diagnostics
if printLevel>0
    fprintf('\n%s\n','Diagnostics on size of boolean vectors for rows:')
    fprintf('%s%u\n','SConsistentMetBool:         ',nnz(model.SConsistentMetBool))
    fprintf('%s%u\n','fluxConsistentMetBool:      ',nnz(model.fluxConsistentMetBool))
    fprintf('%s%u\n','FRuniqueRowBool:               ',nnz(model.FRuniqueRowBool))
    fprintf('%s%u\n','FRnonZeroRowBool1:              ',nnz(model.FRnonZeroRowBool1))
    fprintf('%s%u\n','FRnonZeroRowBool:              ',nnz(model.FRnonZeroRowBool))
   % fprintf('%s%u\n','largestConnectedRowsFRBool: ',nnz(model.largestConnectedRowsFRBool))

    fprintf('\n%s\n','Diagnostics on size of boolean vectors for cols:')
    fprintf('%s%u\n','SIntRxnBool:                 ',nnz(model.SIntRxnBool))
    fprintf('%s%u\n','SConsistentRxnBool:          ',nnz(model.SConsistentRxnBool))
    fprintf('%s%u\n','fluxConsistentRxnBool:       ',nnz(model.fluxConsistentRxnBool))
    fprintf('%s%u\n','FRuniqueColBool:               ',nnz(model.FRuniqueColBool))
    fprintf('%s%u\n','FRnonZeroColBool1:              ',nnz(model.FRnonZeroColBool1))
    fprintf('%s%u\n','FRnonZeroColBool:              ',nnz(model.FRnonZeroColBool))
    %fprintf('%s%u\n','largestConnectedColsFRVBool: ',nnz(model.largestConnectedColsFRVBool))
    fprintf('\n')
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%only use rows that are nonzero, unique upto positive scaling, part of
%the maximal conservation vector, and part of the largest component
selection='c';
switch selection
    case 'a'
        model.FRrows = model.SConsistentMetBool...
            & model.fluxConsistentMetBool...
            & model.FRnonZeroRowBool...
            & model.FRuniqueRowBool;
        model.FRVcols = (model.SConsistentRxnBool | ~model.SIntRxnBool)...
            & model.fluxConsistentRxnBool...
            & model.FRuniqueColBool...
            & model.FRnonZeroColBool;
    case 'b'
        %exchange reactions not considered when rank([F,R]) measured
        model.FRrows = model.SConsistentMetBool...
            & model.fluxConsistentMetBool...
            & model.FRnonZeroRowBool...
            & model.FRuniqueRowBool;
        model.FRVcols = model.SConsistentRxnBool...
            & model.fluxConsistentRxnBool...
            & model.FRuniqueColBool...
            & model.FRnonZeroColBool;
    case 'c'
        model.FRrows = model.SConsistentMetBool...
            & model.fluxConsistentMetBool...
            & model.FRnonZeroRowBool1...
            & model.FRnonZeroRowBool...
            & model.FRuniqueRowBool;
        model.FRVcols = (model.SConsistentRxnBool | ~model.SIntRxnBool)...
            & model.fluxConsistentRxnBool...
            & model.FRuniqueColBool...
            & model.FRnonZeroColBool;
    case 'd'
        model.FRrows = model.SConsistentMetBool...
            & model.fluxConsistentMetBool...
            & model.FRnonZeroRowBool1...
            & model.FRnonZeroRowBool...
            & model.FRuniqueRowBool;
        model.FRVcols = (model.SConsistentRxnBool | ~model.SIntRxnBool)...
            & model.fluxConsistentRxnBool;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%omit both the rows and the columns as specified above
Fr = F(model.FRrows,model.FRVcols);
Rr = R(model.FRrows,model.FRVcols);
Fc = F(model.FRrows,model.FRVcols);
Rc = R(model.FRrows,model.FRVcols);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% method.interface='solveCobraLP';
% method.solver='mosek';
% method.param.MSK_IPAR_OPTIMIZER='MSK_OPTIMIZER_DUAL_SIMPLEX';

method.interface='solveCobraLP';
method.solver='gurobi5';
method.param.Method=1;

modelCheck.S = Rr-Fr;
modelCheck.SIntRxnBool=model.SIntRxnBool;
modelCheck.SIntMetBool=model.SIntMetBool(model.FRrows);
[informC,mC,~]=checkStoichiometricConsistency(modelCheck,0,method);
%save check on consistency
model.mC=mC;
if informC~=1
    disp(length(mC)-nnz(mC))
    error('(R-F) not stoichiometrically consistent')
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% [A,B,C]=bilinearDecomposition(model.S(model.FRrows,model.FRVcols));
% %forward and reverse half stoichiometric matrices
% Fb        =   -B;
% Fb(Fb<0)  =    0;
% Rb        =    B;
% Rb(Rb<0)  =    0;
% Fb(Fb~=0) =    1;
% Rb(Rb~=0) =    1;
% adj = inc2adj([Fb Rb]);
%
% % Test whether a graph is bipartite, if yes, return the two vertex sets
% % Inputs: graph in the form of adjancency list (neighbor list, see adj2adjL.m)
% % Outputs: True/False (boolean), empty set (if False) or two sets of vertices
% tic
% [isit,partA,partB]=isbipartite(adj2adjL(adj));
% toc
% %pause(eps)

%check rank of bilinear version
[A,B,C]=bilinearDecomposition(Fr-Rr);
%forward and reverse half stoichiometric matrices
Frb        =   -B;
Frb(Frb<0)  =    0;
Rrb        =    B;
Rrb(Rrb<0)  =    0;

%rank of [F R]
[rankBilinearFrRr,bilinearFrRrp,bilinearFrRrq] = getRankLUSOL([Frb Rrb]);
model.Frb=Frb;
model.Rrb=Rrb;
model.rankBilinearFrRr=rankBilinearFrRr;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%rank of [F R]
[rankFR,FRp,FRq] = getRankLUSOL([Fr Rr]);

%indices of rows that are dependent
ind=find(model.FRrows);
iR=ind(FRp(1:rankFR));
dR=ind(FRp(rankFR+1:length(FRp)));
model.FRdrows=false(nMet,1);
model.FRdrows(dR)=1;
model.FRirows=false(nMet,1);
model.FRirows(iR)=1;
model.FRq=FRq;
model.FRp=FRp;
model.Fr=Fr;
model.Rr=Rr;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%rank of [F;R]
[rankFRV,FRVp,FRVq]  = getRankLUSOL([Fc;Rc]);

%boolean of non-exchange columns that are dependent
vertInd=find(model.FRVcols);
iC=vertInd(FRVq(1:rankFRV));
dC=vertInd(FRVq(rankFRV+1:length(FRVq)));
model.FRVdcols=false(nRxn,1);
model.FRVdcols(dC)=1;
model.FRVicols=false(nRxn,1);
model.FRVicols(iC)=1;
model.FRVq=FRVq;
model.FRVp=FRVp;
model.Fc=Fc;
model.Rc=Rc;

%sanity check
if nnz(model.FRrows) ~= nnz(model.FRrows & model.FRdrows)
    error('')
end
if nnz(model.FRVcols) ~= nnz(model.FRVcols & model.FRVdcols)
    error('')
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%examine the dependencies between rows and columns if any
model.FRrowRankDeficiency=nnz(model.FRrows)-rankFR;
if model.FRrowRankDeficiency>0
    T  = [Fr Rr];
    %code from Michael
    C1 = T(FRp(1:rankFR),:);
    C2 = T(FRp(rankFR+1:length(FRp)),:);
    W0 = C1' \ C2'; % solves LS problems min ||C1'*W_k - C2_k'||

    model.FRW=sparse(model.FRrowRankDeficiency,nMet);
    model.FRW(:,iR)=W0';%L
    model.FRW(:,dR)=-speye(model.FRrowRankDeficiency);

    %indices of independent rows that other rows are dependent on
    wR=find(sum(abs(model.FRW),1)>0)';
    model.FRwrows=false(nMet,1);
    model.FRwrows(wR)=1;

    %sanity checks
    disp(norm(C1'*W0 - C2',inf)) %should be zero
    disp(norm([W0',-speye(model.FRrowRankDeficiency)]*[C1;C2],inf)) %should be zero

    W = sparse(model.FRrowRankDeficiency,size([Fr Rr],1));
    W(:,FRp(1:rankFR))=W0';
    W(:,FRp(rankFR+1:length(FRp)))=-speye(model.FRrowRankDeficiency);
    disp(norm(W*[Fr Rr],inf)) %should be zero

    model.FRW=sparse(model.FRrowRankDeficiency,nMet);
    model.FRW(:,model.FRrows)=W;
    disp(norm(model.FRW*[F(:,model.FRVcols), R(:,model.FRVcols)],inf)) %should be zero
    %%%%% N B %%%%
    disp(norm(model.FRW*[F,R],inf)) % will not in general be zero as other columns of [F R] could contribute

    % nnz(W)
    % figure; spy(W)
    % figure; spy(abs(W) > 1e-3)  % say
end

model.FRcolRankDeficiency=nnz(model.FRVcols)-rankFRV;
if model.FRcolRankDeficiency>0
    VT  = [Fc;Rc];
    %code from Michael
    VC1  = VT(:,model.FRVq(1:rankFRV)); % independent columns
    VC2  = VT(:,model.FRVq(rankFRV+1:size(VT,2))); % dependent columns
    VW0  = VC1 \ VC2; % solves LS problems min ||VC1*WV_k - VC2_k||

    model.FRVW=sparse(nRxn,model.FRcolRankDeficiency);
    model.FRVW(iC,:)=VW0; % independent columns
    model.FRVW(dC,:)=-speye(model.FRcolRankDeficiency); % dependent columns

    %indices of independent cols that other cols are dependent on
    wC=find(sum(abs(model.FRVW),2)>0);
    model.FRVwcols=false(nRxn,1);
    model.FRVwcols(wC)=1;

    if printLevel>2
        %sanity check
        disp(norm([F(model.FRrows,:); R(model.FRrows,:)]*model.FRVW,inf))%should be zero
        %%%%% N B %%%%
        disp(norm([F;R]*model.FRVW,inf)) % will not in general be zero as other rows [F;R] could contribute
    end

    % nnz(VW0)
    % figure; spy(VW0)
    % figure; spy(abs(VW0) > 1e-3)  % say
end


function fem = update1320nbr(fem_struct,jj)
% update1320nbr takes a node with connectivity between 13 and 20
%  and adds six new nodes and uses delanunay trianglulation to
%  create the new elements. The region is then sprung. An nei is
%  not required but if one is not present, it will be generated.
%
%  Variables
%  fem_struct -- finite element structure from opnml.
%  jj -- the node number that has 12 elements connected to it.
%  fem -- the updated finite element structure.
%        Note:  Only fem.x,fem.y,fem.z,fem.e, and fem.nei
%               get updated.  fem.bnd does not need to be updated.
%
%  Usage -- fem = update1320nbr(fem_struct,jj)
%
%  FileName: update1320nbr.m
%  Written by: Ben Holladay (SEAP Student 2004)
%  Date: July 7,2004
%  Modified:  August 30, 2004 -- Chris Massey
%             June 14,2005 -- Ben Holladay
%                   Fix: Ensure that delaunay doesn't create elements
%                   outside the patch.
% Last Modifed:
%         Sept. 19, 2006 -- Chris Massey, NRL Code 7322, S.S.C. MS 39529
%                      % Add test to make sure the correct nodal neighbor
%                        connectivity existed for the requested update
%         July 14, 2008 -- Chris Massey, NRL Code 7322
%               moved test for 4 neighbors to after the test for nei.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%Checks if the fem_struct has an nei. If not one is generated.
try
    nei = fem_struct.nei;
catch
    fem_struct.nei = ele2nei(fem_struct.e,fem_struct.x,fem_struct.y);
    nei = fem_struct.nei;
end

tempnc = sum(~ismember((fem_struct.nei(jj,:)),0));
if (tempnc < 13 | tempnc > 20)
    error(['There are ',num2str(tempnc),' nodal neighbors for node number ',...
           num2str(jj),'. This routine only works for 13-20 nodal neighbors.']);
end

%Sets the intial fem_struct variables.
x = fem_struct.x;
y = fem_struct.y;
z = fem_struct.z;
enodes = fem_struct.e;

%Adds new nodes,determines connectivity, and labels the bad node,
%neighboring nodesm and neighboring elements.
badnode = jj;
[nbrelem,~] = find(enodes == badnode);
[nnodes,trnc] = size(nei);
temp1 = sum(ismember(nei(jj,:),0));
nc = trnc - temp1;
nbrnode = nei(badnode,1:nc);
newnode = nnodes + [1:6];
vod = ([1:nc,1:nc]);
inrad = max(sqrt((x(nbrnode)-x(badnode)).^2 + (y(nbrnode)-y(badnode)).^2));

%Computes the center angle of each element.
b2 = (x(nbrnode(:))-x(badnode)).^2 + (y(nbrnode(:))-y(badnode)).^2;
a2 = (x(nbrnode(:))-x(nbrnode([2:nc,1]))).^2 + (y(nbrnode(:))-y(nbrnode([2:nc,1]))).^2;
A = (180/pi)*acos((b2+b2([2:nc,1])-a2)./(2*sqrt(b2).*sqrt(b2([2:nc,1]))));
ang = A;

%Sets the patterns for how to add the nodes.
addnode(1,:) = ([2,2,2,2,2,3]);
addnode(2,:) = ([2,2,3,2,2,3]);
addnode(3,:) = ([2,3,2,3,2,3]);
addnode(4,:) = ([3,3,2,3,3,2]);
addnode(5,:) = ([2,3,3,3,3,3]);
addnode(6,:) = ([3,3,3,3,3,3]);
addnode(7,:) = ([3,3,3,3,3,4]);
addnode(8,:) = ([3,3,4,3,3,4]);

%Determines the appropriate pattern for adding nodes
addnodepat = addnode((nc - 12),:);

%Labels reference nodes to be use to determine the new nodes locations.
spb(1) = 1;
spb(2) = spb(1) + addnodepat(1);
spb(3) = spb(2) + addnodepat(2);
spb(4) = spb(3) + addnodepat(3);
spb(5) = spb(4) + addnodepat(4);
spb(6) = spb(5) + addnodepat(5);

%Determines the total angle that each node will cover.
testang(1) = sum([ang([spb(1):(spb(2)-1)])]);
testang(2) = sum([ang([spb(2):(spb(3)-1)])]);
testang(3) = sum([ang([spb(3):(spb(4)-1)])]);
testang(4) = sum([ang([spb(4):(spb(5)-1)])]);
testang(5) = sum([ang([spb(5):(spb(6)-1)])]);
testang(6) = sum([ang([spb(6):nc])]);

%Test to make sure no node is trying to cover more than half of the
%circle.
test = find(testang > 180);
while ~isempty(test)
    hightemp = find(testang > 180);
    hhtempplus = hightemp + 1;
    if hhtempplus == 7; break; end
    spb(vod(hhtempplus)) = spb(vod(hhtempplus)) - 1;
    testang(1) = sum([ang([spb(1):(spb(2)-1)])]);
    testang(2) = sum([ang([spb(2):(spb(3)-1)])]);
    testang(3) = sum([ang([spb(3):(spb(4)-1)])]);
    testang(4) = sum([ang([spb(4):(spb(5)-1)])]);
    testang(5) = sum([ang([spb(5):(spb(6)-1)])]);
    testang(6) = sum([ang([spb(6):nc])]);
    test = find(testang > 180);
end

%First guess of the new coordinates and bathymetry.
M(1,1) = sum(x([(nbrnode([spb(1):spb(2)])),badnode,badnode]));
M(1,2) = sum(y([(nbrnode([spb(1):spb(2)])),badnode,badnode]));
M(1,3) = sum(z([(nbrnode([spb(1):spb(2)])),badnode,badnode]));

M(2,1) = sum(x([(nbrnode([spb(2):spb(3)])),badnode,badnode]));
M(2,2) = sum(y([(nbrnode([spb(2):spb(3)])),badnode,badnode]));
M(2,3) = sum(z([(nbrnode([spb(2):spb(3)])),badnode,badnode]));

M(3,1) = sum(x([(nbrnode([spb(3):spb(4)])),badnode,badnode]));
M(3,2) = sum(y([(nbrnode([spb(3):spb(4)])),badnode,badnode]));
M(3,3) = sum(z([(nbrnode([spb(3):spb(4)])),badnode,badnode]));

M(4,1) = sum(x([(nbrnode([spb(4):spb(5)])),badnode,badnode]));
M(4,2) = sum(y([(nbrnode([spb(4):spb(5)])),badnode,badnode]));
M(4,3) = sum(z([(nbrnode([spb(4):spb(5)])),badnode,badnode]));

M(5,1) = sum(x([(nbrnode([spb(5):spb(6)])),badnode,badnode]));
M(5,2) = sum(y([(nbrnode([spb(5):spb(6)])),badnode,badnode]));
M(5,3) = sum(z([(nbrnode([spb(5):spb(6)])),badnode,badnode]));

M(6,1) = sum(x([(nbrnode([spb(6):nc,1])),badnode,badnode]));
M(6,2) = sum(y([(nbrnode([spb(6):nc,1])),badnode,badnode]));
M(6,3) = sum(z([(nbrnode([spb(6):nc,1])),badnode,badnode]));

M(7,1) = x(badnode);
M(7,2) = y(badnode);
M(7,3) = z(badnode);

M(1,:) = M(1,:) ./ (addnodepat(1) + 3);
M(2,:) = M(2,:) ./ (addnodepat(2) + 3);
M(3,:) = M(3,:) ./ (addnodepat(3) + 3);
M(4,:) = M(4,:) ./ (addnodepat(4) + 3);
M(5,:) = M(5,:) ./ (addnodepat(5) + 3);
M(6,:) = M(6,:) ./ (addnodepat(6) + 3);

x([([newnode(1:6)]),badnode]) = ([M(:,1)]);
y([([newnode(1:6)]),badnode]) = ([M(:,2)]);
z([([newnode(1:6)]),badnode]) = ([M(:,3)]);

%Uses delaunay trianglulation to created a new and updated element list.
tempx = x([badnode,newnode(:)',nbrnode(:)']);
tempy = y([badnode,newnode(:)',nbrnode(:)']);
tri = delaunay(tempx,tempy);
tri2 = tri;

%Convertst the local element list from tri back to its global numberiing.
for i = 1:nc+7
    temp = find(i == tri);
    tmp = ([badnode,newnode(:)',nbrnode(:)']);
    tri2(temp) = tmp(i);
end

%Checks if the elements are in CCW order.
xnodes = x(tri2);
ynodes = y(tri2);
temparea = 0.5*(xnodes(:,1).*(ynodes(:,2)-ynodes(:,3))+xnodes(:,2).*...
    (ynodes(:,3)-ynodes(:,1))+xnodes(:,3).*(ynodes(:,1)-ynodes(:,2)));
Itri = find(temparea < 0);
tri3 = tri2;
tri3(Itri,2) = tri2(Itri,3);
tri3(Itri,3) = tri2(Itri,2);

%Removes elements that would be created outside the patch.
%!!MODIFICATION!!
oldbnd = detbndy(enodes(nbrelem,:));
newbnd = detbndy(tri3);
oldbnd = sort(oldbnd,2);
newbnd = sort(newbnd,2);
temp11 = find(ismember(newbnd,oldbnd,'rows') ~= 1);
while isempty(temp11) == 0
    newbnd = detbndy(tri3);
    newbnd = sort(newbnd,2);
    temp11 = find(ismember(newbnd,oldbnd,'rows') ~= 1);
    temp12 = newbnd(temp11,:);
    for it = 1:size(temp12,1)
        [tr,tc] = find(tri3 == temp12(it,1) | tri3 == temp12(it,2));
        [ta,tb,tc] = unique(tr);
        temp13 = setdiff(1:length(tr),tb);
        temp14 = tr(temp13);
        tri3(temp14,:) = [];
    end
end

%Finds and throws out bad elements and adds new elements to the global list.
tempa = setdiff(1:1:length(enodes),nbrelem);
enodes = enodes(tempa,1:3);
nelems = size(enodes,1);
tri3r = size(tri3,1);
enodes([(nelems + 1):(nelems + tri3r)],:) = tri3;

%Springs the region.
stoptol = 10e-8 * inrad;
tol = stoptol + 10;
i = 1;
imax = 20;
while i <= imax && tol > stoptol
    M2(7,1) = sum([M(2,1),M(1,1),M(3,1),M(5,1),M(4,1),M(6,1)]);
    M2(7,2) = sum([M(2,2),M(1,2),M(3,2),M(5,2),M(4,2),M(6,2)]);
    M2(7,3) = sum([M(2,3),M(1,3),M(3,3),M(5,3),M(4,3),M(6,3)]);

    [r1,c1] = find(enodes == newnode(1));
    nbr1 = unique(enodes(r1,:));
    temp1 = find(nbr1 == newnode(1));
    nbr1(temp1) = [];
	M2(1,1) = sum(x([nbr1]));
    M2(1,2) = sum(y([nbr1]));
    M2(1,3) = sum(z([nbr1]));

    [r2,c] = find(enodes == newnode(2));
    nbr2 = unique(enodes(r2,:));
    temp2 = find(nbr2 == newnode(2));
    nbr2(temp2) = [];
	M2(2,1) = sum(x([nbr2]));
    M2(2,2) = sum(y([nbr2]));
    M2(2,3) = sum(z([nbr2]));

    [r3,c3] = find(enodes == newnode(3));
    nbr3 = unique(enodes(r3,:));
    temp3 = find(nbr3 == newnode(3));
    nbr3(temp3) = [];
	M2(3,1) = sum(x([nbr3]));
    M2(3,2) = sum(y([nbr3]));
    M2(3,3) = sum(z([nbr3]));

    [r4,c4] = find(enodes == newnode(4));
    nbr4 = unique(enodes(r4,:));
    temp4 = find(nbr4 == newnode(4));
    nbr4(temp4) = [];
	M2(4,1) = sum(x([nbr4]));
    M2(4,2) = sum(y([nbr4]));
    M2(4,3) = sum(z([nbr4]));

    [r5,c5] = find(enodes == newnode(5));
    nbr5 = unique(enodes(r5,:));
    temp5 = find(nbr5 == newnode(5));
    nbr5(temp5) = [];
	M2(5,1) = sum(x([nbr5]));
    M2(5,2) = sum(y([nbr5]));
    M2(5,3) = sum(z([nbr5]));

	[r6,c6] = find(enodes == newnode(6));
    nbr6 = unique(enodes(r6,:));
    temp6 = find(nbr6 == newnode(6));
    nbr6(temp6) = [];
	M2(6,1) = sum(x([nbr6]));
    M2(6,2) = sum(y([nbr6]));
    M2(6,3) = sum(z([nbr6]));

    M2(1,:) = M2(1,:) ./ (size((nbr1),1));
    M2(2,:) = M2(2,:) ./ (size((nbr2),1));
    M2(3,:) = M2(3,:) ./ (size((nbr3),1));
    M2(4,:) = M2(4,:) ./ (size((nbr4),1));
    M2(5,:) = M2(5,:) ./ (size((nbr5),1));
    M2(6,:) = M2(6,:) ./ (size((nbr6),1));
    M2(7,:) = M2(7,:) ./ 6;

	tol = max(sqrt((M2(:,1)-M(:,1)).^2 + (M2(:,2)-M(:,2)).^2));

    M = M2;
    x([([newnode(1:6)]),badnode]) = ([M(:,1)]);
    y([([newnode(1:6)]),badnode]) = ([M(:,2)]);
    z([([newnode(1:6)]),badnode]) = ([M(:,3)]);
    i = i + 1;
end

%Updates the nei for the region.
tempnode = ([newnode,badnode,nbrnode]);
tempcount = size(tempnode,2);
for i = 1:tempcount
    [tr,tc] = find(tempnode(i) == enodes);
    tempnbr = unique(enodes(tr,:));
    tmp1 = find(tempnbr == tempnode(i));
    tempnbr(tmp1) = [];
    tempnbrx = x(tempnbr) - x(tempnode(i));
    tempnbry = y(tempnbr) - y(tempnode(i));
    angle = (atan2(tempnbry,tempnbrx))*(180/pi);
    tempang = find(angle < 0);
    angle(tempang) = angle(tempang) + 360;
    [tmp2,tempi] = sort(angle);
    tempnbr1 = tempnbr(tempi);
    tmpr = size(tempnbr,1);
    nei(tempnode(i),:) = ([tempnbr1(1:tmpr)',zeros(1,trnc-tmpr)]);
end

%Createst the output structure.
fem = fem_struct;
fem.x = x;
fem.y = y;
fem.z = z;
fem.e = enodes;
fem.nei = nei;
return


%%*****************************************************************
%% HSDsqlpCpert: perturb C.
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% Kim-Chuan Toh, Michael J. Todd, Reha H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

    function [At,Cpert] = HSDsqlpCpert(blk,At,par,C,X,Cpert,runhist);

    iter = length(runhist.pinfeas);
    prim_infeas = runhist.pinfeas(iter);
    dual_infeas = runhist.dinfeas(iter);
    relgap      = runhist.relgap(iter);
    infeas      = runhist.infeas(iter);
    theta       = runhist.theta(iter);
%%
    Cpertold = Cpert;
    err = max(relgap,infeas);
    for p = 1:size(blk,1)
       pblk = blk(p,:);
       n = sum(pblk{2});
       tmp = max(1,norm(C{p},'fro'))/sqrt(n);
       if (err < 1e-6)
          if (norm(X{p},'fro') < 1e2); const=0.2; else; const=0.3; end
          Cpert(p) = max(const*Cpert(p),1e-10*tmp);
       elseif (err < 1e-2)
          if (norm(X{p},'fro') < 1e2); const=0.4; else; const=0.5; end
          Cpert(p) = max(const*Cpert(p),1e-8*tmp);
       else
	  Cpert(p) = max(0.9*Cpert(p),1e-6*tmp);
       end
       Cpert = min(Cpert,Cpertold);
       if (prim_infeas < min([0.1*dual_infeas, 1e-7*runhist.pinfeas(1)])) ...
      	  & (iter > 1 & dual_infeas > 0.8*runhist.dinfeas(iter-1) & relgap < 1e-4)
          Cpert(p) = 0.5*Cpert(p);
       elseif (dual_infeas < min([0.1*prim_infeas, 1e-7*runhist.dinfeas(1)])) ...
          & (iter > 1 & prim_infeas > 0.8*runhist.pinfeas(iter-1) & relgap < 1e-4)
          Cpert(p) = 0.5*Cpert(p);
       elseif (max(relgap,1e-2*infeas) < 1e-6 & relgap < 0.1*infeas)
          Cpert(p) = 0.5*Cpert(p);
       end
       if (prim_infeas < min([1e-4*dual_infeas,1e-7]) & theta < 1e-6) ...
	  | (prim_infeas < 1e-4 & theta < 1e-10)
          Cpert(p) = 0.1*Cpert(p);
       elseif (dual_infeas < min([1e-4*prim_infeas,1e-7]) & theta < 1e-6) ...
	  | (dual_infeas < 1e-4 & theta < 1e-10)
          Cpert(p) = 0.1*Cpert(p);
       elseif (iter > 1 & theta > 0.9*runhist.theta(iter-1) & infeas < 1e-3)
          Cpert(p) = 0.1*Cpert(p);
       end
       if strcmp(pblk{1},'s')
          Cnew = C{p} + Cpert(p)*speye(n);
          At{p}(:,par.invpermA(p,end-1)) = -svec(pblk,Cnew,1);
       else
          Cnew = C{p} + Cpert(p)*ones(n,1);
          At{p}(:,par.invpermA(p,end-1)) = -Cnew;
       end
    end
%%*****************************************************************************


function [fx,tt]=fxrapt(s,fs,mode)
%FXRAPT RAPT pitch tracker [FX,VUV]=(S,FS)
%
% Input:   s(ns)      Speech signal
%          fs         Sample frequency (Hz)
%          mode       'g' will plot a graph [default if no output arguments]
%                     'u' will include unvoiced fames (with fx=NaN)
%
% Outputs: fx(nframe)     Larynx frequency for each fram,e (or NaN for silent/unvoiced)
%          tt(nframe,3)  Start and end samples of each frame. tt(*,3)=1 at the start of each talk spurt
%
% Plots a graph if no outputs are specified showing lag candidates and selected path
%

% Bugs/Suggestions:
%   (1) Include backward DP pass and output the true cost for each candidate.
%   (2) Add an extra state to distinguish between voiceless and silent
%   (3) N-best DP to allow longer term penalties (e.g. for frequent pitch doubling/halving)

% The algorithm is taken from [1] with the following differences:
%
%      (a)  the factor AFACT which in the Talkin algorithm corresponds roughly
%           to the absolute level of harmonic noise in the correlation window. This value
%           is here calculated as the maximum of three figures:
%                   (i) an absolute floor set by PP.rapt_absnoise
%                  (ii) a multiple of the peak signal set by PP.rapt_signoise
%                 (iii) a multiple of the noise floor set by PP.rapt_relnoise
%      (b) The LPC used in calculating the Itakura distance uses a Hamming window rather than
%          a Hanning window.
%
% A C implementation of this algorithm by Derek Lin and David Talkin is included as  "get_f0.c"
% in the esps.zip package available from http://www.speech.kth.se/esps/esps.zip under the BSD
% license.
%
% Refs:
%      [1]   D. Talkin, "A Robust Algorithm for Pitch Tracking (RAPT)"
%            in "Speech Coding & Synthesis", W B Kleijn, K K Paliwal eds,
%            Elsevier ISBN 0444821694, 1995

%      Copyright (C) Mike Brookes 2006
%      Version: $Id: fxrapt.m 3601 2013-10-11 15:27:30Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   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 can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

s=s(:); % force s to be a column
if nargin<3
    mode=' ';
end
doback=0;   % don't do backwards DP for now

% read in parameters

PP=voicebox;
f0min=PP.rapt_f0min;            % Min F0 (Hz)                               [50]
f0max=PP.rapt_f0max;            % Max F0 (Hz)                               [500]
tframe=PP.rapt_tframe;          % frame size (s)                            [0.01]
tlpw=PP.rapt_tlpw;              % low pass filter window size (s)           [0.005]
tcorw=PP.rapt_tcorw;            % correlation window size (s)               [0.0075]
candtr=PP.rapt_candtr;          % minimum peak in NCCF                      [0.3]
lagwt=PP.rapt_lagwt;            % linear lag taper factor                   [0.3]
freqwt=PP.rapt_freqwt;          % cost factor for F0 change                 [0.02]
vtranc=PP.rapt_vtranc;          % fixed voice-state transition cost         [0.005]
vtrac=PP.rapt_vtrac;            % delta amplitude modulated transition cost [0.5]
vtrsc=PP.rapt_vtrsc;            % delta spectrum modulated transition cost  [0.5]
vobias=PP.rapt_vobias;          % bias to encourage voiced hypotheses       [0.0]
doublec=PP.rapt_doublec;        % cost of exact doubling or halving         [0.35]
absnoise=PP.rapt_absnoise;      % absolute rms noise level                  [0]
relnoise=PP.rapt_relnoise;      % rms noise level relative to noise floor   [2.0]
signoise=PP.rapt_signoise;      % ratio of peak signal rms to noise floor   [0.001]
ncands=PP.rapt_ncands;          % max hypotheses at each frame              [20]
trms=PP.rapt_trms;              % window length for rms measurement         [0.03]
dtrms=PP.rapt_dtrms;            % window spacing for rms measurement        [0.02]
preemph=PP.rapt_preemph;        % s-plane position of preemphasis zero      [-7000]
nfullag=PP.rapt_nfullag;        % number of full lags to try (must be odd)  [7]

% derived parameters (mostly dependent on sample rate fs)

krms=round(trms*fs);            % window length for rms measurement
kdrms=round(dtrms*fs);          % window spacing for rms measurement
rmswin=hanning(krms).^2;
kdsmp=round(0.25*fs/f0max);
hlpw=round(tlpw*fs/2);          % force window to be an odd length
blp=sinc((-hlpw:hlpw)/kdsmp).*hamming(2*hlpw+1).';
fsd=fs/kdsmp;
kframed=round(fsd*tframe);      % downsampled frame length
kframe=kframed*kdsmp;           % frame increment at full rate
rmsix=(1:krms)+floor((kdrms-kframe)/2); % rms index according to Talkin; better=(1:krms)+floor((kdrms-krms+1)/2)
minlag=ceil(fsd/f0max);
maxlag=round(fsd/f0min);        % use round() only because that is what Talkin does
kcorwd=round(fsd*tcorw);        % downsampled correlation window
kcorw=kcorwd*kdsmp;             % full rate correlation window
spoff=max(hlpw-floor(kdsmp/2),1+kdrms-rmsix(1)-kframe);  % offset for first speech frame at full rate
sfoff=spoff-hlpw+floor(kdsmp/2); % offset for downsampling filter
sfi=1:kcorwd;                   % initial decimated correlation window index array
sfhi=1:kcorw;                   % initial correlation window index array
sfj=1:kcorwd+maxlag;
sfmi=repmat((minlag:maxlag)',1,kcorwd)+repmat(sfi,maxlag-minlag+1,1);
lagoff=(minlag-1)*kdsmp;        % lag offset when converting to high sample rate
beta=lagwt*f0min/fs;            % bias towards low lags
log2=log(2);
lpcord=2+round(fs/1000);        % lpc order for itakura distance
hnfullag=floor(nfullag/2);
jumprat=exp((doublec+log2)/2);  % lag ratio at which octave jump cost is lowest
ssq=s.^2;
csssq=cumsum(ssq);
sqrt(min(csssq(kcorw+1:end)-csssq(1:end-kcorw))/kcorw);
afact=max([absnoise^2,max(ssq)*signoise^2,min(csssq(kcorw+1:end)-csssq(1:end-kcorw))*(relnoise/kcorw)^2])^2*kcorw^2;

% downsample signal to approx 2 kHz to speed up autocorrelation calculation
% kdsmp is the downsample factor

sf=filter(blp/sum(blp),1,s(sfoff+1:end));
sp=filter([1 exp(preemph/fs)],1,s); % preemphasised speech for LPC calculation
sf(1:length(blp)-1)=[];         % remove startup transient
sf=sf(1:kdsmp:end);             % downsample to =~2kHz
nsf=length(sf);                 % length of downsampled speech
ns=length(s);                   % length of full rate speech

% Calculate the frame limit to ensure we don't run off the end of the speech or decimated speech:
%   (a) For decimated autocorrelation when calculating sff():  (nframe-1)*kframed+kcorwd+maxlag <= nsf
%   (b) For full rate autocorrelation when calculating sfh():  max(fho)+kcorw+maxlag*kdsamp+hnfllag <= ns
%   (c) For rms ratio window when calculating rr            :  max(fho)+rmsix(end) <= ns
% where max(fho) = (nframe-1)*kframe + spoff

nframe=floor(1+min((nsf-kcorwd-maxlag)/kframed,(ns-spoff-max(kcorw-maxlag*kdsmp-hnfullag,rmsix(end)))/kframe));

% now search for autocorrelation peaks in the downsampled signal

cost=zeros(nframe,ncands);      % cumulative cost
prev=zeros(nframe,ncands);      % traceback pointer
mcands=zeros(nframe,1);         % number of actual candidates excluding voiceless
lagval=repmat(NaN,nframe,ncands-1);    % lag of each voiced candidate
tv=zeros(nframe,3);             % diagnostics: 1=voiceless cost, 2=min voiced cost, 3:cumulative voiceless-min voiced
if doback
    costms=cell(nframe,1);
end

% Main processing loop for each 10 ms frame

for iframe=1:nframe       % loop for each frame (~10 ms)

    % Find peaks in the normalized autocorrelation of subsampled (2Khz) speech
    % only keep peaks that are > 30% of highest peak

    fho=(iframe-1)*kframe+spoff;
    sff=sf((iframe-1)*kframed+sfj);
    sffdc=mean(sff(sfi));       % mean of initial correlation window length
    sff=sff-sffdc;              % subtract off the mean
    nccfd=normxcor(sff(1:kcorwd),sff(minlag+1:end));
    [ipkd,vpkd]=v_findpeaks(nccfd,'q');

    % Debugging: execute the line below to plot the autocorrelation peaks.
    % v_findpeaks(nccfd,'q'); xlabel(sprintf('Lag = (x+%d)*%g ms',minlag-1,1000*kdsmp/fs)); ylabel('Normalized Cross Correlation'); title (sprintf('Frame %d/%d',iframe,nframe));

    vipkd=[vpkd ipkd];
    vipkd(vpkd<max(vpkd)*candtr,:)=[];          % eliminate peaks that are small
    if size(vipkd,1)
        if size(vipkd,1)>ncands-1
            vipkd=sortrows(vipkd);
            vipkd(1:size(vipkd,1)-ncands+1,:)=[];   % eliminate lowest to leave only ncands-1
        end
        lagcan=round(vipkd(:,2)*kdsmp+lagoff);        % convert the lag candidate values to the full sample rate
        nlcan=length(lagcan);
    else
        nlcan=0;
    end

    % If there are any candidate lag values (nlcan>0) then refine their accuracy at the full sample rate

    if nlcan
        laglist=reshape(repmat(lagcan(:)',nfullag,1)+repmat((-hnfullag:hnfullag)',1,nlcan),nfullag*nlcan,1);
        sfh=s(fho+(1:kcorw+max(lagcan)+hnfullag));
        sfhdc=mean(sfh(sfhi));
        sfh=sfh-sfhdc;
        e0=sum(sfh(sfhi).^2);                     % energy of initial correlation window (only needed to store in tv(:,6)
        lagl2=repmat(lagcan(:)',nfullag+kcorw-1,1)+repmat((1-hnfullag:hnfullag+kcorw)',1,nlcan);
        nccf=normxcor(sfh(1:kcorw),sfh(lagl2),afact);

        [maxcc,maxcci]=max(nccf,[],1);
        vipk=[maxcc(:) lagcan(:)+maxcci(:)-hnfullag-1];
        vipk=vipk(:,[1 2 2]);
        maxccj=maxcci(:)'+nfullag*(0:nlcan-1);    % vector index into nccf array
        msk=mod(maxcci,nfullag-1)~=1 & 2*nccf(maxccj)-nccf(mod(maxccj-2,nfullag*nlcan)+1)-nccf(mod(maxccj,nfullag*nlcan)+1)>0;  % don't do quadratic interpolation for the end ones
        if any(msk)
            maxccj=maxccj(msk);
            vipk(msk,3)=vipk(msk,3)+(nccf(maxccj+1)-nccf(maxccj-1))'./(2*(2*nccf(maxccj)-nccf(maxccj-1)-nccf(maxccj+1)))';
        end
        vipk(maxcc<max(maxcc)*candtr,:)=[];          % eliminate peaks that are small
        if size(vipk,1)>ncands-1
            vipk=sortrows(vipk);
            vipk(1:size(vipk,1)-ncands+1,:)=[];   % eliminate lowest to leave only ncands-1
        end

        % vipk(:,1) has NCCF value, vipk(:,2) has integer peak position, vipk(:,3) has refined peak position

        mc=size(vipk,1);
    else
        mc=0;
    end

    % We now have mc lag candidates at the full sample rate

    mc1=mc+1;               % total number of candidates including "unvoiced" possibility
    mcands(iframe)=mc;      % save number of lag candidates (needed for pitch consistency cost calculation)
    if mc
        lagval(iframe,1:mc)=vipk(:,3)';
        cost(iframe,1)=vobias+max(vipk(:,1));   % voiceless cost
        cost(iframe,2:mc1)=1-vipk(:,1)'.*(1-beta*vipk(:,3)');   % local voiced costs
        tv(iframe,2)=min(cost(iframe,2:mc1));
    else
        cost(iframe,1)=vobias;          % if no lag candidates (mc=0), then the voiceless case is the only possibility
    end
    tv(iframe,1)=cost(iframe,1);
    if iframe>1                         % if it is not the first frame, then calculate pitch consistency and v/uv transition costs
        mcp=mcands(iframe-1);
        costm=zeros(mcp+1,mc1);         % cost matrix: rows and cols correspond to candidates in previous and current frames (incl voiceless)

        % if both frames have at least one lag candidate, then calculate a pitch consistency cost

        if mc*mcp
            lrat=abs(log(repmat(lagval(iframe,1:mc),mcp,1)./repmat(lagval(iframe-1,1:mcp)',1,mc)));
            costm(2:end,2:end)=freqwt*min(lrat,doublec+abs(lrat-log2));  % allow pitch doubling/halving
        end

        % if either frame has a lag candidate, then calculate the cost of voiced/voiceless transition and vice versa

        if mc+mcp
            rr=sqrt((rmswin'*s(fho+rmsix).^2)/(rmswin'*s(fho+rmsix-kdrms).^2)); % amplitude "gradient"
            ss=0.2/(distitar(lpcauto(sp(fho+rmsix),lpcord),lpcauto(sp(fho+rmsix-kdrms),lpcord),'e')-0.8);   % Spectral stationarity: note: Talkin uses Hanning instead of Hamming windows for LPC
            costm(1,2:end)= vtranc+vtrsc*ss+vtrac/rr;   % voiceless -> voiced cost
            costm(2:end,1)= vtranc+vtrsc*ss+vtrac*rr;
            tv(iframe,4:5)=[costm(1,mc1) costm(mcp+1,1)];
        end
        costm=costm+repmat(cost(iframe-1,1:mcp+1)',1,mc1);  % add in cumulative costs
        [costi,previ]=min(costm,[],1);
        cost(iframe,1:mc1)=cost(iframe,1:mc1)+costi;
        prev(iframe,1:mc1)=previ;
    else                            % first ever frame
        costm=zeros(1,mc1); % create a cost matrix in case doing a backward recursion
    end
    if mc
        tv(iframe,3)=cost(iframe,1)-min(cost(iframe,2:mc1));
        tv(iframe,6)=5*log10(e0*e0/afact);
    end
    if doback
        costms{iframe}=costm; % need to add repmatted cost into this
    end
end

% now do traceback

best=zeros(nframe,1);
[cbest,best(nframe)]=min(cost(nframe,1:mcands(nframe)+1));
for i=nframe:-1:2
    best(i-1)=prev(i,best(i));
end
vix=find(best>1);
fx=repmat(NaN,nframe,1);                        % unvoiced frames will be NaN
fx(vix)=fs*lagval(vix+nframe*(best(vix)-2)).^(-1); % leave as NaN if unvoiced
tt=zeros(nframe,3);
tt(:,1)=(1:nframe)'*kframe+spoff;       % find frame times
tt(:,2)=tt(:,1)+kframe-1;
jratm=(jumprat+1/jumprat)/2;
tt(2:end,3)=abs(fx(2:end)./fx(1:end-1)-jratm)>jumprat-jratm;    % new spurt if frequency ratio is outside (1/jumprat,jumprat)
tt(1,3)=1;           % first frame always starts a spurt
tt(1+find(isnan(fx(1:end-1))),3)=1; % NaN always forces a new spurt

% plot results if there are no output arguments of if the 'g' mode option is specified

if ~nargout || any(mode=='g')
    tf=spoff+(0:nframe-1)'*kframe;      % one sample before start of each frame
    blag=repmat(NaN,nframe,1);                        % unvoiced frames will be NaN
    blag(vix)=lagval(vix+nframe*(best(vix)-2)); % leave as NaN if unvoiced
    ts=(1:ns)/fs;                       % time scale for speech samples
    tsa=[1:tf(1) tf(end)+kframe+1:ns];  % indexes for unprocessed speech [-1 term is an error methinks]
    sup=repmat(NaN,ns,1);               % unprocessed speech - plot in black
    sup(tsa)=s(tsa);
    sv=reshape(s(tf(1)+1:tf(end)+kframe),kframe,nframe);               % processed speech
    su=sv;
    su(:,best>1)=NaN;                   % delete all voiced samples
    sv(:,best==1)=NaN;                  % delete all unvoiced samples
    tsuv=(tf(1)+1:tf(end)+kframe)/fs;
    su=su(:);
    sv=sv(:);
    ax=zeros(2,1);
    ax(1)=subplot(211);
    plot(ts,sup,'-k',tsuv,su,'r-',tsuv,sv,'b-');
    title('Speech');
    ax(2)=subplot(212);
    plot((tf+(kframe+1)/2)/fs,lagval*1000/fs,'xr',(tf+(kframe+1)/2)/fs,blag*1000/fs,'-b')
    xlabel('Time (s)');
    ylabel('Period (ms)');
    title('Lag Candidates');
    linkaxes(ax,'x');
end
if ~any(mode=='u')
tt(isnan(fx),:)=[];    % remove NaN spurts
fx(isnan(fx),:)=[];
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function v=normxcor(x,y,d)
% Calculate the normalized cross correlation of column vectors x and y
% we can calculate this in two ways but fft is much faster even for nx small
% We must have nx<=ny and the output length is ny-nx+1
% note that this routine does not do mean subtraction even though this is normally a good idea
% if y is a matrix, we correlate with each column
% d is a constant added onto the normalization factor
% v(j)=x'*yj/sqrt(d + x'*x * yj'*yj) where yj=y(j:j+nx-1) for j=1:ny-nx+1

if nargin<3
    d=0;
end
nx=length(x);
[ny,my]=size(y);
nv=1+ny-nx;
if nx>ny
    error('second argument is shorter than the first');
end

nf=pow2(nextpow2(ny));
w=irfft(repmat(conj(rfft(x,nf,1)),1,my).*rfft(y,nf,1));
s=zeros(ny+1,my);
s(2:end,:)=cumsum(y.^2,1);
v=w(1:nv,:)./sqrt(d+(x'*x).*(s(nx+1:end,:)-s(1:end-nx,:)));


function test_tutorial_beamformer20120321

% MEM 10gb
% WALLTIME 02:30:00
% DEPENDENCY ft_redefinetrial ft_freqanalysis ft_volumesegment ft_prepare_singleshell ft_sourceanalysis ft_prepare_leadfield ft_sourceinterpolate ft_sourceplot ft_volumenormalise

load(dccnpath('/home/common/matlab/fieldtrip/data/ftp/tutorial/beamformer/data_all.mat'));

% let's just rename the variable
dataFIC = data_all;

%% Preprocess time windows of interest

cfg = [];
cfg.toilim = [-0.5 0];
dataPre = ft_redefinetrial(cfg, dataFIC);

cfg.toilim = [0.8 1.3];
dataPost = ft_redefinetrial(cfg, dataFIC);

%% Cross-spectral density

cfg = [];
cfg.method    = 'mtmfft';
cfg.output    = 'powandcsd';
cfg.tapsmofrq = 4;
cfg.foilim    = [18 18];
freqPre = ft_freqanalysis(cfg, dataPre);

cfg = [];
cfg.method    = 'mtmfft';
cfg.output    = 'powandcsd';
cfg.tapsmofrq = 4;
cfg.foilim    = [18 18];
freqPost = ft_freqanalysis(cfg, dataPost);

%% Compute (or load) the forward model)

mri = ft_read_mri(dccnpath('/home/common/matlab/fieldtrip/data/ftp/test/ctf/Subject01.mri'));
if ~exist(dccnpath('/home/common/matlab/fieldtrip/data/ftp/tutorial/beamformer/segmentedmri.mat'), 'file')
  % segment the anatomical MRI
  cfg = [];
  cfg.write        = 'no';
  cfg.coordsys     = 'ctf';
  [segmentedmri] = ft_volumesegment(cfg, mri);
else
  % use the segmented MRI that is available
  load(dccnpath('/home/common/matlab/fieldtrip/data/ftp/tutorial/beamformer/segmentedmri.mat'));
end

%% Prepare head model
cfg = [];
% vol = ft_prepare_singleshell(cfg, segmentedmri);
cfg.method = 'singleshell';
vol = ft_prepare_headmodel(cfg, segmentedmri);

%% Prepare leadfield
cfg                 = [];
cfg.grad            = freqPre.grad;
cfg.headmodel       = vol;
cfg.reducerank      = 2;
cfg.channel         = {'MEG','-MLP31', '-MLO12'};
cfg.sourcemodel.resolution = 1;   % use a 3-D grid with a 1 cm resolution
cfg.sourcemodel.unit = 'cm';
[grid] = ft_prepare_leadfield(cfg);

%% Source analysis
cfg              = [];
cfg.frequency    = 18;
cfg.method       = 'dics';
cfg.projectnoise = 'yes';
cfg.sourcemodel         = grid;
cfg.headmodel    = vol;
cfg.lambda       = 0;

sourcePre  = ft_sourceanalysis(cfg, freqPre );
sourcePost = ft_sourceanalysis(cfg, freqPost);

%save source sourcePre sourcePost

%% Plot results
cfg            = [];
cfg.downsample = 2;
cfg.parameter  = 'avg.pow';
sourcePostInt  = ft_sourceinterpolate(cfg, sourcePost , mri);

cfg              = [];
cfg.method       = 'slice';
cfg.funparameter = 'avg.pow';
figure
ft_sourceplot(cfg,sourcePostInt);

%% Plot Condition contrast

sourceDiff = sourcePost;
sourceDiff.avg.pow = (sourcePost.avg.pow - sourcePre.avg.pow) ./ sourcePre.avg.pow;

cfg            = [];
cfg.downsample = 2;
cfg.parameter  = 'avg.pow';
sourceDiffInt  = ft_sourceinterpolate(cfg, sourceDiff , mri);

cfg = [];
cfg.method        = 'slice';
cfg.funparameter  = 'avg.pow';
cfg.maskparameter = cfg.funparameter;
cfg.funcolorlim   = [0.0 1.2];
cfg.opacitylim    = [0.0 1.2];
cfg.opacitymap    = 'rampup';
figure
ft_sourceplot(cfg, sourceDiffInt);

%% Orthogonal cut

cfg = [];
cfg.method        = 'ortho';
cfg.funparameter  = 'avg.pow';
cfg.maskparameter = cfg.funparameter;
cfg.funcolorlim   = [0.0 1.2];
cfg.opacitylim    = [0.0 1.2];
cfg.opacitymap    = 'rampup';
figure
ft_sourceplot(cfg, sourceDiffInt);

%% Template MRI

cfg = [];
cfg.coordsys      = 'ctf';
cfg.nonlinear     = 'no';
sourceDiffIntNorm = ft_volumenormalise(cfg, sourceDiffInt);

cfg = [];
cfg.method        = 'ortho';
%cfg.interactive   = 'yes';
cfg.funparameter  = 'avg.pow';
cfg.maskparameter = cfg.funparameter;
cfg.funcolorlim   = [0.0 1.2];
cfg.opacitylim    = [0.0 1.2];
cfg.opacitymap    = 'rampup';
figure
ft_sourceplot(cfg, sourceDiffIntNorm);

%% Project to a surface
cfg = [];
cfg.coordsys   = 'ctf';
cfg.nonlinear  = 'no';
sourceDiffIntN = ft_volumenormalise(cfg, sourceDiffInt);

cfg = [];
cfg.method         = 'surface';
cfg.funparameter   = 'avg.pow';
cfg.maskparameter  = cfg.funparameter;
cfg.funcolorlim    = [0.0 1.2];
cfg.funcolormap    = 'jet';
cfg.opacitylim     = [0.0 1.2];
cfg.opacitymap     = 'rampup';
cfg.projmethod     = 'nearest';
cfg.surffile       = 'surface_white_both.mat';
cfg.surfdownsample = 10;
figure
ft_sourceplot(cfg, sourceDiffIntN);
view ([90 0])

%% Neural Activity Index (NAI)

sourceNAI = sourcePost;
sourceNAI.avg.pow = sourcePost.avg.pow ./ sourcePost.avg.noise;

cfg = [];
cfg.downsample = 2;
cfg.parameter  = 'avg.pow';
sourceNAIInt = ft_sourceinterpolate(cfg, sourceNAI , mri);

cfg = [];
cfg.method        = 'slice';
cfg.funparameter  = 'avg.pow';
cfg.maskparameter = cfg.funparameter;
cfg.funcolorlim   = [4.0 6.2];
cfg.opacitylim    = [4.0 6.2];
cfg.opacitymap    = 'rampup';
figure
ft_sourceplot(cfg, sourceNAIInt);


% This file is part of the following project:
% Oliver Parson, Siddhartha Ghosh, Mark Weal, Alex Rogers.
% Non-intrusive Load Monitoring using Prior Models of General Appliance Types.
% In: 26th AAAI Conference on Artificial Intelligence. Toronto, Canada. 2012.
% Code available for download: https://sites.google.com/site/oliparson/phd-work/research-files/aaai-2012-code.zip?attredirects=0
% Copyright: Oliver Parson et al., University of Southhampton, 2012.

% Modified by Romano Cicchetti, ETH Zurich, in the context of the NILM-Eval project

function [result] = parsonAppliance(evaluation_and_training_days, setup, fid)

        %load parameters
        dataset = setup.dataset;
        household = setup.household;
        granularity = setup.granularity;
        filtering = setup.filtering;
        if isfield(setup,'filtering2')
            filtering2 = setup.filtering2;
        else
            filtering2 = 'noFiltering';
        end
        filtWindowLength = setup.filtWindowLength;
        filtLRatio = setup.filtLRatio;
        phases = setup.phases;

        applianceName = setup.applianceName;
        state_means = [setup.meanOff, setup.meanOn] ;
        state_covs = [setup.varOff, setup.varOn];
        transition_matrix = [1-setup.transOn, setup.transOn;
                            setup.transOff, 1-setup.transOff];
        likelihoodThreshold = setup.likThresh;
        numOfWindows = setup.numOfWindows;
        windowLength = setup.windowLength;
        trainingType = setup.trainingType;

        windowLength = windowLength/granularity;

        % get training and test data
        evaluation_days = evaluation_and_training_days{1};
        training_days = evaluation_and_training_days{2};
        applianceID = getApplianceID(applianceName);
        if strcmpi(phases, 'indv')
            phase_of_appliance = getPhase(household, applianceID);
            read_smartmeter_option = strcat('powerl', num2str(phase_of_appliance));
            test_data = read_smartmeter_data(dataset, num2str(household, '%02d'), evaluation_days, granularity, read_smartmeter_option);
            training_data = read_smartmeter_data(dataset, num2str(household, '%02d'), training_days, granularity, read_smartmeter_option);
        elseif strcmpi(phases, 'all')
            test_data = read_smartmeter_data(dataset, num2str(household, '%02d'), evaluation_days, granularity, 'powerallphases');
            training_data = read_smartmeter_data(dataset, num2str(household, '%02d'), training_days, granularity, 'powerallphases');
        else
            error('wrong value for phases parameter');
        end
        plug_data  = read_plug_data(dataset, household, applianceID, training_days, granularity);

        % apply filtering method to consumption data
        if ~strcmpi(filtering, 'noFiltering')
            function_handle = str2func(filtering);
            if strcmpi(filtering, 'totalVar')
                test_data = function_handle(test_data, filtLRatio);
                training_data = function_handle(training_data, filtLRatio);
                plug_data = function_handle(plug_data, filtLRatio);
            elseif strcmpi(filtering, 'edgeEnhance5')
                test_data = function_handle(test_data);
                training_data = function_handle(training_data);
                plug_data = function_handle(plug_data);
            else
                test_data = function_handle(test_data, filtWindowLength);
                plug_data = function_handle(plug_data, filtWindowLength);
            end
        end
        if ~strcmpi(filtering2, 'noFiltering')
            function_handle = str2func(filtering2);
            if strcmpi(filtering2, 'totalVar')
                test_data = function_handle(test_data, filtLRatio);
                training_data = function_handle(training_data, filtLRatio);
                plug_data = function_handle(plug_data, filtLRatio);
            elseif strcmpi(filtering2, 'edgeEnhance5')
                test_data = function_handle(test_data);
                training_data = function_handle(training_data);
                plug_data = function_handle(plug_data);
            else
                test_data = function_handle(test_data, filtWindowLength);
                training_data = function_handle(training_data, filtWindowLength);
                plug_data = function_handle(plug_data, filtWindowLength);
            end
        end

        % calculate diff model params
        init = ones(1, length(state_means)) / length(state_means);
        num_states = length(init);
        idx = repmat(1:num_states,num_states,1);
        emit_mean = state_means(idx) - state_means(idx');
        emit_cov = state_covs(idx) + state_covs(idx');

        % make difference hmm
        prior_dhmm_bnet = make_dhmm(init, ...
                          emit_mean, ...
                          emit_cov, ...
                          state_means, ...
                          state_covs, ...
                          transition_matrix);

        prior_hmm_bnet = make_hmm(init, ...
                         state_means, ...
                         state_covs, ...
                         transition_matrix);

        % data evidence
        diffs = [0 diff(test_data)];
        evidence = {};
        evidence(2,:) = num2cell(diffs);

        %create engine
        engine = smoother_engine(jtree_2TBN_inf_engine(prior_dhmm_bnet));

        %select windows of training data
        if strcmpi(trainingType, 'noTraining')

        elseif strcmpi(trainingType, 'aggregateTraining')
                [training_windows, ~] = find_training_ranges_generic(training_data, ...
                            windowLength, prior_dhmm_bnet, numOfWindows);

        elseif strcmpi(trainingType, 'plugTraining')
            plug_data_with_noise = awgn(plug_data, 25, 'measured');
            [training_windows, ~] = find_training_ranges_generic(plug_data_with_noise, ...
                            windowLength, prior_dhmm_bnet, numOfWindows);
        end

        %train models
        if strcmpi(trainingType, 'noTraining')
            trained_hmm_bnet = prior_hmm_bnet;
            trained_dhmm_bnet = prior_dhmm_bnet;
        elseif strcmpi(trainingType, 'aggregateTraining') || strcmpi(trainingType, 'plugTraining')
            [trained_dhmm_bnet, ~] = learn_params_generic(prior_dhmm_bnet, diff(training_windows'));
                emission3 = struct(trained_dhmm_bnet.CPD{4});
                dHMM_means = permute(emission3.mean, [2,3,1]);
                dHMM_covs = permute(emission3.cov, [3,4,1,2]);
            try
                [trained_hmm_bnet, ~] = learn_params_generic(prior_hmm_bnet, training_windows');
            catch err
                trained_hmm_bnet = prior_hmm_bnet;
            end
            trained_dhmm_bnet.CPD{1} = tabular_CPD(trained_dhmm_bnet, 1, 'CPT', ones(1, length(init)) / length(init));
            trained_dhmm_bnet.CPD{2} = trained_hmm_bnet.CPD{2};
            trained_dhmm_bnet.CPD{3} = prior_dhmm_bnet.CPD{3};
        end
        hmm_emissions = struct(trained_hmm_bnet.CPD{2});

        % infer power (viterbi)
        evidence(3,:) = num2cell(test_data);
        [mpe] = my_viterbi_diff(trained_dhmm_bnet, evidence, 1, likelihoodThreshold);
        means = hmm_emissions.mean;
        result = struct;
        result.consumption = means(cell2mat(mpe(1,:)));
        result.appliance_names = {applianceName};

        % write to summary text file
        fprintf(fid, 'trained HMM means:\n');
        fprintf(fid, [repmat('%f\t', 1, size(hmm_emissions.mean(:),2)) '\n'], hmm_emissions.mean(:));
        fprintf(fid, '\ntrained HMM covs:\n');
        fprintf(fid, [repmat('%f\t', 1, size(hmm_emissions.cov(:),2)) '\n'], hmm_emissions.cov(:));
        if strcmpi(trainingType, 'aggregateTraining') || strcmpi(trainingType, 'plugTraining')
            fprintf(fid, '\ntrained dHMM means:\n');
            fprintf(fid, [repmat('%f\t', 1, size(dHMM_means,2)) '\n'], dHMM_means');
            fprintf(fid, '\ntrained dHMM covs:\n');
            fprintf(fid, [repmat('%f\t', 1, size(dHMM_covs,2)) '\n'], dHMM_covs');
            fprintf(fid, '\n\n');
        else
            fprintf(fid, '\n');
        end
end


function [check,box] = checkfp(ha,plr,box,ipx,ipy,fpx,fpy,dblbox,blank)
    if plr==1
        otherplr=2;
    elseif plr==2
        otherplr=1;
    end
    %at initial position there should be own piece
    %and at final there should be blank space
    if box(fpx,fpy)~=0
        check=0;
    %     back step is not allowed till the palyer has choose doubled piece
    elseif fpx<=ipx && plr==1 && dblbox(ipx,ipy)==0
        check=0;
    elseif fpx>=ipx && plr==2 && dblbox(ipx,ipy)==0
        check=0;
    %only diagonal movement is allowed
    elseif (fpx==ipx+1 && fpy==ipy+1) || (fpx==ipx-1 && fpy==ipy+1) || ...
            (fpx==ipx+1 && fpy==ipy-1) || (fpx==ipx-1 && fpy==ipy-1)
        check=1;
    %if approach to kill a single piece and opponent piece is
    %there....its a good move .. so let the player play it..killing will
    %include that specific box=0 and string=blank
    elseif (fpx==ipx+2 && fpy==ipy+2) && ( box(ipx+1,ipy+1)==otherplr )
            check=1;
            box(ipx+1,ipy+1)=0;
            xy=(ipx+1)*10+(ipy+1);
            set(ha(xy),'CData',blank);
    elseif (fpx==ipx-2 && fpy==ipy-2) && ( box(ipx-1,ipy-1)==otherplr )
            check=1;
            box(ipx-1,ipy-1)=0;
            xy=(ipx-1)*10+(ipy-1);
            set(ha(xy),'CData',blank);
    elseif (fpx==ipx+2 && fpy==ipy-2) && ( box(ipx+1,ipy-1)==otherplr )
            check=1;
            box(ipx+1,ipy-1)=0;
            xy=(ipx+1)*10+(ipy-1);
            set(ha(xy),'CData',blank);
    elseif (fpx==ipx-2 && fpy==ipy+2) && ( box(ipx-1,ipy+1)==otherplr )
            check=1;
            box(ipx-1,ipy+1)=0;
            xy=(ipx-1)*10+(ipy+1);
            set(ha(xy),'CData',blank);
    %if approach to kill two pieces together lets check
    elseif (fpx==ipx+4 && fpy==ipy+4) && ( box(ipx+1,ipy+1)==otherplr) && (box(ipx+2,ipy+2)==0) && (box(ipx+3,ipy+3)==otherplr)
        check=1;
        box(ipx+1,ipy+1)=0;
        box(ipx+3,ipy+3)=0;
        xy=(ipx+1)*10+(ipy+1);
        set(ha(xy),'CData',blank);
        xy=(ipx+3)*10+(ipy+3);
        set(ha(xy),'CData',blank);
    elseif (fpx==ipx+4 && fpy==ipy-4) && ( box(ipx+1,ipy-1)==otherplr) && (box(ipx+2,ipy-2)==0) && (box(ipx+3,ipy-3)==otherplr)
        check=1;
        box(ipx+1,ipy-1)=0;
        box(ipx+3,ipy-3)=0;
        xy=(ipx+1)*10+(ipy-1);
        set(ha(xy),'CData',blank);
        xy=(ipx+3)*10+(ipy-3);
        set(ha(xy),'CData',blank);
    elseif (fpx==ipx-4 && fpy==ipy+4) && ( box(ipx-1,ipy+1)==otherplr) && (box(ipx-2,ipy+2)==0) && (box(ipx-3,ipy+3)==otherplr)
        check=1;
        box(ipx-1,ipy+1)=0;
        box(ipx-3,ipy+3)=0;
        xy=(ipx-1)*10+(ipy+1);
        set(ha(xy),'CData',blank);
        xy=(ipx-3)*10+(ipy+3);
        set(ha(xy),'CData',blank);
    elseif (fpx==ipx-4 && fpy==ipy-4) && ( box(ipx-1,ipy-1)==otherplr) && (box(ipx-2,ipy-2)==0) && (box(ipx-3,ipy-3)==otherplr)
        check=1;
        box(ipx-1,ipy-1)=0;
        box(ipx-3,ipy-3)=0;
        xy=(ipx-1)*10+(ipy-1);
        set(ha(xy),'CData',blank);
        xy=(ipx-3)*10+(ipy-3);
        set(ha(xy),'CData',blank);
    elseif (fpx==ipx+4 && fpy==ipy) && ( box(ipx+1,ipy+1)==otherplr) && (box(ipx+2,ipy+2)==0) && (box(ipx+3,ipy+1)==otherplr)
        check=1;
        box(ipx+1,ipy+1)=0;
        box(ipx+3,ipy+1)=0;
        xy=(ipx+1)*10+(ipy+1);
        set(ha(xy),'CData',blank);
        xy=(ipx+3)*10+(ipy+1);
        set(ha(xy),'CData',blank);
    elseif (fpx==ipx+4 && fpy==ipy) && ( box(ipx+1,ipy-1)==otherplr) && (box(ipx+2,ipy-2)==0) && (box(ipx+3,ipy-1)==otherplr)
        check=1;
        box(ipx+1,ipy-1)=0;
        box(ipx+3,ipy-1)=0;
        xy=(ipx+1)*10+(ipy-1);
        set(ha(xy),'CData',blank);
        xy=(ipx+3)*10+(ipy-1);
        set(ha(xy),'CData',blank);
    elseif (fpx==ipx-4 && fpy==ipy) && ( box(ipx-1,ipy+1)==otherplr) && (box(ipx-2,ipy+2)==0) && (box(ipx-3,ipy+1)==otherplr)
        check=1;
        box(ipx-1,ipy+1)=0;
        box(ipx-3,ipy+1)=0;
        xy=(ipx-1)*10+(ipy+1);
        set(ha(xy),'CData',blank);
        xy=(ipx-3)*10+(ipy+1);
        set(ha(xy),'CData',blank);
    elseif (fpx==ipx-4 && fpy==ipy) && ( box(ipx-1,ipy-1)==otherplr) && (box(ipx-2,ipy-2)==0) && (box(ipx-3,ipy-1)==otherplr)
        check=1;
        box(ipx-1,ipy-1)=0;
        box(ipx-3,ipy-1)=0;
        xy=(ipx-1)*10+(ipy-1);
        set(ha(xy),'CData',blank);
        xy=(ipx-3)*10+(ipy-1);
        set(ha(xy),'CData',blank);
    elseif (fpx==ipx && fpy==ipy+4) && ( box(ipx+1,ipy+1)==otherplr) && (box(ipx+2,ipy+2)==0) && (box(ipx+1,ipy+3)==otherplr)
        check=1;
        box(ipx+1,ipy+1)=0;
        box(ipx+1,ipy+3)=0;
        xy=(ipx+1)*10+(ipy+1);
        set(ha(xy),'CData',blank);
        xy=(ipx+1)*10+(ipy+3);
        set(ha(xy),'CData',blank);
    elseif (fpx==ipx && fpy==ipy+4) && ( box(ipx-1,ipy+1)==otherplr) && (box(ipx-2,ipy+2)==0) && (box(ipx-1,ipy+3)==otherplr)
        check=1;
        box(ipx-1,ipy+1)=0;
        box(ipx-1,ipy+3)=0;
        xy=(ipx-1)*10+(ipy+1);
        set(ha(xy),'CData',blank);
        xy=(ipx-1)*10+(ipy+3);
        set(ha(xy),'CData',blank);
    elseif (fpx==ipx && fpy==ipy-4) && ( box(ipx+1,ipy-1)==otherplr) && (box(ipx+2,ipy-2)==0) && (box(ipx+1,ipy-3)==otherplr)
        check=1;
        box(ipx+1,ipy-1)=0;
        box(ipx+1,ipy-3)=0;
        xy=(ipx+1)*10+(ipy-1);
        set(ha(xy),'CData',blank);
        xy=(ipx+1)*10+(ipy-3);
        set(ha(xy),'CData',blank);
    elseif (fpx==ipx && fpy==ipy-4) && ( box(ipx-1,ipy-1)==otherplr) && (box(ipx-2,ipy-2)==0) && (box(ipx-1,ipy-3)==otherplr)
        check=1;
        box(ipx-1,ipy-1)=0;
        box(ipx-1,ipy-3)=0;
        xy=(ipx-1)*10+(ipy-1);
        set(ha(xy),'CData',blank);
        xy=(ipx-1)*10+(ipy-3);
        set(ha(xy),'CData',blank);
    else
        check=0;
    end
end


function [sp_new, scores_new] = change_color_space(color_space, I_rgb, sp, K, im_num, h, w, sph, opts)
% Updates the histograms in the 'sp' variable by recalculating them using
% image transformed into another color space, and color space -specific
% features. This function is not used with default settings.

scores_new = [];

switch color_space
    case 'nrgb'
        I_nrgb = uint8(rgb_to_nrgb(I_rgb));
        [words, k] = compute_features(I_nrgb, I_rgb, 'nrgb', opts, im_num);
        sp_new = compute_histograms(sp, words, k, h, w);
        scores_new = similarity_scores(sp_new, K, opts, 0, sph);
    case 'opp'
        I_opp = uint8(rgb_to_opp(I_rgb));
        [words, k] = compute_features(I_opp, I_rgb, 'opp', opts, im_num);
        sp_new = compute_histograms(sp, words, k, h, w);
        scores_new = similarity_scores(sp_new, K, opts, 0, sph);
    case 'hsv'
        I_hsv = uint8(rgb_to_hsv(I_rgb));
        [words, k] = compute_features(I_hsv, I_rgb, 'hsv', opts, im_num);
        sp_new = compute_histograms(sp, words, k, h, w);
        scores_new = similarity_scores(sp_new, K, opts, 0, sph);
    otherwise
        error('bad image type argument')

end


function [ kSpace, measPara ] = convertKSpace_main( data )
% convert from ISMRMD to processable k-space format
%
% (c) Thomas Kuestner
% ---------------------------------------------------------------------

iFreq = unique(double(data.head.number_of_samples));
iY = unique(double(data.head.idx.kspace_encode_step_1));
iZ = unique(double(data.head.idx.kspace_encode_step_2));
iPha = unique(double(data.head.idx.phase));
iSli = unique(double(data.head.idx.slice));
iRep = unique(double(data.head.idx.repetition));
iAvg = unique(double(data.head.idx.average));
iEcho = unique(double(data.head.idx.contrast));
iCha = unique(double(data.head.active_channels));
measPara.dim = [length(iY), iFreq, length(iZ), length(iPha), iCha];
measPara.LCall = [length(iSli), length(iAvg), length(iEcho), length(iRep)];

if(measPara.dim(3) == 1)
    measPara.dimension = '2D';
else
    if(measPara.dim(4) == 1)
        measPara.dimension = '3D';
    else
        measPara.dimension = '4D';
    end
end

% begin conversion
kSpace = cell(measPara.LCall(1), measPara.dim(5), measPara.LCall(4), measPara.LCall(2));
if(strcmp(measPara.dimension,'2D'))
    [kSpace{:}] = deal(complex(zeros(measPara.dim(1),iFreq,measPara.dim(4),'single'),zeros(measPara.dim(1),iFreq,measPara.dim(4),'single')));
elseif(strcmp(measPara.dimension,'3D'))
    [kSpace{:}] = deal(complex(zeros(measPara.dim(1),iFreq,measPara.dim(3),'single'),zeros(measPara.dim(1),iFreq,measPara.dim(3),'single')));
elseif(strcmp(measPara.dimension,'4D'))
    [kSpace{:}] = deal(complex(zeros(measPara.dim(1),iFreq,measPara.dim(3),measPara.dim(4),'single'),zeros(measPara.dim(1),iFreq,measPara.dim(3),measPara.dim(4),'single')));
end

for idxRep = 1:measPara.LCall(4)
    for idxAvg = 1:measPara.LCall(2)

        if(strcmp(measPara.dimension,'2D'))
            for idxSli = 1:measPara.LCall(1)
                for idxPha = 1:measPara.dim(4)
                    for idxY = 1:measPara.dim(1)
                        lIdx = double(data.head.idx.repetition) == iRep(idxRep) & ...
                            double(data.head.idx.average) == iAvg(idxAvg) & ...
                            double(data.head.idx.slice) == iSli(idxSli) & ...
                            double(data.head.idx.phase) == iPha(idxPha) & ...
                            double(data.head.idx.kspace_encode_step_1) == iY(idxY);
                        if(nnz(lIdx) > 1)
                            lIdx(find(lIdx,nnz(lIdx)-1,'last')) = false;
                        end
                        dataLine = data.data{lIdx};

                        for idxCha = 1:measPara.dim(5)
                            kSpace{idxSli,idxCha,idxRep,idxAvg}(idxY,:,idxPha) = dataLine(2*iFreq*(idxCha-1)+1:2:2*iFreq*idxCha).'+ 1i * dataLine(2*iFreq*(idxCha-1)+2:2:2*iFreq*idxCha).';
                        end
                    end
                end
            end

        elseif(strcmp(measPara.dimension,'3D') || strcmp(measPara.dimension,'4D'))
            for idxZ = 1:measPara.dim(3)
                for idxPha = 1:measPara.dim(4)
                    for idxY = 1:measPara.dim(1)
                        lIdx = double(data.head.idx.repetition) == iRep(idxRep) & ...
                            double(data.head.idx.average) == iAvg(idxAvg) & ...
                            double(data.head.idx.kspace_encode_step_2) == iZ(idxZ) & ...
                            double(data.head.idx.phase) == iPha(idxPha) & ...
                            double(data.head.idx.kspace_encode_step_1) == iY(idxY);
                        if(nnz(lIdx) > 1)
                            lIdx(find(lIdx,nnz(lIdx)-1,'last')) = false;
                        end
                        dataLine = data.data{lIdx};

                        for idxCha = 1:measPara.dim(5)
                            kSpace{1,idxCha,idxRep,idxAvg}(idxY,:,idxZ,idxPha) = dataLine(2*iFreq*(idxCha-1)+1:2:2*iFreq*idxCha).' + 1i * dataLine(2*iFreq*(idxCha-1)+2:2:2*iFreq*idxCha).';
                        end
                    end
                end
            end
        end
    end
end
end


function [x, featDescription] = getBoundaryFeatures(ind, occ, pb, pb2)
% [x, featDescription] = getBoundaryFeatures(ind, occ, pb, pb2)

if islogical(ind)
    ind = find(ind);
end

featDescription = cell(1, 7);
x = zeros(numel(ind), 7);

[imh, imw, no] = size(occ.po_all);
npix = imh*imw;
%n = zeros(numel(ind), 1);
for f = 1:4
    ind2 = ind + npix*(f-1);
    x(:, f) = occ.po_all(ind2);
    featDescription{f} = ['pocc' num2str(f)];
    %n = n + double(x(:, f)>0);
end
f = f+1;
x(:, f) = max(x(:, 1:f-1), [], 2); % occ.po(ind);
featDescription{f} = 'pocc_max';
%f = f+1;
%x(:, f) = n;
if exist('pb', 'var') && ~isempty(pb)
    f = f+1;
    pb_soft = max(pb.pb_soft, [], 3);
    x(:, f) = pb_soft(ind);
    featDescription{f} = 'pb1';
end
if exist('pb2', 'var') && ~isempty(pb2)
    f = f+1;
    x(:, f) = pb2.pb_soft(ind);
    featDescription{f} = 'pb2';
end


function [cerrFeatS,IBSIfeatS,pctDiffS] = compareRadiomicsWithIBSI1OrigImgWithInterp
% Function to compare radiomics features computed using CERR against
% the IBSI benchmark for configuration C.

%% 1. Calc. features for configuration 'C' using CERR
cerrFeatS = calcIBSIPhantomRadiomics('C');

%% Get IBSI benchmarks
ibsiConfigCResult = fullfile(fileparts(fileparts(getCERRPath)),...
            'Unit_Testing/data_for_cerr_tests/IBSI1_CT_phantom/IBSI_results_configC.mat');
temp = load(ibsiConfigCResult);
IBSIfeatS = temp.IBSIfeatS;

%% Compare results for each feature class
pctDiffS = struct();
%Shape features
shapeFeatC = fieldnames(IBSIfeatS.shapeS);
for n = 1:length(shapeFeatC)
    ibsiVal = IBSIfeatS.shapeS.(shapeFeatC{n});
    cerrVal = cerrFeatS.shapeS.(shapeFeatC{n});
    if ismember(shapeFeatC{n},{'volume','filledVolume'})
        cerrVal = cerrVal*1000; %cm^3 to mm^3
    elseif strcmp(shapeFeatC{n},'surfArea')
        cerrVal = cerrVal*100; %cm^2 to mm^2
    elseif ismember(shapeFeatC{n},{'majorAxis','minorAxis','leastAxis'}) || ...
            contains(shapeFeatC{n},'Diameter')
         cerrVal = cerrVal*10; %cm to mm
    elseif strcmp(shapeFeatC{n},'surfToVolRatio')
        cerrVal = cerrVal/10; %cm^-1 to mm^-1
    end
    pctDiff =(cerrVal-ibsiVal)*100/ibsiVal;
    pctDiffS.Shape.(shapeFeatC{n}) = pctDiff;
end

%IVH features
IVHfeatC = fieldnames(IBSIfeatS.Original.ivhFeaturesS);
for n = 1:length(IVHfeatC)
    ibsiVal = IBSIfeatS.Original.ivhFeaturesS.(IVHfeatC{n});
    cerrVal = cerrFeatS.Original.ivhFeaturesS.(IVHfeatC{n});
    pctDiff =(cerrVal-ibsiVal)*100/ibsiVal;
    pctDiffS.IVH.(IVHfeatC{n}) = pctDiff;
end

%First-order intensity features
firstOrdFeatC = fieldnames(IBSIfeatS.Original.firstOrderS);
for n = 1:length(firstOrdFeatC)
    ibsiVal = IBSIfeatS.Original.firstOrderS.(firstOrdFeatC{n});
    cerrVal = cerrFeatS.Original.firstOrderS.(firstOrdFeatC{n});
    pctDiff =(cerrVal-ibsiVal)*100/ibsiVal;
    pctDiffS.FirstOrder.(firstOrdFeatC{n}) = pctDiff;
end

%GLCM features
%Avg
glcmFeatC = fieldnames(IBSIfeatS.Original.glcmFeatS.AvgS);
for n = 1:length(glcmFeatC)
    ibsiVal = IBSIfeatS.Original.glcmFeatS.AvgS.(glcmFeatC{n});
    cerrVal = cerrFeatS.Original.glcmFeatS.AvgS.(glcmFeatC{n});
    pctDiff =(cerrVal-ibsiVal)*100/ibsiVal;
    pctDiffS.GLCM.Avg.(glcmFeatC{n}) = pctDiff;
end
%Merge
glcmFeatC = fieldnames(IBSIfeatS.Original.glcmFeatS.AvgS);
for n = 1:length(glcmFeatC)
    ibsiVal = IBSIfeatS.Original.glcmFeatS.AvgS.(glcmFeatC{n});
    cerrVal = cerrFeatS.Original.glcmFeatS.AvgS.(glcmFeatC{n});
    pctDiff =(cerrVal-ibsiVal)*100/ibsiVal;
    pctDiffS.GLCM.Merge.(glcmFeatC{n}) = pctDiff;
end

%RLM features
%Avg
rlmFeatC = fieldnames(IBSIfeatS.Original.rlmFeatS.AvgS);
for n = 1:length(rlmFeatC)
    ibsiVal = IBSIfeatS.Original.rlmFeatS.AvgS.(rlmFeatC{n});
    cerrVal = cerrFeatS.Original.rlmFeatS.AvgS.(rlmFeatC{n});
    pctDiff =(cerrVal-ibsiVal)*100/ibsiVal;
    pctDiffS.RLM.Avg.(rlmFeatC{n}) = pctDiff;
end
%Merge
rlmFeatC = fieldnames(IBSIfeatS.Original.rlmFeatS.CombS);
for n = 1:length(rlmFeatC)
    ibsiVal = IBSIfeatS.Original.rlmFeatS.CombS.(rlmFeatC{n});
    cerrVal = cerrFeatS.Original.rlmFeatS.CombS.(rlmFeatC{n});
    pctDiff =(cerrVal-ibsiVal)*100/ibsiVal;
    pctDiffS.RLM.Merge.(rlmFeatC{n}) = pctDiff;
end

%NGTDM features
ngtdmFeatC = fieldnames(IBSIfeatS.Original.ngtdmFeatS);
for n = 1:length(ngtdmFeatC)
    ibsiVal = IBSIfeatS.Original.ngtdmFeatS.(ngtdmFeatC{n});
    cerrVal = cerrFeatS.Original.ngtdmFeatS.(ngtdmFeatC{n});
    pctDiff =(cerrVal-ibsiVal)*100/ibsiVal;
    pctDiffS.NGTDM.(ngtdmFeatC{n}) = pctDiff;
end

%SZM features
szmFeatC = fieldnames(IBSIfeatS.Original.szmFeatS);
for n = 1:length(szmFeatC)
    ibsiVal = IBSIfeatS.Original.szmFeatS.(szmFeatC{n});
    cerrVal = cerrFeatS.Original.szmFeatS.(szmFeatC{n});
    pctDiff =(cerrVal-ibsiVal)*100/ibsiVal;
    pctDiffS.GLSZM.(szmFeatC{n}) = pctDiff;
end


function [roi, roiNot] = dtiRoiClip(roi, rlClip, apClip, siClip)
%
% [roi, roiNot] = dtiRoiClip(roi, rlClip, apClip, siClip)
%
% Clips a dti ROI.
%
% HISTORY:
% 2005.01.05 RFD wrote it.

if(nargin<4) siClip = []; end
if(nargin<3) apClip = []; end
if(nargin==1)
    newName = [roi.name '_clip'];
    prompt = {'Left (-80) Right (+80) clip (blank for none):',...
          'Posterior (-120) Anterior (+80) clip (blank for none):',...
          'Inferior (-50) Superior (+90) clip (blank for none):',...
          'New ROI name:'};
    defAns = {'','','',newName};
    resp = inputdlg(prompt,'Clip Current ROI',1,defAns);
    if(isempty(resp))
        disp('User cancelled clip.');
        return;
    end
    rlClip = str2num(resp{1});
    apClip = str2num(resp{2});
    siClip = str2num(resp{3});
    roi.name = resp{4};
end

keep = ones(size(roi.coords,1),1);
if(~isempty(rlClip))
    keep = keep & (roi.coords(:,1)<rlClip(1) | roi.coords(:,1)>rlClip(2));
end
if(~isempty(apClip))
    keep = keep & (roi.coords(:,2)<apClip(1) | roi.coords(:,2)>apClip(2));
end
if(~isempty(siClip))
    keep = keep & (roi.coords(:,3)<siClip(1) | roi.coords(:,3)>siClip(2));
end
coords = roi.coords;
roi.coords = coords(keep,:);
if(nargout>1)
    roiNot = roi;
    roiNot.coords = coords(~keep,:);
    roiNot.name = [roi.name '_NOT'];
end
return;


function opts = getReweightedOptimizationParams

    % Maximum number of majorization-minimization (MM) iterations used for
    % iteratively re-weighted minimization (should be between 5 and 15).
    opts.maxMMIter = 10;

    % Maximum number of scaled conjugate gradients (SCG) iterations in the
    % inner optimization loop used for image reconstruction (should be
    % between 3 and 10).
    opts.maxSCGIter = 5;

    % Maximum number of cross validation (CV) iterations for hyperparameter
    % selection (should be between 10 and 30).
    opts.maxCVIter = 20;

    % Fraction of the observations used for the training stage in
    % hyperparameter selection (should be between 0.90 and 0.95).
    opts.fractionCVTrainingObservations = 0.95;

    % Initial search range (lower and upper bound) for the regularization
    % hyperparameter on a logarithmic scale (should be between 10-12 and
    % 10^0).
    opts.hyperparameterCVSearchRange = [-12 0];

    % Termination tolerance for max(x(t) - x(t-1)) for the estimate of the
    % high-resolution image over the MM and SCG iterations (should be
    % approx. 1e-3).
    opts.terminationTol = 1e-3;

    % Number of levels used for coarse-to-fine optimization. If empty, the
    % number of levels is set automatically (should be between 2 and 5).
    opts.numCoarseToFineLevels = [];

    % The weighting function that is used to determine the observation
    % confidence weights. This function needs to be defined as a struct P
    % with the following fields:
    %   - P.function is a function handle to weighting function to compute
    %     the confidence weights based on the residual error. The residual
    %     errors for the different frames need to be stored as a cell
    %     array.
    %   - P.parameters are additional parameters that are passed to the
    %     function P.function.
    opts.observationWeightingFunction = estimateHuberObservationWeights;

    % The weighting function that is used to determine the adpative prior
    % weights. This function needs to be defined as a struct P with the
    % following fields:
    %   - P.function is a function handle to weighting function to compute
    %     the prior weights based on a given image in a certain transform
    %     domain.
    %   - P.parameters are additional parameters that are passed to the
    %     function P.function.
    opts.priorWeightingFunction = estimateWBTVPriorWeights;


function varargout = quiver(sVF,varargin)
% quiver spherical vector field
%
% Syntax
%   quiver3(sVF)
%
% Options
%  normalized - normalize vectors
%  arrowSize  - arrow size
%  maxHeadSize - head size

% See also
%   S2VectorField/plot
%

% plot the function values
[varargout{1:nargout}] = plot(sVF,varargin{:});

end


function y = cvx_s_diagonal( m, n )
%CVX_S_DIAGONAL Diagonal matrices.
y = cvx_s_banded( m, n, 0, 0 );

% Copyright 2010 Michael C. Grant and Stephen P. Boyd.
% See the file COPYING.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function modelPlane = sc_extract_plane(imgName, img, mask, optA)

% SC_EXTRACT_PLANE:
%
% Extract plane model from an image
%
% Output: modelPlane
% The model plane has the following data structure
%
% modelPlane
% modelPlane.vpData:                        Vanishing point data
% modelPlane.numPlane                       Number of planes
% modelPlane.plane{indPlane}.vLine          The vanishing line of the plane
% modelPlane.plane{indPlane}.imgPlaneProb;  The planar location density
% modelPlane.plane{indPlane}.sourceVP       Which two VPs form the plane
% modelPlane.plane{indPlane}.rotPar(vpInd)  Rotation parameters for aligning
%                                           two sets of lines with the x-axis                                           x-axis
% modelPlane.plane{indPlane}.postProb       Posteior probability of the plane

modelPlane = [];

%% VP detection
imgPath = 'data';
vpFilePath = 'cache\vpdetection';
vpFileName  = [imgName(1:end-4), '-vanishingpoints.txt'];

if(~exist(fullfile(vpFilePath, 'text', vpFileName), 'file'))
    vpExeFile = fullfile('source', 'vpdetection.exe');
    vpDetectCMD = [vpExeFile, ' -indir ', imgPath, ' -infile ', imgName, ' -outdir ', vpFilePath];
    system(vpDetectCMD);
end
% Read vanishing point data
vpData = sc_read_vpdata(fullfile(vpFilePath, 'text', vpFileName));

modelPlane = sc_detect_plane_from_vp(vpData, img, mask, optA);

end

function vpData = sc_read_vpdata(fileName)

% SC_READ_VPDATA: read the data from vanishing point detection algorithm
% Input:
%   - fileName: the txt file containing the pre-computed vanishing point
%   detection code
% Output:
%   - vpData
%   The data structure of vpData
%       - vpData.numVP: number of detected vanishing points
%       - vpData.vp{i}.pos: the vanishing point position in the homogenous coordiante
%       - vpData.vp{i}.score: the score of the vanishing point
%       - vpData.vp{i}.numLines: number of lines supporting the vanishing point
%       - vpData.vp{i}.lines{j}.p1: (x1, y1): starting position
%       - vpData.vp{i}.lines{j}.p2: (x2, y2): ending position
%       - vpData.vp{i}.lines{j}.length: length of the line segment

vpData = [];

% Read data
fid = fopen(fileName);

%% Parse VP positions
temp = fscanf(fid, '%s ', [1 5]);
numVP = 0;
readVPFlag = 1;
VP = [];
while(readVPFlag)
    numVP = numVP + 1;
    vpCurr = fscanf(fid, '%g %g %g %g %g', [5 1]);
    if(~isempty(vpCurr))
        VP(:,numVP) = vpCurr;
    else
        temp = fscanf(fid, '%s ', [1 6]);
        readVPFlag = 0;
    end
end
VP = VP';

vpData.numVP = size(VP, 1);

% Save VP position data
for i = 1: vpData.numVP
    vpData.vp{i}.pos = VP(i, 1:3);
    vpData.vp{i}.score = VP(i, 4);
    vpData.vp{i}.numLines = VP(i, 5);
end

%% Parse each set of line segments for the corresponding VP
for i = 1: vpData.numVP
    numLine = fscanf(fid, '%d ', [1 1]);
    lines = fscanf(fid, '%g %g %g %g %g', [5 numLine]);
    vpData.vp{i}.lines = lines';
end

fclose(fid);
end

function modelPlane = sc_detect_plane_from_vp(vpData, img, mask, optA)

% SC_DETECT_PLANE_FROM_VP: simple plane detection algorithm
% Input:
%     - vpData: vanishing point data
%     - img: input image
%     - mask: hole mask
% Output:
%     - modelPlane

%%

modelPlane = [];

% === Setting up ===
[imgH, imgW, ch] = size(img);
HfilterX = fspecial('gaussian', [1, optA.filterSize], optA.filterSigma);
HfilterY = HfilterX';
% fspecial('gaussian', optA.filterSize, optA.filterSigma);

img = im2double(img);

% === Supporting lines spatial support estimation ===
shapeInserter = vision.ShapeInserter('Shape', 'Lines','BorderColor', 'White');

for i = 1: vpData.numVP
    % The support lines
    imgLines = zeros(imgH, imgW);
    imgLines = step(shapeInserter, imgLines, int16(round(vpData.vp{i}.lines(:,1:4))));
    % Spatial density estimation via blurring
    imgLinesPosMap = imgLines;
    for k = 1:optA.numFilterIter
        imgLinesPosMap = imfilter(imgLinesPosMap, HfilterX, 'conv', 'replicate');
    end
    for k = 1:optA.numFilterIter
        imgLinesPosMap = imfilter(imgLinesPosMap, HfilterY, 'conv', 'replicate');
    end

    % Save results
    modelPlane.vp{i}.imgLines = imgLines;
    modelPlane.vp{i}.imgLinesPosMap = imgLinesPosMap;
end

% === Estimate plane support and plane parameters ===
numPlane = (vpData.numVP)*(vpData.numVP-1)/2;
% Initialize plane data
modelPlane.plane = cell(numPlane, 1);

indPlane = 1;
% A pair of vanishing points forms a plane hypothesis
for i = 1: vpData.numVP - 1
    for j = i+1: vpData.numVP
        % Compute the vanishing line
        modelPlane.plane{indPlane}.vLine = vLineFromTwoVP(vpData.vp{i}.pos, vpData.vp{j}.pos);
        % Element-wise product of two support line density
        modelPlane.plane{indPlane}.imgPlaneProb = modelPlane.vp{i}.imgLinesPosMap.*modelPlane.vp{j}.imgLinesPosMap; % Product of two probability maps
        modelPlane.plane{indPlane}.score = sum(modelPlane.plane{indPlane}.imgPlaneProb(:));
        modelPlane.plane{indPlane}.sourceVP = [i, j];

        indPlane = indPlane + 1;
    end
end

% === Compute rectified rotation parameters ===

for i = 1: numPlane
    for vpInd = 1: 2

        linesCurr = vpData.vp{modelPlane.plane{i}.sourceVP(vpInd)}.lines;
        invalidLineInd = linesCurr(:,5) == 0;
        linesCurr = linesCurr(~invalidLineInd,:);
        numLines = size(linesCurr, 1);

        vLineCurr = modelPlane.plane{i}.vLine;

        % Rectified homography
        H = eye(3);
        H(3,:) = vLineCurr;

        linesStart = cat(2, linesCurr(:,1), linesCurr(:,2),ones(numLines, 1))';
        linesEnd = cat(2, linesCurr(:,3), linesCurr(:,4),ones(numLines, 1))';

        linesStartRect = H*linesStart;
        linesStartRect = linesStartRect./repmat(linesStartRect(3,:), 3, 1);

        linesEndRect = H*linesEnd;
        linesEndRect = linesEndRect./repmat(linesEndRect(3,:), 3, 1);

        linesVec = linesStartRect(1:2, :) - linesEndRect(1:2, :);
        linesSign = linesEndRect(2,:) > linesStartRect(2, :);
        linesSign = 2*linesSign - 1;
        linesLength = sqrt(sum(linesVec.^2, 1));
        linesCos = linesSign.*linesVec(1,:)./linesLength; % repmat(linesLength, 2, 1);

        theta = acos(linesCos);

        % Estimate average theta so that all the supporting lines aligned
        % with the x-axis
        thetaAvg = mean(theta, 2);
        for iter = 1: 5
            thetaDiff = theta - thetaAvg;
            indLargeTheta = thetaDiff > pi/2;
            theta(indLargeTheta) = pi - theta(indLargeTheta);

            indSmallTheta = thetaDiff < -pi/2;
            theta(indSmallTheta) = pi + theta(indSmallTheta);
            thetaAvg = mean(theta, 2);
        end

        thetaEst = thetaAvg;

        modelPlane.plane{i}.rotPar(vpInd) = thetaEst;
    end
end

% === Add a fronto-parallel plane ===

modelPlane.plane{indPlane}.vLine = [0 0 1];
modelPlane.plane{indPlane}.imgPlaneProb = optA.fpPlaneProb*ones(imgH, imgW);
modelPlane.plane{indPlane}.score = sum(modelPlane.plane{indPlane}.imgPlaneProb(:));
modelPlane.plane{indPlane}.rotPar(1) = 0;
modelPlane.plane{indPlane}.rotPar(2) = 0;

numPlane = numPlane + 1;

modelPlane.numPlane = numPlane;

% === Compute posterior probability ===

planeProb = zeros(imgH, imgW, numPlane);
for i = 1 : numPlane
    planeProb(:,:,i) = modelPlane.plane{i}.imgPlaneProb;
end
planeProbSum = sum(planeProb, 3);
planeProb = (planeProb)./repmat(planeProbSum, [1, 1, numPlane]);

modelPlane.postProbHole = planeProb;

% === Propagate posterior probability into the hole region ===

% Get border pixels
%     borderImg = im2double(mask);
%     borderImg = imfilter(borderImg, fspecial('gaussian'));
%     borderImg = (borderImg~=0) & (borderImg~=1);
%     borderImg = borderImg & ~mask;
[distMap, idMap] = bwdist(~mask, 'euclidean');

maskInt = mask;
maskInt(1,:) = 0;   maskInt(end,:) = 0;
maskInt(:,1) = 0;   maskInt(:,end) = 0;

for i = 1:numPlane
    planeProbCh = planeProb(:,:,i);
    planeProb(:,:,i) = planeProbCh(idMap);
    planeProb(:,:,i) = roifill(planeProb(:,:,i), maskInt);
end

planeProbSum = sum(planeProb, 3);
planeProb = (planeProb)./repmat(planeProbSum, [1, 1, numPlane]);

planeProbSum = 1 + numPlane*optA.probConst;
planeProb = (planeProb + optA.probConst)/planeProbSum;

if(0)

    % Get border pixels
    borderImg = im2double(mask);
    borderImg = imfilter(borderImg, fspecial('gaussian'));
    borderImg = (borderImg~=0) & (borderImg~=1);
    borderImg = borderImg & ~mask;

    holeImg = zeros(imgH, imgW) + inf;
    holeImg(borderImg) = 0;
    [~, neighbors] = vl_imdisttf(single(holeImg)) ;
    [v_, u_] = ind2sub([imgH, imgW], neighbors);

    [u, v] = meshgrid(1:imgW,1:imgH);

    holeProb = zeros(imgH, imgW, numPlane);
    for i = 1:numPlane
        planeProbCh = planeProb(:,:,i);
        holeProb(:,:,i) = planeProbCh(neighbors);
    end
    holeProbSum = sum(holeProb, 3);
    holeProb = holeProb./repmat(holeProbSum, [1, 1, numPlane]);

    maskPlane = repmat(mask, [1,1,numPlane]);
    planeProb = (~maskPlane).*planeProb + maskPlane.*holeProb;

end

modelPlane.postProb = planeProb;

end

function vLine = vLineFromTwoVP(vp1, vp2)

A = cat(1, vp1, vp2);

[U S V] = svd(A, 0);
vLine = V(:,end);
vLine = vLine/vLine(3); % [h7, h8, 1]

end


function [fig]=mvg_drawWindows(img,windows,fig)
% function [fig]=mvg_drawWindows(img,windows,numBest) draws the given windows
% on the given input image.
%
% Inputs:
% img, imgRow*imgCol*1 or 3, double, is the input image.
% windows, numWindows*4, double, is a matrix containing the windows.
%                                   Each row corresponds to one window in format:
%                                   windows(i,:)=[xmin,ymin,xmax,ymax];
% fig, 1*1, double, is a figure handle pointing to figure where image and
%                   windows are drawn. If not given, new figure will be
%                   created.
%
% Outputs:
% fig, 1*1, double, is a figure handle pointing to the figure where image
%                   and windows were drawn.
%

% 2011 MVG, Oulu, Finland, Esa Rahtu and Juho Kannala
% 2011 VGG, Oxford, UK, Matthew Blaschko

%% User defined parameters
linewidth = 3;
base_color = [1 0 0];

%% Open figure if needed
if nargin<3
    fig=figure;
end

%% Draw image
figure(fig); clf;
imshow(img);

%% Draw windows
hold on;
for idx = 1:size(windows,1)
    plot(windows(idx,[1 3 3 1 1]),windows(idx,[2 2 4 4 2]),'Color',base_color,'linewidth',linewidth);
end