function [u,p,info] = mgstokes(A,B,f,g,u,p,elem,freeDof,option)
%% MGSTOKES: multigrid methods for Stokes equations
%
% Created by Ming Wang and Jie Zhou on Mar,3,2013 for P2 elements. Add
% other elements by Jie and Long on Mar 24,2013.

t = cputime;

%% Size of systems
N = max(elem(:));
Ndof = length(f) + length(g);

%% Options
% Assign default values to unspecified parameters
if ~exist('option','var'),
    option = [];
end
option = mgoptions(option,Ndof);    % parameters
tol = option.tol; maxIt = option.solvermaxit;
printlevel = option.printlevel;
solver = option.solver;

%% Mesh on each level
N0 = 10;
HB = zeros(N,3);
level = 20;
NL(level+1) = N; % now NL(1:level) = 0;
elemi = cell(level,1);
elemi{level} = elem;
for j = level: -1 : 2
    [elemi{j-1},newHB] = uniformcoarsenred(elemi{j});  % try coasen red refinement
    if (isempty(newHB)) || (size(elemi{j-1},1)< 2*N0)
    % no nodes are removed or it reaches the coarsest level
        NL = NL(j:end);
        break;
    end
    NL(j) = NL(j+1) - size(newHB,1); % update NL(k)
    HB(NL(j)+1:NL(j+1),1:3) = newHB(:,1:3);
end
level = length(NL)-1;    % actual level
elemi = elemi(end-level+1:end);

%% Transfer operators between levels
Pro_u = cell(level,1);
Res_u = cell(level,1);
Pro_p = cell(level,1);
Res_p = cell(level,1);

switch(upper(option.elemType(1:end-2))) % transfer operator of velocity
    case 'P2'
        P2FreeDof = freeDof(1:end/2);   % again only one component
        for i = level:-1:2
            [ProU0,P2FreeDof] = nodeP2TansferUniform(elemi{i-1},elemi{i},P2FreeDof);
            Pro_u{i-1} = ProU0;
            Res_u{i}   = ProU0';
        end
    case 'CR'
        CRfreeDof = freeDof(1:end/2);
        for i=level:-1:2
            [ProU0,CRfreeDof] = transferCRred(elemi{i-1},elemi{i},CRfreeDof);
            Pro_u{i-1} = ProU0;
            Res_u{i}   = ProU0';
        end
    case 'P1B'
        for i=level:-1:2
            Pro_u{i-1} = speye(length(ufreeDof),length(freeEdge));
            Res_u{i} = Pro_u{i-1}';
        end
    case 'ISOP2'
        P2FreeDof = freeDof(1:end/2);  % again only one component
        for i = level:-1:2
            [ProU0,P2FreeDof] = nodeisoP2TansferUniform(elemi{i-1},elemi{i},P2FreeDof);
            Pro_u{i-1} = ProU0;
            Res_u{i}   = ProU0';
        end
end
switch upper(option.elemType(end-1:end))
    case  'P0'
        for k = level:-1:2
            Np(k-1)    = size(elemi{k-1},1);
            Pro_p{k-1} = repmat(speye(Np(k-1)),4,1);
            Res_p{k} = Pro_p{k-1}';
        end
    case  'P1'
       [Pro_p,Res_p] = transferoperator(HB,NL);
end

%% Matrices in each level
% size of u and p
Nu = zeros(level,1);
Np = zeros(level,1);
Nu(level) = size(A,1)/2; % one component of velocity
Np(level) = size(B,1);

