% VL_DEMO_COVDET Demo: VL_COVDET()

% --------------------------------------------------------------------
%                                                               Basics
% --------------------------------------------------------------------

im = vl_impattern('roofs1') ;
im = im(end-128:end,128:320,:) ;

figure(1) ; clf ;
image(im) ; axis image off ;
vl_demo_print('covdet_basic_image') ;

imgs = im2single(rgb2gray(im)) ;
frames = vl_covdet(imgs, 'verbose') ;

hold on ;
vl_plotframe(frames) ;
vl_demo_print('covdet_basic_frames') ;

% --------------------------------------------------------------------
%                                                    Affine adaptation
% --------------------------------------------------------------------

frames = vl_covdet(imgs, 'estimateAffineShape', true, 'verbose') ;

figure(2) ; clf ;
image(im) ; axis image off ; hold on ;
vl_plotframe(frames) ;
vl_demo_print('covdet_affine_frames',.8) ;

% --------------------------------------------------------------------
%                                              Estimating orientations
% --------------------------------------------------------------------

frames = vl_covdet(imgs, 'estimateOrientation', true, 'verbose') ;

figure(3) ; clf ;
image(im) ; axis image off ; hold on ;
vl_plotframe(frames) ;
vl_demo_print('covdet_oriented_frames',.8) ;

% --------------------------------------------------------------------
%                                                   Extracting patches
% --------------------------------------------------------------------

[frames, patches] = vl_covdet(imgs, 'descriptor', 'patch') ;

figure(4) ; clf ;
w = sqrt(size(patches,1)) ;
vl_imarraysc(reshape(patches(:,1:10*10), w,w,[])) ;
axis image off ; hold on ; colormap gray ;
vl_demo_print('covdet_patches') ;

[frames, patches] = vl_covdet(imgs, ...
                              'descriptor', 'patch' ,...
                              'estimateAffineShape', true, ...
                              'estimateOrientation', false) ;

figure(5) ; clf ;
w = sqrt(size(patches,1)) ;
vl_imarraysc(reshape(patches(:,1:10*10), w,w,[])) ;
axis image off ; hold on ; colormap gray ;
vl_demo_print('covdet_affine_patches') ;

% --------------------------------------------------------------------
%                                                  Different detectors
% --------------------------------------------------------------------

names = {'DoG', 'Hessian', ...
         'HarrisLaplace', 'HessianLaplace', ...
         'MultiscaleHarris', 'MultiscaleHessian'} ;
figure(6) ; clf ;
for i = 1:numel(names)
  frames = vl_covdet(imgs, 'method', names{i}) ;

  vl_tightsubplot(3,2,i, 'margintop',0.025, 'marginright', 0.01) ;
  imagesc(im) ; axis image off ;
  hold on ;
  vl_plotframe(frames) ;
  title(names{i}) ;
end

vl_figaspect(3/4) ;
vl_demo_print('covdet_detectors',.9) ;

% --------------------------------------------------------------------
%                                                        Custom frames
% --------------------------------------------------------------------

delta = 15 ;
xr = delta:delta:size(im,2)-delta+1 ;
yr = delta:delta:size(im,1)-delta+1 ;
[x,y] = meshgrid(xr,yr) ;
frames = [x(:)'; y(:)'] ;
frames(end+1,:) = delta/2 ;

[frames, patches] = vl_covdet(imgs, ...
                              'frames', frames, ...
                              'estimateAffineShape', true, ...
                              'estimateOrientation', true) ;

figure(7) ; clf ;
imagesc(im) ;
axis image off ; hold on ; colormap gray ;
vl_plotframe(frames) ;
vl_demo_print('covdet_custom_frames',.8) ;

% --------------------------------------------------------------------
%                                                         Scale spaces
% --------------------------------------------------------------------

[frames, descrs, info] = vl_covdet(imgs) ;

info

figure(8) ; clf ;
vl_plotss(info.gss) ;
colormap gray ;
vl_figaspect(2) ;
vl_demo_print('covdet_gss',.8) ;

figure(9) ; clf ;
vl_plotss(info.css) ;
colormap gray ;
vl_figaspect(2) ;
vl_demo_print('covdet_css',.8) ;

figure(10) ; clf ;
subplot(1,2,1) ;
hist([info.peakScores],10) ;
xlabel('Peak Score') ;
ylabel('Occurences') ;
grid on ;

subplot(1,2,2) ;
hist([info.edgeScores],10) ;
xlabel('Edge Score') ;
ylabel('Occurences') ;
grid on ;

vl_figaspect(2) ;
vl_demo_print('covdet_scores',.9) ;


function sF = rotate_outer(sF, rot)
% rotate a function by a rotation
%
% Syntax
%   sF = sF.rotate_outer(rot)
%
% Input
%  sF - @S2FunHarmonic
%  rot - @rotation
%
% Output
%  sF - @S2FunHarmonic
%

if sF.bandwidth ~= 0
  f = @(v) sF.eval(rotate(v, inv(rot)));
  sF = S2FunHarmonic.quadrature(f, 'bandwidth', sF.bandwidth);
end

end


function mask = compute_inpainting_mask(type,n,options)

% compute_inpainting_mask - compute an inpainting mask.
%
%   mask = compute_inpainting_mask(type,n,options);
%
%   mask=1 : data removed
%
%   Copyright (c) 2008 Gabriel Peyre

options.null = 0;
rho = getoptions(options, 'rho',.8);
qsub = getoptions(options, 'qsub',2);
name = getoptions(options, 'name','');

