function a = gridsel(alg,hyper)

%==============================================================
% GRIDSEL model selection object via grid search
%==============================================================
% A=GRIDSEL(A,H) returns a gridsel object initialized with
% algorithms A and hyperparameters H.
%
% Finds the best of the algorithm from the set A, and trains
% and stores that model.
%
% Hyperparameters.
%
%  a.child=A                -- methods to evaluate
%  a.loss='class_loss'      -- loss measure to use
%  a.score=cv('folds=5')    -- method of evaluating algorithms
%
% Model
%  a.scores=[]      % score of all of methods tried
%  a.best_score=[]  % score of best methods tried
%  a.best_index=[]  % index of best methods tried
%  a.best=[]        % learnt model of best method tried
%
% Example:
% % train 3 svms with C=1,2,3 and validate with 3 fold cross validation
% [r,a]=train(gridsel(param(svm,'C',[1,2,3]),{'score=cv;score.folds=3'}),gen(toy)) ;
%
% Methods:
%  train, test
%=============================================================
% Reference :
% Author    :
% Link      :
%=============================================================

  % hypers
  a.child=alg;     % original algorithm to start with
  a.score=cv; a.score.folds=5;
  a.loss='class_loss';

  % model
  a.all_algs=[];   % all trained models
  a.scores=[];     % score of all of methods tried
  a.best_score=[]; % score of best method tried
  a.best_index=[]; % index of best method tried
  a.best=[];       % learnt model of best method tried (without cv, etc.)

  p=algorithm('gridsel');
  a= class(a,'gridsel',p);

  if nargin==2,
    eval_hyper;
  end;


function [A, C, Q, R, initx, initV] = ensure_AR(A, C, Q, R, initx, initV, k, obs, diagonal)
%
% Ensure that the system matrices have the right form for an autoregressive process.

ss = length(A);
if nargin<8, obs=ones(ss, 1); end
if nargin<9, diagonal=0; end

[coef, C] = SS_to_AR(A, Q, k, diagonal);
[A, C, Q, R, initx, initV] = AR_to_SS(coef, C, obs);


function out=atan2(y,x)

if isa(x,'mp')
 precision=x(1).precision;
 if ~isa(y,'mp')
  y=mp(y,precision);
 end
else
 precision=y(1).precision;
 if ~isa(x,'mp')
  x=mp(x,precision);
 end
end

if any(~isreal(x)) | any(~isreal(y))
 warning('atan2 for mp objects currently ignores imaginary parts')
 x=real(x);
 y=real(y);
end

ex=numel(x); ey=numel(y);
if ex==1
 x=ones(size(y))*x;
elseif ey==1
 y=ones(size(x))*y;
end

out=atan(y./x);
mpPi=mppi(precision);
for ii=1:numel(x)
 if x(ii)<0
  if y(ii)>0
   out(ii)=out(ii)+mpPi;
  else
   out(ii)=out(ii)-mpPi;
  end
 end
end

out=real(out);


% Test file for trigtech/real.m

function pass = test_real(pref)

% Get preferences.
if ( nargin < 1 )
    pref = trigtech.techPref();
end

testclass = trigtech();

% Test a scalar-valued function.
f = testclass.make(@(x) exp(20i*pi*x) + 1i*sin(100*pi*x), [], pref);
g = testclass.make(@(x) cos(20*pi*x), [], pref);
h = real(f);
g = prolong(g, length(h));
pass(1) = norm(h.coeffs - g.coeffs, inf) < 10*vscale(h)*eps;

% Test an array-valued function.
f = testclass.make(@(x) [exp(20i*pi*x) + 1i*sin(100*pi*x), -exp(10i*pi*x)], [], pref);
g = testclass.make(@(x) [cos(20*pi*x), -real(exp(10i*pi*x))], [], pref);
h = real(f);
n = max(length(g),length(h));
g = prolong(g,n); h = prolong(h,n);
pass(2) = norm(h.coeffs - g.coeffs, inf) < 10*max(vscale(h)*eps);

% Test a real function.
f = 1i*testclass.make(@(x) cos(30*pi*x), [], pref);
g = real(f);
pass(3) = numel(g.coeffs) == 1 && g.coeffs == 0;

% Test an array-valued real function.
f = 1i*testclass.make(@(x) [cos(99*pi*x), sin(99*pi*x), exp(cos(pi*x))], [], pref);
g = real(f);
pass(4) = all(size(g.coeffs) == [1, 3]) && all(g.coeffs == 0);

end


function Y=AveEmbedding(dataset,parameters)
dataStr=['./../dataset/',dataset,'-STC2.mat'];
load(dataStr);
parameters.vocSize = size_vocab;
% Step 1. Generate word vector sets
CR_E = randi([-25,25],parameters.wordDim,parameters.vocSize)/100;
disp(strcat('Number of weights E:',num2str(size(CR_E))));
vocab_emb_length = length(vocab_emb_Word2vec_48(1,:));
if vocab_emb_length > size_vocab
    error(['Error, and the size fo vocab_emb is:',vocab_emb_length])
end
CR_E(1:parameters.wordDim,vocab_emb_Word2vec_48_index) = vocab_emb_Word2vec_48(1:parameters.wordDim,1:vocab_emb_length);
% Step 2. Compute TF-IDF
if parameters.weightMode
    fea_All=tf_idf(fea_All);
end
% Step 3. Average Embedding
textSize = length(fea_All(:,1));
fea_vector =[];
for i=1:textSize
    tmp_fea_vector_weight = repmat(fea_All(i,find(fea_All(i,:)>0)),parameters.wordDim,1);
    tmp_fea_vector_matrix = CR_E(:,find(fea_All(i,:)>0)) .* tmp_fea_vector_weight;
    tmp_fea_vector = sum(tmp_fea_vector_matrix,2);
    fea_vector(i,:) = tmp_fea_vector';
end
% Step 4. Normalize features
Y = normalize(fea_vector);
end


Network torchvision.models.mnasnet {
Layer Conv2d-1 {
Type: CONV
Stride { X: 2, Y: 2 }
Dimensions { K: 32, C: 3, R: 3, S: 3, Y: 224, X: 224 }
}
Layer Conv2d-2 {
Type: DSCONV
Stride { X: 1, Y: 1 }
Dimensions { K: 1, C: 32, R: 3, S: 3, Y: 112, X: 112 }
}
Layer Conv2d-3 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 16, C: 32, R: 1, S: 1, Y: 112, X: 112 }
}
Layer Conv2d-4 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 48, C: 16, R: 1, S: 1, Y: 112, X: 112 }
}
Layer Conv2d-5 {
Type: DSCONV
Stride { X: 2, Y: 2 }
Dimensions { K: 1, C: 48, R: 3, S: 3, Y: 112, X: 112 }
}
Layer Conv2d-6 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 24, C: 48, R: 1, S: 1, Y: 56, X: 56 }
}
Layer Conv2d-7 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 72, C: 24, R: 1, S: 1, Y: 56, X: 56 }
}
Layer Conv2d-8 {
Type: DSCONV
Stride { X: 1, Y: 1 }
Dimensions { K: 1, C: 72, R: 3, S: 3, Y: 56, X: 56 }
}
Layer Conv2d-9 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 24, C: 72, R: 1, S: 1, Y: 56, X: 56 }
}
Layer Conv2d-10 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 72, C: 24, R: 1, S: 1, Y: 56, X: 56 }
}
Layer Conv2d-11 {
Type: DSCONV
Stride { X: 1, Y: 1 }
Dimensions { K: 1, C: 72, R: 3, S: 3, Y: 56, X: 56 }
}
Layer Conv2d-12 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 24, C: 72, R: 1, S: 1, Y: 56, X: 56 }
}
Layer Conv2d-13 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 72, C: 24, R: 1, S: 1, Y: 56, X: 56 }
}
Layer Conv2d-14 {
Type: DSCONV
Stride { X: 2, Y: 2 }
Dimensions { K: 1, C: 72, R: 5, S: 5, Y: 56, X: 56 }
}
Layer Conv2d-15 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 40, C: 72, R: 1, S: 1, Y: 28, X: 28 }
}
Layer Conv2d-16 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 120, C: 40, R: 1, S: 1, Y: 28, X: 28 }
}
Layer Conv2d-17 {
Type: DSCONV
Stride { X: 1, Y: 1 }
Dimensions { K: 1, C: 120, R: 5, S: 5, Y: 28, X: 28 }
}
Layer Conv2d-18 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 40, C: 120, R: 1, S: 1, Y: 28, X: 28 }
}
Layer Conv2d-19 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 120, C: 40, R: 1, S: 1, Y: 28, X: 28 }
}
Layer Conv2d-20 {
Type: DSCONV
Stride { X: 1, Y: 1 }
Dimensions { K: 1, C: 120, R: 5, S: 5, Y: 28, X: 28 }
}
Layer Conv2d-21 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 40, C: 120, R: 1, S: 1, Y: 28, X: 28 }
}
Layer Conv2d-22 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 240, C: 40, R: 1, S: 1, Y: 28, X: 28 }
}
Layer Conv2d-23 {
Type: DSCONV
Stride { X: 2, Y: 2 }
Dimensions { K: 1, C: 240, R: 5, S: 5, Y: 28, X: 28 }
}
Layer Conv2d-24 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 80, C: 240, R: 1, S: 1, Y: 14, X: 14 }
}
Layer Conv2d-25 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 480, C: 80, R: 1, S: 1, Y: 14, X: 14 }
}
Layer Conv2d-26 {
Type: DSCONV
Stride { X: 1, Y: 1 }
Dimensions { K: 1, C: 480, R: 5, S: 5, Y: 14, X: 14 }
}
Layer Conv2d-27 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 80, C: 480, R: 1, S: 1, Y: 14, X: 14 }
}
Layer Conv2d-28 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 480, C: 80, R: 1, S: 1, Y: 14, X: 14 }
}
Layer Conv2d-29 {
Type: DSCONV
Stride { X: 1, Y: 1 }
Dimensions { K: 1, C: 480, R: 5, S: 5, Y: 14, X: 14 }
}
Layer Conv2d-30 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 80, C: 480, R: 1, S: 1, Y: 14, X: 14 }
}
Layer Conv2d-31 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 480, C: 80, R: 1, S: 1, Y: 14, X: 14 }
}
Layer Conv2d-32 {
Type: DSCONV
Stride { X: 1, Y: 1 }
Dimensions { K: 1, C: 480, R: 3, S: 3, Y: 14, X: 14 }
}
Layer Conv2d-33 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 96, C: 480, R: 1, S: 1, Y: 14, X: 14 }
}
Layer Conv2d-34 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 576, C: 96, R: 1, S: 1, Y: 14, X: 14 }
}
Layer Conv2d-35 {
Type: DSCONV
Stride { X: 1, Y: 1 }
Dimensions { K: 1, C: 576, R: 3, S: 3, Y: 14, X: 14 }
}
Layer Conv2d-36 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 96, C: 576, R: 1, S: 1, Y: 14, X: 14 }
}
Layer Conv2d-37 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 576, C: 96, R: 1, S: 1, Y: 14, X: 14 }
}
Layer Conv2d-38 {
Type: DSCONV
Stride { X: 2, Y: 2 }
Dimensions { K: 1, C: 576, R: 5, S: 5, Y: 14, X: 14 }
}
Layer Conv2d-39 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 192, C: 576, R: 1, S: 1, Y: 7, X: 7 }
}
Layer Conv2d-40 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 1152, C: 192, R: 1, S: 1, Y: 7, X: 7 }
}
Layer Conv2d-41 {
Type: DSCONV
Stride { X: 1, Y: 1 }
Dimensions { K: 1, C: 1152, R: 5, S: 5, Y: 7, X: 7 }
}
Layer Conv2d-42 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 192, C: 1152, R: 1, S: 1, Y: 7, X: 7 }
}
Layer Conv2d-43 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 1152, C: 192, R: 1, S: 1, Y: 7, X: 7 }
}
Layer Conv2d-44 {
Type: DSCONV
Stride { X: 1, Y: 1 }
Dimensions { K: 1, C: 1152, R: 5, S: 5, Y: 7, X: 7 }
}
Layer Conv2d-45 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 192, C: 1152, R: 1, S: 1, Y: 7, X: 7 }
}
Layer Conv2d-46 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 1152, C: 192, R: 1, S: 1, Y: 7, X: 7 }
}
Layer Conv2d-47 {
Type: DSCONV
Stride { X: 1, Y: 1 }
Dimensions { K: 1, C: 1152, R: 5, S: 5, Y: 7, X: 7 }
}
Layer Conv2d-48 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 192, C: 1152, R: 1, S: 1, Y: 7, X: 7 }
}
Layer Conv2d-49 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 1152, C: 192, R: 1, S: 1, Y: 7, X: 7 }
}
Layer Conv2d-50 {
Type: DSCONV
Stride { X: 1, Y: 1 }
Dimensions { K: 1, C: 1152, R: 3, S: 3, Y: 7, X: 7 }
}
Layer Conv2d-51 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 320, C: 1152, R: 1, S: 1, Y: 7, X: 7 }
}
Layer Conv2d-52 {
Type: CONV
Stride { X: 1, Y: 1 }
Dimensions { K: 1280, C: 320, R: 1, S: 1, Y: 7, X: 7 }
}
Layer Linear-53 {
Type: CONV
Dimensions { K: 1000, C: 1280, R: 1, S: 1, Y: 1, X: 1 }
}
}


% Compare basic MatConvNet and Caffe blocks numerically
rootpath = fileparts(fileparts(mfilename('fullpath')));
run(fullfile(rootpath, 'matlab', 'vl_setupnn.m'));

inputScale = 100;

caffemodel = fullfile('data', 'tmp_caffemodels', 'test_model');
[~,~,~] = mkdir('data');
[~,~,~] = mkdir(fileparts(caffemodel));

