% edge_response_2d.m
% examine variation in edge response for 2D CT
% Copyright 2011-02-23, Jeff Fessler, University of Michigan

if ~isvar('sino'), printm 'setup geometry, image, sinogram'
	down = 2;
	ig = image_geom('nx', 512, 'fov', 50, 'down', down);
	ig.mask = ig.circ > 0;
	sg = sino_geom('ge1', 'units', 'cm', 'down', down);

	% system object
	A = Gtomo2_dscmex(sg, ig);

	% image

	steps = [1 2 4 8] * 10;
        nstep = numel(steps);
	ell = [0 0 15 10 0 1000];
	leg = {};
	for ii=1:nstep
		ell = [ell; [-15+ii*6 0 2 2 0 steps(ii)]];
		leg{end+1} = sprintf('%d HU', steps(ii));
	end
	xtrue = ellipse_im(ig, ell, 'oversample', 2);

	im plc 2 2
	clim = [800 1200];
	im(1, xtrue, 'x', clim), cbar

%	sino = ellipse_sino(sg, ell, 'oversample', 2);
	sino = A * xtrue; % cheat
	im(2, sino, 'sino'), cbar
prompt
end

if ~isvar('fbp'), printm 'fbp 2d fan-beam reconstruction'
	tmp = fbp2(sg, ig);
	fbp = fbp2(sino, tmp, 'window', 'hanning,0.5');
	im(3, fbp, 'FBP', clim), cbar
prompt
end

if ~isvar('kappa'), printm 'kappa'
	wi = ones(size(sino)); % no statistical weighting
	kappa = ig.mask;
end

% use local psf to help select beta
if ~isvar('R'), printm 'R'
	f.l2b = 4; % maybe a bit too big, but ok for now
	f.delta = 10;
	R = Reg1(kappa, 'beta', 2^f.l2b, 'pot_arg', {'hyper3', f.delta});
	qpwls_psf(A, R, 1, ig.mask, Gdiag(wi), 'loop', 1);
prompt
end

if ~isvar('xpwls'), printm 'iterative reconstruction'
	Ab = Gblock(A, 41); % 41 subsets
	f.niter = 20;
	xpwls = pwls_sps_os(fbp(ig.mask), sino, wi, Ab, R, f.niter);
	xpwls = ig.embed(xpwls(:,end));
	im(4, xpwls, clim), cbar
prompt
end

im plc 3 3
pl = @(i) subplot(330+i);

im(1, 'notick', ig.x, ig.y, fbp, clim, 'FBP')
im(4, 'notick', ig.x, ig.y, xpwls, clim, 'PWLS')

tmp1 = fbp(:,end/2+1);
tmp2 = xpwls(:,end/2+1);

pl(2)
plot(ig.x, tmp1)
axis([-20 20 0 1200])
title 'Profile'
xtick([-1 0 1] * 15), ytick([0 1000])

pl(5)
plot(ig.x, tmp2)
axis([-20 20 0 1200])
title 'Profile'
xtick([-1 0 1] * 15), ytick([0 1000])

xx = outer_sum(ig.x, -ell(2:end,1));
pl(3)
plot(xx, (tmp1-1000) * (1 ./ steps), '.-')
ax = [-3 -1 -0.25 1.25];
axis(ax), legend(leg{:}, 'location', 'seo')
title 'Edge response'
xtick([-3 -2 -1]), ytick([0 1])

pl(6)
plot(xx, (tmp2-1000) * (1 ./ steps), '.-')
axis(ax), legend(leg{:}, 'location', 'seo')
title 'Edge response'
xtick([-3 -2 -1]), ytick([0 1])

% ir_savefig eps_c edge_response_2d


function value = year_length_months_greek ( y )

%*****************************************************************************80
%
%% YEAR_LENGTH_MONTHS_GREEK returns the number of months in a Greek year.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 September 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, the year to be checked.
%
%    Output, integer VALUE, the number of months in the year.
%
  if ( year_is_embolismic_greek ( y ) )
    value = 13;
  else
    value = 12;
  end

  return
end


    function y = x5fun(x1,x2,x3,x4,x6,r1,r4,r5)
             yn = -x3^2-r4^2+r1^2+r5^2-x4^2-2*x2*x6+2*x4*x6;
             yd = 2*(x1-x3);
             y = yn/yd;


function varargout = drawArrow(varargin)
%DRAWARROW Draw an arrow on the current axis
%
%   drawArrow(x1, y1, x2, y2)
%   draws an arrow between the points (x1 y1) and (x2 y2).
%
%   drawArrow([x1 y1 x2 y2])
%   gives argument as a single array.
%
%   drawArrow(..., L, W)
%   specifies length and width of the arrow.
%
%   drawArrow(..., L, W, TYPE)
%   also specifies arrow type. TYPE can be one of the following :
%   0: draw only two strokes
%   1: fill a triangle
%   .5: draw a half arrow (try it to see ...)
%
%   Arguments can be single values or array of size N-by-1. In this case,
%   the function draws multiple arrows.
%
%   H = drawArrow(...)
%   return handle(s) to created arrow elements.
%   The handles are returned in a structure with the fields
%   'body', 'wing' and 'head' containing the handles to the different
%   parts of the arrow(s).
%
%   Example
%     t = linspace(0, 2*pi, 200);
%     figure; hold on;
%     plot(t, sin(t));
%     drawArrow([2 -1 pi 0], .1, .05, .5)
%
%   See also
%     drawEdge
%

%   ---------
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 11/11/2004 from drawEdge
%

%   HISTORY
%   2014-09-17 fix managment of handle values as suggested by Benoit Botton
%   2016-05-23 Improve codee and reduce calculations (by JuanPi Carbajal)

if isempty (varargin)
    error ('should specify at least one argument');
end

% parse arrow coordinates
var = varargin{1};
if size (var, 2) == 4
    x1 = var(:,1);
    y1 = var(:,2);
    x2 = var(:,3);
    y2 = var(:,4);
    varargin = varargin(2:end);

elseif length (varargin) > 3
    x1 = varargin{1};
    y1 = varargin{2};
    x2 = varargin{3};
    y2 = varargin{4};
    varargin = varargin(5:end);

else
    error ('MatGeom:drawArrow:invalidArgumentNumber', ...
        'wrong number of arguments, please read the doc');
end
N     = size (x1, 1);

% default values
l = 10  * ones (N, 1); % Body length
w = 5   * ones (N, 1); % Head width
r = 0.1 * ones (N, 1); % Head to body ratio
h = zeros (N, 1);      % Head type

if ~isempty (varargin)
    % Parse parameters
    k      = length (varargin);
    vartxt = 'lwrh';
    cmd    = ['%s = varargin{%d}; %s = %s(:);' ...
              'if length (%s) < N; %s = %s(1) * ones (N , 1); end'];
    for i = 1:k
        v = vartxt(i);
        eval (sprintf (cmd, v, i, v, v, v, v, v));
    end
end

hold on;
oldHold = ishold (gca);
if ~oldHold
    hold on;
end
axis equal;

% angle of the edge
theta = atan2 (y2-y1, x2-x1);

rl = r .* l;
rh = r .* h;
cT = cos (theta);
sT = sin (theta);
% point on the 'left'
xa1 = x2 - rl .* cT - w .* sT / 2;
ya1 = y2 - rl .* sT + w .* cT / 2;
% point on the 'right'
xa2 = x2 - rl .* cT + w .* sT / 2;
ya2 = y2 - rl .* sT - w .* cT / 2;
% point on the middle of the arrow
xa3 = x2 - rh .* cT;
ya3 = y2 - rh .* sT;

% draw main edge
tmp         = line ([x1.'; x2.'], [y1.'; y2.'], 'color', [0 0 1]);
handle.body = tmp;

% draw only 2 wings
ind = find (h == 0);
if ~isempty (ind)
    tmp              = line ([xa1(ind).'; x2(ind).'], [ya1(ind).'; y2(ind).'], ...
                             'color', [0 0 1]);
    handle.wing(:,1) = tmp;

    tmp              = line ([xa2(ind).'; x2(ind).'], [ya2(ind).'; y2(ind).'], ...
                             'color', [0 0 1]);
    handle.wing(:,2) = tmp;
end