rand('state',123456);
switch type
    case 'rand'
        sel = randperm(n^2);
        mask = zeros(n);
        mask(sel(1:round(rho*end)))=1;
    case 'mask'
        n0 = getoptions(options, 'n0',1,1);
        mask = load_image([name '-mask'], n0);
        mask = mean(mask,3);
        mask = rescale( crop(mask,n), 0,1 );
        mask = rescale(mask)<.5;
    case 'checkboard'
        x = floor(0:1/(qsub-1):n); x= x(1:n);
        [Y,X] = meshgrid(x,x);
        mask = mod( X+Y,2 )==0;
    case 'superresol'
        [Y,X] = meshgrid(0:n-1,0:n-1);
        mask = (mod(X,qsub)==0) & (mod(Y,qsub)==0);
        mask = 1-mask;
    case {'grids' 'grids-inv'}
        [Y,X] = meshgrid(0:n-1,0:n-1);
        ngrid = getoptions(options, 'ngrid', 6);
        width = getoptions(options, 'width', 8);
        delta = n/ngrid;
        s = round(delta/2:delta:n);
        mask = zeros(n);
        for i=1:length(s);
            mask( abs(X-s(i))<=width/2 ) = 1;
        end
        mask = (mask + mask')>0;
        if strcmp(type, 'grids-inv')
            mask = 1-mask;
        end
    otherwise
        error('Unknown mask');
end


function out = tn(FUN,x0,varargin)
%TN   Truncated Newton minimization.
%
%   OUT = TN(FUN,X0) minimizes FUN starting at the point X0 using a
%   Hessian-free truncated Newton method. The outer loop is Newton's
%   method, and the inner loop uses a conjugate gradient method to compute
%   an approximation to the Newton direction. Furthermore, the Hessian
%   vector product is approximated using forward finite differences using
%   HESSVEC_FD. FUN is a handle for a function that takes a single vector
%   input and returns two arguments --- the scalar function value and the
%   vector-valued gradient. See POBLANO_OUT for details of the output
%   parameters.
%
%   OUT = TN(FUN,X0,'param',value,...) specifies a
%   parameters and its value. See POBLANO_PARAMS for further details on
%   standard parameters. Additionally, TN requires
%
%   'CGSolver' - Matlab CG method to use {'symmlq'}
%     'symmlq' : symmlq (designed for symmetric indefinite systems)
%     'pcg' :    pcg (designed for symmetric positive definite systems)
%
%   'CGIters' - maximum number of conjugate gradient iterations allowed {5}
%
%   'CGTolType' - CG stopping tolerance type used ('quadratic')
%     'quadratic' :    || R || / || G || <  min(0.5,|| G ||)
%     'superlinear' :  || R || / || G || <  min(0.5,sqrt(|| G ||))
%     'fixed' :        || R || < CGTol
%   where R is the residual and G is the gradient of FUN at X.
%
%   'CGTol' - CG stopping tolerance when CGTolType is 'fixed' {1e-6}
%
%   'HessVecFDStep' - Hessian vector product finite difference step {1e-10}
%     0 - use the default step given in HESSVEC_FD
%     >0 - fixed value to use at the difference step
%
%   PARAMS = TN('defaults') returns a structure containing the
%   default parameters for the particular Poblano method.
%
%
%   Examples
%
%   Suppose the function and gradient of the objective function are
%   specified in an mfile named mysin.m:
%
%     function [f,g]=example1(x,a)
%     if nargin < 2, a = 1; end
%     f = sin(a*x);
%     g = a*cos(a*x);
%
%   We can call the optimization method (using its default
%   parameters) using the command:
%
%     out = tn(@(x) example1(x,3), pi/4);
%
%   To change a parameter, we can specify a param/value input pair
%   as follows:
%
%     out = tn(@(x) example1(x,3), pi/4, 'Display', 'final');
%
%   Alternatively, we can use a structure to define the parameters:
%
%     params.MaxIters = 2;
%     out = tn(@(x) example1(x,3), pi/4, params);
%
%   See also POBLANO_OUT, POBLANO_PARAMS, HESSVEC_FD, FUNCTION_HANDLE.
%
%MATLAB Poblano Toolbox.
%Copyright 2009-2012, Sandia Corporation.

%% Parse parameters

% Create parser
params = inputParser;

% Set Poblano parameters
params = poblano_params(params);

% Set parameters for this method
params.addParamValue('CGIters',5,@(x) x > 0);
params.addParamValue('CGTolType','quadratic',@(x) ismember(x,{'quadratic','superlinear','fixed'}));
params.addParamValue('CGTol',1e-6, @(x) x > 0);
params.addParamValue('HessVecFDStep',1e-10, @(x) x >= 0);
params.addParamValue('CGSolver','symmlq',@(x) ismember(x,{'symmlq','pcg'}));

% Parse input
params.parse(varargin{:});

%% Check input arguments
if (nargin == 1) && isequal(FUN,'defaults') && (nargout == 1)
    out = params.Results;
    return;
elseif (nargin < 2)
    error('Error: invalid input arguments');
end

%% Initialize
xk = x0;
[fk,gk] = feval(FUN,xk);
out = poblano_out(xk,fk,gk,1,params);
ak = 1.0;

cgIters = params.Results.CGIters;
tolType = params.Results.CGTolType;
cgTol = params.Results.CGTol;
hessvecFDstep = params.Results.HessVecFDStep;
solver = params.Results.CGSolver;

%% Main loop
while out.ExitFlag == -1

    % Compute step direction, pk
    ngk = norm(gk);
    switch tolType
        case 'quadratic'
            cgTol = min(1-eps,min(0.5,ngk)*ngk);       % quadratic convergence
        case 'superlinear'
            cgTol = min(1-eps,min(0.5,sqrt(ngk))*ngk); % superlinear convergence
        otherwise
    end

    % Setup Hessian vector product finite difference approximation
    if hessvecFDstep > 0
        hv = @(v) hessvec_fd(v,FUN,xk,gk,hessvecFDstep);
    else
        hv = @(v) hessvec_fd(v,FUN,xk,gk);
    end

    % Keep track of number of function calls are made using hessvec_fd
    global nfev_hessvec_fd;
    nfev_hessvec_fd = 0;

    % Compute step direction, pk
    [pk,cg_flag,cg_relres,ncgfev]= feval(solver, hv, -gk, cgTol, cgIters);

    % Compute step length
    [xk,fk,gk,ak,lsinfo,nfev] = poblano_linesearch(FUN,xk,fk,gk,ak,pk,params.Results);
    if (lsinfo ~= 1)
        if strcmp(params.Results.Display, 'iter')
            fprintf(1,[mfilename,': line search warning = %d\n'],lsinfo);
        end
        pk = -gk;
        [xk,fk,gk,ak,lsinfo,nfev] = poblano_linesearch(FUN,xk,fk,gk,ak,pk,params.Results);
    end

    % Update counts, check exit conditions, etc.
    out = poblano_out(xk,fk,gk,nfev+nfev_hessvec_fd,params,out);
end

clear nfev_hessvec_fd;


function [id,d] = df_query( DF, p)
% DF_QUERY Use the spatial index build with df_build to provide approximate NN
% and distance from the NN.
%
% [id,d] = df_query(DF,p)
%
% Input:
%   DF  output of df_build
%   p  query point
% Output:
%   id  index of the point closer to p (approximate)
%   d  distance from the point closer to p (approximate)

p = p-DF.MIN;
p = p./DF.S;
p = round(p)+1;

p(p<1) = 1;
p(p>DF.C') = DF.C(p>DF.C');

id = DF.N(p(1),p(2),p(3));
d  = DF.D(p(1),p(2),p(3));

end


% mri_superres.m
% NOT DONE!
%
% Explore super-resolution in MRI with rotated propeller sequences
% Inspired by Benoit Desjardins' work.
% Copyright 2003-7-23, Jeff Fessler, The University of Michigan

%
% create k-space samples, in units of 1/mm
%
if ~isvar('kspaces'), printm 'setup kspace'
	fov = 256;	% [mm] typical brain FOV
	N0 = 32;	% nominal image size
	kmax = N0/2*(1/fov);	% for display axes

	k1 = ([-N0/2:N0/2-1]+0.0)/fov;
	[kk1, kk2] = ndgrid(k1, k1);
	kcart = [kk1(:), kk2(:)];

	Nprops = [1 5];	% # of propellers

	for il=1:length(Nprops)
		Nprop = Nprops(il);

		kspace = zeros(Nprop*N0^2, 2);
		for ip=0:Nprop-1
			ang = ip/Nprop * pi;
			rot = [cos(ang) sin(ang); -sin(ang) cos(ang)];
			kspace(ip*N0^2+[1:N0^2],:) = kcart * rot;
			if ip > 0, % avoid repeated DC sampling
				kspace((N0/2+1)^2-1,:) = [];
			end
		end

		kspaces{il} = kspace;
	end, clear ang rot ip il k1 kk1 kk2 kcart

	im plc 2 3
	im('subplot', 1)
	if im
		plot(kspace(:,1), kspace(:,2), '.')
		axis(1.1*[-1 1 -1 1]*kmax), axis square
		xlabel 'k_1 [mm^{-1}]', ylabel 'k_2 [mm^{-1}]'
		title(sprintf('%d k-space samples (%d)', size(kspace,1), Nprop))
	end

	clear kmax kspace
prompt
end

%
% true object
%
if 0 || ~isvar('xtrue'), printm 'setup object'

	% display images with many pixels...
	Ndisp = 256;
	x1d = [-Ndisp/2:Ndisp/2-1] / Ndisp * fov;
	[x1dd x2dd] = ndgrid(x1d, x1d);

	% parameter units all in [mm]
	obj = mri_objects('case1');
	xtrue = obj.image(x1dd, x2dd);

	clim = [0 2];
	im(2, x1d, x1d, xtrue, 'x true', clim), cbar

	clear x1dd x2dd xpar
prompt
end

%
% analytical k-space data
%
if 0 || ~isvar('yd'), printm 'data'

	% noiseless data
	rng(0)
	for ik=1:length(Nprops)
		kspace = kspaces{ik};
		ytrue{ik} = obj.kspace(kspace(:,1), kspace(:,2));
		yd{ik} = ytrue{ik} + 0 * randn(size(ytrue{ik}));
	end
	clear x1dd x2dd xpar kspace ytrue

	[xhatg, yd_g, xg, kg] = mri_grid_linear(kspaces{1}, yd{1}, N0, fov);
	im(3, kg{1}, kg{2}, abs(yd_g), '|y_d|'), cbar

	clear kg yd_g xhatg ik kspace
prompt
end

return

%
% iterative recon
%
if ~isvar('xpcg')

Nlist = [2.^[6:6]];
niter = 20;
beta = 2^-10 * size(kspace,1);
xpcg = {};

for in=1:length(Nlist)
	N = Nlist(in);

	% gridding estimate to initialize iterations
	[xhatg, yhatg, xg] = mri_grid_linear(kspace, yd, N, fov);
	tmp = sprintf('|x| gridding %d', N);
	im(5, xg{1}, xg{2}, abs(xhatg), tmp, clim), cbar

	%
	% create Gnufft class object
	%
	if 1 || ~isvar('G'), printm 'setup Gnufft object'
		omega = 2*pi*kspace*fov/N;
%		minmax(omega(:,1))
		G = Gnufft({omega, [N N], [6 6], 2*[N N], [N/2 N/2]});
	end

	%
	% reconstruct by PCG
	%
	if 1 || ~isvar('xpcg'), printm 'PCG with quadratic penalty'
		mask = true(N);
		R = Robject(mask, 'beta', beta);
		ytmp = yd(:) * (1/fov)^2 * N^2;	% scaling!
		xiter = qpwls_pcg(xhatg(:), G, 1, ytmp, 0, R.C, 1, niter);
		xpcg{in} = embed(xiter(:,end), mask);
		im(6, xg{1}, xg{2}, abs(xpcg{in}), '|x| pcg quad'), cbar
	end
end % fof
end % if

return

if 0
	im plc 2 3
	im(1, x1d, x1d, xtrue, clim, 'x true'), cbar
	xtick([-fov/2 0 fov/2-1])
	ytick([-fov/2 0 fov/2-1])
	for in=1:length(Nlist)
		x1g = [-N/2:N/2-1]'/N * fov;
		tmp = sprintf('N=%d', Nlist(in));
		im(in+1, x1g, x1g, abs(xpcg{in}), tmp, clim), cbar
		xtick([-fov/2 0 fov/2-1])
		ytick([-fov/2 0 fov/2-1])
	end
%	ir_savefig fig_mri_pixel_size_image
return
end

if 1
	clf
	h(1) = plot(x1d, xtrue(:,Ndisp/2+1), 'c-');
	hold on
	llist = {'r:', 'y--', 'g-.', 'r.', 'm-'};
	arg = {'true'};
	for in=1:length(Nlist)
		N = Nlist(in);
		x1g = [-N/2:N/2-1]'/N * fov;
		tmp = xpcg{in};
		h(in+1) = plot(x1g, abs(tmp(:,N/2+1)), llist{in});
		arg = {arg{:}, sprintf('N=%d', N)};
	end
	hold off
	axis([-fov/2 fov/2 0 2.1])
	xlabel 'horizontal position [mm]'
	ylabel '|f(x,0)|'
	set(0, 'DefaultTextFontSize', 12)
%	set(0, 'DefaultAxesFontSize', 14)
	legend(h, arg{:})
%	ir_savefig fig_mri_pixel_size_profile
end


function [tim,perm] = visTextons(textons,fb)
% function [tim,perm] = visTextons(textons,fb)

if size(textons,1) ~= numel(fb),
  error('size(textons,1) ~= numel(fb)');
end

[d,k] = size(textons);

% find the max filter size
maxsz = max(size(fb{1}));
for j = 1:d,
  maxsz = max(maxsz,max(size(fb{j})));
end

% compute the linear combinations of filters
tim = cell(k,1);
for i = 1:k,
  tim{i} = zeros(maxsz);
  for j = 1:d,
    f = fb{j} * textons(j,i);
    off = (maxsz-size(f,1))/2;
    tim{i}(1+off:end-off,1+off:end-off) = tim{i}(1+off:end-off,1+off:end-off) + f;
  end
end

% computer permutation order for decreasing L1 norm
norms = zeros(k,1);
for i = 1:k,
  norms(i) = sum(sum(abs(tim{i})));
end
[y,perm] = sort(norms);
perm = flipud(perm);


function plotCortmagResults(cortMag, showPredicted)
% function plotCortmagResults(cortMag, [showPredicted])
%
if(~exist('showPredicted','var')) showPredicted = 1; end

nROIs = length(cortMag.bins);
pX = ceil(sqrt(nROIs));
pY = ceil(nROIs/pX);
xLower = 0;
xUpper = 100;
yLower = -pi;
yUpper = 4*pi;
for ii=1:nROIs
   mnPh = complexPh2PositiveRad(cortMag.meanPh{ii});
   %mnPh = unwrapPhases(mnPh);
   % plot unshifted data
   uDist = cortMag.corticalDist{ii}-cortMag.distanceShift(ii);
   figure(100);
   subplot(pX,pY,ii);
   plot(uDist,mnPh,'-o');
   set(gca,'xlim',[xLower xUpper]);
   set(gca,'ylim',[yLower yUpper]);
   str = sprintf('ROI %.0f',ii);
   title(str);
%    figure(99);
%    hold on;
%    plot(uDist,(mnPh-cortMag.fovealPhase)/(2*pi)*cortMag.stimulusRadius,'o');
%    hold off;
   %pause
end

figure(101);
%newGraphWin;
symbolString = 'b.';
errorbar(cortMag.allCorticalDist10deg, cortMag.allStimDeg, cortMag.allStimDegSE, symbolString);

% The predicted exponential CMF curve, when the distances have
% been adjusted so that 0 means 10 deg, is:
%
d = sort(cortMag.allCorticalDist10deg);

if showPredicted
    predictedDeg = exp(d*cortMag.fitParms.dScale + log(10));
    hold on; plot(d,predictedDeg,'k-.'); hold off
    cortMag.predictedDeg = predictedDeg;
end

% Guesses about the window parameters
%
xLower = min(cortMag.allCorticalDist10deg);
xUpper = max(cortMag.allCorticalDist10deg);
%xUpper = 15;
yLower = -4;
yUpper = 1.1*cortMag.stimulusRadius;

set(gca,'xlim',[xLower xUpper]);
set(gca,'ylim',[yLower yUpper]);
set(gca,'xtick',round([xLower:5:xUpper]),'ytick',[yLower:4:yUpper])

% Place the resulting data in the figure for later plotting
set(gca,'UserData',cortMag);
disp('Data stored in plot- retrieve it with: cortMag=get(gca,''UserData'')');

subject = eval('cortMag.subject','');
dataType = eval('cortMag.dataType','');
flatDir = eval('cortMag.subdir','');
title([subject,' (',cortMag.hemisphere,',',dataType,',',flatDir,'): ',...
      'dScale=',num2str(cortMag.fitParms.dScale),'  ',...
      'dShift=',num2str(cortMag.fitParms.dShift),'  ',...
      'foveaPh=',num2str(cortMag.fitParms.fovealPhase)]);
%plot(cortMag.allCorticalDist, complexPh2PositiveRad(cortMag.allMeanPh),'x');
return;


function [Population,Fitness] = Second_Stage_EnvironmentalSelection(Population,N,isOrigin)
% The environmental selection of SPEA2

%------------------------------- 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 Kangjia Qiao

    %% Calculate the fitness of each solution
    if isOrigin==1
    %     con = Population.cons;
    %     con(find(con<=0)) = 0;
        Fitness = CalFitness(Population.objs,Population.cons);
    else
        Fitness = CalFitness(Population.objs);
    end

    %% Environmental selection
    Next = Fitness < 1;
    if sum(Next) < N
        [~,Rank] = sort(Fitness);
        Next(Rank(1:N)) = true;
    elseif sum(Next) > N
        Del  = Truncation(Population(Next).objs,sum(Next)-N);
        Temp = find(Next);
        Next(Temp(Del)) = false;
    end
    % Population for next generation
    Population = Population(Next);
    Fitness    = Fitness(Next);
    % Sort the population
    [Fitness,rank] = sort(Fitness);
    Population = Population(rank);
end

function Del = Truncation(PopObj,K)
% Select part of the solutions by truncation

    %% Truncation
    Distance = pdist2(PopObj,PopObj);
    Distance(logical(eye(length(Distance)))) = inf;
    Del = false(1,size(PopObj,1));
    while sum(Del) < K
        Remain   = find(~Del);
        Temp     = sort(Distance(Remain,Remain),2);
        [~,Rank] = sortrows(Temp);
        Del(Remain(Rank(1))) = true;
    end
end


%
% [A,b,c,K]=convertf(A,b,c,K)
%
% converts free variables in a SeDuMi problem into nonnegative LP variables.
%
function [A,b,c,K]=convertf(A,b,c,K)
%
% Get the number of constraints.
%
m=length(b);
%
%  Deal with the following special case.  If A is transposed, transpose
%  it again so that it is of the right size.
%
[Am,An]=size(A);
if (Am ~= m)
  if (An == m)
    fprintf('Transposing A to match b \n');
    A=A';
  else
    fprintf('A is not of the correct size to match b \n');
    return
  end
end
%
%  Deal with the following special case:  if c==0, then c should really
%  be a zero vector of the appropriate size.
%
if (c == 0)
  fprintf('Expanding c to the appropriate size\n');
  [Am,An]=size(A);
  c=zeros(An,1);
end
%
% If c is empty, then act as if it was zero.
%
if (isempty(c))
  fprintf('Expanding empty c to zeros of the appropriate size\n');
  [Am,An]=size(A);
  c=zeros(An,1);
end
%
% If c is a row vector, make it a column vector.
%
[cm,cn]=size(c);
if (cn > cm)
  c=c';
end
%
% Check for any free LP variables and rewrite them as the differences of
% regular LP variables.
%
if (isfield(K,'f'))
  nfree=K.f
  fprintf('Converting %d free variables to LP variables\n',nfree);
  if (isfield(K,'l'))
    nlin=K.l;
  else
    nlin=0;
  end
  [Am,An]=size(A);
  Anew=[A(:,1:nfree) -A(:,1:nfree) A(:,nfree+1:An)];
  A=Anew;
  cnew=[c(1:nfree); -c(1:nfree); c(nfree+1:An)];
  c=cnew;

  K.l=nlin+2*nfree;
  K.f=0;
end


function Population = Truncation(Population,N)
% Limit the size of final popualtion in RVEA*

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

    Choose = true(1,length(Population));
    Cosine = 1 - pdist2(Population.objs,Population.objs,'cosine');
    Cosine(logical(eye(length(Cosine)))) = 0;
    while sum(Choose) > N
        Remain   = find(Choose);
        Temp     = sort(-Cosine(Remain,Remain),2);
        [~,Rank] = sortrows(Temp);
        Choose(Remain(Rank(1))) = false;
    end
    Population = Population(Choose);
end


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

function y = psabr32(t,T,f,F,alpha,A,beta,nu,rho)
% Wu expansion of SABR density (2nd order)
tau = (T-t)/T;
if beta == 1    % account for log normal setting
    u = (log(f) - log(F))./(alpha * sqrt(T));
else
    u = (f^(1-beta)-F.^(1-beta))/(alpha*(1-beta)*sqrt(T));
end
v = log(alpha./A)./(nu*sqrt(T));
a11 = -beta * F^(beta-1).*A./nu.*(u-rho*v);
a10 = u^2*v-rho*u*v.^2;

a = 2*rho+beta*F^(beta-1)*A./nu;
b = 2+beta*(1-beta)*F^(2*(beta-1))*A.^2./nu^2;
c = beta*F^(beta-1)*A./nu;

a23 = rho^4*(20-6*b)-12*rho^3*a+rho^2*(3*a.^2-28+12*b) ...
    +12*a*rho+8-3*a.^2-6*b;
a22 = u^2*(3*a.^2-12*a*rho+6*b*(-1+rho^2)+2*rho^2+10)...
    - 2*u*v.*(rho^3*(2+3*b)+rho^2*(-9*a+3*c) ...
    +rho*(10+3*a.^2-3*b)-(3*a+3*c)) + v.^2.*((2+3*(a-2*rho).^2)*rho^2 ...
    + 6*c*rho*(-1+rho^2)-2);
a21 = u^4+v.^4*rho^2+u^3*v.*(8*rho-6*a)+u*v.^3.*(8*rho^3-6*a*rho^2) ...
    +u^2*v.^2.*(-14*rho^2+12*a*rho-4);
a20 = 3*u^4*v.^2-6*u^3*v.^3*rho+3*u^2*v.^4*rho^2;
y = 1./(nu*T*F^beta*A.^2).*(1+nu*sqrt(T)./(2*(-1+rho^2)).*(a11+a10/tau) ...
    +(nu^2*T)./(24*(1-rho^2)^2).*(a23*tau+a22+a21/tau+a20/tau^2))...
    ./(2*pi*tau*sqrt(1-rho^2)).*exp(-(u^2-2*rho*u*v+v.^2) ...
    ./(2*tau*(1-rho^2)));
end


function [gradient matrix] = pseudoGradient(skel, channels, gradientDirection, ind,rotInd2, ind2Compute)

% PSEUDOGRADIENT ths position function of a node is composed with 2
% terms. The first one is the position of the parent node, the second is an
% additional term to get the real position. This function computes the
% derivative of the second term for the node 'ind2Compute' in respect with
% the channels 'rotInd2' given a set of channels.
%
%	Description:
%
%	[gradient matrix] = PSEUDOGRADIENT(skel, channels,gradientDirection, ind,rotInd2, ind2Compute)
%
%	 Returns:
%	  GRADIENT - the coponents of the gradient (x,y,z)
%     MATRIX - the rotation matrix
%	 Arguments:
%	  SKEL - a skeleton structure
%	  CHANNELS - the channels where the gradient has to be computes
%     GRADIENTDIRECTION - the direction of the gradient
%     IND - a recursive paremeter... start with 0 in general
%     ROTIND2 - the gradient has to be computed in respect with this
%     channel
%     IND2COMPUTE - the node ID where the gradient has to be computed

if (ind == 0)
    ind = ind2Compute;
end
matrix =eye(3);
parent = skel.tree(ind).parent;

rotVal = zeros(1, 3);
derive =0 ;
for j = 1:length(skel.tree(ind).rotInd)
    rind = skel.tree(ind).rotInd(j);
    if (rind == rotInd2)
        derive = 1;
    end
    if rind
        rotVal(j) = channels(rind);
    else
        rotVal(j) = 0;
    end
end

if derive
    tdof = rotationMatrixGradient(deg2rad(rotVal(1)), ...
        deg2rad(rotVal(2)), ...
        deg2rad(rotVal(3)), ...
        skel.tree(ind).order,gradientDirection);

else
    tdof = rotationMatrix(deg2rad(rotVal(1)), ...
        deg2rad(rotVal(2)), ...
        deg2rad(rotVal(3)), ...
        skel.tree(ind).order);

end

torient = rotationMatrix(deg2rad(skel.tree(ind).axis(1)), ...
    deg2rad(skel.tree(ind).axis(2)), ...
    deg2rad(skel.tree(ind).axis(3)), ...
    skel.tree(ind).axisOrder);
torientInv = rotationMatrix(deg2rad(-skel.tree(ind).axis(1)), ...
    deg2rad(-skel.tree(ind).axis(2)), ...
    deg2rad(-skel.tree(ind).axis(3)), ...
    skel.tree(ind).axisOrder(end:-1:1));

if (parent~=0)

   [useless matrixTmp] = pseudoGradient(skel, channels, gradientDirection, parent,rotInd2, ind2Compute);
    matrix = torientInv*tdof*torient*matrixTmp;
else

    derive = 0;
    rotVal = skel.tree(1).orientation;
    for i = 1:length(skel.tree(1).rotInd)
        rind = skel.tree(1).rotInd(i);

        if (rind == rotInd2)
            derive = 1;

        end
        if rind
            rotVal(i) = rotVal(i) + channels(rind);
        end
    end
    if derive

        matrix2 = rotationMatrixGradient(deg2rad(rotVal(1)), ...
            deg2rad(rotVal(2)), ...
            deg2rad(rotVal(3)), ...
            skel.tree(1).order,gradientDirection);

    else

        matrix2 = rotationMatrix(deg2rad(rotVal(1)), ...
            deg2rad(rotVal(2)), ...
            deg2rad(rotVal(3)), ...
            skel.tree(1).axisOrder);

    end

matrix = matrix * matrix2;
end
gradient = skel.tree(ind2Compute).offset*matrix;


function F=frame(ftype,varargin);
%FRAME  Construct a new frame
%   Usage: F=frame(ftype,...);
%
%   `F=frame(ftype,...)` constructs a new frame object *F* of type
%   *ftype*. Arguments following *ftype* are specific to the type of frame
%   chosen.
%
%   Time-frequency frames
%   ---------------------
%
%   `frame('dgt',g,a,M)` constructs a Gabor frame with window *g*,
%   time-shift *a* and *M* channels. See the help on |dgt| for more
%   information.
%
%   `frame('dgtreal',g,a,M)` constructs a Gabor frame for real-valued
%   signals with window *g*, time-shift *a* and *M* channels. See the help
%   on |dgtreal| for more information.
%
%   `frame('dwilt',g,M)` constructs a Wilson basis with window *g* and *M*
%   channels. See the help on |dwilt| for more information.
%
%   `frame('wmdct',g,M)` constructs a windowed MDCT basis with window *g*
%   and *M* channels. See the help on |wmdct| for more information.
%
%   `frame('filterbank',g,a,M)` constructs a filterbank with filters *g*,
%   time-shifts of *a* and *M* channels. For the ease of implementation, it
%   is necessary to specify *M*, even though it strictly speaking could be
%   deduced from the size of the windows. See the help on |filterbank| for
%   more information on the parameters. Similarly, you can construct a
%   uniform filterbank by selecting `'ufilterbank'`, a positive-frequency
%   filterbank by selecting `'filterbankreal'` or a uniform
%   positive-frequency filterbank by selecting `'ufilterbankreal'`.
%
%   `frame('nsdgt',g,a,M)` constructs a non-stationary Gabor frame with
%   filters *g*, time-shifts of *a* and *M* channels. See the help on
%   |nsdgt| for more information on the parameters. Similarly, you can
%   construct a uniform NSDGT by selecting `'unsdgt'`, an NSDGT for
%   real-valued signals only by selecting `'nsdgtreal'` or a
%   uniform NSDGT for real-valued signals by selecting `'unsdgtreal'`.
%
%   Wavelet frames
%   --------------
%
%   `frame('fwt', w, J)` constructs a wavelet frame with wavelet definition
%   *w* and *J* number of filterbank iterations. Similarly, a redundant time
%   invariant wavelet representation can be constructed by selecting `'ufwt'`.
%   See the help on |fwt| and |ufwt| for more information.
%
%   `frame('wfbt', wt)` constructs a wavelet filterbank tree defined by
%   the wavelet filterbank tree definition *wt*. Similarly, an undecimated
%   wavelet filterbank tree can be constructed by selecting `'uwfbt'`. See the
%   help on |wfbt| and |uwfbt| for more information.
%
%   `frame('wpfbt', wt)` constructs a wavelet packet filterbank tree
%   defined by the wavelet filterbank tree definition *wt*. Similarly, an
%   undecimated wavelet packet filterbank tree can be constructed by selecting
%   `'uwpfbt'`. See the help on |wpfbt| and |uwpfbt| for more information.
%
%   Pure frequency frames
%   ---------------------
%
%   `frame('dft')` constructs a basis where the analysis operator is the
%   |dft|, and the synthesis operator is its inverse, |idft|. Completely
%   similar to this, you can enter the name of any of the cosine or sine
%   transforms |dcti|, |dctii|, |dctiii|, |dctiv|, |dsti|, |dstii|,
%   |dstiii| or |dstiv|.
%
%   `frame('reddft',red)` constructs so called harmonic Parseval tight
%   frame or redundant dft with redundancy `red`. The frame accepts any
%   `red`, but |frana| will only work for signal lengths `Ls` for which
%   the number of coefficients `Ls*red` is an integer.
%
%   `frame('dftreal')` constructs a normalized |fftreal| basis for
%   real-valued signals of even length only. The basis is normalized
%   to ensure that is it orthonormal.
%
%   Special / general frames
%   ------------------------
%
%   `frame('gen',g)` constructs a general frame with a synthesis matrix *g*.
%   The frame atoms must be stored as column vectors in the matrix.
%
%   `frame('identity')` constructs the canonical orthonormal basis, meaning
%   that all operators return their input as output, so it is the dummy
%   operation.
%
%   Container frames
%   ----------------
%
%   `frame('fusion',w,F1,F2,...)` constructs a fusion frame, which is
%   the collection of the frames specified by *F1*, *F2*,... The vector
%   *w* contains a weight for each frame. If *w* is a scalar, this weight
%   will be applied to all the sub-frames.
%
%   `frame('tensor',F1,F2,...)` constructs a tensor product frame, where the
%   frames *F1, *F2*,... are applied along the 1st, 2nd etc. dimensions. If
%   you don't want any action along a specific dimension, use the `identity`
%   frame along that dimension. Any remaining dimensions in the input
%   signal are left alone.
%
%   Wrapper frames
%   --------------
%
%   Frames types in this section are "virtual". They serve as a wrapper for
%   a different type of frame.
%
%   `frame('erbletfb',fs,Ls,...)` constructs an Erb-let filterbank frame for
%   a given samp. frequency *fs* working with signals of length *Ls*. See
%   |erbfilters| for a description of additional parameters as all
%   parameters other than the frame type string 'erbletfb' are passed to it.
%   NOTE: The resulting frame is defined only for a single signal length
%   *Ls*. Shorter signals will be zero-padded, signals longer than *Ls*
%   cannot be processed.
%   The actual frame type is 'filterbank' or 'filterbankreal'.
%
%   `frame('cqtfb',fs,fmin,fmax,bins,Ls,...)` constructs a CQT filterbank
%   frame for a given samp. frequency *fs* working with signals of length
%   *Ls*. See |cqtfilters| for a description of other parameters.
%   NOTE: The resulting frame is defined only for a single signal length
%   *Ls*. Shorter signals will be zero-padded, signals longer than *Ls*
%   cannot be processed.
%   The actual frame type is 'filterbank' or 'filterbankreal'.
%
%   Examples
%   --------
%
%   The following example creates a Modified Discrete Cosine Transform frame,
%   analyses an input signal and plots the frame coefficients:::
%
%      F=frame('wmdct','gauss',40);
%      c=frana(F,greasy);
%      plotframe(F,c,'dynrange',60);
%
%   See also: frana, frsyn, plotframe

complainif_notenoughargs(nargin,1,'FRAME');

if ~ischar(ftype)
  error(['%s: First argument must be a string denoting the type of ' ...
         'frame.'],upper(mfilename));
end;

ftype=lower(ftype);

% True if the frame only works with real-valued input.
F.realinput=0;

% True if the frame only works with a fixed length.
F.fixedlength = 0;

% Handle the windowed transforms
switch(ftype)
 case {'dgt','dwilt','wmdct','filterbank','ufilterbank',...
       'nsdgt','unsdgt','wfbt','uwfbt','wpfbt'}
  F.g=varargin{1};

 case {'dgtreal','filterbankreal','ufilterbankreal',...
      'nsdgtreal','unsdgtreal'}
  F.g=varargin{1};
  F.realinput=1;

 case {'fwt','ufwt'}
  F.g=varargin{1};
  F.J=varargin{2};
  complainif_notposint(F.J,'J','FRAME');
end;

% Input param checking
switch(ftype)
  case 'fusion'
     wtmp = varargin{1};
     % Check w
     if ~isnumeric(varargin{1}) || ...
        ~(isscalar(wtmp) || numel(wtmp) == numel(varargin) -1)
       error('%s: Weights are not in a correct format.',upper(mfilename));
     end

     % Check frame objects
     for ii=2:numel(varargin)
        complainif_notvalidframeobj(varargin{ii},'FRAME');
     end
  case 'tensor'
     % Check frame objects
     for ii=1:numel(varargin)
        complainif_notvalidframeobj(varargin{ii},'FRAME');
     end
end

% For parsing optional parameters to the transforms.
vargs={};
definput=struct();

%% ---- Pre-optional parameters
% Common operations to deal with the input parameters.
switch(ftype)
  case {'dgt','dgtreal'}
    F.a=varargin{2};
    F.M=varargin{3};

    vargs=varargin(4:end);
    definput.keyvals.lt=[0 1];
    definput.flags.phase={'freqinv','timeinv'};

  case {'dwilt','wmdct'}
    F.M=varargin{2};
  case {'filterbank','ufilterbank','filterbankreal','ufilterbankreal'}
    F.a=varargin{2};
    F.M=varargin{3};

    [F.a,~]=comp_filterbank_a(F.a,F.M,struct());

  case {'nsdgt','unsdgt','nsdgtreal','unsdgtreal'}
    F.a=varargin{2};
    F.M=varargin{3};

    % Sanitize 'a' and 'M'. Make M a column vector of length N,
    % where N is determined from the length of 'a'
    F.a=F.a(:);
    F.N=numel(F.a);
    F.M=bsxfun(@times,F.M(:),ones(F.N,1));
  case {'ufwt'}
    vargs=varargin(3:end);
    definput.flags.scaling={'sqrt','noscale','scale'};
  case {'uwfbt'}
    vargs=varargin(2:end);
    definput.flags.scaling={'sqrt','noscale','scale'};
  case {'wpfbt'}
    vargs=varargin(2:end);
    definput.flags.interscaling={'intsqrt','intnoscale','intscale'};
  case {'uwpfbt'}
    vargs=varargin(2:end);
    definput.flags.interscaling={'intsqrt','intnoscale','intscale'};
    definput.flags.scaling={'sqrt','noscale','scale'};
end;

[F.flags,F.kv]=ltfatarghelper({},definput,vargs);

F.type=ftype;
F.origargs=varargin;
F.vargs=vargs;

%% ------ Post optional parameters

% Default value, works for all bases
F.red=1;

% Default value, frame works for all lengths
F.length=@(Ls) Ls;

switch(ftype)
  case 'gen'
    F.g=varargin{1};
    F.frana=@(insig) F.g'*insig;
    F.frsyn=@(insig) F.g*insig;
    F.length = @(Ls) size(F.g,1);
    F.red = size(F.g,2)/size(F.g,1);

  case 'identity'
    F.frana=@(insig) insig;
    F.frsyn=@(insig) insig;

  case 'dft'
    F.frana=@(insig) dft(insig,[],1);
    F.frsyn=@(insig) idft(insig,[],1);

  case 'reddft'
    F.red = 1;
    if nargin > 1, F.red = varargin{1}; end
    if ~isscalar(F.red) || F.red < 1
        error('%s: Redundancy must be greater or equal to 1.',upper(mfilename));
    end
    F.frana     = @(insig) dft(insig,F.red*size(insig,1),1);
    F.frsyn     = @(insig) postpad(idft(insig,[],1),size(insig,1)/F.red);
    F.lengthcoef= @(Ncoef) Ncoef/F.red;
    F.clength   = @(L) L*F.red;

  case 'dftreal'
    F.frana=@(insig) fftreal(insig,[],1)/sqrt(size(insig,1));
    F.frsyn=@(insig) ifftreal(insig,(size(insig,1)-1)*2,1)*sqrt((size(insig,1)-1)*2);
    F.length=@(Ls) ceil(Ls/2)*2;
    F.lengthcoef=@(Ncoef) (Ncoef-1)*2;
    F.realinput=1;
    F.clength = @(L) floor(L/2)+1;

  case 'dcti'
    F.frana=@(insig) dcti(insig,[],1);
    F.frsyn=@(insig) dcti(insig,[],1);

  case 'dctii'
    F.frana=@(insig) dctii(insig,[],1);
    F.frsyn=@(insig) dctiii(insig,[],1);

  case 'dctiii'
    F.frana=@(insig) dctiii(insig,[],1);
    F.frsyn=@(insig) dctii(insig,[],1);

  case 'dctiv'
    F.frana=@(insig) dctiv(insig,[],1);
    F.frsyn=@(insig) dctiv(insig,[],1);

  case 'dsti'
    F.frana=@(insig) dsti(insig,[],1);
    F.frsyn=@(insig) dsti(insig,[],1);

  case 'dstii'
    F.frana=@(insig) dstii(insig,[],1);
    F.frsyn=@(insig) dstiii(insig,[],1);

  case 'dstiii'
    F.frana=@(insig) dstiii(insig,[],1);
    F.frsyn=@(insig) dstii(insig,[],1);

  case 'dstiv'
    F.frana=@(insig) dstiv(insig,[],1);
    F.frsyn=@(insig) dstiv(insig,[],1);

  case 'dgt'
    F.coef2native=@(coef,s) reshape(coef,[F.M,s(1)/F.M,s(2)]);
    F.native2coef=@(coef)   reshape(coef,[size(coef,1)*size(coef,2),size(coef,3)]);
    F.frana=@(insig) F.native2coef(comp_dgt(insig,F.g,F.a,F.M,F.kv.lt,F.flags.do_timeinv,0,0));
    F.frsyn=@(insig) comp_idgt(F.coef2native(insig,size(insig)),F.g,F.a,F.kv.lt,F.flags.do_timeinv,0);
    F.length=@(Ls) dgtlength(Ls,F.a,F.M,F.kv.lt);
    F.red=F.M/F.a;

  case 'dgtreal'
    F.coef2native=@(coef,s) reshape(coef,[floor(F.M/2)+1,s(1)/(floor(F.M/ ...
                                                      2)+1),s(2)]);
    F.native2coef=@(coef)   reshape(coef,[size(coef,1)*size(coef,2),size(coef,3)]);
    F.frana=@(insig) F.native2coef(comp_dgtreal(insig,F.g,F.a,F.M,F.kv.lt,F.flags.do_timeinv));
    F.frsyn=@(insig) comp_idgtreal(F.coef2native(insig,size(insig)),F.g,F.a,F.M,F.kv.lt,F.flags.do_timeinv);
    F.length=@(Ls) dgtlength(Ls,F.a,F.M,F.kv.lt);
    F.red=F.M/F.a;
    F.lengthcoef=@(Ncoef) Ncoef/(floor(F.M/2)+1)*F.a;
    F.clength = @(L) L/F.a*(floor(F.M/2)+1);

  case 'dwilt'
    F.coef2native=@(coef,s) reshape(coef,[2*F.M,s(1)/F.M/2,s(2)]);
    F.native2coef=@(coef)   reshape(coef,[size(coef,1)*size(coef,2),size(coef,3)]);
    F.frana=@(insig) F.native2coef(comp_dwilt(insig,F.g,F.M));
    F.frsyn=@(insig) comp_idwilt(F.coef2native(insig,size(insig)),F.g);
    F.length=@(Ls) dwiltlength(Ls,F.M);

  case 'wmdct'
    F.coef2native=@(coef,s) reshape(coef,[F.M,s(1)/F.M,s(2)]);
    F.native2coef=@(coef)   reshape(coef,[size(coef,1)*size(coef,2),size(coef,3)]);
    F.frana=@(insig) F.native2coef(comp_dwiltiii(insig,F.g,F.M));
    F.frsyn=@(insig) comp_idwiltiii(F.coef2native(insig,size(insig)),F.g);
    F.length=@(Ls) dwiltlength(Ls,F.M);

  case 'filterbank'
    F.red=sum(F.a(:,2)./F.a(:,1));
    F.length=@(Ls) filterbanklength(Ls,F.a);
    F.lengthcoef=@(Ncoef) Ncoef/F.red;
    F.native2coef=@(coef) cell2mat(coef(:));
    F.coef2native=@(coef,s) vect2cell(coef,round(s(1)/F.red*F.a(:,2)./F.a(:,1)));
    F.frana=@(insig) F.native2coef(comp_filterbank(insig,F.g,F.a));
    F.frsyn=@(insig) comp_ifilterbank(F.coef2native(insig,size(insig)),...
                                      F.g,F.a,round(size(insig,1)/F.red));
    F.destructor=@() clear('comp_filterbank','comp_ifilterbank');

  case 'filterbankreal'
    F.red=2*sum(F.a(:,2)./F.a(:,1));
    F.length=@(Ls) filterbanklength(Ls,F.a);
    F.lengthcoef=@(Ncoef) 2*Ncoef/(F.red);
    F.native2coef=@(coef) cell2mat(coef(:));
    F.coef2native=@(coef,s) vect2cell(coef,round(2*s(1)/F.red*F.a(:,2)./F.a(:,1)));
    F.frana=@(insig) F.native2coef(comp_filterbank(insig,F.g,F.a));
    F.frsyn=@(insig) 2*real(comp_ifilterbank(F.coef2native(insig,size(insig)),F.g,F.a,...
                                             round(2*size(insig,1)/F.red)));
    F.destructor=@() clear('comp_filterbank','comp_ifilterbank');

  case 'ufilterbank'
    F.red=sum(F.a(:,2)./F.a(:,1));
    F.length=@(Ls) filterbanklength(Ls,F.a);
    F.lengthcoef=@(Ncoef) round(Ncoef/F.red);
    F.coef2native=@(coef,s) reshape(coef,[s(1)/F.M,F.M,s(2)]);
    F.native2coef=@(coef)   reshape(coef,[size(coef,1)*size(coef,2),size(coef,3)]);
    F.frana=@(insig) F.native2coef(ufilterbank(insig,F.g,F.a));
    F.frsyn=@(insig) ifilterbank(F.coef2native(insig,size(insig)),F.g,F.a);

  case 'ufilterbankreal'
    F.red=2*sum(F.a(:,2)./F.a(:,1));
    F.length=@(Ls) filterbanklength(Ls,F.a);
    F.lengthcoef=@(Ncoef) round(Ncoef/F.red*2);
    F.coef2native=@(coef,s) reshape(coef,[s(1)/F.M,F.M,s(2)]);
    F.native2coef=@(coef)   reshape(coef,[size(coef,1)*size(coef,2),size(coef,3)]);
    F.frana=@(insig) F.native2coef(ufilterbank(insig,F.g,F.a));
    F.frsyn=@(insig) 2*real(ifilterbank(F.coef2native(insig,size(insig)),F.g, ...
                                        F.a));

  case 'nsdgt'
    F.coef2native=@(coef,s) mat2cell(coef,F.M,s(2));
    F.native2coef=@(coef) cell2mat(coef(:));
    F.length=@(Ncoef) sum(F.a);
    F.lengthcoef=@(Ncoef) sum(F.a);
    F.red=sum(F.M)/sum(F.a);
    F.frana=@(insig) F.native2coef(nsdgt(insig,F.g,F.a,F.M));
    F.frsyn=@(insig) insdgt(F.coef2native(insig,size(insig)),F.g,F.a);

  case 'unsdgt'
    F.coef2native=@(coef,s) reshape(coef,[F.M(1),s(1)/F.M(1),s(2)]);
    F.native2coef=@(coef)   reshape(coef,[size(coef,1)*size(coef,2),size(coef,3)]);
    F.frana=@(insig) F.native2coef(unsdgt(insig,F.g,F.a,F.M));
    F.frsyn=@(insig) insdgt(F.coef2native(insig,size(insig)),F.g,F.a);
    F.length=@(Ncoef) sum(F.a);
    F.lengthcoef=@(Ncoef) sum(F.a);
    F.red=sum(F.M)/sum(F.a);

  case 'nsdgtreal'
    F.coef2native=@(coef,s) mat2cell(coef,floor(F.M/2)+1,s(2));
    F.native2coef=@(coef) cell2mat(coef(:));
    F.frana=@(insig) F.native2coef(nsdgtreal(insig,F.g,F.a,F.M));
    F.frsyn=@(insig) insdgtreal(F.coef2native(insig,size(insig)),F.g,F.a,F.M);
    F.length=@(Ncoef) sum(F.a);
    F.lengthcoef=@(Ncoef) sum(F.a);
    F.red=sum(F.M)/sum(F.a);
    F.clength=@(L) sum(floor(F.M/2)+1);

  case 'unsdgtreal'
    F.coef2native=@(coef,s) reshape(coef,floor(F.M(1)/2)+1,s(1)/ ...
                                    (floor(F.M(1)/2)+1),s(2));
    F.native2coef=@(coef)   reshape(coef,[size(coef,1)*size(coef,2),size(coef,3)]);
    F.frana=@(insig) F.native2coef(unsdgtreal(insig,F.g,F.a,F.M));
    F.frsyn=@(insig) insdgtreal(F.coef2native(insig,size(insig)),F.g,F.a,F.M);
    F.length=@(Ncoef) sum(F.a);
    F.lengthcoef=@(Ncoef) sum(F.a);
    F.red=sum(F.M)/sum(F.a);
    F.clength=@(L) numel(F.M)*(floor(F.M(1)/2)+1);

  case 'fusion'
    F.w=varargin{1};
    F.frames=varargin(2:end);

    if any(cellfun(@(fEl) fEl.realinput,F.frames))
        error(['%s: Real-valued-input-only frames are not currently ',...
               'supported in the fusion frame.'],upper(mfilename));
    end

    F.Nframes=numel(F.frames);
    F.w=bsxfun(@times,F.w(:),ones(F.Nframes,1));
    F.length = @(Ls) comp_framelength_fusion(F,Ls);
    F.red=sum(cellfun(@framered,F.frames));

    % These definitions binds F itself, so they must execute last
    F.frana=@(insig) comp_frana_fusion(F,insig);
    F.frsyn=@(insig) comp_frsyn_fusion(F,insig);

  case 'tensor'
    % This frame type is currently broken. It must be reworked to reshape
    % to the standard layout in order not to break all the assumptions.
    F.frames=varargin;
    F.Nframes=numel(F.frames);
    for ii=1:F.Nframes
        if F.frames{ii}.realinput
            error(['It is not safe to embed a real-valued-input-only frame ' ...
                   'into the tensor frame.']);
        end;
    end;

    F.frana=@(insig) comp_frana_tensor(F,insig);
    F.frsyn=@(insig) comp_frsyn_tensor(F,insig);

    F.length=@(Ls) comp_framelength_tensor(F,Ls);

    F.red=prod(cellfun(@framered,F.frames));

  case {'fwt','dwt'}
    % We have to initialize F.g here already
    [F.g, F.info]=fwtinit({'strict',F.g});
    F.red= 1/(F.g.a(1)^(F.J)) + sum(1./(F.g.a(1).^(0:F.J-1))*sum(1./F.g.a(2:end)));
    F.frana=@(insig) wavcell2pack(comp_fwt(insig,F.g.h,F.g.a,F.J,'per'));
    F.frsyn=@(insig) comp_ifwt(...
                        wavpack2cell(insig,fwtclength(size(insig,1)/F.red,F.g,F.J)),...
                               F.g.g,F.g.a,F.J,size(insig,1)/F.red,'per');
    F.length=@(Ls) fwtlength(Ls,F.g,F.J);
  case {'wfbt'}
    [F.g,F.info]=wfbtinit({'strict',F.g});
    F.red = sum(1./treeSub(F.g));
    % comp_ specific
    [F.wtPath, F.rangeLoc, F.rangeOut] = treeBFranges(F.g);

    F.coef2native = @(coef,s) wavpack2cell(coef,wfbtclength(s(1)/F.red,F.g));
    F.native2coef = @(coef) wavcell2pack(coef);
    F.frana=@(insig) F.native2coef(comp_wfbt(insig,F.g.nodes(F.wtPath),...
                                             F.rangeLoc,F.rangeOut,'per'));
    F.frsyn=@(insig) comp_iwfbt(F.coef2native(insig,size(insig)),...
                                F.g.nodes(F.wtPath(end:-1:1)),...
                                [nodesInLen(F.wtPath(end:-1:1),size(insig,1)/F.red,1,F.g);size(insig,1)/F.red],...
                                F.rangeLoc(end:-1:1),F.rangeOut(end:-1:1),...
                                'per');
    F.length=@(Ls) wfbtlength(Ls,F.g);
  case {'wpfbt'}
    F.g=wfbtinit({'strict',F.g});
    F.red = sum(cellfun(@(aEl) sum(1./aEl),nodesSub(nodeBForder(0,F.g),F.g)));
    % comp_ specific
    F.wtPath = nodeBForder(0,F.g);
    F.rangeLoc = nodesLocOutRange(F.wtPath,F.g);
    [F.pOutIdxs,F.chOutIdxs] = treeWpBFrange(F.g);

    F.coef2native = @(coef,s) wavpack2cell(coef,...
                    s(1)./cell2mat(cellfun(@(aEl) aEl(:),...
                    reshape(nodesSub(nodeBForder(0,F.g),F.g),[],1),...
                    'UniformOutput',0))./F.red);
    F.native2coef = @(coef) wavcell2pack(coef);

    F.frana=@(insig) F.native2coef(...
                        comp_wpfbt(insig,F.g.nodes(F.wtPath),...
                                   F.rangeLoc,'per',F.flags.interscaling));
    F.frsyn=@(insig) comp_iwpfbt(F.coef2native(insig,size(insig)),...
                                 F.g.nodes(F.wtPath(end:-1:1)),...
                                 F.pOutIdxs,F.chOutIdxs,...
                                 size(insig,1)/F.red,...
                                 'per',F.flags.interscaling);
    F.length=@(Ls) wfbtlength(Ls,F.g);
  case {'ufwt'}
    F.g=fwtinit({'strict',F.g});
    F.coef2native = @(coef,s) reshape(coef,[s(1)/(F.J*(numel(F.g.a)-1)+1),F.J*(numel(F.g.a)-1)+1,s(2)]);
    F.native2coef = @(coef) reshape(coef,[size(coef,1)*size(coef,2),size(coef,3)]);
    F.frana=@(insig) F.native2coef(comp_ufwt(insig,F.g.h,F.g.a,F.J,F.flags.scaling));
    F.frsyn=@(insig) comp_iufwt(F.coef2native(insig,size(insig)),F.g.g,F.g.a,F.J,F.flags.scaling);
    F.length=@(Ls) Ls;
    F.red=(F.J*(numel(F.g.a)-1)+1);
  case {'uwfbt'}
    F.g=wfbtinit({'strict',F.g});

    % comp_ specific
    [F.wtPath, F.rangeLoc, F.rangeOut] = treeBFranges(F.g);
    F.nodesUps = nodesFiltUps(F.wtPath,F.g);
    F.red = sum(cellfun(@numel,F.rangeOut));

    F.coef2native = @(coef,s) reshape(coef,[s(1)/F.red,F.red,s(2)]);
    F.native2coef = @(coef) reshape(coef,[size(coef,1)*size(coef,2),size(coef,3)]);
    F.frana=@(insig) F.native2coef(...
                        comp_uwfbt(insig,F.g.nodes(F.wtPath),F.nodesUps,...
                                   F.rangeLoc,F.rangeOut,F.flags.scaling));
    F.frsyn=@(insig) comp_iuwfbt(F.coef2native(insig,size(insig)),...
                                 F.g.nodes(F.wtPath(end:-1:1)),...
                                 F.nodesUps(end:-1:1),F.rangeLoc(end:-1:1),...
                                 F.rangeOut(end:-1:1),F.flags.scaling);
    F.length=@(Ls) Ls;
  case {'uwpfbt'}
    F.g= wfbtinit({'strict',varargin{1}});
    F.red = sum(cellfun(@(fEl) numel(fEl.g),F.g.nodes));
    % comp_ specific
    F.wtPath = nodeBForder(0,F.g);
    F.nodesUps = nodesFiltUps(F.wtPath,F.g);
    F.rangeLoc = nodesLocOutRange(F.wtPath,F.g);
    [F.pOutIdxs,F.chOutIdxs] = treeWpBFrange(F.g);

    F.coef2native = @(coef,s) reshape(coef,[s(1)/F.red,F.red,s(2)]);
    F.native2coef = @(coef) reshape(coef,[size(coef,1)*size(coef,2),size(coef,3)]);
    F.frana=@(insig) F.native2coef(...
                        comp_uwpfbt(insig,F.g.nodes(F.wtPath),F.rangeLoc,...
                                    F.nodesUps,F.flags.scaling,...
                                    F.flags.interscaling));
    F.frsyn=@(insig) comp_iuwpfbt(F.coef2native(insig,size(insig)),...
                                  F.g.nodes(F.wtPath(end:-1:1)),...
                                  F.nodesUps(end:-1:1),F.pOutIdxs,F.chOutIdxs,...
                                  F.flags.scaling,F.flags.interscaling);
    F.length=@(Ls) Ls;

  %%%%%%%%%%%%%%%%%%%%
  %% WRAPPER FRAMES %%
  %%%%%%%%%%%%%%%%%%%%
  case {'erbletfb','cqtfb'}
    switch(ftype)
        case 'erbletfb'
            [g,a,~,L] = erbfilters(varargin{:});
        case 'cqtfb'
            [g,a,~,L] = cqtfilters(varargin{:});
    end
    % Search for the 'complex' flag
    do_complex = ~isempty(varargin(strcmp('complex',varargin)));
    if do_complex
       F = frameaccel(frame('filterbank',g,a,numel(g)),L);
    else
       F = frameaccel(frame('filterbankreal',g,a,numel(g)),L);
    end
    F.fixedlength = 1;

  otherwise
    error('%s: Unknown frame type: %s',upper(mfilename),ftype);

end;

% This one is placed at the end, to allow for F.red to be defined
% first.
if ~isfield(F,'lengthcoef')
    F.lengthcoef=@(Ncoef) Ncoef/framered(F);
end;


function [v1,v2,v3,v4,v5,v6,v7]=lsqnonlinSym(funhandle,varargin)
% LSQNONLINSYM   A wrap of lsqnonlin to use symbolic toolbox to obtain Jacobian.
% Limitation: The cost function and nonlinear constraint function cannot
% have conditional operation, such as if, max, min etc.
% Its usage is the same as the original lsqnonlin.
%
% Example: see lsqnonlinSym_example
%
% By Yi Cao, Cranfield University, 25/09/2007
%
x0=varargin{1};
n=numel(x0);

x=[];
for k=1:n
    eval(['syms x' num2str(k)]);
    eval(['x=[x;x' num2str(k) '];']);
end

nv=numel(varargin);
if nv>4
    F=feval(funhandle,x,varargin(5:end));
else
    F=feval(funhandle,x);
end
F=simplify(F);
Fx=simplify(jacobian(F,x));

if nv>=4
    opt = varargin{4};
    opt = optimset(opt,'jacobian','on');
else
    opt = optimset('jacobian','on');
end

switch numel(varargin)
    case 1
        [v1,v2,v3,v4,v5,v6,v7]=lsqnonlin(@symFun,x0,[],[],opt,F,Fx);
    case 2
        LB=varargin{2};
        [v1,v2,v3,v4,v5,v6,v7]=lsqnonlin(@symFun,x0,LB,[],opt,F,Fx);
    otherwise
        LB=varargin{2};
        UB=varargin{3};
        [v1,v2,v3,v4,v5,v6,v7]=lsqnonlin(@symFun,x0,LB,UB,opt,F,Fx);
end

function [E,J]=symFun(x0,F,Fx)
n=numel(x0);
for k=1:n
    eval(['x' num2str(k) '=x0(k);']);
end
E = eval(F);
if nargout>1
    J  = eval(Fx);
end


function [r,ir,iu] = unique(r,varargin)
% disjoint list of rotations
%
% Syntax
%   u = unique(r)
%   u = unique(r,'tolerance',0.01)
%   [u,ir,iu] = unique(r)
%
% Input
%  r   - @rotation
%  tol - double (default 1e-3)
%
% Output
%  u - @rotation
%  ir - index such that u = r(ir)
%  iu - index such that r = u(iu)
%
% Flags
%  stable - prevent sorting
%
% See also
% unique
%

a = r.a(:); b = r.b(:); c = r.c(:); d = r.d(:); i = r.i(:);

if length(r) < 1000 && ~check_option(varargin,'tolerance')

  abcd = [a.^2,b.^2,c.^2,d.^2,a.*b,a.*c,a.*d,b.*c,b.*d,c.*d, i(:)];

  tol = get_option(varargin,'tolerance',1e-3);

  % in case it should not be sorted
  if check_option(varargin,'stable')
    [~,ir,iu] = unique(round(abcd./tol),'rows','stable');
  else
    [~,ir,iu] = uniquetol(abcd,tol,'ByRows',true,'DataScale',1);
  end

else % faster but less accurate

  tol = get_option(varargin,'tolerance',5*degree);

  % ensure upper hemisphere
  ind = d < 0;
  a(ind) = -a(ind); b(ind) = -b(ind);
  c(ind) = -c(ind); d(ind) = -d(ind);

  [~,ir,iu] = uniquetol([a,b,c,d,i],tol / 100 / degree, ...
    'ByRows',true,'DataScale',1);

end

% remove duplicated points
r.a = a(ir);
r.b = b(ir);
r.c = c(ir);
r.d = d(ir);
r.i = i(ir);


function K = processConstraintSparse(K, bound)
% 这个函数对K进行处理，使其满足约束
% 用于稀疏矩阵方法
% 输入：
%      整体劲度矩阵 K
%      边界条件，格式参考README.MD
% 输出：
%      处理后的 K

for i = 1 : size(bound, 1)
    % 对所有的约束进行处理
    element_num = bound(i, 1);
    x_or_y      = bound(i, 2);
    j           = element_num * 2 - 2 + x_or_y;
    K(j, :) = 0;
    K(:, j) = 0;
    K(j, j) = 1;
end


function unit = seconds2unit(seconds,seconds0,seconds1)
% Convert seconds past midnight to unit times
%
% USAGE:
%   [UNIT] = seconds2unit(SECONDS,SECONDS0,SECONDS1)
%
% INPUTS:
%   SECONDS - m by 1 column vector of times expressed as seconds past midnight
%               (e.g. 1:00:00 is 3600, 12:00:15 is 43215) where m>=2
%   SECONDS0   - Base time, maps to 0 un the unit interval
%   SECONDS1   - End time, maps to 1 un the unit interval
%
% OUTPUTS:
%   UNIT    - m by 1 column vector of times measured as fraction of time
%               between min(SECONDS) and max(SECONDS)
%
% COMMENTS:
%   This is a helper function for REALIZED_KERNEL
%
%  See also WALL2UNIT, WALL2SECONDS, SECONDS2WALL, REALIZED_KERNEL, REALIZED_VARIANCE

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 1    Date: 5/1/2008

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if nargin~=3
    error('Three inputs required.')
end
if any(seconds<0)
    error('SECONDS must be non-negative');
end
if seconds0<0
    error('SECONDS0 must be non-negative');
end
if seconds1<0
    error('SECONDS1 must be non-negative');
end

if (seconds1-seconds0)<eps
    error('SECONDS1 must be larger than SECONDS0.');
end

if size(seconds,2)>size(seconds,1)
    seconds=seconds';
end

seconds = double(seconds);
seconds0 = double(seconds0);
seconds1 = double(seconds1);

if size(seconds,2)>1
    error('SECONDS must be an m by 1 column vector');
end
if length(seconds)<2
    error('SECONDS must contain at least 2 elements');
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
unit=(seconds-seconds0)/(seconds1 - seconds0);


function test_ft_spiketriggeredspectrum_stat()

% MEM 2gb
% WALLTIME 00:10:00
% DEPENDENCY ft_spiketriggeredspectrum_stat ft_spiketriggeredspectrum_tfr

nSpikes = 10000;
randPhases = [];
kappa = 1;
nChans = 3;
for iChan = 1:nChans
  for iFreq = 1:6
    randPhases(:,iChan,iFreq) = randnwrap(nSpikes+1,kappa/iChan);
  end
end
figure, rose(randPhases(:,1,1))
sts = [];
sts.fourierspctrm{1} = rand(size(randPhases)).*exp(1i*randPhases); % random weighting
sts.trial{1}     = sort(ceil(rand(1,nSpikes)*100));
sts.time{1}         = rand(1,nSpikes);
for iChan = 1:nChans
  sts.lfplabel{iChan} = strcat('chan', num2str(iChan));
end
sts.cfg = [];
sts.dimord         = 'rpt_chan_freq';
sts.freq           = 10:10:60;
sts.label   = {'unit1'};
sts.trialtime      = repmat([0,1],[100 1]);

cfg = [];
cfg.timwin = 0.5;
cfg.winstepsize = 0.001;
cfg.method = 'ppc0';
sts_tfr = ft_spiketriggeredspectrum_stat(cfg,sts);
figure, imagesc(sts_tfr.time, sts_tfr.freq, squeeze(sts_tfr.ppc0(1,:,:))), colorbar
%
cfg.method = 'ppc1';
sts_tfr = ft_spiketriggeredspectrum_stat(cfg,sts);

figure, imagesc(sts_tfr.time, sts_tfr.freq, squeeze(sts_tfr.ppc1(1,:,:))), colorbar
%
cfg = [];
cfg.method = 'ppc1';
sts_tfr = ft_spiketriggeredspectrum_stat(cfg,sts);

figure, plot(sts_tfr.freq, squeeze(sts_tfr.ppc1(1,:,:))), colorbar

%%
% now make a case where no phase locking should be present

% ft_spiketriggeredspectrum_tfr
nSpikes = 10000;
randPhases = [];
kappa = 1;
nChans = 3;
for iChan = 1:nChans
  for iFreq = 1:6
    randPhases(:,iChan,iFreq) = randnwrap(nSpikes+1,0.001/iChan);
  end
end
figure, rose(randPhases(:,1,1))
sts = [];
sts.fourierspctrm{1} = rand(size(randPhases)).*exp(i*randPhases); % random weighting
sts.trial{1}     = sort(ceil(rand(1,nSpikes)*100));
sts.time{1}         = rand(1,nSpikes);
for iChan = 1:nChans
  sts.lfplabel{iChan} = strcat('chan', num2str(iChan));
end
sts.cfg = [];
sts.dimord         = 'rpt_chan_freq';
sts.freq           = 10:10:60;
sts.label   = {'unit1'};
sts.trialtime      = repmat([0,1],[100 1]);

cfg = [];
cfg.timwin = 0.5;
cfg.winstepsize = 0.001;
cfg.method = 'ppc0';
sts_tfr = ft_spiketriggeredspectrum_stat(cfg,sts);

figure, imagesc(sts_tfr.time, sts_tfr.freq, squeeze(sts_tfr.ppc0(1,:,:))), colorbar
function r=randnwrap(n,k)
r=angle(exp(1i*randn(n,1)*sqrt(1/k)));


function pass = test_hermpts(pref)

if ( nargin == 0 )
    pref = chebfunpref();
end

% TODO: These values were computed in V4. Should perhaps check more carefully?

% Choose a tolerance:
tol = 10*pref.chebfuneps;

% Test a small n (using REC)
n = 42;
[x] = hermpts(n);
pass(1) = all(size(x) == [n, 1]);
[x, w, v] = hermpts(n);
pass(2) = all(size(x) == [n, 1]) && all(size(w) == [1, n]) && ...
    all(size(v) == [n, 1]);
pass(3) = abs(w*x) < tol && abs(w*x.^2 - sqrt(pi)/2) < tol;
pass(4) = abs(x(37) - 5.660357581283058) < 10*tol;
pass(5) = abs(w(17) - 0.032202101288908) < tol;
pass(6) = abs(v(17) - 0.311886101735772) < tol;

% Test a larger n (using ASY)
n = 251;
[x] = hermpts(n);
pass(11) = all(size(x) == [n, 1]);
[x, w, v] = hermpts(n);
pass(7) = all(size(x) == [n, 1]) && all(size(w) == [1, n]) && ...
    all(size(v) == [n, 1]);
pass(8) = abs(w*x) < tol && abs(w*x.^2 - sqrt(pi)/2) < 300*tol;
pass(9) = abs(x(37) - -13.292221459334638) < 4*tol;
pass(10) = abs(w(123) - 0.117419270715955) < 10*tol;
pass(11) = abs(v(123) - 0.915560323259764) < 100*tol;

% Test on 'prob':
[x2, w2, v2] = hermpts(n, 'prob');
pass(12) = norm(x - x2./sqrt(2), inf) < tol;
pass(13) = norm(w - w2./sqrt(2), inf) < tol;

[x3, w3, v3] = hermpts(n, 'phys');
pass(14) = norm(x - x3, inf) == 0;
pass(15) = norm(w - w3, inf) == 0;

end


clear all; close all;
I=imread('circbw.tif');
J=watershed(I, 8);
figure;
subplot(121);  imshow(I);
subplot(122);  imshow(J);


function [vec, outnames] = contrast_code(vec)
% :Usage:
% ::
%
%     [vec,outnames] = contrast_code(vec)
%
% Changes values to 1, -1, or 0 for contrast coding

outnames = [];

if iscell(vec) && ~isempty(vec)
    [indic, names]  = string2indicator(vec);

    for i = 2:length(names)
        vec = indic(:, i - 1) - indic(:, i);
        outnames{i - 1} = [names{i - 1} ' - ' names{i}];
    end

elseif ~isempty(vec)
    wh = find(vec > 0);

    vec(wh) = 1;

    wh = find(vec < 0);

    vec(wh) = -1;

end

return


function [ ishow, list_num, list, nregion ] = voxels_region_3d ( maxlist, ...
  nx, ny, nz, ishow )

%*****************************************************************************80
%
%% VOXELS_REGION_3D arranges a set of voxels into contiguous regions in 3D.
%
%  Discussion:
%
%    On input, the ISHOW array contains zero and nonzero values.  The nonzero
%    values are taken to be active voxels.  On output, the zero voxels remain
%    zero, and all the active voxels have been assigned a value which now
%    indicates membership in a region, or group of contiguous voxels.
%
%    On output, the array LIST contains information about the regions.
%    The last used element of LIST is LIST_NUM.
%
%    The number of elements in region NREGION is NELEM = LIST(LIST_NUM).
%    The (I,J,K) indices of the last element in this region are in
%    LIST(LIST_NUM-3) through LIST(LIST_NUM-1), and the first element is
%    listed in LIST(LIST_NUM-3*NELEM), LIST(LIST_NUM-3*NELEM+1),
%    LIST(LIST_NUM-3*NELEM+2).
%
%    The number of elements in NREGION-1 is listed in LIST(LIST_NUM-3*NELEM-1),
%    and the (I,J,K) indices of the these elements are listed there.
%
%  Picture:
%
%    Input:
%
%      0  2  0  0 17  0  3
%      0  0  3  0  1  0  4
%      1  0  4  8  8  0  7
%      3  0  6 45  0  0  0
%      3 17  0  5  9  2  5
%
%    Output:
%
%      0  1  0  0  2  0  3
%      0  0  2  0  2  0  3
%      4  0  2  2  2  0  3
%      4  0  2  2  0  0  0
%      4  4  0  2  2  2  2
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer MAXLIST, the maximum length of the array used to
%    list the elements of the regions.
%
%    Input, integer NX, NY, NZ, the number of voxels in the X, Y and
%    Z directions.
%
%    Input, integer ISHOW(NX,NY,NZ), the only significance to
%    the entries is whether they are zero or nonzero.
%
%    Output, integer ISHOW(NX,NY,NZ), the nonzero entries have now been revalued
%    so that contiguous entries have the same value, indicating a grouping into
%    a region.
%
%    Output, integer LIST_NUM, the number of entries of LIST that were used.
%    However, if MAXLIST < LIST_NUM, then there was not enough space in
%    LIST to store the data properly, and LIST should not be used,
%    although the data in ISHOW should be correct.
%
%    Output, integer LIST(MAXLIST), contains, in stack form, a list
%    of the indices of the elements in each region.
%
%    Output, integer NREGION, the number of regions discovered.
%

%
%  Reset all nonzero entries of ISHOW to -1.
%
  for i = 1 : nx
    for j = 1 : ny
      for k = 1 : nz
        if ( ishow(i,j,k) ~= 0 )
          ishow(i,j,k) = -1;
        end
      end
    end
  end
%
%  Start the number of items in the region list at 0.
%
  list_num = 0;
%
%  Start the number of regions at 0.
%
  nregion = 0;
%
%  The stack begins empty.
%
  nstack = 0;
%
%  Search for an unused "ON" voxel from which we can "grow" a new region.
%
  for i = 1 : nx
    for j = 1 : ny
      for k = 1 : nz
%
%  We found a voxel that is "ON", and does not belong to any region.
%
        if ( ishow(i,j,k) == -1 )
%
%  Increase the number of regions.
%
          nregion = nregion + 1;
%
%  Add this voxel to the region.
%
          ishow(i,j,k) = nregion;
%
%  Add this voxel to the stack.
%
          stack(nstack+1) = i;
          stack(nstack+2) = j;
          stack(nstack+3) = k;

          stack(nstack+4) = 1;

          nstack = nstack + 4;
%
%  Add this voxel to the description of the region.
%
          nelements = 1;

          if ( list_num + 3 <= maxlist )
            list(list_num+1) = i;
            list(list_num+2) = j;
            list(list_num+3) = k;
          end

          list_num = list_num + 3;

          while ( 1 )
%
%  Find all neighbors of BASE that are "ON" but unused.
%  Mark them as belonging to this region, and stack their indices.
%
            ibase = stack(nstack-3);
            jbase = stack(nstack-2);
            kbase = stack(nstack-1);

            ilo = max ( ibase-1, 1 );
            ihi = min ( ibase+1, nx );
            jlo = max ( jbase-1, 1 );
            jhi = min ( jbase+1, ny );
            klo = max ( kbase-1, 1 );
            khi = min ( kbase+1, nz );

            nabes = 0;

            for i2 = ilo : ihi
              for j2 = jlo : jhi
                for k2 = klo : khi
%
%  We found a neighbor to our current search point, which is "ON" and unused.
%
                  if ( ishow(i2,j2,k2) == -1 )
%
%  Increase the number of neighbors.
%
                    nabes = nabes + 1;
%
%  Mark the neighbor as belonging to the region.
%
                    ishow(i2,j2,k2) = nregion;
%
%  Add the neighbor to the stack.
%
                    stack(nstack+1) = i2;
                    stack(nstack+2) = j2;
                    stack(nstack+3) = k2;

                    nstack = nstack + 3;
%
%  Add the neighbor to the description of the region.
%
                    nelements = nelements + 1;

                    if ( list_num + 3 <= maxlist )
                      list(list_num+1) = i2;
                      list(list_num+2) = j2;
                      list(list_num+3) = k2;
                    end

                    list_num = list_num + 3;

                  end
                end
              end
            end
%
%  If any new neighbors were found, take the last one as the basis
%  for a deeper search.
%
            if ( 0 < nabes )
              stack(nstack+1) = nabes;
              nstack = nstack + 1;
              continue
            end
%
%  If the current search point had no new neighbors, drop it from the stack.
%
            ncan = stack(nstack) - 1;
            nstack = nstack - 3;
            stack(nstack) = ncan;
%
%  If there are still any unused candidates at this level, take the
%  last one as the basis for a deeper search.
%
            if ( 0 < stack(nstack) )
              continue
            end
%
%  If there are no more unused candidates at this level, then we need
%  to back up a level in the stack.  If there are any candidates at
%  that earlier level, then we can still do more searching.
%
            nstack = nstack - 1;

            if ( nstack <= 0 )
              break
            end

          end
%
%  If we have exhausted the stack, we have completed this region.
%  Tag the number of elements to the end of the region description list.
%
          list_num = list_num + 1;
          if ( list_num <= maxlist )
            list(list_num) = nelements;
          end

        end

      end
    end
  end
%
%  Print some warnings.
%
  if ( maxlist < list_num )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'VOXELS_REGION - Warning!\n' );
    fprintf ( 1, '  MAXLIST was too small to list the regions.\n' );
    fprintf ( 1, '  Do not try to use the LIST array!\n' );
    fprintf ( 1, '  The ISHOW data is OK, however.\n' );
  end

  return