Ai = cell(level,1);
Bi = cell(level,1);
bigAi = cell(level,1);
Ai{level} = A;
Bi{level} = B;
bigAi{level} = [Ai{level} Bi{level}'; Bi{level} sparse(Np(level),Np(level))];

for j = level:-1:2
    Ai{j-1} = blkdiag(Res_u{j},Res_u{j})*Ai{j}*blkdiag(Pro_u{j-1},Pro_u{j-1});  % Ac = Res*Af*Pro
    Bi{j-1} = Res_p{j}*Bi{j}*blkdiag(Pro_u{j-1},Pro_u{j-1});
    Nu(j-1) = size(Ai{j-1},1)/2;
    Np(j-1) = size(Bi{j-1},1);
    bigAi{j-1} = [Ai{j-1} Bi{j-1}'; Bi{j-1} sparse(Np(j-1),Np(j-1))];
end
Ndof = 2*Nu+Np;

%% matrix used for smoothing
if isfield(option,'smootherType')
smoother = option.smootherType;
else
smoother ='LSCDGS';
end
auxMat = cell(level,1);
switch(smoother)
    case 'LSCDGS'
        for k = 2:level
            Bt = (Bi{k})';
            BBt = Bi{k}*Bt;
            BABt = Bi{k}*Ai{k}*Bt;
            Su = tril(Ai{k});
            Sp = tril(BBt);
            Spt = triu(BBt);
            DSp = diag(BBt);
            auxMat{k} = struct('Bt',Bt,'BBt',BBt,'BABt',BABt,'Su',Su,...
                               'Spt',Spt,'Sp',Sp,'DSp',DSp);
        end
    case 'IUzawa'
        for k = 2:level
            Bt = (Bi{k})';
            DA = 2*diag(Ai{k});
            invDA = spdiags(1./DA,0,2*Nu(k),2*Nu(k));
            BinvDABt = Bi{k}*invDA*Bt;
            auxMat{k} = struct('Bt',Bt,'DA',DA,'BinvDABt',BinvDABt);
        end
end
Ai_IU = cell(level,1);
Si_IU = cell(level,1);
SSi_IU = cell(level,1);
Res_IU = cell(level,1);
Pro_IU = cell(level,1);
if strcmp(option.smootherbarSp,'VCYCLE')
%     option.smootherbarSp = 'VCYCLE';
    innerMGoption.solver = 'NO';
    innerMGoption.N0 = 10;
    for j = 2:level
        [tempvar,tempvar,Ai_IU{j},Si_IU{j},SSi_IU{j},Res_IU{j},Pro_IU{j}] = ...
                mg(auxMat{j}.BBt,ones(Np(j),1),elemi{j},innerMGoption);
    end
end

%% Solver
%  initial set up
bigF = [f; g-mean(g)];
bigu = [u; p];
bigr = bigF - bigAi{level}*bigu;
nb = norm(bigF);
err = zeros(maxIt,1);
err(1) = norm(bigr)/nb;
k = 1;

while (max(err(k)) > tol) && (k <= maxIt)
    k = k + 1;
    switch (solver)
        case 'VCYCLE'
            bigerru = vcycle(bigr);
        case 'WCYCLE'
            bigerru = wcycle(bigr);
    end
    bigu = bigu+bigerru;
    bigr = bigr - bigAi{level}*bigerru;
    % compute the relative error
    err(k) = norm(bigr)/nb;
    if printlevel >= 2
        fprintf('#dof: %8.0u, MG %8s iter: %2.0u, err = %8.4e\n',...
            Ndof(level), solver,k-1, err(k));
    end
end
err = err(1:k);
itStep = k-1;
u = bigu(1:2*Nu(level));
p = bigu(2*Nu(level)+1:end);

%% Output
if k > maxIt
    flag = 1;
else
    flag = 0;
end
time = cputime-t;
if printlevel >= 1
    fprintf('#dof: %6.0u,  #nnz: %6.0u, level: %2.0u  MG %6s iter: %2.0u,  err = %8.4e,  time = %4.2g s\n',...
             Ndof(level), nnz(bigAi{level}), level, solver, itStep, err(end), time);
end
if (flag == 1) && (printlevel>0)
   fprintf('NOTE: the iterative method does not converge! \n');
end
info = struct('solverTime',time,'itStep',itStep,'error',err,'flag',flag,'stopErr',max(err(end,:)));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunctions vcycle, wcycle, smoothing
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% subfunctions v-cycle
     function e = vcycle(r,J)
        if nargin < 2, J = level; end
        if J == 1 % solver in the coaresest grid
            e = zeros(size(r));
            e(1:end-1) = bigAi{J}(1:end-1,1:end-1)\r(1:end-1);
            e(2*Nu(J)+1:end) = e(2*Nu(J)+1:end)-mean(e(2*Nu(J)+1:end));
            return
        end
        ru = r(1:2*Nu(J)); rp = r(2*Nu(J)+1:end);

        % pre-smoothing
        [eu,ep] = smoothing(zeros(2*Nu(J),1),zeros(Np(J),1),ru,rp,J);

        % form residual and restrict onto coarse grid
        rru = ru-Ai{J}*eu-(Bi{J})'*ep;
        rrp = rp-Bi{J}*eu;
        ruc = reshape(Res_u{J}*reshape(rru,Nu(J),2),2*Nu(J-1),1);
        rpc = Res_p{J}*rrp;

        % coarse grid correction
        rc = [ruc;rpc];
        ec = vcycle(rc,J-1);

        % correction on the fine grid
        tempeu = reshape(Pro_u{J-1}*reshape(ec(1:2*Nu(J-1)),Nu(J-1),2),2*Nu(J),1);
        tempep = Pro_p{J-1}*ec(2*Nu(J-1)+1:end);
        eu = tempeu + eu;
        ep = tempep + ep;

        % post-smoothing
        [eu,ep] = smoothing(eu,ep,ru,rp,J);
        e = [eu;ep];

     end

 %% Wcycle MG
     function e = wcycle(r,J)
        if nargin < 2, J = level; end
        if J == 1 % solver in the coaresest grid
            e = zeros(size(r));
            e(1:end-1) = bigAi{J}(1:end-1,1:end-1)\r(1:end-1);
            e(2*Nu(J)+1:end) = e(2*Nu(J)+1:end)-mean(e(2*Nu(J)+1:end));
            return
        end
        ru = r(1:2*Nu(J));
        rp = r(2*Nu(J)+1:end);
%
        % pre-smoothing in the fine grid
        [eu,ep] = smoothing(zeros(2*Nu(J),1),zeros(Np(J),1),ru,rp,J);

        % form residual and restrict onto coarse grid
        rru = ru - Ai{J}*eu-(Bi{J})'*ep;
        rrp = rp - Bi{J}*eu;
        ruc = reshape(Res_u{J}*reshape(rru,Nu(J),2),2*Nu(J-1),1);
        rpc = Res_p{J}*rrp;

        % coarse grid correction
        rc = [ruc;rpc];
        ec = wcycle(rc,J-1);
        % once more for w-cycle
        ec = ec+ wcycle(rc - bigAi{J-1}*ec,J-1);

        % correction on the fine grid
        tempeu = reshape(Pro_u{J-1}*reshape(ec(1:2*Nu(J-1)),Nu(J-1),2),2*Nu(J),1);
        tempep = Pro_p{J-1}*ec(2*Nu(J-1)+1:end);
        eu = eu + tempeu;
        ep = ep + tempep;

        % post-smoothing in the fine grid
        [eu,ep] = smoothing(eu,ep,ru,rp,J);
        e = [eu;ep];
     end

 %% Smoothing
    function [u,p] = smoothing(u,p,f,g,J)
        if strcmp(smoother,'LSCDGS')
            [u,p] = StokesLSCDGS(u,p,f,g,Ai{J},Bi{J},auxMat{J},elemi{J},option,...
                                 Ai_IU{J},Si_IU{J},SSi_IU{J},Res_IU{J},Pro_IU{J});
        elseif strcmp(smoother,'BDDGS')
            % e = StokesBiDGStri(Ai{J}, Bi{J}, e, r, smoothStep, invDM{J},area{J});
        elseif strcmp(smoother,'iUzawa')
            [u,p] = StokesIUzawa(u,p,f,g,Ai{J},Bi{J},auxMat{J},elemi{J},option,...
                                 Ai_IU{J},Si_IU{J},SSi_IU{J},Res_IU{J},Pro_IU{J});
        end
    end
end


function [y_rvm] = rvr_predict(X,  model)
% RVR_PREDICT % Predicts labels and computes accuracy for new
% data with a learnt RVM model
%   input ----------------------------------------------------------------
%
%       o X        : (N x D), N  input data points of D dimensionality.
%
%
%
%   output ----------------------------------------------------------------
%
%       o y_rvm            : (N x 1), decision values
%
%
%% Predict labels given model and data

%%%%%% Test
weights    = model.weights;
kernel_    = model.kernel_;
width      = model.width;
bias       = model.bias;
RVs_idx    = model.RVs_idx;
RVs        = model.RVs;

% Compute RVM over test data and calculate error
PHI	= SB1_KernelFunction(X(:), RVs, kernel_, width);
y_rvm	= PHI*weights + bias;

end


function TC = rotate(TC,varargin)

%Rotate component orientations
% TC = ROTATE(TC,BEARING) Rotates the horizontal components of a threecomp
% object so that the first horizontal component (N, R or 1) points in the
% direction given by BEARING. If the rotation is to north/east or
% radial/transverse then the horizontral channel names will be renamed to
% ??N/??E or ??R/??T accordingly. If the rotation is to an arbitrary
% direction, the channels will be renamed to ??2/??1.
%
% TC = ROTATE(TC) is shorthand for ROTATE(TC,-BACKAZIMUTH). This common
% usage rotates horizontal traces to the radial and transverse directions,
% where radial is inline (and away) from the source. The transverse
% direction points +90 from the radial. For example,for a wave arriving
% from due south, there is no rotation and N+ becomes R+, while E+ becomes
% T+. This useage requires that the backAzimuth property be set.

% TODO: DISALLOW NO BACKAZIMUTH AND NO ARGUMENT

% CHECK ARGUMENTS AND GET FINAL BEARING
if length(varargin)>=1  % bearing provided by user
    bearing = varargin{1};
    if ~isa(bearing,'double')
       error('Threecomp:rotate:badArgumentType','Rotation angle must be a number');
    end
    if bearing<-360 || bearing>720
       error('Threecomp:rotate:badArgumentValue','Rotation angle must be between 0 and 360');
    end
else
    bearing = NaN * zeros(size(TC));
    for n = 1:numel(TC)
        if ~isempty(TC(n).backAzimuth)
            bearing(n) = TC(n).backAzimuth+180;
        end
        bearing = reshape(bearing,size(TC));
        bearing = mod(bearing,360);
    end
end

% COMPUTE ROTATION ANGLES
orientation = get(TC,'ORIENTATION');
for n = 1:length(TC)
   if isempty(orientation)
       error('ThreeComp:rotate:noOrientations','Component orientations must be provided');
   end
end
rotAngle = mod(bearing-orientation(:,3),360);

% APPLY ROTATION
for n = 1:length(TC)
    data = double(TC(n).traces(2:3))';
    rotMatrix = [cosd(rotAngle(n)) sind(rotAngle(n)) ; -sind(rotAngle(n)) cosd(rotAngle(n))];
    Dout = rotMatrix * data;
    TC(n).traces(2) = set(TC(n).traces(2),'DATA',Dout(1,:));
    TC(n).traces(3) = set(TC(n).traces(3),'DATA',Dout(2,:));
    newOrientation = TC(n).orientation;
    newOrientation([3 5]) = mod(newOrientation([3 5])+rotAngle(n),360);
    TC(n).orientation = newOrientation;
end

% ADJUST CHANNEL NAMES
for n = 1:length(TC)
    channels = get(TC(n),'CHANNEL');
    chan2 = channels{1}(2);
    chan2 = chan2{1};
    chan3 = channels{1}(3);
    chan3 = chan3{1};
    if abs(TC(n).orientation(3)-0)<0.5
        chan2(3) = 'N';
        chan3(3) = 'E';
    elseif abs(mod((TC(n).orientation(3)+180),360)-TC(n).backAzimuth)<0.5
        chan2(3) = 'R';
        chan3(3) = 'T';
    else
        chan2(3) = '2';
        chan3(3) = '1';
    end
    TC(n).traces(2) = set(TC(n).traces(2),'CHANNEL',chan2);
    TC(n).traces(3) = set(TC(n).traces(3),'CHANNEL',chan3);
end


function [x,I,exitFlag]=getMinNrFluxes(model, toMinimize, params,scores)
% getMinNrFluxes
%   Returns the minimal set of fluxes that satisfy the model using
%   mixed integer linear programming.
%
%	model         a model structure
%   toMinimize    either a cell array of reaction IDs, a logical vector
%                 with the same number of elements as reactions in the model,
%                 of a vector of indexes for the reactions that should be
%                 minimized (opt, default model.rxns)
%   params        parameter structure as used by getMILPParams (opt)
%   scores        vector of weights for the reactions. Negative scores
%                 should not have flux. Positive scores are not possible in this
%                 implementation, and they are changed to max(scores(scores<0)).
%                 Must have the same dimension as toMinimize (find(toMinimize)
%                 if it is a logical vector) (opt, default -1 for all reactions)
%
%   x             the corresponding fluxes for the full model
%   I             the indexes of the reactions in toMinimize that were used
%                 in the solution
%   exitFlag      1: optimal solution found
%                -1: no feasible solution found
%                -2: optimization time out
%
%   NOTE: Uses 1000 mmol/gDW/h as an arbitary large flux. Could possibly
%   cause problems if the fluxes in the model are larger than that.
%
%   Usage: [x,I,exitFlag]=getMinNrFluxes(model, toMinimize, params, scores)

exitFlag=1;

if nargin<2
    toMinimize=model.rxns;
elseif ~islogical(toMinimize) && ~isnumeric(toMinimize)
    toMinimize=convertCharArray(toMinimize);
else
    toMinimize=model.rxns(toMinimize);
end

%For passing parameters to the solver
if nargin<3
    params=struct();
end

if nargin<4
    %It says that the default is -1, but that is to fit with other code
    scores=ones(numel(toMinimize),1)*1;
else
    if numel(scores)~=numel(toMinimize)
        EM='The number of scores must be the same as the number of reactions to minimize';
        dispEM(EM);
    end

    %Change positive scores to have a small negative weight. This is a
    %temporary solution.
    scores(scores>=0)=max(scores(scores<0));

    %It says that the default is -1, but that is to fit with other code
    scores=scores*-1;
end

%Check if the model is in irreversible format
if any(model.rev)
    %Convert the model to irreversible format
    irrevModel=convertToIrrev(model);

    %Find the indexes for the reactions in toMinimize
    [indexes, I]=ismember(strrep(irrevModel.rxns,'_REV',''),toMinimize);
else
    irrevModel=model;

    %Find the indexes for the reactions in toMinimize
    [indexes, I]=ismember(irrevModel.rxns,toMinimize);
end

indexes=find(indexes);
%Adjust scores to fit with reversible
scores=scores(I(indexes));

%Add binary constraints in the following manner: -  Add one unique
%"metabolite" for each integer reaction as a substrate.
%   These metabolites can have net production
%-  Add reactions for the production of each of those metabolites. The
%   amount produced in one reaction unit must be larger than the largest
%   possible flux in the model (but not too large to avoid bad scaling)

%Calculate a solution to the problem without any constraints. This is to
%get an estimate about the magnitude of fluxes in the model and to get a
%feasible start solution.
sol=solveLP(irrevModel,1);

%Return an empty solution if the non-constrained problem couldn't be solved
if isempty(sol.x)
    x=[];
    I=[];
    exitFlag=-1;
    return;
end

%Take the maximal times 5 to have a safe margin. If it's smaller than 1000,
%then use 1000 instead.
maxFlux=max(max(sol.x)*5,1000);

intArray=speye(numel(irrevModel.rxns))*-1;
intArray=intArray(indexes,:);
prob.a=[irrevModel.S;intArray];
a=[sparse(numel(irrevModel.mets),numel(indexes));speye(numel(indexes))*maxFlux];
prob.a=[prob.a a];
prob.ints.sub=numel(irrevModel.rxns)+1:numel(irrevModel.rxns)+numel(indexes);

prob.c=[zeros(numel(irrevModel.rxns),1);scores(:);zeros(size(prob.a,1),1)]; %Minimize the number of fluxes
prob.A=[prob.a -speye(size(prob.a,1))];
prob.blc=[irrevModel.b(:,1);zeros(numel(indexes),1)];
if size(irrevModel.b,2)==2
    prob.buc=[irrevModel.b(:,2);inf(numel(indexes),1)];
else
    prob.buc=[irrevModel.b(:,1);inf(numel(indexes),1)];
end
prob.blx=[irrevModel.lb;zeros(numel(indexes),1)];
prob.bux=[irrevModel.ub;ones(numel(indexes),1)];
prob.lb = [prob.blx; prob.blc];
prob.ub = [prob.bux; prob.buc];
prob.osense=1;
prob.csense=repmat('E', 1, size(prob.a,1),1);
prob.b=zeros(size(prob.a,1), 1);

%Use the output from the linear solution as starting point. Only the values
%for the integer variables will be used, but all are supplied.
prob.sol.int.xx=zeros(numel(prob.c),1);
prob.sol.int.xx(prob.ints.sub(sol.x(indexes)>10^-12))=1;
prob.x0=[];
prob.vartype=repmat('C', size(prob.A,2), 1);
prob.vartype(prob.ints.sub) = 'I'; % with .lb = 0 and .ub = 1, they are binary
% integers (glpk in octave only allows 'continuous' or '', not 'binary')
prob=rmfield(prob,{'blx','bux','blc','buc'});

% Optimize the problem
res = optimizeProb(prob,params);
isFeasible=checkSolution(res);

if ~isFeasible
    x=[];
    I=[];
    exitFlag=-1;
    return;
end

xx=res.full(1:numel(irrevModel.rxns));
I=res.full(numel(xx)+1:end);

%Check if Mosek aborted because it reached the time limit
%TODO: modify for cobra/gurobi
% if strcmp('MSK_RES_TRM_MAX_TIME',res.rcode)
%     exitFlag=-2;
% end

%Map back to original model from irrevModel
x=xx(1:numel(model.rxns));
if numel(irrevModel.rxns)>numel(model.rxns)
    x(model.rev~=0)=x(model.rev~=0)-xx(numel(model.rxns)+1:end);
end

I=ismember(toMinimize,strrep(irrevModel.rxns(indexes(I>10^-12)),'_REV',''));
end


function [GB] = PB2GB(PB)
% Convert computery things from petabytes to gigabytes.
% Chad A. Greene 2012
GB = PB*1048576;


function [truth, normal_axis] = is2dlike(N)

chkarg(istypesizeof(N, 'int', [1, Axis.count]), ...
	'"N" should be length-%d row vector with integer elements.');

[~, imin] = min(N);
n = Axis.elems(imin);
[h, v] = cycle(n);

truth =  N(n)/N(h) < 0.1 && N(n)/N(v) < 0.1;
normal_axis = n;


function linplus_test0192 ( )

%*****************************************************************************80
%
%% TEST0192 tests R8CC_WRITE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2009
%
%  Author:
%
%    John Burkardt
%
  m = 5;
  n = 5;
  nz_num = 12;

  a_file = 'r8cc_a.txt';
  col = [ 1, 4, 6, 8, 10, 13 ];
  col_file = 'r8cc_col.txt';
  row = [ 1, 2, 4, 1, 2, 3, 5, 4, 5, 1, 2, 5 ];
  row_file = 'r8cc_row.txt';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST0192\n' );
  fprintf ( 1, '  For a matrix in the R8CC format,\n' );
  fprintf ( 1, '  (double precision compressed column sparse)\n' );
  fprintf ( 1, '  R8CC_WRITE writes the matrix to 3 files.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix rows M     = %d\n', m );
  fprintf ( 1, '  Matrix columns N  = %d\n', n );
  fprintf ( 1, '  Nonzeros NZ_NUM   = %d\n', nz_num );

  i4vec_print ( n+1, col, '  The COL vector:' );

  i4vec_print ( nz_num, row, '  The ROW vector:' );

  a = r8cc_indicator ( m, n, nz_num, col, row );

  r8cc_print ( m, n, nz_num, col, row, a, '  The R8CC matrix:' );

  r8cc_write ( col_file, row_file, a_file, m, n, nz_num, col, row, a );

  return
end


function results = test_sscor(eeg, model, is_ensemble)
% Test phase of the sum of squared correlations (SSCOR)-based
% steady-state visual evoked potentials (SSVEPs) detection [1].
% * This script is the exact same as test_trca.m except for the function
% name.
%
% function results = test_sscor(eeg, model, is_ensemble)
%
% Input:
%   eeg             : Input eeg data
%                     (# of targets, # of channels, Data length [sample])
%   model           : Learning model for tesing phase of the ensemble
%                     SSCOR-based method
%   is_ensemble     : 0 -> SSCOR-based method,
%                     1 -> Ensemble SSCOR-based method (defult: 1)
%
% Output:
%   results         : The target estimated by this method
%
% See also:
%   train_sscor.m
%
% Reference:
%   [1] G. R. Kumar and M. R. Reddy,
%       "Designing a Sum of Squared Correlations Framework for Enhancing SSVEP
%        Based BCIs",
%       IEEE Trans. Neural Syst. Rehabil. Eng., vol. 27, pp. 2044-2050, 2019.
%
% Kuan-Jung Chiang and Masaki Nakanishi, 25-Nov-2019
% Swartz Center for Computational Neuroscience, Institute for Neural
% Computation, University of California San Diego
% E-mail: masaki@sccn.ucsd.edu

if ~exist('is_ensemble', 'var') || isempty(is_ensemble)
    is_ensemble = 1; end

if ~exist('model', 'var')
    error('Training model based on SSCOR is required. See train_sscor().');
end

fb_coefs = [1:model.num_fbs].^(-1.25)+0.25;

for targ_i = 1:1:model.num_targs
    test_tmp = squeeze(eeg(targ_i, :, :));
    for fb_i = 1:1:model.num_fbs
        testdata = filterbank(test_tmp, model.fs, fb_i);
        for class_i = 1:1:model.num_targs
            traindata =  squeeze(model.trains(class_i, fb_i, :, :));
            if ~is_ensemble
                w = squeeze(model.W(fb_i, class_i, :));
            else
                w = squeeze(model.W(fb_i, :, :))';
            end
            r_tmp = corrcoef(testdata'*w, traindata'*w);
            r(fb_i,class_i) = r_tmp(1,2);
        end % class_i
    end % fb_i
    rho = fb_coefs*r;
    [~, tau] = max(rho);
    results(targ_i) = tau;
end % targ_i


function Z = iminpaint(im,M)
% Z = iminpaint(im,M)
%
% Inputs:
%   im  w by h by c image
%   M  w by h mask
% Outputs:
%   Z  w by h by c image with mask in-painted with Laplacian
%
  if islogical(M)
    assert(numel(M) == size(im,1)*size(im,2));
    if any(size(M)==1)
      M = reshape(M,size(im,1),size(im,2));
    end
  end
  L = fd_laplacian(size(im(:,:,1)));
  b = find(M);
  X = reshape(im,[],size(im,3));
  Z = min_quad_with_fixed(-L,[],b,X(b,:),[],[],struct('force_Aeq_li',true));
  Z = reshape(Z,size(im));
end


function epsn = ReduceBoundary(eF, k, MaxK, cp)
% The shrink of the dynamic constraint boundary

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

% This function is written by Ruwang Jiao

    z        = 1e-8;
    Nearzero = 1e-15;
    B        = MaxK./power(log((eF + z)./z), 1.0./cp);
    B(B==0)  = B(B==0) + Nearzero;
    f        = eF.* exp( -(k./B).^cp );
    tmp      = find(abs(f-z) < Nearzero);
    f(tmp)   = f(tmp).*0 + z;
    epsn     = f - z;
    epsn(epsn<=0) = 0;
end


function [strategy, MEU, niter] = solve_limid(engine, varargin)
% SOLVE_LIMID Find the (locally) optimal strategy for a LIMID
% [strategy, MEU, niter] = solve_limid(inf_engine, ...)
%
% strategy{d} = stochastic policy for node d (a decision node)
% MEU = maximum expected utility
% niter = num iterations used
%
% The following optional arguments can be specified in the form of name/value pairs:
% [default in brackets]
%
% max_iter - max. num. iterations [ 1 ]
% tol - tolerance required of consecutive MEU values, used to assess convergence [1e-3]
% order - order in which decision nodes are optimized [ reverse numerical order ]
%
% e.g., solve_limid(engine, 'tol', 1e-2, 'max_iter', 10)

bnet = bnet_from_engine(engine);

% default values
max_iter = 1;
tol = 1e-3;
D = bnet.decision_nodes;
order = D(end:-1:1);

args = varargin;
nargs = length(args);
for i=1:2:nargs
  switch args{i},
   case 'max_iter', max_iter  = args{i+1};
   case 'tol',      tol = args{i+1};
   case 'order',    order = args{i+1};
   otherwise,
    error(['invalid argument name ' args{i}]);
  end
end

CPDs = bnet.CPD;
ns = bnet.node_sizes;
N = length(ns);
evidence = cell(1,N);
strategy = cell(1, N);

iter = 1;
converged = 0;
oldMEU = 0;
while ~converged & (iter <= max_iter)
  for d=order(:)'
    engine = enter_evidence(engine, evidence, 'exclude', d);
    [m, pot] = marginal_family(engine, d);
    %pot = marginal_family_pot(engine, d);
    [policy, score] = upot_to_opt_policy(pot);
    e = bnet.equiv_class(d);
    CPDs{e} = set_fields(CPDs{e}, 'policy', policy);
    engine = update_engine(engine, CPDs);
    strategy{d} = policy;
  end
  engine = enter_evidence(engine, evidence);
  [m, pot] = marginal_nodes(engine, []);
  %pot = marginal_family_pot(engine, []);
  [dummy, MEU] = upot_to_opt_policy(pot);
  if approxeq(MEU, oldMEU, tol)
    converged = 1;
  end
  oldMEU = MEU;
  iter = iter + 1;
end
niter = iter - 1;


%% Setup
p = get_default_truck_trailer_params();
p.noiseLevel = 0.01;
p.trailerWheelbase = 11;
p.forwardTarget = [20 2 0.3];
xx = [0 -5 0 0];
opt = odeset('Events', @obstacleEvents);
dt = 0.2;
p.forwardGain = [0 0 0];
p.feedbackGain = [0 0 0];
%% setup viz
figure(101)
ax(1) = subplot(131);
tp1 = truck_trailer_plot(p, ax(1));
tp1.updateFig([0 -5 0 0])
title('Initial Pose')
ax(2) = subplot(132);
tp2 = truck_trailer_plot(p, ax(2));
tp2.updateFig([p.forwardTarget 0]);
title('Intermediate Target')
ax(3) = subplot(133);
tp2 = truck_trailer_plot(p, ax(3));
title('Final Target')

%% setup viz
figure(101)
ax(1) = subplot(121);
tp1 = truck_trailer_plot(p, ax(1));
tp1.updateFig([0 -5 0 0])
title('Initial Pose')
ax(2) = subplot(122);
tp2 = truck_trailer_plot(p, ax(2));
title('Desired Target')

set(gcf, 'Color', 'w');
export_fig ttproblem.png

%% setup viz
figure(101)
ax(1) = subplot(121);
tp1 = truck_trailer_plot(p, ax(1));
tp1.updateFig([20 2 0.3 0])
title('Intermediate Target #1')
ax(2) = subplot(122);
tp2 = truck_trailer_plot(p, ax(2));
tp2.updateFig([20 0 0  0])
title('Intermediate Target #2')
%%
set(gcf, 'Color', 'w');
export_fig ttinter.png


function Population = EnvironmentSelection(Population,N)
% The environmental selection of NSGA-II

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    %% Non-dominated sorting
    PopObj = Population.objs;
    [FrontNo,MaxFNo] = NDSort(PopObj,N);
    Next   = false(1,length(FrontNo));
    Next(FrontNo<MaxFNo) = true;

    %% Calculate the crowding distance of each solution
    CrowdDis = CrowdingDistance(PopObj,FrontNo);

    %% Select the solutions in the last front based on their crowding distances
    Last     = find(FrontNo==MaxFNo);
    [~,Rank] = sort(CrowdDis(Last),'descend');
    Next(Last(Rank(1:N-sum(Next)))) = true;

    %% Population for next generation
    Population = Population(Next);
end


clc;

close all;
clear all;

disp('Running all the experiments together')

%featuriziation
% comment out when done with featurizer
promptresponse = input('would you like to featurize? [Y/N enter for N]', 's');
if (promptresponse=='y' || promptresponse=='Y')

    [num,txt,raw] = xlsread('data\final104.xls');
    descriptions = raw(2:size(raw,1),2);
    style_ratings = num(1:size(num,1),1);
    comfort_ratings = num(1:size(num,1),4);
    overal_ratings = num(1:size(num,1),5);
    [featurs, headers] = featurize_bigram(descriptions,20,0,1);
    csvwrite('data\forWeka_featuresonly.csv', featurs);
    csvwrite('data\headers.csv', headers);
end

ccaregression_generalized
ccaregression_generalized_adjusted
linearRegression_generalized
naivbayes_generalized
regresstree_generalized
%svm_generalized


function plot_selectProfile(~, eventObj)

hFig = gcf;
idxPC = eventObj.Source.Parent.Parent.UserData.idxPC; % index of PC(s)

hAxes = gca;

try

    % Get new value
    view(2);
    hRotate = rotate3d;
    if strcmpi(hRotate.Enable, 'On'), reactivateRotate3d = true; else reactivateRotate3d = false; end

    % Var1: Select profile
    % [x, y] = ginput_2016a_mod(2);
    % lineStart = [x(1) y(1)];
    % lineEnd   = [x(2) y(2)];

    % Var2: Select profile
    [lineStart(1), lineStart(2)] = ginput_2016a_mod(1); % get start point
    zMax = max(get(gca, 'ZLim'));
    hStart = plot3(lineStart(1), lineStart(2), zMax, 'o', 'Color', 'r', 'MarkerSize', 5, 'LineWidth', 2);
    [lineEnd(1), lineEnd(2)] = ginput_2016a_mod(1); % get end point
    hEnd = plot3(lineEnd(1), lineEnd(2), zMax, 'o', 'Color', 'r', 'MarkerSize', 5, 'LineWidth', 2);
    hLine1 = plot3([lineStart(1); lineEnd(1)], [lineStart(2); lineEnd(2)], [zMax; zMax], '-', 'LineWidth', 3, 'Color', 'k');
    hLine2 = plot3([lineStart(1); lineEnd(1)], [lineStart(2); lineEnd(2)], [zMax; zMax], ':', 'LineWidth', 3, 'Color', 'w');

    % Define width
    prompt = {'Profile width:'};
    dlgTitle = '';
    noLines = 1;
    defaultAnswer = {'1'};
    answer = inputdlg(prompt, dlgTitle, noLines, defaultAnswer);
    delete(hStart); delete(hEnd); delete(hLine1); delete(hLine2); % delete points and line
    if ~isempty(answer{1})
        lineWidth = str2num(answer{1});
    else
        return;
    end

    if reactivateRotate3d, hRotate.Enable  = 'On'; end

catch

    error('Error selecting ''Profile''!'); return;

end

% Select profile
for i = 1:numel(idxPC)

    if i == 1, logLevelOrig = msg('O', 'GetLogLevel'); end
    msg('O', 'SetLogLevel', 'off');

    % Select profile!
    hFig.UserData.PC{idxPC(i)}.select('All');
    az = hFig.UserData.PC{idxPC(i)}.select('Profile', lineStart, lineEnd, lineWidth);

    msg('O', 'SetLogLevel', logLevelOrig);

end

view(az, 0);

% Update plot!
for i = 1:numel(idxPC)
    plot_setPrmAndPlot(hFig, idxPC(i));
end

end


function str = cvx_replicate_structure( str, sz )
%CVX_REPLICATE_STRUCTURE Apply matrix structure to N-D arrays.

szs  = size( str );
nmat = prod( sz );
omat = ones( 1, nmat );
[ r, c, v ] = find( str );
nelm = length( r );
oelm = ones( 1, nelm );
r = r( : );
nvec = ( 0 : nmat - 1 ) * szs( 1 );
r = r( :, omat ) + nvec( oelm, : );
c = c( : );
c = c( :, omat );
nvec = ( 0 : nmat - 1 ) * szs( 2 );
c = c( :, omat ) + nvec( oelm, : );
v = v( : );
v = v( :, omat );
str = sparse( r, c, v, nmat * szs( 1 ), nmat * szs( 2 ) );

% Copyright 2005-2014 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function modelPlane = sr_extract_plane(imgPath, imgName, opt)

% SR_EXTRACT_PLANE:
%
% Extract plane model from an image
%
% The code is adapted and modified from the paper
%
% Jia-Bin Huang, Sing Bing Kang, Narendra Ahuja, Johannes Kopf
% Image Completion using Planar Structure Guidance
% ACM Transactions on Graphics (Proceedings of SIGGRAPH 2014).
%
% 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

% =========================================================================
% Vanishing point detection
% =========================================================================

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

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

img = imread(fullfile(imgPath, imgName));

% =========================================================================
% Plane localization
% =========================================================================

modelPlane = sr_detect_plane_from_vp(vpData, img, opt);
% figure(1), imshow(modelPlane.postProb(:,:,1:3));

visVPFlag = 0;
if(visVPFlag)
    [vpVis, planeVis] = vis_vp(img, vpData, modelPlane.postProb(:,:,1:3));
end
end

function [vpVis, planeVis] = vis_vp(img, vpData, postProb)

img = im2double(img);

[imgH, imgW, nCh] = size(img);
vpVis = zeros(imgH, imgW, nCh);
planeVis = zeros(imgH, imgW, nCh);
whiteImg = ones(imgH, imgW, nCh);
% blackImg = zeros()
alphaW = 0.5;

imgW = img*(1 - alphaW) + whiteImg*(alphaW);

% Plot VP
h1 = figure(1);
imshow(imgW); hold on;
lineWidth = 4;
for vpInd = 1: vpData.numVP
    vpCurr = vpData.vp{vpInd};
    for lineInd = 1: vpCurr.numLines
        lineCur = vpCurr.lines(lineInd,:);
        lineCur = lineCur + 1;
        if(vpInd == 1)
            plot(lineCur([1,3]), lineCur([2,4]), 'r', 'LineWidth', lineWidth);
        elseif(vpInd == 2)
            plot(lineCur([1,3]), lineCur([2,4]), 'g', 'LineWidth', lineWidth);
        elseif(vpInd == 3)
            plot(lineCur([1,3]), lineCur([2,4]), 'b', 'LineWidth', lineWidth);
        end
    end
end
hold off;
print(h1, '-dpng', fullfile('paper\planar_struct_SR_CVPR2015\figures\plane', 'vpdetection.png'));

% Plot posterior
alphaP = 0.9;
imgP = (1 - alphaP)*img + alphaP*postProb;
figure(2); imshow(imgP);
imwrite(imgP, fullfile('paper\planar_struct_SR_CVPR2015\figures\plane', 'planedetection.png'));

end
function vpData = sr_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 = sr_detect_plane_from_vp(vpData, img, opt)

% 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, opt.filterSize], opt.filterSigma);
HfilterY = HfilterX';
% fspecial('gaussian', opt.filterSize, opt.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:opt.numFilterIter
        imgLinesPosMap = imfilter(imgLinesPosMap, HfilterX, 'conv', 'replicate');
    end
    for k = 1:opt.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}.imgPlaneProb(mask) = 1e-10;
        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;

    end
end

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

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

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 = bsxfun(@rdivide, planeProb, planeProbSum);
planeProb = planeProb + 0.1; % blur the posterior map
planeProb = bsxfun(@rdivide, planeProb, sum(planeProb, 3));

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 test_suite=test_find_local_extrema
% tests for cosmo_find_local_extrema
%
% #   For CoSMoMVPA's copyright information and license terms,   #
% #   see the COPYING file distributed with CoSMoMVPA.           #
    try % assignment of 'localfunctions' is necessary in Matlab >= 2016
        test_functions=localfunctions();
    catch % no problem; early Matlab versions can use initTestSuite fine
    end
    initTestSuite

function test_find_local_maxima_basics
    % generate tiny dataset with 6 voxels
    ds=cosmo_synthetic_dataset('ntargets',1,'nchunks',1);
    nh=cosmo_spherical_neighborhood(ds,'radius',1,'progress',false);
    % find local maxima within neighborhood of 1 voxel radius
    [feature_ids,scores]=cosmo_find_local_extrema(ds,nh);
    assertEqual(feature_ids,[1 5 3]);
    assertElementsAlmostEqual(scores,[2.0317 1.1908 -1.4437],...
                                        'absolute',1e-4);

    % only return two feature ids
    [feature_ids,scores]=cosmo_find_local_extrema(ds,nh,'count',2);
    assertEqual(feature_ids,[1 5]);
    assertElementsAlmostEqual(scores,[2.0317 1.1908],...
                                        'absolute',1e-4);

    % use another fitness function, namely local minima
    [feature_ids,scores]=cosmo_find_local_extrema(ds,nh,'fitness',@min);
    assertEqual(feature_ids,[3 4]);
    assertElementsAlmostEqual(scores,[-1.4437 -0.5177 ],...
                                        'absolute',1e-4);

    nh=cosmo_spherical_neighborhood(ds,'radius',2,'progress',false);
    [feature_ids,scores]=cosmo_find_local_extrema(ds,nh);
    assertEqual(feature_ids,[1 6]);
    assertElementsAlmostEqual(scores,[2.0317 -1.3265],...
                                        'absolute',1e-4);

    nh=cosmo_spherical_neighborhood(ds,'radius',1,'progress',false);
    ds.samples(:)=NaN;
    [feature_ids,scores]=cosmo_find_local_extrema(ds,nh);
    assertEqual(feature_ids,zeros(1,0));
    assertElementsAlmostEqual(scores,zeros(1,0));

function test_find_local_maxima_exceptions
    ds=cosmo_synthetic_dataset('ntargets',1,'nchunks',1);
    nh=cosmo_spherical_neighborhood(ds,'radius',1,'progress',false);

    aet=@(varargin)assertExceptionThrown(@()...
                        cosmo_find_local_extrema(varargin{:}),'');
    aet(cosmo_stack({ds,ds}),nh)
    ds.fa.i=ds.fa.i(end:-1:1);
    aet(ds,nh)

    ds.fa.i=ds.fa.i(end:-1:1);

    nh.a=struct();
    ds.samples=1;
    aet(ds,nh);


function c = truncCoeffs(f, N)
%TRUNCCOEFFS   Trigonometric least square coefficients of a CLASSICFUN.
%   C = TRUNCCOEFFS(F, N) returns the 'middle' 2*N+1 trigonometric
%   Fourier coefficients of F.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Call TRUNCCOEFFS() of the .ONEFUN:
c = truncCoeffs(f.onefun, N);
end


function calpak_test695 ( )

%*****************************************************************************80
%
%% CALPAK_TEST695 tests YMDF_DIF_ENGLISH.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 April 2013
%
%  Author:
%
%    John Burkardt
%
  dhi = 1;
  dlo = 1;
  fhi = 0.0;
  flo = 0.0;
  mhi = 1;
  mlo = 1;
  seed = 123456789;
  yhi = 1960;
  ylo = 1970;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CALPAK_TEST695\n' );
  fprintf ( 1, '  YMDF_DIF_ENGLISH gets the day difference\n' );
  fprintf ( 1, '  between YMDF dates.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  YMDF1        YMDF2        (YMDF2 - YMDF1)\n' );
  fprintf ( 1, '\n' );

  for i = 1 : 10

    [ y1, m1, d1, f1, seed ] = ymdf_uniform_english ( ylo, mlo, dlo, flo, ...
      yhi, mhi, dhi, fhi, seed );

    s1 = ymdf_to_s_english ( y1, m1, d1, f1 );

    [ y2, m2, d2, f2, seed ] = ymdf_uniform_english ( ylo, mlo, dlo, flo, ...
      yhi, mhi, dhi, fhi, seed );

    s2 = ymdf_to_s_english ( y2, m2, d2, f2 );

    [ days, ierror ] = ymdf_dif_english ( y1, m1, d1, f1, y2, m2, d2, f2 );

    fprintf ( 1, '  %10s  %10s  %11.2f\n', s1, s2, days );

  end

  return
end


function inside = checkInside(oR,q,varargin)
% check for points to be inside the orientation region

% avoid q beeing orientation
q = quaternion(q);

if isempty(oR.N), inside = true(size(q)); return; end
if isempty(q), inside = false(size(q)); return; end

% verify all conditions are satisfies
inside1 = dot_outer(oR.N,q)<1e-3;
inside2 = dot_outer(oR.N,q)>-1e-3;

% either q or -q needs to satisfy the condition
inside = reshape(all(inside1,1) | all(inside2,1),size(q));

end


function fmincon_test ( )

%*****************************************************************************80
%
%% FMINCON_TEST tests the FMINCON library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 September 2010
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FMINCON_TEST\n' );
  fprintf ( 1, '  MATLAB version.\n' );
  fprintf ( 1, '  Test the FMINCON library.\n' );

  fmincon_test01 ( );
  fmincon_test02 ( );
  fmincon_test03 ( );
  fmincon_test04 ( );
  fmincon_test05 ( );
  fmincon_test06 ( );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FMINCON_TEST\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


function [imgs, ds, pix_resolutions] = ptv_create_pyramid(img, nlevels, pix_resolution, varargin)
%    create_iso_pyramid(img, nlevels, pix_resolution, varargin, {force_isotropic, k_down, interp_type, fine_rescale})
%
    if isempty(img)
        imgs = [];
        ds = [];
        pix_resolutions = [];
        return;
    end
    interp_type = 0;
    force_isotropic = false;
    if nargin >= 4
        force_isotropic = varargin{1};
    end
    if force_isotropic
        error('not implemented');
    end
    k_down = 0.5;
    if nargin >= 5
        k_down = varargin{2};
    end
    if nargin >= 6
        interp_type = varargin{3};
    end
    fine_rescale = false;
    if nargin >= 7
        fine_rescale = varargin{4};
    end

    ndims = 2;
    if size(img, 3) > 1
        ndims = 3;
    end

    Nch = size(img, 4);
    Nimgs = size(img, 5);
    pix_resolutions = zeros(nlevels, 3);

    pix_resolutions(1, :) = pix_resolution;
    imgs = cell(nlevels, 1);
    imgs{1} = img;
    ds = ones(nlevels, ndims);
    for i = 2 : nlevels
        if ~force_isotropic
            d = ones(1, ndims);
        end

        idxs = cell(ndims, 1);
        newsz = zeros(ndims, 1);
        if fine_rescale
            sigmas = 0.5 * ones(ndims, 1) * (0.5/k_down * (i-1));
        else
            sigmas = 0.4 * ones(ndims, 1) * (0.5/k_down);
        end

        newsz = ceil(k_down * size(imgs{i-1}(:,:,:, 1)));
        if ndims == 3
            imgs{i} = zeros([newsz, Nch, Nimgs]);
        elseif ndims == 2
            imgs{i} = zeros([newsz, 1, Nch, Nimgs]);
        end
        pix_resolutions(i, :) = pix_resolutions(i - 1, :) / k_down;

        for jj = 1 : Nch
            for ii = 1 : Nimgs
                if ndims == 2
                    if fine_rescale
                        t = imgaussfilt(imgs{1}(:,:, 1, jj, ii), sigmas);
                    else
                        t = imgaussfilt(imgs{i-1}(:,:, 1, jj, ii), sigmas);
                    end
                    imgs{i}(:,:, :, jj, ii) = imresize(t, size(imgs{i}(:,:, 1, jj, ii)), 'bilinear');
                elseif ndims == 3
                    if fine_rescale
                        t = imgaussfilt3(imgs{1}(:,:,:, jj, ii), sigmas);
                    else
                        t = imgaussfilt3(imgs{i-1}(:,:,:, jj, ii), sigmas);
                    end
                    imgs{i}(:,:,:, jj, ii) = volresize(t, size(imgs{i}(:,:,:, jj, ii)), interp_type);
                end
            end
        end
    end
end


function [dynpcm2] = inH2O2dynpcm2(inH2O)
% Convert pressure from inches of water column at 4 degrees to dyne/cm2
% Chad Greene 2012
dynpcm2 = inH2O*2490.89;


function [cost_total,grad_total] = RAECost(theta, alpha_cat, cat_size, beta, dictionary_length, hiddenSize, ...
    lambda, We_orig, data_cell, labels, freq_orig, sent_freq, f, f_prime)

[~, ~, ~, ~, ~, ~, ~, Wcat, bcat, We] = getW(1, theta, hiddenSize, cat_size, dictionary_length);

szWe = length(We(:));
szbcat = length(bcat(:));
szWcat = length(Wcat(:));
theta1 = theta;
theta1(end-szWe+1:end) = We;
theta1(end-szWe-szbcat-szWcat+1:end-szWe) = [];

theta2 = theta;
theta2(end-szWe+1:end) = We_orig(:) + We(:);
lambda2 = lambda;
lambda(3) = lambda2(4);
lambda(4) = lambda2(3);

% disp('DEBUGGING: DELETE AFTERWARDS')
% data_cell=data_cell(1:2);
% update W using Greedy Unsupervised RAE
[costRAE, gradRAE, allKids] = computeCostAndGradRAE([], theta1, 0, alpha_cat, cat_size, beta, dictionary_length, hiddenSize, ...
    (alpha_cat)*lambda, We_orig , data_cell, labels, freq_orig, f, f_prime);

WegradRAE = gradRAE(end-szWe+1:end);
gradRAE(end-szWe+1:end) = 0;
gradRAE = [gradRAE; zeros(szbcat+szWcat,1)];
gradRAE(end-szWe+1:end) = WegradRAE;

cost_total =  costRAE;
grad_total =  gradRAE;


function P = vecRt_r()
% vec(R') = P * r
P = sparse([1,2,3,4,5,6,7,8,9],...
            [1,4,7,2,5,8,3,6,9],...
            [1,1,1,1,1,1,1,1,1],...
            9,9);
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Q = INVERSEKINEMATIC_KUKA_KR_30_L16_2(robot, T)
%   Solves the inverse kinematic problem for the KUKA KR5 ARC robot
%   where:
%   robot stores the robot parameters.
%   T is an homogeneous transform that specifies the position/orientation
%   of the end effector.
%
%   A call to Q=INVERSEKINEMATIC_KUKA_KR_30_L16_2 returns 8 possible solutions, thus,
%   Q is a 6x8 matrix where each column stores 6 feasible joint values.
%
%
%   Example code:
%
%   robot=load_robot('kuka', 'KR_30_L16_2');
%   q = [0 0 0 0 0 0];
%   T = directkinematic(robot, q);
%   %Call the inversekinematic for this robot
%   qinv = inversekinematic(robot, T);
%   check that all of them are feasible solutions!
%   and every Ti equals T
%   for i=1:8,
%        Ti = directkinematic(robot, qinv(:,i))
%   end
%	See also DIRECTKINEMATIC.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function q = inversekinematic_kuka_kr20_3(robot, T)

%initialize q,
%eight possible solutions are generally feasible
q=zeros(6,8);

% %Evaluate the parameters
% theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
L6=abs(d(6));

%T= [ nx ox ax Px;
%     ny oy ay Py;
%     nz oz az Pz];
Px=T(1,4);
Py=T(2,4);
Pz=T(3,4);

%Compute the position of the wrist, being W the Z component of the end effector's system
W = T(1:3,3);

% Pm: wrist position
Pm = [Px Py Pz]' - L6*W;

%first joint, two possible solutions admited:
% if q(1) is a solution, then q(1) + pi is also a solution
q1=atan2(Pm(2), Pm(1));

%solve for q2
q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm);
%the other possible solution is q1 + pi
q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm);