% draw a full arrow
ind = find (h ~= 0);
if ~isempty (ind)
    tmp         = patch ([x2(ind) xa1(ind) xa3(ind) xa2(ind) x2(ind)].', ...
                         [y2(ind) ya1(ind) ya3(ind) ya2(ind) y2(ind)].', [0 0 1]);
    handle.head = tmp;
end

% format output arguments
if nargout > 0
    varargout{1} = handle;
end

if ~oldHold
    hold off;
end


function g = diskfun(f, varargin)
% DISKFUN returns a diskfun representing the BALLFUN F evaluated at a
% planar slice.
%
%   G = DISKFUN(F) is the slice of F in the XY plane.
%
%   G = DISKFUN(F, 'x', C) is the slice of F in the plane X = C,
%   scaled to the unit disk; G is a diskfun.
%
%   G = DISKFUN(F, 'y', C) is the slice of F in the plane Y = C,
%   scaled to the unit disk; G is a diskfun.
%
%   G = DISKFUN(F, 'z', C) is the slice of F in the plane Z = C,
%   scaled to the unit disk; G is a diskfun.
%
%   G = DISKFUN(F, PHI, THETA, PSI, C) rotates F using Euler angles phi, theta,
%   and psi with the ZXZ convention and then evaluates it in the plane Z = C
%   (C = 0 by default)
%
% % See also BALLFUN/SPHEREFUN.

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

if isempty( f )
   g = diskfun();
   return
end

% Parse user inputs to get the Euler angles
[phi, theta, psi, c] = parseInputs(varargin{:});

% Throw an error if c>1 or c<1
if abs(c) > 1
    error('CHEBFUN:BALLFUN:diskfun:sliceNotInBall',...
        ['The specified slice does not lie in unit ball.']);
end

% Rotate f using Euler angles phi, theta and psi
f = rotate(f, phi, theta, psi);

% Get the size
[m,n,~] = size(f);

% If n is odd, make it even
m = m + 1-mod(m,2);
n = n + mod(n,2);

% Evaluation points in [c, 1]
rho = chebpts(m)*sqrt(1-c^2);
rho = rho(ceil(m/2):end);

% Evaluation points in [-pi,pi)
lambda = pi*trigpts(n);

% Build the grid and evaluate at the plane Z = C
r  = sqrt(rho.^2+c^2);

theta = atan2(rho,c);
G = zeros(length(r),n);
for i = 1:length(r)
   G(i,:) = fevalm(f, r(i), lambda, theta(i));
end

% Return the diskfun
g = diskfun(real(G));
end

function [phi, theta, psi, c] = parseInputs(varargin)
% Parse user inputs to DISKFUN.
c = 0;
if nargin == 0
    phi = 0;
    theta = 0;
    psi = 0;
elseif ischar(varargin{1})
    phi = 0;
    if strcmp(varargin{1},'x')
        theta = -pi/2;
        psi = pi/2;
        % Evaluate at Y-Z plane
    elseif strcmp(varargin{1},'y')
        % Evaluate at X-Z plane
        theta = -pi/2;
        psi = 0;
    elseif strcmp(varargin{1},'z')
        % Evaluate at X-Y plane
        theta = 0;
        psi = 0;
    end
    if nargin >= 2
       c = varargin{2};
    end
else
    phi = varargin{1};
    theta = 0;
    psi = 0;
    if nargin >= 2
        theta = varargin{2};
    end
    if nargin >= 3
        psi = varargin{3};
    end
    if nargin >= 4
        c = varargin{4};
    end
end
end


function a = band(a,p,q)
%BAND         Extract band from matrix a, lower bandwidth p, upper bandwidth q
%   if parameter q is omitted, q:=p
%
%   res = band(a,p,q)
%

% written  12/02/96     S.M. Rump
% modified 04/04/04     S.M. Rump  set round to nearest for safety
%                                    improved performance
% modified 04/06/05     S.M. Rump  rounding unchanged
%

  if nargin<3
    q = p;
  end

  a = tril(triu(a,-p),q);


function [tBreak,a,b,c,m,p]=essential_distMinAnglePair_discontinuityDistance(Q21)
a=Q21(1,1)+Q21(2,2);
b=Q21(1,2)-Q21(2,1);
c=Q21(3,3);

m=norm([a;b]);
p=sign(a)*acos(clip(b/m));

%tBreak=modAngle(3/2*pi-p);
tBreak=-0.5*pi-p;

function v=clip(v)
v=min(1,max(-1,v));


function [con_vec] = spm_mlm_makecon (mlm,w)
% Make contrast to test if the subset of coefficients indexed by w = 0 ?
% FORMAT [con_vec] = spm_mlm_makecon (mlm,w)
%
% mlm           MLM data structure containing
%               [p x d] matrix of regression coefficients mlm.wmean
% w             [p x d] matrix of comprising 1's and 0's with
%               1s selecting the coefficients of interest
%
% con_vec       Vectorised contrast matrix that can be passed
%               to spm_mlm_posthoc.m
%
%___________________________________________________________________________
% Copyright (C) 2011 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_mlm_makecon.m 4651 2012-02-09 16:03:39Z will $

[p,d]=size(mlm.wmean);

con_vec=[];
for i=1:p,
    for j=1:d,
        if w(i,j)==1
            con=zeros(p,d);
            con(i,j)=1;
            con_vec=[con_vec;con(:)'];
        end
    end
end


function [score] = EMRtest(x,model)
% [score] = EMRtest(x,model): Efficient Manifold Ranking for out-of-sample
%                             retrieval
% Input:
%       - x:  the query point, row vector.
%       - model:  the model learned by EMR
%
% Output:
%       - score: the ranking scores for each point in the database
%
% Usage:
%
%      See: http://www.zjucadcg.cn/dengcai/Data/Examples.html#EMR
%
%Reference:
%
%	 Bin Xu, Jiajun Bu, Chun Chen, Deng Cai, Xiaofei He, Wei Liu, Jiebo
%	 Luo, "Efficient Manifold Ranking for Image Retrieval",in Proceeding of
%	 the 34th International ACM SIGIR Conference on Research and
%	 Development in Information Retrieval (SIGIR), 2011, pp. 525-534.
%
%   version 2.0 --Feb./2012
%   version 1.0 --Sep./2010
%
%   Written by Bin Xu (binxu986 AT gmail.com)
%              Deng Cai (dengcai AT gmail.com)

r = model.r;
a = model.a;
p = size(model.landmarks,1);

% Z construction
D = EuDist2(x,model.landmarks);
[dump,idx] = sort(D);
dump = dump(1:r)/dump(r);
dump = 0.75 * (1 - dump.^2);
z = sparse(idx(1:r),1,dump,p,1);

Z = [model.Z z];
Z = Z';

nSmp =size(Z,1);

y0 = zeros(nSmp,1);
y0(end) = 1;

% Efficient Ranking
feaSum = full(sum(Z,1));
D = Z*feaSum';
D = max(D, 1e-12);
D = D.^(-.5);
H = spdiags(D,0,nSmp,nSmp)*Z;

C = speye(p);
A = H'*H-(1/a)*C;

tmp = H'*y0;
tmp = A\tmp;
score = y0 - H*tmp;

score(end) = [];


% Demo4: Fault detection and fault diagnosis for TE process using KPCA
% X: training samples
% Y: test samples

%%%%%%%%%%%%%%%%%%%%%%%%%%     KPCA       %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Improve the performance by adjusting the following parameters:
% 1. sigma  % kernel width
% 2. pcr    % principal contribution rate
% ---------------------------------------------------------------------%

clc
clear all
close all
addpath(genpath(pwd))

% load TE process data
load train
load test

% Normalization
[X, Y] = normalize(train,test);

% Train KPCA model
model = kpca_train(X,'type',1,'sigma',800,'fd',1);

% Test a new sample Y (vector of matrix)
model = kpca_test(model,Y);

% Plot the result
plotResult(model.SPE_limit,model.SPE_test);
plotResult(model.T2_limit,model.T2_test);

% Fault diagnosis
[CPs_T2_test_s, CPs_SPE_test_s] = CPsKPCA(X,Y,model, ...
    'start_time',300,'end_time',500,'theta',0.7);

% Plot Contribution Plots
plotCPs(CPs_SPE_test_s)
plotCPs(CPs_T2_test_s)


function [P3, S_bar, V, Z, RO, Tr] = random_nr_motion(T, J, K, state)
% [P3, S_bar, V, Z, RO, Tr] = random_nr_motion(T, J, K, state)
%
%  INPUT:
%
%  T           - number of frames
%  J           - number of points
%  K           - number of deformation basis
%
%  OUTPUT:
%
%  P3          - (3*T) x J 3D-motion matrix:    P3([t t+T t+2*T],:) contains the 3D coordinates of the J points at time t
%  S_bar       - shape average:                 3 x J matrix
%  V           - deformation shapes:            (3*K) x J matrix     ( V((n-1)*3+[1:3],:) contains the n-th deformation basis )
%  Z           - deformation weights:           T x K matrix
%  RO          - rotation:                      cell array           ( RO{t} gives the rotation matrix at time t )
%  Tr          - translation:                   T x 2 matrix

rand('state', state);
randn('state', state);

S_bar = rand(3, J);

[q,r] = qr(rand(3*J));

V = zeros(3*K, J);
for kk=1:K,
   V(1+(kk-1)*3:3*kk, :) = reshape(q(:,kk), 3, J);
end

Z = randn(T,K);
Tr = randn(T,2);

a = (rand(T,1)-0.5)*2*pi;
b = (rand(T,1)-0.5)*2*pi;
c = (rand(T,1)-0.5)*2*pi;
P3 = zeros(3*T, J);
for t=1:T,
   R1 = [1 0 0; 0 cos(a(t)) -sin(a(t)); 0 sin(a(t)) cos(a(t))];
   R2 = [cos(b(t)) 0 sin(b(t)); 0 1 0; -sin(b(t)) 0 cos(b(t))];
   R3 = [cos(c(t)) -sin(c(t)) 0; sin(c(t)) cos(c(t)) 0; 0 0 1];

   RO{t} = R1*R2*R3;

   Sdef = S_bar;
   for kk = 1:K,
      Sdef = Sdef+Z(t,kk)*V((kk-1)*3+[1:3],:);
   end;
   Sdef = RO{t}*Sdef +[Tr(t,:)'; 0]*ones(1, J);

   P3([t t+T t+2*T], :) = Sdef;
end


function [texture structure] = structure_texture_decomposition_rof(im, theta, nIters, alp)
%
% Decompose the input IMAGE into structure and texture parts using the
% Rudin-Osher-Fatemi method. The final output is a linear combination
% of the decomposed texture and the structure parts.
%
% According to Wedel etal "An Improved Algorithm for TV-L1 optical flow"
%       equations (8)-(10)
%
% Test code
% [im1, im2, tu, tv] = read_image_flow_tune_para(4,0);
% [t1 s1] = structure_texture_decomposition_rof(im1);
% [t2 s2] = structure_texture_decomposition_rof(im2);
% indx = ~isnan(tu) | ~isnan(tv);
% uv = cat(3, tu, tv);
% uv(isnan(uv)) = 0;
% figure; imshow(-abs(partial_deriv(cat(3,im1, im2), uv)).*indx, []); title('original');
% figure; imshow(-abs(partial_deriv(cat(3,s1, s2), uv)).*indx, []);  title('structure');
% figure; imshow(-abs(partial_deriv(cat(3,t1, t2), uv)).*indx, []);  title('texture');
% tmp = partial_deriv(cat(3,t1, t2, uv);
% figure; imshow(t1, []); figure; imshow(s1, []);

%   Author: Deqing Sun, Department of Computer Science, Brown University
%   Contact: dqsun@cs.brown.edu
%   $Date: 2009 $
%
% Copyright 2009-2010, Brown University, Providence, RI. USA
%
%                          All Rights Reserved
%
% All commercial use of this software, whether direct or indirect, is
% strictly prohibited including, without limitation, incorporation into in
% a commercial product, use in a commercial service, or production of other
% artifacts for commercial purposes.
%
% Permission to use, copy, modify, and distribute this software and its
% documentation for research purposes is hereby granted without fee,
% provided that the above copyright notice appears in all copies and that
% both that copyright notice and this permission notice appear in
% supporting documentation, and that the name of the author and Brown
% University not be used in advertising or publicity pertaining to
% distribution of the software without specific, written prior permission.
%
% For commercial uses contact the Technology Venture Office of Brown University
%
% THE AUTHOR AND BROWN UNIVERSITY DISCLAIM ALL WARRANTIES WITH REGARD TO
% THIS SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND
% FITNESS FOR ANY PARTICULAR PURPOSE.  IN NO EVENT SHALL THE AUTHOR OR
% BROWN UNIVERSITY BE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL
% DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR
% PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS
% ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF
% THIS SOFTWARE.

if nargin == 1
    theta   = 1/8;
    nIters  = 100;
    alp     = 0.95;  % alp = 0.75 results in 4:1
end;

% Rescale the input image to [-1 1]
IM   = scale_image(im, -1,1);

% Backup orginal images
im   = IM;

% stepsize
delta = 1.0/(4.0*theta);

for iIm = 1:size(im,3)

    % Initialize dual variable p to be 0
    p = zeros([size(im,1) size(im,2) 2]);

    % Gradient descend
    I = squeeze(IM(:,:,iIm));

    for iter = 1:nIters

        % Compute divergence        eqn(8)
        div_p = imfilter(p(:,:,1), [-1 1 0], 'corr', 0)+ ...
                imfilter(p(:,:,2), [-1 1 0]', 'corr', 0);

        I_x = imfilter(I+theta*div_p, [-1 1], 'replicate');

        I_y = imfilter(I+theta*div_p, [-1 1]', 'replicate');

        % Update dual variable      eqn(9)
        p(:,:,1) = p(:,:,1) + delta*(I_x);
        p(:,:,2) = p(:,:,2) + delta*(I_y);

        % Reproject to |p| <= 1     eqn(10)
        reprojection = max(1.0, sqrt(p(:,:,1).^2 + p(:,:,2).^2));
        p(:,:,1) = p(:,:,1)./reprojection;
        p(:,:,2) = p(:,:,2)./reprojection;

    end

    % compute divergence
    div_p = imfilter(p(:,:,1), [-1 1 0], 'corr', 0)+ ...
            imfilter(p(:,:,2), [-1 1 0]', 'corr', 0);

    % compute structure component
    IM(:,:,iIm) = I + theta*div_p;

end;

texture   = squeeze(scale_image(im - alp*IM, 0, 255));

if nargout == 2
    structure = squeeze(scale_image(IM, 0, 255)); %(u-min(u(:)))/(max(u(:))-min(u(:))) - 1;
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 = sprice_2(a, b, r, n, f, k, t,cp)
% sabr prices using an admissible region kl <= k <= ku where sabr is used
% for 0 <= k <= kl
% we use a put pricing function f(x) = K^mu exp(a + bx + cx^2)
% for ku < k < +infty
% we use a call pricing function f(x) = K^(-nu) exp(a + bx^(-1) + cx(-2)
eps = 1e-004;

%find index where density is positive
s = @(x) psabr(a, b, r, n, f, x, t);
index = find(s(k)<0,1,'last');

if isempty(index)
   kl = .25 *f;
else
   kl = max(.25 * f,k(index+1));% lower strike level (free parameter)
end

%kl = .5 * f;                               % lower strike level

s = @(x) sprice(a, b, r, n, f, x, t,-1);    % sabr price (-1) for puts

s1 = s(kl-eps);                             % for calc of derivatives
s2 = s(kl);                                 % for calc of derivatives
s3 = s(kl+eps);                             % for calc of derivatives

V1 = log(s2);                               % log put price

U2 = (s3-s1)/(2*eps);                           % derivative of the price
V2 = U2/s2;                                 % derivative of the log price

U3 = (s3-2*s2+s1)/eps^2;                    % second derivative of the price
V3 = U3/s2 - V2^2;                     % second derivative of the log price

% fix mu and solve
%       V1 = mu log kl + a + b kl + c kl^2
%       V2 = mu / kl + b + 2c kl
%       V3 = - mu / kl^2 + 2c

mu = -V3*kl^2; bl = V2 - mu / kl; al = V1 - mu *log(kl) - bl * kl;

ku = 15 * f;                               % upper strike level
s = @(x) sprice(a, b, r, n, f, x, t,1);     % sabr price (1) for calls
s1 = s(ku-eps);                             % for calc of derivatives
s2 = s(ku);                                 % for calc of derivatives
s3 = s(ku+eps);                             % for calc of derivatives

V1 = log(s2);                               % log call price

U2 = (s3-s1)/(2*eps);                           % derivative of the price
V2 = U2/s2;                                 % derivative of the log price

U3 = (s3-2*s2+s1)/eps^2;                    % second derivative of the price
V3 = U3/s2 - (U2/s2)^2;                     % second derivative f the price

% fix nu and solve
%       V1 = -nu log ku + a + b/ku + c/ku^2
%       V2 = -nu / ku - b / ku^2 - 2c/ku^3
%       V3 = nu / ku^2 + 2 b / ku^3 - 6 c / ku^4

nu = 2; cu = (-1.5*nu / ku + .5*V3 * ku - V2)*ku^3/5;
bu = -ku^2*(V2 + nu/ku +2*cu/ku^3);
au = V1 + nu * log(ku) - bu / ku - cu / ku^2;

% the sabr volatility for the admissible region kl <= k <= ku
sigma = svol(a,b,r,n,f,k((kl<=k)&(k<=ku)),t);

    d1= (log(f./k((kl<=k)&(k<=ku)))+(0.5*t*sigma.*sigma))./(sqrt(t)*sigma); % BS d1 with sabr vol
    d2= (log(f./k((kl<=k)&(k<=ku)))-(0.5*t*sigma.*sigma))./(sqrt(t)*sigma); % BS d2 with sabr vol

if cp==1
    yu = k(k>ku).^(-nu) .* exp(au+bu./k(k>ku)+cu./k(k>ku).^2);              % call price ku < k < +infty
    ym = f.*normcdf( d1,0,1)-k((kl<=k)&(k<=ku)).*normcdf( d2,0,1);          % admissible region kl <= k <= ku
    yl = k(k<kl).^mu .* exp(al + bl.*k(k<kl)) + f - k(k<kl); % call price using call-put parity 0<= k < kl
else
    yl = k(k<kl).^mu .* exp(al + bl .* k(k<kl));               % put price 0<= k < kl
    ym = k((kl<=k)&(k<=ku)).*normcdf(-d2,0,1)-f.*normcdf(-d1,0,1);          % admissible region kl <= k <= ku
    yu = k(k>ku).^(-nu) .* exp(au+bu./k(k>ku)+cu./k(k>ku).^2) - f + k(k>ku);% put prices using call-out parity ku < k < +infty
end

y = [yl ym yu];                                                             % output
end


function f = dot(F, G)
%DOT   Vector dot product.
%   DOT(F, G) returns the dot product of the SPHEREFUN objects F and G. DOT(F,
%   G) is the same as F'*G.
%
% See also SPHEREFUNV/CROSS.

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

if ( isempty(F) || isempty(G) )
    f = spherefun();
    return
end

Fc = F.components;
Gc = G.components;

f = Fc{1}.*Gc{1} + Fc{2}.*Gc{2} + Fc{3}.*Gc{3};

end


function dtiComputePathwayDistanceMatrixFromFG (fg, outputFilename)

% function [distances] = dtiComputePathwayDistanceMatrixFromFG (fg, outputFilename)
%
% Computes a pathway distance matrix from a mrDiffusion fibergroup
%
% fg = input fibergroup
% outputFilename = filename for output distance matrix (.dis extension)

outputFilename

fid = fopen (outputFilename, 'rb');
if (fid ~= -1)
    reply = input(['The file ''' outputFilename ''' exists. Do you want to overwrite the contents? (Y/N) [N]'], 's');
    if isempty(reply)
        reply = 'N';
    end
    if (not (reply == 'y')) & (not (reply == 'Y'))
        return;
    end
    fclose(fid);
end;
fid = fopen (outputFilename, 'wb');
if (~fid)
    fprintf ('Error opening output file!');
    return;
end;
dmatrix = repmat (0, round(length(fg.fibers)*((length(fg.fibers)+1)/2)), 1);

fprintf ('Computing %d comparisons for %d pathways...\n', length(dmatrix), length(fg.fibers));
fprintf ('[Each tick is %d comparisons]\n', round(length(dmatrix)/100));
for p1 = 1:length(fg.fibers)
    points1 = fg.fibers{p1}(:,1:8:length(fg.fibers{p1}));
    p1Cpp = p1-1;
    for p2 = 1:p1-1
        points2 = fg.fibers{p2}(:,1:8:length(fg.fibers{p2}));
        [indices, bestSqDist] = nearpoints(points1, points2);
        bestDist = sqrt(bestSqDist);
        avgDistance1 = mean (bestDist);
        [indices, bestSqDist] = nearpoints(points2, points1);
        bestDist = sqrt(bestSqDist);
        avgDistance2 = mean (bestDist);
        avgDistance = (avgDistance1 + avgDistance2)/2.0;
        p2Cpp = p2-1;
        index = p2Cpp+p1Cpp*(p1Cpp-1)/2+1;
        if (mod(index, round(length(dmatrix)/100)) == 0)
            fprintf ('.');
        end;
        dmatrix(index) = avgDistance;
    end;
end;

fprintf ('\n\nWriting distance matrix to disk...\n');
fwrite (fid, length(fg.fibers), 'int');
fwrite (fid, dmatrix, 'float');
fclose (fid);
fprintf ('Done!\n');


function owen_values_test ( )

%*****************************************************************************80
%
%% OWEN_VALUES_TEST demonstrates the use of OWEN_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 November 2005
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'OWEN_VALUES_TEST:\n' );
  fprintf ( 1, '  OWEN_VALUES stores values of\n' );
  fprintf ( 1, '  the Owen T function.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '           H             A             T\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, h, a, t ] = owen_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %12f  %12f  %24.16f\n',  h, a, t );

  end

  return
end


function [psi] = Pa2psi(Pa)
% Convert pressure from pascals to pounds-per-square-inch.
% Chad Greene 2012
psi = Pa/6894.757;


%[2012]-"A new fruit fly optimization algorithm: Taking the financial
%distress model as an example"

% (9/12/2020)

function FOA = jFruitFlyOptimizationAlgorithm(feat,label,opts)
% Parameters
lb    = 0;
ub    = 1;
thres = 0.5;

if isfield(opts,'N'), N = opts.N; end
if isfield(opts,'T'), max_Iter = opts.T; end
if isfield(opts,'thres'), thres = opts.thres; end

% Objective function
fun = @jFitnessFunction;
% Number of dimensions
dim = size(feat,2);
% Initial
X = zeros(N,dim);
Y = zeros(N,dim);
for i = 1:N
  for d = 1:dim
    X(i,d) = lb + (ub - lb) * rand();
    Y(i,d) = lb + (ub - lb) * rand();
  end
end
% Compute solution
S = zeros(N,dim);
for i = 1:N
  for d = 1:dim
    % Distance between X and Y axis
    dist = sqrt(X(i,d) ^ 2 + Y(i,d) ^ 2);
    % Solution
    S(i,d) = 1 / dist;
  end
  % Boundary
  SB = S(i,:); SB(SB > ub) = ub; SB(SB < lb) = lb;
  S(i,:) = SB;
end
% Pre
fit   = zeros(1,N);
fitG  = inf;
curve = inf;
t = 1;
% Iterations
while t <= max_Iter
  % Fitness
  for i = 1:N
    % Fitness
    fit(i) = fun(feat,label,(S(i,:) > thres),opts);
    % Update better solution
    if fit(i) < fitG
      fitG = fit(i);
      Xgb  = S(i,:);
      % Update X & Y
      Xb   = X(i,:);
      Yb   = Y(i,:);
    end
  end
	for i = 1:N
    for d = 1:dim
      % Random in [-1,1]
      r1 = -1 + 2 * rand();
      r2 = -1 + 2 * rand();
      % Compute new X & Y
      X(i,d) = Xb(d) + (ub - lb) * r1;
      Y(i,d) = Yb(d) + (ub - lb) * r2;
      % Distance between X and Y axis
      dist   = sqrt((X(i,d) ^ 2) + (Y(i,d) ^ 2));
      % Solution
      S(i,d) = 1 / dist;
    end
    % Boundary
    SB = S(i,:); SB(SB > ub) = ub; SB(SB < lb) = lb;
    S(i,:) = SB;
  end
  curve(t) = fitG;
  fprintf('\nGeneration %d Best (FOA)= %f',t,curve(t))
  t = t + 1;
end
% Select features
Pos   = 1:dim;
Sf    = Pos((Xgb > thres) == 1);
sFeat = feat(:,Sf);
% Store results
FOA.sf = Sf;
FOA.ff = sFeat;
FOA.nf = length(Sf);
FOA.c  = curve;
FOA.f  = feat;
FOA.l  = label;
end


function [transParameters,nu,lambda]=igarch_transform(parameters,p,q,errorType,constant)
% IGARCH(P,Q) parameter transformation.  Used to map parameters from a IGARCH
% process to the positive unit simplex. Used in the estimation of IGARCH.
%
% USAGE:
%   [TRANSPARAMETERS,NU,LAMBDA]=igarch_transform(PARAMETERS,P,Q,ERRORTYPE,CONSTANT)
%
% INPUTS:
%   PARAMETERS       - Column parameter vector
%   P                - Positive, scalar integer representing the number of
%                      symmetric innovations
%   Q                - Non-negative, scalar integer representing the number
%                      of lags of conditional variance (0 for ARCH)
%   ERRORTYPE       - One of:
%                        1 - Gaussian Innovations
%                        2 - T-distributed errors
%                        3 - Generalized Error Distribution
%                        4 - Skewed T distribution
%   CONSTANT         - 1 if model includes a constant, 0 otherwise
%
% OUTPUTS:
%   TRANSPARAMETERS - A CONSTANT+p+q-1 column vector of transformed parameters corresponding to
%                      [omega,alpha(1),...,alpha(p) beta1 ... beta(q-1)]'
%                      where the final beta has been excluded
%   NU               - Distribution kurtosis parameter, empty if not applicable
%   LAMBDA           - Distribution asymmetry parameter, empty if not applicable
%
% COMMENTS:
%   Input parameters must satisfy:
%    (1) omega > 0
%    (2) alpha(i) >= 0 for i = 1,2,...,p
%    (3) beta(i)  >= 0 for i = 1,2,...,q
%    (4) sum(alpha(i) + beta(j)) = 1 for i = 1,2,...p and j = 1,2,...q
%    (5) nu>2 of Students T and nu>1 for GED
%    (6) -.99<lambda<.99 for Skewed T
%
% See also IGARCH

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 3    Date: 9/1/2005

%Constraints are omega>0
%nu>2.01 or 1.01
%-.99<lambda<.99
% alpha>0
% gamma>-alpha(i)
% beta>0
% sum(alpha)+0.5sum(gamma)+sum(beta)<1

nu=[];
lambda=[];

%Handle nu, >2.01 in the case of a T, 1.01<nu<50 in the case of a GED
%T uses square, ged uses a logistic
if errorType==2 || errorType==4
    nu=parameters(p+q+constant);
    nu=sqrt(nu-2.01);
elseif errorType==3
    nu=parameters(p+q+constant);
    temp=(nu-1)/49;
    nu=log(temp/(1-temp));
end

%Lambda must be between -.99 and .99.  Use a logistic
if errorType==4
    lambda=parameters(p+q+constant+1);
    temp=(lambda+0.995)/1.99;
    lambda=log(temp/(1-temp));
end

%Use log transform for omega
if constant
    omega=parameters(1);
    tomega=log(omega);
else
    tomega = [];
end

%Parse the parameters
alpha=parameters(constant+1:p+constant);
beta=parameters(constant+p+1:constant+p+q-1);

%Upper bound to keep it a bit away from 1
UB=.999998;

%Check that the parameters satisfy the necessary constraints
if isempty(beta)
    sumbeta = 0;
else
    sumbeta = sum(beta);
end
if  any(alpha<0) || any(beta<0) || (sum(alpha)+sumbeta)>=UB
    error('These do not conform to the necessary set of restrictions to be transformed.')
end

%Finally, must be certain that none of the parameters are exactly zero, and
%that the alpha2+gamma2>0
alpha(alpha==0)=1e-8;
beta(beta==0)=1e-8;

%Finally, up the upper bound a tiny bit
UB=UB+1e-8*(p+q);

%Set the scale
scale=UB;
%Initialze the transformed alpha
talpha=alpha;
for i=1:p
    %Scale the alpha
    talpha(i)=alpha(i)./scale;
    %Use an inverse logistic
    talpha(i)=log(talpha(i)./(1-talpha(i)));
    %Update the scale
    scale=scale-alpha(i);
end

if q>1
    %Initialize the beta
    tbeta=beta;
    %Iterate over betas
    for i=1:(q-1)
        %Scale the betas
        tbeta(i)=tbeta(i)./scale;
        %Use an inverse logistic
        tbeta(i)=log(tbeta(i)./(1-tbeta(i)));
        %Update the scale
        scale=scale-beta(i);
    end
else
    tbeta = [];
end

%Regroup the parameters
transParameters=[tomega;talpha;tbeta];


function [eta Heta inner_it stop_tCG storedb] ...
                      = tCG(problem, x, grad, eta, Delta, options, storedb)
% tCG - Truncated (Steihaug-Toint) Conjugate-Gradient method
% minimize <eta,grad> + .5*<eta,Hess(eta)>
% subject to <eta,eta> <= Delta^2

% This file is part of Manopt: www.manopt.org.
% This code is an adaptation to Manopt of the original GenRTR code:
% RTR - Riemannian Trust-Region
% (c) 2004-2007, P.-A. Absil, C. G. Baker, K. A. Gallivan
% Florida State University
% School of Computational Science
% (http://www.math.fsu.edu/~cbaker/GenRTR/?page=download)
% See accompanying license file.
% The adaptation was executed by Nicolas Boumal.
% Change log:
%   NB Feb. 12, 2013:
%       We do not project r back to the tangent space anymore: it was not
%       necessary, and as of Manopt 1.0.1, the proj operator does not
%       coincide with this notion anymore.
%   NB April 3, 2013:
%       tCG now also returns Heta, the Hessian at x along eta. Additional
%       esthetic modifications.
%   NB Dec. 2, 2013:
%       If options.useRand is activated, we now make sure the preconditio-
%       ner is not used, as was originally intended in GenRTR. In time, we
%       may want to investigate whether useRand can be modifed to work well
%       with preconditioning too.
%   NB Jan. 9, 2014:
%       Now checking explicitly for model decrease at each iteration. The
%       first iteration is a Cauchy point, which necessarily realizes a
%       decrease of the model cost. If a model increase is witnessed
%       (which is theoretically impossible if a linear operator is used for
%       the Hessian approximation), then we return the previous eta. This
%       ensures we always achieve at least the Cauchy decrease, which
%       should be sufficient for convergence.

% All terms involving the trust-region radius will use an inner product
% w.r.t. the preconditioner; this is because the iterates grow in
% length w.r.t. the preconditioner, guaranteeing that we will not
% re-enter the trust-region.
%
% The following recurrences for Prec-based norms and inner
% products come from [CGT2000], pg. 205, first edition.
% Below, P is the preconditioner.
%
% <eta_k,P*delta_k> =
%          beta_k-1 * ( <eta_k-1,P*delta_k-1> + alpha_k-1 |delta_k-1|^2_P )
% |delta_k|^2_P = <r_k,z_k> + beta_k-1^2 |delta_k-1|^2_P
%
% therefore, we need to keep track of
% 1)   |delta_k|^2_P
% 2)   <eta_k,P*delta_k> = <eta_k,delta_k>_P
% 3)   |eta_k  |^2_P
%
% initial values are given by:
%    |delta_0|_P = <r,z>
%    |eta_0|_P   = 0
%    <eta_0,delta_0>_P = 0
% because we take eta_0 = 0 (if useRand = false).
%
% [CGT2000] Conn, Gould and Toint: Trust-region methods, 2000.

inner = problem.M.inner;

theta = options.theta;
kappa = options.kappa;

if ~options.useRand % and therefore, eta == 0
    Heta = problem.M.zerovec(x);
    r = grad;
    e_Pe = 0;
else % and therefore, no preconditioner
    % eta (presumably) ~= 0 was provided by the caller
    [Heta storedb] = getHessian(problem, x, eta, storedb);
    r = problem.M.lincomb(x, 1, grad, 1, Heta);
    e_Pe = inner(x, eta, eta);
end
r_r = inner(x, r, r);
norm_r = sqrt(r_r);
norm_r0 = norm_r;

% precondition the residual
if ~options.useRand
    [z storedb] = getPrecon(problem, x, r, storedb);
else
    z = r;
end

% compute z'*r
z_r = inner(x, z, r);
d_Pd = z_r;

% Initial search direction
delta  = problem.M.lincomb(x, -1, z);
if ~options.useRand % and therefore, eta == 0
    e_Pd = 0;
else % and therefore, no preconditioner
    e_Pd = inner(x, eta, delta);
end

% If the Hessian or a linear Hessian approximation is in use, it is
% theoretically guaranteed that the model value decreases monotonically
% with each iteration of tCG. Hence, there is no need to monitor the model
% value. But, when a nonlinear Hessian approximation is used (such as the
% built-in finite-difference approximation for example), the model may
% increase. It is then important to terminate the tCG iterations and return
% the previous (the best-so-far) iterate. The variable below will hold the
% model value.
model_fun = @(eta, Heta) inner(x, grad, eta) + .5*inner(x, eta, Heta);
if ~options.useRand
    model_value = 0;
else
    model_value = model_fun(eta, Heta);
end

% Pre-assume termination b/c j == end
stop_tCG = 5;

% Begin inner/tCG loop
j = 0;
for j = 1 : options.maxinner

    [Hdelta storedb] = getHessian(problem, x, delta, storedb);

    % Compute curvature
    d_Hd = inner(x, delta, Hdelta);

    alpha = z_r/d_Hd;
    % <neweta,neweta>_P =
    % <eta,eta>_P + 2*alpha*<eta,delta>_P + alpha*alpha*<delta,delta>_P
    e_Pe_new = e_Pe + 2.0*alpha*e_Pd + alpha*alpha*d_Pd;

    if options.debug > 2,
        fprintf('DBG:   (r,r)  : %e\n',r_r);
        fprintf('DBG:   (d,Hd) : %e\n',d_Hd);
        fprintf('DBG:   alpha  : %e\n',alpha);
    end

    % Check against negative curvature and trust-region radius violation.
    % If either condition triggers, we bail out.
    if d_Hd <= 0 || e_Pe_new >= Delta^2,
        % want
        %  ee = <eta,eta>_prec,x
        %  ed = <eta,delta>_prec,x
        %  dd = <delta,delta>_prec,x
        tau = (-e_Pd + sqrt(e_Pd*e_Pd + d_Pd*(Delta^2-e_Pe))) / d_Pd;
        if options.debug > 2,
            fprintf('DBG:     tau  : %e\n', tau);
        end
        eta  = problem.M.lincomb(x, 1,  eta, tau,  delta);
        Heta = problem.M.lincomb(x, 1, Heta, tau, Hdelta);
        if d_Hd <= 0,
            stop_tCG = 1;     % negative curvature
        else
            stop_tCG = 2;     % exceeded trust region
        end
        break;
    end

    % No negative curvature and eta_prop inside TR: accept it
    e_Pe = e_Pe_new;
    new_eta  = problem.M.lincomb(x, 1,  eta, alpha,  delta);
    new_Heta = problem.M.lincomb(x, 1, Heta, alpha, Hdelta);

    % Verify that the model cost decreased in going from eta to new_eta. If
    % the cost increased (which can only occur if the Hessian approximation
    % is nonlinear), then we return the previous eta (which necessarily is
    % the best reached so far, according to the model cost). Otherwise, we
    % accept the new eta and go on.
    new_model_value = model_fun(new_eta, new_Heta);
    if new_model_value > model_value
        stop_tCG = 6;
        break;
    end

    eta = new_eta;
    Heta = new_Heta;

    % Update the residual
    r = problem.M.lincomb(x, 1, r, alpha, Hdelta);

    % Compute new norm of r
    r_r = inner(x, r, r);
    norm_r = sqrt(r_r);

    % Check kappa/theta stopping criterion
    if j >= options.mininner && norm_r <= norm_r0*min(norm_r0^theta, kappa)
        % Residual is small enough to quit
        if kappa < norm_r0^theta,
            stop_tCG = 3;  % linear convergence
        else
            stop_tCG = 4;  % superlinear convergence
        end
        break;
    end

    % Precondition the residual
    if ~options.useRand
        [z storedb] = getPrecon(problem, x, r, storedb);
    else
        z = r;
    end

    % Save the old z'*r
    zold_rold = z_r;
    % Compute new z'*r
    z_r = inner(x, z, r);

    % Compute new search direction
    beta = z_r/zold_rold;
    delta = problem.M.lincomb(x, -1, z, beta, delta);

    % Update new P-norms and P-dots [CGT2000, eq. 7.5.6 & 7.5.7]
    e_Pd = beta*(e_Pd + alpha*d_Pd);
    d_Pd = z_r + beta*beta*d_Pd;

end  % of tCG loop
inner_it = j;

end


function f = note2freq(N)
% MUSIC.NOTE2FREQ Converts a scientific pitch note to a frequency.
%   F = MUSIC.NOTE2FREQ(N) returns the frequency of note N. N is a note string
%   in scientific pitch notation, or a cell array of strings.
%
%   Example
%      f = music.note2freq('F#6')  % returns 1480.0
%
%   See also music.note2tone, music.note2cent, music.freq2note.

%    Author: E. Johnson
%    Copyright 2010 The MathWorks, Inc.

T = music.note2tone(N);
f = music.tone2freq(T);


function calpak_test0125 ( )

%*****************************************************************************80
%
%% CALPAK_TEST0125 tests JED_TO_YMDF_ALEXANDRIAN and YMDF_TO_JED_ALEXANDRIAN.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 March 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CALPAK_TEST0125\n' );
  fprintf ( 1, '  For the Alexandrian calendar:\n' );
  fprintf ( 1, '  JED_TO_YMDF_ALEXANDRIAN: JED -> YMDF.\n' );
  fprintf ( 1, '  YMDF_TO_JED_ALEXANDRIAN: YMDF -> JED.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  JED (in)    YMDF               JED (out)\n' );
  fprintf ( 1, '\n' );

  jed_epoch = epoch_to_jed_alexandrian ( );

  i = 0;

  while ( 1 )

    i = i + 1;
    jed1 = jed_test ( i );

    if ( jed1 < 0.0 )
      break
    end

    if ( jed_epoch <= jed1 )

      [ y2, m2, d2, f2 ] = jed_to_ymdf_alexandrian ( jed1 );

      s2 = ymd_to_s_alexandrian ( y2, m2, d2 );

      jed3 = ymdf_to_jed_alexandrian ( y2, m2, d2, f2 );

      fprintf ( 1, '  %11.2f     %25s  %11.2f\n', jed1, s2, jed3 );

    end

  end

  return
end


% SCRIPT TEST FOR THE KUKA LBR ROBOT KINEMATICS

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
robot = load_robot('KUKA', 'LBR_IIWA_R820_COP');
close all

% test both solutions: based on the transpose an on the Moore-Penrose
q = -pi/2*[1,1,1,1,1,1,1]';
drawrobot3d(robot, q)
targetposition = [0.5, -0.5, 0.5]';
vmag = 0.3;
delta_time = 50/1000;
qs = []
for i=1:20
    T = directkinematic(robot, q);
    J = manipulator_jacobian(robot, q);
    J
    error = targetposition-T(1:3, 4);
    vref = vmag*error/norm(error);
    wref = [0 0 0]';
    vwref = [vref; wref];
    iJ = pinv(J)
    qd = iJ*vwref;
    q = q + qd*delta_time;
    qs = [qs q];
    drawrobot3d(robot, q)
    pause(0.01)
    norm(error)
    if norm(error) < 0.01
        break
    end
end

norm(error)
figure, plot(qs', 'LineWidth',3.0)
legend('q1', 'q2','q3','q4','q5','q6','q7')


function r8vec_legendre_test ( )

%*****************************************************************************80
%
%% R8VEC_LEGENDRE_TEST tests R8VEC_LEGENDRE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 June 2011
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8VEC_LEGENDRE_TEST\n' );
  fprintf ( 1, '  R8VEC_LEGENDRE computes N Legendre points in [R1,R2].\n' );

  r1 = -1.0;
  r2 = +1.0;
  n = 5;

  r = r8vec_legendre ( n, r1, r2 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  N = %d, R1 = %g, R2 = %g\n', n, r1, r2 );

  r8vec_print ( n, r, '  Legendre points:' );

  r1 =   0.0;
  r2 = +10.0;
  n = 7;

  r = r8vec_legendre ( n, r1, r2 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  N = %d, R1 = %g, R2 = %g\n', n, r1, r2 );

  r8vec_print ( n, r, '  Legendre points:' );

  return
end


function g =  getGroupOverlap(row, col)

    N = row*col;

    g = sparse(zeros(N,1));
    g = diag(g);

    %% build overlapping group

    % top let corner group

    g(1, 1 ) = 1;
    g(1, 2 ) = 1;
    g(1, 1+row ) = 1;
    g(1, 2+row ) = 1;

     % bottom let corner group

    g(row, row-1 ) = 1;
    g(row, row ) = 1;
    g(row, row-1+row ) = 1;
    g(row, row+row ) = 1;

    % top right corner group

    g((col-1)*row+1, (col-1)*row+1 ) = 1;
    g((col-1)*row+1, (col-2)*row+1 ) = 1;
    g((col-1)*row+1, (col-1)*row+2 ) = 1;
    g((col-1)*row+1, (col-2)*row+2 ) = 1;

    % bottom right corner group

    g((col-1)*row+row , (col-1)*row+row-1 ) = 1;
    g((col-1)*row+row , (col-2)*row+row-1 ) = 1;
    g((col-1)*row+row , (col-1)*row+row ) = 1;
    g((col-1)*row+row , (col-2)*row+row ) = 1;

    % boundary group

    for i=2:col-1
        % top row rgoup

        j = 1;

        g( (i-1)*row+j  , (i-2)*row+j   ) = 1;
        g( (i-1)*row+j  , (i-2)*row+j+1 ) = 1;

        g( (i-1)*row+j  , (i-1)*row+j   ) = 1;
        g( (i-1)*row+j  , (i-1)*row+j+1 ) = 1;

        g( (i-1)*row+j  , i*row+j   ) = 1;
        g( (i-1)*row+j  , i*row+j+1 ) = 1;

        % bottom row group

        j = row;

        g( (i-1)*row+j  , (i-2)*row+j-1 ) = 1;
        g( (i-1)*row+j  , (i-2)*row+j   ) = 1;

        g( (i-1)*row+j  , (i-1)*row+j-1 ) = 1;
        g( (i-1)*row+j  , (i-1)*row+j   ) = 1;

        g( (i-1)*row+j  , i*row+j-1 ) = 1;
        g( (i-1)*row+j  , i*row+j   ) = 1;

    end

    for j=2:row-1
          % left column rgoup

        i=1;

        g( (i-1)*row+j  , (i-1)*row+j-1 ) = 1;
        g( (i-1)*row+j  , (i-1)*row+j   ) = 1;
        g( (i-1)*row+j  , (i-1)*row+j+1 ) = 1;
        g( (i-1)*row+j  , i*row+j-1 ) = 1;
        g( (i-1)*row+j  , i*row+j   ) = 1;
        g( (i-1)*row+j  , i*row+j+1 ) = 1;

        % right  column group

        i=col;

        g( (i-1)*row+j  , (i-2)*row+j-1 ) = 1;
        g( (i-1)*row+j  , (i-2)*row+j   ) = 1;
        g( (i-1)*row+j  , (i-2)*row+j+1 ) = 1;
        g( (i-1)*row+j  , (i-1)*row+j-1 ) = 1;
        g( (i-1)*row+j  , (i-1)*row+j   ) = 1;
        g( (i-1)*row+j  , (i-1)*row+j+1 ) = 1;

    end

    for i=2:col-1
        for j=2:row-1

            g( (i-1)*row+j  , (i-2)*row+j-1 ) = 1;
            g( (i-1)*row+j  , (i-2)*row+j   ) = 1;
            g( (i-1)*row+j  , (i-2)*row+j+1 ) = 1;
            g( (i-1)*row+j  , (i-1)*row+j-1 ) = 1;
            g( (i-1)*row+j  , (i-1)*row+j   ) = 1;
            g( (i-1)*row+j  , (i-1)*row+j+1 ) = 1;
            g( (i-1)*row+j  , i*row+j-1 ) = 1;
            g( (i-1)*row+j  , i*row+j   ) = 1;
            g( (i-1)*row+j  , i*row+j+1 ) = 1;

        end
    end

end


function polygon_properties_test05 ( )

%*****************************************************************************80
%
%% POLYGON_PROPERTIES_TEST05 tests POLYGON_CONTAINS_POINT, POLYGON_CONTAINS_POINT_2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 May 2014
%
%  Author:
%
%    John Burkardt
%
  n = 5;
  test_num = 4;
  p_test = [ ...
    1.0,  1.0; ...
    3.0,  4.0; ...
    0.0,  2.0; ...
    0.5, -0.25 ]';
  v = [ ...
    0.0, 0.0; ...
    1.0, 0.0; ...
    2.0, 1.0; ...
    1.0, 2.0; ...
    0.0, 2.0 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'POLYGON_PROPERTIES_TEST05\n' );
  fprintf ( 1, '  POLYGON_CONTAINS_POINT determines if\n' );
  fprintf ( 1, '  a point is in a polygon.\n' );
  fprintf ( 1, '  POLYGON_CONTAINS_POINT_2 determines if\n' );
  fprintf ( 1, '  a point is in a polygon.\n' );

  r8mat_transpose_print ( 2, n, v, '  The polygon vertices:' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '          P          In1  In2\n' );
  fprintf ( 1, '\n' );

  for test = 1 : test_num

    p(1:2,1) = p_test(1:2,test);

    inside1 = polygon_contains_point ( n, v, p );

    inside2 = polygon_contains_point_2 ( n, v, p );

    fprintf ( 1, '  %14.6g%14.6g    %d    %d\n', p(1:2), inside1, inside2 );

  end

  return
end


function [spls, corresp, spls_adj, joints, root_id] = merge_nearby_joints(spls, corresp, spls_adj, joints, root_id)
while true
    for i=joints(:,1)'
        edges = find( spls_adj(i,:)==1 );
        edges = edges(edges~=i);
        tmp = ismember(edges, joints(:,1)');
        if sum(tmp)~=1, continue, end;

        j = edges(tmp);
        edges = find( spls_adj(j,:)==1 );
        edges = edges(edges~=j);
        tmp = ismember(edges, joints(:,1)');
        if sum(tmp)~=1, continue, end;

      % update the location
        spls( i,: ) = mean( spls( [i,j],: ) );
        spls( j,: ) = NaN;
        % update the correspondents
       corresp(corresp==j ) = i;

        % update the A matrix
        for k=1:size(spls_adj,1)
            if spls_adj( j,k ) == 1,
                spls_adj( i,k )=1;
                spls_adj( k,i)=1;
            end
        end
        % remove the row
        spls_adj( j,: ) = 0;
        spls_adj( :,j ) = 0;

        segments(j) = 0;
        j = find( joints(:,1)==j );
        if root_id == j
            root_id = find( joints(:,1)==i );
        end
        joints(j,:) = [];
%         break;
    end
    if i == joints(end,1), break, end;
end

%%
figure('Name','Merge nearby joints','NumberTitle','off');set(gcf,'color','white');view3d rot;
movegui('north');
plot_skeleton(spls, spls_adj);
axis off; axis equal; camorbit(0,0,'camera'); axis vis3d; view(-90,0);view3d rot;


function value = a_to_i4 ( ch )

%*****************************************************************************80
%
%% A_TO_I4 returns the index of an alphabetic character.
%
%  Example:
%
%    CH  A_TO_I4
%
%    'A'   1
%    'B'   2
%    ...
%    'Z'  26
%    'a'  27
%    'b'  28
%    ...
%    'z'  52
%    '$'   0
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 February 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, character CH, a character.
%
%    Output, integer VALUE, is the alphabetic index of the character,
%    between 1 and 26 if the character is a capital letter,
%    between 27 and 52 if it is lower case, and 0 otherwise.
%
  if ( 'A' <= ch && ch <= 'Z' )
    value = ch - 'A' + 1;
  elseif ( 'a' <= ch && ch <= 'z' )
    value = ch  - 'a' + 27;
  else
    value = 0;
  end

  return
end


function h = drawPlane3d(plane, varargin)
%DRAWPLANE3D Draw a plane clipped by the current axes.
%
%   drawPlane3d(PLANE) draws a plane of the format:
%       [x0 y0 z0  dx1 dy1 dz1  dx2 dy2 dz2]
%
%   drawPlane3d(...,'PropertyName',PropertyValue,...) sets the value of the
%   specified patch property. Multiple property values can be set with
%   a single statement. See function patch for details.
%
%   drawPlane3d(AX,...) plots into AX instead of GCA.
%
%   H = drawPlane3d(...) returns a handle H to the patch object.
%
%   Example
%
%     p0 = [1 2 3];
%     v1 = [1 0 1];
%     v2 = [0 -1 1];
%     plane = [p0 v1 v2];
%     axis([-10 10 -10 10 -10 10]);
%     drawPlane3d(plane)
%     drawLine3d([p0 v1])
%     drawLine3d([p0 v2])
%     set(gcf, 'renderer', 'zbuffer');
%
%   See also
%   planes3d, createPlane, patch

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% INRA - TPV URPOI - BIA IMASTE
% created the 17/02/2005.
%
%   HISTORY
%   2008-10-30 replace intersectPlaneLine by intersectLinePlane, add doc
%   2010-10-04 fix a bug for planes touching box by one corner
%   2011-07-19 fix a bug for param by Xin KANG (Ben)
%

% Parse and check inputs
valFun = @(x) size(x,1)==1 && isPlane(x);
defOpts.FaceColor = 'm';
[hAx, plane, varargin] = ...
    parseDrawInput(plane, valFun, 'patch', defOpts, varargin{:});

% extract axis bounds to crop plane
lim = get(hAx, 'xlim');
xmin = lim(1);
xmax = lim(2);
lim = get(hAx, 'ylim');
ymin = lim(1);
ymax = lim(2);
lim = get(hAx, 'zlim');
zmin = lim(1);
zmax = lim(2);

% create lines corresponding to cube edges
lineX00 = [xmin ymin zmin 1 0 0];
lineX01 = [xmin ymin zmax 1 0 0];
lineX10 = [xmin ymax zmin 1 0 0];
lineX11 = [xmin ymax zmax 1 0 0];

lineY00 = [xmin ymin zmin 0 1 0];
lineY01 = [xmin ymin zmax 0 1 0];
lineY10 = [xmax ymin zmin 0 1 0];
lineY11 = [xmax ymin zmax 0 1 0];

lineZ00 = [xmin ymin zmin 0 0 1];
lineZ01 = [xmin ymax zmin 0 0 1];
lineZ10 = [xmax ymin zmin 0 0 1];
lineZ11 = [xmax ymax zmin 0 0 1];

% compute intersection points with each plane
piX00 = intersectLinePlane(lineX00, plane);
piX01 = intersectLinePlane(lineX01, plane);
piX10 = intersectLinePlane(lineX10, plane);
piX11 = intersectLinePlane(lineX11, plane);
piY00 = intersectLinePlane(lineY00, plane);
piY01 = intersectLinePlane(lineY01, plane);
piY10 = intersectLinePlane(lineY10, plane);
piY11 = intersectLinePlane(lineY11, plane);
piZ00 = intersectLinePlane(lineZ00, plane);
piZ01 = intersectLinePlane(lineZ01, plane);
piZ10 = intersectLinePlane(lineZ10, plane);
piZ11 = intersectLinePlane(lineZ11, plane);

% concatenate points into one array
points = [...
    piX00;piX01;piX10;piX11; ...
    piY00;piY01;piY10;piY11; ...
    piZ00;piZ01;piZ10;piZ11;];

% check validity: keep only points inside window (with tolerance)
ac = sqrt (eps);
ivx = points(:,1) >= xmin-ac & points(:,1) <= xmax+ac;
ivy = points(:,2) >= ymin-ac & points(:,2) <= ymax+ac;
ivz = points(:,3) >= zmin-ac & points(:,3) <= zmax+ac;
valid = ivx & ivy & ivz;
pts = unique(points(valid, :), 'rows');

% If there is no intersection point, escape.
if size(pts, 1) < 3
    disp('plane is outside the drawing window');
    if nargout > 0
        h = [];
    end
    return;
end

% the two spanning lines of the plane
d1 = plane(:, [1:3 4:6]);
d2 = plane(:, [1:3 7:9]);

% position of intersection points in plane coordinates
u1 = linePosition3d(pts, d1);
u2 = linePosition3d(pts, d2);

% reorder vertices in the correct order
ind = convhull(u1, u2);
ind = ind(1:end-1);

% draw the patch
htmp = patch( ...
    'XData', pts(ind,1), ...
    'YData', pts(ind,2), ...
    'ZData', pts(ind,3), ...
    'Parent', hAx, varargin{:});

% Do not return axis if not requested
% avoids output when called without semicolon
if nargout > 0
    h = htmp;
end


function Y = symmetrize(X,grps)
%SYMMETRIZE Symmetrize a tensor X in specified modes.
%
%   Y = symmetrize(X) will symmetrize a tensor X with respect to all
%   modes so that Y is symmetric with respect to any permutation of
%   indices.
%
%   Y = symmetrize(X,MODES) will symmetrize a tensor X with respect to the
%   modes specified by the vector MODES of mode indices. The second
%   argument may alternatively be a cell array of vectors of modes to,
%   e.g., specify that it should be symmetric with respect to mode [1 3] as
%   well as [2 4].
%
%   See also TENSOR, TENSOR/ISSYMMETRIC.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

%T. Kolda, April 2011
n = ndims(X);
sz = size(X);

% Check that grps exists; if not, create it.
if ~exist('grps','var')
    grps = {1:n};
end

% Check that grps is a cell array.
if ~iscell(grps)
    grps = {grps};
end

if ~isnumeric(grps{1})
    error('MODES must be numeric');
end

% Check tensor dimensions for compatibility with symmetrization
ngrps = length(grps);
for i = 1:ngrps
    dims = grps{i};
    for j = dims(2:end)
        if sz(j) ~= sz(dims(1))
            error('Dimension mismatch for symmetrization');
        end
    end
end

% Check for no overlap in the sets
for i = 1:ngrps
    for j = i+1:ngrps
        if ~isempty(intersect(grps{i},grps{j}))
            error('Cannot haver overlapping symmetries');
        end
    end
end

% Create the combinations for each symmetrized subset
combos = cell(ngrps,1);
for i = 1:ngrps
    combos{i} = perms(grps{i});
end

% Create all the permuations to be averaged
total_perms = prod(cellfun(@length,combos));
sym_perms = repmat(1:n, total_perms, 1);
for i = 1:ngrps
    ntimes = prod(cellfun(@length,combos(1:i-1)));
    ncopies = prod(cellfun(@length,combos(i+1:end)));
    nelems = length(combos{i});

    idx = 1;
    for j = 1:ntimes
        for k = 1:nelems
            for l = 1:ncopies
                sym_perms(idx,grps{i}) = combos{i}(k,:);
                idx = idx + 1;
            end
        end
    end
end

% Create an average tensor
Y = tenzeros(size(X));
for i = 1:total_perms
    Y = Y + permute(X,sym_perms(i,:));
end
Y = Y / total_perms;

% It's not *exactly* symmetric due to oddities in differently ordered
% summations and so on, so let's fix that.
% Idea borrowed from Gergana Bounova:
% http://www.mit.edu/~gerganaa/downloads/matlab/symmetrize.m
for i = 1:total_perms
    Z = permute(Y,sym_perms(i,:));
    Y.data(:) = max(Y.data(:),Z.data(:));
end


function poly2 = resamplePolygon(poly, n)
%RESAMPLEPOLYGON  Distribute N points equally spaced on a polygon.
%
%   POLY2 = resamplePolygon(POLY, N)
%   Resample the input polygon POLY such that the resulting polygon POLY2
%   has N vertices. All points of POLY2 belong to the initial polygon, but
%   are not necessarily vertices of the original polygon.
%
%
%   Example
%     % creates a polygon from an ellipse
%     elli = [20 30 40 20 30];
%     poly = ellipseToPolygon(elli, 500);
%     figure; drawPolygon(poly, 'b');
%     % resample the polygon with a fixed number of vertices
%     poly2 = resamplePolygon(poly, 20);
%     drawPolygon(poly2, 'm');
%     drawPoint(poly2, 'mo');
%     axis equal; axis([-20 60 0 60]);
%
%   See also
%     polygons2d, resamplePolygonByLength, smoothPolygon, resamplePolyline
%

% ------
% Author: David Legland
% E-mail: david.legland@inra.fr
% Created: 2011-12-09, using Matlab 7.9.0.529 (R2009b)
% Copyright 2011-2022 INRA - Cepia Software Platform

poly2 = resamplePolyline(poly([1:end 1],:), n+1);
poly2(end, :) = [];


% mri_exp_mult_mex_test
% test mri_exp_mult_mex

L = 10;
N = 30;
M = 20;
rng(0)
A = randn(N,L) + 1i * randn(N,L);
ur = randn(N,1);
ui = randn(N,1);
vr = randn(M,1);
vi = randn(M,1);
u = ur + 1i * ui;
v = vr + 1i * vi;

d1 = A' * exp(-ur * v.');
d2 = mri_exp_mult_mex(A, ur, v);
printf('double u real case error: %g%%', max_percent_diff(d1, d2))

d1 = A' * exp(-u * vr.');
d2 = mri_exp_mult_mex(A, u, vr);
printf('double v real case error: %g%%', max_percent_diff(d1, d2))

if 0 % test both complex, both real
	mri_exp_mult_mex(A, ur, vr);
	mri_exp_mult_mex(A, u, v);
end

if ~is_pre_v7
	u = single(u);
	v = single(v);
	ur = single(ur);
	vr = single(vr);
	A = single(A);

	d1 = A' * exp(-ur * v.');
	d2 = mri_exp_mult_mex(A, ur, v);
	printf('single u real case error: %g%%', max_percent_diff(d1, d2))

	d1 = A' * exp(-u * vr.');
	d2 = mri_exp_mult_mex(A, u, vr);
	printf('single v real case error: %g%%', max_percent_diff(d1, d2))
end


function [D,nn_list] = compute_nn_distance(X,nbr_nn, options)

% compute_nn_distance - compute the distance to the nearest neighbors
%
%   [D,nn_list] = compute_nn_distance(X,nbr_nn, options);
%
% X is a (d,n) set of n points in R^d
% nbr_nn is the number of nearest neighbors to retrieve.
%
%   You can set Y=options.Y, otherwise the algorithm assumes
%       the target points are Y=X.
%
% D is a (n,nbr_nn) matrix of distance, D(i,j) is distance between point
%   Y(:,i) and point X(nn_list(i,j),:)
% nn_list(i,:) is the set of nearst neighbors
%
%   options.use_nntools = 0 force the use of the slow matlab code
%       for nearest neighbors computations.
%
%   options.exlude_self = 1 to avoid taking a point it self neighbor
%       (works only when Y=X).
%
%   If you set options.pca_numvecs < d, then a preprocessing step of
%       dimensionnality reduction is performed
%
%   Copyright (c) 2006 Gabriel Peyr?

options.null = 0;

if isfield(options, 'use_nntools')
    use_nntools = options.use_nntools;
else
    use_nntools = 1;
end
if isfield(options, 'exlude_self')
    exlude_self = options.exlude_self;
else
    exlude_self = 0;
end

if isfield(options, 'Y')
    Y = options.Y;
else
    Y = [];
end
if exlude_self && ~isempty(Y)
    warning('You can not use exlude_self with options.Y enabled.');
end

if nargin<2
    nbr_nn = size(X,2);
end

nbr_nn = min(nbr_nn,size(X,2));

d = size(X,1);
n = size(X,2);
if d>2*n
    warning('Matrix seems to be of wrong dimension, should be dimension x nbr_points');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% dimensionnality reduction for fast search
if isfield(options, 'pca_numvecs')
    m = options.pca_numvecs;
else
    m = d;
end
if m>0 && m<d
    %%% perform dimensionnality reduction
    [P,tmp,v,Psi] = pca(X,m);
    X = X - repmat(Psi,[1,size(X,2)]);
    X = P' * X;
    if ~isempty(Y)
        Y = Y - repmat(Psi,[1,size(Y,2)]);
        Y = P' * Y;
    end
    d = m;
end

if not(exist('nn_prepare')==3) && use_nntools
    warning('TSTool not installed, use slow Matlab code instead.');
    use_nntools = 0;
end

if isempty(Y)
    %%% compute distance from X to X %%%
    if exlude_self==0
        nbr_nn = nbr_nn - 1;
    end
    if use_nntools
        % use fast mex code
        atria = nn_prepare(X');
        [nn_list,D] = nn_search(X', atria, 1:n, nbr_nn, 0);
    else
        % use slow matlab code
        D1 = sqrt( compute_distance_matrix(X) );
        D1 = D1 + diag( Inf + zeros(size(D1,1),1) );
        % find closest points
        [D,nn_list] = sort(D1);
        D = D(1:nbr_nn,:)';
        nn_list = nn_list(1:nbr_nn,:)';
    end
    if exlude_self==0
        % add self reference
        nn_list = [(1:n)', nn_list];
        D = [zeros(n,1), D];
    end
else
    %%% compute distances from Y to X %%%
    if exist('nn_prepare')>0 && use_nntools
        % use fast mex code
        atria = nn_prepare(X');
        [nn_list,D] = nn_search(X', atria, Y', nbr_nn, 0);
    else
        % use slow matlab code
        D1 = sqrt( compute_distance_matrix(X,Y) );
        % find closest points
        [D,nn_list] = sort(D1,2);
        D = D(:,1:nbr_nn)';
        nn_list = nn_list(:,1:nbr_nn)';
    end
end


function y = rz_func(x)

% nodal crossing function

% input

%  x = simulation time argument (days)

% output

%  y = z-component of unit position vector

% Orbital Mechanics with MATLAB

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

global jdtdbi iplanet1

% compute planet's state vector

jdtdb = jdtdbi + x;

[r, v] = pecliptic(jdtdb, iplanet1, 11);

% z-component of unit position vector

y = r(3) / norm(r);


% run cell detection on spatial masks U
function [ops, stat, model] = sourcery(ops)

tic
[ops, U0, model, U2]    = get_svdForROI(ops);

%  U0 = my_conv2(U0, ops.sig, [1 2]);

ops.fig         = getOr(ops, 'fig', 1);
ops.ThScaling   = getOr(ops, 'ThScaling', 1);

% reshape U to be (nMaps x Y x X)
U0 =  reshape(U0, [], size(U0,ndims(U0)))';
Ly = numel(ops.yrange);
Lx = numel(ops.xrange);
[nSVD, Npix] = size(U0);
U0 = reshape(U0, nSVD, Ly, Lx);

% compute neuropil basis functions for cell detection
S = getNeuropilBasis(ops, Ly, Lx, 'Fourier'); % 'raisedcosyne', 'Fourier'
S = normc(S);
nBasis = size(S,2);

StU = S'*U0(:,:)'; % covariance of neuropil with spatial masks
StS = S'*S; % covariance of neuropil basis functions

% make cell mask with ops.diameter
d0   = ceil(ops.diameter); % expected cell diameter in pixels
sig = ceil(d0/4);
dx = repmat([-d0:d0], 2*d0+1, 1);
dy = dx';
rs = dx.^2 + dy.^2 - d0^2;
dx = dx(rs<=0);
dy = dy(rs<=0);

% initialize cell matrices
mPix    = zeros(numel(dx), 1e4);
mLam    = zeros(numel(dx), 1e4);

iter = 0;
icell = 0;
r = rand(1e4,1);

L   = sparse(Ly*Lx, 0);
LtU = zeros(0, nSVD);
LtS = zeros(0, nBasis);

% regress maps onto basis functions and subtract neuropil contribution
% U = Ucell + neu'*S'
% neu = inv(S'*S) * (S'*U')
neu     = StS\StU;
Ucell   =  U0 - reshape(neu' * S', size(U0));

nBasis = size(S,2);

%
while 1
    iter = iter + 1;

    % residual is smoothed at every iteration
    us = my_conv2_circ(Ucell, sig, [2 3]);

    % compute log variance at each location
    V = sq(mean(us.^2,1));
    V = double(V);

    um = sq(mean(Ucell.^2,1));
    um = my_conv2_circ(um, sig, [1 2]);

    V = V./um ;
    %     V = log(V./um);
    V = double(V);
    % do the morphological opening trick
    % take the running max of the running min
    % this normalizes the brightness of the image
    if iter==1
        lbound = -my_min2(-my_min2(V, d0), d0);
    end

    V = V - lbound;

    if iter==1
        % find indices of all maxima  in plus minus 1 range
        % use the median of these peaks to decide stopping criterion
        maxV    = -my_min(-V, 1, [1 2]);
        ix      = (V > maxV-1e-10);

        % threshold is the mean peak, times a potential scaling factor
        pks = V(ix);

        Th  = ops.ThScaling * median(pks(pks>1e-4));

        ops.Vcorr = V;
    end

    % just in case this goes above original value
    V = min(V, ops.Vcorr);

    % find local maxima in a +- d neighborhood
    maxV = -my_min(-V, d0, [1 2]);

    % find indices of these maxima above a threshold
    ix  = (V > maxV-1e-10) & (V > Th);
    ind = find(ix);

    if iter==1
       Nfirst = numel(ind);
    end

    if numel(ind)==0
        break;
    end

    new_codes = normc(us(:, ind));

    ncells = icell;
    LtU(ncells+size(new_codes,2), nSVD) = 0;

    % each source needs to be iteratively subtracted off
    for i = 1:size(new_codes,2)
        icell = icell + 1;
        [ipix, ipos] = getIpix(ind(i), dx, dy, Lx, Ly);

        Usub = Ucell(:, ipix);

        lam = max(0, new_codes(:, i)' * Usub);

        % threshold pixels
        lam(lam<max(lam)/5) = 0;

        mPix(ipos,icell) = ipix;
        mLam(ipos,icell) = lam;

        % extract biggest connected region of lam only
        mLam(:,icell)   = normc(getConnected(mLam(:,icell), rs)); % ADD normc HERE and BELOW!!!
        lam             = mLam(ipos,icell) ;

        L(ipix,icell)   = lam;

        LtU(icell, :)   = U0(:,ipix) * lam;
        LtS(icell, :)   = lam' * S(ipix,:);
    end

    % ADD NEUROPIL INTO REGRESSION HERE
    LtL     = full(L'*L);
    codes   = ([LtL LtS; LtS' StS]+ 1e-3 * eye(icell+nBasis))\[LtU; StU];
    neu     = codes(icell+1:end,:);
    codes   = codes(1:icell,:);
%     codes = (LtL+ 1e-3 * eye(icell))\LtU;

    % subtract off everything
    Ucell = U0 - reshape(neu' * S', size(U0)) - reshape(double(codes') * L', size(U0));

    % re-estimate masks
    L   = sparse(Ly*Lx, icell);
    for j = 1:icell
        ipos = find(mPix(:,j)>0);
        ipix = mPix(ipos,j);

        Usub = Ucell(:, ipix)+ codes(j, :)' * mLam(ipos,j)';

        lam = max(0, codes(j, :) * Usub);
        % threshold pixels
        lam(lam<max(lam)/5) = 0;

        mLam(ipos,j) = lam;

        % extract biggest connected region of lam only
        mLam(:,j) = normc(getConnected(mLam(:,j), rs));
        lam = mLam(ipos,j);

        L(ipix,j) = lam;

        LtU(j, :) = U0(:,ipix) * lam;
        LtS(j, :) = lam' * S(ipix,:);

        Ucell(:, ipix) = Usub - (Usub * lam)* lam';
    end
    %
    err(iter) = mean(Ucell(:).^2);

    Vnew = sq(sum(Ucell.^2,1));

    fprintf('%d total ROIs, err %4.4f, thresh %4.4f \n', icell, err(iter), Th)
    if ops.fig

        figure(1)
        subplot(1,2, 1);
        imagesc(ops.Vcorr, [0 2*Th])
        axis off

        subplot(1,2, 2);
        imagesc(V, [0 2*Th])
        axis off

        figure(2)
        [~, iclust, lam] = drawClusters(ops, r, mPix, mLam, Ly, Lx);

        drawnow
    end

     if (numel(ind)<Nfirst * getOr(ops, 'stopSourcery', 1/10)) || (iter>= getOr(ops, 'maxIterRoiDetection', 100))
        break;
    end
end

% this runs only the mask re-estimation step, on non-smoothed PCs
% (because smoothing is done during clustering to help)
if getOr(ops, 'refine', 1)
	sourceryAddon;
else
	fprintf('no refinement of ROIs done\n')
end

mLam  =  mLam(:, 1:icell);
mPix  =  mPix(:, 1:icell);

mLam = bsxfun(@rdivide, mLam, sum(mLam,1));
%%

% subtract off neuropil only
Ucell = U0 - reshape(neu' * S', size(U0));

% populate stat with cell locations and footprint
stat = getFootprint(ops, codes, Ucell, mPix, mLam);

% compute compactness of ROIs
stat = anatomize(ops, mPix, mLam, stat);

[~, iclust, lam] = drawClusters(ops, r, mPix, mLam, Ly, Lx);

model.L     = L;
model.S     = S;
model.LtS   = LtS;
model.LtL   = LtL;
model.StS   = StS;

% get anatomical projection weights
stat = weightsMeanImage(ops, stat, model);


function [color,X]=som_fuzzycolor(sM,T,R,mode,initRGB,S)

% SOM_FUZZYCOLOR Heuristic contraction projection/soft cluster color coding for SOM
%
% function [color,X]=som_fuzzycolor(map,[T],[R],[mode],[initRGB],[S])
%
%  sM        (map struct)
%  [T]       (scalar) parameter that defines the speed of contraction
%              T<1: slow contraction, T>1: fast contraction. Default: 1
%  [R]       (scalar) number of rounds, default: 30
%  [mode]    (string) 'lin' or 'exp', default: 'lin'
%  [initRGB] (string) Strings accepted by SOM_COLORCODE,  default: 'rgb2'
%  [S]       (matrix) MxM matrix a precalculated similarity matrix
%  color     (matrix) of size MxRx3 resulting color codes at each step
%  X         (matrix) of size MxRx2 coordiantes for projected unit weight vectors
%             at each step of iteration. (Color code C is calculated using this
%             projection.)
%
% The idea of the projection is to use a naive contraction model which
% pulls the units together. Units that are close to each other in the
% output space (clusters) contract faster into the same point in the
% projection. The original position for each unit is its location in
% the topological grid.
%
% This is an explorative tool to color code the map units so that
% similar units (in the sense of euclidean norm) have similar coloring
% (See also SOM_KMEANSCOLOR) The tool gives a series of color codings
% which start from an initial color coding (see SOM_COLORCODE) and
% show the how the fuzzy clustering process evolves.
%
% The speed of contraction is controlled by the input parameter T. If
% it is high the projection contracts more slowly and reveals more
% intermediate stages (hierarchy).  A good value for T must be
% searched manually. It is probable that the default values do not
% yield good results.
%
% The conatrction process may be slow. In this case the mode can be
% set to 'exp' instead of 'lin', however, then the computing becomes
% heavier.
%
% EXAMPLE
%
%  load iris; % or any other map struct sM
%  [color]=som_fuzzycolor(sM,'lin',10);
%  som_show(sM,'color',color);
%
% See also SOM_KMEANSCOLOR, SOM_COLORCODE, SOM_CLUSTERCOLOR
%
% REFERENCES
%
% Johan Himberg, "A SOM Based Cluster Visualization and Its
% Application for False Coloring", in Proceedings of International
% Joint Conference on Neural Networks (IJCNN2000)},
% pp. 587--592,Vol. 3, 2000
%
% Esa Alhoniemi, Johan Himberg, and Juha Vesanto, Probabilistic
% Measures for Responses of Self-Organizing Map Units, pp. 286--290,
% in Proceedings of the International ICSC Congress on Computational
% Intelligence Methods and Applications (CIMA '99)}, ICSC Academic
% Press}, 1999
%
% Outline of the heuristic
%
% First a matrix D of squared pairwise euclidean distances
% D(i,j)=d(i,j)^2 between map weight vectors is calculated. This
% matrix is transformed into a similarity matrix S,
% s(i,j)=exp(-(D(i,j)/(T.^2*v)), where T is a free input parameter and
% v the variance of all elements of D v=var(D(:)). The matrix is
% further normalized so that all rows sum to one. The original
% topological coordinates X=som_unit_coords(sM) are successively
% averaged using this matrix. X(:,:,i)=S^i*X(:,:,1); As the process is
% actually a series of successive weighted averagings of the initial
% coordinates, all projected points eventually contract into one
% point.  T is a user defined parameter that defines how fast the
% projection contracts into this center point. If T is too small, the
% process will end into the center point at once.
%
% In practise, we don't calculate powers of S, but compute
%
%  X(:,:,i)=S.*X(:,:,i-1); % mode: 'lin'
%
% The contraction process may be slow if T is selected to be large,
% then for each step the similarity matrix is squared
%
%  X(:,:,i)=S*X(:,:,1); S=S*S % mode: 'exp'
%
% The coloring is done using the function SOM_COLORCODE according to
% the projections in X, The coordinates are rescaled in order to
% achieve maximum color resolution.

% Contributed to SOM Toolbox vs2, 2000 by Johan Himberg
% Copyright (c) by Johan Himberg
% http://www.cis.hut.fi/projects/somtoolbox/

% Previously rownorm function normalized the rows of S erroneously
% into unit length, this major bug was corrected 14042003. Now the
% rownorm normalizes the rows to have unit sum as it should johan 14042003

%% Check input arguments

if isstruct(sM),
   if ~isfield(sM,'topol')
      error('Topology field missing.');
   end
   M=size(sM.codebook,1);
else
   error('Requires a map struct.');
end

if nargin<2 || isempty(T),
   T=1;
end
if ~vis_valuetype(T,{'1x1'})
   error('Input for T must be a scalar.');
end

if nargin<3 || isempty(R),
   R=30;
end
if ~vis_valuetype(R,{'1x1'})
   error('Input for R must be a scalar.');
end

if nargin < 4 || isempty(mode),
   mode='lin';
end
if ~ischar(mode),
   error('String input expected for mode.');
else
   mode=lower(mode);
   switch mode
   case {'lin','exp'}
   otherwise
      error('Input for mode must be ''lin'' or ''exp''.');
   end
end

if nargin < 5 || isempty(initRGB)
   initRGB='rgb2';
end

if ischar(initRGB),
   try
      dummy=som_colorcode(sM,initRGB);
   catch
      error(['Color code ''' initRGB ''' not known, see SOM_COLORCODE.']);
   end
else
   error('Invalid color code string');
end

if nargin<6 || isempty(S),
   S=fuzzysimilarity(sM,1./T);
end

if ~vis_valuetype(S,{[M M]}),
   error('Similarity matrix must be a MunitsxMunits matrix.')
end

x = maxnorm(som_unit_coords(sM.topol.msize,sM.topol.lattice,'sheet'));

x = x-repmat(mean(x),size(x,1),1);

X(:,:,1)=x;
color(:,:,1)=som_colorcode(x,'rgb2',1);

%%% Actions

for i=1:R,
   switch mode
   case 'exp'
      S=rownorm(S*S);
      tmpX=S*X(:,:,1);
   case 'lin'
      tmpX=S*X(:,:,i);
   end
   X(:,:,i+1)=tmpX;
   color(:,:,i+1)=som_colorcode(X(:,:,i+1),initRGB);
end

color(isnan(color))=0;

function r=fuzzysimilarity(sM,p)
  % Calculate a "fuzzy response" similarity matrix
  % sM: map
  % p: sharpness factor
  d=som_eucdist2(sM,sM);
  v=std(sqrt(d(:))).^2;
  r=rownorm(exp(-p^2*(d./v)));
  r(~isfinite(r))=0;
  return;

function X = rownorm(X)

  r = sum(X,2);
  X = X ./ r(:,ones(size(X,2),1));
  return;

function X = maxnorm(X)

  for i=1:size(X,2), r = (max(X(:,i))-min(X(:,i))); if r, X(:,i) = X(:,i) / r; end, end
  return;


function [X, Y, Z] = faceLocations(m)
% [X, Y, Z] = faceLocations(m)
% m is a mesh type
% This function returns the X, Y, and Z
% each one is a face variable itself, and can be used for the calculation
% of face variable as a function of locations
% SYNOPSIS:
%
%
% PARAMETERS:
%
%
% RETURNS:
%
%
% EXAMPLE:
%
% SEE ALSO:
%  cellLocations

% Written by Ali A. Eftekhari
% See the license file
X=createFaceVariable(m, 0);
Y=createFaceVariable(m, 0);
Z=createFaceVariable(m, 0);
N=m.dims;
d = m.dimension;
switch d
case {1, 1.5, 1.8}
    	X.xvalue= m.facecenters.x;
    case {2, 2.5, 2.8}
        X.xvalue= repmat(m.facecenters.x, 1, N(2));
        X.yvalue= repmat(m.cellcenters.y', N(1)+1, 1);
        Y.xvalue= repmat(m.cellcenters.x, 1, N(2)+1);
        Y.yvalue= repmat(m.facecenters.y', N(1), 1);
    case {3, 3.2}
        X.xvalue= repmat(m.facecenters.x, 1, N(2), N(3));
        X.yvalue= repmat(m.cellcenters.y', N(1)+1, 1, N(3));
        z=zeros(1,1,N(3));
        z(1,1,:)= m.cellcenters.z;
        X.zvalue= repmat(z, N(1)+1, N(2), 1);
        % Y
        Y.xvalue= repmat(m.cellcenters.x, 1, N(2)+1, N(3));
        Y.yvalue= repmat(m.facecenters.y', N(1), 1, N(3));
        Y.zvalue= repmat(z, N(1), N(2)+1, 1);
        % Z
        z=zeros(1,1,N(3)+1);
        z(1,1,:)= m.facecenters.z;
        Z.xvalue= repmat(m.cellcenters.x, 1, N(2), N(3)+1);
        Z.yvalue= repmat(m.cellcenters.y', N(1), 1, N(3)+1);
        Z.zvalue= repmat(z, N(1), N(2), 1);
end


% RANSACFITFUNDMATRIX - fits fundamental matrix using RANSAC
%
% Usage:   [F, inliers] = ransacfitfundmatrix(x1, x2, t)
%
% Arguments:
%          x1  - 2xN or 3xN set of homogeneous points.  If the data is
%                2xN it is assumed the homogeneous scale factor is 1.
%          x2  - 2xN or 3xN set of homogeneous points such that x1<->x2.
%          t   - The distance threshold between data point and the model
%                used to decide whether a point is an inlier or not.
%                Note that point coordinates are normalised to that their
%                mean distance from the origin is sqrt(2).  The value of
%                t should be set relative to this, say in the range
%                0.001 - 0.01
%
% Note that it is assumed that the matching of x1 and x2 are putative and it
% is expected that a percentage of matches will be wrong.
%
% Returns:
%          F       - The 3x3 fundamental matrix such that x2'Fx1 = 0.
%          inliers - An array of indices of the elements of x1, x2 that were
%                    the inliers for the best model.
%
% See Also: RANSAC, FUNDMATRIX

% Copyright (c) 2004-2005 Peter Kovesi
% School of Computer Science & Software Engineering
% The University of Western Australia
% http://www.csse.uwa.edu.au/
%
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in
% all copies or substantial portions of the Software.
%
% The Software is provided "as is", without warranty of any kind.

% February 2004  Original version
% August   2005  Distance error function changed to match changes in RANSAC

function [F, inliers] = ransacfitfundmatrix(x1, x2, t, feedback)

    if ~all(size(x1)==size(x2))
        error('Data sets x1 and x2 must have the same dimension');
    end

    if nargin == 3
	feedback = 0;
    end

    [rows,npts] = size(x1);
    if rows~=2 & rows~=3
        error('x1 and x2 must have 2 or 3 rows');
    end

    if rows == 2    % Pad data with homogeneous scale factor of 1
        x1 = [x1; ones(1,npts)];
        x2 = [x2; ones(1,npts)];
    end

    % Normalise each set of points so that the origin is at centroid and
    % mean distance from origin is sqrt(2).  normalise2dpts also ensures the
    % scale parameter is 1.  Note that 'fundmatrix' will also call
    % 'normalise2dpts' but the code in 'ransac' that calls the distance
    % function will not - so it is best that we normalise beforehand.
    [x1, T1] = normalise2dpts(x1);
    [x2, T2] = normalise2dpts(x2);

    s = 8;  % Number of points needed to fit a fundamental matrix. Note that
            % only 7 are needed but the function 'fundmatrix' only
            % implements the 8-point solution.

    fittingfn = @fundmatrix;
    distfn    = @funddist;
    degenfn   = @isdegenerate;
    % x1 and x2 are 'stacked' to create a 6xN array for ransac
    [F, inliers] = ransac([x1; x2], fittingfn, distfn, degenfn, s, t, feedback);

    % Now do a final least squares fit on the data points considered to
    % be inliers.
    F = fundmatrix(x1(:,inliers), x2(:,inliers));

    % Denormalise
    F = T2'*F*T1;

%--------------------------------------------------------------------------
% Function to evaluate the first order approximation of the geometric error
% (Sampson distance) of the fit of a fundamental matrix with respect to a
% set of matched points as needed by RANSAC.  See: Hartley and Zisserman,
% 'Multiple View Geometry in Computer Vision', page 270.
%
% Note that this code allows for F being a cell array of fundamental matrices of
% which we have to pick the best one. (A 7 point solution can return up to 3
% solutions)

function [bestInliers, bestF] = funddist(F, x, t);

    x1 = x(1:3,:);    % Extract x1 and x2 from x
    x2 = x(4:6,:);

    if iscell(F)  % We have several solutions each of which must be tested

	nF = length(F);   % Number of solutions to test
	bestF = F{1};     % Initial allocation of best solution
	ninliers = 0;     % Number of inliers

	for k = 1:nF
	    x2tFx1 = zeros(1,length(x1));
	    for n = 1:length(x1)
		x2tFx1(n) = x2(:,n)'*F{k}*x1(:,n);
	    end

	    Fx1 = F{k}*x1;
	    Ftx2 = F{k}'*x2;

	    % Evaluate distances
	    d =  x2tFx1.^2 ./ ...
		 (Fx1(1,:).^2 + Fx1(2,:).^2 + Ftx2(1,:).^2 + Ftx2(2,:).^2);

	    inliers = find(abs(d) < t);     % Indices of inlying points

	    if length(inliers) > ninliers   % Record best solution
		ninliers = length(inliers);
		bestF = F{k};
		bestInliers = inliers;
	    end
	end

    else     % We just have one solution
	x2tFx1 = zeros(1,length(x1));
	for n = 1:length(x1)
	    x2tFx1(n) = x2(:,n)'*F*x1(:,n);
	end

	Fx1 = F*x1;
	Ftx2 = F'*x2;

	% Evaluate distances
	d =  x2tFx1.^2 ./ ...
	     (Fx1(1,:).^2 + Fx1(2,:).^2 + Ftx2(1,:).^2 + Ftx2(2,:).^2);

	bestInliers = find(abs(d) < t);     % Indices of inlying points
	bestF = F;                          % Copy F directly to bestF

    end

%----------------------------------------------------------------------
% (Degenerate!) function to determine if a set of matched points will result
% in a degeneracy in the calculation of a fundamental matrix as needed by
% RANSAC.  This function assumes this cannot happen...

function r = isdegenerate(x)
    r = 0;


classdef MaOEACSS < ALGORITHM
% <multi/many> <real/integer/label/binary/permutation>
% Many-objective evolutionary algorithms based on coordinated selection
% strategy
% t --- 0 --- Threshold value in environmental selection

%------------------------------- Reference --------------------------------
% Z. He and G. G. Yen, Many-objective evolutionary algorithms based on
% coordinated selection strategy, IEEE Transactions on Evolutionary
% Computation, 2017, 21(2): 220-233.
%------------------------------- 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".
%--------------------------------------------------------------------------

    methods
        function main(Algorithm,Problem)
            %% Parameter setting
            t = Algorithm.ParameterSet(0);

            %% Generate random population
            Population = Problem.Initialization();
            Zmin       = min(Population.objs,[],1);

            %% Optimization
            while Algorithm.NotTerminated(Population)
                MatingPool = MatingSelection(Population.objs,Zmin);
                Offspring  = OperatorGA(Problem,Population(MatingPool));
                Zmin       = min([Zmin;Offspring.objs],[],1);
                Population = EnvironmentalSelection([Population,Offspring],Zmin,t,Problem.N);
            end
        end
    end
end


function [A,B,C,G] = sopls(X,Y,nhidden,reg,verbose)
% SOPLS  Sparse orthogonalized partial least squares where the elastic
% net is implemented using either a native implementation or the glmnet package
%
% The native implementation supports arbitrary alpha (ridge penalty) and
% lambda (lasso penalty) values and supports a coupling matrix for alpha to
% implement smoothing. It requires a fixed value for lambda.
%
% The glmnet implementation is much faster but uses alpha to mix between
% ridge and lasso while lambda determines that amount of regularization. It
% also does not support a coupling matrix. It learns the optimal lambda
% using inner cross-validation.
%
% X: ninput x nsamples input data matrix
% Y: noutput x nsamples output data matrix
% nhidden: number of components [1]
% reg: employed regression method
%
% A: noutput x nhidden weight matrix
% B: ninput x nhidden weight matrix
% C: 1 x nhidden bias vector
% G: output parameters for debugging purposes

% Parse inputs

if nargin < 3, nhidden = 1; end
if nargin < 4, error('please specify regularizer'); end
if nargin < 5, verbose = false; end

ninput = size(X,1);
noutput = size(Y,1);
nsamples = size(X,2);

if nhidden > 1,

    G = cell(1,nhidden);

    % Run nhidden times sequentially

    R = Y;
    A = zeros(noutput,nhidden);
    B = zeros(ninput,nhidden);
    C = zeros(1,nhidden);
    for i=1:nhidden,

      if verbose
        fprintf('learning hidden variable %d of %d\n',i,nhidden);
      end

      [A(:,i),B(:,i),C(i),G{i}] = sopls(X,R,1,reg);
      if i < nhidden,    % deflate
        Z = B(:,i)'*X + C(i); % hidden activations
        R = R - A(:,i)*Z; % Y activations after deflation
      end

    end
else

    % Run for a single hidden unit

    B = zeros(ninput,1);
    C = 0;
    iter = 0;
    maxiter = 100;
    tol = nhidden*noutput*(1e-10);

    % Initialize A to first principal component of Y

    optseig.disp = 0;
    if nsamples < noutput,
        [d1,d2] = eigs(Y'*Y,[],1,'LM',optseig);
        A = Y*d1;
        A = A/sqrt(A'*A);
    else
        [A,d2] = eigs(Y*Y',[],1,'LM',optseig);
    end

    Aold = A;
    while iter < maxiter,

        Z = A'*Y;    % reconstruct Z given A from output Y

        try

          f = reg.train(X',Z');

          % all regularizers should use this convention
          B = f.model.weights;
          C = f.model.bias;

        catch

          % this should not happen
          warning(lasterr);
          B = zeros(ninput,1);
          C = 0;

        end

        Z = B'*X + C;   % reconstruct Z given B and C

        % Find optimal A under constraint A'*A = 1

        Syz = Y*Z'/nsamples;
        denom = sqrt(Syz'*Syz);
        if denom,
            A = Syz/denom;
        else
            A = Aold;
        end
        if any(isnan(A(:))),    % check...
            error('nans!!!\n');
        end

        if sumsqr(A - Aold) < tol,
          iter = maxiter;
        else
          iter = iter + 1;
          Aold = A;
        end
    end

    G = f;

end


function q = times(q1,q2,takeRight)
% quaternion .* quaternion and quaternion .* vector3d
%
% Syntax
%   q = q1 .* q2
%
%   q = times(q1,q2)
%   q = times(q1,q2,takeRight)
%
% Input
%  q1 - @quaternion
%  q2 - @quaternion
%  takeRight - logical, use as output left or right input
%
% Output
%  q  - @quaternion

if isa(q1,'quaternion') && isa(q2,'quaternion')

  % which input will become the output?
  if nargin == 3
    if takeRight, q = q2; else, q = q1; end
  elseif isa(q1,'rotation')
    q = q1;
  else
    q = q2;
  end

  a1 = q1.a; b1 = q1.b; c1 = q1.c; d1 = q1.d;
  a2 = q2.a; b2 = q2.b; c2 = q2.c; d2 = q2.d;

  %standard algorithm
  q.a = a1 .* a2 - b1 .* b2 - c1 .* c2 - d1 .* d2;
  q.b = b1 .* a2 + a1 .* b2 - d1 .* c2 + c1 .* d2;
  q.c = c1 .* a2 + d1 .* b2 + a1 .* c2 - b1 .* d2;
  q.d = d1 .* a2 - c1 .* b2 + b1 .* c2 + a1 .* d2;

elseif isa(q1,'quaternion') && isa(q2,'double')

  q1.a = q1.a .* q2;
  q1.b = q1.b .* q2;
  q1.c = q1.c .* q2;
  q1.d = q1.d .* q2;
  q = q1;

elseif isa(q2,'quaternion') && isa(q1,'double')

  q2.a = q1 .* q2.a;
  q2.b = q1 .* q2.b;
  q2.c = q1 .* q2.c;
  q2.d = q1 .* q2.d;
  q = q2;

else

  q = rotate(q2,q1);

end


function f = cost(X)
  global P;
  global PA;
  % Note that it is very much inefficient to explicitly construct the
  % matrix X in this way. Seen as we only need to know the entries
  % of Xmat corresponding to the mask P, it would be far more
  % efficient to compute those only.
  Xmat = X.U*X.S*X.V';
  f = .5*norm( P.*Xmat - PA , 'fro')^2;
end


% Modelling Sensor Directivity in 3D Example
%
% This example demonstrates how the sensitivity of a large single element
% detector varies with the angular position of a point-like source. It is a
% 3D version of the Modelling Sensor Directivity in 2D example.
%
% author: Ben Cox
% date: 29th October 2010
% last update: 23rd February 2012
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox

% This file is part of k-Wave. k-Wave is free software: you can
% redistribute it and/or modify it under the terms of the GNU Lesser
% General Public License as published by the Free Software Foundation,
% either version 3 of the License, or (at your option) any later version.
%
% k-Wave is distributed in the hope that it will be useful, but WITHOUT ANY
% WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for
% more details.
%
% You should have received a copy of the GNU Lesser General Public License
% along with k-Wave. If not, see <http://www.gnu.org/licenses/>.

clear all;

% =========================================================================
% SIMULATION
% =========================================================================

% create the computational grid
Nx = 64;            % number of grid points in the x direction
Ny = 64;            % number of grid points in the y direction
Nz = 64;            % number of grid points in the z direction
dx = 100e-3/Nx;     % grid point spacing in the x direction [m]
dy = dx;            % grid point spacing in the y direction [m]
dz = dx;            % grid point spacing in the z direction [m]
kgrid = makeGrid(Nx, dx, Ny, dy, Nz, dz);

% define the properties of the propagation medium
medium.sound_speed = 1500;	% [m/s]

% create the time array
[kgrid.t_array, dt] = makeTime(kgrid, medium.sound_speed);
Nt = length(kgrid.t_array);

% define a large area detector
sensor.mask = zeros(Nx, Ny, Nz);
sensor.mask(Nx/2+1, (Ny/2-9):(Ny/2+11),(Nz/2-9):(Nz/2+11)) = 1;

% define equally spaced point sources lying on a circle centred at the
% centre of the detector face
Nangles = 11;
circle = makeCartCircle(25*dx, Nangles, [0,0], pi);
circle = [circle; zeros(1,Nangles)];

% find the binary sensor mask most closely corresponding to the cartesian
% points coordinates from makeCartCircle
circle3D = cart2grid(kgrid,circle);

% find the indices of the sources in the binary source mask
source_positions = find(circle3D == 1);

% define a time varying sinusoidal source
source_freq = 0.25e6;
source_mag = 1;
source.p = source_mag*sin(2*pi*source_freq*kgrid.t_array);

% filter the source to remove high frequencies not supported by the grid
source.p = filterTimeSeries(kgrid, medium, source.p);

% pre-allocate array for storing the output time series
single_element_data = zeros(Nt,length(source_positions));

% run a simulation for each of these sources to see the effect that the
% angle from the detector has on the measured signal
for source_loop = 1:length(source_positions)

    % select a point source
    source.p_mask = zeros(Nx,Ny,Nz);
    source.p_mask(source_positions(source_loop)) = 1;

    % create a display mask to display the transducer
    display_mask = source.p_mask + sensor.mask;

    % run the simulation
    input_args = {'PMLSize', 10, 'DisplayMask', display_mask, 'PlotScale', [-0.2 0.2], 'PlotFreq', 50,  'DataCast', 'single'};
    sensor_data = kspaceFirstOrder3D(kgrid, medium, source, sensor, input_args{:});

    % average the data recorded for each grid point to simulate the
    % measured signal from a large aperture, single element, detector
    single_element_data(:,source_loop) = sum(sum(sensor_data,1),1);

end

% =========================================================================
% VISUALISATION
% =========================================================================

% plot source points and sensor mask
voxelPlot(circle3D + sensor.mask);
view([-34 34])

% plot the time series recorded for each of the sources
figure;
plot(kgrid.t_array, single_element_data);
colormap(getColorMap);
xlabel('time [s]');
ylabel('pressure');
title('time series from each direction');

% calculate angle between source and centre of detector face
angles = atan((kgrid.y(source_positions))./kgrid.x(source_positions));

% plot the maximum amplitudes for each of the sources, showing that the
% detector sensitivity falls off at low angles as expected.
figure;
plot(angles,max(single_element_data),'o')
colormap(getColorMap);
xlabel('angle between source and centre of detector face');
ylabel('maximum detected pressure from each direction')


function ptCloudOut = interpolatePtCloud(camK, camKc, ptCloudIn, imROI, imMask, verbose)
%% Interpolate the input sparse point cloud

%% License
% ACADEMIC OR NON-PROFIT ORGANIZATION NONCOMMERCIAL RESEARCH USE ONLY
% Copyright (c) 2018 Bingyao Huang
% All rights reserved.

% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:

% The above copyright notice and this permission notice shall be included in all
% copies or substantial portions of the Software.

% If you publish results obtained using this software, please cite our paper.

% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
% FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
% AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
% LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
% OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
% SOFTWARE.

%% interpolate point cloud
x = ptCloudIn.Location(:,1);
y = ptCloudIn.Location(:,2);
z = ptCloudIn.Location(:,3);

% # of interpolated points in X Y meshgrid
numPts = 100;

% xRange contains lower and higher bounds
xRange = ptCloudIn.XLimits;
xDiff = xRange(1) - xRange(2);
xStep = -xDiff / numPts;

yRange = ptCloudIn.YLimits;
yDiff = yRange(1) - yRange(2);
yStep = -yDiff / numPts;

%% use griddata to interpolate a mesh from given point cloud
[xq, yq] = meshgrid(xRange(1):xStep:xRange(2), yRange(1):yStep:yRange(2));
zq = griddata(x, y, z, xq, yq, 'v4');

% interpolated denser point cloud
pts3d = [xq(:), yq(:), zq(:)];

%% only keep the interpolated points in mask ROI
% project interpolated mesh points to camera image
pts2d = cv.projectPoints(pts3d, zeros(3,1), zeros(3,1), camK, 'DistCoeffs', camKc);
pts2dColor = pts2d;

% remove points that are out of camera's fov
inlierIdx = (pts2d(:,2) > 0) & (pts2d(:,1) > 0) & (pts2d(:,2) < size(imMask,1)) & (pts2d(:,1) < size(imMask,2));
pts2dColor(~inlierIdx,:) = 1;
pts2d = pts2d(inlierIdx,:);
pts3d = pts3d(inlierIdx,:);

% only keep masked 2d, 3d points
pixVal = interp2(im2single(imMask), pts2d(:, 1), pts2d(:, 2));
pts2dInliers = pts2d(pixVal>0,:);
pts3dInliers = pts3d(pixVal>0,:);

% convert to single for color interpolation
imROI = im2single(imROI);

% assign color to pt3d
r = interp2(imROI(:,:,1), pts2dInliers(:, 1), pts2dInliers(:, 2));
g = interp2(imROI(:,:,2), pts2dInliers(:, 1), pts2dInliers(:, 2));
b = interp2(imROI(:,:,3), pts2dInliers(:, 1), pts2dInliers(:, 2));

% create ptCloud with colors
ptCloudOut = pointCloud(pts3dInliers, 'Color', [r,g,b]);

%% Debug info
if(verbose)
    %% plot point cloud
%     figure;
%     pcshow(ptCloudOut, 'VerticalAxis', 'y', 'VerticalAxisDir', 'down', 'MarkerSize', 60);
%     title('Interpolated point cloud with color')
%     daspect([1 1 1]);
%     % view(3);
%     axis vis3d tight;

    %% plot mesh (edges only)
    figure;
    scatter3(x,y,z, 'bo'); hold on
    mesh(xq, yq, zq);hold off
    title('Interpolated mesh, Nodes are shown as blue circles');
    daspect([1 1 1]);
    % view(3);
    axis vis3d tight;

    %% plot surface
    % interpolate color
    r = reshape(interp2(imROI(:,:,1), pts2dColor(:, 1), pts2dColor(:, 2)), [numPts+1, numPts+1]);
    g = reshape(interp2(imROI(:,:,2), pts2dColor(:, 1), pts2dColor(:, 2)), [numPts+1, numPts+1]);
    b = reshape(interp2(imROI(:,:,3), pts2dColor(:, 1), pts2dColor(:, 2)), [numPts+1, numPts+1]);

    figure;
    h = surf(xq, yq, zq, cat(3, r, g, b), 'FaceColor', 'texturemap', 'EdgeColor', 'texturemap', 'FaceLighting', 'gouraud', 'LineStyle', 'none');
    title('Reconstructed mesh')
    h.XData(pixVal==0) = nan;
    h.YData(pixVal==0) = nan;
    h.ZData(pixVal==0) = nan;
    h.CData(pixVal==0) = nan;
    %     h.EdgeColor = [0.3,0.3,0.3];
    daspect([1 1 1]);
    % view(3);
    axis vis3d tight;
    rotate3d on
end
end


function adj = meshAdjacencyMatrix(faces, varargin)
%MESHADJACENCYMATRIX Compute adjacency matrix of a mesh from set of faces
%
%   ADJMAT = meshAdjacencyMatrix(FACES)
%   Returns a sparse NV-by-NV matrix (NV being the maximum vertex index)
%   containing vertex adjacency of the mesh represented by FACES.
%   FACES is either a NF-by-3, a NF-by-4 index array, or a Nf-by-1 cell
%   array.
%
%   Example
%     [v f] = createCube;
%     adj = meshAdjacencyMatrix(f);
%
%   See also
%     meshes3d, triangulateFaces, smoothMesh

% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2013-04-30,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2013 INRA - Cepia Software Platform.

% Ensures faces is a N-by-3 or N-by-4 array
if iscell(faces) || (isnumeric(faces) && size(faces, 2) > 4)
    faces = triangulateFaces(faces);
end

% forces faces to b efloating point array, for sparse function
if ~isfloat(faces)
    faces = double(faces);
end

% populate a sparse matrix
if size(faces, 2) == 3
    adj = sparse(...
        [faces(:,1); faces(:,1); faces(:,2); faces(:,2); faces(:,3); faces(:,3)], ...
        [faces(:,3); faces(:,2); faces(:,1); faces(:,3); faces(:,2); faces(:,1)], ...
        1.0);
elseif size(faces, 2) == 4
    adj = sparse(...
        [faces(:,1); faces(:,1); faces(:,2); faces(:,2); faces(:,3); faces(:,3); faces(:,4); faces(:,4)], ...
        [faces(:,4); faces(:,2); faces(:,1); faces(:,3); faces(:,2); faces(:,4); faces(:,3); faces(:,1)], ...
        1.0);
end

% remove double adjacencies
adj = min(adj, 1);


function lissajous ( )

%*****************************************************************************80
%
%% LISSAJOUS uses MATLAB to draw a closed planar curve.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 May 2011
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'LISSAJOUS:\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Make a plane curve by connecting a series of points.\n' );

  xy = load ( 'lissajous.txt' );
%
%  Plot the data.
%
  plot ( xy(:,1), xy(:,2), 'LineWidth', 2, 'Color', 'm' );
  hold on
%
%  To avoid clutter, only plot every 10th point.
%
  plot ( xy(1:10:end,1), xy(1:10:end,2), 'k.' );
  grid on
  axis ( [ -1.2, +1.2, -1.2, +1.2 ] )
  axis equal
  xlabel ( '<--- X --->' );
  ylabel ( '<--- Y --->' );
  title ( 'Lissajous, x=sin(3t+pi/2), y=sin(4t)' );
  hold off

  fprintf ( 1, '\n' );
  fprintf ( 1, 'LISSAJOUS:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  return
end


%
% Get a motor trajectory from the b-spline control points,
% using an adaptive method to choose the number of evaluations
% based on the distance along the trajectory.
%
% Input
%
%  P: [ncpt x 2] control points
%  neval: (optional) number of evaluations
%    otherwise, we choose this adaptively
%
% Output
%  stk: [m x 2] trajectory
%
function stk = get_stk_from_bspline(P,neval)
    nland = size(P,1);
    if ~exist('neval','var')

        % set the number of evaluations adaptively,
        % based on the size of the stroke
        PM = defaultps;
        neval = PM.spline_min_neval;
        s = bspline_gen_s(nland,neval);
        stk = bspline_eval(s,P);
        sumdist = sum_pair_dist(stk);
        neval = max(neval,ceil(sumdist./PM.spline_grain));
        neval = min(neval,PM.spline_max_neval);

    end

    s = bspline_gen_s(nland,neval);
    stk = bspline_eval(s,P);
end

function s = sum_pair_dist(D)
    x1 = D(1:end-1,:);
    x2 = D(2:end,:);
    z = sqrt(sum((x1-x2).^2,2));
    s = sum(z);
end