end


function A = metric_05 ( p )

%*****************************************************************************80
%
%% METRIC_05 evaluates metric #5 at any point.
%
%  Discussion:
%
%    This routine evaluates the matrix that determines the metric
%    at a point.
%
%    It is not diagonal.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 May 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real P(2), the point at which the metric matrix is to
%    be evaluated.
%
%    Output, real A[2,2], the metric matrix.
%
  A = [ 2.0, 3.0; 3.0, 5.0 ];

  return
end


function [xsecx,xsecy] = lc_xsect(eqlat,eqlon,depth,width,length,...
        lat1,lon1,lat2,lon2)

    %LC_XSECTION
    %
    %	[xsecx, xsecy] = LC_xsection(eqlat,eqlon,depth,width,length,...
    %                                        lat1,lon1,lat2,lon2)        (1)
    %
    %	[xsecx, xsecy] = LC_xsection(eqlat,eqlon,depth,width,length,...
    %                                        lat0,lon0,azimuth)          (2)
    %
    %	[xsecx, xsecy] = LC_xsection(eqlat,eqlon,depth,width)         (3)
    %
    %	Function to make a cross section of data points on a map
    %	created by LC_MAP (Lambert Conformal).
    %	The WIDTH of the zone from which the data points is given
    %	in "km" and represent the total width (1/2 on one side, 1/2 on
    %	the other side).
    %	The LENGTH of the xsection in "km" is only used in method (2),
    %	but this argument is still neccessary in argument list of
    %	method (1) in order to keep the argument list in the right order.
    %	The data points location are given by EQLAT, EQLON and DEPTH.
    %	The cross section location can be given in any one of three ways:
    %
    %	  (1) given latitudes and longitudes of two points on the map,
    %             using the arguments as described above.
    %
    %	  (2) given latitude and longitude of a center point and an azimuth.
    %
    %	  (3) using the cursor to select two points on the map by clicking
    %	      a mouse button above the desired points.
    %
    %	If the output argument is used, the distance-depth data is kept
    %	in variables for other use; otherwise the xsection will be plotted
    %	on a new figure window.
    %
    %	It is possible to set the symbol type, size and line width by
    %	setting the following global variables:
    %	"symb_type", "symb_size" and "symb_width" respectively. Otherwise
    %	it will use the defaults.
    %
    %	It is also possible to set the minimum and maximum depth of the
    %	cross-section by setting the following global variables:
    %	"mindepth" and "maxdepth".  If either or both are not set, it
    %	will use 0 km as the minimum depth and/or the depth of the deepest
    %	data point as the maximum depth.
    %
    %	NOTE:
    %	It is assumed that LC_MAP was used before using this function!
    %	This is neccessary to set global variables used by this function.

    report_this_filefun(mfilename('fullpath'));

    global torad Re scale
    global sine_phi0 phi0 lambda0 phi1 phi2
    global maxlatg minlatg maxlong minlong
    global symb_type symb_size symb_width
    global label1 label2
    global mindepth maxdepth

    todeg = 180 / pi;

    if nargin < 9

        if nargin == 8	% method 2: given lat & lon of center point and angle

            lat0 = lat1;
            lon0 = lon1;
            [x0, y0] = lc_tocart(lat0,lon0);
            azimuth = lat2;

            if azimuth >= 180, azimuth = azimuth - 180; end
            theta0 = ((lon0*torad - lambda0) * sine_phi0) * todeg;
            alpha = azimuth - theta0;
            beta = (90 - azimuth) + theta0;

            x2 = ((length / 2) * cos(beta*torad));
            y2 = ((length / 2) * sin(beta*torad));
            x1 = x0 - x2;
            y1 = y0 - y2;
            x2 = x0 + x2;
            y2 = y0 + y2;

            [lat1, lon1] = lc_froca(x1,y1);
            [lat2, lon2] = lc_froca(x2,y2);

        elseif nargin == 4	% method 3: selection of the end points by mouse

            limits = ginput(2);
            x1 = limits(1,1);
            y1 = limits(1,2);
            x2 = limits(2,1);
            y2 = limits(2,2);

            if x1 > x2
                xtemp = x1; ytemp = y1;
                x1 = x2; y1 = y2;
                x2 = xtemp; y2 = ytemp;
            end

            [lat1, lon1] = lc_froca(x1,y1);
            [lat2, lon2] = lc_froca(x2,y2);

            x0 = (x1 + x2) / 2;
            y0 = (y1 + y2) / 2;
            [lat0, lon0] = lc_froca(x0,y0);
            dx = x2 - x1;
            dy = y2 - y1;

            alpha = 90 - (atan(dy/dx)*todeg);
            length = sqrt(dx^2 + dy^2);

        else
            disp('ERROR: incompatible number of arguments')
            help lc_xsection
            return
        end

    elseif nargin == 9	% method 1: given lat & lon of the two end points

        [x1, y1] = lc_tocart(lat1,lon1);
        [x2, y2] = lc_tocart(lat2,lon2);

        if x1 > x2
            xtemp = x1; ytemp = y1;
            x1 = x2; y1 = y2;
            x2 = xtemp; y2 = ytemp;
        end

        x0 = (x1 + x2) / 2;
        y0 = (y1 + y2) / 2;
        [lat0, lon0] = lc_froca(x0,y0);
        dx = x2 - x1;
        dy = y2 - y1;

        alpha = 90 - (atan(dy/dx)*todeg);
        length = sqrt(dx^2 + dy^2);

    else

        disp('ERROR: incompatible number of arguments')
        help lc_xsection
        return
    end

    % correction factor to correct for longitude away from the center meridian
    theta0 = ((lon0*torad - lambda0) * sine_phi0) * todeg;

    % correct the XY azimuth of the Xsection line with the above factor to obtain
    % the true azimuth
    azimuth = alpha + theta0;
    if azimuth < 0, azimuth = azimuth + 180; end

    % convert XY coordinate azimuth to a normal angle like we used to deal with
    sigma = 90 - alpha;

    % transformation matrix to rotate the data coordinate w.r.t the Xsection line
    transf = [cos(sigma*torad) sin(sigma*torad)
        -sin(sigma*torad) cos(sigma*torad)];

    % inverse transformation matrix to rotate the data coordinate back
    invtransf = [cos(-sigma*torad) sin(-sigma*torad)
        -sin(-sigma*torad) cos(-sigma*torad)];

    % convert the map coordinate of the events to cartesian coordinates
    idx_map = find(minlatg < eqlat & eqlat < maxlatg & ...
        minlong < eqlon & eqlon < maxlong);
    [eq(1,:) eq(2,:)] = lc_tocart(eqlat,eqlon);

    % create new coordinate system at center of Xsection line
    eq0(1,:) = eq(1,:) - x0;
    eq0(2,:) = eq(2,:) - y0;

    % rotate this last coordinate system so that X-axis correspond to Xsection line
    eq0p = transf * eq0;

    % project the event data to the Xsection line
    eq1(1,:) = eq0p(1,:);
    eq1(2,:) = eq0p(2,:) * 0;

    % convert back to the original coordinate system
    eq1p = invtransf * eq1;
    eq2(1,:) = eq1p(1,:) + x0;
    eq2(2,:) = eq1p(2,:) + y0;

    % plot the Xsection line on the map
    plot([x1 x2],[y1 y2],'--','LineWidth',1.5)

    % label the Xsection end points
    xlim = get(gca,'XLim');
    ylim = get(gca,'YLim');
    label_dist = (2 / 100) * sqrt((2*xlim(2))^2 + (2*ylim(2))^2);
    label_pt(1,1) = -(length/2 + label_dist);
    label_pt(2,1) = 0;
    label_pt(1,2) = length/2 + label_dist;
    label_pt(2,2) = 0;
    rlabel_pt = invtransf * label_pt;
    label_pt(1,:) = rlabel_pt(1,:) + x0;
    label_pt(2,:) = rlabel_pt(2,:) + y0;
    if isempty(label1), label1 = 'A'; end
    if isempty(label2), label2 = 'A'''; end
    lbl1_h = text(label_pt(1,1),label_pt(2,1),label1,'FontSize',14,...
        'Vertical','middle','Horizontal','center','FontWeight','bold');
    lbl2_h = text(label_pt(1,2),label_pt(2,2),label2,'FontSize',14,...
        'Vertical','middle','Horizontal','center','FontWeight','bold');

    % create a box of width "width" around the Xsection line and plot it
    box(1,1) = -length/2; box(2,1) = width/2;
    box(1,2) = length/2; box(2,2) = width/2;
    box(1,3) = length/2; box(2,3) = -width/2;
    box(1,4) = -length/2; box(2,4) = -width/2;
    xbox = [box(1,:) box(1,1)];
    ybox = [box(2,:) box(2,1)];
    rbox = invtransf * [xbox ; ybox];
    rbox(1,:) = rbox(1,:) + x0;
    rbox(2,:) = rbox(2,:) + y0;
    plot(rbox(1,:),rbox(2,:),'-.k','LineWidth',0.8)

    % check if symbol parameters global variables are set, if not --> defaults
    if isempty(symb_type), symb_type = '+'; end
    if isempty(symb_size), symb_size = 6; end
    if isempty(symb_width), symb_width = [0.5]; end

    % plot the events on the map
    plot(eq(1,idx_map),eq(2,idx_map),symb_type,'MarkerSize',symb_size,...
        'LineWidth',symb_width)

    % find index of all events which are within the given box width
    idx_box = find(abs(eq0p(2,:)) <= width/2 & abs(eq0p(1,:)) <= length/2);

    % Open another graphic window for the cross section
    map_fig = gcf;
    xsec_fig = map_fig + 1;
    figure_w_normalized_uicontrolunits(xsec_fig)
    set(xsec_fig,'PaperPosition',[1 .5 9 6.9545])

    % Plot events on cross section figure
    xdist = eq1(1,idx_box) + (length / 2);

    global Xwbz Ywbz
    Xwbz = xdist;
    Ywbz = depth(idx_box);

    xsecx = xdist;
    xsecy = depth(idx_box);

    plot(xdist,depth(idx_box),symb_type,'MarkerSize',symb_size,...
        'LineWidth',symb_width,'YDir','reverse')

    if isempty(maxdepth)
        maxZ = max(depth(idx_box));
    else
        maxZ = maxdepth;
    end

    if isempty(mindepth)
        minZ = 0;
    else
        minZ = mindepth;
    end

    if length > (maxZ - minZ)*11/8.5
        position = [.1 .1 .7 ((maxZ-minZ)/length)*0.7*11/8.5];
    else
        position = [.1 .1 (length/(maxZ-minZ))*0.7*8.5/11 .7];
    end
    set(gca,'Position',position,'XLim',[0 length],'Ylim',[-maxZ -minZ],...
        'LineWidth',2)

    % Plot labels
    Xstring = ['Distance from ' label1 ' (km)'];
    set(gca,'XLabel',text(0,0,Xstring),'YLabel',text(0,0,'Depth (km)'))
    label_base1 = 1 + .04;
    label_base2 = 1 + .06;
    label_base3 = 1 + .08;
    lbl3_h = text(0,label_base2,label1,'FontSize',14,'Horizontal','center',...
        'FontWeight','bold','Vertical','middle','Units','norm');
    lat1_dm = sprintf('    %2.2i N %4.2f''',fix(lat1),(frac(lat1)*60));
    lon1_dm = sprintf('   %3.3i W %4.2f''',abs(fix(lon1)),abs(frac(lon1)*60));
    lbl5_h = text(0,label_base1,lat1_dm,'FontSize',12,'Horizontal','left',...
        'Vertical','bottom','Units','norm');
    lbl6_h = text(0,label_base3,lon1_dm,'FontSize',12,'Horizontal','left',...
        'Vertical','bottom','Units','norm');

    lbl4_h = text(1,label_base2,label2,'FontSize',14,'Horizontal','center',...
        'FontWeight','bold','Vertical','middle','Units','norm');
    lat2_dm = sprintf('%2.2i N %4.2f''    ',fix(lat2),(frac(lat2)*60));
    lon2_dm = sprintf('%3.3i W %4.2f''    ',abs(fix(lon2)),abs(frac(lon2)*60));
    lbl7_h = text(1,label_base1,lat2_dm,'FontSize',12,...
        'Horizontal','right','Vertical','bottom','Units','norm');
    lbl8_h = text(1,label_base3,lon2_dm,'FontSize',12,...
        'Horizontal','right','Vertical','bottom','Units','norm');

    % Go back to map figure
    %figure_w_normalized_uicontrolunits(map_fig)


