function [beta_median,beta_std,beta_lbound,beta_ubound,sigma_median,beta_mean_median,beta_mean_lbound,beta_mean_ubound,sigma_mean_median]...
    =panel4estimates(N,n,q,beta_gibbs,sigma_gibbs,cband, beta_mean,sigma_mean)

% compute first the  percentiles for the mean model values, i.e. beta_mean
% and sigma_man
%
% for jj=1:n^2
%  sigma_mean_median(jj,1)=[quantile(sigma_mean(jj,:),0.5)];
% end
%    for jj=1:q
%    beta_mean_median(jj,1)=[quantile(beta_mean(jj,:),0.5)];
%    beta_mean_lbound(jj,1)=[quantile(beta_gibbs(jj,1),(1-cband)/2)];
%    beta_mean_ubound(jj,1)=[quantile(beta_gibbs(jj,1),1-(1-cband)/2)];
%    end
% as the VAR coefficients are estimated for each unit, loop over units
% move to the country specific coefficients
for ii=1:N
   % loop over VAR coefficients
   for jj=1:q
   beta_median(jj,1,ii)=[quantile(beta_gibbs(jj,:,ii),0.5)];
   beta_std(jj,1,ii)=std(beta_gibbs(jj,:,ii));
   beta_lbound(jj,1,ii)=[quantile(beta_gibbs(jj,1,ii),(1-cband)/2)];
   beta_ubound(jj,1,ii)=[quantile(beta_gibbs(jj,1,ii),1-(1-cband)/2)];
   end
   % loop over sigma entries
   for jj=1:n^2
   sigma_median(jj,1,ii)=[quantile(sigma_gibbs(jj,:,ii),0.5)];
   end
end


function  [h, compUpV, compUp] =  lfmComputeH4VP(gamma1_p, gamma1_m, sigma2, t1, ...
    preFactor, preExp, mode)

% LFMCOMPUTEH4VP Helper function for computing part of the LFMVXLFM kernel.
% FORMAT
% DESC computes a portion of the LFMVXLFM kernel.
% ARG gamma1 : Gamma value for first system.
% ARG gamma2 : Gamma value for second system.
% ARG sigma2 : length scale of latent process.
% ARG t1 : first time input (number of time points x 1).
% ARG t2 : second time input (number of time points x 1).
% ARG mode: indicates in which way the vectors t1 and t2 must be transposed
% RETURN h : result of this subcomponent of the kernel for the given values.
%
% COPYRIGHT : Mauricio A. Alvarez, 2010
%
% SEEALSO : lfmComputeH4Hat, lfmXlfmKernCompute

% KERN

% This could also be used with str2func changing between 'lfm' and 'lfmvp'

if mode==0
    if nargout > 1
        [compUpV{1}, compUp{1}] = lfmvpComputeUpsilonVector(gamma1_p,sigma2, t1, 0);
        [compUpV{2}, compUp{2}] = lfmvpComputeUpsilonVector(gamma1_m,sigma2, t1, 0);
        h =  compUpV{1}*( preExp(:,1)/preFactor(1) - preExp(:,2)/preFactor(2)).' ...
            + compUpV{2}*( preExp(:,2)/preFactor(3) - preExp(:,1)/preFactor(4)).';
    else
        h =  lfmvpComputeUpsilonVector(gamma1_p,sigma2, t1, 0)*( preExp(:,1)/preFactor(1) - preExp(:,2)/preFactor(2)).' ...
            + lfmvpComputeUpsilonVector(gamma1_m,sigma2, t1, 0)*( preExp(:,2)/preFactor(3) - preExp(:,1)/preFactor(4)).';
    end
else
    if nargout > 1
        compUp{1} = lfmComputeUpsilonVector(gamma1_p,sigma2, t1);
        compUp{2} = lfmComputeUpsilonVector(gamma1_m,sigma2, t1);
        h =  compUp{1}*(  preExp(:,2)/preFactor(2) - preExp(:,1)/preFactor(1)).' ...
            + compUp{2}*( preExp(:,1)/preFactor(4) - preExp(:,2)/preFactor(3)).';
        compUpV = compUp;
    else
        h =  lfmComputeUpsilonVector(gamma1_p,sigma2, t1)*(  preExp(:,2)/preFactor(2) - preExp(:,1)/preFactor(1)).' ...
            + lfmComputeUpsilonVector(gamma1_m,sigma2, t1)*( preExp(:,1)/preFactor(4) - preExp(:,2)/preFactor(3)).';
    end
end


function nc=normcor(a,b)
m=size(a);
n=size(b);
a=im2double(a);
b=im2double(b);
a1=mean2(a);
b1=mean2(b);
c1=0;c2=0;
for i=1:m
    for j=1:n
      c1=c1+(a(i,j)-a1);
      c2=c2+(b(i,j)-b1);
      num=c1*c2;
      c3=(c1^2)*(c2^2);
      dem=sqrt(c3);
    end
end
nc=num/dem;


function [ywc1, ywt1, ywc2, ywt2] = wavefilter(X, fig)

% wavelets (Haar) filtering: see Lubik, Matthes, Verona (2019)

%  ywc1 has the  cycles at frequencies 8-32 quarters (loose 16 datapoints)
%  ywc2 has the  cycles at frequencies 8-64 quarters (loose 32 datapoints)
%  ywt1 and  ywt2 have  the trends  and are defined  as  residuals

enddT=size(X,1);
ywc1=zeros(enddT,size(X,2));   ywt1=zeros(enddT,size(X,2));
ywc2=zeros(enddT,size(X,2));   ywt2=zeros(enddT,size(X,2));

for qq=1:size(X,2)
%    y=zeros(enddT,1);
    xx1=zeros(enddT,1); xx2=zeros(enddT,1); xx3=zeros(enddT,1);
    xx4=zeros(enddT,1); xx5=zeros(enddT,1);

    y=squeeze(X(:,qq));

    % 2-4  quarters cycles
    for  tt=2:length(y)
    %xx1(tt,1)=(1/2)*(y(tt)-(y(tt)-y(tt-1)));
    xx1(tt,1)=(1/2)*(y(tt)-y(tt-1));
    end
    % 4-8  quarters cycles
    for  tt=4:length(y)
    xx2(tt,1)=(1/4)*(y(tt)+y(tt-1)-(y(tt-2)+y(tt-3)) );
    end
    % 8-16  quarters cycles
    for  tt=8:length(y)
    xx3(tt,1)=(1/8)*(y(tt)+y(tt-1)+y(tt-2)+y(tt-3)- ...
                  (y(tt-4)+y(tt-5)+y(tt-6)+y(tt-7)) );
    end
    % 16-32 quarters cycles
    for  tt=16:length(y)
    xx4(tt,1)=(1/16)*(y(tt)+y(tt-1)+y(tt-2)+y(tt-3)+y(tt-4)+ ...
                    y(tt-5)+y(tt-6)+y(tt-7)-...
                   (y(tt-8)+y(tt-9)+y(tt-10)+y(tt-11)+y(tt-12)+ ...
                    y(tt-13)+y(tt-14)+y(tt-15)) );
    end
    %  32-64 quarters cycles
    for  tt=32:length(y)
     xx5(tt,1)=(1/32)*(y(tt)+y(tt-1)+y(tt-2)+y(tt-3)+y(tt-4)+y(tt-5)+ ...
        y(tt-6)+y(tt-7)+y(tt-8)+y(tt-9)+y(tt-10)+ y(tt-11)+y(tt-12)+ ...
         y(tt-13)+ y(tt-14)+y(tt-15) - ...
        (y(tt-16)+y(tt-17)+y(tt-18)+y(tt-19)+y(tt-20)+y(tt-21)+ ...
         y(tt-22)+y(tt-23)+y(tt-24)+y(tt-25)+y(tt-26)+y(tt-27)+ ...
         y(tt-28)+y(tt-29)+y(tt-30)+y(tt-31) ) );
    end
   % cyclical components
   ywc1(16:length(y),qq)=xx3(16:length(y))+xx4(16:length(y));
   ywc2(32:length(y),qq)=xx3(32:length(y))+...
                       xx4(32:length(y))+xx5(32:length(y));
   % trend components (actual-BC cycles-high frequnecy cycles)
   ywt1(16:length(y),qq)=y(16:length(y))-ywc1(16:length(y),qq) ...
                         -xx1(16:length(y))-xx2(16:length(y));
   ywt2(32:length(y),qq)=y(32:length(y))-ywc2(32:length(y),qq) ...
                         -xx1(32:length(y))-xx2(32:length(y));

% xxx=[xx1 xx2 xx3 xx4  xx5]
% figure(100)
%  plot(xxx)

  if fig==1
      % plot  cyclical components
   figure(1)
   plot(ywc1(1:length(y),qq),'r', 'linewidth',2); hold  on;
   plot(ywc2(1:length(y), qq),'b','linewidth',2); hold  off; axis  tight;
   legend('BC(8-32)','BC+LOW(8-64)')
   pause
   end
 end

end


%% Ex. 8 Another example of elementary functions with a vectorial variable

a = [2 3 5];
b = 2*a.^2+3*a+4

%Output:
%        b = 18 31 69
%   Remark: The content of b is
%    b = [2*(a(1))^2+3*a(1)+4 2*(a(2))^2+3*a(2)+4 2*(a(3))^2+3*a(3)+4].


function par_estc
% transport parameter estimation with derivatives             Holzbecher January 2006

global xfit cfit T D c0 c1

% Example values for Chlorid in Marmara Sea Sediment Core
T = 3.15e11;  % [s] 10.000 years
D = 1.0e-5;  % [cm*cm/s]
c0 = 0;      % [mmol/l]
c1 = 619;    % [mmol/l]
xmax = 4000; % [cm]

% specify fitting data
xfit = [0 20 40 60 100 120 140 160 215 255 275 300 375 450 525 600 750 1050 1200 1350 1650 1950 2250 2550 2700 3000 3450 3900];
cfit = [619 597 608 615 619 615 621 571 621 618 619 625 577 612 608 612 609 590 582 582 556 494 457 489 487 444 381 371];

x = [0:xmax/400:xmax];
options = optimset('Display','iter','TolFun',1e-9);
v = fzero(@myfun,0.2e-8,options);
display (['Best fit for v = ' num2str(v)]);

h = 1./(2.*sqrt(D*T)); e = diag(eye(size(x,2)));
plot (xfit,cfit,'o',x,c0+0.5*c1*(erfc(h*(x-v*T*e'))+(exp((v/D)*x)).*erfc(h*(x+v*T*e'))),'-');
legend ('given','modelled');
xlabel ('depth [cm]'); ylabel ('chloride concentration [mmol/l]');
text(0.1*xmax,c1*0.65,['sedimentation velocity [cm/a]: ' num2str(v*3.15e7)]);
e = diag(eye(size(xfit,2)));
normc = norm(cfit-c0+0.5*c1*(erfc(h*(xfit-v*T*e'))+(exp((v/D)*xfit)).*erfc(h*(xfit+v*T*e'))));
text(0.1*xmax,c1*0.6,['norm of residuals: ' num2str(normc)]);

function f = myfun(v);
global xfit cfit T D c0 c1

e=diag(eye(size(xfit,2))); h=1./(2.*sqrt(D*T));
arg1 = h*(xfit-v*T*e'); arg2 = h*(xfit+v*T*e'); arg3 = (v/D)*xfit;

% solve advection diffusion equation for c with c(t=0)=c0 and c(x=0)=c1
c = c0 + 0.5*c1*(erfc(arg1)+(exp(arg3).*erfc(arg2)));

% compute derivative of solution due to v
cv = c1*((T*h/sqrt(pi))*(exp(-arg1.*arg1)-exp(arg3).*exp(-arg2.*arg2))+0.5*(xfit/D).*exp(arg3).*erfc(arg2));

% specify function f to vanish
f = 2*(c-cfit)*cv';


function [xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk,dxpdalpha] = project_points2(X,f,c,k,alpha)

%project_points2.m
%
%[xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points2(X,om,T,f,c,k,alpha)
%
%Projects a 3D structure onto the image plane.
%
%INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points)
%       (om,T): Rigid motion parameters between world coordinate frame and camera reference frame
%               om: rotation vector (3x1 vector); T: translation vector (3x1 vector)
%       f: camera focal length in units of horizontal and vertical pixel units (2x1 vector)
%       c: principal point location in pixel units (2x1 vector)
%       k: Distortion coefficients (radial and tangential) (4x1 vector)
%       alpha: Skew coefficient between x and y pixel (alpha = 0 <=> square pixels)
%
%OUTPUT: xp: Projected pixel coordinates (2xN matrix for N points)
%        dxpdom: Derivative of xp with respect to om ((2N)x3 matrix)
%        dxpdT: Derivative of xp with respect to T ((2N)x3 matrix)
%        dxpdf: Derivative of xp with respect to f ((2N)x2 matrix if f is 2x1, or (2N)x1 matrix is f is a scalar)
%        dxpdc: Derivative of xp with respect to c ((2N)x2 matrix)
%        dxpdk: Derivative of xp with respect to k ((2N)x4 matrix)
%
%Definitions:
%Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X)
%The coordinate vector of P in the camera reference frame is: Xc = R*X + T
%where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om);
%call x, y and z the 3 coordinates of Xc: x = Xc(1); y = Xc(2); z = Xc(3);
%The pinehole projection coordinates of P is [a;b] where a=x/z and b=y/z.
%call r^2 = a^2 + b^2.
%The distorted point coordinates are: xd = [xx;yy] where:
%
%xx = a * (1 + kc(1)*r^2 + kc(2)*r^4 + kc(5)*r^6)      +      2*kc(3)*a*b + kc(4)*(r^2 + 2*a^2);
%yy = b * (1 + kc(1)*r^2 + kc(2)*r^4 + kc(5)*r^6)      +      kc(3)*(r^2 + 2*b^2) + 2*kc(4)*a*b;
%
%The left terms correspond to radial distortion (6th degree), the right terms correspond to tangential distortion
%
%Finally, convertion into pixel coordinates: The final pixel coordinates vector xp=[xxp;yyp] where:
%
%xxp = f(1)*(xx + alpha*yy) + c(1)
%yyp = f(2)*yy + c(2)
%
%
%NOTE: About 90 percent of the code takes care fo computing the Jacobian matrices
%
%
%Important function called within that program:
%
%rodrigues.m: Computes the rotation matrix corresponding to a rotation vector
%
%rigid_motion.m: Computes the rigid motion transformation of a given structure

[m,n] = size(X);

Y = X;

inv_Z = 1./Y(3,:);

x = (Y(1:2,:) .* (ones(2,1) * inv_Z)) ;

bb = (-x(1,:) .* inv_Z)'*ones(1,3);
cc = (-x(2,:) .* inv_Z)'*ones(1,3);