%solve for q3
q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm);
%solver for q3 for both cases
q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm);

%the next matrix doubles each column. For each two columns, two different
%configurations for theta4, theta5 and theta6 will be computed. These
%configurations are generally referred as wrist up and wrist down solution
q = [q1         q1         q1        q1       q1+pi   q1+pi   q1+pi   q1+pi;
     q2_1(1)    q2_1(1)    q2_1(2)   q2_1(2)  q2_2(1) q2_2(1) q2_2(2) q2_2(2);
     q3_1(1)    q3_1(1)    q3_1(2)   q3_1(2)  q3_2(1) q3_2(1) q3_2(2) q3_2(2);
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0];

%leave only the real part of the solutions
% q=real(q);
% q = [pi/4 -pi/6 pi/4 0.1 0.1 0.1];
% T = directkinematic(robot,q)
% T =
%    -0.3864   -0.6500    0.6544    0.4980
%    -0.1062    0.7361    0.6685    0.5003
%    -0.9162    0.1888   -0.3535    1.0008
%          0         0         0    1.0000
%q =
%    0.7854   -0.5236    0.7854    0.1000    0.1000    0.1000

%Note that in this robot, the joint q3 has a non-simmetrical range. In this
%case, the joint ranges from 60 deg to -219 deg, thus, the typical normalizing
%step is avoided in this angle (the next line is commented). When solving
%for the orientation, the solutions are normalized to the [-pi, pi] range
%only for the theta4, theta5 and theta6 joints.