close all
clear

%% Setup and Parameters
x0 = [-5; -5; 0; 0];
time_total = 20.0;
dt = 0.2;
P = 100*eye(4);
Q = 10*eye(4);
R = eye(2);
N = 8;
xmin = [-5; -5; -5; -5];
xmax = [5; 5; 5; 5];
umin = [-1; -1];
umax = [1; 1];

%% Discrete-time double integrator 2D
system.dt = dt;
system.A = [1 0 dt 0;
    0 1 0 dt;
    0 0 1 0;
    0 0 0 1];
system.B = [0.5*dt^2 0;
    0 0.5*dt^2;
    dt 0;
    0 dt];
system.xl = xmin;
system.xu = xmax;
system.ul = umin;
system.uu = umax;

%% MPC-CBF parameters
params.Q = Q;
params.R = R;
params.P = P;
params.N = N;
params.gamma = 0.5;

%% Obstacle
obs.pos = [-2; -2.25];
obs.r = 1.5;

%% Simulate MPC-CBF
gamma_list = linspace(0.1, 0.5, 5);
controller_mpc_cbf_list = {};
for i = 1:size(gamma_list, 2)
    new_params = params;
    new_params.N = 5;
    new_params.gamma = gamma_list(i);
    controller_mpc_cbf = MPCCBF(x0, system, new_params);
    controller_mpc_cbf.obs = obs;
    controller_mpc_cbf.sim(time_total);
    controller_mpc_cbf_list{i} = controller_mpc_cbf;