if nargout > 1,
    dxdom = zeros(2*n,3);
    dxdom(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(1:3:end,:) + bb .* dYdom(3:3:end,:);
    dxdom(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(2:3:end,:) + cc .* dYdom(3:3:end,:);

    dxdT = zeros(2*n,3);
    dxdT(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(1:3:end,:) + bb .* dYdT(3:3:end,:);
    dxdT(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(2:3:end,:) + cc .* dYdT(3:3:end,:);
end;

% Add distortion:

r2 = x(1,:).^2 + x(2,:).^2;

if nargout > 1,
    dr2dom = 2*((x(1,:)')*ones(1,3)) .* dxdom(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdom(2:2:end,:);
    dr2dT = 2*((x(1,:)')*ones(1,3)) .* dxdT(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdT(2:2:end,:);
end;

r4 = r2.^2;

if nargout > 1,
    dr4dom = 2*((r2')*ones(1,3)) .* dr2dom;
    dr4dT = 2*((r2')*ones(1,3)) .* dr2dT;
end

r6 = r2.^3;

if nargout > 1,
    dr6dom = 3*((r2'.^2)*ones(1,3)) .* dr2dom;
    dr6dT = 3*((r2'.^2)*ones(1,3)) .* dr2dT;
end;

% Radial distortion:

cdist = 1 + k(1) * r2 + k(2) * r4 + k(5) * r6;

if nargout > 1,
    dcdistdom = k(1) * dr2dom + k(2) * dr4dom + k(5) * dr6dom;
    dcdistdT = k(1) * dr2dT + k(2) * dr4dT + k(5) * dr6dT;
    dcdistdk = [ r2' r4' zeros(n,2) r6'];
end;

xd1 = x .* (ones(2,1)*cdist);

if nargout > 1,
    dxd1dom = zeros(2*n,3);
    dxd1dom(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdom;
    dxd1dom(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdom;
    coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3));
    dxd1dom = dxd1dom + coeff.* dxdom;

    dxd1dT = zeros(2*n,3);
    dxd1dT(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdT;
    dxd1dT(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdT;
    dxd1dT = dxd1dT + coeff.* dxdT;

    dxd1dk = zeros(2*n,5);
    dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,5)) .* dcdistdk;
    dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,5)) .* dcdistdk;
end;

% tangential distortion:

a1 = 2.*x(1,:).*x(2,:);
a2 = r2 + 2*x(1,:).^2;
a3 = r2 + 2*x(2,:).^2;

delta_x = [k(3)*a1 + k(4)*a2 ;
    k(3) * a3 + k(4)*a1];

%ddelta_xdx = zeros(2*n,2*n);
aa = (2*k(3)*x(2,:)+6*k(4)*x(1,:))'*ones(1,3);
bb = (2*k(3)*x(1,:)+2*k(4)*x(2,:))'*ones(1,3);
cc = (6*k(3)*x(2,:)+2*k(4)*x(1,:))'*ones(1,3);

if nargout > 1,
    ddelta_xdom = zeros(2*n,3);
    ddelta_xdom(1:2:end,:) = aa .* dxdom(1:2:end,:) + bb .* dxdom(2:2:end,:);
    ddelta_xdom(2:2:end,:) = bb .* dxdom(1:2:end,:) + cc .* dxdom(2:2:end,:);

    ddelta_xdT = zeros(2*n,3);
    ddelta_xdT(1:2:end,:) = aa .* dxdT(1:2:end,:) + bb .* dxdT(2:2:end,:);
    ddelta_xdT(2:2:end,:) = bb .* dxdT(1:2:end,:) + cc .* dxdT(2:2:end,:);

    ddelta_xdk = zeros(2*n,5);
    ddelta_xdk(1:2:end,3) = a1';
    ddelta_xdk(1:2:end,4) = a2';
    ddelta_xdk(2:2:end,3) = a3';
    ddelta_xdk(2:2:end,4) = a1';
end;

xd2 = xd1 + delta_x;

if nargout > 1,
    dxd2dom = dxd1dom + ddelta_xdom ;
    dxd2dT = dxd1dT + ddelta_xdT;
    dxd2dk = dxd1dk + ddelta_xdk ;
end;

% Add Skew:

xd3 = [xd2(1,:) + alpha*xd2(2,:);xd2(2,:)];

% Compute: dxd3dom, dxd3dT, dxd3dk, dxd3dalpha
if nargout > 1,
    dxd3dom = zeros(2*n,3);
    dxd3dom(1:2:2*n,:) = dxd2dom(1:2:2*n,:) + alpha*dxd2dom(2:2:2*n,:);
    dxd3dom(2:2:2*n,:) = dxd2dom(2:2:2*n,:);
    dxd3dT = zeros(2*n,3);
    dxd3dT(1:2:2*n,:) = dxd2dT(1:2:2*n,:) + alpha*dxd2dT(2:2:2*n,:);
    dxd3dT(2:2:2*n,:) = dxd2dT(2:2:2*n,:);
    dxd3dk = zeros(2*n,5);
    dxd3dk(1:2:2*n,:) = dxd2dk(1:2:2*n,:) + alpha*dxd2dk(2:2:2*n,:);
    dxd3dk(2:2:2*n,:) = dxd2dk(2:2:2*n,:);
    dxd3dalpha = zeros(2*n,1);
    dxd3dalpha(1:2:2*n,:) = xd2(2,:)';
end;

% Pixel coordinates:
if length(f)>1,
    xp = xd3 .* (f(:) * ones(1,n))  +  c(:)*ones(1,n);
    if nargout > 1,
        coeff = reshape(f(:)*ones(1,n),2*n,1);
        dxpdom = (coeff*ones(1,3)) .* dxd3dom;
        dxpdT = (coeff*ones(1,3)) .* dxd3dT;
        dxpdk = (coeff*ones(1,5)) .* dxd3dk;
        dxpdalpha = (coeff) .* dxd3dalpha;
        dxpdf = zeros(2*n,2);
        dxpdf(1:2:end,1) = xd3(1,:)';
        dxpdf(2:2:end,2) = xd3(2,:)';
    end;
else
    xp = f * xd3 + c*ones(1,n);
    if nargout > 1,
        dxpdom = f  * dxd3dom;
        dxpdT = f * dxd3dT;
        dxpdk = f  * dxd3dk;
        dxpdalpha = f .* dxd3dalpha;
        dxpdf = xd3(:);
    end;
end;

if nargout > 1,
    dxpdc = zeros(2*n,2);
    dxpdc(1:2:end,1) = ones(n,1);
    dxpdc(2:2:end,2) = ones(n,1);
end;

return;

% Test of the Jacobians:

n = 10;

X = 10*randn(3,n);
om = randn(3,1);
T = [10*randn(2,1);40];
f = 1000*rand(2,1);
c = 1000*randn(2,1);
k = 0.5*randn(5,1);
alpha = 0.01*randn(1,1);

[x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X,om,T,f,c,k,alpha);

% Test on om: OK

dom = 0.000000001 * norm(om)*randn(3,1);
om2 = om + dom;

[x2] = project_points2(X,om2,T,f,c,k,alpha);

x_pred = x + reshape(dxdom * dom,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on T: OK!!

dT = 0.0001 * norm(T)*randn(3,1);
T2 = T + dT;

[x2] = project_points2(X,om,T2,f,c,k,alpha);

x_pred = x + reshape(dxdT * dT,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on f: OK!!

df = 0.001 * norm(f)*randn(2,1);
f2 = f + df;

[x2] = project_points2(X,om,T,f2,c,k,alpha);

x_pred = x + reshape(dxdf * df,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on c: OK!!

dc = 0.01 * norm(c)*randn(2,1);
c2 = c + dc;

[x2] = project_points2(X,om,T,f,c2,k,alpha);

x_pred = x + reshape(dxdc * dc,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on k: OK!!

dk = 0.001 * norm(k)*randn(5,1);
k2 = k + dk;

[x2] = project_points2(X,om,T,f,c,k2,alpha);

x_pred = x + reshape(dxdk * dk,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on alpha: OK!!

dalpha = 0.001 * norm(k)*randn(1,1);
alpha2 = alpha + dalpha;

[x2] = project_points2(X,om,T,f,c,k,alpha2);

x_pred = x + reshape(dxdalpha * dalpha,2,n);

norm(x2-x)/norm(x2 - x_pred)


clf reset;
echo on

% This script demonstrates the use of the RSOM.
clc;

% load the example dissimilarity data
load exampleDissimilarity.mat;

% display the eigenvalue spectrum
[V, D] = eig(Dissim);
eVals = diag(D);
figure; bar(eVals);

pause % Strike any key to continues...
clc;

% init RSOM
sMap = rsom_lininit(Dissim,  [10 10]);

% train the RSOM
sMap = rsom_batchtrain(sMap, Dissim);

% Display the U-Matrix
figure;
rsom_show(sMap, Dissim);

% The U-Matrix shows clearly the structure of the data, i.e. that two
% clusters are available

pause % Strike any key to continues...
clc;

% do linear embedding of the distance matrix into a 3 dimensional space
x = cmdscale(Dissim.^(1/2));
x = x(:,1:3);

% plot the resulting data
h = figure; hold on;
plot3(x(1:100, 1), x(1:100, 2), x(1:100, 3), '.b');
plot3(x(101:200, 1), x(101:200, 2), x(101:200, 3), '.r');

% Since approximated vectorial data are available now, we can compute
% the (approximated) neuron positions and plot them
Neurons = sMap.cCodebook * x;
figure(h);

% create a som struct
sMapSOM = som_map_struct(3, sMap.topol);
som_grid(sMapSOM,'Coord',Neurons);

echo off;


function par_estc
% transport parameter estimation with derivatives             Holzbecher January 2006

global xfit cfit T D c0 c1

% Example values for Chlorid in Marmara Sea Sediment Core
T = 3.15e11;  % [s] 10.000 years
D = 1.0e-5;  % [cm*cm/s]
c0 = 0;      % [mmol/l]
c1 = 619;    % [mmol/l]
xmax = 4000; % [cm]

% specify fitting data
xfit = [0 20 40 60 100 120 140 160 215 255 275 300 375 450 525 600 750 1050 1200 1350 1650 1950 2250 2550 2700 3000 3450 3900];
cfit = [619 597 608 615 619 615 621 571 621 618 619 625 577 612 608 612 609 590 582 582 556 494 457 489 487 444 381 371];

x = [0:xmax/400:xmax];
options = optimset('Display','iter','TolFun',1e-9);
v = fzero(@myfun,0.2e-8,options);
display (['Best fit for v = ' num2str(v)]);

h = 1./(2.*sqrt(D*T)); e = diag(eye(size(x,2)));
plot (xfit,cfit,'o',x,c0+0.5*c1*(erfc(h*(x-v*T*e'))+(exp((v/D)*x)).*erfc(h*(x+v*T*e'))),'-');
legend ('given','modelled');
xlabel ('depth [cm]'); ylabel ('chloride concentration [mmol/l]');
text(0.1*xmax,c1*0.65,['sedimentation velocity [cm/a]: ' num2str(v*3.15e7)]);
e = diag(eye(size(xfit,2)));
normc = norm(cfit-c0+0.5*c1*(erfc(h*(xfit-v*T*e'))+(exp((v/D)*xfit)).*erfc(h*(xfit+v*T*e'))));
text(0.1*xmax,c1*0.6,['norm of residuals: ' num2str(normc)]);

function f = myfun(v)
global xfit cfit T D c0 c1

e=diag(eye(size(xfit,2))); h=1./(2.*sqrt(D*T));
arg1 = h*(xfit-v*T*e'); arg2 = h*(xfit+v*T*e'); arg3 = (v/D)*xfit;

% solve advection diffusion equation for c with c(t=0)=c0 and c(x=0)=c1
c = c0 + 0.5*c1*(erfc(arg1)+(exp(arg3).*erfc(arg2)));

% compute derivative of solution due to v
cv = c1*((T*h/sqrt(pi))*(exp(-arg1.*arg1)-exp(arg3).*exp(-arg2.*arg2))+0.5*(xfit/D).*exp(arg3).*erfc(arg2));

% specify function f to vanish
f = 2*(c-cfit)*cv';


function [ r, seed ] = r8col_uniform_abvec ( m, n, a, b, seed )

%*****************************************************************************80
%
%% R8COL_UNIFORM_ABVEC fills an R8COL with scaled pseudorandom numbers.
%
%  Discussion:
%
%    An R8COL is an array of R8 values, regarded as a set of column vectors.
%
%    The user specifies a minimum and maximum value for each row.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 December 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Paul Bratley, Bennett Fox, Linus Schrage,
%    A Guide to Simulation,
%    Springer Verlag, pages 201-202, 1983.
%
%    Bennett Fox,
%    Algorithm 647:
%    Implementation and Relative Efficiency of Quasirandom
%    Sequence Generators,
%    ACM Transactions on Mathematical Software,
%    Volume 12, Number 4, pages 362-376, 1986.
%
%    Peter Lewis, Allen Goodman, James Miller,
%    A Pseudo-Random Number Generator for the System/360,
%    IBM Systems Journal,
%    Volume 8, pages 136-143, 1969.
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns in
%    the array.
%
%    Input, real A(M), B(M), the lower and upper limits.
%
%    Input/output, integer SEED, the "seed" value, which
%    should NOT be 0.  On output, SEED has been updated.
%
%    Output, real R(M,N), the array of pseudorandom values.
%
  i4_huge = 2147483647;

  for j = 1 : n

    for i = 1 : m

      k = floor ( seed / 127773 );

      seed = 16807 * ( seed - k * 127773 ) - k * 2836;

      if ( seed < 0 )
        seed = seed + i4_huge;
      end

      r(i,j) = a(i) + ( b(i) - a(i) ) * seed * 4.656612875E-10;

    end
  end

  return
end


function [ r, seed ] = r8row_uniform_abvec ( m, n, a, b, seed )

%*****************************************************************************80
%
%% R8ROW_UNIFORM_ABVEC fills an R8ROW with scaled pseudorandom numbers.
%
%  Discussion:
%
%    An R8ROW is an array of R8 values, regarded as a set of row vectors.
%
%    The user specifies a minimum and maximum value for each column.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 January 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Paul Bratley, Bennett Fox, Linus Schrage,
%    A Guide to Simulation,
%    Springer Verlag, pages 201-202, 1983.
%
%    Bennett Fox,
%    Algorithm 647:
%    Implementation and Relative Efficiency of Quasirandom
%    Sequence Generators,
%    ACM Transactions on Mathematical Software,
%    Volume 12, Number 4, pages 362-376, 1986.
%
%    Peter Lewis, Allen Goodman, James Miller,
%    A Pseudo-Random Number Generator for the System/360,
%    IBM Systems Journal,
%    Volume 8, pages 136-143, 1969.
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns in
%    the array.
%
%    Input, real A(N), B(N), the lower and upper limits.
%
%    Input/output, integer SEED, the "seed" value, which
%    should NOT be 0.  On output, SEED has been updated.
%
%    Output, real R(M,N), the array of pseudorandom values.
%
  i4_huge = 2147483647;

  for i = 1 : m

    for j = 1 : n

      k = floor ( seed / 127773 );

      seed = 16807 * ( seed - k * 127773 ) - k * 2836;

      if ( seed < 0 )
        seed = seed + i4_huge;
      end

      r(i,j) = a(j) + ( b(j) - a(j) ) * seed * 4.656612875E-10;

    end
  end

  return
end


function btv_test05 ( )

%*****************************************************************************80
%
%% BTV_TEST05 tests BURGERS_TIME_VISCOUS with the expansion initial condition.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 April 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BTV_TEST05\n' );
  fprintf ( 1, '  Test BURGERS_TIME_VISCOUS with the expansion initial condition.\n' );
  fprintf ( 1, '  Use periodic boundaries.\n' );

  nx = 81;
  nt = 200;
  t_max = 2.0;
  nu = 0.01;
  bc = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Initial condition: expansion\n' );
  fprintf ( 1, '  Number of space nodes = %d\n', nx );
  fprintf ( 1, '  Number of time steps = %d\n', nt );
  fprintf ( 1, '  Final time T_MAX = %g\n', t_max );
  fprintf ( 1, '  Viscosity = %g\n', nu );
  fprintf ( 1, '  Boundary condition = %d\n', bc );

  U = burgers_time_viscous ( @ic_expansion, nx, nt, t_max, nu, bc );

  x = linspace ( -1.0, +1.0, nx );

  figure ( 5 )

  plot ( x, U(1:50:(nt+1),:), 'Linewidth', 3 )
  grid on
  xlabel ( '<-- X -->' )
  ylabel ( '<-- U(X,T) -->' )
  title ( 'Burgers equation solutions over time, initial condition expansion' )

  filename = 'btv_test05.png';
  print ( '-dpng', filename )
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Saved plot as "%s"\n', filename );

  return
end


function [pos, vel] = vectrs (ra, dec, pmra, pmdec, parllx, rv)

% this function converts angular quantities related to a star's
% position and motion to vectors.

% input

%  ra     = right ascension in hours

%  dec    = declination in degrees

%  pmra   = proper motion in ra in milliarcseconds per year

%  pmdec  = proper motion in dec in milliarcseconds per year

%  parllx = parallax in milliarcseconds

%  rv     = radial velocity in kilometers/second

% output

%  pos = position vector, equatorial rectangular coordinates,
%           with respect to solar system barycenter, components in au

%  vel = velocity vector, equatorial rectangular coordinates,
%        with respect to solar system barycenter, components in au/day

% ported from NOVAS 3.0

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

seccon = 180.0d0 * 3600.0d0 / pi;

% speed of light in kilometers/second

c = 1.0d-3 * 2997924580.0d0;

% au in kilometers

aukm = 1.0d-3 * 499.0047838061d0 * c;

% if parallax is unknown, undetermined, or zero, set it to 1e-6
% milliarcsecond, corresponding to a distance of 1 gigaparsec

paralx = parllx;

if ( paralx <= 0.0d0 )
    paralx = 1.0d-6;
end

% convert right ascension, declination, and parallax to position
% vector in equatorial system with units of au

dist = 1.0d0 / sin (paralx * 1.0d-3 / seccon);

r = ra * 54000.0d0 / seccon;

d = dec * 3600.0d0 / seccon;

cra = cos(r);

sra = sin(r);

cdc = cos(d);

sdc = sin(d);

pos(1) = dist * cdc * cra;

pos(2) = dist * cdc * sra;

pos(3) = dist * sdc;

% compute doppler factor, which accounts for change in
% light travel time to star

k = 1.d0 / (1.0d0 - rv / c);

% convert proper motion and radial velocity to orthogonal components
% of motion with units of au/day

pmr = pmra  / (paralx * 365.25d0) * k;

pmd = pmdec / (paralx * 365.25d0) * k;

rvl = rv * 86400.0d0 / aukm       * k;

% transform motion vector to equatorial system

vel(1) = - pmr * sra - pmd * sdc * cra + rvl * cdc * cra;

vel(2) =   pmr * cra - pmd * sdc * sra + rvl * cdc * sra;

vel(3) =               pmd * cdc       + rvl * sdc;


%----------------------------------------------------------------------
%Find the largest eigenvalue
%----------------------------------------------------------------------

function [ opts ] = find_max_eigenv ( matrix )

[v,s]     =       eig(matrix);
sort_s    =       sort(diag(s),'descend');
opts      =       sort_s(1);


function out = chebcoeffs(f, N, kind)
%CHEBCOEFFS   Chebyshev polynomial coefficients of a TRIGTECH.
%   A = CHEBCOEFFS(F, N) or A = CHEBCOEFFS(F, N, 1) returns a column vector of
%   the first N coefficients in the expansion of F in a series of Chebyshev
%   polynomials of the first kind, ordered starting with the coefficient of
%   T_0(x).
%
%   A = CHEBCOEFFS(F, N, 2) does the same but for a series expansion in
%   Chebyshev polynomials of second kind, ordered starting with the coefficient
%   of U_0(x).
%
%   If F is array-valued with P columns, then A is an NxP matrix.
%
% See also LEGCOEFFS, TRIGCOEFFS.

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

if ( (nargin < 2) || isempty(N) )
    error('CHEBFUN:TRIGTECH:chebcoeffs:input', ...
        'F does not have a finite Chebyshev series. Please input N.');
end

% Use first-kind Chebyshev polynomials by default.
if ( (nargin < 3) || isempty(kind) )
    kind = 1;
end

% Trivial empty case:
if ( N <= 0 )
    out = [];
    return
end

% [TODO]: Is there a fast transfrom from TRIGTECH to CHEBTECH?
% Since f is a TRIGTECH it is assumed to be smooth and periodic on [-1,1].
% Computing the chebyshev coefficients via innner products requires working with
% non-periodic, but smooth functions on [-1,1]. The right representation for f
% is then a Chebyshev expansion. We therefore convert f to a (happy) Chebyshev
% interpolant and return the coefficients. As an arbitrary choice we will
% convert f to a chebtech1 and then compute the resulting coefficients.
out = chebcoeffs(chebtech1(@(x) f.feval(x)), N, kind);

end


function sol = Inverse_Kinematics(q,pd,l)
%
px = pd(1);
py = pd(2);
l1 = l(1);
l2 = l(2);
distance = px^2 + py^2;
if distance > (l1+l2)^2
    k = pd/norm(pd);
    p = (l1+l2)*k;
    px = p(1);
    py = p(2);
elseif distance < (l1-l2)^2
    k = pd/norm(pd);
    p = (l1-l2)*k;
    px = p(1);
    py = p(2);
end

cosq2 = (distance - l1^2 - l2^2)/2/l1/l2;
sinq2 = sqrt(1-cosq2^2);
sincosq2 = zeros(2,2);
sincosq2(:,1) = [sinq2;cosq2];
sincosq2(:,2) = [-sinq2;cosq2];
solutions = zeros(2,4);
for i=1:2
    sinq2 = sincosq2(1,i);
    cosq2 = sincosq2(2,i);
    A = l1 + l2*cosq2;
    B = l2*sinq2;
    sinq1 = (px*A - py*B)/(A^2+B^2);
    cosq1 = (px*B + py*A)/(A^2+B^2);
    q1 = atan2(sinq1,cosq1);
    q2 = atan2(sinq2,cosq2);
    solutions(:,i) = [q1;q2];
end

if solutions(1,1) > solutions(1,2)
    solutions(1,3) = solutions(1,1) - 2*pi;
    solutions(1,4) = solutions(1,2) + 2*pi;
else
    solutions(1,3) = solutions(1,1) + 2*pi;
    solutions(1,4) = solutions(1,2) - 2*pi;
end

if solutions(2,1) > solutions(2,2)
    solutions(2,3) = solutions(2,1) - 2*pi;
    solutions(2,4) = solutions(2,2) + 2*pi;
else
    solutions(2,3) = solutions(2,1) + 2*pi;
    solutions(2,4) = solutions(2,2) - 2*pi;
end

delta = 10000;
index = 0;
for i=1:4
    if norm(q - solutions(:,i))<delta
        delta = norm(q - solutions(:,i));
        index = i;
    end
end
sol = solutions(:,index);
end


function res = recenterTransform3d(transfo, center)
%RECENTERTRANSFORM3D Change the fixed point of an affine 3D transform.
%
%   TRANSFO2 = recenterTransform3d(TRANSFO, CENTER)
%   where TRANSFO is a 4x4 transformation matrix, and CENTER is a 1x3 row
%   vector, computes the new transformations that uses the same linear part
%   (defined by the upper-left 3x3 corner of the transformation matrix) as
%   the initial transform, and that will leave the point CENTER unchanged.
%
%
%
%   Example
%   % creating a re-centered rotation using:
%   rot1 = createRotationOx(pi/3);
%   rot2 = recenterTransform3d(rot1, [3 4 5]);
%   % will give the same result as:
%   rot3 = createRotationOx([3 4 5], pi/3);
%
%
%   See also
%   transforms3d, createRotationOx, createRotationOy, createRotationOz
%   createTranslation3d
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2010-07-27,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

% remove former translation part
res = eye(4);
res(1:3, 1:3) = transfo(1:3, 1:3);

% create translations
t1 = createTranslation3d(-center);
t2 = createTranslation3d(center);

% compute translated transform
res = t2*res*t1;


clc;
clear all;
close all;
A=imread('cameraman.tif');
figure,imshow(uint8(A))
title('Original Image');
A=double(A);
[s1 s2]=size(A);
% bs=input('Enter the block sizes for division of the image: '); % Block Size
bs=256;

% Haar
temp=double(zeros(size(A)));
for y=1:bs:s1-bs+1
    for x=1:bs:s2-bs+1
        croppedImage = A((y:y+bs-1),(x:x+bs-1));
        t=getHaarTransform(croppedImage,bs);
        temp((y:y+bs-1),(x:x+bs-1))=t;
    end
end
figure,imshow(uint8(temp))

% Inverse Haar
temp1=double(zeros(size(A)));
for y=1:bs:s1-bs+1
    for x=1:bs:s2-bs+1
        croppedImage = temp((y:y+bs-1),(x:x+bs-1));
        t=getInvHaarTransform(croppedImage,bs);
        temp1((y:y+bs-1),(x:x+bs-1))=t;
    end
end
figure,imshow(uint8(temp1))


%  Figure 10.71      Feedback Control of Dynamic Systems, 6e
%                        Franklin, Powell, Emami
%
% fig10_71.m is a script to generate Fig. 10.71 the linear RTP response
% LQG method with internal model
% RTP chamber demo Example
clf;
a=[-0.068208813989939 0.014929776357245 0.000000065782442;...
   0.045809672136430 -0.118134773528570 0.021802006306129;...
   0.000000433637498 0.046839194867347 -0.100884399149872];
b3=[0.37873508304055 0.110575403544586 0.022912893467038;...
   0.000000002974222 0.449046982554739 0.073572900808161;...
   0.000000027324116 0.000702342292121 0.417797228783230];
c3=eye(3,3);
d=0*eye(3,3);
sysp=ss(a,b3,c3,d);
[eol]=eig(a);
[zol]=tzero(sysp);
[zol22]=tzero(a,b3(:,2),c3(2,:),d(2,2));
% Combine 3 lamps into a single actuator
b=b3(:,1)+b3(:,2)+b3(:,3);
% Select center temperature
c=c3(2,:);
aa=[zeros(1,1) c;zeros(3,1) a];
bb=[zeros(1,1);b];
%
% weight temperature differences
q1hat=[eye(1,1) zeros(1,3);0 2 -1 -1; 0 -1 2 -1; 0 -1 -1 2]+1e-6*eye(4,4);
q1hat=diag([1 10 10 10])*q1hat;
q2hat=eye(1,1);
[kk,sric,ee]=lqr(aa,bb,q1hat,q2hat);
k1=kk(1,1);
ko=kk(:,2:4);
acl=[a-b*ko b;-k1*c zeros(1,1)];
bcl=[zeros(3,1);k1];
ccl=[c zeros(1,1)];
dcl=zeros(1,1);
[ecl]=eig(acl);
[zcl]=tzero(acl,bcl,ccl,dcl);
%CL DC gain
[cldcgain]=dcl-ccl*inv(acl)*bcl;
%CL Step Response
%[y,t]=step(acl,bcl,ccl,dcl);
%s1y=plot(y(:,:,1));
%grid;
%pause;
%Control effort
%cclu=[-ko eye(1,1)];
%dclu=[0];
%[uu,t]=step(acl,bcl,cclu,dclu);
%su=plot(uu(:,:,1));
%grid;
%pause
%Step in all channels
%t=0:.1:100;
%u=[25*ones(1,251) 25*ones(1,500)0*ones(1,250)];
%u10=[u'];
%sysc1=ss(acl,bcl,ccl,dcl);
%[yy1,t]=lsim(sysc1,u10,t);
%plot(t,yy1)
%grid
%pause;
%cclu=[-ko eye(1,1)];
%dclu=[0];
%syscl2=ss(acl,bcl,cclu,dclu);
%[uu1,t]=lsim(syscl2,u10,t);
%su=plot(uu1(:,:,1));
%grid;
%pause
% Estimator design
qe=eye(1,1);
re=0.001*eye(1,1);
[ll,pp,el]=lqe(a,b,c,qe,re);
ac=zeros(1,1);
bc=-k1;
cc=eye(1,1);
dc=-ko;
acle=[a, b*cc, -b*ko;
   bc*c, ac, zeros(1,3);
   ll*c, b*cc, a-ll*c-b*ko];
bcle=[zeros(3,1);-bc;zeros(3,1)];
ccle=[c, zeros(1,4)];
dcle=zeros(1,1);
[ecle]=eig(acle);
[zcle]=tzero(acle,bcle,ccle,dcle);
dcgain=dcle-ccle*inv(acle)*bcle;
t=0:.1:100;
R=[0:.1:25, 25*ones(1,500), 0*ones(1,250)];
R11=[R'];
R11=[R11 R11 R11 R11 R11 R11];
syscl=ss(acle,bcle,ccle,dcle);
[yy,t]=lsim(syscl,R,t);
figure(1);
plot(t,yy,'--','LineWidth',2);
grid on;
hold on;
plot(t,R11,'-');
xlabel('Time (sec)');
ylabel('Temperature (K)');
title('Fig. 10.71 (a) Internal model controller: temperature tracking response');
%grid
h_axes = findobj(get(gcf,'Children'),'Type','axes');
grey = [0.7,0.7,0.7];
set(h_axes,'xcolor',grey,'ycolor',grey, ...
    'GridLineStyle','-','MinorGridLineStyle','-', ...
    'Units','pixels');
grid on
c11=copyobj(h_axes,gcf);
set(c11,'color','none','xcolor','k', ...
    'xgrid','off','ycolor','k', ...
    'ygrid','off');
%legend('y')
%pause;
hold off;
cclu=[zeros(1,3), eye(1,1), -ko];
dclu=zeros(1,1);
syscu=ss(acle,bcle,cclu,dclu);
[uuu,t]=lsim(syscu,R,t);
figure(2);
plot(t(1:753,:),uuu(1:753,:),'LineWidth',2);
hold on;
plot(t(752:818,:),0*ones(67,3),'LineWidth',2);
hold on;
plot(t(753:818,:),uuu(753:818,:),'--','LineWidth',2);
hold on;
plot(t(819:1001,:),uuu(819:1001,:),'LineWidth',2);
xlabel('Time (sec)');
ylabel('Lamp voltage');
title('Fig. 10.71 (b) Internal model controller: control effort');
legend('u');
nicegrid;


%% Example of plotting a 2D Gaussian Mixture Model
clear all;
%% 1) Generate a random GMM

rot_mat = @(theta) [cos(theta), -sin(theta);sin(theta) cos(theta)];

Priors       = [0.3,0.3,0.3];
Priors       = Priors./sum(Priors);

Mu(:,1)      = [-2 0]';
Mu(:,2)      = [2 1]';
Mu(:,3)      = [0 -2]';
Sigma        = [];
Sigma(:,:,1) = rot_mat(pi/5)  * [3,0;0,0.1];
Sigma(:,:,2) = rot_mat(-pi/5) * [3,0;0,0.1];
Sigma(:,:,3) = rot_mat(pi/10) * [3,0;0,0.1];

%% Plot the Contours of the GMM

close all;
figure;
hold on; grid on;
plot_gmm_contour(gca,Priors,Mu,Sigma,[0 0 1]);
box on;


function src_dirs_rad = sphESPRIT(Us)
%SPHESPRIT DoA estimation using ESPRIT in the SHD
%
%   This routine spherical ESPRIT method to SH signals and returns the
%   analyzed DoAs without a grid search. It implements the 3-recurrence
%   relationship variant proposed by Jo & Choi in
%
%   B. Jo and J.-W. Choi, “Parametric direction-of-arrival estimation with
%   three recurrence relations of spherical harmonics,”
%   J. Acoust. Soc. Amer.,vol. 145, no. 1, pp. 480–488, Jan. 2019.
%
%   Subspace approaches such as ESPRIT can offer higher spatial resolution
%   than beamforming approaches such as the steered-response power, as long
%   as the source signals are not correlated between them and with the
%   reverberant/diffuse sound.
%
%   Inputs:
%       Us: (order+1)^2xK signal subspace taken from the first K eigenvectors
%       of the spatial correlation matrix, after sorting in eigenvalue
%       descending order
%
%   Outputs:
%       est_dirs:   nSrcx2 [azi elev] of estimated directions from
%           peak-finding
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% SPHESPRIT.M - 5/3/2019
% Archontis Politis, archontis.politis@tuni.fi
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[LambdaXYp, LambdaXYm, LambdaZ] = getLambda(Us);
[PsiXYp, PsiXYm, PsiZ] = getPsi(Us, LambdaXYp, LambdaXYm, LambdaZ);
[V, ~] = eig(PsiXYp, PsiZ, 'qz'); % A*V = B*V*D -> [V,D]=eig(A,B).
PhiXYp = V\(PsiXYp*V);
PhiXYm = V\(PsiXYm*V);
PhiZ   = V\(PsiZ*V);
phiX   = real(diag(PhiXYp+PhiXYm)/2);
phiY   = real(diag(PhiXYp-PhiXYm)/(2i));
phiZ   = real(diag(PhiZ));

azim = atan2(phiY,phiX);
elev = atan2(phiZ,sqrt(phiX.^2+phiY.^2));
src_dirs_rad = [azim elev];

end

function Ynimu = getYnimu(Ynm, ni, mu)

N = sqrt(size(Ynm,2))-1;
[idx_nimu, idx_nm] = muni2q(N,ni,mu);
Ynimu = zeros(size(Ynm,1),N^2);
Ynimu(:,idx_nimu) = Ynm(:,idx_nm);

end

function [idx_nimu, idx_nm] = muni2q(order,ni,mu)

nm = [];
for n=0:order-1
    nm = [nm; n*ones(2*n+1,1) (-n:n)'];
end
nimu = [nm(:,1)+ni nm(:,2)+mu];
qnm = nm(:,1).^2+nm(:,1)+nm(:,2)+1;
qnimu = nimu(:,1).^2+nimu(:,1)+nimu(:,2)+1;
idx_valid = find(abs(nimu(:,2))<=nimu(:,1));
idx_nm = qnimu(idx_valid);
idx_nimu = qnm(idx_valid);

end

function Wnimu = getWnimu(order, mm, ni, mu)

nm = [];
for n=0:order-1
    nm = [nm; n*ones(2*n+1,1) (-n:n)'];
end
if mm==1
    nimu = [nm(:,1)+ni nm(:,2)+mu];
elseif mm==-1
    nimu = [nm(:,1)+ni -nm(:,2)+mu];
end
w_nimu = sqrt( (nimu(:,1)-nimu(:,2)-1).*(nimu(:,1)-nimu(:,2))./((2*nimu(:,1)-1).*(2*nimu(:,1)+1)) );
Wnimu = diag(w_nimu);

end

function Vnimu = getVnimu(order, ni, mu)

nm = [];
for n=0:order-1
    nm = [nm; n*ones(2*n+1,1) (-n:n)'];
end
nimu = [nm(:,1)+ni nm(:,2)+mu];
v_nimu = sqrt( (nimu(:,1)-nimu(:,2)).*(nimu(:,1)+nimu(:,2)) ./((2*nimu(:,1)-1).*(2*nimu(:,1)+1)) );
Vnimu = diag(v_nimu);

end

function [PsiXYp, PsiXYm, PsiZ] = getPsi(Us, LambdaXYp, LambdaXYm, LambdaZ)

pinvUs = pinv(getYnimu(Us.',0,0).');
PsiXYp = pinvUs*LambdaXYp;
PsiXYm = pinvUs*LambdaXYm;
PsiZ   = pinvUs*LambdaZ;

end

function [PhiXYp, PhiXYm, PhiZ] = getPhi(src_dirs_rad)

PhiZ = diag(cos(src_dirs_rad(:,2)));
PhiXYp = diag( sin(src_dirs_rad(:,2)).*exp(1i*src_dirs_rad(:,1)) );
PhiXYm = diag( sin(src_dirs_rad(:,2)).*exp(-1i*src_dirs_rad(:,1)) );

end

function [LambdaXYp, LambdaXYm, LambdaZ] = getLambda(Us)

order = sqrt(size(Us,1))-1;
LambdaXYp =  getWnimu(order, 1,1,-1)*getYnimu(Us.', 1,-1).' - getWnimu(order,-1,0,0)*getYnimu(Us.',-1,-1).';
LambdaXYm = -getWnimu(order,-1,1,-1)*getYnimu(Us.', 1, 1).' + getWnimu(order, 1,0,0)*getYnimu(Us.',-1, 1).';
LambdaZ   =  getVnimu(order,   0, 0)*getYnimu(Us.',-1, 0).' + getVnimu(order,   1,0)*getYnimu(Us.', 1, 0).';

end


function A = metric_03 ( p )

%*****************************************************************************80
%
%% METRIC_03 evaluates metric #3 at any point.
%
%  Discussion:
%
%    This routine evaluates the matrix that determines the metric
%    at a point.
%
%    This particular matrix exaggerates distances in the Y direction.
%
%    It is diagonal, and it is spatially constant.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 May 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real P(2), the point at which the metric matrix is to
%    be evaluated.
%
%    Output, real A[2,2], the metric matrix.
%
  A = [ 1.0, 0.0; 0.0, 100.0 ];

  return
end


function [ a_lu, info ] = r83_np_fa ( n, a )

%*****************************************************************************80
%
%% R83_NP_FA factors a R83 matrix without pivoting.
%
%  Discussion:
%
%    The R83 storage format is used for a tridiagonal matrix.
%    The superdiagonal is stored in entries (1,2:N), the diagonal in
%    entries (2,1:N), and the subdiagonal in (3,1:N-1).  Thus, the
%    original matrix is "collapsed" vertically into the array.
%
%    Because this routine does not use pivoting, it can fail even when
%    the matrix is not singular, and it is liable to make larger
%    errors.
%
%    R83_NP_FA and R83_NP_SL may be preferable to the corresponding
%    LINPACK routine SGTSL for tridiagonal systems, which factors and solves
%    in one step, and does not save the factorization.
%
%  Example:
%
%    Here is how a R83 matrix of order 5 would be stored:
%
%       *  A12 A23 A34 A45
%      A11 A22 A33 A44 A55
%      A21 A32 A43 A54  *
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 November 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be at least 2.
%
%    Input, real A(3,N), the tridiagonal matrix.
%
%    Output, integer INFO, singularity flag.
%    0, no singularity detected.
%    nonzero, the factorization failed on the INFO-th step.
%
%    Output, real A_LU(3,N), factorization information.
%
  info = 0;

  a_lu(1:3,1:n) = a(1:3,1:n);

  for i = 1 : n-1

    if ( a_lu(2,i) == 0.0E+00 )
      info = i;
      fprintf ( 1, '\n' );
      fprintf ( 1, 'R83_NP_FA - Fatal error!\n' );
      fprintf ( 1, '  Zero pivot on step %d\n', info );
      return;
    end
%
%  Store the multiplier in L.
%
    a_lu(3,i) = a_lu(3,i) / a_lu(2,i);
%
%  Modify the diagonal entry in the next column.
%
    a_lu(2,i+1) = a_lu(2,i+1) - a_lu(3,i) * a_lu(1,i+1);

  end

  if ( a_lu(2,n) == 0.0E+00 )
    info = n;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R83_NP_FA - Fatal error!\n' );
    fprintf ( 1, '  Zero pivot on step %d\n', info );
    return;
  end

  return
end


function [x,state] = struct_triu(z,task)
%STRUCT_TRIU Upper triangular matrix.
%   [x,state] = struct_triu(z) generates x as a lower triangular matrix by
%   using the vector z to fill the matrix column by column. For a matrix x
%   of order n, the vector z should have length n*(n+1)/2. The structure
%   state stores information which is reused in computing the right and
%   left Jacobian-vector products.
%
%   struct_triu(z,task) computes the right or left Jacobian-vector
%   product of this transformation, depending on the structure task. Use
%   the structure state and add the field 'r' of the same shape as z or the
%   field 'l' of the same shape as x to obtain the structure task for
%   computing the right and left Jacobian-vector products
%
%      (dF(:)/dz(:).')*task.r(:) and
%      (dF(:)/dz(:).')'*task.l(:) + conj((dF(:)/dconj(z(:)).')'*task.l(:)),
%
%   respectively. Here, F(z) represents this transormation, (:) signifies
%   vectorization and the derivative w.r.t. z (conj(z)) is a partial
%   derivative which treats conj(z) (z) as constant. The output has the
%   same shape as x or z for the right and left Jacobian-vector products,
%   respectively.
%
%   See also struct_band, struct_diag, struct_tridiag, struct_tril.

%   Authors: Laurent Sorber (Laurent.Sorber@cs.kuleuven.be)
%            Marc Van Barel (Marc.VanBarel@cs.kuleuven.be)
%            Lieven De Lathauwer (Lieven.DeLathauwer@kuleuven-kulak.be)
%
%   References:
%   [1] L. Sorber, M. Van Barel, L. De Lathauwer, "Structured data fusion,"
%       ESAT-SISTA Internal Report 13-177, KU Leuven, 2013.

if nargin < 2, task = []; end
n = 0.5*(-1+sqrt(1+8*length(z)));
[x,state] = struct_band(z,task,[n n],[0 n-1]);

end


%  Computes the hessian numerically
%
%  ::
%
%    [H,issue]=numerical(fh,xbest,hessian_type)
%
%  Args:
%
%     - **fh** [char|function handle]: one-dimensional objective function
%     - **xbest** [vector]: point at which the hessian has to be computed
%     - **hessian_type** [{'fd'}|'opg']: type of hessian computed : finite
%       differences or outer-product-gradient
%
%  Returns:
%     :
%
%     - **H** [d x d matrix]: hessian
%     - **issue** [''|char]: description of any problem encountered during the
%       calculation of the hessian.
%
%


function engine = bk_ff_hmm_inf_engine(bnet)
% BK_FF_HMM_INF_ENGINE Naive (HMM-based) implementation of fully factored form of Boyen-Koller
% engine = bk_ff_hmm_inf_engine(bnet)
%
% This is implemented on top of the forwards-backwards algo for HMMs,
% so it is *less* efficient than exact inference! However, it is good for educational purposes,
% because it illustrates the BK algorithm very clearly.

[persistent_nodes, transient_nodes] = partition_dbn_nodes(bnet.intra, bnet.inter);
assert(isequal(sort(bnet.observed), transient_nodes));
[engine.prior, engine.transmat] = dbn_to_hmm(bnet);

ss = length(bnet.intra);

engine.bel = [];
engine.bel_marginals = [];
engine.marginals = [];

engine = class(engine, 'bk_ff_hmm_inf_engine', inf_engine(bnet));


function [r1,r2] = quad_roots(a1, a2, a3)

t1 = -a2/2./a1;
t2 = sqrt(a2.^2 - 4*a1.*a3)/2./a1;
r1 = t1 + t2;
r2 = t1 - t2;


function [ds, S, p] = multivar_dist(X, varargin)
% multivariate normality checking and diagnostic plots
%
% :Usage:
% ::
%
%     [ds, S, p] = multivar_dist(X)
%
% :Input:
%
%     given matrix X with cases = rows, cols = variables
%
% :Optional input:
%
% 'noplot' : suppress plot
%
% :Outputs:
%
%   **ds:**
%        is matrix of squared distances, case numbers, and
%        expected chi2 values (in columns in this order)
%        rows are cases
%
%        NOTE: Sorted in order of ascending distance!
%
%   **S:**
%        estimated covariance matrix
%
%   **mv_distance:**
%        squared distances in original order of rows
%
%   **p:**
%        p-values in original order of rows
%
% ..
%    by Tor Wager
% ..

    % ..
    %    determine multivariate standard deviation matrix S
    % ..

    doplot = true;
    doverbose = true;

    if any(strcmp(varargin, 'noplot')), doplot = false; end
    if any(strcmp(varargin, 'noverbose')), doverbose = false; end

    % center
    Xs = X - repmat(mean(X), size(X, 1), 1);

    % covariance matrix S
    S = (Xs' * Xs) ./ (size(Xs, 1)-1);

    % -----------------------------------------------------
    % * get squared distance
    % -----------------------------------------------------

    % squared distance, Johnson & Wichern p. 201
    % (X-mean(X))' * inv(S) * (X - mean(X)) generalized to matrices
    d = Xs * inv(S) * Xs';
    d = diag(d);            % what do the off-diagonals signify?

    % -----------------------------------------------------
    % * compare with chi2 distribution
    % -----------------------------------------------------

    % calculate chi2 threshold
    % chi2 value compares the number of points within the ellipsoid
    % contour to those outside; roughly alpha of the squared distances
    % should be within the ellipsoid (for rough general test of normality).
    % outliers will have very high chi2 values

    t = chi2inv(.5, size(X, 2));  % for general test
    p50 = 100 * (sum(d > t) ./ length(d));

    if doverbose
        fprintf(1, 'Expected 50%% of points within 50%% normal ellipsoid, found %3.2f%%\n', p50);
    end

    t = chi2inv(.95, size(X, 2));  % for general test
    p95 = sum(d > t);

    if doverbose
        fprintf(1, 'Expected %3.2f outside 95%% ellipsoid, found %3.0f\n', .05*length(d), p95);
    end

    % -----------------------------------------------------
    % * get case numbers and sort by distance
    % -----------------------------------------------------
    d(:,2) = (1:length(d))';
    ds = sortrows(d, 1);

    % -----------------------------------------------------
    % * get chi2 quantiles for qd plot
    % -----------------------------------------------------
    q = (((1:size(ds, 1)) - .5) ./ size(ds, 1))';
    q = chi2inv(q, size(X, 2));
    ds(:,3) = q;

    % -----------------------------------------------------
    % * get distance^2 in original order and p-values
    % -----------------------------------------------------
    ds = sortrows(ds, 2);
    p = 1 - chi2cdf(ds(:,1), size(X, 2));

    % -----------------------------------------------------
    % * plot the results in a figure
    % -----------------------------------------------------
    if doplot

        figure('color', 'w');
        subplot(1, 3, 1); hold on; grid on
        plot([1:size(d, 1); 1:size(d, 1)], [zeros(size(d, 1), 1) d(:,1)]', 'b', 'LineWidth', 1.5);
        xlabel('Case number');
        ylabel('Squared stat. distance from origin');
        wh = (d(:,1) > t); d2 = d; d2(:,1) = d2(:,1) .* wh;  % zero out the non-"significant" chi2 cases
        plot([1:size(d2, 1); 1:size(d2, 1)], [zeros(size(d2, 1), 1) d2(:,1)]', 'r', 'LineWidth', 1.5);
        title('d^2, red cases outside 95% normal ellipsoid');
        plot([0 size(d, 1)], [t t], 'k');
        set(gca, 'YLim', [0 max(t+.5, max(d(:,1)))]);

        subplot(1, 3, 2); hold on; grid on
        plot(ds(:,3), ds(:,1), 'MarkerSize', 0.01, 'Color', 'w');
        for i = 1:size(ds, 1)
            text(ds(i,3), ds(i, 1), num2str(ds(i, 2)), 'Color', 'k');
        end
        xlabel('Expected chi2 value'), ylabel('Squared distance');
        plot([0 max([ds(:,3);ds(:,1)])], [0 max([ds(:,3);ds(:,1)])], 'k', 'LineWidth', 1.5);
        title('Line with slope = 1 is normal');

        subplot(1, 3, 3);
        imagesc(cov(X'));
        colorbar

    end

end


function M = getmassmat(node,elem2dof,area,type,K)
%% GETMASSMAT Get mass matrix of the finite element space
%
% M = GETMASSMAT(elem2edge,area,Dlambda,elemType,K) get mass matrix of the finite element
% space specified by elemType.
%
% The type can be:
% - lump: lumped mass matrix for P1 element
% - HB: Hierarchical basis for P2 element
% - NB: Nodal basis for P2 element
% - NBB: Nodal basis with bubble functions for P2 element
%
% All cases for P2 element are added by Lin Zhong.

N = size(node,1);
NT = length(area);
Ndof = double(max(elem2dof(:)));

%% Coefficients
if ~exist('type','var'), type = 'P1'; end
if ~exist('K','var'), K = []; end

%% Assembling
n = size(elem2dof,2);
switch n
    case 3 %% P1 element
    %% Assemble the mass matrix by the mass lumping
    if strcmp(type,'lump')
        M = accumarray([elem2dof(:,1);elem2dof(:,2);elem2dof(:,3)],...
                       [area;area;area]/3,[Ndof,1]);
        if exist('K','var') && ~isempty(K) && ~isnumeric(K) % K is a function
            M = K(node).*M;
        elseif exist('K','var') && ~isempty(K) && isnumeric(K) && size(K,1) == N
            M = K.*M;
        end
        M = spdiags(M,0,N,N);
    else %% Assemble the full mass matrix
        if exist('K','var') && ~isempty(K) && ~isnumeric(K) % K is a function
            center = (node(elem2dof(:,1),:) + node(elem2dof(:,2),:) + ...
                      node(elem2dof(:,3),:))/3;
            area = K(center).*area;
        elseif exist('K','var') && ~isempty(K) && isnumeric(K) && size(K,1) == NT
            area = K.*area;
        end
        M = sparse(N,N);
        for i = 1:3
            for j = 1:3
                   Mij = area*((i==j)+1)/12;
                   M = M + sparse(elem2dof(:,i),elem2dof(:,j),Mij,Ndof,Ndof);
            end
        end
    end
    case 6  %% P2 element
    %% Mass matrices for NBB bases
    if strcmp(type,'NBB')
        elem2dof(:,7) = uint32(Ndof+(1:NT)'); % add bubble index
        Ndof = double(max(elem2dof(:)));
        elemM(1:NT,7)   = 9/20;
        elemM(1:NT,1:3) = 1/20;
        elemM(1:NT,4:6) = 2/15;
        elemM = elemM.*repmat(area,1,7);
        diagM = accumarray(elem2dof(:), elemM(:), [Ndof 1]);
        M = spdiags(double(diagM),0,Ndof,Ndof);
        return;
    end
    %% Mass matrices for HB or NB bases
    [lambda, w] = quadpts(4);
    nQuad = size(lambda,1);
    if strcmp(type,'HB')
        phi(:,1) = lambda(:,1);
        phi(:,2) = lambda(:,2);
        phi(:,3) = lambda(:,3);
    elseif strcmp(type,'NB')
        phi(:,1) = lambda(:,1).*(2*lambda(:,1)-1);
        phi(:,2) = lambda(:,2).*(2*lambda(:,2)-1);
        phi(:,3) = lambda(:,3).*(2*lambda(:,3)-1);
    end
    phi(:,4) = 4*lambda(:,2).*lambda(:,3);
    phi(:,5) = 4*lambda(:,3).*lambda(:,1);
    phi(:,6) = 4*lambda(:,1).*lambda(:,2);

    Nbases = 6; NMhalf = 21;
    ii = zeros(NMhalf*NT,1);
    jj = zeros(NMhalf*NT,1);
    sM = zeros(NMhalf*NT,1);
    index = 0;
    for i = 1:Nbases
        for j = i:Nbases
            Mij = 0;
            for p = 1:nQuad
                Mij = Mij + w(p)*phi(p,i).*phi(p,j);
            end
            Mij = Mij.*area;
            ii(index+1:index+NT) = double(elem2dof(:,i));
            jj(index+1:index+NT) = double(elem2dof(:,j));
            sM(index+1:index+NT) = Mij;
            index = index + NT;
        end
    end
    clear Mij
    diagIdx = (ii == jj);   upperIdx = ~diagIdx;
    M = sparse(ii(diagIdx),jj(diagIdx),sM(diagIdx),Ndof,Ndof);
    MU = sparse(ii(upperIdx),jj(upperIdx),sM(upperIdx),Ndof,Ndof);
    M = M + MU + MU';
end


%%%
%> @brief  computes the pose vector v from an homogeneous transformation A
%> param   A homogeneous transformation
%> return  v pose vector
%> @author Giorgio Grisetti
%%%
function v = t2v(A)
% T2V homogeneous transformation to vector
v(1:2,1) = A(1:2,3);
v(3,1) = atan2(A(2,1), A(1,1));
end


% Nonlinear model of two CSTRs in series from
%
% Henson, M.A. and Seborg, D.E., Feedback Linearizing Control, Chap. 4 of Nonlinear Process
%   Control, Edited by Hensen, M.A. and Seborg, D.E., Prentice Hall (1997)
%
%  t -- time (not used)
%  y -- state value vector
%  xdot -- set to the vector of state derivatives

function xdot = cstr5(t,y)

global u

% Input (1):
% Coolant Flowrate (L/min)
qc = u;

% States (4):
% Concentration of A in Reactor #1 (mol/L)
Ca1 = y(1);
% Temperature of Reactor #1 (K)
T1 = y(2);
% Concentration of A in Reactor #2 (mol/L)
Ca2 = y(3);
% Temperature of Reactor #2 (K)
T2 = y(4);

% Parameters
% Flowrate (L/min)
q = 100;
% Feed Concentration of A (mol/L)
Caf = 1.0;
% Feed Temperature (K)
Tf	= 350.0 ;
% Coolant Temperature (K)
Tcf = 350.0;
% Volume of Reactor #1 (L)
V1 = 100;
% Volume of Reactor #2 (L)
V2 = 100;
% UA1 or UA2 - Overall Heat Transfer Coefficient (J/min-K)
UA1 = 1.67e5;
UA2 = 1.67e5;
% Pre-exponential Factor for A->B Arrhenius Equation
k0 = 7.2e10;
% EoverR - E/R (K) - Activation Energy (J/mol) / Gas Constant (J/mol-K)
EoverR = 1e4;
% Heat of Reaction - Actually (-dH) for an exothermic reaction (J/mol)
dH = 4.78e4;
% Density of Fluid (g/L)
rho = 1000;
% Density of Coolant Fluid (g/L)
rhoc = 1000;
% Heat Capacity of Fluid (J/g-K)
Cp = 0.239;
% Heat Capacity of Coolant Fluid (J/g-K)
Cpc = 0.239;

%	Dynamic Balances

%	dCa1/dt
xdot(1,1) = q*(Caf-Ca1)/V1 - k0*Ca1*exp(-EoverR/T1);
%	dT1/dt
xdot(2,1) = q*(Tf-T1)/V1 + (dH*k0/rho/Cp)*Ca1*exp(-EoverR/T1) + ...
   rhoc*Cpc/rho/Cp/V1 * qc * (1-exp(-UA1/qc/rhoc/Cpc)) * (Tcf-T1);
%	dCa2/dt
xdot(3,1) = q*(Ca1-Ca2)/V2 - k0*Ca2*exp(-EoverR/T2);
%	dT2/dt
xdot(4,1)  = q*(T1-T2)/V2 + (dH*k0/rho/Cp)*Ca2*exp(-EoverR/T2) + ...
   rhoc*Cpc/rho/Cp/V2 * qc * (1-exp(-UA2/qc/rhoc/Cpc)) * ...
   (T1 - T2 + exp(-UA1/qc/rhoc/Cpc)*(Tcf-T1));


function [rc,rcb,Pf,Pb] = pc2rcv(pc,R0)

%function [rc,rcb] = pc2rcv(pc,R0)
% Transforms normalized correlation matrices pc into
% forward and backward reflection coefficients rc and rcb.

% S. de Waele, March 2003.

if ~isstatv(pc), error('Partial correlations non-stationairy!'), end

s = kingsize(pc);
order = s(3)-1;
dim = s(1); I = eye(dim);
%if nargin == 1, R0 = I; end

rc = zeros(s);  rc(:,:,1)  = I;
Pf   = zeros(s); Pf(:,:,1)   = R0;

rcb = zeros(s);  rcb(:,:,1) = I;
Pb   = zeros(s); Pb(:,:,1)  = R0;

for p = 1:order,
   TsqrtPf = Tsqrt( Pf(:,:,p)); %square root M defined by: M=Tsqrt(M)*Tsqrt(M)'
   TsqrtPb= Tsqrt(Pb(:,:,p));
   %reflection coefficients
   rc(:,:,p+1) = -TsqrtPf *pc(:,:,p+1) *inv(TsqrtPb);
   rcb(:,:,p+1)= -TsqrtPb*pc(:,:,p+1)'*inv(TsqrtPf );
   %residual matrices
   Pf(:,:,p+1)  = (I-TsqrtPf *pc(:,:,p+1) *pc(:,:,p+1)'*inv(TsqrtPf ))*Pf(:,:,p);
   Pb(:,:,p+1) = (I-TsqrtPb*pc(:,:,p+1)'*pc(:,:,p+1) *inv(TsqrtPb))*Pb(:,:,p);
end %for p = 2:order,


function complextest_AD1()
% Test AD for a complex optimization problem on a product manifold (struct)

    % Verify that Manopt was indeed added to the Matlab path.
    if isempty(which('spherecomplexfactory'))
        error(['You should first add Manopt to the Matlab path.\n' ...
		       'Please run importmanopt.']);
    end

    % Verify that the deep learning tool box was installed
    assert(exist('dlarray', 'file') == 2, ['Deep learning tool box is '...
    'needed for automatic differentiation.\n Please install the'...
    'latest version of the deep learning tool box and \nupgrade to Matlab'...
    ' R2021b if possible.'])

    % Generate the problem data.
    n = 100;
    A = randn(n) + 1i*randn(n);
    A = .5*(A+A');

    % Create the product manifold
    S = spherecomplexfactory(n);
    manifold.x = S;
    manifold.y = S;
    problem.M = productmanifold(manifold);  %struct

    % For Matlab R2021b or later, define the problem cost function as usual
    % problem.cost  = @(X) -real(X.x'*A*X.y);

    % For Matlab R2021a or earlier, translate the cost function into a
    % particular format with the basic functions in /functions_AD
    problem.cost  = @(X) -creal(cprod(cprod(ctransp(X.x), A), X.y));

    % Define the gradient and the hessian via automatic differentiation
    problem = manoptAD(problem);

    % Numerically check gradient and Hessian consistency.
    figure;
    checkgradient(problem);
    figure;
    checkhessian(problem);

    % Solve.
    [x, xcost, info] = trustregions(problem);          %#ok<ASGLU>

    % Test
    ground_truth = svd(A);
    distance = abs(ground_truth(1) - (-problem.cost(x)));
    fprintf('The distance between the ground truth and the solution is %e \n',distance);

end


function A = create_2d_image_downsample_matrix(szin, k)
    szout = ceil(szin / k);

    npixin = prod(szin);
    npixout = prod(szout);

    A = sparse(npixout, npixin);
    [m1, m2] = ndgrid(1:szin(1), 1:szin(2));

    mm1 = ceil(m1 / k);
    mm2 = ceil(m2 / k);

    m1 = m1(:); m2 = m2(:);
    mm1 = mm1(:); mm2 = mm2(:);

    rr = (mm2-1)*szout(1) + mm1;
    cc = (m2-1)*szin(1) + m1;
    A = sparse(rr, cc, 1/k^2, npixout, npixin);

%     return;

end


function [fSignificanceLevel] = pf_OverallSignificance(nNumberNodesTotal, nNumberNodesHit, fHitProbability)

fIncSumProb = [];
fSumProb = 0;
for nCnt_= 0:nNumberNodesTotal
  nNumberCombinations = nchoosek(nNumberNodesTotal, nCnt_);
  fProbCombination = ((1 - fHitProbability)^(nNumberNodesTotal - nCnt_) * fHitProbability^nCnt_) * nNumberCombinations;
  fSumProb = fSumProb + fProbCombination;
  fIncSumProb = [fIncSumProb; fSumProb];
end
fSignificanceLevel = 1 - (fIncSumProb(nNumberNodesHit + 1));


function pass = test_harmonic( )
% simple tests for cylindrical harmonic command.

tol = 3e2*chebfunpref().cheb2Prefs.chebfun2eps;

%example: build via jbessel w/known bessel root
 %L = 5, m = 3;
 F = diskfun(@(t,r) sqrt(2)/ (sqrt(pi)*abs(besselj(5+1, 15.7001740797116)))...
     *besselj(5, 15.7001740797116*r).*cos(5*t), 'polar');
 G = diskfun.harmonic(5,3);
 pass(1) = norm( F-G ) < tol;

 F = diskfun(@(t,r) sqrt(2)/ (sqrt(pi)*abs(besselj(5+1, 15.7001740797116)))...
     *besselj(5, 15.7001740797116*r).*sin(5*t), 'polar');
 G = diskfun.harmonic(-5,3);
 pass(2) = norm( F-G ) < tol;

%L = 0, m = 5;
 F = diskfun(@(t,r) sqrt(2)/ (sqrt(2*pi)*abs(besselj(0+1, 14.9309177084877)))...
     *besselj(0, 14.9309177084877*r), 'polar');
 G = diskfun.harmonic(0,5);
 pass(3) = norm(F-G) < tol;

%test orthonormality

A=diskfun.harmonic(29,10);
B=diskfun.harmonic(6,23);
C = diskfun.harmonic(-4, 3);
pass(4) = abs(sum2(A.*A)-1) < tol;
pass(5) = abs(sum2(B.*B)-1) < tol;
pass(6) = abs(sum2(A.*B)) < tol;
pass(7) = abs(sum2(A.*C)) < tol;
pass(8) = abs(sum2(C.*C)-1) < tol;

%neumann conditions, test orthonormal
A = diskfun.harmonic(10,8, 'neumann');
B = diskfun.harmonic(-4, 3, 'neumann');
pass(8) = abs(sum2(A.*A)-1) < tol;
pass(9) = abs(sum2(B.*B)-1) < tol;
pass(10) = abs(sum2(A.*B)) < tol;

end


function S=triplyPeriodicMinimal(varargin)

% function S=triplyPeriodicMinimal(X,Y,Z,typeStr)
% ------------------------------------------------------------------------
% This function creates the image S which can be used to define triply
% periodic minimal surfaces. The input consists of a grid of coordinates
% (X,Y,Z) and the surface type (typeStr).
%
% 2009 Created
% 2016 Updated input handling
% 2018/12/13 Added comments at top of function
% ------------------------------------------------------------------------

%%
%Parse input
switch nargin
    case 2
        P=varargin{1};
        X=P(:,1); Y=P(:,2); Z=P(:,3); %Get coordinates
        typeStr=varargin{2}; %Get the surface type string
    case 4
        X=varargin{1};
        Y=varargin{2};
        Z=varargin{3};
        typeStr=varargin{4};
    otherwise
        error('False number of input arguments.');
end

%Evaluate metric on coordinates
switch typeStr
    case 'p' %Schwarz P
        S=(cos(X)+cos(Y)+cos(Z));
    case 'd' %Schwarz D
        S=(sin(X).*sin(Y).*sin(Z))...
            +(sin(X).*cos(Y).*cos(Z))...
            +(cos(X).*sin(Y).*cos(Z))...
            +(cos(X).*cos(Y).*sin(Z));
    case 'g' %Schoen Gyroid
        S=(sin(X).*cos(Y))+(sin(Y).*cos(Z))+(cos(X).*sin(Z));
    case 'n' %Neovius
        S=3*(cos(X)+ cos(Y)+ cos(Z))+ (4*cos(X).*cos(Y).*cos(Z));
    case 'w'
        S=2*(cos(X).*cos(Y)+cos(Z).*cos(X)+cos(Y).*cos(Z))-(cos(2*X)+cos(2*Y)+cos(2*Z));
    case 'pw'
        S=(4.*(cos(X).*cos(Y)+cos(Y).*cos(Z)...
            +cos(Z).*cos(X))-3.*cos(X).*cos(Y).*cos(Z))+2.4;
    otherwise
        error('unknown surface type requested')
end

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function [dateV] = doy2date(doyV,yearV)
% DOY2DATE.m will convert a vector of day of year numbers and years
% and convert them to MATLAB date format.
%
% Sample Call:
%  doyV = [54;200.4315];
%  yearV = [2009;2009];
%  [dateV] = doy2date(doyV,yearV);
%
% Inputs:
%  doyV -> vector of day of year numbers (n x 1)
%  yearV -> vector of years (n x 1)
%
% Outputs:
%  dateV -> vector of MATLAB dates (n x 1)
%
% AUTHOR    : A. Booth (ashley [at] boothfamily [dot] com)
% DATE      : 22-May-2009 09:34:53
% Revision  : 1.00
% DEVELOPED : 7.4.0.287 (R2007a) Windows XP
% FILENAME  : doy2date.m

%Check size of incoming vectors
if (size(doyV,1)== 1) | (size(doyV,2) == 1) %Make sure only a nx1 vector
    if size(doyV,1)<size(doyV,2)
        doyV = doyV';
        colflip = 1; %take note if the rows were flipped to col
    else
        colflip = 0;
    end
else %check to see that vectors are columns:
    error('DOY vector can not be a matrix')
end

%year vector
if (size(yearV,1)== 1) | (size(yearV,2) == 1) %Make sure only a nx1 vector
    if size(yearV,1)<size(yearV,2)
        yearV = yearV';
%         colflip = 1; %take note if the rows were flipped to col
    else
%         colflip = 0;
    end
else %check to see that vectors are columns:
    error('Year vector can not be a matrix')
end

%Check to make sure sizes of the vectors are the same
if ~min(size(doyV) == size(yearV))
    error('Day of year vector and year vector must be the same size')
end

%Make year into date vector
z = zeros(length(yearV),5);
dv = horzcat(yearV,z);

%Calc matlab date
dateV = doyV + datenum(dv);

% flip output if input was flipped
if colflip
    dateV = dateV';
end

% disp('Completed doy2date.m')
% ===== EOF [doy2date.m] ======


function index = findPoint(coord, points, varargin)
%FINDPOINT Find index of a point in an set from its coordinates.
%
%   IND = findPoint(POINT, ARRAY)
%   Returns the index of point whose coordinates match the 1-by-2 row array
%   POINT in the N-by-2 array ARRAY. If the point is not found, returns 0.
%   If several points are found, keep only the first one.
%
%   If POINT is a M-by-2 array, the result is a M-by-1 array, containing
%   the index in the array of each point given by COORD, or 0 if the point
%   is not found.
%
%   IND = findPoint(POINT, ARRAY, TOL)
%   use specified tolerance, to find point within a distance of TOL.
%   Default tolerance is zero.
%
%   See also
%    points2d, minDistancePoints, distancePoints, findClosestPoint

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2003-07-17
% Copyright 2003-2022 INRA - TPV URPOI - BIA IMASTE

% number of points
np = size(coord, 1);

% allocate memory for result
index = zeros(np, 1);

% specify the tolerance
tol = 0;
if ~isempty(varargin)
    tol = varargin{1};
end

if tol == 0
    for i = 1:np
        % indices of matches
        ind = find(points(:,1) == coord(i,1) & points(:,2) == coord(i,2));

        % format current result
        if isempty(ind)
            index(i) = 0;
        else
            index(i) = ind(1);
        end
    end
else
    for i = 1:np
        % indices of matches
        ind = find(sqrt(sum(bsxfun(@minus, points, coord) .^ 2, 2)) <= tol);

        % format current result
        if isempty(ind)
            index(i) = 0;
        else
            index(i) = ind(1);
        end
    end
end


function groupAssignment = prtUtilEquallySubDivideData(Y,nDivisions)
% prtUtilEquallySubDivideData  Equally sub-divide into several groups.
%   By equally sub-divide I mean make it so that each group has the same
%   number of data points of each class.
%
% Syntax: groupAssignment = prtUtilEquallySubDivideData(Y,nDivisions)
%
% Inputs:
%   Y - The class label vector for the dataset
%   nDivision - The number of division to make
%
% Outputs:
%   groupAssignment = Integer assignments specifying the group for each
%       datapoint. These are randomly drawn
%

if nDivisions > length(Y)
   warning('The number of requested divisions is larger than the amount of data. The number of divisions was changed to the length of the data.')
   nDivisions = length(Y);
end

% Leave One Out quick method...
if nDivisions == length(Y)
   groupAssignment = randperm(nDivisions)';
   return
end

% Y may contain NaNs. These are treated at missing data. All NaNs are
% distributed evenly across the folds. To do this we make a virtual class
% for nan and then use the same code below.
nanVals = isnan(Y);
if any(nanVals)
    Y(nanVals) = max(Y(~nanVals))+1;
end

sortedY = sort(Y);
nSamples = length(Y);
sortedGroupAssignment = repmat((1:nDivisions)',ceil(nSamples/nDivisions),1);
sortedGroupAssignment = sortedGroupAssignment(1:nSamples);

% Randomize within each class and revert back to the original order
groupAssignment = zeros(size(Y));
uY = unique(Y);
nClasses = length(uY);
for iClass = 1:nClasses
   cSortedGroupAssignment = sortedGroupAssignment(sortedY == uY(iClass));
   groupAssignment(Y==uY(iClass)) = cSortedGroupAssignment(randperm(sum(sortedY == uY(iClass))));
end


function bessel_i1_values_test ( )

%*****************************************************************************80
%
%% BESSEL_I1_VALUES_TEST demonstrates the use of BESSEL_I1_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 February 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BESSEL_I1_VALUES_TEST:\n' );
  fprintf ( 1, '  BESSEL_I1_VALUES stores values of \n' );
  fprintf ( 1, '  the Bessel I1 function.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      X            I1(X)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, fx ] = bessel_i1_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %12f  %24.16f\n', x, fx );

  end

  return
end


function a = conex2_inverse ( alpha )

%*****************************************************************************80
%
%% CONEX2_INVERSE returns the inverse of the CONEX2 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real ALPHA, the scalar defining A.
%    A common value is 100.0.  ALPHA must not be zero.
%
%    Output, real A(3,3), the matrix.
%
  a = zeros ( 3, 3 );

  if ( alpha == 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'CONEX2_INVERSE - Fatal error!\n' );
    fprintf ( 1, '  The input value of ALPHA was zero.\n' );
    error ( 'CONEX2_INVERSE - Fatal error!' );
  end

  a(1,1) = 1.0;
  a(1,2) = ( 1.0 - alpha * alpha ) / alpha;
  a(1,3) = ( 1.0 + alpha * alpha ) / alpha^2;

  a(2,1) = 0.0;
  a(2,2) = alpha;
  a(2,3) = 1.0;

  a(3,1) = 0.0;
  a(3,2) = 0.0;
  a(3,3) = 1.0;

  return
end


function circle2 = transformCircle3d(circle, tfm)
%TRANSFORMCIRCLE3D Transform a 3D circle with a 3D affine transformation.
%
%   CIRCLE2 = transformPlane3d(CIRCLE, TFM)
%
%   Example
%     circle = [1 1 1 2 45 45 0];
%     tfm = createRotationOz(pi);
%     circle2 = transformCircle3d(circle, tfm);
%     figure('color','w'); hold on; axis equal tight; view(-10,25);
%     xlabel('x'); ylabel('y'); zlabel('z');
%     drawCircle3d(circle,'r'); drawPoint3d(circle(1:3),'r+')
%     drawCircle3d(circle2,'g'); drawPoint3d(circle2(1:3),'g+')
%
%   See also
%     transforms3d, transformPoint3d, transformVector3d, transformLine3d,
%     transformPlane3d
%

% ------
% Author: oqilipo
% E-mail: N/A
% Created: 2022-12-03, using MATLAB 9.13.0.2080170 (R2022b) Update 1
% Copyright 2022

parser = inputParser;
addRequired(parser, 'circle', @(x) validateattributes(x, {'numeric'},...
    {'ncols',7,'real','finite','nonnan'}));
addRequired(parser, 'tfm', @isTransform3d);
parse(parser, circle, tfm);
circle = parser.Results.circle;
tfm = parser.Results.tfm;

% Compute transformation from local basis to world basis
initialTfm = localToGlobal3d(circle(1:3), circle(5), circle(6), circle(7));
% Add the additional transformation
newTfm = tfm*initialTfm;

% Convert to Euler angles
[phi, theta, psi] = rotation3dToEulerAngles(newTfm, 'ZYZ');

% Create transformed circle
circle2 = [transformPoint3d(circle(1:3), tfm), circle(4), theta, phi, psi];


clear;
beta1=[2 0.9];
beta0=[3 2   ];
% fitfunc=@(x,betav)(betav(1)*x.^2+betav(2)*x+betav(3));
fitfunc=@(x,betav) betav(1)*x.^3.*1./(exp(x./betav(2))-1);
% fitfunc=@(x,betav) (sin(betav(1).*x)+0.1*cos(betav(2).*x)+0.15*sin(betav(3).*x));
% fitfunc=@(x,betav) betav(1).*x+betav(2)+betav(3)*x.^2+betav(4).*x.^3;
% fitfunc=@(x,betav) betav(3)*sqrt(2*pi)*abs(betav(2)) *normpdf(x,betav(1),abs(betav(2)));
% fitfunc=@(x,betav) betav(1).*x+betav(2)+betav(3)*x.^2+betav(4).*x.^3+...
% betav(5).*x.^4+betav(6).*x.^5+betav(7).*x.^6+betav(8).*x.^7;

error=0.01 ;
x=[10*eps:0.5:10];
y=fitfunc(x,beta1);
y=y+(randn(size(y)))*error;
yerr=error.*ones(size(y));

errorbarm([x; y; yerr; ])
% pause
textposition=[];
plotbool=true;
axisin=[-10 15 0 12];
axisin=[];
headercell={'Nonlinear Fit'  '$f(x)=\frac{A_0}{\sqrt{2\pi\sigma^2}}\exp{\frac{-(x-\mu)^2}{2\sigma}}$' ''};
mylabel={'xaxis','yaxis [cm]','$\mu$','$\sigma$' '$A_0$'};

[beta betaerr chi prob chiminvec]=wnonlinfit(x,y,yerr,fitfunc,beta0...
    ,'chitol',5,'label',mylabel,'position',textposition,...
    'header',headercell,...
    'printchi','off','axis',axisin,'errprec',2);

 print -dpdf -cmyk test.pdf

% figure(2)
% print -dpdf -cmyk testres.pdf


function Ht = riskmetrics(data,lambda,backCast)
% Computes the Riskmetrics or other EWMA covariance
%
% USAGE:
%  [HT] = riskmetrics(DATA,LAMBDA,BACKCAST)
%
% INPUTS:
%   DATA     - A T by K matrix of zero mean residuals -OR-
%                K by K by T array of covariance estimators (e.g. realized covariance)
%   LAMBDA   - EWMA smoothing parameter 0<LAMBDA<1
%   BACKCAST - [OPTIONAL] Covariance matrix to use as initial value.  If
%                omitted, a backward EWMA is used
%
% OUTPUTS:
%   HT       - A [K K T] dimension matrix of conditional covariances
%
% COMMENTS:
%   The conditional variance, H(t), of an EWMA covariance model
%      H(t) = (1-lambda)*r(t-1,:)'*r(t-1,:) + lambda*H(t-1)
%
% EXAMPLES:
%   % The standard RiskMetrics EWMA covariance is computed using
%   Ht = riskmetrics(data,.94)
%   % The standard RiskMetrics EWMA covariance using the unconditional
%   % covariance as the backcast
%   Ht = riskmetrics(data,.94,cov(data))

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 3    Date: 03/10/2011

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
switch nargin
    case 2
        backCast = [];
    case 3
        % nothing
    otherwise
        error('2 or 3 inputs required.')
end

if ndims(data)==2
    [T,k] = size(data);
    temp = zeros(k,k,T);
    for t=1:T
        temp(:,:,t) = data(t,:)'*data(t,:);
    end
    data = temp;
end

if lambda<=0 || lambda>=1
    error('LAMBDA must be between 0 and 1.')
end

if isempty(backCast)
    endPoint = max(min(floor(log(.01)/log(lambda)),T),k);
    weights = (1-lambda).*lambda.^(0:endPoint-1);
    weights = weights/sum(weights);
    backCast = zeros(k);
    for i=1:endPoint
        backCast = backCast + weights(i)*data(:,:,i);
    end

end

backCast = (backCast+backCast)/2;
if min(eig(backCast))<0
    error('BACKCAST must be positive semidefinite if provided.')
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Ht = zeros(k,k,T);
Ht(:,:,1) = backCast;
for i=2:T
    Ht(:,:,i) = (1-lambda)*data(:,:,i-1) + lambda * Ht(:,:,i-1);
end


%%
% Test for power diagrams

addpath('../toolbox/');
addpath('power_bounded/');
addpath('power_diagrams/');

rep = 'results/semi-discrete/';
[~,~] = mkdir(rep);

% number of sites
N = 200;
% x and y coordinate of the sites
xy = rand(N, 2);
sigma = .1;
xy = sigma*randn(N, 2)+1/2;

% weights
w = ones(N,1);

% the bounding box in clockwise order
bb = [0,0; 0,1; 1,1; 1,0];
% get the power diagram
[V,C] = power_bounded(xy(:,1),xy(:,2), w, bb);
% [PD, PDinf] = powerDiagramWrapper(xy, w);
% draw the resulted power diagram
plot_power(xy,V,C,bb);

q = .01;
axis([-q 1+q -q 1+q]);

% target histogram
Atgt = ones(N,1)/N;

% optimization run
tau = .0004 * N; % gradient step size
niter = 200;
iter_disp = round( [1:8 niter/16 niter/8 niter/4 niter/2 niter] );
iter_disp = 1:2:niter;
err = []; kdisp = 0;
for i=1:niter
    progressbar(i,niter);
    [V,C] = power_bounded(xy(:,1),xy(:,2), w, bb);
    A = area_power(xy,V,C,bb);
    if intersect(i,iter_disp)
        kdisp = kdisp+1;
        clf; plot_power(xy,V,C,bb); axis([-q 1+q -q 1+q]);
        saveas(gcf, [rep 'iteration-' num2str(kdisp), '.eps'], 'epsc');
        drawnow;
    end
    A = A/sum(A); % be sure to be normalized ...
    wnew = w - tau * (A(:)-Atgt(:));
    err(i) = norm(w-wnew);
    w = wnew;
end

% error decay
plot(err); axis tight;

% plot final OT
clf; plot_power(xy,V,C,bb, 2);
saveas(gcf, [rep 'matching.eps'], 'epsc');
axis([-q 1+q -q 1+q]);


function f = tsfilter(ts,flt,mode)

% function f = tsfilter(ts,flt,mode)
%
% <ts> is a b x t matrix with time-series data oriented along the rows
% <flt> is a row vector with the filter (in either the Fourier domain or space domain)
% <mode> (optional) is
%   0 means interpret <flt> as a magnitude filter in the Fourier domain,
%     and do the filtering in the Fourier domain.
%   [1 sz A] means interpret <flt> as a magnitude filter in the Fourier domain,
%     but do the filtering in the space domain using imfilter.m and 'replicate'.
%     in order to convert the Fourier filter to the space domain, we use
%     fouriertospace1D.m and sz and use A as the <mode> input to fouriertospace1D.m.
%     you can omit A, in which case we default A to 1 (which means to ensure that
%     the space filter sums to 1).
%   2 means interpret <flt> as a space-domain filter and do the filtering in the
%     space domain using imfilter.m and 'replicate'.
%   default: 0.
%
% return the filtered time-series data.  we force the output to be real-valued.
% in general, beware of wraparound and edge issues!
%
% history:
% - 2017/10/30 - speed-ups
%
% example:
% flt = zeros(1,100);
% flt(40:60) = 1;
% flt = ifftshift(flt);
% figure; plot(tsfilter(randn(1,100),flt));

% SEE ALSO IMAGEFILTER.M

% constants
num = 1000;  % number to do at a time

% input
if ~exist('mode','var') || isempty(mode)
  mode = 0;
end
if length(mode)==2
  mode = [mode 1];
end

% construct space filter if necessary
if mode(1)==1
  flt = fouriertospace1D(flt,mode(2),[],mode(3));
end

% do it
switch mode(1)
case 0
  f = zeros(size(ts),class(ts));
  for p=1:ceil(size(ts,1)/num)
%    statusdots(p,ceil(size(ts,1)/num));
    mn = (p-1)*num+1;
    mx = min(size(ts,1),(p-1)*num+num);
    f(mn:mx,:) = real(ifft(fft(ts(mn:mx,:),[],2) .* repmat(flt,[mx-mn+1 1]),[],2));
%SLOW:
%    f = cat(1,f,real(ifft(fft(ts(mn:mx,:),[],2) .* repmat(flt,[mx-mn+1 1]),[],2)));
  end
case {1 2}
  f = processmulti1D(@imfilter,ts,flt,'replicate','same','conv');
end


function [C1, C2, U1, U2, H, K, N] = meshCurvatures(vertices, faces, varargin)
%MESHCURVATURES Compute principal curvatures on mesh vertices.
%
%   [C1, C2] = meshCurvatures(VERTICES, FACES)
%   Computes the principal curvatures C1 and C2 for each vertex of the mesh
%   defined by VERTICES and FACES.
%
%   [C1, C2] = meshCurvatures(..., PNAME, PVALUE)
%   Provides additional input arguments based on a list of name-value pairs
%   of arguments. Parameter names can be:
%   * 'SmoothingSteps'      (integer, default: 3)
%       Specifies the number of steps for smoothing vertex curvature
%       tensors.
%   * 'Verbose'             (boolean, default: true)
%       Displays details about algorithm processing.
%   * 'ShowProgress'        (boolean, default: true)
%       Displays a text-based progress bar.
%
%   Algorithm
%   The function is adapted from the "compute_curvature" function, in the
%   "toolbox_graph" fro Gabriel Peyre.
%   The basic idea is to define a curvature tensor for each edge, by
%   assigning a minimum curvature equal to zero along the edge, and a
%   maximum curvature equal to the dihedral angle across the edge.
%   Averaging around the neighbors of a vertex v yields a summation formula
%   over the neighbor edges to compute the curvature tensor of a vertex:
%           1
%   C(v) = ----     Sum       \beta(e) || e \cap A(v) || ebar ebar^t
%          A(v)  {e \in A(v)}
%   where:
%   * A(v) is the neighborhood region, usually defined as a 'ring' around
%       the vertex v
%   * beta(e) is the dihedral angle between the normals of the two faces
%       incident to edge e
%   * || e \cap A(v) || is the length of e (more exactly, the length of the
%       part of e contained within the neighborhood region
%   * ebar is the normalized edge
%
%   The curvature tensor is then decomposed into C = P D P^-1, with P
%   containing main direction vectors and normal, and D being a diagonal
%   matrix with the two main curvatures and zero along the diagonal.
%
%   References
%   * David Cohen-Steiner and Jean-Marie Morvan (2003).
%       "Restricted Delaunay triangulations and normal cycle".
%       In Proc. 19th Annual ACM Symposium on Computational Geometry,
%       pages 237-246.
%   * Pierre Alliez, David Cohen-Steiner, Olivier Devillers, Bruno Levy,
%       and Mathieu Desbrun (2003). "Anisotropic Polygonal Remeshing".
%       ACM Transactions on Graphics.
%       (SIGGRAPH '2003 Conference Proceedings)
%   * Mario Botsch, Leif Kobbelt, M. Pauly, P. Alliez, B. Levy (2010).
%       "Polygon Mesh Processing", Taylor and Francis Group, New York.
%
%   Example
%     [v, f] = torusMesh;
%     f2 = triangulateFaces(f);
%     [c1, c2] = meshCurvatures(v, f2);
%     figure; hold on; axis equal; view(3);
%     drawMesh(v, f2, 'VertexColor', c1 .* c2);
%
%   See also
%     meshes3d, drawMesh, triangulateFaces
%

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2021-09-21, using Matlab 9.10.0.1684407 (R2021a) Update 3
% Copyright 2021-2022 INRAE - BIA Research Unit - BIBS Platform (Nantes)

%% Process input arguments

% default values for options
nIters = 3;
verbose = true;
showProgress = true;

while length(varargin) > 1
    name = varargin{1};
    if strcmpi(name, 'SmoothingSteps')
        nIters = varargin{2};
    elseif strcmpi(name, 'Verbose')
        verbose = varargin{2};
    elseif strcmpi(name, 'ShowProgress')
        showProgress = varargin{2};
    else
        error('Unknown option: %s', name);
    end
    varargin(1:2) = [];
end

% validate vertices
if ~isnumeric(vertices) || size(vertices, 2) ~= 3
    error('Requires vertices to be a N-by-3 numeric array');
end

% ensure faces are triangular
if ~isnumeric(faces) || size(faces, 2) > 3
    warning('requires triangle mesh, forces triangulation');
    faces = triangulateFaces(faces);
end

%% Retrieve adjacency relationships

if verbose
    disp('compute adjacencies');
end

% number of elements of each type
nv = size(vertices, 1);
nf = size(faces, 1);

% ev1 and ev2 are indices of source and target vertex of each edge
% (recomputed later)
ev1 = [faces(:,1); faces(:,2); faces(:,3)];
ev2 = [faces(:,2); faces(:,3); faces(:,1)];

% Compute sparse matrix representing edge-to-face adjacency
s = [1:nf 1:nf 1:nf]';
A = sparse(ev1, ev2, s, nv, nv);

% converts sparse matrix to indices of adjacent vertices and faces
[~, ~, ef1] = find(A);         % index of 'right' face
[ev1, ev2, ef2] = find(A');    % index of 'left' face, and of vertices

% edges are consdered twice (one for each vertex)
% keep only the edge with lower source index
inds = find(ev1 < ev2);
ef1 = ef1(inds);
ef2 = ef2(inds);
ev1 = ev1(inds);
ev2 = ev2(inds);

% number of edges
ne = length(ev1);

%% Compute geometry features

% compute edge direction vectors
edgeVectors = vertices(ev2,:) - vertices(ev1,:);

% normalize edge direction vecotrs
d = sqrt(sum(edgeVectors.^2, 2));
edgeVectors = bsxfun(@rdivide, edgeVectors, d);

% avoid too large numerics
d = d ./ mean(d);

% normals to faces
normals = meshFaceNormals(vertices, faces);

% ensure normals point outward the mesh
if meshVolume(vertices, faces) < 0
    normals = -normals;
end

% inner product of normals
dp = sum(normals(ef1, :) .* normals(ef2, :), 2);

% compute the (unsigned) dihedral angle between the normals of the two
% faces incident to each edge
beta = acos(min(max(dp, -1), 1));

% relatice orientation of face normals cross product and edge orientation
cp = crossProduct3d(normals(ef1, :), normals(ef2, :));
si = sign(sum(cp .* edgeVectors, 2));

% compute signed dihedral angle
beta = beta .* si;

%% Compute tensors

if verbose
    disp('compute edge tensors');
end

% curvature tensor of each edge
T = zeros(3, 3, ne);
for i = 1:3
    for j = 1:i
        T(i, j, :) = reshape(edgeVectors(:,i) .* edgeVectors(:,j), 1, 1, ne);
        T(j, i, :) = T(i, j, :);
    end
end
T = bsxfun(@times, T, reshape(d .* beta, [1 1 ne]));

% curvature tensor of each vertex by pooling edge tensors
Tv = zeros(3, 3, nv);
w = zeros(1, 1, nv);
for k = 1:ne
    if showProgress
        displayProgress(k, ne);
    end
    Tv(:,:,ev1(k)) = Tv(:,:,ev1(k)) + T(:,:,k);
    Tv(:,:,ev2(k)) = Tv(:,:,ev2(k)) + T(:,:,k);
    w(:,:,ev1(k)) = w(:,:,ev1(k)) + 1;
    w(:,:,ev2(k)) = w(:,:,ev2(k)) + 1;
end
w(w < eps) = 1;
Tv = Tv ./ repmat(w, [3 3 1]);

if verbose
    disp('average vertex tensors');
end

% apply smoothing on the tensor field
for i = 1:3
    for j = 1:3
        a = Tv(i, j, :);
        a = smoothMeshFunction(vertices, faces, a(:), nIters);
        Tv(i, j, :) = reshape(a, [1 1 nv]);
    end
end

%% Retrieve curvatures and eigen vectors from tensors

if verbose
    disp('retrieve curvatures');
end

% allocate memory
U = zeros(3, 3, nv);
D = zeros(3, nv);

% iterate over vertices
for k = 1:nv
    % display progress
    if showProgress
        displayProgress(k,nv);
    end

    % extract eigenvectors and eigenvalues for current vertex
    [u, d] = eig(Tv(:,:,k));
    d = real(diag(d));

    % sort acording to [normal, min curv, max curv]
    [~, I] = sort(abs(d));
    D(:, k) = d(I);
    U(:, :, k) = real(u(:,I));
end

% retrieve main curvatures and associated directions
C1 = D(2,:)';
C2 = D(3,:)';
U1 = squeeze(U(:,3,:))';
U2 = squeeze(U(:,2,:))';

% enforce C1 < C2
inds = find(C1 > C2);
C1tmp = C1;
U1tmp = U1;
C1(inds) = C2(inds);
C2(inds) = C1tmp(inds);
U1(inds,:) = U2(inds,:);
U2(inds,:) = U1tmp(inds,:);

% compute optional output arguments
if nargout > 4
    % average and gaussian curvatures
    H = (C1 + C2) / 2;
    K = C1 .* C2;

    if nargout > 6
        % normal vector for each vertex
        N = squeeze(U(:,1,:))';
    end
end

function displayProgress(n, N)
% Display the progress of current step using a text-based progress bar.
%
% based on the 'progressbar' function in G. Peyre's Graph Toolbox.

% width of the progress bar
w = 20;

% compute progress ratio as an integer betsween 0 and w
p = min( floor(n/N*(w+1)), w);

global pprev;
if isempty(pprev)
    pprev = -1;
end

if p ~= pprev
    str1 = repmat('*', 1, p);
    str2 = repmat('.', 1, w-p);
    str = sprintf('[%s%s]', str1, str2);
    if n > 1
        % clear previous string
        fprintf(repmat('\b', [1 length(str)]));
    end
    fprintf(str);
end

pprev = p;
if n == N
    fprintf('\n');
end


function [R,eff] = randmio_und_signed(W, ITER)
%RANDMIO_UND     Random graph with preserved signed degree distribution
%
%   R       = randmio_und_signed(W,ITER);
%   [R,eff] = randmio_und_signed(W,ITER);
%
%   This function randomizes an undirected network with positively and
%   negatively signed connections, while preserving the positively and
%   negatively signed degree distribution. The function does not preserve
%   the strength distribution in weighted networks.
%
%   Input:      W,      undirected (binary/weighted) connection matrix
%               ITER,   rewiring parameter
%                       (each edge is rewired approximately ITER times)
%
%   Output:     R,      randomized network
%               eff,    number of actual rewirings carried out
%
%   Reference:  Maslov and Sneppen (2002) Science 296:910
%
%
%   2011-2015
%   Dani Bassett, UCSB
%   Olaf Sporns,  Indiana U
%   Mika Rubinov, U Cambridge

%   Modification History:
%   Mar 2011: Original (Dani Bassett, based on randmio_und.m)
%   Mar 2012: Limit number of rewiring attempts,
%             count number of successful rewirings (Olaf Sporns)
%   Dec 2015: Rewritten the core of the rewiring algorithm to allow
%             unbiased exploration of all network configurations. The new
%             algorithm allows positive-positive/negative-negative
%             rewirings, in addition to the previous positive-positive/0-0
%             and negative-negative/0-0 rewirings (Mika Rubinov).

if nargin('randperm')==1
    warning('This function requires a recent (>2011) version of MATLAB.')
end

R     = double(W);              % sign function requires double input
n     = size(R,1);
ITER  = ITER*n*(n-1)/2;

% maximal number of rewiring attempts per 'iter'
maxAttempts = round(n/2);
% actual number of successful rewirings
eff = 0;

for iter=1:ITER
    att=0;
    while (att<=maxAttempts)    %while not rewired
        %select four distinct vertices
        nodes = randperm(n,4);
        a = nodes(1);
        b = nodes(2);
        c = nodes(3);
        d = nodes(4);

        r0_ab = R(a,b);
        r0_cd = R(c,d);
        r0_ad = R(a,d);
        r0_cb = R(c,b);

        %rewiring condition
        if      (sign(r0_ab)==sign(r0_cd)) && ...
                (sign(r0_ad)==sign(r0_cb)) && ...
                (sign(r0_ab)~=sign(r0_ad))

            R(a,d)=r0_ab; R(a,b)=r0_ad;
            R(d,a)=r0_ab; R(b,a)=r0_ad;
            R(c,b)=r0_cd; R(c,d)=r0_cb;
            R(b,c)=r0_cd; R(d,c)=r0_cb;

            eff = eff+1;
            break;
        end %rewiring condition
        att=att+1;
    end %while not rewired
end %iterations


% DEMVOWELS2 Model the vowels data with a 2-D FGPLVM using RBF kernel.

% FGPLVM

% Fix seeds
randn('seed', 1e5);
rand('seed', 1e5);

dataSetName = 'vowels';
experimentNo = 2;

% load data
[Y, lbls] = lvmLoadData(dataSetName);

% Set up model
options = fgplvmOptions('fitc');
options.numActive = 200;
latentDim = 2;
d = size(Y, 2);

model = fgplvmCreate(latentDim, d, Y, options);

% Optimise the model.
iters = 1000;
display = 1;

model = fgplvmOptimise(model, display, iters);

% Save the results.
capName = dataSetName;;
capName(1) = upper(capName(1));
save(['dem' capName num2str(experimentNo) '.mat'], 'model');

if exist('printDiagram') & printDiagram
  fgplvmPrintPlot(model, lbls, capName, experimentNo);
end

% Load the results and display dynamically.
fgplvmResultsDynamic(dataSetName, experimentNo, 'vector')

errors = fgplvmNearestNeighbour(model, lbls);


function [kWh] = eV2kWh(eV)
% Convert energy or work from electron volts to kilowatt-hours.
% Chad A. Greene 2012
kWh = eV*4.4504925e-26;


% StackExchange Signal Processing Q81493
% https://dsp.stackexchange.com/questions/81493
% Applying 2D Sinc Interpolation for Upsampling in the Fourier Domain (DFT / FFT)
% References:
%   1.
% Remarks:
%   1.  B
% TODO:
% 	1.  C
% Release Notes
% - 1.0.000     29/12/2021
%   *   First release.

%% General Parameters

subStreamNumberDefault = 79;

run('InitScript.m');

figureIdx           = 0;
figureCounterSpec   = '%04d';

generateFigures = ON;

%% Simulation Parameters

numRowsI = 5000;
numColsI = 5200;

numRowsO = 10000;
numColsO = 10400;

sincRadius = 5;

%% Generate / Load Data

mX      = GenTest([numRowsI, numColsI], sincRadius);
mYRef   = GenTest([numRowsO, numColsO], sincRadius);

mY = DftReSample2D(mX, [numRowsO, numColsO]);

%% Analysis

disp(['The interpolation error is given by: ', num2str(max(abs(mYRef - mY), [], 'all'))]);

%% Auxilizary Function

function [ mX ] = GenTest( vSize, sincRadius )

vX = linspace(-sincRadius, sincRadius, vSize(2) + 1);
vX(end) = [];
vY = linspace(-sincRadius, sincRadius, vSize(1) + 1);
vY = vY(:);
vY(end) = [];

% mX = abs(vX) + abs(vY) + sinc(sqrt(vX .^2 + vY .^2));
mX = sinc(sqrt(vX .^2 + vY .^2));

end

%% Restore Defaults

% set(0, 'DefaultFigureWindowStyle', 'normal');
% set(0, 'DefaultAxesLooseInset', defaultLoosInset);


function plot2dGaussian(Priors,Mus,Sigmas)
%PLOT2DGAUSSIAN Summary of this function goes here
%   Detailed explanation goes here

disp('in plot2dGaussians');

GMM=struct('Priors',Priors,'Mus',Mus,'Sigmas',Sigmas,'K',size(Mus,2));

spacing=1000;
xGMM=marginalizeGMM(GMM,1);
xs=linspace(min(xGMM.Mus'-2*sqrt(squeeze(xGMM.Sigmas))),max(xGMM.Mus'+2*sqrt(squeeze(xGMM.Sigmas))),spacing);
yGMM=marginalizeGMM(GMM,2);
ys=linspace(min(yGMM.Mus'-2*sqrt(squeeze(yGMM.Sigmas))),max(yGMM.Mus'+2*sqrt(squeeze(yGMM.Sigmas))),spacing);
pdraw=zeros(1,spacing^2);
[X Y]=meshgrid(xs,ys);
x=X(:);
y=Y(:);
vtests2=[x y]';
GMM.K
for k=1:GMM.K
    pdraw=pdraw+GMM.Priors(k).*gaussPDF(vtests2,GMM.Mus(:,k),GMM.Sigmas(:,:,k))';
end

ppdraw=reshape(pdraw,spacing,spacing);

contourf(xs,ys,ppdraw);
%pcolor(xs,ys,ppdraw);
%shading interp;

set(gca,'YDir','normal');

end


function jed = moon_phase_to_jed ( n, phase )

%*****************************************************************************80
%
%% MOON_PHASE_TO_JED calculates the JED of a moon phase.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 March 2013
%
%  Reference:
%
%    William Press, Brian Flannery, Saul Teukolsky, William Vetterling,
%    Numerical Recipes: The Art of Scientific Computing,
%    Cambridge University Press.
%
%  Parameters:
%
%    Input, integer N, specifies that the N-th such phase
%    of the moon since January 1900 is to be computed.
%
%    Input, integer PHASE, specifies which phase is to be computed.
%    0=new moon,
%    1=first quarter,
%    2=full,
%    3=last quarter.
%
%    Output, real JED, the Julian Ephemeris Date on which the
%    requested phase occurs.
%
  degrees_to_radians = pi / 180.0;
%
%  First estimate.
%
  j = 2415020 + 28 * n + 7 * phase;
%
%  Compute a correction term.
%
  c = n + phase / 4.0;

  t = c / 1236.85;

  xtra = 0.75933 + 1.53058868 * c + ( 0.0001178 - 0.000000155 * t ) * t * t;

  as = degrees_to_radians * ( 359.2242 + 29.105356 * c );

  am = degrees_to_radians * ( 306.0253 + 385.816918 * c + 0.010730 * t * t );

  if ( phase == 0 || phase == 2 )

    xtra = xtra + ( 0.1734 - 0.000393 * t ) * sin ( as ) - 0.4068 * sin ( am );

  elseif ( phase == 1 || phase == 3 )

    xtra = xtra + ( 0.1721 - 0.0004 * t ) * sin ( as ) - 0.6280 * sin ( am );

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, 'MOON_PHASE_TO_JED - Fatal error!\n' );
    fprintf ( 1, '  Illegal PHASE option = %d\n', phase );
    error ( 'MOON_PHASE_TO_JED - Fatal error!' );

  end

  jed = j + xtra;

  return
end


function varargout = power_internal1(varargin)
%power_internal1
% Used for cases such as 2^x, and is treated as evaluation-based operators

switch class(varargin{1})

    case 'double'
        varargout{1} = varargin{2}.^varargin{1};

    case 'sdpvar'
        if isa(varargin{2},'sdpvar')
            x = varargin{2};
            y = varargin{1};
            varargout{1} = exp(y*log(x)); %x^y = exp(log(x^y))
        else
            if length(varargin{1}) > 1 || size(varargin{2},1) ~= size(varargin{2},2)
                error('Inputs must be a scalar and a square matrix. To compute elementwise POWER, use POWER (.^) instead.');
            end
            x = varargin{2};
            y = varargin{1};
            if isa(x,'double') && x==1 && length(y)==1
                varargout{1} = 1;
            else
                varargout{1} = InstantiateElementWise(mfilename,varargin{:});
            end
        end

    case 'char'

        X = varargin{3};
        Y = varargin{4};
        F=[];
        if Y>=1
            operator = struct('convexity','none','monotonicity','increasing','definiteness','positve','model','callback');
        elseif Y>=0
            operator = struct('convexity','none','monotonicity','decreasing','definiteness','positive','model','callback');
        else
            % Base is negative, so the power has to be an integer
            F = (integer(X));
            operator = struct('convexity','none','monotonicity','decreasing','definiteness','none','model','callback');
        end

        operator.bounds = @bounds_power;
        operator.convexhull = @convexhull_power;
        operator.derivative = @(x)derivative(x,Y);
        if Y >= 0
            operator.inverse = @(x,Y)inverse(x,Y);
        end

        varargout{1} = F;
        varargout{2} = operator;
        varargout{3} = [X(:);Y(:)];
    otherwise
        error('SDPVAR/power_internal1 called with CHAR argument?');
end

% This should not be hidden here....
function [L,U] = bounds_power(xL,xU,base)
if base >= 1
    L = base^xL;
    U = base^xU;
elseif base>= 0
    L = base^xU;
    U = base^xL;
else
    disp('Not implemented yet. Report bug if you need this')
    error
end

function x = inverse(y,base)
if y <=0
    x = -inf;
else
    x = log(y)/log(base);
end

function df = derivative(x,base)
if length(base)~=length(x)
    base = base*ones(size(x));
end
f = base.^x;
df = log(base)*f;

function [Ax, Ay, b] = convexhull_power(xL,xU,base)
fL = base^xL;
fU = base^xU;
dfL = log(base)*fL;
dfU = log(base)*fU;
[Ax,Ay,b] = convexhullConvex(xL,xU,fL,fU,dfL,dfU);


% This code demonstrates how the boundary matching code works,
% and how the benchmark uses it.  This is not meant to show
% how to run the benchmark.  See the README file for that.

% setup
present = 'color';
iid = 101085;
nthresh = 10;

% read the image
im = rgb2gray(double(imread(imgFilename(iid)))/255);
figure(1); clf;
imshow(im);

% create a pb image
%pb = pbNitzberg(im);
pb = pbGM(im);
figure(2); clf;
imagesc(pb,[0 1]);
axis image; axis off; truesize;

% read segmentations
segs = readSegs(present,iid);

% match the first seg and a thresholded pb
bmap1 = double(seg2bmap(segs{1}));
bmap2 = double(pb > 0.5);
[match1,match2,cost,oc] = correspondPixels(bmap1,bmap2,0.01,1000);
h=figure(3); clf;
plotMatch(h,bmap1,bmap2,match1,match2);
title('Seg1 vs. Pb>0.5','Color',[1 1 1]);

% compare the pb and segs
[thresh,cntR,sumR,cntP,sumP] = boundaryPR(pb,segs,nthresh);

% precision/recall plot
r = cntR./(sumR+(sumR==0));
p = cntP./(sumP+(sumP==0));
f = 2.*r.*p./(r+p+((r+p)==0));
figure(4); clf;
plot(r,p,'-o');
axis equal; axis([0 1 0 1]);
xlabel('Recall'); ylabel('Precision');

% find best F-measure (should interpolate)
[t,idx] = max(f(:));
title(sprintf('F=%.2g at (R,P)=(%.2g,%.2g) t=%.2g',...
              f(idx),r(idx),p(idx),thresh(idx)));


function f = p42_f ( n, x )

%*****************************************************************************80
%
%% P42_F evaluates the objective function for problem 42.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 March 2002
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    MJD Powell,
%    An Efficient Method for Finding the Minimum of a Function of
%    Several Variables Without Calculating Derivatives,
%    Computer Journal,
%    Volume 7, Number 2, pages 155-162, 1964.
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the argument of the objective function.
%
%    Output, real F, the value of the objective function.
%
  if ( x(2) == 0.0 )
    term = 0.0;
  else
    arg = ( x(1) + 2.0 * x(2) + x(3) ) / x(2);
    term = exp ( - arg^2 );
  end

  f = 3.0 ...
    - 1.0 / ( 1.0 + ( x(1) - x(2) )^2 ) ...
    - sin ( 0.5 * pi * x(2) * x(3) ) ...
    - term;

  return
end