%normalize q to [-pi, pi]
q(1,:) = normalize(q(1,:));
q(2,:) = normalize(q(2,:));

% solve for the last three joints
% for any of the possible combinations (theta1, theta2, theta3)
for i=1:2:size(q,2),
    qtemp = solve_spherical_wrist2(robot, q(:,i), T, 1,'geometric'); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i)=qtemp;

    qtemp = solve_spherical_wrist2(robot, q(:,i), T, -1, 'geometric'); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i+1)=qtemp;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for second joint theta2, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q2 = solve_for_theta2(robot, q, Pm)

%Evaluate the parameters
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

%See geometry
L2=abs(a(2));
L3=abs(d(4));
A2 = (a(3));

%See geometry of the robot
%compute L4
L4 = sqrt(A2^2 + L3^2);

%The inverse kinematic problem can be solved as in the IRB 140 (for example)

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

beta = atan2(-p1(2), p1(1));
gamma = real(acos((L2^2+r^2-L4^2)/(2*r*L2)));

%return two possible solutions
%elbow up and elbow down
%the order here is important and is coordinated with the function
%solve_for_theta3
q2(1) = pi/2 - beta - gamma; %elbow up
q2(2) = pi/2 - beta + gamma; %elbow down

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for third joint theta3, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q3 = solve_for_theta3(robot, q, Pm)