end

%% Computational time benchmark
for i = 1:size(gamma_list, 2)
    controller_mpc_cbf = controller_mpc_cbf_list{i};
    fprintf('Computational time for MPC-CBF3: mean %.3f, std %.3f, min %.3f, max %.3f, input cost %.3f, min dist %f\n',...
    [mean(controller_mpc_cbf.solvertime),...
    std(controller_mpc_cbf.solvertime),...
    min(controller_mpc_cbf.solvertime),...
    max(controller_mpc_cbf.solvertime),...
    controller_mpc_cbf.u_cost,...
    min(controller_mpc_cbf.distlog)]);
end


function [ X ] = special_add_left( X, S )
% add on Lie group-Lie Algebra space
    s_theta=S(1:3);
    s_p=S(4:6);
    sizeS=size(S,1);
    NumberOfLandmarks=(sizeS-6)/3;
    X.position=X.position+X.orientation*jaco_r(-s_theta)*s_p;
    if NumberOfLandmarks>=1
        s_landmarksMatrix=reshape(S(7:end),3,NumberOfLandmarks);
        X.landmarks(1:3,:)=X.landmarks(1:3,:)+X.orientation*jaco_r(-s_theta)*s_landmarksMatrix;
    end
    X.orientation=X.orientation*so3_exp(s_theta);