%%
layers = {};
layers{end+1} = struct(...
  'name', 'conv', ...
  'type', 'conv', ...
  'stride', [2, 2], ...
  'pad', [1, 1, 1, 1], ...
  'weights', {{rand(3, 3, 10, 5, 'single'), rand(5, 1, 'single')}});

layers{end+1} = struct(...
  'name', 'relu', ...
  'type', 'relu');

layers{end+1} = struct(...
  'name', 'norm', ...
  'type', 'normalize', ...
  'param', [5, 1, 2e-5, 0.75]);

layers{end+1} = struct(...
  'name', 'softmax', ...
  'type', 'softmax');

%%
net_ = struct();
net_.meta.normalization.imageSize = [20, 20, 10];
net_.meta.normalization.averageImage = rand(1, 1, 10);

diffStats = zeros(numel(layers), 3);
for li = 1:numel(layers)
  net_.layers = layers(li);
  layerName = layers{li}.name;
  simplenn_caffe_deploy(net_, caffemodel, 'doTest', false, ...
    'outputBlobName', layerName, 'silent', true);
  res = simplenn_caffe_compare(net_, caffemodel, [], ...
    'randScale', inputScale, 'silent', true);
  diffStats(li, :) = res.(layerName);
end

fprintf('Results: \n');
layerNames = cellfun(@(l) l.name, layers, 'UniformOutput', false);
fprintf('Layer   %s\n', sprintf('% 10s', layerNames{:}));
fprintf('MeanErr %s\n', sprintf('% 10.2e', diffStats(:, 2)));
fprintf('MaxErr  %s\n', sprintf('% 10.2e', diffStats(:, 3)));

rmdir(fileparts(caffemodel), 's');


function [W,W0] = setWeight(W,p)
% Set the weight vectors

%------------------------------- 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 Tomoaki Takagi

    if 0 <= p && p <= 1
        %% Static approach
        W0 = [];
        M = size(W,2);
        % Distribution control of weight vector set
        TF = W < 1.0 / M;
        W(TF) = W(TF) * p * M;
        W(~TF) = 1.0 - (1.0 - W(~TF)) * (1.0 - p) * M / (M - 1);
    else
        %% Dynamic approach
        W0 = W;
    end
end


function Xsol=Bouguet2Tsai()
%
% JMM Montiel. 6 sep 2005

% load matlab_calibration.mat
% fc = fc_frontal;
% kc = kc_frontal;
% cc = cc_frontal;
% alpha_c = alpha_c_frontal;
% cam = initialize_cam;
% Huai{
load H:\relaylatest\toolbox_calib\calib_casios\Calib_Results.mat
cam.k1 =    -kc(1);
cam.k2 =    kc(2);
cam.nRows = 720;
cam.nCols = 1280;
cam.Cx =    cc(1);
cam.Cy =    cc(2);
d=0.0112;
cam.f =     d*fc(1);
cam.dx =    d;
cam.dy =    d;
cam.model = 'two_distortion_parameters';

cam.K =     sparse( [ fc(1)   0     cc(1);
                0  fc(2)    cc(2);
                0    0     1] );

% Huai}

% % generate distorted table
% nCols = 320;
% nRows = 240;
% inc=round(min(nCols,nRows)/20)
% [u_grid,v_grid]=meshgrid(-150:inc:nCols+150,-150:inc:nRows+150);
% [nGrid,mGrid]=size(u_grid);
% undistorted=[reshape(u_grid,1, nGrid*mGrid); reshape(v_grid,1,nGrid*mGrid)];
% distorted = distort_bouguet(undistorted,kc,fc,cc)

addpath TOOLBOX_Calib;

Xini = [cam.k1; cam.k2; cam.f; cam.Cx; cam.Cy]

% Image size
nCols = cam.nCols;
nRows = cam.nRows;

% Step for the image grid
inc = round(min(nCols,nRows)/30);
% Image grid
[u_grid,v_grid] = meshgrid(3*inc:inc:nCols-3*inc,3*inc:inc:nRows-3*inc);
[nGrid,mGrid] = size(u_grid);
uv_d = [reshape(u_grid,1, nGrid*mGrid); reshape(v_grid,1,nGrid*mGrid)];

xy_u = normalize(uv_d,fc,cc,kc,alpha_c);
x_distort = [(xy_u(1,:)*fc(1) + cc(1));(xy_u(2,:)*fc(2) + cc(2))];

% Non-linear minimization, minimizes the error in the image grid between the
% Bouguet's calibration model and the one we use in our code. Xsol should
% return the calibration parameters for our code in the following order:
% k1, k2, f, cx, cy. dx and dy are set here to 0.0112
Xsol = lsqnonlin('matlab2tsai_error',...
    Xini,[0;0;0;0;0],[3*cam.k1;3*cam.k2;3*cam.f;nCols;nRows],...
    optimset('LargeScale','on','Display','Iter','TolFun',1e-14),cam,uv_d,x_distort)