%Evaluate the parameters
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

%See geometry
L2=abs(a(2));
L3=abs(d(4));

A2 = (a(3));

%See geometry of the robot
%compute L4
L4 = sqrt(A2^2 + L3^2);

%the angle phi is fixed
phi=acos((A2^2+L4^2-L3^2)/(2*A2*L4));

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

beta = real(acos((L2^2 + L4^2 - r^2)/(2*L2*L4)));

%return two possible solutions
%elbow up and elbow down solutions
%the order here is important
q3(1) = pi - phi - beta;
q3(2) = pi - phi + beta;


function [F,vars] = sort_internal(t,X,data);%ii,D,V);

% Hack to figure out all the sorted variables, not just this index.
% Sort is implemented in a slighlt different way (general feature in
% future versions) that allows one element in an operator to modell all
% elements. Reduces the number of calls to the operator code.

ii = data.i;
D = data.D;
V = data.V;

var_start = getvariables(t)-ii+1;
n = length(X);
var_end   = getvariables(t)-ii+1 + n-1;
vars = var_start:var_end;

% Is this a location variable instead of the actuial sort variable. If so,
% shift everything back to get the indicies of the sorted variables.
if data.isthisloc == 1
    vars = vars - n;
end

t   = recover(vars);
loc = recover(vars+n);

[M,m] = derivebounds(X);
X = reshape(X,1,n);

% Standard model
F = (sum(D,1) == 1) + (sum(D,2) == 1);
F = F + (t == sum(V,2));
F = F + (diff(t) >= 0);
for i = 1:n
   di = D(i,:);
   vi = V(i,:);
   F = F + (-(-m)'.*(1-di) <= X-vi <= (M)'.*(1-di));
   F = F + (m'.*di <=  vi <= M'.*di);
end

% Cuts
F = F + (X == sum(V,1));
F = F + (sum(t) == sum(X));