end


function Offspring = Exploration(Problem,PC,NPC,nND,N)
% Individual exploration in BCE

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

    PCObj  = PC.objs;
    NPCObj = NPC.objs;

    %% Normalization
    fmax   = max(PCObj,[],1);
    fmin   = min(PCObj,[],1);
    PCObj  = (PCObj-repmat(fmin,size(PCObj,1),1))./repmat(fmax-fmin,size(PCObj,1),1);
    NPCObj = (NPCObj-repmat(fmin,size(NPCObj,1),1))./repmat(fmax-fmin,size(NPCObj,1),1);

    %% Determine the size of the niche
    d  = pdist2(PCObj,PCObj);
    d(logical(eye(length(d)))) = inf;
    d  = sort(d,2);
    r0 = mean(d(:,min(3,size(d,2))));
    r  = nND/N*r0;

    %% Detect the solutions in PC to be explored
    d = pdist2(PCObj,NPCObj);
    S = find(sum(d<=r,2)<=1);

    %% Generate new solutions
    if ~isempty(S)
        MatingPool = randi(length(PC),1,length(S));
        Offspring  = OperatorGAhalf(Problem,PC([S',MatingPool]));
    else
        Offspring = [];
    end
end


function [dLdp,g,x] = spm_mci_adjoint (Pr,M,U,Y)
% Gradient of log joint from adjoint method
% FORMAT [dLdp,g,x] = spm_mci_adjoint (Pr,M,U,Y)
%
% Pr        Parameters (vectorised and in M.V subspace)
% M         Model structure
% U         Inputs  [Nin x N]
% Y         Data
%
% dLdp      Gradient    [Np x 1]
% g         Outputs     [N x Nout]
% x         States      [N x Nstates]
%
% If M.adjlike=1 this function returns gradient of log likelihood
%
% This function uses integrators from MATLAB's ODE Suite
%
% B. Sengupta, K. Friston and W. Penny (2014) Efficient Gradient
% Computation for Dynamical Models. Neuroimage,98, 521-527.
%__________________________________________________________________________
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% Will Penny and Biswa Sengupta
% $Id: spm_mci_adjoint.m 6697 2016-01-27 14:57:28Z spm $

try, adjlike=M.adjlike; catch, adjlike=0; end

% Parameters in original space
P = M.V*Pr+M.vpE;

if isempty(U)
    U=zeros(1,M.N);
end

[g,x] = spm_mci_fwd(P,M,U);

% Gradient at computed times
% When computing output sensitivities, assume dydx=L, ie not a
% function of x. Generalise later
[tmp,L]=feval(M.g,M.x0,U(:,1),P,M);
e=Y-g;
djdx=e*M.iCe*L;

% integrate adjoint equation
lambda = spm_mci_adjoint_int(U,P,M,x,djdx);

% If observation function becomes dependent on parameters
% djdp will need updating
djdp = zeros(1,M.Np);

dLdp=zeros(1,M.Np);
for n=1:M.N,
    % Evaluate parameter Jacobian
    if isfield(M,'dfdp')
        Fp = feval(M.dfdp,x(n,:)',U(:,n),P,M);
    else
        Fp = spm_diff(M.f,x(n,:)',U(:,n),P,M,3);
    end
    dLdp=dLdp+djdp-lambda(n,:)*Fp;
end

if ~adjlike
    dlogpriordp = spm_mci_gprior_deriv (Pr,M);
    dLdp=dLdp+dlogpriordp;
end


function Z = spinv(A, B)
% SPINV    Evaluate the sparse inverse matrix
%
% z = sinv(A)  returns the elements of inv(A)_ij, for which A_ij
%      is different from zero.
%
% z = sinv(LD, 1)  returns the elements of inv(A)_ij, for which A_ij
%     is different from zero, and where LD is the LDL cholesky
%     decomposition of A. LD has to be in the form returned by ldlchol
%     in SuiteSparse by Tim Davis.
%
%   Note! If z = sinv(LD, 1) is used LD must not be modified in Matlab
%   after ldlchol. Matlab destroys the symbolic sparsity structure in
%   the Cholesky decomposition, which is needed in the spinv
%   algorithm. If LD is modified the worst scenario is memory corruption.
%
%
%     For details, see:
%     Jarno Vanhatalo and Aki Vehtari (2008). Modelling local and
%     global phenomena with sparse Gaussian processes. Proceedings of
%     the 24th Conference on Uncertainty in Artificial Intelligence
%

% Copyright (c) 2008-2010      Jarno Vanhatalo

% This software is distributed under the GNU General Public
% License (version 2 or later); please refer to the file
% License.txt, included with the software, for details.

warning('Using M-file. Compile MEX-file!');

    n = size(A,1);

    if nargin == 1
        [LD, p, q] = ldlchol(A);
    else
        LD = A;
    end

    [I,J,ld] = find(LD);
    temp = [I(:) J(:) ; J(:) I(:)];
    temp = sortrows(unique(temp,'rows'),2);
    Iz = temp(:,1); Jz = temp(:,2);

    % Find the column starting points
    a1=zeros(n,1);
    a2 = cumsum(histc(J,1:n));
    a1(1) = 1; a1(2:end) = a2(1:end-1) + 1;
    az1=zeros(n,1);
    az2 = cumsum(histc(Jz,1:n));
    az1(1) = 1; az1(2:end) = az2(1:end-1) + 1;

    for j=1:n
        indaz{j} = az1(j):az2(j);
        indIz{j} = Iz(indaz{j})';
    end

    % Evaluate the sparse inverse
    z = zeros(size(Iz));
    z(end) = 1./ld(end);
    % Allocate memory
    cindit=zeros(n,1);
    for jj = n-1:-1:1
        fil = ld(a1(jj)+1:a1(jj+1)-1);
        fi = I(a1(jj)+1:a1(jj+1)-1);
        lfi = length(fi);
        Zt = zeros(lfi,lfi);
        indz = cumsum(histc(indIz{jj},[0 ; fi]));
        indz = az1(jj) + indz(1:end-1);

        i4=0;
        for i1 = 1:lfi
            cind1=indaz{fi(i1)};
            Icind1=indIz{fi(i1)};
            indfi = lfi;
            i2=length(Icind1);
            go = true;
            while go
                if Icind1(i2)==jj  % Find the indeces for the jj'th rows in fi columns
                    i4=i4+1;
                    cindit(i4)=cind1(i2);
                    go = false;
                end
                if indfi >= 1 && fi(indfi) == Icind1(i2) % Find the indeces for the fi'th rows in i2'nd columns
                    Zt(indfi,i1) = z(cind1(i2));
                    indfi = indfi-1;
                end
                i2 = i2-1;
            end
        end
        % remove extras
        cindi=cindit(1:i4);

        zij = -fil'*Zt;
        z(cindi) = zij;
        z(indz) = zij;
        zij = 1./ld(a1(jj)) - fil'*z(indz);
        z(az1(jj)-1+find(indIz{jj}==jj,1)) = zij;
    end

    Z = sparse(Iz,Jz,z);

    if nargin == 1
        r(q) = 1:n;
        Z = Z(r,r);
    end
end


function [ a, b ] = p01_lim ( dim_num )

%*****************************************************************************80
%
%% P01_LIM returns the integration limits for problem 01.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 March 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the dimension of the argument.
%
%    Output, real A(DIM_NUM), B(DIM_NUM), the lower and upper
%    limits of integration.
%
  a(1:dim_num) = 0.0;
  b(1:dim_num) = 1.0;

  return
end


function mrk= mrk_evenlyInBlocks(blk, msec, varargin)

% MRK_EVENLYINBLOCKS - inserts additional markers between the existing
% markers, starting msec after the existing markers.
%
% Synopsis:
%   MRK= mrk_evenlyInBlocks(BLK, MSEC, <OPT>)
%
% Arguments:
%   BLK:  [STRUCT] structure defining blocks. It must have a field 'ival',
%         with each row defining a time interval ('block') in msec.
%
%   MSEC: [DOUBLE] distance of markers within each block in milliseconds
%
% OPT - struct or property/value list of optional fields/properties:
%      .OffsetStart -  specify offset in milliseconds after which the first
%                      after an existing marker block is to be set (default 0)
%      .OffsetEnd -    minimum length between block and next marker
%                      (default msec)
%
% Returns:
%   MRK: marker structure

% Author: Benjamin B
% 7-2010: Documented, extended, cleaned up (Matthias T)
% 5-2015 adapted to the new toolbox (Laura A, Benjamin B, Markus W)

props= {'OffsetEnd'     msec   '!DOUBLE[1]'
        'OffsetStart'   0      '!DOUBLE[1]'
       };

opt= opt_proplistToStruct(varargin{:});
opt= opt_setDefaults(opt, props, 1);
opt_checkProplist(opt, props);

misc_checkType(blk, 'STRUCT(ival)');
misc_checkType(blk.ival, 'DOUBLE[- 2]', 'blk.ival');
misc_checkType(msec, '!DOUBLE[1]');

mrk= struct('time',[], 'event',struct);
mrk.event= struct('blkno',[]);

if isfield(blk, 'y'),
  [nClasses, nBlocks]= size(blk.y);
  mrk.y= zeros(nClasses,0);
  mrk.className= blk.className;
end

% fields from blk.event will be adapted and added to mrk.event. Let's prepare:
if isfield(blk, 'event'),
  eventFields= fieldnames(blk.event);
else
  eventFields= {};
end
for Fld= eventFields,
  fld= Fld{1};
  mrk.event.(fld)= [];
end

nBlocks= size(blk.ival,1);
for bb= 1:nBlocks,
  new_time= blk.ival(bb,1)+opt.OffsetStart:msec:blk.ival(bb,2)-opt.OffsetEnd;
  nMrk= length(new_time);
  mrk.time= cat(2, mrk.time, new_time);
  mrk.event.blkno= cat(1, mrk.event.blkno, bb*ones(nMrk,1)); % blkno has to be column vector for later functions
  % adapt fields from blk.event and add to mrk.event
  for Fld= eventFields,
    fld= Fld{1};
    val= blk.event.(fld)(bb);
    mrk.event.(fld)= cat(1, mrk.event.(fld), repmat(val, [nMrk 1]));
  end
  if isfield(blk, 'y'),
    new_y= zeros(nClasses, nMrk);
    iClass= find(blk.y(:,bb));
    new_y(iClass,:)= 1;
    mrk.y= cat(2, mrk.y, new_y);
  end
end


% Test file for chebtech/trigcoeffs.m

function pass = test_trigcoeffs(pref)

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

for n = 1:2
    if ( n == 1 )
        testclass = chebtech1();
    else
        testclass = chebtech2();
    end

    %%
    % Check a few simple examples.

    f = testclass.make(@(x) zeros(size(x)), [], pref);
    p = trigcoeffs(f);
    pass(n,1) = (norm(p, inf) <= 10*vscale(f)*eps);

    f = testclass.make(@(x) 3*ones(size(x)), [], pref);
    p = trigcoeffs(f);
    pass(n,2) = (norm(p - 3, inf) < 10*vscale(f)*eps);

    f = testclass.make(@(x) 1+cos(pi*x), [], pref);
    p = trigcoeffs(f,3);
    pass(n,3) = (norm(p - [0.5 1 0.5]', inf) < 10*vscale(f)*eps);
    p = trigcoeffs(f,5);
    pass(n,4) = (norm(p - [0 0.5 1 0.5 0]', inf) < 10*vscale(f)*eps);
    p = trigcoeffs(f,1);
    pass(n,5) = (norm(p - 1, inf) < 10*vscale(f)*eps);

    f = testclass.make(@(x) 1 + exp(2*1i*pi*x) + exp(-1i*pi*x), [], pref);
    p = trigcoeffs(f,5);
    pass(n,6) = (norm(p - [0 1 1 0 1 ]', inf) ...
        < 10*vscale(f)*eps);
    p = trigcoeffs(f,9);
    pass(n,7) = (norm(p - [0 0 0 1 1 0 1 0 0 ]', inf) ...
        < 10*vscale(f)*eps);
    p = trigcoeffs(f,3);
    pass(n,8) = (norm(p - [1 1 0]', inf) ...
        < 10*vscale(f)*eps);

    %%
    % Verify operation for array-valued chebtech objects.

    f = testclass.make(@(x) [3*ones(size(x)), 1+cos(pi*x), ...
        1 + exp(2*1i*pi*x) + exp(-1i*pi*x)], [], pref);
    p = trigcoeffs(f,5);
    p_exact = [0 0   0;...
               0 0.5 1;...
               3 1   1;...
               0 0.5 0;...
               0 0   1];
    pass(n,9) = (norm(p(:) - p_exact(:), inf) < 10*max(vscale(f)*eps));

    p = trigcoeffs(f,7);
    p_exact = [0 0   0;...
               0 0   0;...
               0 0.5 1;...
               3 1   1;...
               0 0.5 0;...
               0 0   1;...
               0 0   0];
    pass(n,10) = (norm(p(:) - p_exact(:), inf) < 10*max(vscale(f)*eps));

    p = trigcoeffs(f,3);
    p_exact = [0 0.5 1;...
               3 1   1;...
               0 0.5 0];
    pass(n,11) = (norm(p(:) - p_exact(:), inf) < 10*max(vscale(f)*eps));

    p = trigcoeffs(f,0);
    pass(n,12) = isempty(p);
end

end


%% Test Problems for DSDP
clc
clear

%% CSDP Options Function
clc
csdpset

a = csdpset

%% LP1 [-31.4]
clc
f = -[6 5]';
A = sparse([1,4; 6,4; 2, -5]);
b = [16;28;6];
lb = [0;0];
ub = [10;10];

opts = [];
opts.display = 2;
opts.maxiter = 15;
% opts.writeprob = 'prob.dat-s';
% opts.writesol = 'sol.dat-s';
[x,ff,e,i,X] = csdp(f,A,b,lb,ub,[],[],opts)

%% LP2 [2]
clc
f = [8,1]';
A = sparse([-1,-2;1,-4;3,-1;1,5;-1,1;-1,0;0,-1]);
b = [-4,2,21,39,3,0,0]';

[x,p,d,e] = csdp(f,A,b)

%% LP3 [-3.75]
clc
f = -[-1, 2]';
A = sparse([2, 1;-4, 4]);
b = [5, 5]';

[x,p,d,e] = csdp(f,A,b)

%% LP4 [-97.5]
clc
clear all
f = -[1 2 3]';
A = sparse([-1,1,1; 1,-3,1]);
b = [20,30]';
Aeq = sparse([1 1 1]);
A = [A;Aeq;-Aeq];
beq = 40;
b = [b;beq;-beq];
lb = [0 0 0]';
ub =[40 inf inf]';

[x,fval,e,i] = csdp(f,A,b,lb,ub)

% [x,ff] = clp(f,[A;Aeq],[-Inf(size(b));beq],[b;beq],lb,ub)

%% SDP1 [1.4142]
clc
clear
%Objective
f = 1;
%SDP [x sqrt(2); sqrt(2) x] >= 0
A = eye(2);
C = -[0 sqrt(2); sqrt(2) 0];
sdp = sparse([C(:) A(:)]);
%Options
opts.display=2;
[x,ff,e,i] = csdp(f,[],[],[],[],sdp,[],opts)

%% SDP 1b Johan [-1]
clc
clear
%Objective
f = 1;
%SDP [1 x; x 1] >= 0
A = [0 1; 1 0];
C = -eye(2);
sdp = sparse([C(:) A(:)]);
%Options
opts.display = 2;
tic
[x,ff,e,i] = csdp(f,[],[],[],[],sdp,[],opts)
toc

%% SDP2 [4]
clc
clear
%Objective
f = [1;1];
%Linear Constraints
lb = [0;0];
ub = [10;10];
%SDP Constraints [x1 2; 2 x2] >= 0
C = -[0 2; 2 0];
A0 = [1 0; 0 0];
A1 = [0 0; 0 1];
sdp = sparse([C(:) A0(:) A1(:)]);
%Setup Options
opts.display=2;
[x,ff,e,i] = csdp(f,[],[],lb,ub,sdp,[],opts)

%% SDP3 [1.2] (Note assume matrices are symmetric triu to avoid x5)
clc
clear
%Objective
f = [1 0 0 0]';
%SDP Constraint1 [x2 x3;x3 x4] <= x1*eye(2)
C = zeros(2);
A0 = eye(2);
A1 = -[1 0; 0 0];
A2 = -[0 1; 1 0];
A3 = -[0 0; 0 1];
sdp{1} = sparse([C(:) A0(:) A1(:) A2(:) A3(:)]);
%SDP Constraint2 [x2 x3; x3 x4] >= [1 0.2; 0.2 1]
C = [1 0.2; 0.2 1];
A0 = zeros(2);
A1 = [1 0; 0 0];
A2 = [0 1; 1 0];
A3 = [0 0; 0 1];
sdp{2} = sparse([C(:) A0(:) A1(:) A2(:) A3(:)]);
%Setup Options
opts.display=2;
[x,ff,e,i] = csdp(f,[],[],[],[],sdp,[],opts)

%% SDP4 [10.1787]
clc
clear
%Objective
f = [1 1 1];
%Linear Constraints
lb = [10;0;0];
ub = [1000;1000;1000];
%SDP Constraints [x1 1 2; 1 x2 3; 2 3 100] >= 0
C = -[0 1 2; 1 0 3; 2 3 100];
A0 = [1 0 0; 0 0 0; 0 0 0];
A1 = [0 0 0; 0 1 0; 0 0 0];
A2 = zeros(3);
sdp = sparse([C(:) A0(:) A1(:) A2(:)]);
%Setup Options
opts.display=2;
[x,ff,e,i] = csdp(f,[],[],lb,ub,sdp,[],opts)

%% SDP CSDP Example
clc
clear
%Objective
f = -[1 2];
%SDP Constraint 1
C = [2 1; 1 2];
A0 = -[3 1; 1 3];
A1 = -zeros(2);
sdp{1} = sparse([C(:) A0(:) A1(:)]);
%SDP Constraint 2
C = [3 0 1; 0 2 0; 1 0 3];
A0 = -zeros(3);
A1 = -[3 0 1; 0 4 0; 1 0 5];
sdp{2} = sparse([C(:) A0(:) A1(:)]);
%SDP Constraint 3
C = zeros(2);
A0 = -[1 0; 0 0];
A1 = -[0 0; 0 1];
sdp{3} = sparse([C(:) A0(:) A1(:)]);
%Setup Options
opts.display=2;
% opts.writesol='sol.dat-s';
[x,ff,e,i,X] = csdp(f,[],[],[],[],sdp,[],opts)

X{1}
X{2}
X{3}

%% SDP5
% clc
% clear
% load dsdpdebug
%
% opts.display=2;
% [y,fvals,exitflag,stats,X] = csdp(model.f,[],[],[],[],model.sdcone,[],opts)

%% SDP 6 (testing memory leaks - use task manager to view matlab ram)
% clc
% clear all
% %Number of cones
% n = 10000;
% %Objective
% f = [1;1];
% %Linear Constraints
% lb = [0;0];
% ub = [10;10];
% %SDP Constraints [x1 2; 2 x2] >= 0
% ind = triu(ones(2))==1;
% C = [0 2; 2 0];
% A0 = [1 0; 0 0];
% A1 = [0 0; 0 1];
% sdp = repmat({sparse([C(:) A0(:) A1(:)])},n,1);
% %Setup Options
% opts.display=2;
% opts.maxtime = 10;
% [x,ff,e,i,X] = csdp(f,[],[],lb,ub,sdp,[],opts);

%% YALMIP comparisons
% %%
% x = sdpvar(1,1);
% %y = sdpvar(1,1);
% z = sdpvar(1,1);
%
% X = [x 2;2 z];
%
% F = set('X>=0');
% % F = F+set('x>=1');
% F = F+set('z>=1');
% F = F+set('x<=10');
% F = F+set('z<=10');
% sol = solvesdp(F,x+z)
%
% %%
% [At,b1,c1,K]=readsdpa('complete2uub.dat-s')
%
% A1 = full(At)
%
% %%
% [At,b2,c2,K]=readsdpa('prob.dat-s')
%
% A2 = full(At)
%
%
% %%
% t = sdpvar(1,1);
% Y = sdpvar(2,2);
% F = set('Y<=t*eye(2)');
% F = F+set('Y>=[1 0.2;0.2 1]');
% sol = solvesdp(F,t)
%
% %%
% [At,b1,c1,K]=readsdpa('test.dat-s')
%
% A1 = full(At)
%
% %%
% [At,b2,c2,K]=readsdpa('prob.dat-s')
%
% A2 = full(At)
%
%
% %%
% clc
% x = sdpvar(1,1);
% y = sdpvar(1,1);
%
% con = [x + 4*y <= 16; 6*x + 4*y <= 28, 2*x - 5*y <= 6; 0 <= x <= 10; 0 <= y <= 10];
% obj = -6*x - 5*y;
%
% opts = sdpsettings('solver','csdp');
% sol = solvesdp(con,obj,opts)
%
% double(x)
% double(y)
% double(obj)
%
% %%
% [At,b1,c1,K]=readsdpa('lp1.dat-s')
%
% A1 = full(At)
%
% %%
% [At,b2,c2,K]=readsdpa('prob.dat-s')
%
% A2 = full(At)


function [y,newmap] = cmsort(x,map,opt)
%CMSORT   Sort color map.
%
%   NEWMAP = CMSORT(MAP) sorts the color map MAP by luminance.
%
%   NEWMAP = CMSORT(MAP,OPT) where OPT is one of the strings 'red',
%   'green', 'blue', 'hue', 'saturation', 'value' and 'luminance', sorts
%   the map according to the specified criterion.
%
%   [Y,NEWMAP] = CMSORT(X,MAP) and [Y,NEWMAP] = CMSORT(X,MAP,OPT) also
%   changes the index matrix so the image remains unchanged after the
%   sorting of the color map.
%
%   The case of the option string does not matter and the string may be
%   truncated since only the first letter is used internally.

%   Author:      Peter J. Acklam
%   Time-stamp:  1998-04-15 13:42:03
%   E-mail:      jacklam@math.uio.no (Internet)
%   URL:         http://www.math.uio.no/~jacklam

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Check and identify input arguments.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if ( nargin < 1 )
   error( 'Not enough input arguments.' );
elseif ( nargin == 1 )          % CMSORT(MAP)
   map = x;
   opt = 'lum';
   index_given = 0;
elseif ( nargin == 2 )
   if ischar(map)               % CMSORT(MAP,OPT)
      map = x;
      opt = map;
      index_given = 0;
      if ( nargout > 1 )
         error( 'Too many output arguments.' );
      end
   else                         % CMSORT(X,MAP)
      opt = 'lum';
      index_given = 1;
   end
elseif ( nargin == 3 )          % CMSORT(X,MAP,OPT)
   index_given = 1;
else
   error( 'Too many input arguments.' );
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Now create the key by which the color map is sorted.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

swtch = lower( opt(1) );

if strcmp( swtch, 'r' )         % Red.
   key = map(:,1);
elseif strcmp( swtch, 'g' )     % Green.
   key = map(:,2);
elseif strcmp( swtch, 'b' )     % Blue.
   key = map(:,3);
elseif strcmp( swtch, 'h' )     % Hue.
   hsv = rgb2hsv( map );
   key = hsv(:,1);
elseif strcmp( swtch, 's' )     % Saturation.
   hsv = rgb2hsv( map );
   key = hsv(:,2);
elseif strcmp( swtch, 'v' )     % Value.
   hsv = rgb2hsv( map );
   key = hsv(:,3);
elseif strcmp( swtch, 'l' )     % Luminance.
   w = [ 0.298936 ; 0.587043 ; 0.114021 ];      % RGB weights.
   key = map*w;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Sort color map and fix index matrix (if given).
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[ dummy, idx ] = sort( key );   % Sort the keys.
newmap = map(idx,:);            % Rearrange color map.

if index_given
   n = length( idx );           % Number of colors.
   xdi = zeros( n, 1 );
   xdi(idx) = 1:n;
   y = zeros( size( x ) );
   y(:) = xdi(x);
else
   y = map;
end