function [x,y,info] = sedumi(A,b,c,K,pars)
%      [x,y,info] = sedumi(A,b,c,K,pars)
%
% SEDUMI  Self-Dual-Minimization/ Optimization over self-dual homogeneous
%         cones.
%
% >  X = SEDUMI(A,b,c) yields an optimal solution to the linear program
%      MINIMIZE c'*x SUCH THAT A*x = b, x >= 0
%      x is a vector of decision variables.
%      If size(A,2)==length(b), then it solves the linear program
%      MINIMIZE c'*x SUCH THAT A'*x = b, x >= 0
%
% >  [X,Y,INFO] = SEDUMI(A,b,c) also yields a vector of dual multipliers Y,
%      and a structure INFO, with the fields INFO.pinf, INFO.dinf and
%      INFO.numerr.
%
%    (1) INFO.pinf=INFO.dinf=0: x is an optimal solution (as above)
%      and y certifies optimality, viz.\ b'*y = c'*x and c - A'*y >= 0.
%      Stated otherwise, y is an optimal solution to
%      MAXIMIZE b'*y SUCH THAT c-A'*y >= 0.
%      If size(A,2)==length(b), then y solves the linear program
%      MAXIMIZE b'*y SUCH THAT c-A*y >= 0.
%
%    (2) INFO.pinf=1: there cannot be x>=0 with A*x=b, and this is certified
%      by y, viz. b'*y > 0 and A'*y <= 0. Thus y is a Farkas solution.
%
%    (3) INFO.dinf=1: there cannot be y such that c-A'*y >= 0, and this is
%      certified by x, viz. c'*x <0, A*x = 0, x >= 0. Thus x is a Farkas
%      solution.
%
%    (I)   INFO.numerr = 0: desired accuracy achieved (see PARS.eps).
%    (II)  INFO.numerr = 1: numerical problems warning. Results are accurate
%          merely to the level of PARS.bigeps.
%    (III) INFO.numerr = 2: complete failure due to numerical problems.
%
%    INFO.feasratio is the final value of the feasibility indicator. This
%    indicator converges to 1 for problems with a complementary solution, and
%    to -1 for strongly infeasible problems. If feasratio in somewhere in
%    between, the problem may be nasty (e.g. the optimum is not attained),
%    if the problem is NOT purely linear (see below). Otherwise, the reason
%    must lie in numerical problems: try to rescale the problem.
%
% >  [X,Y,INFO] = SEDUMI(A,b,0) or SEDUMI(A,b) solves the feasibility problem
%    FIND x>=0 such that A*x = b
%
% >  [X,Y,INFO] = SEDUMI(A,0,c) or SEDUMI(A,c) solves the feasibility problem
%    FIND y such that A'*y <= c
%
% >  [X,Y,INFO] = SEDUMI(A,b,c,K) instead of the constraint "x>=0", this
%      restricts x to a self-dual homogeneous cone that you describe in the
%      structure K. Up to 5 fields can be used, called K.f, K.l, K.q, K.r and
%      K.s, for Free, Linear, Quadratic, Rotated quadratic and Semi-definite.
%      In addition, there are fields K.xcomplex, K.scomplex and K.ycomplex
%      for complex-variables.
%
%    (1) K.f is the number of FREE, i.e. UNRESTRICTED primal components.
%      The dual components are restricted to be zero. E.g. if
%      K.f = 2 then x(1:2) is unrestricted, and z(1:2)=0.
%      These are ALWAYS the first components in x.
%
%    (2) K.l is the number of NONNEGATIVE components. E.g. if K.f=2, K.l=8
%      then x(3:10) >=0.
%
%    (3) K.q lists the dimensions of LORENTZ (quadratic, second-order cone)
%      constraints. E.g. if K.l=10 and K.q = [3 7] then
%          x(11) >= norm(x(12:13)),
%          x(14) >= norm(x(15:20)).
%      These components ALWAYS immediately follow the K.l nonnegative ones.
%      If the entries in A and/or c are COMPLEX, then the x-components in
%      "norm(x(#,#))" take complex-values, whenever that is beneficial.
%       Use K.ycomplex to impose constraints on the imaginary part of A*x.
%
%    (4) K.r lists the dimensions of Rotated LORENTZ
%      constraints. E.g. if K.l=10, K.q = [3 7] and K.r = [4 6], then
%          2*x(21)x(22) >= norm(x(23:24))^2,
%          2*x(25)x(26) >= norm(x(27:30))^2.
%      These components ALWAYS immediately follow the K.q ones.
%      Just as for the K.q-variables, the variables in "norm(x(#,#))" are
%      allowed to be complex, if you provide complex data. Use K.ycomplex
%      to impose constraints on the imaginary part of A*x.
%
%    (5) K.s lists the dimensions of POSITIVE SEMI-DEFINITE (PSD) constraints
%      E.g. if K.l=10, K.q = [3 7] and K.s = [4 3], then
%          mat( x(21:36),4 ) is PSD,
%          mat( x(37:45),3 ) is PSD.
%      These components are ALWAYS the last entries in x.
%
%    (a) K.xcomplex lists the components in f,l,q,r blocks that are allowed
%     to have nonzero imaginary part in the primal. For f,l blocks, these
%    (b) K.scomplex lists the PSD blocks that are Hermitian rather than
%      real symmetric.
%    (c) Use K.ycomplex to impose constraints on the imaginary part of A*x.
%
%    The dual multipliers y have analogous meaning as in the "x>=0" case,
%    except that instead of "c-A'*y>=0" resp. "-A'*y>=0", one should read that
%    c-A'*y resp. -A'*y are in the cone that is described by K.l, K.q and K.s.
%    In the above example, if z = c-A'*y and mat(z(21:36),4) is not symmetric/
%    Hermitian, then positive semi-definiteness reflects the symmetric/
%    Hermitian parts, i.e. Z + Z' is PSD.
%
%    If the model contains COMPLEX data, then you may provide a list
%    K.ycomplex, with the following meaning:
%      y(i) is complex if ismember(i,K.ycomplex)
%      y(i) is real otherwise
%    The equality constraints in the primal are then as follows:
%          A(i,:)*x = b(i)      if imag(b(i)) ~= 0 or ismember(i,K.ycomplex)
%          real(A(i,:)*x) = b(i)  otherwise.
%    Thus, equality constraints on both the real and imaginary part
%    of A(i,:)*x should be listed in the field K.ycomplex.
%
%    You may use EIGK(x,K) and EIGK(c-A'*y,K) to check that x and c-A'*y
%    are in the cone K.
%
% >  [X,Y,INFO] = SEDUMI(A,b,c,K,pars) allows you to override the default
%      parameter settings, using fields in the structure `pars'.
%
%    (1) pars.fid     By default, fid=1. If fid=0, then SeDuMi runs quietly,
%      i.e. no screen output. In general, output is written to a device or
%      file whose handle is fid. Use fopen to assign a fid to a file.
%
%    (2) pars.alg     By default, alg=2. If alg=0, then a first-order wide
%      region algorithm is used, not recommended. If alg=1, then SeDuMi uses
%      the centering-predictor-corrector algorithm with v-linearization.
%      If alg=2, then xz-linearization is used in the corrector, similar
%      to Mehrotra's algorithm. The wide-region centering-predictor-corrector
%      algorithm was proposed in Chapter 7 of
%        J.F. Sturm, Primal-Dual Interior Point Approach to Semidefinite Pro-
%        gramming, TIR 156, Thesis Publishers Amsterdam, 1997.
%
%    (3) pars.theta, pars.beta   By default, theta=0.25 and beta=0.5. These
%      are the wide region and neighborhood parameters. Valid choices are
%      0 < theta <= 1 and 0 < beta < 1. Setting theta=1 restricts the iterates
%      to follow the central path in an N_2(beta)-neighborhood.
%
%    (4) pars.stepdif, pars.w. By default, stepdif = 2 and w=[1 1].
%       This implements an adaptive heuristic to control ste-differentiation.
%       You can enable primal/dual step length differentiation by setting stepdif=1 or 0.
%      If so, it weights the rel. primal, dual and gap residuals as
%      w(1):w(2):1 in order to find the optimal step differentiation.
%
%    (5) pars.eps     The desired accuracy. Setting pars.eps=0 lets SeDuMi run
%      as long as it can make progress. By default eps=1e-8.
%
%    (6) pars.bigeps  In case the desired accuracy pars.eps cannot be achieved,
%     the solution is tagged as info.numerr=1 if it is accurate to pars.bigeps,
%     otherwise it yields info.numerr=2.
%
%    (7) pars.maxiter Maximum number of iterations, before termination.
%
%    (8) pars.stopat  SeDuMi enters debugging mode at the iterations specified in this vector.
%
%    (9) pars.cg      Various parameters for controling the Preconditioned conjugate
%     gradient method (CG), which is only used if results from Cholesky are inaccurate.
%    (a) cg.maxiter   Maximum number of CG-iterates (per solve). Theoretically needed
%          is |add|+2*|skip|, the number of added and skipped pivots in Cholesky.
%          (Default 49.)
%    (b) cg.restol    Terminates if residual is a "cg.restol" fraction of duality gap.
%          Should be smaller than 1 in order to make progress (default 5E-3).
%    (c) cg.refine    Number of refinement loops that are allowed. The maximum number
%          of actual CG-steps will thus be 1+(1+cg.refine)*cg.maxiter. (default 1)
%    (d) cg.stagtol  Terminates if relative function progress less than stagtol (5E-14).
%    (e) cg.qprec    Stores cg-iterates in quadruple precision if qprec=1 (default 0).
%
%    (10) pars.chol   Various parameters for controling the Cholesky solve.
%     Subfields of the structure pars.chol are:
%    (a) chol.canceltol: Rel. tolerance for detecting cancelation during Cholesky (1E-12)
%    (b) chol.maxu:   Adds to diagonal if max(abs(L(:,j))) > chol.maxu otherwise (5E5).
%    (c) chol.abstol: Skips pivots falling below abstol (1e-20).
%    (d) chol.maxuden: pivots in dense-column factorization so that these factors
%      satisfy max(abs(Lk)) <= maxuden (default 5E2).
%
%    (11) pars.vplot  If this field is 1, then SeDuMi produces a fancy
%      v-plot, for research purposes. Default: vplot = 0.
%
%    (12) pars.errors  If this field is 1 then SeDuMi outputs some error
%    measures as defined in the Seventh DIMACS Challenge. For more details
%    see the User Guide.
%
% Bug reports can be submitted at http://sedumi.mcmaster.ca.
%
% See also mat, vec, cellK, eyeK, eigK

% This file is part of SeDuMi 1.21 by Imre Polik and Oleksandr Romanko
% Copyright (C) 2009 Imre Polik, Lehigh University, Bethlehem, PA, USA (since 1.21)
%
% Copyright (C) 2006 McMaster University, Hamilton, CANADA  (since 1.1)
%
% Copyright (C) 2001 Jos F. Sturm (up to 1.05R5)
%   Dept. Econometrics & O.R., Tilburg University, the Netherlands.
%   Supported by the Netherlands Organization for Scientific Research (NWO).
%
% Affiliation SeDuMi 1.03 and 1.04Beta (2000):
%   Dept. Quantitative Economics, Maastricht University, the Netherlands.
%
% Affiliations up to SeDuMi 1.02 (AUG1998):
%   CRL, McMaster University, Canada.
%   Supported by the Netherlands Organization for Scientific Research (NWO).
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc.,  51 Franklin Street, Fifth Floor, Boston, MA
% 02110-1301, USA

% J.F. Sturm, "Using SeDuMi 1.02, a MATLAB toolbox for optimization over
% symmetric cones," Optimization Methods and Software 11-12 (1999) 625-653.
% http://sedumi.mcmaster.ca

cputime0=cputime;
% ************************************************************
% INITIALIZATION
% ************************************************************
% ----------------------------------------
% Check input
% ----------------------------------------
if (nargin < 5)
    pars.fid = 1;
    if nargin < 3
        if nargin < 2
            error('Should have at least (A,b) or (A,c) arguments')
        end
        if length(b) == max(size(A))
            c = b; b = 0;           % given (A,c): LP feasibility problem
        else
            c = 0;                  % given (A,b): LP feasibility problem
        end
    end
    if isstruct(c)
        if nargin == 4
            pars = K;      % given (A,b,K,pars) or (A,c,K,pars)
        end
        K = c;          % given (A,b,K) or (A,c,K): cone feasibility problem
        if length(b) == max(size(A))
            c = b; b = 0;
        else
            c = 0;
        end
    elseif nargin < 4
        K.l = max(size(A));    % given (A,b,c): default to LP problem.
    end
end
% ----------------------------------------
% Bring data in (strict) internal SeDuMi form, check parameters,
% and print welcome.
% ----------------------------------------
[A,b,c,K,prep,origcoeff] = pretransfo(A,b,c,K,pars);
[N,m]=size(A);
if issparse(A)
    if sprank([A;b'])>sprank(A)
        info.pinf=1;
        x=[];
        %A dual improving direction could be computed easily
        y=[A;b']\[zeros(N,1);1];

        warning('SeDuMi:PrimalInfeasible','The problem is primal infeasible. there is no x such that Ax=b.')
        return
    elseif sprank(A)<m
        error('The coefficient matrix is not full row rank.')
    end
else
    if rank([A;b'])>rank(A)
        info.pinf=1;
        x=[];
        y=[A;b']\[zeros(N,1);1];
        warning('SeDuMi:PrimalInfeasible', 'The problem is primal infeasible, there is no x such that Ax=b.')
        return
    elseif rank(A)<m
        error('The coefficient matrix is not full row rank.')
    end
end
if prep.cpx.dim>0
    origcoeff=[];      % No error measures for complex problems.
end
lponly = (K.l == length(c));
pars = checkpars(pars,lponly);
% ----------------------------------------
% Print welcome and statistics of cone-problem
% ----------------------------------------
my_fprintf(pars.fid,'SeDuMi 1.21 by AdvOL, 2005-2008 and Jos F. Sturm, 1998-2003.\n');
switch pars.alg
    case 0
        my_fprintf(pars.fid,'Alg = 0: No corrector, ');
    case  1
        my_fprintf(pars.fid,'Alg = 1: v-corrector, ');
    case 2
        my_fprintf(pars.fid,'Alg = 2: xz-corrector, ');
end
switch pars.stepdif
    case 0
    case 1
        my_fprintf(pars.fid,'Step-Differentiation, ');
    case 2
        my_fprintf(pars.fid,'Adaptive Step-Differentiation, ');
end
my_fprintf(pars.fid,'theta = %5.3f, beta = %5.3f\n',pars.theta,pars.beta);
% --------------------------------------------------
% Print preprocessing information
% --------------------------------------------------
if pars.prep==1
    if isfield(prep,'sdp')
        blockcount=0;
        varcount=0;
        for sdpind=1:length(prep.sdp)
            if prep.sdp{sdpind}(1)==1
                blockcount=blockcount+1;
                varcount=varcount+prep.sdp{sdpind}(2);
            end
        end
        if blockcount>0
            my_fprintf(pars.fid,'Detected %i diagonal SDP block(s) with %i linear variables\n',blockcount,varcount);
        end
    end
    if isfield(prep,'freeblock1') && ~isempty(prep.freeblock1)
        my_fprintf(pars.fid,'Detected %i free variables in the linear part\n',length(prep.freeblock1));
    end
    if isfield(prep,'Kf') && prep.Kf>0
        switch pars.free
            case 0
                my_fprintf(pars.fid,'Split %i free variables\n',prep.Kf);
            case 1
                my_fprintf(pars.fid,'Put %i free variables in a quadratic cone\n',prep.Kf);
        end
    end
end
% --------------------------------------------------
% Remove dense columns (if any)
% --------------------------------------------------
Ablkjc = partitA(A,K.mainblks);
[dense,DAt.denq] = getdense(A,Ablkjc,K,pars);
if ~isempty(dense.cols)
    dense.A = A(dense.cols,:)';
    A(dense.cols,:) = 0.0;
    Ablkjc = partitA(A,K.mainblks);
else
    dense.A = sparse(length(b),0);
end
% ----------------------------------------
% Order constraints from sparse to dense, and find corresponding
% incremental nonzero pattern "Aord.dz" of At*dy in PSD part.
% ----------------------------------------
Aord.lqperm = sortnnz(A,[],Ablkjc(:,3));        % Sparse LP+Lorentz
DAt.q = findblks(A,Ablkjc,2,3,K.qblkstart);     % Lorentz ddotA-part
if ~isempty(DAt.q)
    DAt.q(dense.q,:) = 0.0;
    DAt.q = DAt.q + spones(extractA(A,Ablkjc,1,2,K.mainblks(1),K.mainblks(2)));
    Aord.qperm = sortnnz(DAt.q,[],[]);
else
    Aord.qperm = (1:length(b))';
end
[Aord.sperm, Aord.dz] = incorder(A,Ablkjc(:,3),K.mainblks(3));  % PSD
% ----------------------------------------
% Get nz-pattern of ADA.
% ----------------------------------------
ADA = getsymbada(A,Ablkjc,DAt,K.sblkstart);
% ----------------------------------------
% Ordering and symbolic factorization of ADA.
% ----------------------------------------
L = symbchol(ADA);
% --------------------------------------------------
% Symbolic fwsolve dense cols: L\[dense.A, dense.blkq],
% sparse ordering for dense column factorization
% --------------------------------------------------
symLden = symbcholden(L,dense,DAt);
% ----------------------------------------
% Initial solution
% ----------------------------------------
[d, v,vfrm, y,y0, R] = sdinit(A,b,c,dense,K,pars);
n = length(vfrm.lab);                         % order of K
merit = (sum(R.w) + max(R.sd,0))^2 * y0 / R.b0;  % Merit function
my_fprintf(pars.fid,'eqs m = %g, order n = %g, dim = %g, blocks = %g\n',...
    length(b),n,length(c),1 + length(K.q) + length(K.s));
my_fprintf(pars.fid,'nnz(A) = %d + %d, nnz(ADA) = %d, nnz(L) = %d\n',nnz(A),nnz(dense.A), nnz(ADA), nnz(L.L));
if ~isempty(dense.cols)
    my_fprintf(pars.fid,'Handling %d + %d dense columns.\n',...
        length(dense.cols),length(dense.q));
end
my_fprintf(pars.fid,' it :     b*y       gap    delta  rate   t/tP*  t/tD*   feas cg cg  prec\n');
my_fprintf(pars.fid,'  0 :            %8.2E %5.3f\n',merit,0);
% ----------------------------------------
% Initialize iterative statistics
% ----------------------------------------
STOP = 0;
err.maxb = 0.0;                 % Estimates the need to recompute residuals
iter = 0;
if pars.vplot == 1
    vlist = [vfrm.lab];
    ratelist = [];
end
wr.delta = 0.0;
wr.desc = 1;
%Seems unnecessary
%rate = 1.0;
feasratio = 0.0;
cputime1 = cputime;
% ************************************************************
% MAIN PREDICTOR-CORRECTOR LOOP
% ************************************************************
while STOP == 0
    iter = iter+1;
    if any(iter == pars.stopat)
        keyboard
    end

    if pars.stepdif==2 && ...
            (iter>20 || (iter>1 && (err.kcg + Lsd.kcg>3)) || ...
            (iter>5 && abs(1-feasratio)<0.05) )
        pars.stepdif=1;
    end
    % --------------------------------------------------
    % Compute ADA
    % --------------------------------------------------
    DAt = getDAtm(A, Ablkjc, dense, DAt.denq, d, K);
    ADA = getada1(ADA, A, Ablkjc(:,3), Aord.lqperm, d, K.qblkstart);
    ADA = getada2(ADA, DAt, Aord, K);
    [ADA,absd] = getada3(ADA, A, Ablkjc(:,3), Aord, invcholfac(d.u, K, d.perm), K);
    % ------------------------------------------------------------
    % Block Sparse Cholesky: ADA(L.perm,L.perm) = L.L*diag(L.d)*L.L'
    % ------------------------------------------------------------
    [L.L,L.d,L.skip,L.add] = blkchol(L,ADA,pars.chol,absd);
    % ------------------------------------------------------------
    % Factor dense columns
    % ------------------------------------------------------------
    [Lden, L.d] = deninfac(symLden, L,dense,DAt,d,absd, K.qblkstart,pars.chol);
    % ----------------------------------------
    % FACTORIZATION of self-dual embedding
    % ----------------------------------------
    Lsd = sdfactor(L,Lden, dense,DAt, d,v,y, A,c,K,R, y0,pars);
    % ------------------------------------------------------------
    % Compute and take IPM-step
    % from (v,y,v, y0) --> (xscl,y,zscl,y0)
    % ------------------------------------------------------------
    y0Old = y0;
    [xscl,yNxt,zscl,y0Nxt, w,relt, dxmdz,err, wr] = ...
        wregion(L,Lden,Lsd,...
        d,v,vfrm,A,DAt,dense, R,K,y,y0,b, pars, wr);
    % ------------------------------------------------------------
    % Evaluate the computed step.
    % ------------------------------------------------------------
    if y0Nxt > 0
        R.b = R.b + err.b / y0Nxt;
        R.sd = R.sd + err.g / y0Nxt;
        R.b0 = R.b0 + err.db0 / y0Nxt;
        y0 = y0Nxt;
    else
        R.b = (y0Nxt * R.b + err.b) / y0Old;      % In fact, we should have y0=0.
        R.sd = (y0Nxt * R.sd + err.g) / y0Old;
        R.b0 = (y0Nxt * R.b0 + err.db0) / y0Old;
        R.w(2) = abs(y0Nxt/y0Old) * R.w(2);        %=0: dual feasible
        R.c = (y0Nxt/y0Old) * R.c;
        R.maxRc = norm(R.c,inf);
        y0 = y0Old;
    end
    R.maxRb = norm(R.b,inf);                % Primal residual
    R.w(1) = 2 * pars.w(1) * R.maxRb / (1+R.maxb);
    meritOld = merit;
    merit = (sum(R.w) + max(R.sd,0))^2 * y0 / R.b0;
    rate = merit / meritOld;
    if (rate >= 0.9999) && (wr.desc == 1)
        % ------------------------------------------------------------
        % STOP = -1  --> Stop due to numerical problems
        % ------------------------------------------------------------
        STOP = -1;                  % insuf. progress in descent direction.
        iter = iter - 1;
        y0 = y0Old; %#ok
        my_fprintf(pars.fid,'Run into numerical problems.\n');
        break
    end
    feasratio = dxmdz(1) / v(1);            % (deltax0/x0) - (deltaz0/z0)
    % --------------------------------------------------
    % Primal-Dual transformation
    % --------------------------------------------------
    y = yNxt;
    by = full(sum(b.*y));
    [d,vfrm] = updtransfo(xscl,zscl,w,d,K);
    v = frameit(vfrm.lab,vfrm.q,vfrm.s,K);
    x0 = sqrt(d.l(1)) * v(1);
    % ----------------------------------------
    % SHOW ITERATION STATISTICS
    % ----------------------------------------
    my_fprintf(pars.fid,' %2.0f : %10.2E %8.2E %5.3f %6.4f %6.4f %6.4f %6.2f %2d %2d  ',...
        iter,by/x0,merit,wr.delta,rate,relt.p,relt.d,feasratio,err.kcg, Lsd.kcg);
    if pars.vplot == 1
        vlist = [vlist vfrm.lab/sqrt((R.b0*y0)/n)];
        ratelist = [ratelist rate];
    end
    % ----------------------------------------
    % If we get in superlinear region of LP,
    % try to guess optimal solution:
    % ----------------------------------------
    if lponly && (rate < 0.05)
        [xsol,ysol] = optstep(A,b,c, y0,y,d,v,dxmdz, ...
            K,L,symLden,dense, Ablkjc,Aord,ADA,DAt, feasratio, R,pars);
        if ~isempty(xsol)
            STOP = 2;                   % Means that we guessed right !!
            feasratio = 1 - 2*(xsol(1)==0);
            break
        end
    elseif (by > 0) && (abs(1+feasratio) < 0.05) && (R.b0*y0 < 0.5)
        if max(eigK(full(qreshape(Amul(A,dense,y,1),1,K)),K)) <= pars.eps * by
            STOP = 3;                   % Means Farkas solution found !
            break
        end
    end
    % --------------------------------------------------
    % OPTIMALITY CHECK: stop if y0*resid < eps * (x0+z0).
    % For feas. probs, we should divide the residual by x0, otherwise by z0.
    % Before stopping, recompute R.norm, since it may have changed due to
    % residual updates (the change should be small though).
    % --------------------------------------------------
    r0 = sum(R.w);
    cx = by + y0*R.sd - x0 / d.l(1);
    rgap = max(cx-by,0) / max([abs(cx),abs(by),1e-3 * x0]);
    precision1=y0*r0/(1+x0);
    precision2=(y0 * r0 + rgap)/x0;
    my_fprintf(pars.fid,'%1.1E\n',max(precision1,precision2));
    if precision1 < pars.eps       % P/D residuals small
        if precision2 < pars.eps    %Approx feasible and optimal
            STOP = 1;
            break
        elseif y0 * R.maxRb + x0 * R.maxb < -pars.eps * cx   % Approx Farkas
            STOP = 1;
            break
        elseif y0 * R.maxRc + x0 * R.maxc < pars.eps * by    % Approx Farkas
            STOP = 1;
            break;
        end
    end
    if iter >= pars.maxiter
        my_fprintf(pars.fid,'Maximum number of iterations reached.\n');
        STOP = -1;
    end
end % while STOP == 0.
my_fprintf(pars.fid,'\n');
clear ADA
nnzLadd=nnz(L.add);
nnzLskip=nnz(L.skip);
normLL=full(max(max(abs(L.L))));
clear L
% ************************************************************
% FINAL TASKS:
% ************************************************************
cputime2=cputime;
info.iter = iter;
info.feasratio = feasratio;
info.pinf = 0; info.dinf = 0;
info.numerr = 0;
% ------------------------------------------------------------
% Create x = D*v.
% ------------------------------------------------------------
if STOP == 2                % Exact optimal solution found (LP)
    x = xsol;
    y = ysol;
elseif STOP == 3            % Farkas solution y found (in early stage)
    x = zeros(length(c),1);
else
    x = [sqrt(d.l).*v(1:K.l); asmDxq(d,v,K); psdscale(d,v,K,1)];
end
% --------------------------------------------------
% Compute cx, Ax, etc.
% --------------------------------------------------
x0 = x(1);
cx = full(sum(c.*x));
abscx = sum(abs(c).*abs(x));
by = full(sum(b.*y));
Ax = Amul(A,dense,x,0);
Ay = full(Amul(A,dense,y,1));      % "full" since y may be scalar.
normy = norm(y);
normx = norm(x(2:end));
clear A

% ------------------------------------------------------------
% Determine infeasibility
% ------------------------------------------------------------

pinf = norm(x0*b-Ax);
z = qreshape(Ay-x0*c,1,K);
dinf = max(eigK(z,K));
if x0 > 0
    relinf = max(pinf / (1+R.maxb), dinf / (1+R.maxc)) / x0;
    % ------------------------------------------------------------
    % If infeasibility larger than epsilon, evaluate Farkas-infeasibility
    % ------------------------------------------------------------
    if relinf > pars.eps
        pdirinf = norm(Ax);
        ddirinf = max(eigK(qreshape(Ay,1,K),K));
        if cx < 0.0
            reldirinf = pdirinf / (-cx);
        else
            reldirinf = inf;
        end
        if by > 0.0
            reldirinf = min(reldirinf, ddirinf / by);
        end
        % ------------------------------------------------------------
        % If the quality of the Farkas solution is good and better than
        % the approx. feasible soln, set x0=0: Farkas solution found.
        % ------------------------------------------------------------
        if (reldirinf < pars.eps) || (relinf > max(pars.bigeps, reldirinf))
            x0 = 0.0;
            pinf = pdirinf;
            dinf = ddirinf;
        end
    end % relinf too large
end % x0 > 0
% ------------------------------------------------------------
% Interpret the solution as feasible:
% ------------------------------------------------------------
info.r0 = Inf;
if x0 > 0
    x = x / x0;
    y = y / x0;
    pinf = pinf /x0;
    dinf = dinf / x0;
    cx = cx/ x0;
    by = by / x0;
    normx = normx / x0;
    normy = normy / x0;
    if cx <= by                % zero or negative duality gap
        r0 = 0;
    elseif cx == 0.0           % Dual feasibility problem
        r0 = -by/(R.maxb*normy +1E-10 * x0);
    elseif by == 0.0           % Primal feasibility problem
        r0 = cx / (R.maxc*normx +1E-10 * x0);
    else                       % Optimization problem
        r0 = (cx-by)/(abs(by) + 1E-5 * (x0+abscx));
    end
    if r0 == 0,
        sigdig = Inf;
    else
        sigdig = -log10(r0);
    end
    my_fprintf(pars.fid,...
        'iter seconds digits       c*x               b*y\n');
    my_fprintf(pars.fid,'%3d %8.1f %5.1f %- 17.10e %- 17.10e\n',...
        iter,cputime2-cputime1,sigdig,cx,by);
    my_fprintf(pars.fid,'|Ax-b| = %9.1e, [Ay-c]_+ = %9.1E, |x|=%9.1e, |y|=%9.1e\n',...
        pinf,dinf,normx,normy);
    % ------------------------------------------------------------
    % Determine level of numerical problems with x0>0 (feasible)
    % ------------------------------------------------------------
    info.r0 = max([r0 ; pinf;dinf] ./ [1; 1+R.maxb+(1E-3)*R.maxRb;...
            1+R.maxc+(1E-3)*R.maxRc]);
    if STOP == -1
        if info.r0 > pars.bigeps
            my_fprintf(pars.fid, 'No sensible solution found.\n');
            info.numerr = 2;                          % serious numerical error
        elseif info.r0 > pars.eps
            info.numerr = 1;                          % moderate numerical error
        else
            info.numerr = 0;                          % achieved desired accuracy
        end
    else
        info.r0 = min( info.r0, pars.eps );
    end
else  % (if x0>0)
    % --------------------------------------------------
    % Infeasible problems: pinf==norm(Ax), dinf==max(eigK(At*y,K)).
    % --------------------------------------------------
    if pinf < -pars.bigeps * cx
        info.r0 = abs(pinf/cx);
        info.dinf = 1;
        abscx = -cx;
        pinf = pinf / abscx;
        normx = normx / abscx;
        x = x / abscx;
        my_fprintf(pars.fid, 'Dual infeasible, primal improving direction found.\n');
    end
    if dinf < pars.bigeps * by
        info.r0 = abs(dinf/by);
        info.pinf = 1;
        dinf = dinf / by;
        normy = normy / by;
        y = y / by;
        my_fprintf(pars.fid, 'Primal infeasible, dual improving direction found.\n');
    end
    my_fprintf(pars.fid,'iter seconds  |Ax|    [Ay]_+     |x|       |y|\n');
    my_fprintf(pars.fid,'%3d %8.1f %9.1e %9.1e %9.1e %9.1e\n',...
        iter,cputime2-cputime1,pinf,dinf,normx,normy);
    % --------------------------------------------------
    % Guess infeasible, but stopped due to numerical problems
    % --------------------------------------------------
    if info.pinf + info.dinf == 0
        my_fprintf(pars.fid, 'Failed: no sensible solution/direction found.\n');
        info.numerr = 2;
    elseif STOP == -1
        if (pinf > -pars.eps * cx) && (dinf > pars.eps * by)
            info.numerr = 1;
        else
            info.numerr = 0;
        end
    end
end
% ----------------------------------------
% - Bring xsol into the complex format of original (At,c),
% - transform q-variables into r-variables (Lorentz),
% - bring ysol into complex format, indicated by K.ycomplex.
% - at 0's in ysol where rows where removed.
% ----------------------------------------'
[x,y,K] = posttransfo(x,y,prep,K,pars); %#ok
% Detailed timing
%Preprocessing+IPM+Postprocessing
info.timing=[cputime1-cputime0 cputime2-cputime1 cputime-cputime2];
% Total time (for backward compatibility)
info.cpusec=sum(info.timing);
my_fprintf(pars.fid,'\nDetailed timing (sec)\n')
my_fprintf(pars.fid,'   Pre          IPM          Post\n')
my_fprintf(pars.fid,'%1.3E    ',info.timing)
my_fprintf(pars.fid,'\n')

% ----------------------------------------
% Make a fancy v-plot if desired
% ----------------------------------------
if pars.vplot == 1
    subplot(2,1,1)
    plot(0:iter,vlist,'o',[0 iter],[1 1],'b',...
        [0 iter],[pars.theta pars.theta],'g')
    title('Wide region v-plot')
    xlabel('iterations')
    ylabel('normalized v-values')
    subplot(2,1,2)
    plot(1:iter,ratelist)
    axis([0 iter 0 1])
    title('Reduction rates')
    xlabel('iterations')
    ylabel('reduction rate')
end
my_fprintf(pars.fid,'Max-norms: ||b||=%d, ||c|| = %d,\n',R.maxb,R.maxc);
my_fprintf(pars.fid,'Cholesky |add|=%d, |skip| = %d, ||L.L|| = %g.\n',...
    nnzLadd, nnzLskip, normLL);

% ----------------------------------------
% Compute error measures if needed
% ----------------------------------------
if ~isempty(origcoeff)
    %Reload the original coefficients
    s=(origcoeff.c)-(origcoeff.At)*sparse(y);       %To make s sparse
    cx=sum((origcoeff.c).*x);        %faster than c'*x
    by=sum((origcoeff.b).*y);
    xs=sum(x.*s);
    normb=norm(origcoeff.b,1);
    normc=norm(origcoeff.c,1);
    info.err=zeros(1,6);
    %     Error measures.
    %     Primal infeasibility
    info.err(1)=norm(x'*(origcoeff.At)-(origcoeff.b)',2)/(1+normb);
    %Let us get rid of the K.f part, since the free variables don't make
    %any difference in the cone infeasibility.
    %origcoeff.K.f=0;
    %     Primal cone infeasibility
    info.err(2)=max(0,-min(eigK(full(x(origcoeff.K.f+1:end)),origcoeff.K))/(1+normb));
    %     Dual infeasibility
    %info.err(3)=0.0; %s is not maintained explicitely
    %     Dual cone infeasibility
    info.err(4)=max(0,-min(eigK(full(s(origcoeff.K.f+1:end)),origcoeff.K))/(1+normc));
    %     Relative duality gap
    info.err(5)=(cx-by)/(1+abs(cx)+abs(by));
    %     Relative complementarity
    info.err(6)=xs/(1+abs(cx)+abs(by));

    my_fprintf(pars.fid,'\nDIMACS error measures\n')
    my_fprintf(pars.fid,'  PInf     PConInf     DInf    DConInf    RelGap    RelComp\n')
    my_fprintf(pars.fid,'%2.2E  ',info.err)
    my_fprintf(pars.fid,'\n')
end


close all;
clc;

num_samples_per_digit = 10;
rng('default');
digit_set = 0:9;
K = length(digit_set);
% Number of subspaces
trial.K = K;
cluster_sizes = num_samples_per_digit*ones(1, K);
trial.cluster_sizes = cluster_sizes;
% maximum dimension for each subspace
D = 10;
trial.D = D;
S = sum(cluster_sizes);
trial.S = S;
% identify sample indices for each digit
sample_list = [];
% experiment number
r = 1;
for k=1:K
    digit = digit_set(k);
    digit_indices = md.digit_indices(digit);
    num_digit_samples = length(digit_indices);
    % initialize the random number generator for repeatability
    rng( (r-1) * K + k);
    choices = randperm(num_digit_samples, cluster_sizes(k));
    selected_indices = digit_indices(choices);
    % fprintf('%d ', selected_indices);
    % fprintf('\n');
    sample_list = [sample_list selected_indices];
end
[Y, true_labels] = md.selected_samples(sample_list);
% Perform PCA to reduce dimensionality
Y = spx.la.pca.low_rank_approx(Y, 100);
Yn = spx.norm.normalize_l2(Y);
if true
    fprintf('\n\n\n Point Statistics:\n\n');
    [M, S]  = size(Yn);
    angle_result = spx.cluster.subspace.nearest_same_subspace_neighbors_by_inner_product(Yn, cluster_sizes);
    spx.cluster.subspace.print_nearest_neighbor_result(angle_result);
end

if false
    mf = spx.graphics.Figures;
    % Ambient space dimension and number of data points
    [trial.M, trial.S] = size(Y);
    % Number of subspaces
    trial.K = num_samples_per_digit;
    % maximum dimension for each subspace
    trial.D = 5;
    trial.cluster_sizes = cluster_sizes;
    % Solve the sparse subspace clustering problem
    tstart = tic;
    try
        clustering_result = solver(Y, trial.D, trial.K);
    catch ME
        % we will move on to next one
        fprintf('Problem in processing this example. %s: %s\n', ME.identifier, ME.message);
        % move on to next example
        rethrow(ME);
        error('cannot continue.');
    end
    trial.elapsed_time = toc (tstart);
    % graph connectivity
    trial.connectivity = clustering_result.connectivity;
    % estimated number of clusters
    trial.estimated_num_subspaces = clustering_result.num_clusters;
    % Time to compare the clustering
    cluster_labels = clustering_result.labels;
    comparison_result = spx.cluster.clustering_error(cluster_labels, true_labels, trial.K);
    trial.clustering_error_perc = comparison_result.error_perc;
    trial.clustering_acc_perc = 100 - comparison_result.error_perc;
    % Compute the statistics related to subspace preservation
    spr_stats = spx.cluster.subspace.subspace_preservation_stats(clustering_result.Z, trial.cluster_sizes);
    trial.spr_error = spr_stats.spr_error;
    trial.spr_flag = spr_stats.spr_flag;
    trial.spr_perc = spr_stats.spr_perc;
    fprintf('\nclustering error: %0.2f %% , clustering accuracy: %0.2f %%, mean spr error: %0.2f preserving : %0.2f %%, connectivity: %0.2f, elapsed time: %0.2f sec', trial.clustering_error_perc, trial.clustering_acc_perc, spr_stats.spr_error, spr_stats.spr_perc, trial.connectivity, trial.elapsed_time);
    fprintf('\n\n');
    Z = full(abs(clustering_result.Z));
    s = spx.stats.format_descriptive_statistics(Z(:));
    fprintf(s);
    fprintf('\n');
    fprintf('Missed points: \n');
    fprintf('%d ', find(comparison_result.misses));
    fprintf('\n');
    mf.new_figure('Representations');
    imshow(Z);
end


function G = divgrad(F)
%DIVGRAD   Laplacian of a CHEBFUN2V.
%   F = DIVGRAD(F) returns the Laplacian of a CHEBFUN2V i.e.,
%       divgrad(F) = F(1)_xx + F(2)_yy
%
% This command is not defined for a chebfun2v with 3 components.
%
% Also see CHEBFUN2V/LAP.

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

nComponents = F.nComponents;
if ( nComponents > 2 )
    error('CHEBFUN:CHEBFUN2V:divgrad:components',...
        'Command is not defined for CHEBFUN2V objects with >2 components.')
end

Fc = F.components;
G = diff(Fc{1}, [2,0]) + diff(Fc{2}, [0,2]);

end


function [ goodClusters ] = ClusterSelection( clusters, image, s, w )
%UNTITLED Summary of this function goes here
%   Detailed explanation goes here
height = length(image(:,1,1));
width = length(image(1,:,1));
discHeight = height/s;
discWidth = width/s;
numClusters = size(clusters,3);
windowClusters = zeros(discHeight,discWidth);
dists = zeros(1,numClusters);
boxedImage = OutlineRegion(image,ones(discHeight,discWidth));

for i=1:discHeight
    for j=1:discWidth
        hist = SimpleHist1D(image((i-1)*s+1:i*s,(j-1)*s+1:j*s,:),w);
        for k=1:numClusters
            dists(k) = Distance1D(hist,clusters(:,:,k));
        end
        [dist,windowClusters(i,j)] = min(dists); %#ok<ASGLU>
    end
end

display(windowClusters);
goodClusters = [];
response = '';

binaryImage = zeros(discHeight,discWidth);
for i=1:length(goodClusters)
    binaryImage = binaryImage + (windowClusters == goodClusters(i));
end
currentImage = DeleteWindowsImage(boxedImage,binaryImage);
imshow(currentImage);

while (not(strcmp(response,'done')))
    [x,y] = ginput(1);
    i = floor(y/s)+1;
    j = floor(x/s)+1;
    if (any(windowClusters(i,j)==goodClusters))
        goodClusters = goodClusters(goodClusters ~= windowClusters(i,j));
    else
        goodClusters = [goodClusters windowClusters(i,j)]; %#ok<AGROW>
        display(goodClusters);
    end
    binaryImage = zeros(discHeight,discWidth);
    for i=1:length(goodClusters)
        binaryImage = binaryImage + (windowClusters == goodClusters(i));
    end
    currentImage = DeleteWindowsImage(boxedImage,binaryImage);
    imshow(currentImage);
    response = input('more, or done?', 's');
end

end


function [sol, info] = gsp_proj_filterbank(x, ~ , G, W, y, param)
%GSP_PROJ_FILTERBANK Projection onto the synthesis coefficients
%   Usage:  sol = gsp_proj_filterbank(x, 0, G, W, y, param);
%           sol = gsp_proj_filterbank(x, 0, G, W, y);
%           [sol, info] = gsp_proj_filterbank(...)
%
%   Input parameters:
%         x     : Input signal
%         G     : Graph structure
%         W     : Filterbank (cell array of functions)
%         y     : Measurements
%         param : Structure of optional parameters
%   Output parameters
%         sol   : Solution.
%         info  : Structure summarizing informations at convergence
%
%   `gsp_proj_filterbank(x, gamma, G, W, param)` can solves:
%
%   .. sol = argmin_{z} 0.5*||x - z||_2^2  such that W^* x =y
%
%   .. math::  sol = \min_{z} \frac{1}{2} \|x - z\|_2^2 \text{ s. t. }  W^* x = y
%
%   Where $W$ is the linear analysis operator associated with the
%   filterbank.
%
%   The function can use different techniques
%
%   * 'exact' : if the Fourier basis is computed, go for this one
%   * 'cheby' : use the pseudo-inverse filters of the filterbank with
%     chebyshev approximation. It works well for well-conditionned
%     filterbanks.
%   * 'lanczos' : use the pseudo-inverse filters of the filterbank with
%     lanczos approximation. It works well for well-conditionned
%     filterbanks.
%   * 'proj_b2': scallable and robust way to do it. However, the
%     convergence maybe slow and might require a lot of filtering
%     operations.
%
%   param is a Matlab structure containing the following fields:
%
%   * *param.verbose* : 0 no log, 1 a summary at convergence, 2 print main
%     steps (default: 1)
%   * *param.eps* : tolerance for the pseudo inverse method
%   * *param.proj_method*: selected method
%
%   info is a Matlab structure containing the following fields:
%
%   * *info.algo* : Algorithm used
%   * *info.iter* : Number of iteration
%   * *info.time* : Time of exectution of the function in sec.
%   * *info.final_eval* : Final evaluation of the function
%   * *info.crit* : Stopping critterion used
%
%
%   See also:  gsp_solve_l1 gsp_proj_b2_filterbank
%

% Author: Nathanael Perraudin
% Date: 25 March 2014
% Testing: test_gsp_proj_filerbank

if nargin < 5
    error('GSP_PROJ_FILTERBANK: Not enought input arguments');
end

if nargin < 6, param=struct; end

if ~isfield(param, 'eps'), param.eps = 1e-8; end
if ~isfield(param, 'proj_method'),
    if gsp_check_fourier(G)
        param.proj_method = 'exact';
    else
        [A,B] = gsp_filterbank_bounds(G,W);
        if B/A > 20
            warning('Filterbank ill-conditioned, going for a slow method');
            param.proj_method = 'primal_dual';
        else
            param.proj_method = 'cheby';
        end
    end
end

t = tic;

if size(y,2)==1 && size(x,2)>1
    y = repmat(y,1,size(x,2));
end
if isfield(param,'method')
    param2 = rmfield(param,'method');
else
    param2 = param;
end
if strcmp(param.proj_method, 'proj_b2') || strcmp(param.proj_method, 'primal_dual')
    [~,B] = gsp_filterbank_bounds(G,W);
    if ~isfield(param,'paramproj'), param.paramproj = struct; end
    paramproj = param.paramproj;
    paramproj.A = @(x) gsp_filter_synthesis(G,W,x,param2);
    paramproj.At = @(x) gsp_filter_analysis(G,W,x,param2);
    paramproj.nu = B^2;
    paramproj.epsilon = sqrt(G.N)*param.eps;
    paramproj.y = y;
    paramproj.method  = param.proj_method;

    [sol,info] = proj_linear_eq(x,0,paramproj);

else
    if strcmp(param.proj_method, 'cheby')  || strcmp(param.proj_method, 'lanczos')
        [A,B] = gsp_filterbank_bounds(G,W);
        if B/A > 20
            warning('Filterbank ill-conditioned, check your solution!');
        end
    end
    Wd = gsp_design_can_dual(W,param.eps);
    sol =  x - gsp_filter_analysis(G,Wd,(gsp_filter_synthesis(G,W,x,param2)-y),param2);

    info.iter = 1;
    info.final_eval = 0;
    info.crit = 'Direct computation';
end

    info.time = toc(t);
    info.algo = param.proj_method;

end


function [Jul1,Jul2]=GPS2TT(Jul1,Jul2)
%GPS2TT  Convert from the timescale used by the Global Positioning System
%           (GPS)  given as a two-part Julian date to terrestrial time
%           (TT), represented as a two-part Julian date.
%
%INPUTS:    Jul1, Jul2  Two parts of a Julian date given in GPS time. The
%                       units of the date are days. The full date is the
%                       sum of both terms. The date is broken into two
%                       parts to provide more bits of precision. It does
%                       not matter how the date is split.
%
%OUTPUTS:   Jul1, Jul2  The time as a Julian date in TT.
%
%This function just calls GPS2TAI and TAI2TT.
%
%Many temporal coordinate systems standards are compared in [1].
%
%REFERENCES:
%[1] D. F. Crouse, "An Overview of Major Terrestrial, Celestial, and
%    Temporal Coordinate Systems for Target Tracking," Formal Report, Naval
%    Research Laboratory, no. NRL/FR/5344--16-10,279, 10 Aug. 2016, 173
%    pages.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

[Jul1,Jul2]=GPS2TAI(Jul1,Jul2);
[Jul1,Jul2]=TAI2TT(Jul1,Jul2);
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function [varargout] = nanmax(varargin)
%NANMAX Maximum value, ignoring NaNs.
%   M = NANMAX(A) returns the maximum of A with NaNs treated as missing.
%   For vectors, M is the largest non-NaN element in A.  For matrices, M is
%   a row vector containing the maximum non-NaN element from each column.
%   For N-D arrays, NANMAX operates along the first non-singleton
%   dimension.
%
%   [M,NDX] = NANMAX(A) returns the indices of the maximum values in A.  If
%   the values along the first non-singleton dimension contain more than
%   one maximal element, the index of the first one is returned.
%
%   M = NANMAX(A,B) returns an array the same size as A and B with the
%   largest elements taken from A or B.  Either one can be a scalar.
%
%   [M,NDX] = NANMAX(A,[],DIM) operates along the dimension DIM.
%
%   See also MAX, NANMIN, NANMEAN, NANMEDIAN, NANMIN, NANVAR, NANSTD.

%   Copyright 1993-2004 The MathWorks, Inc.
%   $Revision: 1.1.8.1 $  $Date: 2010/03/16 00:15:49 $

% Call [m,ndx] = max(a,b) with as many inputs and outputs as needed
[varargout{1:nargout}] = max(varargin{:});


%  Figure 10.28      Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%
% fig10_28.m is a script to generate Fig. 10.28,
% the step response of the collocated
% design for the satellite with PD compensation
% response at theta-2
clf;
m=[1, 0.1]; k0=[0, 0.091] ; d0=[0, 0.0036]; k1=[0, 0.4];
[f,g,h,j]=twomass(m,k0,d0); [f1,g,h,j] = twomass(m,k1,d0);
h1=[0, 0, 1, 0];

nc1=0.25*[2, 1];
dc1=[1/40, 1];

[ac,bc,cc,dc]=tf2ss(nc1, dc1);
[aol,bol,col,dol]= series(ac, bc,cc,dc,f,g,h1,j);
[acl]=aol-bol*col;
ccl2=[h,0*cc]
[aol1,bol1,col1,dol1] = series(ac,bc,cc,dc,f1,g,h1,j);
acl1=aol1-bol1*col1;
t=0:0.5:100;
sys=ss(acl,bol,ccl2,dol);
[y,t]=step(sys,t);
plot(t,y);
xlabel('Time (sec)');
ylabel('Amplitude');
grid on;
title('Fig. 10.28: Response at \theta_2 of the collocated design')


function [nodeRho, elemRho, cur, area, v] = curvaturedensity(node,elem, normal, gamma)

n = normal(elem(:,1),:) + normal(elem(:,2),:) + normal(elem(:,3),:);

v12 = node(elem(:,2),:) - node(elem(:,1),:);
v13 = node(elem(:,3),:) - node(elem(:,1),:);

v = cross(v12,v13,2);

area = 0.5*sqrt(sum(v.^2,2));

v = v./[2*area,2*area,2*area];

elemRho = abs(9 - sum(n.^2,2))./area;
elemRho = elemRho +sqrt(eps);

for i = 1:12
    nodeRho = accumarray(elem(:),repmat(elemRho.*area,3,1),[size(node,1),1]);
%     showsolution(node,elem,nodeRho);
%     axis equal;
%     colorbar;
%     pause(0.1);
    b = accumarray(elem(:),[area;area;area],[size(node,1),1]);
    nodeRho =nodeRho./b;
    elemRho = (nodeRho(elem(:,1)) + nodeRho(elem(:,2)) + nodeRho(elem(:,3)))/3;
end

cur = nodeRho;
elemRho = (elemRho/max(elemRho)).^(gamma);
nodeRho = (nodeRho/max(nodeRho)).^(gamma);


% POWER_BOUNDED computes the power cells about the points (x,y) inside
% the bounding box (must be a rectangle or a square) crs.  If crs is not supplied, an
% axis-aligned box containing (x,y) is used.
% It is optimised to work fast on large number of sites (e.g. 10000 sites or more)
% Input:
%   * x, y: coordinate of the Voronoi point (numPoints x 1)
%   * wts: weights of each point (numPoints x 1)
%   * crs: vortices of the bounding box in cw order (numVert x 2)
% Output:
%   * V: x,y-coordinate of vertices of the power cells
%   * C: indices of the Voronoi cells from V
% See Matlab's voronoin for more information about the output
% Made by: Aaron Becker, atbecker@uh.edu, and Muhammad Kasim, muhammad.kasim@wolfson.ox.ac.uk

function [V,C] = power_bounded(x,y, wts, crs)
    bnd=[min(x) max(x) min(y) max(y)]; %data bounds
    if nargin < 3
        crs=double([bnd(1) bnd(4);bnd(2) bnd(4);bnd(2) bnd(3);bnd(1) bnd(3);bnd(1) bnd(4)]);
    end

    rgx = max(crs(:,1))-min(crs(:,1));
    rgy = max(crs(:,2))-min(crs(:,2));
    rg = max(rgx,rgy);
    midx = (max(crs(:,1))+min(crs(:,1)))/2;
    midy = (max(crs(:,2))+min(crs(:,2)))/2;

    % add 4 additional edges
    xA = [x; midx + [0;0;-5*rg;+5*rg]];
    yA = [y; midy + [-5*rg;+5*rg;0;0]];

    if (all(wts == 0))
        [vi,ci] = voronoin([xA,yA]);
    else
        [vi,ci] = powerDiagram2([xA,yA], [wts;zeros(4,1)]);
    end

    % remove the last 4 cells
    C = ci(1:end-4);
    V = vi;
    % use Polybool to crop the cells
    %Polybool for restriction of polygons to domain.

    maxX = max(crs(:,1)); minX = min(crs(:,1));
    maxY = max(crs(:,2)); minY = min(crs(:,2));
    for ij=1:length(C)
        % thanks to http://www.mathworks.com/matlabcentral/fileexchange/34428-voronoilimit
        Cij = C{ij};
        if (length(Cij) == 0) continue; end;

        % first convert the contour coordinate to clockwise order:
        pts = V(Cij,:);
        K = convhull(pts);
        K = K(end-1:-1:1);
        C{ij} = Cij(K);
        X2 = pts(K,1);
        Y2 = pts(K,2);

        % if all points are inside the bounding box, then skip it
        if (all((X2 <= maxX) & (X2 >= minX) & (Y2 <= maxY) & (Y2 >= minY))) continue; end;

        [xb, yb] = clip_polygons(crs(:,1),crs(:,2),X2,Y2);
        % xb = xb'; yb = yb';
        ix=nan(1,length(xb));
        for il=1:length(xb)
            if any(V(:,1)==xb(il)) && any(V(:,2)==yb(il))
                ix1=find(V(:,1)==xb(il));
                ix2=find(V(:,2)==yb(il));
                for ib=1:length(ix1)
                    if any(ix1(ib)==ix2)
                        ix(il)=ix1(ib);
                    end
                end
                if isnan(ix(il))==1
                    lv=length(V);
                    V(lv+1,1)=xb(il);
                    V(lv+1,2)=yb(il);
                    ix(il)=lv+1;
                end
            else
                lv=length(V);
                V(lv+1,1)=xb(il);
                V(lv+1,2)=yb(il);
                ix(il)=lv+1;
            end
        end
        C{ij} = ix;
    end
end


function [Lrgb bgm] = watershed_seg(ip_wshed,ip_img)
% Watershed segmentation algorithm

se = strel('disk', 5); % disk = 20
%Opening
Io = imopen(ip_img, se);

%Opening-by-reconstruction (Iobr)
Ie = imerode(ip_img, se);
Iobr = imreconstruct(Ie, ip_img);

%Opening-closing (Ioc)
Ioc = imclose(Io, se);

%Opening-closing by reconstruction (Iobrcbr)
Iobrd = imdilate(Iobr, se);
Iobrcbr = imreconstruct(imcomplement(Iobrd), imcomplement(Iobr));
Iobrcbr = imcomplement(Iobrcbr);

%Regional maxima of opening-closing by reconstruction (fgm)
fgm = imregionalmax(Iobrcbr);

%Regional maxima superimposed on original image (I2)
I2 = ip_img;
I2(fgm) = 255;

se2 = strel(ones(3, 3)); % ones(5,5)
fgm2 = imclose(fgm, se2);
fgm3 = imerode(fgm2, se2);

fgm4 = bwareaopen(fgm3, 20); %(fgm3, 20)

%Modified regional maxima superimposed on original image (fgm4)
I3 = ip_img;
I3(fgm4) = 255;

%Thresholded opening-closing by reconstruction (bw)
bw = im2bw(Iobrcbr, graythresh(Iobrcbr));

%Watershed ridge lines (bgm)
D = bwdist(bw);
DL = watershed(D);
bgm = DL == 0;

gradmag2 = imimposemin(ip_wshed, bgm | fgm4);

L = watershed(gradmag2);

%Markers and object boundaries superimposed on original image (I4)
I4 = ip_img;
I4(imdilate(L == 0, ones(3, 3)) | bgm | fgm4) = 255;

Lrgb = label2rgb(L, 'jet', 'w', 'shuffle');


classdef CAMOEA < ALGORITHM
% <multi> <real/integer/label/binary/permutation>
% Clustering based adaptive multi-objective evolutionary algorithm

%------------------------------- Reference --------------------------------
% Y. Hua, Y. Jin, K. Hao, A clustering-based adaptive evolutionary
% algorithm for multiobjective optimization with irregular Pareto fronts,
% IEEE Transactions on Cybernetics, 2019, 49(7): 2758-2770.
%------------------------------- 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 Yicun Hua

    methods
        function main(Algorithm,Problem)
            %% Generate random population
            Population = Problem.Initialization();

            %% Optimization
            while Algorithm.NotTerminated(Population)
                % Generate offspring randomly
                MatingPool = randperm(Problem.N);
                Offspring  = OperatorGA(Problem,Population(MatingPool));

                % Elitism strategy
                UniPop = [Population,Offspring];
                PopObj = UniPop.objs;
                [FrontNo,MaxFNo] = NDSort(PopObj,Problem.N);

                % The number of individuals to be selected in the last
                % non-dominated front
                K = Problem.N - sum(FrontNo<MaxFNo);

                if K ~= 0
                    % Normalization
                    pareto_population = find(FrontNo<MaxFNo);
                    last_population   = find(FrontNo == MaxFNo);
                    Zmin = min(PopObj(FrontNo == MaxFNo,:));
                    Zmax = max(PopObj(FrontNo == MaxFNo,:));
                    S = sum(FrontNo == MaxFNo);
                    MaxFnorm = (PopObj(last_population,:)-repmat(Zmin,S,1))./repmat(Zmax-Zmin,S,1);

                    % Clustering-based reference points generation
                    [Ref] = Reference_Generation( MaxFnorm,Problem.M,K);

                    % Clustering-based environmental selection
                    [reference_population] = Reference_Point_Selection(MaxFnorm,last_population,Ref,K,Problem.M);
                else
                    pareto_population    = find(FrontNo<=MaxFNo);
                    reference_population = [];
                end
                Population = UniPop([pareto_population,reference_population]);
            end
        end
    end
end


function [registered]=nonrigidICP(target,source,Ft,Fs,iterations)

% INPUT
% -target: vertices of target mesh; n*3 array of xyz coordinates
% -source: vertices of source mesh; n*3 array of xyz coordinates
% -Ft: faces of target mesh; n*3 array
% -Fs: faces of source mesh; n*3 array
% -iterations: number of iterations; usually > 100
%
% OUTPUT
% -registered: registered source vertices on target mesh. Faces are not affected and remain the same is before the registration (Fs).

%EXAMPLE

% load EXAMPLE
% [registered]=nonrigidICP(target,source,Ft,Fs,200);

tic
clf
%initial allignment and scaling
[error1,source,transform]=rigidICP(target,source,0);

%plot of the meshes
h=trisurf(Fs,source(:,1),source(:,2),source(:,3),0.3,'Edgecolor','none');
hold
light
lighting phong;
set(gca, 'visible', 'off')
set(gcf,'Color',[1 1 0.88])
view(90,90)
set(gca,'DataAspectRatio',[1 1 1],'PlotBoxAspectRatio',[1 1 1]);
trisurf(Ft,target(:,1),target(:,2),target(:,3),0.75,'Edgecolor','none');
alpha(0.6)

for i =1:iterations
    % find index of max deviating point
    [distancemax,I,error,Reallignedsource]=ICPmanu_allign2(target,source);
     distancemax=distancemax
     % define measure for decreasing surface area to be transformed
     areafactor=0.5+24*i/iterations;

    for j=1:4
        areafactor=areafactor*j;
        r=areafactor/25;

        if r>1
            r=1;
        end
        %decrease sample size for large surfaces

        [nfs,nvs] = reducepatch(Fs,source,r);
        [nft,nvt] = reducepatch(Ft,target,r);
        %define index of max deviating point on reduced surface
        Inieuw=knnsearch(nvs,source(I,:));
        % define order of neighbouring vertices
        [distancemap]=surfacemap(nvs,nfs,Inieuw);
        d2=distancemap(:,3);
        %use distance to define specific surface area of interest
        d2=d2.^areafactor;
        [error,Reallignedsource,transform]=ICPmanu2weigthed(nvt,nvs,d2);
        [distancemap]=surfacemap(source,Fs,I);
        d2=distancemap(:,3);
        d2=d2.^areafactor;
        correction=1-d2;
        Reallignedsource=transform.b*source*transform.T+repmat(transform.c(1,1:3),length(source(:,1)),1);
        source=horzcat(source(:,1).*correction,source(:,2).*correction,source(:,3).*correction)+horzcat(Reallignedsource(:,1).*d2,Reallignedsource(:,2).*d2,Reallignedsource(:,3).*d2);
        [error,source,transform]=rigidICP(target,source,1);
            delete(h)
            h=trisurf(Fs,source(:,1),source(:,2),source(:,3),d2,'Edgecolor','none');
            pause (0.1)
    end
end
registered=source;
toc


function p = quantize(p,R,minV,maxV,minS,maxS)
%
% p = quantize(p,R,type) -- "quantize" elements of KDE p to R bits
%

%p.centers   = round(1000*p.centers)/1000;
%p.means     = round(1000*p.means)/1000;
%p.ranges    = round(1000*p.ranges)/1000;
%p.bandwidth = ceil(10000*p.bandwidth)/10000;

p.centers   = roundVals(p.centers  ,minV,maxV,R);
p.means     = roundVals(p.means    ,minV,maxV,R);
p.ranges    = roundVals(p.ranges   ,minV,maxV,R);
p.bandwidth = roundVals(p.bandwidth,minS,maxS,R);

function x = roundVals(x,minV,maxV,R)
 scale = 2^R ./ repmat(maxV-minV,size(x)./size(maxV));
 minV = repmat(minV,size(x)./size(minV)); x = x - minV; x = max(x,0);
 x = x .* scale; x = min(round(x),2^R-1); x = x./scale; x = x + minV;


classdef OptimizerNullSpace < Optimizer

    properties (GetAccess = public, SetAccess = protected)
        type = 'NullSpace';
    end

    properties (Access = private)
        tau
        lineSearchTrials
        lineSearch
        costOld
        upperBound
        lowerBound
        tol = 1e-8
        nX
        hasConverged
        acceptableStep
        oldDesignVariable
        oldCost
        incrementalScheme
        hasFinished
        mOld
        meritNew
        nConstr
        meritGradient

        globalCost
        globalConstraint
        globalCostGradient
        globalMerit
        globalLineSearch
        globalDual
        globalDesignVar
    end

    methods (Access = public)

        function obj = OptimizerNullSpace(cParams)
            obj.initOptimizer(cParams);
            obj.init(cParams);
            obj.outputFunction.monitoring.create(cParams);
            obj.createPrimalUpdater(cParams);
            obj.createDualUpdater(cParams);
            obj.prepareFirstIter();
        end

        function obj = solveProblem(obj)
            obj.hasConverged = false;
            obj.cost.computeFunctionAndGradient();
            obj.constraint.computeFunctionAndGradient();
            obj.hasFinished = false;
            obj.printOptimizerVariable();
            while ~obj.hasFinished
                obj.update();
                obj.updateIterInfo();
                obj.updateMonitoring();
                obj.checkConvergence();
                obj.printOptimizerVariable();
            end
        end

    end

    methods(Access = private)

        function init(obj,cParams)
            obj.upperBound             = cParams.uncOptimizerSettings.ub;
            obj.lowerBound             = cParams.uncOptimizerSettings.lb;
            obj.cost                   = cParams.cost;
            obj.constraint             = cParams.constraint;
            obj.designVariable         = cParams.designVar;
            obj.dualVariable           = cParams.dualVariable;
            obj.incrementalScheme      = cParams.incrementalScheme;
            obj.nConstr                = cParams.constraint.nSF;
            obj.nX                     = length(obj.designVariable.value);
            obj.maxIter                = cParams.maxIter;
            obj.hasConverged           = false;
            obj.nIter                  = 0;
        end

        function prepareFirstIter(obj)
            obj.cost.computeFunctionAndGradient();
            obj.costOld = obj.cost.value;
            obj.designVariable.updateOld();
            obj.dualVariable.value = zeros(obj.nConstr,1);
        end

        function obj = update(obj)
            x0 = obj.designVariable.value;
            obj.saveOldValues(x0);
            if obj.nIter ~= 0
                obj.dualUpdater.update();
            end
            obj.mOld = obj.computeMeritFunction(x0);
            obj.calculateInitialStep();
            obj.acceptableStep   = false;
            obj.lineSearchTrials = 0;
            DJ = obj.cost.gradient;
            Dg = obj.constraint.gradient;
            while ~obj.acceptableStep
                obj.computeMeritGradient(DJ,Dg);
                x = obj.updatePrimal();
                obj.designVariable.update(x);
                obj.dualUpdater.update();
                obj.checkStep(x,x0);
            end
            obj.updateOldValues(x);
        end

        function obj = calculateInitialStep(obj)
            if obj.nIter == 0
                obj.cost.computeFunctionAndGradient();
                obj.constraint.computeFunctionAndGradient();
                x       = obj.designVariable.value;
                l       = obj.dualVariable.value;
                DJ      = obj.cost.gradient;
                Dg      = obj.constraint.gradient;
                aJ      = 1;
                DmF     = aJ*(DJ + Dg*l);
                factor  = 1;
                obj.primalUpdater.computeFirstStepLength(DmF,x,factor);
            else
                factor = 1.2;
                obj.primalUpdater.increaseStepLength(factor);
            end
        end

        function displayIter(obj,x)
            m = obj.designVariable.mesh;
            bm = m.createBoundaryMesh();
            s.backgroundMesh = m;
            s.boundaryMesh   = bm;
            um = UnfittedMesh(s);
            um.compute(x);
            figure()
            um.plot();
        end

        function x = updatePrimal(obj)
            x       = obj.designVariable.value;
            g       = obj.meritGradient;
            x       = obj.primalUpdater.update(g,x);
        end

        function computeMeritGradient(obj,DJ,Dg)
            l       = obj.dualVariable.value;
            aJ      = 1;
            DmF     = aJ*(DJ + Dg*l);
            obj.meritGradient = DmF;
        end

        function checkStep(obj,x,x0)
            mNew = obj.computeMeritFunction(x);
            if obj.nIter == 0 %&& mNew <= obj.mOld
                obj.acceptableStep = true;
                obj.meritNew       = mNew;
                obj.dualUpdater.updateOld();
            end
            if mNew < obj.mOld
                obj.acceptableStep = true;
                obj.meritNew = mNew;
                obj.dualUpdater.updateOld();
            elseif obj.primalUpdater.isTooSmall()
                error('Convergence could not be achieved (step length too small)')
            else
                obj.primalUpdater.decreaseStepLength();
                obj.designVariable.update(x0);
                obj.lineSearchTrials = obj.lineSearchTrials + 1;
            end
        end

        function mF = computeMeritFunction(obj,x)
            obj.designVariable.update(x)
            obj.cost.computeFunctionAndGradient();
            obj.constraint.computeFunctionAndGradient();
            J  = obj.cost.value;
            h  = obj.constraint.value;
            l  = obj.dualVariable.value;
            aJ = 1;
            AJ = aJ*(J + l'*h);
            mF = AJ;
        end

        function obj = saveOldValues(obj,x)
            obj.designVariable.update(x);
            obj.cost.computeFunctionAndGradient();
            obj.constraint.computeFunctionAndGradient();
            obj.oldCost            = obj.cost.value;
            obj.oldDesignVariable  = x;
        end

        function obj = updateOldValues(obj,x)
            obj.designVariable.update(x);
            obj.cost.computeFunctionAndGradient();
            obj.constraint.computeFunctionAndGradient();
        end

        function obj = checkConvergence(obj)
           if abs(obj.meritNew - obj.mOld) < obj.tol && obj.checkConstraint()
               obj.hasConverged = true;
           else

           end

        end

        function obj = updateMonitoring(obj)
            s.nIter            = obj.nIter;
            s.tau              = obj.primalUpdater.tau;
            s.lineSearch       = obj.lineSearch;
            s.lineSearchTrials = obj.lineSearchTrials;
            s.oldCost          = obj.oldCost;
            s.hasFinished      = obj.hasFinished;
            s.meritNew         = obj.meritNew;
            obj.outputFunction.monitoring.compute(s);
        end

        function updateIterInfo(obj)
            obj.increaseIter();
            obj.updateStatus();
        end

        function increaseIter(obj)
            obj.nIter = obj.nIter + 1;
        end

        function updateStatus(obj)
            obj.hasFinished = obj.hasConverged || obj.hasExceededStepIterations();
        end

        function itHas = hasExceededStepIterations(obj)
            iStep = obj.incrementalScheme.iStep;
            nStep = obj.incrementalScheme.nSteps;
            itHas = obj.nIter >= obj.maxIter*(iStep/nStep);
        end

        function saveVariablesForAnalysis(obj)
            i                           = obj.nIter + 1;
            obj.globalCost(i)           = obj.cost.value;
            obj.globalConstraint(:,i)   = obj.constraint.value;
            obj.globalCostGradient(i)   = norm(obj.cost.gradient);
%             obj.globalMerit(i)          = obj.meritNew;
%             obj.globalLineSearch(i)     = obj.primalUpdater.tau;
            obj.globalDual(:,i)         = obj.dualVariable.value;
            obj.globalDesignVar(:,i)    = obj.designVariable.value;
            if obj.hasConverged
                c = obj.globalCost;
                h = obj.globalConstraint;
                g = obj.globalCostGradient;
%                 m = obj.globalMerit;
%                 t = obj.globalLineSearch;
                d = obj.globalDual;
                v = obj.globalDesignVar;
                save('NullSpaceCant04.mat',"c","g","h","d","v");
            end
        end

    end

end


% Copyright 2017 Lime Microsystems Ltd.
%
% Licensed under the Apache License, Version 2.0 (the "License");
% you may not use this file except in compliance with the License.
% You may obtain a copy of the License at
%
%    http://www.apache.org/licenses/LICENSE-2.0
%
% Unless required by applicable law or agreed to in writing, software
% distributed under the License is distributed on an "AS IS" BASIS,
% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
% See the License for the specific language governing permissions and
% limitations under the License.
%
% just for fun now, uses compressed ASCII (6 bit) and BCH(15,11) FEC
% Note real encoding uses Viterbi over several radio frames!  Viterbi is more efficient than block coding.
% also have not muxed DPCCH into DPDCH
function [y,din,dout]=WCDMADLtxtMsgWrite2( txt, bitsPerFrame )
	m=4;
	n=2^m-1; % also from bchpoly (1)
	k=11; % from bchpoly (2) % 21 maybe better than 16, as gives 14 chars instead of 10, with t=2 bit error correct
	t=1; % from bchpoly (3)
	maxMsg=floor(k*floor(bitsPerFrame/n)/6);
	qpsk=[1+i,1-i,-1+i,-1-i];
	% if message too short, add ' 's
	if length(txt)<maxMsg
		chars=maxMsg-length(txt);
		tmpTxt=[txt,repmat(32,1,chars)];
	% if message too long, cut
	elseif length(txt)>maxMsg
		tmpTxt=txt(1:maxMsg);
	else % length(txt)==maxMsg
		tmpTxt=txt;
	end
	pad1=randint(1,(k*ceil(maxMsg*6/k)-maxMsg*6)); % unused bits prior to encoding
	pad2=randint(1,(bitsPerFrame-n*floor(bitsPerFrame/n))); % unused part of encoded message, pad with random numbers 26
	printf('WCDMAtxtMsgULWrite[%s],BCH(%g,%g)\n', tmpTxt, n, k );
	din=[reshape(de2bi(ASCII6enc(tmpTxt),6,2,"left-msb")',1,[])];
  data=[din,pad1]; % pad to fit FEC
	% convert to ?xk matrix prior to encoding
	data=reshape(data,length(data)/k,k);
	data=bchenco(data,n,k);
	% convert back to linear matrix and pad to bits per frame
	l=size(data);
	data=reshape(data,1,n*l(1));
	idx=[data,pad2]; % pad unused bits with random data
	% interleave to reduce effect of burst errors
%	idx=reshape(reshape(idx,15,40)',1,600); % was 15 10 150
  dout=matintrlv(idx,15,40);
	idx=sum( ( (reshape(dout,2,(length(dout)/2))').*[2,1])' )+1;
	y=qpsk(idx); % convert to qpsk
end


function [stat] = doMAPForward(B, S, param, stat)

if isempty(B)
    fprintf('Empty proposal set.\n');
    stat = [];
    return;
end
nB = size(B,2);
if nargin == 3 || isempty(stat)
    % initialization
    stat.W = [];
    stat.Xp = []; % optimal w_{ij} given the output set
    stat.X = zeros(nB,1); % assignment
    % construct W
    [stat.W, stat.Xp] = getW(B, S, param);
    stat.BGp = stat.Xp;
    stat.nms = zeros(size(B,2),1);
    stat.f = sum(stat.Xp);
    stat.O = [];
end

%% loop
while numel(stat.O) < min(param.maxnum, nB)
    V = max(stat.W - repmat(stat.Xp, [1 nB]),0);
    [score, vote] = max(sum(V) + stat.nms(:)');
    if score == 0 % no supporters
        break
    end
    tmpf = stat.f + score + param.phi;

    if (tmpf > stat.f)
        mask = V(:,vote) > 0;
        stat.X(mask) = vote;
        stat.O(end + 1) = vote;
        stat.Xp(mask) = stat.W(mask,vote);
        stat.f = tmpf;
        stat.nms = stat.nms + ...
            param.gamma*getNMSPenalty(B, B(:,vote));
    else
        break
    end
end


function fr = comp_insdgfb(c,g,shift,Ls,dual)
%COMP_INSDGFB  Non-stationary Gabor filterbank synthesis
%   Usage: fr = comp_insdgfb(c,g,shift,Ls,dual)
%          fr = comp_insdgfb(c,g,shift,Ls)
%          fr = comp_insdgfb(c,g,shift)
%
%   Input parameters:
%         c         : Transform coefficients (matrix or cell array)
%         g         : Cell array of Fourier transforms of the analysis
%                     windows
%         shift     : Vector of frequency shifts
%         Ls        : Original signal length (in samples)
%         dual      : Synthesize with the dual frame
%   Output parameters:
%         fr        : Synthesized signal (Channels are stored in the
%                     columns)
%
%   Given the cell array *c* of non-stationary Gabor coefficients, and a
%   set of filters *g* and frequency shifts *shift* this function computes
%   the corresponding non-stationary Gabor filterbank synthesis.
%
%   If *dual* is set to 1 (default), an attempt is made to compute the
%   canonical dual frame for the system given by *g*, *shift* and the size
%   of the vectors in *c*. This provides perfect reconstruction in the
%   painless case, see the references for more information.
%
%   See also:  cqt, icqt, erblett, ierblett
%
%   References:  ltfatnote018 dogrhove12

% Author: Nicki Holighaus
% Date: 10.04.13

%% Check input arguments
if nargin < 5
    dual = 1;
    if nargin < 3
        error('Not enough input arguments');
    end
end

if iscell(c) == 0 % If matrix format coefficients were used, convert to
    % cell
    [M,N,CH] = size(c);
    c = reshape(c,N*M,CH);
    c = mat2cell(c,M*ones(N,1),CH);
else
    N = length(c);
    CH = size(c{1},2);
    M = cellfun(@(x) size(x,1),c);
end

timepos = cumsum(shift);        % Calculate positions from shift vector
NN = timepos(end);              % Reconstruction length before truncation
timepos = timepos-shift(1);     % Adjust positions

fr = zeros(NN,CH,assert_classname(c{1},g{1})); % Initialize output

if nargin < 4
    Ls = NN; % If original signal length is not given do not truncate
end

if dual == 1 % Attempt to compute canonical dual frame
    g = nsgabdual(g,shift,M,Ls);
end

%% The overlap-add procedure including multiplication with the synthesis
% windows

if numel(M) == 1
    M = M*ones(N,1);
end

for ii = 1:N
    Lg = length(g{ii});

    win_range = mod(timepos(ii)+(-floor(Lg/2):ceil(Lg/2)-1),NN)+1;

    temp = fft(c{ii})*M(ii);
    temp = temp(mod([end-floor(Lg/2)+1:end,1:ceil(Lg/2)]-1,M(ii))+1,:);

    fr(win_range,:) = fr(win_range,:) + ...
        bsxfun(@times,temp,g{ii}([Lg-floor(Lg/2)+1:Lg,1:ceil(Lg/2)]));
end

fr = ifft(fr);
fr = fr(1:Ls,:); % Truncate the signal to original length (if given)


function Offspring = Operator(Problem,Particle,Pbest,Gbest,Parameter)
% Particle swarm optimization in MPSO/D
% c1   ---   2 --- Parameter in updating particle's velocity
% c2   ---   2 --- Parameter in updating particle's velocity
% CR   --- 0.5 --- Parameter CR in differental evolution
% F    --- 0.5 --- Parameter F in differental evolution
% proM ---   1 --- The expectation of number of bits doing mutation
% disM ---  20 --- The distribution index of polynomial mutation

%------------------------------- 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".
%--------------------------------------------------------------------------

    %% Parameter setting
    if nargin > 4
        [c1,c2,CR,F,proM,disM] = deal(Parameter{:});
    else
        [c1,c2,CR,F,proM,disM] = deal(2,2,0.5,0.5,1,20);
    end
    ParticleDec = Particle.decs;
    PbestDec    = Pbest.decs;
    GbestDec    = Gbest.decs;
    [N,D]       = size(ParticleDec);
    ParticleVel = Particle.adds(zeros(N,D));

    %% Particle swarm optimization
    Lower = repmat(Problem.lower,N,1);
    Upper = repmat(Problem.upper,N,1);
    DoPSO = repmat(rand(N,1)<0.5,1,D);
    W     = 0.9 - Problem.FE./Problem.maxFE*0.8;
    r1    = repmat(rand(N,1),1,D);
    r2    = repmat(rand(N,1),1,D);
    OffVel        = ParticleVel;
    OffDec        = ParticleDec;
    OffVel(DoPSO) = W.*ParticleVel(DoPSO) + c1.*r1(DoPSO).*(PbestDec(DoPSO)-ParticleDec(DoPSO)) + c2.*r2(DoPSO).*(GbestDec(DoPSO)-ParticleDec(DoPSO));
    OffDec(DoPSO) = ParticleDec(DoPSO) + OffVel(DoPSO);
    % Set the infeasible decision variables to the value of their parents
    Invalid         = OffDec < Lower | OffDec > Upper;
    OffDec(Invalid) = ParticleDec(Invalid);

    %% DE
    Site = ~DoPSO & rand(N,D)<CR;
    OffDec(Site) = ParticleDec(Site) + F.*(GbestDec(Site)-PbestDec(Site));
    % Set the infeasible decision variables to boundary values
    OffDec = max(min(OffDec,Upper),Lower);

    %% Polynomial mutation
    Site  = rand(N,D) < proM/D;
    mu    = rand(N,D);
    temp  = Site & mu<=0.5;
    OffDec(temp) = OffDec(temp)+(Upper(temp)-Lower(temp)).*((2.*mu(temp)+(1-2.*mu(temp)).*...
                   (1-(OffDec(temp)-Lower(temp))./(Upper(temp)-Lower(temp))).^(disM+1)).^(1/(disM+1))-1);
    temp = Site & mu>0.5;
    OffDec(temp) = OffDec(temp)+(Upper(temp)-Lower(temp)).*(1-(2.*(1-mu(temp))+2.*(mu(temp)-0.5).*...
                   (1-(Upper(temp)-OffDec(temp))./(Upper(temp)-Lower(temp))).^(disM+1)).^(1/(disM+1)));
    Offspring = Problem.Evaluation(OffDec,OffVel);
end


function [Set_PSM, Set_TDS, OperatingTime] = ORCD (CT, OLF_Min, OLF_Max, LC, FC, C1, C2, TOP_Desired, IDMT_Saturation, Tolerance_Dn, Discrimination_Time, MinPSM, MaxPSM, StepPSM, MinTDS, MaxTDS, StepTDS)

[PSM_Max, PSM_Min] = PSM(CT, OLF_Max, OLF_Min, LC, MaxPSM, MinPSM, StepPSM);

TOP_Desired_For_Present_Relay=TOP_Desired+Discrimination_Time;
n=round((PSM_Max-PSM_Min)/StepPSM);
PSM_Temp=PSM_Min;
D=zeros(1,4);
    for i=1:n+1
        M=(FC/(PSM_Temp*CT));
        if (M>=IDMT_Saturation)
            M=IDMT_Saturation;
        end

        K=C1/((M^C2)-1);

        TDS_Desired=TOP_Desired_For_Present_Relay/K;

        TDS_Rounded_Up=ceil(TDS_Desired/StepTDS)*StepTDS;
        Difference_Up=abs(TDS_Rounded_Up-TDS_Desired);

        TDS_Rounded_Dn=floor(TDS_Desired/StepTDS)*StepTDS;
        Difference_Dn=abs(TDS_Rounded_Dn-TDS_Desired);

        Difference=min(Difference_Up,Difference_Dn);

        D(i,1)=PSM_Temp;
        D(i,2)=K;
        D(i,3)=TDS_Desired;
        D(i,4)=Difference;

        PSM_Temp=PSM_Temp+StepPSM;
        if (i~=n+1)
            D=[D;zeros(1,4)];
        end
    end
    D;
    [~,I]=min(D(:,4));
    Set_PSM=D(I,1);
    Set_TDS_Up=ceil(D(I,3)/StepTDS)*StepTDS;
    Set_TDS_Dn=floor(D(I,3)/StepTDS)*StepTDS;
    OperatingTime_Up=D(I,2)*Set_TDS_Up;
    OperatingTime_Dn=D(I,2)*Set_TDS_Dn;
    TOP_Error_Up=abs(TOP_Desired_For_Present_Relay-OperatingTime_Up);
    TOP_Error_Dn=abs(TOP_Desired_For_Present_Relay-OperatingTime_Dn);
    TOP_Error=min(TOP_Error_Up,TOP_Error_Dn);
    if (TOP_Error==TOP_Error_Dn && TOP_Error<=Tolerance_Dn)
        OperatingTime=OperatingTime_Dn;
        Set_TDS=Set_TDS_Dn;
    else
        OperatingTime=OperatingTime_Up;
        Set_TDS=Set_TDS_Up;
    end


tic

hfig = figure;
InitializeAppData(hfig);
ResetDisplayParams(hfig);
i_fish = 8;
[cIX_0,gIX_0,M] = LoadSingleFishDefault(i_fish,hfig,[2,1]);

% [cIX_0,gIX_0] = LoadCluster_Direct(i_fish,2,1);

cIX = cIX_0;
gIX = gIX_0;

%% PCA
[coeff,score,latent,tsquared,explained,mu] = pca(M);
% figure;subplot(121);imagesc(coeff);subplot(122);imagesc(score);title('M');

% data to pass on from PCA:
k = 100;
Z0 = score(:,1:k);
% figure;imagesc(Z0);
t1 = toc

%% ICA
tic
numK = 139;%20;
Z = Z0';
[Zica, W, T, mu] = fastICA(Z,numK);

t2 = toc
%%
n = size(M',2);
Zr = T \ W' * Zica + repmat(mu,1,n);

figure;
subplot(131)
imagesc(Zica);
subplot(132)
imagesc(W);
subplot(133)
imagesc(Zr);

%% clustering
gIX = kmeans(Zr',numK,'distance','correlation');

UpdateIndices_Manual(hfig,cIX,gIX);

figure('Position',[50,100,1400,800]);
% isCentroid,isPlotLines,isPlotBehavior,isPlotRegWithTS
subplot(121)
setappdata(hfig,'isPlotBehavior',1);
setappdata(hfig,'isStimAvr',0);
setappdata(hfig,'isPlotLines',0);
UpdateTimeIndex(hfig);
DrawTimeSeries(hfig,cIX,gIX);

% right plot
subplot(122)
I = LoadCurrentFishForAnatPlot(hfig,cIX,gIX);
DrawCellsOnAnat(I);

% kmeansPlot(Zr,gIX)
%% bad? pick top 100 cells for each component
% gIX = zeros(size(gIX));
% for i = 1:numK
%     [B,IX] = sort(Zica(i,:),'descend');
%     IX_keep = IX(1:100); % this is too crude ~~~~~~
%
%     gIX(IX_keep) = i;
% end
% IX = (gIX~=0);
% cIX = cIX(IX);
% gIX = gIX(IX);

%% pick top ? cells for each component % still crude, no
cutoff_std = 3;

Z2 = Zica;Z2(Z2<cutoff_std)=nan;
% manual code to not assign values to NaN columns (facepalm)
[~,gIX_1] = max(vertcat(nan(1,size(Z2,2)),Z2));
% gIX_1 = gIX_1';
gIX = gIX_1-1;
IX = (gIX~=0);
cIX = cIX_0(IX);
gIX = gIX_1(IX);

%%
% [cIX,gIX] = SelectClusterRange(cIX,gIX,[3,4]);

UpdateIndices_Manual(hfig,cIX,gIX);
%%
figure('Position',[50,100,1400,800]);
% isCentroid,isPlotLines,isPlotBehavior,isPlotRegWithTS
subplot(121)
setappdata(hfig,'isPlotBehavior',1);
setappdata(hfig,'isStimAvr',0);
setappdata(hfig,'isPlotLines',0);
UpdateTimeIndex(hfig);
DrawTimeSeries(hfig,cIX,gIX);

% right plot
subplot(122)
I = LoadCurrentFishForAnatPlot(hfig,cIX,gIX);
DrawCellsOnAnat(I);


function title = p21_title ( )

%*****************************************************************************80
%
%% P21_TITLE returns a title for problem 21.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 March 2000
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, string TITLE, a title for the problem.
%
  title = 'The Hilbert Matrix Function F = x''Ax';

  return
end


% construct the Target Active Feature (TAF) network
% This net exploits the ridge loss and the hinge loss to calculate target
% active features.

% Input:
%  filter_sz    -   filter size [height width depth], target size in the
%                   input feature map
% Output:
%  IAF_net      -   the initialized IAF network

% By Xin Li, April 8 2018

function [TAF_net,filter_sz] = TAF_net_init(filter_sz)
rng('default');

channel=filter_sz(3);

rw=ceil(filter_sz(2)/2);
rh=ceil(filter_sz(1)/2);

fw=2*rw+1;
fh=2*rh+1;

filter_sz = [fh,fw,channel];

TAF_net=dagnn.DagNN();

%% conv layer 1-1 for regression
TAF_net.addLayer('conv11', dagnn.Conv('size', [fw,fh,channel,1],...
    'hasBias', true, 'pad',...
    [rh,rh,rw,rw], 'stride', [1,1]), 'input', 'conv11', {'conv11_f', 'conv11_b'});

f = TAF_net.getParamIndex('conv11_f') ;
TAF_net.params(f).value=single(randn(fh,fw,channel,1) /...
    sqrt(rh*rw*channel))/1e8;
TAF_net.params(f).learningRate=1;
TAF_net.params(f).weightDecay=1e3;

f = TAF_net.getParamIndex('conv11_b') ;
TAF_net.params(f).value=single(zeros(1,1));
TAF_net.params(f).learningRate=2;
TAF_net.params(f).weightDecay=1e3;

%%
TAF_net.addLayer('L2Loss',...
    RegressionL2Loss(),{'conv11','label_gaussian'},'objective_r');

% for the scale sensitive features, we first select the discriminative
% features with the regression loss then we select the scale sensitive from
% them with the ranking loss. The ranking loss is constructed in the file
% TAF_model.m

end


function pass = test_sum3(pref)
% Test file for @chebfun3t/sum3.

% Obtain preferences.
if ( nargin == 0 )
    pref = chebfunpref();
end
tol = 1e1 * pref.cheb3Prefs.chebfun3eps;

% Check empty case
pass(1) = isempty(sum3(chebfun3t()));

% Check constant
f = chebfun3t(@(x,y,z) 1);
pass(2) = abs(sum3(f) - 8) < tol;

% Runge function
f = chebfun3t(@(x,y,z) 1./(1+x.^2+y.^2+z.^2));
pass(3) = abs(sum3(f) - 4.28685406230184188268) < tol;

% Different domains
f = chebfun3t(@(x,y,z) x, [0, 1, 0, 2, 0, 3]);
pass(4) = abs(sum3(f) - 3) < tol;

f = chebfun3t(@(x,y,z) y, [0, 1, 0, 2, 0, 3]);
pass(5) = abs(sum3(f) - 6) < tol;

f = chebfun3t(@(x,y,z) z, [0, 1, 0, 2, 0, 3]);
pass(6) = abs(sum3(f) - 9) < tol;

end


filename='test2d_micro';
%filename='RVE_Square_Triangle_FineFine';
%filename = 'MicroQuad';
ptype = 'MICRO';
method = 'SIMPALL';
%method = 'SIMP_P3';
materialType = 'ISOTROPIC';
initial_case = 'circleInclusion';
cost={'chomog_alphabeta'};
weights=[1];
constraint = {'volumeConstraint'};
constraint_case = 'EQUALITY';
%incrementFactor = 1;
designVariable = 'Density';
%designVariable = 'LevelSet';
filterType = 'P1';
fracRadius = 0.4;
%optimizer = 'IPOPT';
optimizer = 'DualNestedInPrimal';
%optimizer = 'AlternatingPrimalDual';

optimizerUnconstrained = 'PROJECTED GRADIENT';
line_search_initiator = 'INCREASING LAST STEP';
incrementFactor = 2.95;

%optimizerUnconstrained = 'SLERP';

nsteps = 1;
Vfrac_final = 0.25;
Perimeter_target=1;
optimality_final = 0.2*1e-3;
constr_final =1e-12;

Vfrac_initial = 0.8;
optimality_initial = 0.2*1e-3;
constr_initial = 1e-12;

TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;

%Micro
epsilon_isotropy_initial=1e-1;
epsilon_isotropy_final = 1e-3;
micro.alpha =[1 1 0]';
micro.beta =[1 1 0]';

%nsteps = 10;

% For all tests
plotting = true;
printing = true;
monitoring = true;
monitoring_interval = 1;
maxiter = 2000;


function DEM_demo_factor_analysis
% Demo for Probabilistic Factor Analysis; This uses a hierarchical model
% under the constraint that the causes have a deterministic and stochastic
% components.  The aim is to recover the true subspace of the real causes.

rng('default')

% non-hierarchical linear generative model (static)
%==========================================================================
n     = 8;
m     = 2;
M     = spm_DEM_M('FA',[n m]);

% create data
%==========================================================================
N     = 8;                                        % length of data sequence
X     = randn(size(M(1).pE));
DEM   = spm_DEM_generate(M,N,{X},{4});

% Initialise parameters
%--------------------------------------------------------------------------
DEM.class = 'FA';
DEM   = spm_dem_initialise(DEM);

% DEM estimation
%==========================================================================
DEM.M(1).E.nE = 16;
DEM   = spm_DEM(DEM);

% compare real and estimated factor and causes
%==========================================================================

% plot
%--------------------------------------------------------------------------
subplot(2,2,2)
v     = DEM.qU.v{2};
u     = DEM.pU.v{2};
plot(v'*pinv(full(v'))*u')
hold on
plot(u',':')
title({'real and rotated causes','Factor analysis'},'FontSize',16)
axis square
grid on


function juldaystr = datenum2julday(dnum)
[yyyy, mm, dd, hh, mi, ss] = datevec(dnum);
dnum_jan1 = datenum(yyyy, 1, 1);
dnum_diff = dnum - dnum_jan1;
julday = ceil(dnum_diff + eps); % eps turns datenum(1997,1,1) into day 1 rather than day 0
juldaystr = sprintf('%4d%03d', yyyy, julday);