% Definition of location
F = F + (loc == D*[(1:n)']) + (1 <= loc <= n);


function om = add_quad_cost(om, name, idx, Q, c, k, varsets)
%ADD_QUAD_COST  Adds a set of user costs to the model.
%   OM.ADD_QUAD_COST(NAME, Q, C);
%   OM.ADD_QUAD_COST(NAME, Q, C, K);
%   OM.ADD_QUAD_COST(NAME, Q, C, K, VARSETS);
%   OM.ADD_QUAD_COST(NAME, IDX_LIST, Q, C);
%   OM.ADD_QUAD_COST(NAME, IDX_LIST, Q, C, K);
%   OM.ADD_QUAD_COST(NAME, IDX_LIST, Q, C, K, VARSETS);
%
%   Adds a named block of quadratic costs to the model. Costs are of the
%   form
%       F(X) = 1/2 * X'*Q*X + C'*X + K
%   where Q is an NX x NX matrix (possibly sparse), C is an NX x 1 vector,
%   K is a scalar and NX is the number of elements in X. Here X is the vector
%   formed by combining the specified VARSETS (the full optimization vector
%   by default). Alternatively, if Q is an NX x 1 vector or empty, then F(X)
%   is also NX x 1, and K can be either NX + 1 or scalar.
%       F(X) = 1/2 * Q .* X.^2 + C .* X + K
%
%   Indexed Named Sets
%       A cost set can be identified by a single NAME, as described
%       above, such as 'PgCost', or by a name that is indexed by one
%       or more indices, such as 'PgCost(3,4)'. For an indexed named
%       set, before adding the cost sets themselves, the dimensions
%       of the indexed set must be set by calling INIT_INDEXED_NAME.
%
%       The constraints are then added using the following, where
%       all arguments are as described above, except IDX_LIST is a cell
%       array of the indices for the particular cost set being added.
%
%       OM.ADD_QUAD_COST(NAME, IDX_LIST, Q, C, K);
%       OM.ADD_QUAD_COST(NAME, IDX_LIST, Q, C, K, VARSETS);
%
%   Examples:
%       om.add_quad_cost('quad_cost1', Q1, c1, 0);
%       om.add_quad_cost('lin_cost2',  [], c2, k2, {'Vm', 'Pg', 'z'});
%
%       om.init_indexed_name('c', {2, 3});
%       for i = 1:2
%         for j = 1:3
%           om.add_quad_cost('c', {i, j}, Q{i,j}, ...);
%         end
%       end
%
%   See also OPT_MODEL, PARAMS_QUAD_COST, EVAL_QUAD_COST.

%   MP-Opt-Model
%   Copyright (c) 2008-2020, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MP-Opt-Model.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://github.com/MATPOWER/mp-opt-model for more info.

%% initialize input arguments
if iscell(idx)          %% indexed named set
    if nargin < 7
        varsets = {};
    end
else                    %% simple named set
    if nargin < 6
        varsets = {};
    else
        varsets = k;
    end
    if nargin < 5
        k = 0;
    else
        k = c;
    end
    c = Q;
    Q = idx;
    idx = {};
end

%% convert varsets from cell to struct array if necessary
varsets = om.varsets_cell2struct(varsets);
nv = om.varsets_len(varsets);   %% number of variables

%% check sizes
[MQ, NQ] = size(Q);
[Mc, Nc] = size(c);
if MQ
    if NQ ~= MQ && NQ ~= 1
        error('@opt_model/add_quad_cost: Q (%d x %d) must be square or a column vector (or empty)', MQ, NQ);
    end
end
if Mc && Nc ~= 1
    error('@opt_model/add_quad_cost: c (%d x %d) must be a column vector (or empty)', Mc, Nc);
end
if MQ
    if Mc && Mc ~= MQ
        error('@opt_model/add_quad_cost: dimensions of Q (%d x %d) and c (%d x %d) are not compatible', MQ, NQ, Mc, Nc);
    end
    nx = MQ;
else
    if ~Mc
        error('@opt_model/add_quad_cost: Q and c cannot both be empty');
    end
    nx = Mc;
end
if nx ~= nv
    error('@opt_model/add_quad_cost: dimensions of Q (%d x %d) and c (%d x %d) do not match\nnumber of variables (%d)\n', MQ, NQ, Mc, Nc, nv);
end

%% size of named cost set
if NQ == 1 || isempty(Q)    %% if Q is a column vector or empty
    N = nx;                 %%   cost is element-wise, i.e. a vector
else                        %% otherwise Q is a square matrix
    N = 1;                  %%   cost is scalar
end

%% add the named quadratic cost set
om.add_named_set('qdc', name, idx, N, Q, c, k, varsets);


function [geom_data, valid_ids] = Read_geom_files_dynamic(users, model_param_data_dir)

    geom_data = [];
    valid_ids = [];

    load('../../pca_generation/pdm_68_aligned_wild.mat');

    user_files = [];

    for i=1:numel(users)

        geom_file = [model_param_data_dir, '/' users{i} '.csv'];

        user_files = cat(1, user_files, str2num(users{i}(3:5)));

        if(i == 1)
            tab = readtable(geom_file);
            column_names = tab.Properties.VariableNames;
            valid_ind = cellfun(@(x) ~isempty(x) && x==1, strfind(column_names, 'confidence'));
            shape_inds = cellfun(@(x) ~isempty(x) && x==1, strfind(column_names, 'p_'));
        end

        res = dlmread(geom_file, ',', 1, 0);

        valid = res(:, valid_ind) > 0.7;
        res = res(:, shape_inds);

        % Do not consider global parameters
        res = res(:, 7:end);

        actual_locs = res * V';
        res = cat(2, actual_locs, res);

        valid_ids = cat(1, valid_ids, valid);

        geom_data_curr_p = res;

        geom_data = cat(1, geom_data, geom_data_curr_p);

    end

    if(numel(users) > 0)
        uq_ids = unique(user_files)';

        for u=uq_ids
            geom_data(user_files==u,:) = bsxfun(@plus, geom_data(user_files==u,:), -median( geom_data(valid_ids & user_files==u,:)));
        end
    end

end


function mpc = t_case3p_c
%T_CASE3P_C   Six bus hybrid test case, 2 single-phase buses, 4 3-phase buses.
%
%   One bus is a hybrid PV bus (PV on single-phase side). Three phase bus
%   solution should match T_CASE3P_A

%% MATPOWER Case Format : Version 2
mpc.version = '2';

%%-----  Power Flow Data  -----%%
%% system MVA base
mpc.baseMVA = 100;

%% bus data
%	bus_i	type	Pd	Qd	Gs	Bs	area	Vm	Va	baseKV	zone	Vmax	Vmin
mpc.bus = [
	1	3	0	0	0	0	1	1	0.20115203	12.47	1	1.1	0.9;
% 	2	2	0	0	0	0	1	1	0	12.47	1	1.1	0.9;
% 	3	2	0	0	0	0	1	1	0	12.47	1	1.1	0.9;
	4	2	0	0	0	0	1	1	0	12.47	1	1.1	0.9;
% 	5	1	90	30	0	0	1	1	0	12.47	1	1.1	0.9;
% 	6	1	0	0	0	0	1	1	0	12.47	1	1.1	0.9;
% 	7	1	100	35	0	0	1	1	0	12.47	1	1.1	0.9;
% 	8	1	0	0	0	0	1	1	0	12.47	1	1.1	0.9;
% 	9	1	125	50	0	0	1	1	0	12.47	1	1.1	0.9;
];

%% generator data
%	bus	Pg	Qg	Qmax	Qmin	Vg	mBase	status	Pmax	Pmin	Pc1	Pc2	Qc1min	Qc1max	Qc2min	Qc2max	ramp_agc	ramp_10	ramp_30	ramp_q	apf
mpc.gen = [
	1	72.3	27.03	300	-300	1.0024291384	100	1	250	10	0	0	0	0	0	0	0	0	0	0	0;
	4	0	0	300	-300	1	100	1	250	10	0	0	0	0	0	0	0	0	0	0	0;
% 	2	163	6.54	300	-300	1.025	100	1	300	10	0	0	0	0	0	0	0	0	0	0	0;
% 	3	85	-10.95	300	-300	1.025	100	1	270	10	0	0	0	0	0	0	0	0	0	0	0;
];

%% branch data
%	fbus	tbus	r	x	b	rateA	rateB	rateC	ratio	angle	status	angmin	angmax
mpc.branch = [
	1	4	0	0.0576	0	250	250	250	0	0	1	-360	360;
% 	4	5	0.017	0.092	0.158	250	250	250	0	0	1	-360	360;
% 	5	6	0.039	0.17	0.358	150	150	150	0	0	1	-360	360;
% 	3	6	0	0.0586	0	300	300	300	0	0	1	-360	360;
% 	6	7	0.0119	0.1008	0.209	150	150	150	0	0	1	-360	360;
% 	7	8	0.0085	0.072	0.149	250	250	250	0	0	1	-360	360;
% 	8	2	0	0.0625	0	250	250	250	0	0	1	-360	360;
% 	8	9	0.032	0.161	0.306	250	250	250	0	0	1	-360	360;
% 	9	4	0.01	0.085	0.176	250	250	250	0	0	1	-360	360;
];

%%-----  OPF Data  -----%%
%% generator cost data
%	1	startup	shutdown	n	x1	y1	...	xn	yn
%	2	startup	shutdown	n	c(n-1)	...	c0
mpc.gencost = [
	2	1500	0	3	0.11	5	150;
	2	1500	0	3	0.11	5	150;
% 	2	2000	0	3	0.085	1.2	600;
% 	2	3000	0	3	0.1225	1	335;
];

%%-----  3 Phase Model Data  -----%%
%% system data
mpc.freq = 60;      %% frequency, Hz
mpc.basekVA = 1000; %% system kVA base

%% bus data
%	busid	type	basekV	Vm1	Vm2	Vm3	Va1	Va2	Va3
mpc.bus3p = [
	1	1	12.47	1	1	1	0	-120	120;
	2	1	12.47	1	1	1	0	-120	120;
	3	1	4.16	1	1	1	0	-120	120;
	4	1	4.16	1	1	1	0	-120	120;
];

%% buslink data
%	linkid	busid	bus3pid	status
mpc.buslink = [
	1	4	1	1;
];

%% branch data
%	brid	fbus	tbus	status	lcid	len
mpc.line3p = [
	1	1	2	1	1	2000/5280;
	2	3	4	1	1	2500/5280;
];

%% transformer
%	xfid	fbus	tbus	status	R	X	basekVA	basekV
mpc.xfmr3p = [
	1	2	3	1	0.01	0.06	6000	12.47;
];

%% load
%	ldid	ldbus	status	Pd1	Pd2	Pd3	ldpf1	ldpf2	ldpf3
mpc.load3p = [
	1	4	1	1275	1800	2375	0.85	0.9	0.95;
];

%% gen
%	genid	gbus	status	Vg1	Vg2	Vg3	Pg1	Pg2	Pg3	Qg1	Qg2	Qg3
mpc.gen3p = [
% 	1	1	1	1	1	1	2000	2000	2000	0	0	0;
];

%% line construction
%	lcid	R11	R21	R31	R22	R32	R33	X11	X21	X31	X22	X32	X33	C11	C21	C31	C22	C32	C33
mpc.lc = [
	1	0.457541	0.15594 	0.153474	0.466617	0.157996	0.461462	1.078	0.501648	0.384909	1.04813	0.423624	1.06502	15.0671	-4.86241	-1.85323	15.875	-3.09098	14.3254
];


% source code to produce analytical predictions
% for 2d fast variance predictions paper for IEEE.
%
% Empirical and FFT_based predictions are computed and saved
%
% Copyright by Yingying Zhang-O'Connor, University of Michigan 2006

clear
clc

% -------------------------------
% set up parameters
% -------------------------------
printm 'set up parameters'
% calibration for single integral
% true to compute on the fly; false to load the saved table
t.calibrate = true;

t.ellipse = false;
t.zubal = false;
t.ncat = true;
t.fov = 'ellipse'; % 'ellipse' or 'disc'

t.const = false; % standard: \til{R}_phi = kappa_{center}
t.kappa = true; % certainty-based \til{R}_phi = kappa_j

if t.zubal
	img = 1;
	nx = 128/img;
	ny = nx;
	nb = ceil(888/4/img);
	na = ceil(984/4/img);
	f.ray_spacing = 0.9765625*4*img; %mm
	f.strip_width = 0.9765625*4*img;
	f.pixel_size = 0.9765625*4*img;
elseif t.ncat
	img = 2;
	nx = 512/img;
	ny = nx;
	nb = ceil(888/img);
	na = ceil(984/img);
	f.ray_spacing = 0.9765625*img; %mm
	f.strip_width = 0.9765625*img;
	f.pixel_size = 0.9765625*img;
	f.channel_offset = 0;
end

% fan-beam parameters
f.orbit = 360;
f.orbit_start = 0;
f.dis_src_det = 949.075;
f.dis_iso_det = 408.075;

% phantom parameters
if t.ellipse
	m.water = 0.2; % cm^2/g
	m.bone = 0.1;
	d.ellipse = 0.7*m.water/10; % g's g/cc; pixel size is mm, so convert into mm^(-1)
	d.bone = (2.5-1)*m.bone/10;
	d.soft = 1*m.water/10;
	c_x = (nx+1)/2;
	c_y = (ny+1)/2;
	bx_center = 0;
	by_center = 0;
	bx_radius = (nx - 38)/2;
	by_radius = 25;
	disc_radius = 10;
end

% ------------------------------
% generate the true phantom
% ------------------------------
printm 'set up xtrue'
xtrue = zeros(nx,ny);
if t.ellipse
	x_center = [bx_center; bx_center+20];
	y_center = [by_center; by_center];
	x_radius = [bx_radius; disc_radius];
	y_radius = [by_radius; disc_radius];
	ampl = [d.ellipse; d.bone];
	params = [x_center y_center x_radius y_radius zeros(2,1) ampl];
	params(:,1:4) = params(:,1:4)*f.pixel_size;
	xtrue = zeros(nx,ny);
	xtrue = ellipses(nx, ny, params, f.pixel_size, f.pixel_size);

elseif t.zubal
	dir = '~fessler/l/src/matlab/alg/data';
	xtrue = read_zubal_attn(dir) * 2.1; % mulist = [0.002 0.0096 0.0120] *2.1 to make CT value;

	if nx < 128
		xtrue = downsample2(xtrue,img);
	end

elseif t.ncat
%	xtrue = fld_read('~yzz/data/phantoms/ncat/ncat,256,slice,140.fld')/2;
%	ddir = path_find_dir([filesep 'transmission']);
%	ddir = strrep(ddir, 'transmission', 'data');
%	xtrue = fld_read([ddir '/ncat,256,slice,140,ct,x100.fld']) / 2;
	ddir = '~fessler/l/dat/phantom,digital/unc/ncat1.1';
	xtrue = fld_read([ddir '/ncat,256,slice,140.fld']) / 2;

	if (nx < 1024) && size(xtrue,2) == 1024
		img = 1024/nx;
		xtrue = downsample2(xtrue,img);
	end
end

% -------------------------------
% generate mask
% -------------------------------
printm 'generate support'
if t.ellipse
	x_mask = bx_radius+5;
	y_mask = by_radius+5;
elseif t.zubal || t.ncat
	if streq(t.fov, 'ellipse')
		printm('ellipse FOV')
		x_mask = (nx - nx/16 + 10)/2;
		y_mask = (ny - (ny/4 + nx/16) + 50)/2;
	elseif streq(t.fov, 'disc')
		printm('circular FOV')
		ss = ((-(nb-1)/2:(nb-1)/2).' - f.channel_offset) * f.ray_spacing;
		smax = max(abs(ss));
		rmax = sin(smax/f.dis_src_det)*(f.dis_src_det - f.dis_iso_det);
		x_mask = floor((rmax-2)/2);
		y_mask = x_mask;
	end
end
f.support = sprintf('ellipse 0 0 %g %g', x_mask, y_mask);

% new code by jf showing how to generate system matrix
sg = sino_geom('fan', ...
	'nb', nb, 'na', na, ...
	'dsd', f.dis_src_det, ...
	'dod', f.dis_iso_det, ...
	'ds', f.ray_spacing, ...
	'strip_width', f.strip_width, ...
	'orbit', f.orbit, ...
	'orbit_start', f.orbit_start, ...
	'offset_s', 0);

ig = image_geom('nx', nx, 'ny', ny, 'dx', f.pixel_size);
ig.mask = ig.circ(max(x_mask, y_mask));
if x_mask ~= y_mask
	warn 'need to put in ellipse using ellipse_im'
end

G = Gtomo2_wtmex(sg, ig);

if 0 % this is an old way - do not use it
	sys = arg_pair('system', 14, ...
	'nx', nx, 'ny', ny, ...
	'nb', nb, 'na', na, ...
	'support',	f.support, ...
	'pixel_size',	f.pixel_size, ...
	'ray_spacing',	f.ray_spacing, ...
	'strip_width',	f.strip_width, ...
	'orbit',	f.orbit, ...
	'orbit_start',	f.orbit_start, ...
	'src_det_dis',	f.dis_src_det, ...
	'obj2det_x',	f.dis_iso_det, ...
	'obj2det_y',	f.dis_iso_det, ...
	'flip_y', 1, ...
	'scale', 0, ...
	'channel_offset', f.channel_offset, ...
	'source_offset', 0);
	Gdsc = Gtomo2_dscmex(sys, 'nthread', 2); % use it to generate the mask
	mask = Gdsc.arg.mask;
end

% -------------------------------
% system matrix
% -------------------------------
printm 'set up fan-beam system using DD projector'

if 0 % this is an old way - do not use it
	G = Gtomo2_dd(mask, nb, na, ...
	'pixel_size', f.pixel_size, ...
	'ray_spacing', f.ray_spacing, ...
	'strip_width', f.strip_width, ...
	'dis_src_det', f.dis_src_det, ...
	'dis_iso_det', f.dis_iso_det, ...
	'orbit', f.orbit, ...
	'orbit_start', f.orbit_start, ...
	'xscale', 1, ...
	'yscale', 1, ...
	'channel_offset', 0);
end

% ---------------------------------
% compute statistical weighting wi
% ---------------------------------
printm 'compute statistical weighting wi'
lb = reshape(G*xtrue(mask(:)), [nb na]);
bi = 10^6;
yb = bi*exp(-lb);
yi = poissrnd(yb);
wi = yb; % this is the true case. In reality no yb. Use yi plug-in method
lhat = -log(yi./bi);

W = diag_sp(wi(:));

% ---------------------------------
% choose \alpha for all penalty
% ---------------------------------
% regularization parameter
if nx == 128
	f.l2b = 12; % for 128x128
elseif nx == 32
	f.l2b = 14;
elseif (nx == 512) || (nx == 256)
	f.l2b = 11;
else
	f.l2b = 17;
end
f.beta = 2^f.l2b;

% ----------------------------------------
% Compute the H for all penalty cases
% ----------------------------------------
dx = f.pixel_size;
dy = dx;
np = G.arg.np; % number of pixels inside mask
ds = f.ray_spacing;
dp = 1; % distance power
dbeta = 2*pi/na;

printm('Polar frequency coordinates')
% decide rhomax
rhomax = 1 / (2 * dx);
drho = 1/nx/dx;
rho1 = (-nx/2:nx/2-1) / (nx * dx);
rho = rho1(nx/2 + 1:end);
rho = rho(:);
nrho = length(rho);
nPhi = na/4;
Phi = ((0:nPhi-1)')/nPhi * 2 * pi;
dPhi = abs(Phi(2)-Phi(1));

printm('compute w_0(Phi): angular dependent weighting')
nw = na/4;
phiw = ((0:nw-1)')/nw * pi; % to avoid = pi/2 since tan(pi/2) = inf
dphiw = abs(phiw(2)-phiw(1));
% w_0(Phi)
[wknot, tw] = adw_fan(G.arg, wi, mask, Phi);
sprintf('w_0(Phi) pre-compute: %g seconds', tw)

%
% pre-compute G(\rho, \Phi) in H(\rho, \Phi) at image center
%
% Jacobian: J(sj) \approx J(s_center) at image center: Ds0/Dsd
Jj = G.arg.dis_src_iso/G.arg.dis_src_det;

if streq(t.fov, 'ellipse')
	if 1 % d_0(\phi) at image center
		fov = x_mask .* y_mask ./ sqrt((x_mask .* cos(phiw)).^2 ...
			+ (y_mask .* sin(phiw)).^2);
		fov = 2 .* fov(:) .* dx;
	end

elseif streq(t.fov, 'disc')
	fov = sum(mask(:,nx/2)) .* dx;
	fov = repmat(fov, [length(phiw) 1]);
end

tt = cputime;
G_center = zeros(nrho, nPhi);
for ia = 1:length(phiw)
	G_center = G_center + fov(ia).* sinc(rho * (fov(ia) .* sin(Phi-phiw(ia))).').^2;
end
sprintf('G_0(rho, Phi) at center pre-compute: %g seconds', cputime - tt)
% A_0(\rho,\Phi) at image center
A = dx .* sinc(dx .* (rho * cos(Phi).')) .* ...
	dy .* sinc(dx .* (rho * sin(Phi).')) .* ...
	repmat((sinc(ds .* Jj .* rho)), [1 length(Phi)]); % Nrho x Phi

Ga = G_center .* (A.^2) .* dphiw ./ abs(ds .* dx .* dy .* dbeta);

% -----------------------------------------
% Compute the approx standard deviation map
% -----------------------------------------
if t.const || t.kappa

	kappan = G' * wi(:);
	kappad = G' * (ones(size(wi(:))));
	kappa = sqrt( kappan ./ kappad );

	f.data_dir = 'data/standard_penal/256image/';

	if t.const
	printm('standard quadratic penalty')
	printm('kappa is a const for all pixels')
	kappa_r = embed(kappa, mask);
	kappa_cen = kappa_r(nx/2+1, ny/2+1) .* ones(size(kappa));
	kappa_v = kappa; % varies for pixel

	his = f.l2b + log2(kappa_v.^2) - log2(kappa_cen.^2);
	his = hist(his, (6:0.5:14).'); % histogram
	clear kappa_r

	kappa = kappa_cen;
	kappa_r = embed(kappa, mask);

	% empirical and fft_based for comparison
	f.iot = [f.data_dir 'std_iot,iter71,ntime250,l2b11,256,nblock41,const.fld'];
	std_iot = fld_read([f.data_dir 'std_iot,iter71,ntime250,l2b11,256,nblock41,const.fld']);
	std_fft = fld_read([f.data_dir 'std_vh,dbsized_mask,iter71,ntime250,l2b11,256,nblock41,const.fld']);

	elseif t.kappa
	printm('certainty-base quadratic penalty')
	printm('kappa varies for pixels')
	kappa_r = embed(kappa, mask);

	% empirical and fft_based for comparison
%	std_iot = fld_read([f.data_dir 'std_iot,iter71,ntime250,l2b11,256,nblock41,kappa.fld']);
	std_iot = zeros(size(kappa_r)); % fake jf
%	std_fft = fld_read([f.data_dir 'std_vh,dbsized_mask,iter71,ntime250,l2b11,256,nblock41,kappa.fld']);
	std_fft = zeros(size(kappa_r)); % fake jf
	end

	r_sum_con = 1 + sqrt(2).^(2-dp);
	nl = [1 0 1 1]; ml = [0 -1 -1 1];
	phil = atan2(ml,nl);

	uu = rho * cos(Phi).';
	vv = rho * sin(Phi).';
	R1 = 0;
	for il = 1:4
	Rl = 4 .* sin((uu .* nl(il) + vv .* ml(il))*2*pi*dx/2).^2 ./ (sqrt(nl(il)^2 + ml(il)^2)).^dp;
	R1 = R1 + Rl; % rho x Phi
	end

	printm('double summation')
	tt = cputime;
	for ia = 1:size(wknot,2)
	ticker(mfilename, ia, size(wknot,2))
	Hd = wknot(:,ia) * Ga(:,ia).'; % Np x Nrho
	Hu = wknot(:,ia) * (Ga(:,ia) .* rho).'; % Np x Nrho

	R = kappa.^2 * R1(:,ia).'; % Np x Nrho
	int_rho(:,ia) = rhomax .* mean(Hu ./ ((Hd + f.beta .* R).^2), 2);
	end
	var_pre = mean(int_rho,2) .* (2*pi) .* (dx .* dy);
	std_pre = embed(sqrt(var_pre), mask);
	sprintf('db-integral prediction: %g second', cputime - tt)

	if t.calibrate
	% use extended mask for FFT in calibration
	maskcal = true([nx ny]);
	Gcal = Gtomo2_dd(maskcal, nb, na, ...
	'pixel_size', f.pixel_size, ...
	'ray_spacing', f.ray_spacing, ...
	'strip_width', f.strip_width, ...
	'dis_src_det', f.dis_src_det, ...
	'dis_iso_det', f.dis_iso_det, ...
	'orbit', f.orbit, ...
	'orbit_start', f.orbit_start, ...
	'xscale', 1, ...
	'yscale', 1, ...
	'channel_offset', 0);

	c.l2b = [2:0.15:20].'; % column
	c.beta = 2.^c.l2b;
	for indx = 1:length(c.beta)
	c.R0 = Robject(maskcal, 'edge_type', 'tight', 'beta', c.beta(indx), ...
	'type_denom', 'matlab', 'potential', 'quad', 'distance_power', dp);
	% mask size matters for smaller \beta
	[psf0, c.var0(indx), fwhm_t(indx)] = qpwls_psf(Gcal, c.R0, 1, maskcal, [], [0 0]);
	end
	c.var0 = c.var0(:); % column
	fwhm_t = fwhm_t(:);

	ej = zeros([nx ny]);
	ej(nx/2+1,ny/2+1) = 1;
	center_in = find(ej(mask(:))==1);
	[wknotcal, tw] = adw_fan(G.arg, ones(size(wi)), mask, Phi);
	CC = (dx .* dy) ./ (ds);
	KK = CC/(rhomax)^3;
	for indx = 1:length(c.beta)
	c.var_pre(indx) = mean(((dx .* dy) .* (2*pi) ./ 3) ./ ...
	(KK .* wknotcal(center_in,:) .* 2 + (2*pi*dx)^2*c.beta(indx)*r_sum_con));
	end
	c.var_pre = c.var_pre(:);

	ratio = sqrt(c.var0(:) ./ c.var_pre);

%	fld_write('data/cal,var_fft,fwhm,qpuls,l2b2to20,inl2b,2w0.fld', [c.var0 fwhm_t]);
%	fld_write('data/cal,ratio,in_std,qpuls,l2b2to20,inl2b,2w0.fld', ratio)
	else
		% load the calibration scale factor needed for single integral
		% Calibration is done for a range of fwhms
		% about (1.13)^2 in our case
		tmp = fld_read('data/cal,var_fft,fwhm,qpuls,l2b2to20,inl2b,2w0.fld');
		fwhm_t = tmp(:,2);
		ratio = fld_read('data/cal,ratio,in_std,qpuls,l2b2to20,inl2b,2w0.fld');
	end
	Rr = Robject(kappa_r, 'edge_type', 'tight', 'beta', f.beta, ...
		'type_denom', 'matlab', 'potential', 'quad', 'distance_power', dp);
	[psf, var, fwhm] = qpwls_psf(G, Rr, 1, mask, W, [0 0]);
	f_loc = find(abs(fwhm - fwhm_t) == min(abs(fwhm - fwhm_t)));

	CC = (dx .* dy) ./ (ds);
	KK = CC/(rhomax)^3;
	tt = cputime;
	var_pre1 = mean(1 ./ (KK .* wknot * 2 + (2*pi*dx)^2*f.beta*r_sum_con .* repmat(kappa.^2,[1 length(Phi)])), 2);
	std_pre1 = embed(sqrt(var_pre1 .* (dx .* dy) .* (2*pi) ./ 3), mask);
	std_pre1 = std_pre1 .* ratio(f_loc);
	sprintf('one-integral prediction: %g second', cputime - tt)
end

% jf: display results
im pl 2 2
im(1, std_pre), cbar
im(2, std_pre1), cbar


function p = towardsback(p0, vp, amount)
dir = vp{3} - p0;
dir = dir / norm(dir);
p = p0 + amount * dir;


function c = mod(a,b)
	checkinputs(a,b);
	if numel(b)>1
		ind = b~=0;
		c = uint64mod(int64(a(ind)),int64(b(ind)));
		c(~ind) = a(~ind);
	else
		if b == 0
			c = a;
		else
			c = uint64mod(a,b);
		end
	end
end


function groupedNodeStruct = findGroups(mesh,nodeList)
% Takes a list of nodes and tries to group it into unconnected unique clusters
%
%   groupedNodeStruct = findGroups(mesh,nodeList)
%
%

numVertices=length(mesh.connectionMatrix);
groupsFound=0;
currentGroup=1;

rowsToSearch=[1:numVertices];
foundAll=0;

searchConnectionMatrix=mesh.connectionMatrix(nodeList,nodeList);

counter=0;

while (~foundAll)
   nodesFoundThisTime=1;
   allNodesFound=[];
   [startNode x]=find(searchConnectionMatrix);
   currentNodes=startNode(1); % First non-zero row is the initial node

   while(nodesFoundThisTime)
      [y connectedNodes]=find(searchConnectionMatrix(currentNodes,:));
      connectedNodes=unique(connectedNodes(:));  % All the things connected to current node set
      nodesFoundThisTime=length(connectedNodes);

      if (nodesFoundThisTime)
         searchConnectionMatrix(:,currentNodes)=0;
         allNodesFound=[allNodesFound;connectedNodes];
         currentNodes=connectedNodes;

      end % if nothing found

   end % Loop while you're finding something
   groupedNodeStruct{currentGroup}.nodeList=unique(nodeList(allNodesFound));
   groupedNodeStruct{currentGroup}.tempList=unique(allNodesFound);
   currentGroup=currentGroup+1;

   foundAll=~(sum(searchConnectionMatrix(:)));

   counter=counter+1;
   disp(counter);

end

fprintf('%d groups found',counter);

% Re-generate this...
searchConnectionMatrix=mesh.connectionMatrix(nodeList,:);
searchConnectionMatrix=searchConnectionMatrix(:,nodeList);
searchConnectionMatrix=searchConnectionMatrix;


function [fileNameOut, extension] = lrsWriteRay(Q,modelName,vertexBool,param)
% Outputs a file for lrs to convert an V-representation (vertex / ray) of a
% polyhedron to a H-representation (half-space) via facet enumeration
%
% V-representation:
%     m is the number of input rows, each being a vertex, ray or line.
%     n is the number of input columns and d=n-1 is dimension of the input.
%     Each vertex is given in the form:
%
%     1   v_1   v_1  ... v_d
%
%     Each ray is given in the form:
%
%     0   r_1   r_2...   r_d
%
%     where  r_1  ...   r_d  is a point on the ray.
%
%     There must be at least one vertex in each file.
%     For bounded polyhedra there will be no rays entered.
%     The coefficients can be entered as integers or rationals in the format x/y.
%     An input line can be specified as a ray and then included in the linearity option (see below).
%
% INPUT
% Q             m x n integer matrix where each row is a variable and each column is a vertex or ray
% modelName     string giving the prefix of the *.ext file that will contain the vertex representation
%               It is assumed the file is pwd/*.ine, otherwise provide the full path.
%
% OPTIONAL INPUT
% vertexBool         n x 1 Boolean vector indicating which columns of Q are vertices
%                    By default, all columns of Q are assumed to be rays.
%
% param:             parameter structure with the following fields:
%    *.positivity:    if equals to 1, then positive orthant base
%    *.inequality:    if equals to 1, then represent as two inequalities rather than a single equality
%    *.shellScript:   if equals to 1, then lrs is run through a bash script
%    *.redund         if equals to 0, then remove redundant linear equalities

% Ronan Fleming 2021

[nMonomial,nRay]=size(Q);
if ~exist('vertexBool','var')
    fprintf('%s\n',['Assuming ' int2str(nRay) ' rays']);
    vertexBool = zeros(nRay,1);
end

if ~exist('param','var')
    param = struct();
end
if ~isfield(param,'positivity')
    param.positivity  = 0;
end
if ~isfield(param,'inequality')
    param.inequality  = 0;
end
if ~isfield(param,'shellScript')
    param.shellScript  = 0;
end
if ~isfield(param,'facetEnumeration')
    %assume vertex enumeration, unless specified that it is facet enumeration
    param.facetEnumeration  = 1;
end
if ~isfield(param,'redund')
    param.redund  = 1;
end

if param.redund ==0
    %remove all redundant halfspaces
    redundCmd = 'redund 0 0';
end

%lrs wants each row to be a ray so transpose
Q=Q';

extension = '.ext';
fileNameOut=[modelName extension];
fid=fopen(fileNameOut,'w');
fprintf(fid,'%s\n%s\n',modelName,'V-representation');

if any(~vertexBool)
    ind = find(vertexBool);
    if ~isempty(ind)
        fprintf(fid,'%s%s','linearity ',int2str(nnz(vertexBool)));
        for j = 1:length(ind)
            fprintf(fid,'%s%s',' ',int2str(ind(j)));
        end
        fprintf(fid,'\n');
    end
end

fprintf(fid,'%s\n','begin');

if ~any(vertexBool)
    fprintf(fid,'%s\n',[int2str(nRay+1) ' ' int2str(nMonomial+1) ' integer']);
    %there must be at least one vertex
    for r=1:nRay+1
        if r==1
            for d=1:nMonomial+1
                if d==1
                    fprintf(fid,'%s',[int2str(1)]);
                else
                    fprintf(fid,'%s',[' ' int2str(0)]);
                end
            end
        else
            for d=1:nMonomial+1
                if d==1
                    fprintf(fid,'%s',[int2str(0)]);
                else
                    fprintf(fid,'%s',[' ' int2str(Q(r-1,d-1))]);
                end
            end
        end
        fprintf(fid,'\n');
    end
else
    fprintf(fid,'%s\n',[int2str(nRay) ' ' int2str(nMonomial+1) ' integer']);
    for r=1:nRay
        for d=1:nMonomial+1
            if d==1
                fprintf(fid,'%s',[int2str(vertexBool(r)+0)]);
            else
                fprintf(fid,'%s',[' ' int2str(Q(r,d-1))]);
            end
        end
        fprintf(fid,'\n');
    end
end

%set of vertices specifying positive orthant
if param.positivity
    for i=1:nMonomial
        fprintf(fid,'%s',int2str(1));
        for j=1:nMonomial
            if i==j
                fprintf(fid,'%s',[' ' int2str(1)]);
            else
                fprintf(fid,'%s',[' ' int2str(0)]);
            end
        end
        fprintf(fid,'\n');
    end
end

fprintf(fid,'%s\n','end');
if param.redund==0
    %remove redundant halfspaces
    fprintf(fid,'%s\n',redundCmd);
end
fclose(fid);


function LU = getbounds(F)

K.f = 0;
K.l = 0;
L = [];
U = [];
LU =  yalmip('getbounds',1:yalmip('nvars'));
F = flatten(F);
for i = 1:length(F.clauses)
    if F.clauses{i}.type == 2
        X = F.clauses{i}.data;
        AB = getbase(X);
        K.l = prod(size(X));
        variables = getvariables(X);
        [lb,ub,cand_rows] = findulb(AB,K);
        LU(variables,1) = max([lb LU(variables,1)]')';
        LU(variables,2) = min([ub LU(variables,2)]')';
    elseif F.clauses{i}.type == 3
        % FIX : Extract from equalities and binary constraints
    end
end

binary = yalmip('binvariables');
LU(binary,1) = 0;
LU(binary,2) = 1;

% Try to bound some nonlinear terms
% FIX: complete code
[mt,variable_type] = yalmip('monomtable');
quadratic = find(variable_type == 2);
if ~isempty(quadratic)
    M = mt(quadratic,:);
    for i = 1:size(M,1)
        [ii,jj] = find(M(i,:));
        if length(ii) == 1
            LU(quadratic(i),1) = min([0 LU(jj,1)^2]);
            LU(quadratic(i),2) = max([LU(jj,1)^2 LU(jj,2)^2]);
        end
    end
end


function pot = convert_to_pot(CPD, pot_type, domain, evidence)
% CONVERT_TO_POT Convert a tabular CPD to one or more potentials
% pots = convert_to_pot(CPD, pot_type, domain, evidence)
%
% pots{i} = CPD evaluated using evidence(domain(:,i))
% If 'domains' is a single row vector, pots will be an object, not a cell array.

ncases = size(domain,2);
assert(ncases==1); % not yet vectorized

sz = dom_sizes(CPD);
ns = zeros(1, max(domain));
ns(domain) = sz;

local_ev = evidence(domain);
obs_bitv = ~isemptycell(local_ev);
odom = domain(obs_bitv);
T = convert_to_table(CPD, domain, local_ev, obs_bitv);

switch pot_type
 case 'u',
  pot = upot(domain, sz, T, 0*myones(sz));
 case 'd',
  ns(odom) = 1;
  pot = dpot(domain, ns(domain), T);
 case {'c','g'},
  % Since we want the output to be a Gaussian, the whole family must be observed.
  % In other words, the potential is really just a constant.
  p = T;
  %p = prob_node(CPD, evidence(domain(end)), evidence(domain(1:end-1)));
  ns(domain) = 0;
  pot = cpot(domain, ns(domain), log(p));
 case 'cg',
  T = T(:);
  ns(odom) = 1;
  can = cell(1, length(T));
  for i=1:length(T)
    can{i} = cpot([], [], log(T(i)));
  end
  pot = cgpot(domain, [], ns, can);
 otherwise,
  error(['unrecognized pot type ' pot_type])
end


classdef RVEAiGNG < ALGORITHM
% <multi/many> <real/integer/label/binary/permutation>
% RVEA based on improved growing neural gas
% alpha --- 2 --- The parameter controlling the rate of change of penalty

%------------------------------- Reference --------------------------------
% Q. Liu, Y. Jin, M. Heiderich, T. Rodemann, and G. Yu, An adaptive
% reference vector-guided evolutionary algorithm using growing neural gas
% for many-objective optimization of irregular problems, IEEE Transactions
% on Cybernetics, 2022, 52(5): 2698-2711.
%--------------------------------------------------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

% This function is written by Qiqi Liu

    methods
        function main(Algorithm,Problem)
            %% Parameter setting
            params.N = Problem.N;
            params.MaxIt = 50;
            params.L = 50;
            params.epsilon_b = 0.2;
            params.epsilon_n = 0.006;
            params.alpha = 0.5;
            params.delta = 0.995;
            params.T = 50;

            %% Parameter setting
            alpha = Algorithm.ParameterSet(2);
            [V,Problem.N] = UniformPoint(Problem.N,Problem.M);
            Population    = Problem.Initialization();
            net = InitilizeGrowingGasNet(V,Population,params);
            Archive = UpdateArchive(Population,[],Problem.N);
            scale = ones(1,Problem.M);
            zmin = min(Population.objs,[],1);
            genFlag = [];

            while Algorithm.NotTerminated(Population)
                MatingPool = randi(length(Population),1,Problem.N);
                Offspring  = OperatorGA(Problem,Population(MatingPool));
                zmin       = min([zmin;Offspring.objs],[],1);
                [Population,net,V,Archive,scale,genFlag] = EnvironmentalSelection([Population,Offspring],V,(Problem.FE/Problem.maxFE)^alpha,net,params,Archive,Problem,scale,zmin,genFlag);
            end
        end
    end
end