% load sample EEG data
load sampleEEGdata

% wavelet parameters
min_freq = 2;
max_freq = 48;
num_frex = 30;

% other wavelet parameters
frequencies = logspace(log10(min_freq),log10(max_freq),num_frex);
time = -1:1/EEG.srate:1;
half_of_wavelet_size = (length(time)-1)/2;

% FFT parameters (use next-power-of-2)
n_wavelet     = length(time);
n_data        = EEG.pnts*EEG.trials;
n_convolution = n_wavelet+n_data-1;
n_conv_pow2   = pow2(nextpow2(n_convolution));
wavelet_cycles= 4;

chan2plot = 'fz';

% define baseline period
baselinetime = [ -500 -200 ]; % in ms

% convert baseline window time to indices
[junk,baselineidx(1)]=min(abs(EEG.times-baselinetime(1)));
[junk,baselineidx(2)]=min(abs(EEG.times-baselinetime(2)));

tf_data = zeros(2,length(frequencies),EEG.pnts);

fft_data = fft(reshape(EEG.data(strcmpi(chan2plot,{EEG.chanlocs.labels}),:,:),1,[]),n_conv_pow2);

for fi=1:length(frequencies)

    % create wavelet and get its FFT
    wavelet = exp(2*1i*pi*frequencies(fi).*time) .* exp(-time.^2./(2*( wavelet_cycles /(2*pi*frequencies(fi)))^2));
    fft_wavelet = fft(wavelet,n_conv_pow2);
    fft_wavelet = fft_wavelet./max(fft_wavelet);

    % run convolution
    convolution_result_fft = ifft(fft_wavelet.*fft_data,n_conv_pow2);
    convolution_result_fft = convolution_result_fft(1:n_convolution);
    convolution_result_fft = convolution_result_fft(half_of_wavelet_size+1:end-half_of_wavelet_size);
    convolution_result_fft = reshape(convolution_result_fft,EEG.pnts,EEG.trials);

    % put power data into time-frequency matrix
    tf_data(1,fi,:) = mean(abs(convolution_result_fft).^2,2);
    tf_data(2,fi,:) = median(abs(convolution_result_fft).^2,2);
end

baseline_power = squeeze(mean(tf_data(1,:,baselineidx(1):baselineidx(2)),3));
dbconverted = 10*log10( squeeze(bsxfun(@rdivide,tf_data(1,:,:),baseline_power) ));

%%

surf(dbconverted(4:end,155:539))
h=findobj('Type','surface');
%set(h,'CData',double(asdf(4:end,155:539)))
set(gca,'clim',[-3 3],'yscale','log'), shading interp, axis off
set(gcf,'color','k')
cmap=(1+[cos(linspace(0,pi*2,100)); sin(linspace(0,pi*2,100)); cos(linspace(0,pi*2,100))])/2;
colormap(cmap')


function [x_world] = Robot2World(x_robot,x)
    %% Change of coordinates from robot fram to world frame

    x_world = zeros(size(x));
    for i = 1:size(x,2)
    x_world(1,i) = cos(x_robot(3))*(x(1,i)) - sin(x_robot(3))*(x(2,i)) + x_robot(1);
    x_world(2,i) = cos(x_robot(3))*(x(2,i)) + sin(x_robot(3))*(x(1,i)) + x_robot(2);
    end
end


% The OxyGEN proyect by Profoty.xyz
% Together against the Covid-19
% V4.0 rev.17/03/2020
%
% Website/blog: oxygen.protofy.xyz
% Contact: oxygen@protofy.xyz

%GEOMETRICAL PARAMETERS

clear;

%Length from the bearing to the hinge
l0 = 27.5;

%Length of the vertical wall that supports the hinge
h0 = 6.5;

%Lenght of the vertical support of the bearing
hb = 4.0;

%Bearing radius
br = 1.1;

%Array of different max and min positions of the bearing
h1a = [6.5, 7.1, 7.7, 8.3, 8.9];
h2a = [20.5, 20.5, 20.5, 20.5, 20.5];

%Number of cycles per cam turn (1 to 3).
nc = 1;

%Minimum radius of the camshaft
rmin = 5;

%BREATHING CYCLE PARAMETERS

%Duration of the inhale cycle / duration of the whole cycle
lambda1 = 0.500;
lambda2 = 0.04;

%Soft transition between inhale and exhale cycles
dpsi21 = 0.05;
dpsi12 = 0.04;

%Adjust parameters of the inhale curve
ga1 = 3.1;
gb1 = .85;
ff1 = 50;

%Adjust parametes of the exhale curve
ga2 = 3.1;
gb2 = .80;
ff2 = 50;

%GENERATION OF THE CURVES AND THE CAMSHAFT

%Generation of the geometry

l = sqrt(l0^2+hb^2);

for i = 1:numel(h1a);

    ymin(i) = h1a(i);
    ymax(i) = h2a(i);

    alphamin(i) = acos((h0 - h1a(i)) / l);
    alphamax(i) = acos((h0 - h2a(i)) / l);

    xmin(i) = l*sin(alphamin(i));
    xmax(i) = l*sin(alphamax(i));

    d(i) = sqrt((xmin(i)-xmax(i))^2 + (ymin(i)-ymax(i))^2);

    alphatan(i) = (alphamin(i) + alphamax(i)) / 2;

    xtan(i) = l*sin(alphatan(i));
    ytan(i) = h0 - l*cos(alphatan(i));

    xsup(i) = xtan(i) + (d(i)/2)*cos(alphatan(i));
    xinf(i) = xtan(i) - (d(i)/2)*cos(alphatan(i));

    ysup(i) = ytan(i) + (d(i)/2)*sin(alphatan(i));
    yinf(i) = ytan(i) - (d(i)/2)*sin(alphatan(i));

    xcam(i) = xtan(i) + (d(i)/2 + rmin + br)*cos(alphatan(i));
    ycam(i) = ytan(i) + (d(i)/2 + rmin + br)*sin(alphatan(i));

end;

%Time increment
dt = 0.01;

%time coordinate during the whole cycle
theta = 0:dt:2*pi;

%Generation of the soft transition between inhale and exhale curves
psi1 = [];
psi2 = [];

for i = 1:numel(theta);

    if theta(i) < (lambda2-dpsi21/2)*2*pi;
        psi1(i) = 0;

    elseif theta(i) < (lambda2+dpsi21/2)*2*pi;
        psi1(i) = 0.5 + 0.5*cos((theta(i)-(lambda2+dpsi21/2)*2*pi)/(2*dpsi21));

    else theta(i) < (lambda1-dpsi12/2)*2*pi;
        psi1(i) = 1;

    end;

    psi2(i) = 1 - psi1(i);

end;

%Generation of the complete breathing cycle rho(theta)

rho = [];
rho1 = [];
rho2 = [];
rho2next = [];

rhomin = 1000;
rhomax = 0;

for i = 1:numel(theta);

    %Inhale curve
    rho1(i) = ff1*gampdf(theta(i), ga1, gb1);
    rho1next(i) = ff1*gampdf(theta(i)+2*pi, ga1, gb1);

    %Exhale curve
    rho2(i) = ff2*gampdf(theta(i), ga2, gb2);
    rho2next(i) = ff2*gampdf(theta(i)+2*pi, ga2, gb2);

    rho(i) = max(rho1(i), rho1next(i));

    %Capturing min and max in order to generate the normalized curve
    if rho(i) > rhomax
        rhomax = rho(i);
    end;

    if rho(i) < rhomin
        rhomin = rho(i);
    end;

end;

%Generation of a normalised curve and camshaft

rhonorm = [];
rhocam = [];

for i = 1:numel(theta);

    rhonorm(i) = (rho(i)-rhomin)/(rhomax-rhomin);

    for n = 1:numel(d)

        rhocam(n,i) = rmin + rhonorm(i)*d(n);

    end;

end;

%Generation of the first derivate of the camshaft geometry to analize and
%validate the design

drho = [];
drhonorm = [];
drhocam = [];

a = rmin;
b = 1/dt;

for i = 1 : numel(rho)-1;

    drho(i) = b*(rho(i+1)-rho(i));
    drhonorm(i) = b*(rhonorm(i+1)-rhonorm(i));
    drhocam(i) = b*(rhocam(i+1)-rhocam(i)) + a;

end;

drho(numel(rho)) = b*(rho(1)-rho(numel(rho)));
drhonorm(numel(rhonorm)) = b*(rhonorm(1)-rhonorm(numel(rhonorm)));
drhocam(numel(rhocam)) = b*(rhocam(1)-rhocam(numel(rhocam))) + a;

%X and Y coordinates during two cycles (for 2-cycle camshaft plot)

theta2 = [theta/2, (2*pi+theta)/2];

rhocam2 = [rhocam, rhocam];
drhocam2 = [drho, drho];

%X and Y coordinates during three cycles (for 3-cycle camshaft plot)

theta3 = [theta/3, (2*pi+theta)/3, (4*pi+theta)/3];

rhocam3 = [rhocam, rhocam, rhocam];
drhocam3 = [drho, drho, drho];

%GENERATION OF THE PLOTS

%Trying to print it out in real size (FAIL)
%set(gcf,'PaperUnits','centimeters');
%set(gcf,'PaperSize',[42 29.7]);

fpos = figure('Name', 'Dimensions', 'Units', 'centimeters', 'NumberTitle', 'off');
set(gcf,'PaperUnits','centimeters', 'PaperSize', [42/2 27.9/2]);
set(gcf, 'units', 'centimeters', 'position', [0, 0, 28, 20]);
fpos = gcf;
hold on

xlim([-5 35]);
ylim([0 30]);

for i = 1:numel(xmin)

    plot([0, xmin(i)], [h0, ymin(i)], '-x')
    plot([0, xmax(i)], [h0, ymax(i)], '-x')

    plot([0, xtan(i)], [h0, ytan(i)], '--xb')

    plot([xsup(i), xinf(i)], [ysup(i), yinf(i)], '--xr')
    scatter(xcam(i), ycam(i))

end;

hold off

%Plot of the breathing cycle interpolation by curves

fcycle = figure('Name', 'Breath Cycle curves', 'Units', 'centimeters', 'NumberTitle', 'off');

hold on
plot(theta, psi1, '-k')
plot(theta, psi2, '-k')
plot(theta, rho1, '-g')
plot(theta, rho1next, '-g')
plot(theta, rho2, '-g')
plot(theta, rho2next, '-g')
%plot(theta, drho, '-r')
plot(theta, rho, '-b')
hold off

set(gcf,'PaperUnits','centimeters', 'PaperSize', [42/2 27.9/2]);
set(gcf, 'units', 'centimeters', 'position', [0, 0, 27.9, 27.9]);
fcam3 = gcf;

%Plot of the normalized breathing cycle and first derivate

fnorm = figure('Name', 'Normalized Breath Cycle', 'Units', 'centimeters', 'NumberTitle', 'off');

hold on
%plot(theta, drhonorm, '-r')
plot(theta, rhonorm, '-b')
hold off

set(gcf,'PaperUnits','centimeters', 'PaperSize', [42/2 27.9/2]);
set(gcf, 'units', 'centimeters', 'position', [0, 0, 27.9, 27.9]);
fcam3 = gcf;

%Plot of a 1-cycle camshaft

for n = 1:numel(d)

    fcam1 = figure('Name', '1-Cycle Camshaft', 'Units', 'centimeters', 'NumberTitle', 'off');
    %hold on

    %polarplot(theta, drhocam, '-r')
    polar(theta, rhocam(n,:), '-b')

    %rlim([0 25]);
    hold off

    set(gcf,'PaperUnits','centimeters', 'PaperSize', [42 27.9]);
    set(gcf, 'units', 'centimeters', 'position', [0, 0, 27.9, 27.9]);
    fcam3 = gcf;

end

% %Plot of a 2-cycle camshaft
%
% fcam2 = figure('Name', '2-Cycle', 'Units', 'centimeters', 'NumberTitle', 'off');
%
% %hold on;
% %polar(theta, drhocam2, '-r')
% polar(theta2, rhocam2, '-b')
% %hold off;
%
% set(gcf,'PaperUnits','centimeters', 'PaperSize', [42 27.9]);
% set(gcf, 'units', 'centimeters', 'position', [0, 0, 27.9, 27.9]);
% fcam3 = gcf;
%
%
% %Plot of a 3-cycle camshaft
%
% fcam3 = figure('Name', '3-Cycle', 'Units', 'centimeters', 'NumberTitle', 'off');
%
% %hold on;
% %polar(theta,drhocam3)
% polar(theta3, rhocam3)
% %hold off;
%
% set(gcf,'PaperUnits','centimeters', 'PaperSize', [42 27.9]);
% set(gcf, 'units', 'centimeters', 'position', [0, 0, 27.9, 27.9]);
% fcam3 = gcf;

% The OxyGEN proyect by Profoty.xyz
% Together against the Covid-19
% V4.0 rev.17/03/2020
%
% Website/blog: oxygen.protofy.xyz
% Contact: oxygen@protofy.xyz


clear all
        % Desemnarea unor variabile simbolice
syms x u v t
        % Generarea unei functii de o variabila
f=sin(x)^3;
        % Deschiderea unei ferestre grafice noi
figure(1)
        % Reprezentarea functiei f intre limitele implicite
ezplot(f)
        % Reprezentarea functiei f intre limitele [0, 8*pi]
figure(2)
ezplot(f,[0,8*pi])
        % Generarea unei functii de doua variabile
g=u^2-v^2+1;
        % Reprezentarea functiei g de doua variabile intre limitele date
figure(3)
ezplot(g,[-2,2,-5,5])
        % Generarea unei functii parametrice
x=sin(t);
y=cos(t);
        % Reprezentarea functiei parametrice intre limitele impuse
figure(4)
ezplot(x,y)


% Chemical master equation for 2H + O <-> H2O

L = 7;  % QTT dimension in state variables

Trange = 0:0.5:5;
d0ts = 12*ones(1,numel(Trange)-1);

tol = 1e-7;
eps = 1e-12;

% initial distribution
l = [13,15,0]; % H, O, H2O

% One shift z=-1 (for O)
S1 = tt_shf(L);
S2 = round(S1*S1, eps); %  double shift z=-2 (for H)
% z=1 for H2O will be S1'

I = tt_eye(2,3*L);
e1 = tt_tensor(num2cell([1;0]*ones(1,L), 1));
e2 = zeros(2^L,1); e2(2)=1; e2 = tt_tensor(reshape(e2, 2*ones(1,L)),eps);

x = tt_x(2,L);
o = tt_ones(2,L);

w1 = 2*mtkron(o-e1-e2, o-e1, o);
w2 = 1*mtkron(o, o, o-e1);
% w = mtkron(x, x, o);

% CME matrix
A = -(mtkron(S2,S1,S2')*diag(w1) - diag(w1)); % forward
A = A - (mtkron(S2',S1',S2)*diag(w2) - diag(w2)); % backward
A = round(A, eps);

% initial state
e1 = zeros(2^L,1); e1(l(1)+1)=1;
u0 = tt_tensor(e1);
e1 = zeros(2^L,1); e1(l(2)+1)=1;
u0 = tkron(u0, tt_tensor(e1));
e1 = zeros(2^L,1); e1(l(3)+1)=1;
u0 = tkron(u0, tt_tensor(e1));
% make QTT
u0 = tt_reshape(u0, 2*ones(1,3*L), eps);

Nt = numel(Trange)-1;
u = u0;
ll = zeros(Nt, 3);
tic;
for t=1:Nt
    d0t = d0ts(t);
    tau = (Trange(t+1)-Trange(t))/(2^d0t);

    Grad_t = IpaS(d0t,-1)/tau;
%     CN_term = IpaS(d0t,1)*0.5;   % Crank-Nicolson
    CN_term = tt_eye(2,d0t);   % Backw Euler
    e1t = tt_tensor(num2cell([1;0]*ones(1,d0t), 1));
    et = tt_ones(2, d0t);

    M = tkron(I, Grad_t)+tkron(A, CN_term);

    rhs = u/tau; %-0.5*A*u;
    rhs = tkron(rhs, e1t);
    U = tkron(u, et);

    U = amen_solve2(M, rhs, tol, 'x0', U);

    % Extract the final solution
    ext = tt_unit(2,d0t,2*ones(d0t,1));
    u = dot(ext, U, 3*L+1, U.d);

    mass = dot(u, tt_ones(u.n))
    u = u/mass;

    u2 = tt_reshape(u, 2^L*ones(1,3));
    % It should be the delta-function
    % Find the final distribution
    [v,l]=tt_stat(u2, 'lm');
    l=l-1
    ll(t,:)=l;
end
toc;

sp = zeros(2^L, 4);
for z=1:2^L
    [v,l]=tt_stat(u2(:,:,z), 'lm');
    if (v>tol)
        sp(z, 1)=l(1)-1;
        sp(z, 2)=l(2)-1;
        sp(z, 3)=z-1;
        sp(z, 4)=log2(v);
    else
        sp(z,:)=NaN;
    end
end
z = z-1;
scatter3(sp(1:z,1), sp(1:z,2), sp(1:z,3), 100, sp(1:z,4), 'filled');
xlabel('H');
ylabel('O');
zlabel('H2O');
title('log2(P)');
colorbar;


 function [w0, phi] = adw_fan_new(sg, ig, wi)
%function [w0, phi] = adw_fan_new(sg, ig, wi)
%
% Compute the angular-dependent weighting for fan-beam geometry.
% w0(\Phi) = w(x0, y0, \Phi)
% For fully corrected penalty:
%	w(s',\beta') * J(s') | \phi'=\Phi + ...
%	w(s',\beta') * J(s') | \phi'=\Phi-pi
% See fessler chapter 3.5.3 (?).
% Useful for regularization design and variance prediction.
%
% in:
%	sg	sino_geom()
%	ig	image_geom()
%	wi	[nb,na] var(yi)
% out:
%	w0	[np,na] angular-dependent weighting, np = sum(ig.mask(:))
%
% Copyright 2005-4-07, Yingying Zhang & Jeff Fessler, The University of Michigan

if nargin == 1 && streq(sg, 'test'), adw_fan_test, return, end
if nargin < 3, ir_usage, end

[x y] = ndgrid(ig.x, ig.y); % image domain pixel locations

r0 = sqrt(x.^2 + y.^2);
phi0 = atan2(y,x); % use it for our purpose since atan will have pi flip
% [phi0 r0] = cart2pol(x,y);
% image domain dummy variables for polar coordinates

% dummy variables in freq. domain
phi = sg.ar;

% sinogram domain locations
beta = sg.ar; % angles in radians
gamma = sg.s / sg.dsd;

% loop over \phi_i
Dcf = sg.dsd + sg.dfs;
ratio_fcf = sg.dfs / Dcf;
% w0 = zeros(ig.nx, ig.ny, sg.na);
w0 = zeros([size(r0(ig.mask),1) sg.na]);

ticker reset
for ia = 1:sg.na
	ticker(mfilename, ia, sg.na)

	% -------------------------
	% for \phi' = \phi
	% -------------------------

	% compute new variables in (3.5.2)
	r_p = r0 .* cos(phi(ia) - phi0); % use beta, not outer_sum(gamma, beta)
%	since evaluate at \phi_p = \phi(view angle)
%	r_p = x .* cos(phi(ia)) + y .* sin(phi(ia));
	s_p = Dcf .* (asin(r_p / sg.dso) - asin(ratio_fcf .* r_p / sg.dso));
	beta_p = phi(ia) - asin(r_p / sg.dso);

	% compute Jacobian determinant (2.7.9)
	Jacob_p = abs(sg.dso * cos(s_p / sg.dsd) - ...
	sg.offset * sin(s_p / sg.dsd)) / sg.dsd;

	% find corresponding wi: nearest neighbor method
	s_cen = sg.w;
	% caution: this looks wrong!!! (sg.nb + 1)/2 + ob.offset_s; % for nb = even % <-----(yy) plus:(to me account for the right index) or (hugo) minus?
	s_loc = round(s_p / sg.d + s_cen);
	s_loc = min(s_loc, sg.nb);
	s_loc = max(s_loc, 1);
	beta_loc = round((mod(beta_p,2*pi)-deg2rad(sg.orbit_start)) / (2*pi/sg.na)) + 1;
	beta_loc = min(beta_loc, sg.na);
	beta_loc = max(beta_loc, 1);

	% find the corresponding index in column-stack wi
	loc = 1 + (s_loc(:)-1) + (beta_loc(:)-1)*sg.nb;

	% compute w0 at \phi' = \phi
%	w0(:,:,ia) = w0(:,:,ia) + reshape(wi(loc(:)), [ig.nx ig.ny]) .* (1 ./ Jacob_p);
	wi_temp = wi(loc(:));
	wi_p = wi_temp(ig.mask);
	Jacob = 1 ./ Jacob_p(ig.mask);
	w0(:,ia) = wi_p .* Jacob;

	clear s_loc beta_loc loc

	% -------------------------
	% for \phi' = \phi - pi
	% -------------------------
	phi_pi = phi(ia) - pi;
	r_p = r0 .* cos(phi_pi - phi0);
%	r_p = x .* cos(phi(ia)) + y .* sin(phi(ia));
	s_p = Dcf .* (asin(r_p / sg.dso) - asin(ratio_fcf .* r_p / sg.dso));
	beta_p = phi_pi - asin(r_p / sg.dso);

	% Jacob_p again: not needed since same
%	Jacob_p = abs(sg.dso*cos(s_p / sg.dsd) - ...
%	sg.offset * sin(s_p / sg.dsd)) / sg.dsd;

	% find corresponding wi: nearest neighbor method
	s_loc = round(s_p / sg.d + s_cen);
	s_loc = min(s_loc, sg.nb);
	s_loc = max(s_loc, 1);

	beta_loc = round((mod(beta_p,2*pi)-deg2rad(sg.orbit_start)) / (2*pi/sg.na)) + 1;
	beta_loc = min(beta_loc, sg.na);
	beta_loc = max(beta_loc, 1);

	% find the corresponding index in column-stack wi
	loc = 1 + (s_loc(:)-1) + (beta_loc(:)-1)*sg.nb;

	% compute w0 at \phi' = \phi - pi and sum with \phi' = \phi
%	w0(:,:,ia) = w0(:,:,ia) + reshape(wi(loc(:)), [ig.nx ig.ny]) .* (1 ./ Jacob_p);
	wi_temp = wi(loc(:));
	wi_p = wi_temp(ig.mask);
	w0(:,ia) = w0(:,ia) + wi_p .* Jacob;

	clear s_loc beta_loc loc

	% angle_dependent blur
%	b0(:,:,ia) = (sg.ds / Dcf) .* sqrt(sg.dso^2 + r0^2 + 2 .* sg.dso .* Dcf .* cos(phi(ia)-phi0));
end

function adw_fan_test
down = 8;
sg = sino_geom('ge1', 'down', down);
ig = image_geom('nx', 512, 'fov', 500, 'down', down);
wi = sg.ones;
w0 = adw_fan_new(sg, ig, wi);
w0 = ig.embed(w0);
im clf, im(w0(:,:,1:10:end)), cbar


% OZMOVIE  This program creates a movie of the Newton-Krylov solution of
% the Ornstein-Zernike equations.
%
function ozmovie
global L U rho
n=257;
epsilon=.1; sigma=2.0; rho=.2; beta=10; L=9;
dx=L/(n-1); r=0:dx:L; r=r';
%
% U is a global variable used in the function evaluation. See the text.
%
U=elj(r,sigma,epsilon,beta);
%
tol=[1.d-8,1.d-8];
x=zeros(2*n,1);
t1=cputime;
parms=[40,80,-.1];
[sol, it_hist, ierr,oz_hist] = nsoli(x,'oz',tol,parms);
%
% Build the movie of the iterations in h. Put the
% iteration counter on the screen.
%
[hr,hc]=size(oz_hist); n=hr/2;
for im=1:hc
    axis([0 9 -4 4]);
    h=oz_hist(1:n,im);
    L=9; dx=L/(n-1); r=0:dx:L; r=r';
    plot(r,h,'-'); axis([0 9 -2 3]); text(3,-1,num2str(im));
    xlabel('r'); ylabel('h');
    mov(im)=getframe;
end
%
% Showtime!
%
movie(mov,1,1);
%
%
%
function u=elj(r,sigma,epsilon,beta)
n2=length(r);
ra=r(2:n2);
r12=(sigma./ra).^12; r6=(sigma./ra).^6;
ua=exp(-4*beta*epsilon*(r12-r6));
u=[0,ua']';


function f=comp_iufilterbank_td(c,g,a,Ls,skip,ext)
%COMP_IUFILTERBANK_TD   Synthesis Uniform filterbank by conv2
%   Usage:  f=comp_iufilterbank_td(c,g,a,Ls,skip,ext);
%
%   Input parameters:
%         c    : N*M*W array of coefficients.
%         g    : Filterbank filters - filtLen*M array.
%         a    : Upsampling factor - scalar.
%         Ls   : Output length.
%         skip : Delay of the filters - scalar or array of length M.
%         ext  : Border exension technique.
%
%   Output parameters:
%         f  : Output Ls*W array.
%

%input channel number
W=size(c,3);
%filter number
M=size(g,2);
%length of filters
filtLen = size(g,1);
% Allow filter delay only in the filter support range
if(all(skip>=filtLen) || all(skip<0))
  error('%s: The filter zero index position outside of the filter support.', upper(mfilename));
end

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

% Output memory allocation
f=zeros(Ls,W,assert_classname(c,g));

if(~strcmp(ext,'per'))
    ext = 'zero';
end

skipOut = a*(filtLen-1)+skip;

% W channels are done simultaneously
for m=1:M
   cext = comp_extBoundary(squeeze(c(:,m,:)),filtLen-1,ext,'dim',1);
   ftmp = conv2(g(:,m),comp_ups(cext,a));
   f = f + ftmp(1+skipOut(m):Ls+skipOut(m),:);
end


function [m] = in2m(in)
% Convert length from inches to meters.
% Chad Greene 2012
m = in*.0254;


function value = r8_asin ( x )

%*****************************************************************************80
%
%% R8_ASIN evaluates the arc-sine of an R8 argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the arc-sine of X.
%
  persistent asincs
  persistent nterms
  persistent sqeps

  if ( isempty ( nterms ) )
    asincs = [ ...
    +0.10246391753227159336573148305785E+00, ...
    +0.54946487221245833306011195902924E-01, ...
    +0.40806303925449692851307056149246E-02, ...
    +0.40789006854604435455598823905612E-03, ...
    +0.46985367432203691616048530136218E-04, ...
    +0.58809758139708058986454385552074E-05, ...
    +0.77732312462777632750557528163795E-06, ...
    +0.10677423340082039235047504956587E-06, ...
    +0.15092399536022808262386434401064E-07, ...
    +0.21809724080055385496609614713930E-08, ...
    +0.32075984262789614433261959667376E-09, ...
    +0.47855369646781034461493133918953E-10, ...
    +0.72251287362910432263848754537112E-11, ...
    +0.11018334742255783705372701334987E-11, ...
    +0.16947632539203354877423745651078E-12, ...
    +0.26261558667348224162283241502416E-13, ...
    +0.40958299813281178408828069291110E-14, ...
    +0.64244793108803655891727944887091E-15, ...
    +0.10128142198228221693973361222041E-15, ...
    +0.16039221897380787560050597464746E-16, ...
    +0.25503501355807141715298789676373E-17, ...
    +0.40701403797862382855487165672106E-18, ...
    +0.65172671712881144437889267575466E-19, ...
    +0.10467453037096796954244891716266E-19, ...
    +0.16858725563380328094989095185066E-20, ...
    +0.27221936305040227625164341247999E-21, ...
    +0.44059293900347550617126830079999E-22, ...
    +0.71466685243375937853063168000000E-23, ...
    +0.11615793343859516051798971733333E-23, ...
    +0.18915234552354685801184187733333E-24, ...
    +0.30855772044244342399827968000000E-25, ...
    +0.50416366022162453412970495999999E-26, ...
    +0.82502725502400865081753600000000E-27, ...
    +0.13520032631020947208055466666666E-27, ...
    +0.22184326876541720216644266666666E-28, ...
    +0.36442494054085079212578133333333E-29, ...
    +0.59920218558643813307733333333333E-30, ...
    +0.98584812059573785810261333333333E-31, ...
    +0.16222501166399014393173333333333E-31 ]';
    nterms = r8_inits ( asincs, 39, 0.1 * r8_mach ( 3 ) );
    sqeps = sqrt ( 6.0 * r8_mach ( 3 ) );
  end

  y = abs ( x );

  if ( x < - 1.0 - sqeps )

    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_ASIN - Fatal error!\n' );
    fprintf ( 1, '  X < - 1.0\n' );
    error ( 'R8_ASIN - Fatal error!' )

  elseif ( x < - 1.0 )

    value = - 0.5 * pi;

  elseif ( x < 1.0 )

    z = 0.0;
    if ( sqeps < y )
      z = y * y;
    end

    if ( z <= 0.5 )
      value = x * ( 1.0 + r8_csevl ( 4.0 * z - 1.0, asincs, nterms ) );
    else
      value = 0.5 * pi - sqrt ( 1.0 - z ) * ( 1.0 + ...
        r8_csevl ( 3.0 - 4.0 * z, asincs, nterms ) );
    end

    if ( x < 0.0 )
      value = - abs ( value );
    elseif ( 0.0 < x )
      value = + abs ( value );
    end

  elseif ( x < 1.0 + sqeps )

    value = 0.5 * pi;

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_ASIN - Fatal error!\n' );
    fprintf ( 1, '  1.0 < X\n' );
    error ( 'R8_ASIN - Fatal error!' )

  end

  return
end


function out = conwaylaws(nhood)
%CONWAYLAWS Applies Conway's genetic laws to a single pixel.
%   OUT = CONWAYLAWS(NHOOD) applies Conway's genetic laws to a single
%   pixel and its 3-by-3 neighborhood, NHOOD.
%
%   Copyright 2002-2020 Gatesmark
%
%   This function, and other functions in the DIPUM Toolbox, are based
%   on the theoretical and practical foundations established in the
%   book Digital Image Processing Using MATLAB, 3rd ed., Gatesmark
%   Press, 2020.
%
%   Book website: http://www.imageprocessingplace.com
%   License: https://github.com/dipum/dipum-toolbox/blob/master/LICENSE.txt

num_neighbors = sum(nhood(:)) - nhood(2, 2);
if nhood(2, 2) == 1
   if num_neighbors <= 1
      out = 0; % Pixel dies from isolation.
   elseif num_neighbors >= 4
      out = 0; % Pixel dies from overpopulation.
   else
      out = 1; % Pixel survives.
   end
else
   if num_neighbors == 3
      out = 1; % Birth pixel.
   else
      out = 0; % Pixel remains empty.
   end
end


function hermite_polynomial_test07 ( )

%*****************************************************************************80
%
%% HERMITE_POLYNOMIAL_TEST07 tests HE_QUADRATURE_RULE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 March 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'HERMITE_POLYNOMIAL_TEST07:\n' );
  fprintf ( 1, '  HE_QUADRATURE_RULE computes the quadrature rule\n' );
  fprintf ( 1, '  associated with He(n,x);\n' );

  n = 7;
  [ x, w ] = he_quadrature_rule ( n );

  r8vec2_print ( n, x, w, '      X            W' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Use the quadrature rule to estimate:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    Q = Integral ( -oo < X < +00 ) X^E exp(-X^2) dx\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   E       Q_Estimate      Q_Exact\n' );
  fprintf ( 1, '\n' );

  for e = 0 : 2 * n - 1
    if ( e == 0 )
      f = ones ( n, 1 );
    else
      f(1:n) = x(1:n).^e;
    end
    q = w' * f;
    q_exact = he_integral ( e );
    fprintf ( 1, '  %2d  %14g  %14g\n', e, q, q_exact );
  end

  return
end


%% cmaperise
% Below is a demonstration of the features of the |cmaperise| function

%%
clear; close all; clc;

%% Syntax
% |Cmapped=cmaperise(C,cmap,clim);|

%% Description
% This function creates RGB colors for the data C using the colormap cmap
% and the limits clim.

%% Examples
%

[X,Y,Z]=peaks(50);
[F,V,C]=surf2patch(X,Y,Z,Z);

clim=[min(C(:)) max(C(:))];

%RGB color set with map 1
cmap1=viridis(25);
C_rgb_1=cmaperise(C,cmap1,clim);

%RGB color set with map 2
cmap2=gray(25);
C_rgb_2=cmaperise(C,cmap2,clim);

%RGB color set with map 3
cmap3=gjet(25);
C_rgb_3=cmaperise(C,cmap3,clim);

%% Visualize

% Offset coordinates so plots are side by side
V2=V;
V2(:,1)=V2(:,1)-min(V2(:,1))+max(V(:,1));
V3=V2;
V3(:,1)=V3(:,1)-min(V3(:,1))+max(V2(:,1));

cFigure;

subplot(1,2,1);
title('Colormapped')
hp=gpatch(F,V,C,'none');
legend(hp,'Colormapped data');
axisGeom;
camlight headlight; colormap(gca,cmap1); colorbar;

subplot(1,2,2);
title('RGB (red-green-blue) painted')
hp1=gpatch(F,V,C_rgb_1,'none');
hp2=gpatch(F,V2,C_rgb_2,'none');
hp3=gpatch(F,V3,C_rgb_3,'none');
legend([hp1 hp2 hp3],{'RGB colored with map 1','RGB colored with map 2','RGB colored with map 3'});
axisGeom;
camlight headlight;

drawnow;

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>
%%
% _*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 plot_subdivided(V,F,u,k)
%PLOT_SUBDIVIDED Plot the function u on the mesh V,F which has subdivided,
%with Loop subdivision, k times
%
% plot_subdivided(V,F,u,k);
%
% Inputs:
%  V,F  the input mesh to be subdivided
%  u  the input function to be subdivided along with the mesh
%  k  how many times to subdivide
%

[Vu,Fu,S] = loop(V,F,k);
uu = S*u;

t = tsurf(Fu,Vu, 'CData',uu);
shading interp;
axis equal;
axis off;
colormap(cbrewer('Blues', 500));
light('Position',[-1.5 1 1],'Style','local');
lights = camlight;
set(t, 'FaceLighting','gouraud', 'FaceColor','interp');
set(t, 'DiffuseStrength',0.5, 'SpecularStrength',0.2, 'AmbientStrength',0.3);
camproj('perspective');
add_shadow([t],lights);

end


function M = fixedrankembeddedfactory(m, n, k)
% Manifold struct to optimize fixed-rank matrices w/ an embedded geometry.
%
% function M = fixedrankembeddedfactory(m, n, k)
%
% Manifold of m-by-n real matrices of fixed rank k. This follows the
% embedded geometry described in Bart Vandereycken's 2013 paper:
% "Low-rank matrix completion by Riemannian optimization".
%
% Paper link: http://arxiv.org/pdf/1209.3834.pdf
%
% A point X on the manifold is represented as a structure with three
% fields: U, S and V. The matrices U (mxk) and V (nxk) are orthonormal,
% while the matrix S (kxk) is any /diagonal/, full rank matrix.
% Following the SVD formalism, X = U*S*V'. Note that the diagonal entries
% of S are not constrained to be nonnegative.
%
% Tangent vectors are represented as a structure with three fields: Up, M
% and Vp. The matrices Up (mxk) and Vp (mxk) obey Up'*U = 0 and Vp'*V = 0.
% The matrix M (kxk) is arbitrary. Such a structure corresponds to the
% following tangent vector in the ambient space of mxn matrices:
%   Z = U*M*V' + Up*V' + U*Vp'
% where (U, S, V) is the current point and (Up, M, Vp) is the tangent
% vector at that point.
%
% Vectors in the ambient space are best represented as mxn matrices. If
% these are low-rank, they may also be represented as structures with
% U, S, V fields, such that Z = U*S*V'. Their are no resitrictions on what
% U, S and V are, as long as their product as indicated yields a real, mxn
% matrix.
%
% The chosen geometry yields a Riemannian submanifold of the embedding
% space R^(mxn) equipped with the usual trace (Frobenius) inner product.
%
%
% Please cite the Manopt paper as well as the research paper:
%     @Article{vandereycken2013lowrank,
%       Title   = {Low-rank matrix completion by {Riemannian} optimization},
%       Author  = {Vandereycken, B.},
%       Journal = {SIAM Journal on Optimization},
%       Year    = {2013},
%       Number  = {2},
%       Pages   = {1214--1236},
%       Volume  = {23},
%       Doi     = {10.1137/110845768}
%     }
%
% See also: fixedrankfactory_2factors fixedrankfactory_3factors

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Dec. 30, 2012.
% Contributors:
% Change log:
%
%	Feb. 20, 2014 (NB):
%       Added function tangent to work with checkgradient.
%
%   June 24, 2014 (NB):
%       A couple modifications following
%       Bart Vandereycken's feedback:
%       - The checksum (hash) was replaced for a faster alternative: it's a
%         bit less "safe" in that collisions could arise with higher
%         probability, but they're still very unlikely.
%       - The vector transport was changed.
%       The typical distance was also modified, hopefully giving the
%       trustregions method a better initial guess for the trust region
%       radius, but that should be tested for different cost functions too.
%
%    July 11, 2014 (NB):
%       Added ehess2rhess and tangent2ambient, supplied by Bart.
%
%    July 14, 2014 (NB):
%       Added vec, mat and vecmatareisometries so that hessianspectrum now
%       works with this geometry. Implemented the tangent function.
%       Made it clearer in the code and in the documentation in what format
%       ambient vectors may be supplied, and generalized some functions so
%       that they should now work with both accepted formats.
%       It is now clearly stated that for a point X represented as a
%       triplet (U, S, V), the matrix S needs to be diagonal.

    M.name = @() sprintf('Manifold of %dx%d matrices of rank %d', m, n, k);

    M.dim = @() (m+n-k)*k;

    M.inner = @(x, d1, d2) d1.M(:).'*d2.M(:) + d1.Up(:).'*d2.Up(:) ...
                                             + d1.Vp(:).'*d2.Vp(:);

    M.norm = @(x, d) sqrt(M.inner(x, d, d));

    M.dist = @(x, y) error('fixedrankembeddedfactory.dist not implemented yet.');

    M.typicaldist = @() M.dim();

    % Given Z in tangent vector format, projects the components Up and Vp
    % such that they satisfy the tangent space constraints up to numerical
    % errors. If Z was indeed a tangent vector at X, this should barely
    % affect Z (it would not at all if we had infinite numerical accuracy).
    M.tangent = @tangent;
    function Z = tangent(X, Z)
        Z.Up = Z.Up - X.U*(X.U'*Z.Up);
        Z.Vp = Z.Vp - X.V*(X.V'*Z.Vp);
    end

    % For a given ambient vector Z, applies it to a matrix W. If Z is given
    % as a matrix, this is straightfoward. If Z is given as a structure
    % with fields U, S, V such that Z = U*S*V', the product is executed
    % efficiently.
    function ZW = apply_ambient(Z, W)
        if ~isstruct(Z)
            ZW = Z*W;
        else
            ZW = Z.U*(Z.S*(Z.V'*W));
        end
    end

    % Same as apply_ambient, but applies Z' to W.
    function ZtW = apply_ambient_transpose(Z, W)
        if ~isstruct(Z)
            ZtW = Z'*W;
        else
            ZtW = Z.V*(Z.S'*(Z.U'*W));
        end
    end

    % Orthogonal projection of an ambient vector Z represented as an mxn
    % matrix or as a structure with fields U, S, V to the tangent space at
    % X, in a tangent vector structure format.
    M.proj = @projection;
    function Zproj = projection(X, Z)

        ZV = apply_ambient(Z, X.V);
        UtZV = X.U'*ZV;
        ZtU = apply_ambient_transpose(Z, X.U);

        Zproj.M = UtZV;
        Zproj.Up = ZV  - X.U*UtZV;
        Zproj.Vp = ZtU - X.V*UtZV';

    end

    M.egrad2rgrad = @projection;

    % Code supplied by Bart.
    % Given the Euclidean gradient at X and the Euclidean Hessian at X
    % along H, where egrad and ehess are vectors in the ambient space and H
    % is a tangent vector at X, returns the Riemannian Hessian at X along
    % H, which is a tangent vector.
    M.ehess2rhess = @ehess2rhess;
    function rhess = ehess2rhess(X, egrad, ehess, H)

        % Euclidean part
        rhess = projection(X, ehess);

        % Curvature part
        T = apply_ambient(egrad, H.Vp)/X.S;
        rhess.Up = rhess.Up + (T - X.U*(X.U'*T));
        T = apply_ambient_transpose(egrad, H.Up)/X.S;
        rhess.Vp = rhess.Vp + (T - X.V*(X.V'*T));

    end

    % Transforms a tangent vector Z represented as a structure (Up, M, Vp)
    % into a structure with fields (U, S, V) that represents that same
    % tangent vector in the ambient space of mxn matrices, as U*S*V'.
    % This matrix is equal to X.U*Z.M*X.V' + Z.Up*X.V' + X.U*Z.Vp'. The
    % latter is an mxn matrix, which could be too large to build
    % explicitly, and this is why we return a low-rank representation
    % instead. Note that there are no guarantees on U, S and V other than
    % that USV' is the desired matrix. In particular, U and V are not (in
    % general) orthonormal and S is not (in general) diagonal.
    % (In this implementation, S is identity, but this might change.)
    M.tangent2ambient = @tangent2ambient;
    function Zambient = tangent2ambient(X, Z)
        Zambient.U = [X.U*Z.M + Z.Up, X.U];
        Zambient.S = eye(2*k);
        Zambient.V = [X.V, Z.Vp];
    end

    % This retraction is second order, following general results from
    % Absil, Malick, "Projection-like retractions on matrix manifolds",
    % SIAM J. Optim., 22 (2012), pp. 135-158.
    M.retr = @retraction;
    function Y = retraction(X, Z, t)
        if nargin < 3
            t = 1.0;
        end

        % See personal notes June 28, 2012 (NB)
        [Qu, Ru] = qr(Z.Up, 0);
        [Qv, Rv] = qr(Z.Vp, 0);

        % Calling svds or svd should yield the same result, but BV
        % advocated svd is more robust, and it doesn't change the
        % asymptotic complexity to call svd then trim rather than call
        % svds. Also, apparently Matlab calls ARPACK in a suboptimal way
        % for svds in this scenario.
        % [Ut St Vt] = svds([X.S+t*Z.M , t*Rv' ; t*Ru , zeros(k)], k);
        [Ut, St, Vt] = svd([X.S+t*Z.M , t*Rv' ; t*Ru , zeros(k)]);

        Y.U = [X.U Qu]*Ut(:, 1:k);
        Y.V = [X.V Qv]*Vt(:, 1:k);
        Y.S = St(1:k, 1:k) + eps*eye(k);

        % equivalent but very slow code
        % [U S V] = svds(X.U*X.S*X.V' + t*(X.U*Z.M*X.V' + Z.Up*X.V' + X.U*Z.Vp'), k);
        % Y.U = U; Y.V = V; Y.S = S;

    end

    M.exp = @exponential;
    function Y = exponential(X, Z, t)
        if nargin < 3
            t = 1.0;
        end
        Y = retraction(X, Z, t);
        warning('manopt:fixedrankembeddedfactory:exp', ...
               ['Exponential for fixed rank ' ...
                'manifold not implemented yet. Used retraction instead.']);
    end

    % Less safe but much faster checksum, June 24, 2014.
    % Older version right below.
    M.hash = @(X) ['z' hashmd5([sum(X.U(:)) ; sum(X.S(:)); sum(X.V(:)) ])];
    %M.hash = @(X) ['z' hashmd5([X.U(:) ; X.S(:) ; X.V(:)])];

    M.rand = @random;
    % Factors U and V live on Stiefel manifolds, hence we will reuse
    % their random generator.
    stiefelm = stiefelfactory(m, k);
    stiefeln = stiefelfactory(n, k);
    function X = random()
        X.U = stiefelm.rand();
        X.V = stiefeln.rand();
        X.S = diag(sort(rand(k, 1), 1, 'descend'));
    end

    % Generate a random tangent vector at X.
    % TODO: consider a possible imbalance between the three components Up,
    % Vp and M, when m, n and k are widely different (which is typical).
    M.randvec = @randomvec;
    function Z = randomvec(X)
        Z.Up = randn(m, k);
        Z.Vp = randn(n, k);
        Z.M  = randn(k);
        Z = tangent(X, Z);
        nrm = M.norm(X, Z);
        Z.Up = Z.Up / nrm;
        Z.Vp = Z.Vp / nrm;
        Z.M  = Z.M  / nrm;
    end

    M.lincomb = @lincomb;

    M.zerovec = @(X) struct('Up', zeros(m, k), 'M', zeros(k, k), ...
                                                        'Vp', zeros(n, k));

    % New vector transport on June 24, 2014 (as indicated by Bart)
    % Reference: Absil, Mahony, Sepulchre 2008 section 8.1.3:
    % For Riemannian submanifolds of a Euclidean space, it is acceptable to
    % transport simply by orthogonal projection of the tangent vector
    % translated in the ambient space.
    M.transp = @project_tangent;
    function Z2 = project_tangent(X1, X2, Z1)
        Z2 = projection(X2, tangent2ambient(X1, Z1));
    end

    M.vec = @vec;
    function Zvec = vec(X, Z)
        Zamb = tangent2ambient(X, Z);
        Zamb_mat = Zamb.U*Zamb.S*Zamb.V';
        Zvec = Zamb_mat(:);
    end
    M.mat = @(X, Zvec) projection(X, reshape(Zvec, [m, n]));
    M.vecmatareisometries = @() true;

end

% Linear combination of tangent vectors
function d = lincomb(x, a1, d1, a2, d2) %#ok<INUSL>

    if nargin == 3
        d.Up = a1*d1.Up;
        d.Vp = a1*d1.Vp;
        d.M  = a1*d1.M;
    elseif nargin == 5
        d.Up = a1*d1.Up + a2*d2.Up;
        d.Vp = a1*d1.Vp + a2*d2.Vp;
        d.M  = a1*d1.M  + a2*d2.M;
    else
        error('fixedrank.lincomb takes either 3 or 5 inputs.');
    end

end


function node_display ( v, bi, p )

%% NODE_DISPLAY displays nodes within a boundary.
%
%  Parameters:
%
%    Input, real V(V_NUM,2), the coordinates of vertices used to define the curves.
%
%    Input, integer BI(BI_NUM), a sequence of indices into V.  Each closed curve
%    is defined by giving a sequence of indices, which is terminated by
%    repeating the starting index.  Thus, BI = { 3, 1, 5, 3, 4, 2, 9, 7, 4 }
%    describes two curves: ( 3, 1, 5, 3 ) and (4, 2, 9, 7, 4 ).
%
%    Input,  real P(P_NUM,2), the coordinates of nodes.
%
  bi_num = length ( bi );

  clf
  hold on;
  next = 1;
  s = bi(1);
  t2 = s;
  draw = 1;

  while ( next < bi_num )
    t1 = t2;
    next = next + 1;
    t2 = bi(next);
    if ( draw )
      line ( [ v(t1,1), v(t2,1) ], [ v(t1,2), v(t2,2) ], 'LineWidth', 2, 'Color', 'k' );
      if ( t2 == s )
        draw = 0;
      end
    else
      s = t2;
      draw = 1;
    end

  end

  plot ( p(:,1), p(:,2), 'b.', 'MarkerSize', 15 );

  plot ( v(:,1), v(:,2), 'r.', 'MarkerSize', 15 );

  axis equal
  grid on
  hold off

  return
end


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% function [y,dy] = affine2Dsparse(w,x,varargin)
%
% computes y = kron(I2,Q)*w = [w(1),w(2);w(4),w(5)] * x + w([3,6])
% and the derivative wrt. w.
% Q = {[x(:,1),x(:,2),1]}, dy = Q;
% if no argumanets are given, the parameters for the identity map are returned.
%
% see also transformations/contents.m, trafo.m
%==============================================================================

function [y,dy] = affine2Dsparse(w,x,varargin)

% the persitent variable stores the matrix
% Q(x) = kron( I_2 , [x(:,1),x(:,2),1] );
persistent Q

if nargin == 0,
  runMinimalExample;
  return;
else
  y = mfilename('fullfile');
  dy = [1;0;0;0;1;0];         % parameterization of identity
  if ischar(w),  Q  = []; w = []; end; % reset Q
  if isempty(w), return;          end;
end;

% test for need of updating Q
OK = ~any(isempty(Q));
if OK,
  m1 = size(Q{1});
  OK = (2*m1(1) == size(x,1)) && (2*m1(2) == numel(w));
end;

if ~OK,
  n = length(x)/2;
  Q = {[reshape(x,[],2),ones(n,1)]};
  if nargout == 0, return; end;
end;

w  = reshape(w,3,2);
% mimicing Qfull*w as [Q*w(:,1),Q*w(:,2)]
y  = [Q{1}*w(:,1);Q{1}*w(:,2)];
dy = Q;

%------------------------------------------------------------------------------
function runMinimalExample
help(mfilename);
fprintf('%s: minimal example\n',mfilename)

omega = [0,10,0,2]; m = [8,9];
w = 22/pi;c = (omega(2:2:end)-omega(1:2:end))'/2;
R = [ cos(w),-sin(w);sin(w),cos(w)];
g = (eye(2)-R)*reshape(c,2,1);
w = [R(1,1);R(1,2);g(1);R(2,1);R(2,2);g(2)]
x = getNodalGrid(omega,m);
z = feval(mfilename,w,x);
t = affine2D(w,x);
n = norm(z-t);
fprintf('diff between affine2D and affine2Dsparse is %s\n',num2str(n));

assert(n<1e-13,'difference to non-sparse version too big')
FAIRfigure(1); clf;
plotGrid(x,omega,m,'color','r'); axis image; hold on;
plotGrid(z,omega,m,'color','b'); axis image; hold off;
%==============================================================================


function[invmat]=matinv(mat,str)
%MATINV  Fast inversion of arrays of small matrices.
%
%   MATINV is a low-level function called by POLYSMOOTH.
%
%   Let MAT be an array of K different M x M matrices A1,A2,...,AK.
%   INV=MATINV(MAT) then returns an array of N inverse matrices.
%
%   If MAT has dimensions K1 x K2 x .... M x M, then MATINV returns an
%   array of the same size containing the inverses of the M x M matrices.
%
%   For example, MAT could be 10 x 10 x 4 x 4, in which case the inverses
%   of one hundred 4 x 4 matrices are found.
%
%   MAT can have any dimensionality so long as the matrices to be inverted
%   occupy the last two dimensions.  The last dimension is interpreted as
%   "columns" and the second to last as "rows."
%
%   Note that MATINV only works matrices with M=2 through M=8.
%   ____________________________________________________________
%
%   Algorithms
%
%   MATINV can use either of two different algorithms.  This is specified
%   with INV=MATINV(MAT,STR).
%
%   MATINV(MAT,'direct') uses algebraic expressions for 2 x 2 and 3 x 3
%   matrix inverses together with Boltz's block diagonal recursion formula.
%   For details, see the following links
%
%        http://mathworld.wolfram.com/MatrixInverse.html
%        http://en.wikipedia.org/wiki/Invertible_matrix.
%
%   MATINV(MAT,'loop') uses Matlab's INV function together with a
%   straightforward loop.
%
%   The direct algorithm, which is the default, can be much faster when
%   MAT is large and the dimension to be inverted is small.
%   ____________________________________________________________
%
%   See also MATMULT.
%
%   'matinv --t' runs some tests.
%
%   Usage: inv=matinv(mat);
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2008--2020 J.M. Lilly --- type 'help jlab_license' for details

if strcmpi(mat, '--t')
    matinv_test,return
end

ndims=lnsd(mat);
K=size(mat,ndims);

if K~=size(mat,ndims-1)
    error('The last two dimensions of MAT must be the same for it to be invertible.')
end

if nargin==1
    str='direct';
end

if K>12
    error('MATINV number of dimensions of matrix should be no more than M=12.')
end

sizemat=size(mat);
if length(sizemat)>2
    %[mat,index]=matinv_strip(mat);

    if ~isempty(strfind(str,'dir'))
        invmat=matinv_direct(mat,ndims,K);
    elseif ~isempty(strfind(str,'loo'))
        invmat=matinv_loop(mat);
    end

else
    invmat=inv(mat);
end

function[mat,index]=matinv_strip(mat)

sizemat=size(mat);
mat=reshape(mat,prod(sizemat(1:end-2)),sizemat(end-1),sizemat(end));
bool=~isnan(sum(sum(mat,3),2));
index=find(bool);
mat=mat(index,:,:);

function[invmat]=matinv_loop(mat)

sizemat=size(mat);
mat=reshape(mat,prod(sizemat(1:end-2)),sizemat(end-1),sizemat(end));
invmat=0*mat;

%[lastmsg,lastid]=lastwarn;
warning('off','MATLAB:illConditionedMatrix');
warning('off','MATLAB:nearlySingularMatrix');
%warning('off','MATLAB:singularMatrix');
for i=1:size(mat,1)
     invmat(i,:,:)=inv(squeeze(mat(i,:,:)));
end

invmat=reshape(invmat,sizemat);
warning('on','MATLAB:illConditionedMatrix');
warning('on','MATLAB:nearlySingularMatrix');
warning('on','MATLAB:singularMatrix');

function[invmat]=matinv_direct(mat,ndims,K)

%if K==1
%    invmat=1./mat;
if K==2
    invmat=matinv_twoxtwo(mat,ndims);
elseif K==3
    invmat=matinv_threexthree(mat,ndims);
else
    invmat=matinv_block(mat,ndims,K);
end

function[invmat]=matinv_twoxtwo(mat,ndims)
ac=vindex(mat,1,ndims);
bd=vindex(mat,2,ndims);

a=vindex(ac,1,ndims-1);
c=vindex(ac,2,ndims-1);
b=vindex(bd,1,ndims-1);
d=vindex(bd,2,ndims-1);

deta=a.*d-b.*c;
a=a./deta;
b=b./deta;
c=c./deta;
d=d./deta;

ac=0*ac;
bd=0*bd;

ac=vindexinto(ac,d,1,ndims-1);
ac=vindexinto(ac,-c,2,ndims-1);

bd=vindexinto(bd,-b,1,ndims-1);
bd=vindexinto(bd,a,2,ndims-1);

invmat=zeros(size(mat));
invmat=vindexinto(invmat,ac,1,ndims);
invmat=vindexinto(invmat,bd,2,ndims);

function[mat]=matinv_threexthree(mat,ndims)

c1=vindex(mat,1,ndims);
c2=vindex(mat,2,ndims);
c3=vindex(mat,3,ndims);

a11=vindex(c1,1,ndims-1);
a21=vindex(c1,2,ndims-1);
a31=vindex(c1,3,ndims-1);

a12=vindex(c2,1,ndims-1);
a22=vindex(c2,2,ndims-1);
a32=vindex(c2,3,ndims-1);

a13=vindex(c3,1,ndims-1);
a23=vindex(c3,2,ndims-1);
a33=vindex(c3,3,ndims-1);

b11=a22.*a33-a23.*a32;
b21=a23.*a31-a21.*a33;
b31=a21.*a32-a22.*a31;

b12=a13.*a32-a12.*a33;
b22=a11.*a33-a13.*a31;
b32=a12.*a31-a11.*a32;

b13=a12.*a23-a13.*a22;
b23=a13.*a21-a11.*a23;
b33=a11.*a22-a12.*a21;

deta=a11.*b11+a12.*b21+a13.*b31;
%This looks funny because b21 is minus

b11=b11./deta;
b21=b21./deta;
b31=b31./deta;

b12=b12./deta;
b22=b22./deta;
b32=b32./deta;

b13=b13./deta;
b23=b23./deta;
b33=b33./deta;

clear deta

c1=vindexinto(c1,b11,1,ndims-1);
c1=vindexinto(c1,b21,2,ndims-1);
c1=vindexinto(c1,b31,3,ndims-1);

c2=vindexinto(c2,b12,1,ndims-1);
c2=vindexinto(c2,b22,2,ndims-1);
c2=vindexinto(c2,b32,3,ndims-1);

c3=vindexinto(c3,b13,1,ndims-1);
c3=vindexinto(c3,b23,2,ndims-1);
c3=vindexinto(c3,b33,3,ndims-1);

mat=vindexinto(mat,c1,1,ndims);
mat=vindexinto(mat,c2,2,ndims);
mat=vindexinto(mat,c3,3,ndims);

function[invmat]=matinv_block(mat,ndims,K)

%This keeps the two matrices about the same size, which seems the
%fastest option in tests

i1=1:floor(K/2);
i2=floor(K/2)+1:K;

%i1,i2

ac=vindex(mat,i1,ndims);
bd=vindex(mat,i2,ndims);

a=vindex(ac,i1,ndims-1);
c=vindex(ac,i2,ndims-1);
b=vindex(bd,i1,ndims-1);
d=vindex(bd,i2,ndims-1);

%vsize(a,b,c,d)

%Recursion
ainv=matinv(a);

dcab=matinv(d-matmult(matmult(c,ainv,ndims-1),b,ndims-1));

bnew=-matmult(matmult(ainv,b,ndims-1),dcab,ndims-1);
anew=ainv+matmult(matmult(-bnew,c,ndims-1),ainv,ndims-1);
cnew=-matmult(matmult(dcab,c,ndims-1),ainv,ndims-1);
dnew=dcab;

ac=vindexinto(ac,anew,i1,ndims-1);
ac=vindexinto(ac,cnew,i2,ndims-1);

bd=vindexinto(bd,bnew,i1,ndims-1);
bd=vindexinto(bd,dnew,i2,ndims-1);

invmat=zeros(size(mat));
invmat=vindexinto(invmat,ac,i1,ndims);
invmat=vindexinto(invmat,bd,i2,ndims);

function[]=matinv_test
disp('Testing that direct and looping algorithms match for non-singular matrices.')

rng(0);

for n=2:12

    mat=randn(n);

    invmat=inv(mat);
    invmat2=matinv(mat);
    disp(['MATINV testing case of ' int2str(n) 'x' int2str(n) ' matrices.'])
    reporttest('MATINV with single matrix',aresame(invmat,invmat2,1e-3))
    mat=randn(10000,n,n);
    tic
    invmat=matinv(mat,'loop');
    t1=toc;

    tic
    invmat2=matinv(mat,'direct');
    t2=toc;

    tol=1e-1;
    bool=false(size(mat,1),1);
    for i=1:size(mat)
        bool(i)=rcond(squeeze(mat(i,:,:)))>=tol;
    end

    vindex(invmat,invmat2,find(bool),1);

    reporttest('MATINV with 10000 random matrices',aresame(invmat,invmat2,1e-2))
    disp(['MATINV was ' num2str(t1./t2) ' times faster than INV.'])
end


function J=calcCartRRJacob(components,xState,useHalfRange,lTx,lRx)
%%CALCCARTRRJACOB Compute the Jacobian matrix for a Cartesian measurement
%           possibly with a bistatic range rate component in 1D, 2D or 3D
%           for a Cartesian target state. This function can be useful when
%           using an extended Kalman filter (EKF) with Cartesian-converted
%           measurements and range rate. Without the range rate component,
%           the result is just the measurement matrix, which, when
%           multiplied by the target state, extracts the position
%           components and is often designated by H in the discrete-time
%           Kalman filter.
%
%INPUTS: components This specified whether the output should contain a
%               range-rate component. Possible values are:
%               0 The output contains position and range rate rows. This is
%                 the default if an empty matrix is passed.
%               1 The output just has position rows.
%        xState The xDimX1 target state vector in the global coordinate
%               system with [x;y;z;xDot;yDot;zDot] components in 3D or
%               [x;y;xDot;yDot] components in 2D.
%  useHalfRange A boolean value specifying whether the bistatic range
%               (and thus the range rate) value has been divided by two.
%               This normally comes up when operating in monostatic mode,
%           lTx The transmitter state vector in the global coordinate
%               system with [x;y;z;xDot;yDot;zDot] components in 3D or
%               [x;y;xDot;yDot] components in 2D. If omitted, then a vector
%               of zeros is used.
%           lRx The receiver state vector in the global coordinate system
%               with [x;y;z;xDot;yDot;zDot] components in 3D or
%               [x;y;xDot;yDot] components in 2D. If omitted, then a vector
%               of zeros is used.
%
%OUTPUTS: J The (xDim/2+1)XxDim Jacobian matrix. Each row is a component
%           of x-y-z-range rate and each column is the derivative with
%           respect to x,y,z,xDot,yDot,zDot. If range rate is not desired,
%           then the matrix is (xDim/2)XxDim in size.
%
%The derivatives of the position components with themselves are clear (1
%for common components and zero for different components). The derivatives
%of the range rate components are obtained using the rangeRateGradient
%function.
%
%February 2017 David F.Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

xDim=size(xState,1);
posDim=xDim/2;
measDim=posDim+1;

if(isempty(components))
    components=0;
end

if(components==0)
    if(nargin<3||isempty(useHalfRange))
        useHalfRange=false;
    end

    if(nargin<4||isempty(lTx))
        lTx=zeros(xDim,1);
    end

    if(nargin<5||isempty(lRx))
        lRx=zeros(xDim,1);
    end

    J=zeros(measDim,xDim);
    %Position components are just passed through.
    J(1:posDim,1:posDim)=eye(posDim,posDim);

    J(posDim+1,:)=rangeRateGradient(xState,useHalfRange,lTx,lRx);
else
    J=zeros(posDim,xDim);
    J(1:posDim,1:posDim)=eye(posDim,posDim);
end

end

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


%R2T Convert rotation matrix to a homogeneous transform
%
% T = R2T(R) is a homogeneous transform equivalent to an orthonormal
% rotation matrix R with a zero translational component.
%
% Notes::
% - Works for T in either SE(2) or SE(3)
%  - if R is 2x2 then T is 3x3, or
%  - if R is 3x3 then T is 4x4.
% - Translational component is zero.
% - For a rotation matrix sequence returns a homogeneous transform
%   sequence.
%
% See also T2R.

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

function T = r2t(R)

    % check dimensions: R is SO(2) or SO(3)
    d = size(R);
    if d(1) ~= d(2)
        error('matrix must be square');
    end
    if ~any(d(1) == [2 3])
        error('argument is not a rotation matrix (sequence)');
    end

    Z = zeros(d(1),1);
    B = [Z' 1];

    if numel(d) == 2
        % single matrix case
        T = [R Z; B];
    else
        %  matrix sequence case
        T = zeros(4,4,d(3));
        for i=1:d(3)
            T(:,:,i) = [R(:,:,i) Z; B];
        end
    end


% DEMSWISSROLL1 Model the face swiss roll with a 2-D GPLVM.

% FGPLVM

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

dataSetName = 'swissRollFull';
experimentNo = 2;

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

% Set up model
options = fgplvmOptions('dtcvar');
%options.optimiser = 'conjgrad';

latentDim = 2;
d = size(Y, 2);
options.initX = 'isomap';
model = fgplvmCreate(latentDim, d, Y, options);

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

model = fgplvmOptimise(model, display, iters);

% Save the results.
modelWriteResult(model, dataSetName, experimentNo);

if exist('printDiagram') & printDiagram
  lvmPrintPlot(model, lbls, dataSetName, experimentNo);
end

% Display results
fgplvmScatterPlotColor(model, model.y(:, 3));


function determ = line_adj_determinant ( n )

%*****************************************************************************80
%
%% LINE_ADJ_DETERM returns the determinant of the LINE_ADJ matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real DETERM, the determinant.
%
       if ( mod ( n, 4 ) == 1 )
    determ =   0.0;
  elseif ( mod ( n, 4 ) == 2 )
    determ = - 1.0;
  elseif ( mod ( n, 4 ) == 3 )
    determ =   0.0;
  elseif ( mod ( n, 4 ) == 0 )
    determ = + 1.0;
  end

  return
end


function xEst=DopplerOnlyInit6D(rDot,lRx,algorithm,opts,AbsTol,scratchFolderPath,execPath)
%%DOPPLERONLYINIT6D Determine the 3D position and velocity of a target
%          using six simultaneous Doppler measurements. The measurements
%          are assumed to be one-way (from a moving emitter). The receivers
%          are stationary. This function makes use of the function
%          solvePolySysWithExtProg to call an external program to solve the
%          simultaneous multivariate polynomials that define the solution.
%
%INPUTS: rDot A 6X1 or 1X6 vector of Doppler measurements of the emitter.
%     lRx A 3X6 set of the six Cartesian locations of the receivers
%         taking the Doppler measurements in rDot.
% algorithm An optional parameter specifying which algorithm to use. The
%         solvers for simultaneous multivariate polynomials are external
%         programs called via solvePolySysWithExtProg. Possible values
%         are:
%         0 (The default if omitted or an empty matrix is passed) Use
%           Bertini.
%         1 Use PHCpack.
%         2 Use the certified homotopy algorithm that is built into
%           Macaulay2 (in NAG4M2). This only uses the normalized solver
%           with the default options.
%    opts An optional input specifying options for the solver in the
%         function solvePolySysWithExtProg. This is described in more
%         detail in solvePolySysWithExtProg. Omitting this parameter or
%         passing am empty matrices uses the default values, except if
%         Bertini is used, then SecurityMaxNorm, EndpointFiniteThreshold
%         and PathTruncationThreshold are set to 1e9 by default.
%  AbsTol An absolute tolerance on the imaginary part of a solution to the
%         multivariate polynomials used here real. The default if this
%         parameter is omitted or an empty matrix is passed is 1e-8.
% scratchFolderPath An optional parameter specifying the folder in which
%         temporary files will be written. This is needed, because all
%         solvers only works through files. If this parameter is omitted or
%         an empty matrix is passed, then a folder named temp in the folder
%         enclosing this function is used. Note that if an error occurs
%         while executing this function, then temporary files might be left
%         in the temp folder after this function ends.
%  execPath The command line command to use to execute the solver. The
%         default if this parameter is omitted or an empty matrix is
%         passed is just the standard name on the command line: bertini,
%         phc and M2 for each of the algorithms. The default assumes that
%         the program is already in the default search path. For example,
%         on *NIX systems, one can usually put the executable in
%         /usr/local/bin for it to be in the default search path.
%         However, for some reason, that directory is often not
%         included in Matlab's search path. Matlab 2016a's help under
%         "Run External Commands, Scripts, and Programs" specifically
%         says how to add that folder to the default search path.
%
%OUTPUTS: xEst A 6XnumEst set of solutions.
%
%The algorithm solves a system of simultaneous multivariate polynomials as
%formulated in [1] (Equation numbers in the code refer to [1]). However,
%there is a typo in [1], which is corrected in the same derivation in [2].
%
%EXAMPLE:
% lRx=[1000,  500, 1100, 2500,     0, 1000;
%      3000, 2500, 2500,    0,     0, 1000;
%         0,    0,    0,  400,  8000, 8000];%(Stationary) sensor locations
% xTrue=[1e3;5e3;4e3;0;300;0];
% rDot=zeros(6,1);
% for curMeas=1:6
%     diff=xTrue(1:3)-lRx(:,curMeas);
%     rDot(curMeas)=(diff'/norm(diff))*xTrue(4:6);
% end
% DopplerOnlyInit6D(rDot,lRx,0)
%
%REFERENCES:
%[1] T.-L. Lee, S.-S. Lin, W.-W. Lin, S.-T. Yau, and J. Zhu, "Polynomial
%    calculations in Doppler tracking," Communications in Information and
%    Systems, vol. 12, no. 2, pp. 157-184, 2012.
%[2] Y.-C. Kuo, W.-W. Lin, and S.-T. Yau, "A novel efficient homotopy
%    continuation method in tracking," Communications in Information and
%    Systems, vol. 14, no. 1, pp. 57-78, 2014.
%
%March 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%We subtract out the locations of the sensors and then put them back in at
%the end. This should help with numerical sensitivity issues when
%localizing things on the ground but using ECEF coordinates. The offset is
%removed in the end.
centerOfRegion=mean(lRx,2);
lRx=bsxfun(@minus,lRx,centerOfRegion);

%Now, we scale the distances so that the farthest one is 1. This should
%help deal with convergence problems when the scale of the probolem
%changes. The scaling will be reversed after the estimation.
scalFactor=sqrt(max(sum(lRx.*lRx,1)));
lRx=lRx/scalFactor;
rDot=rDot/scalFactor;

if(nargin<3||isempty(algorithm))
    algorithm=0;
end

if(nargin<4)
    opts=[];
end

if(nargin<5||isempty(AbsTol))
    AbsTol=1e-8;
end

if(nargin<5)
    scratchFolderPath=[];
end

if(nargin<6)
    execPath=[];
end

if(algorithm==0&&isempty(opts))%If using Bertini, set default tolerances
    %Default options when using Bertini
    opts=struct('SecurityMaxNorm',1e9,'EndpointFiniteThreshold',1e9,'PathTruncationThreshold',1e9);
elseif(algorithm==0)
    if(~isfield(opts,'SecurityMaxNorm'))
       opts.SecurityMaxNorm=1e9;
    end

    if(~isfield(opts,'EndpointFiniteThreshold'))
        opts.EndpointFiniteThreshold=1e9;
    end

    if(~isfield(opts,'PathTruncationThreshold'))
        opts.PathTruncationThreshold=1e9;
    end
end

%The following are from Equation 4.
V0=lRx';
n=sum(V0.*V0,2);

u1=lRx(:,1);
u2=lRx(:,2);
u3=lRx(:,3);
u4=lRx(:,4);
u5=lRx(:,5);
u6=lRx(:,6);

%From Equation 5
RDot=diag(rDot);

%From Equation 6
C=[-1, 1, 0, 0, 0, 0;
   -1, 0, 1, 0, 0, 0;
   -1, 0, 0, 1, 0, 0;
   -1, 0, 0, 0, 1, 0;
   -1, 0, 0, 0, 0, 1];

%From Equation 10
A=(V0'*(C'*C)*V0)\(V0'*(C'*C));

%From Equation 15
CHat=[0, -1, 1, 0, 0, 0;
      0, -1, 0, 1, 0, 0;
      0, -1, 0, 0, 1, 0;
      0, -1, 0, 0, 0, 1];

%From Equation 16 --Note that the RDot matrix has been removed. This is
%because including RDot makes the solution inconsistent with Equation 22.
%However, the RDot term needs to be included in the QR decomposition.
AHat=([(u2-u3)';
      (u2-u4)';
      (u2-u5)';
      (u2-u6)']*A+CHat);

%Equation 18
u1Hat=(1/2)*A*n-u1;

%Get the T values used in Equation 17 (needs the RDot term with AHat.
[~,R]=qr(AHat*RDot);
T1=R(1:2,1:2);
T2=R(1:2,3:end);

%This is from Equation 17
r12Transform=-T1\T2;
%Extract the elements of the transformation
at11=r12Transform(1,1);
at12=r12Transform(1,2);
at13=r12Transform(1,3);
at14=r12Transform(1,4);
at21=r12Transform(2,1);
at22=r12Transform(2,2);
at23=r12Transform(2,3);
at24=r12Transform(2,4);

%A term in the first formula in Equation 23.
M=(1/2)*(A'*A)*RDot;
%Extract the elements
m1=M(1);
m2=M(2);
m3=M(3);
m4=M(4);
m5=M(5);
m6=M(6);
m7=M(7);
m8=M(8);
m9=M(9);
m10=M(10);
m11=M(11);
m12=M(12);
m13=M(13);
m14=M(14);
m15=M(15);
m16=M(16);
m17=M(17);
m18=M(18);
m19=M(19);
m20=M(20);
m21=M(21);
m22=M(22);
m23=M(23);
m24=M(24);
m25=M(25);
m26=M(26);
m27=M(27);
m28=M(28);
m29=M(29);
m30=M(30);
m31=M(31);
m32=M(32);
m33=M(33);
m34=M(34);
m35=M(35);
m36=M(36);

%A vector term in the first formula in Equation 23.
mVec=u1Hat'*A*RDot;
%Extract the elements
mv1=mVec(1);
mv2=mVec(2);
mv3=mVec(3);
mv4=mVec(4);
mv5=mVec(5);
mv6=mVec(6);

%The first polynomial in Equation 23. The components are ordered
%[r3;r4;r5;r6].
terms1=zeros(5,24);%Allocate space for the terms.

terms1(1,1)=(-at11*(rDot(1)+mv1)-at21*mv2-mv3);
terms1(2:end,1)=[1;0;0;0];%r3

terms1(1,2)=(at11^3*m1+m15+at11*(at21^2*m2+m3)+at11^2*(m13+at21*m7)+at21*(at21*(m14+at21*m8)+m9));
terms1(2:end,2)=[3;0;0;0];%r3^3

terms1(1,3)=(-at12*(rDot(1)+mv1)-at22*mv2-mv4);
terms1(2:end,3)=[0;1;0;0];%r4

terms1(1,4)=(2*at21*at22*m14+m21+at12*m3+at11^2*(3*at12*m1+m19+at22*m7)+2*at11*(at21*at22*m2+at12*(m13+at21*m7))+at21^2*(at12*m2+m20+3*at22*m8)+at22*m9);
terms1(2:end,4)=[2;1;0;0];%r3^2*r4

terms1(1,5)=(at12^2*m13+at22^2*m14+m16+at11*(3*at12^2*m1+at22^2*m2+m4+2*at12*(m19+at22*m7))+at21*(m10+2*at12*at22*m2+2*at22*m20+at12^2*m7+3*at22^2*m8));
terms1(2:end,5)=[1;2;0;0];%r3*r4^2

terms1(1,6)=(at12^3*m1+m22+at12*(at22^2*m2+m4)+at12^2*(m19+at22*m7)+at22*(m10+at22*(m20+at22*m8)));
terms1(2:end,6)=[0;3;0;0];%r4^3

terms1(1,7)=(-at13*(rDot(1)+mv1)-at23*mv2-mv5);
terms1(2:end,7)=[0;0;1;0];%r5

terms1(1,8)=(2*at21*at23*m14+m27+at13*m3+at11^2*(3*at13*m1+m25+at23*m7)+2*at11*(at21*at23*m2+at13*(m13+at21*m7))+at21^2*(at13*m2+m26+3*at23*m8)+at23*m9);
terms1(2:end,8)=[2;0;1;0];%r3^2*r5

terms1(1,9)=2*(at22*at23*m14+at12*(at21*at23*m2+at13*(m13+at21*m7))+at11*(at13*m19+at22*at23*m2+at13*at22*m7+at12*(3*at13*m1+m25+at23*m7))+at21*(at23*m20+at22*(at13*m2+m26+3*at23*m8)));
terms1(2:end,9)=[1;1;1;0];%r3*r4*r5

terms1(1,10)=(at22^2*(at13*m2+m26)+m28+at13*m4+at12^2*(3*at13*m1+m25+at23*m7)+2*at12*(at22*at23*m2+at13*(m19+at22*m7))+at23*(m10+2*at22*m20+3*at22^2*m8));
terms1(2:end,10)=[0;2;1;0];%r4^2*r5

terms1(1,11)=(at13^2*m13+at23^2*m14+m17+at11*(3*at13^2*m1+at23^2*m2+m5+2*at13*(m25+at23*m7))+at21*(m11+2*at13*at23*m2+2*at23*m26+at13^2*m7+3*at23^2*m8));
terms1(2:end,11)=[1;0;2;0];%r3*r5^2

terms1(1,12)=(at22*m11+2*at13*at22*at23*m2+m23+at13^2*(m19+at22*m7)+at12*(3*at13^2*m1+at23^2*m2+m5+2*at13*(m25+at23*m7))+at23*(2*at22*m26+at23*(m20+3*at22*m8)));
terms1(2:end,12)=[0;1;2;0];%r4*r5^2;

terms1(1,13)=(at13^3*m1+m29+at13*(at23^2*m2+m5)+at13^2*(m25+at23*m7)+at23*(m11+at23*(m26+at23*m8)));
terms1(2:end,13)=[0;0;3;0];%r5^3

terms1(1,14)=(-at14*(rDot(1)+mv1)-at24*mv2-mv6);
terms1(2:end,14)=[0;0;0;1];%r6

terms1(1,15)=(2*at21*at24*m14+at14*m3+m33+at11^2*(3*at14*m1+m31+at24*m7)+2*at11*(at21*at24*m2+at14*(m13+at21*m7))+at21^2*(at14*m2+m32+3*at24*m8)+at24*m9);
terms1(2:end,15)=[2;0;0;1];%r3^2*r6

terms1(1,16)=2*(at22*at24*m14+at12*(at21*at24*m2+at14*(m13+at21*m7))+at11*(at14*m19+at22*at24*m2+at14*at22*m7+at12*(3*at14*m1+m31+at24*m7))+at21*(at24*m20+at22*(at14*m2+m32+3*at24*m8)));
terms1(2:end,16)=[1;1;0;1];%r3*r4*r6

terms1(1,17)=(at22^2*(at14*m2+m32)+m34+at14*m4+at12^2*(3*at14*m1+m31+at24*m7)+2*at12*(at22*at24*m2+at14*(m19+at22*m7))+at24*(m10+2*at22*m20+3*at22^2*m8));
terms1(2:end,17)=[0;2;0;1];%r4^2*r6

terms1(1,18)=2*(at23*at24*m14+at13*(at21*at24*m2+at14*(m13+at21*m7))+at11*(at23*at24*m2+at14*m25+at14*at23*m7+at13*(3*at14*m1+m31+at24*m7))+at21*(at24*m26+at23*(at14*m2+m32+3*at24*m8)));
terms1(2:end,18)=[1;0;1;1];%r3*r5*r6

terms1(1,19)=2*(at14*at22*at23*m2+at23*at24*m20+at22*at24*m26+at22*at23*m32+at13*(at14*m19+at22*at24*m2+at14*at22*m7)+at12*(at23*at24*m2+at14*m25+at14*at23*m7+at13*(3*at14*m1+m31+at24*m7))+3*at22*at23*at24*m8);
terms1(2:end,19)=[0;1;1;1];%r4*r5*r6

terms1(1,20)=(at23^2*(at14*m2+m32)+m35+at14*m5+2*at13*(at23*at24*m2+at14*m25+at14*at23*m7)+at13^2*(3*at14*m1+m31+at24*m7)+at24*(m11+2*at23*m26+3*at23^2*m8));
terms1(2:end,20)=[0;0;2;1];%r5^2*r6

terms1(1,21)=(at14^2*m13+at24^2*m14+m18+at11*(3*at14^2*m1+at24^2*m2+m6+2*at14*(m31+at24*m7))+at21*(m12+2*at14*at24*m2+2*at24*m32+at14^2*m7+3*at24^2*m8));
terms1(2:end,21)=[1;0;0;2];%r3*r6^2

terms1(1,22)=(at22*m12+2*at14*at22*at24*m2+m24+at14^2*(m19+at22*m7)+at12*(3*at14^2*m1+at24^2*m2+m6+2*at14*(m31+at24*m7))+at24*(2*at22*m32+at24*(m20+3*at22*m8))) ;
terms1(2:end,22)=[0;1;0;2];%r4*r6^2

terms1(1,23)=(at23*m12+2*at14*at23*at24*m2+m30+at14^2*(m25+at23*m7)+at13*(3*at14^2*m1+at24^2*m2+m6+2*at14*(m31+at24*m7))+at24*(2*at23*m32+at24*(m26+3*at23*m8)));
terms1(2:end,23)=[0;0;1;2];%r5*r6^2

terms1(1,24)=(at14^3*m1+m36+at14*(at24^2*m2+m6)+at14^2*(m31+at24*m7)+at24*(m12+at24*(m32+at24*m8)));
terms1(2:end,24)=[0;0;0;3];%r6^3

%Scale the coefficients
terms1(1,:)=terms1(1,:)/max(abs(terms1(1,:)));

%cHat is defined after Equation 20.
cHat=(1/4)*n'*(A'*A)*n-u1'*A*n+u1'*u1;

%A term in the second formula in Equation 23.
Mt=(1/4)*(A'*A);
%Extract the elements
mt1=Mt(1);
mt2=Mt(2);
mt3=Mt(3);
mt4=Mt(4);
mt5=Mt(5);
mt6=Mt(6);
mt7=Mt(7);
mt8=Mt(8);
mt9=Mt(9);
mt10=Mt(10);
mt11=Mt(11);
mt12=Mt(12);
mt13=Mt(13);
mt14=Mt(14);
mt15=Mt(15);
mt16=Mt(16);
mt17=Mt(17);
mt18=Mt(18);
mt19=Mt(19);
mt20=Mt(20);
mt21=Mt(21);
mt22=Mt(22);
mt23=Mt(23);
mt24=Mt(24);
mt25=Mt(25);
mt26=Mt(26);
mt27=Mt(27);
mt28=Mt(28);
mt29=Mt(29);
mt30=Mt(30);
mt31=Mt(31);
mt32=Mt(32);
mt33=Mt(33);
mt34=Mt(34);
mt35=Mt(35);
mt36=Mt(36);

%A vector term in the second formula in Equation 23.
mVec2=u1Hat'*A;
%Extract the elements
mvt1=mVec2(1);
mvt2=mVec2(2);
mvt3=mVec2(3);
mvt4=mVec2(4);
mvt5=mVec2(5);
mvt6=mVec2(6);

%The second polynomial in Equation 23. The components are ordered
%[r3;r4;r5;r6].
terms2=zeros(5,46);%Allocate space for the terms.

terms2(1,1)=cHat;
terms2(2:end,1)=[0;0;0;0];%Constant term

terms2(1,2)=(at11^4*mt1+mt15+at11^2*(mt13+mt3+at21^2*(mt2+mt7))+at21^4*mt8+at21^2*(mt14+mt9));
terms2(2:end,2)=[4;0;0;0];%r3^4

terms2(1,3)=2*(2*at11^3*at12*mt1+at11^2*at21*at22*(mt2+mt7)+at11*at12*(mt13+mt3+at21^2*(mt2+mt7))+at21*at22*(mt14+2*at21^2*mt8+mt9));
terms2(2:end,3)=[3;1;0;0];%r3^3*r4

terms2(1,4)=(mt16+mt21+at12^2*(mt13+mt3)+4*at11*at12*at21*at22*(mt2+mt7)+at11^2*(6*at12^2*mt1+mt19+mt4+at22^2*(mt2+mt7))+at21^2*(mt10+mt20+at12^2*(mt2+mt7)+6*at22^2*mt8)+at22^2*(mt14+mt9));
terms2(2:end,4)=[2;2;0;0];%r3^2*r4^2

terms2(1,5)=2*(at11*at12*(2*at12^2*mt1+mt19+mt4+at22^2*(mt2+mt7))+at21*at22*(mt10+mt20+at12^2*(mt2+mt7)+2*at22^2*mt8));
terms2(2:end,5)=[1;3;0;0];%r3*r4^3

terms2(1,6)=(at12^4*mt1+at22^2*(mt10+mt20)+mt22+at12^2*(mt19+mt4+at22^2*(mt2+mt7))+at22^4*mt8);
terms2(2:end,6)=[0;4;0;0];%r4^4

terms2(1,7)=2*(2*at11^3*at13*mt1+at11^2*at21*at23*(mt2+mt7)+at11*at13*(mt13+mt3+at21^2*(mt2+mt7))+at21*at23*(mt14+2*at21^2*mt8+mt9));
terms2(2:end,7)=[3;0;1;0];%r3^3*r5

terms2(1,8)=2*(2*at12^3*at13*mt1+at12^2*at22*at23*(mt2+mt7)+at12*at13*(mt19+mt4+at22^2*(mt2+mt7))+at22*at23*(mt10+mt20+2*at22^2*mt8));
terms2(2:end,8)=[0;3;1;0];%r4^3*r5

terms2(1,9)=(mt17+mt27+at13^2*(mt13+mt3)+4*at11*at13*at21*at23*(mt2+mt7)+at11^2*(6*at13^2*mt1+mt25+mt5+at23^2*(mt2+mt7))+at21^2*(mt11+mt26+at13^2*(mt2+mt7)+6*at23^2*mt8)+at23^2*(mt14+mt9));
terms2(2:end,9)=[2;0;2;0];%r3^2*r5^2

terms2(1,10)=(mt23+mt28+at13^2*(mt19+mt4)+4*at12*at13*at22*at23*(mt2+mt7)+at22^2*(mt11+mt26+at13^2*(mt2+mt7))+at12^2*(6*at13^2*mt1+mt25+mt5+at23^2*(mt2+mt7))+at23^2*(mt10+mt20+6*at22^2*mt8));
terms2(2:end,10)=[0;2;2;0];%r4^2*r5^2

terms2(1,11)=2*(at11*at13*(2*at13^2*mt1+mt25+mt5+at23^2*(mt2+mt7))+at21*at23*(mt11+mt26+at13^2*(mt2+mt7)+2*at23^2*mt8));
terms2(2:end,11)=[1;0;3;0];%r3*r5^3

terms2(1,12)=2*(at12*at13*(2*at13^2*mt1+mt25+mt5+at23^2*(mt2+mt7))+at22*at23*(mt11+mt26+at13^2*(mt2+mt7)+2*at23^2*mt8));
terms2(2:end,12)=[0;1;3;0];%r4*r5^3

terms2(1,13)=(at13^4*mt1+at23^2*(mt11+mt26)+mt29+at13^2*(mt25+mt5+at23^2*(mt2+mt7))+at23^4*mt8);
terms2(2:end,13)=[0;0;4;0];%r5^4

terms2(1,14)=-2*(at11*at14*(1+mvt1)+at21*at24*mvt2);
terms2(2:end,14)=[1;0;0;1];%r3*r6

terms2(1,15)=2*(2*at11^3*at14*mt1+at11^2*at21*at24*(mt2+mt7)+at11*at14*(mt13+mt3+at21^2*(mt2+mt7))+at21*at24*(mt14+2*at21^2*mt8+mt9));
terms2(2:end,15)=[3;0;0;1];%r3^3*r6

terms2(1,16)=-2*(at12*at14*(1+mvt1)+at22*at24*mvt2);
terms2(2:end,16)=[0;1;0;1];%r4*r6

terms2(1,17)=2*(2*at12^3*at14*mt1+at12^2*at22*at24*(mt2+mt7)+at12*at14*(mt19+mt4+at22^2*(mt2+mt7))+at22*at24*(mt10+mt20+2*at22^2*mt8));
terms2(2:end,17)=[0;3;0;1];%r4^3*r6

terms2(1,18)=-2*(at13*at14*(1+mvt1)+at23*at24*mvt2);
terms2(2:end,18)=[0;0;1;1];%r5*r6

terms2(1,19)=2*(2*at13^3*at14*mt1+at13^2*at23*at24*(mt2+mt7)+at13*at14*(mt25+mt5+at23^2*(mt2+mt7))+at23*at24*(mt11+mt26+2*at23^2*mt8));
terms2(2:end,19)=[0;0;3;1];%r5^3*r6

terms2(1,20)=(-at14^2*(1+mvt1)-at24^2*mvt2-mvt6);
terms2(2:end,20)=[0;0;0;2];%r6^2

terms2(1,21)=(mt18+at14^2*(mt13+mt3)+mt33+4*at11*at14*at21*at24*(mt2+mt7)+at11^2*(6*at14^2*mt1+mt31+mt6+at24^2*(mt2+mt7))+at21^2*(mt12+mt32+at14^2*(mt2+mt7)+6*at24^2*mt8)+at24^2*(mt14+mt9));
terms2(2:end,21)=[2;0;0;2];%r3^2*r6^2

terms2(1,22)=(mt24+mt34+at14^2*(mt19+mt4)+4*at12*at14*at22*at24*(mt2+mt7)+at22^2*(mt12+mt32+at14^2*(mt2+mt7))+at12^2*(6*at14^2*mt1+mt31+mt6+at24^2*(mt2+mt7))+at24^2*(mt10+mt20+6*at22^2*mt8));
terms2(2:end,22)=[0;2;0;2];%r4^2*r6^2

terms2(1,23)=(mt30+mt35+at14^2*(mt25+mt5)+4*at13*at14*at23*at24*(mt2+mt7)+at23^2*(mt12+mt32+at14^2*(mt2+mt7))+at13^2*(6*at14^2*mt1+mt31+mt6+at24^2*(mt2+mt7))+at24^2*(mt11+mt26+6*at23^2*mt8));
terms2(2:end,23)=[0;0;2;2];%r5^2*r6^2

terms2(1,24)=2*(at11*at14*(2*at14^2*mt1+mt31+mt6+at24^2*(mt2+mt7))+at21*at24*(mt12+mt32+at14^2*(mt2+mt7)+2*at24^2*mt8));
terms2(2:end,24)=[1;0;0;3];%r3*r6^3

terms2(1,25)=2*(at12*at14*(2*at14^2*mt1+mt31+mt6+at24^2*(mt2+mt7))+at22*at24*(mt12+mt32+at14^2*(mt2+mt7)+2*at24^2*mt8));
terms2(2:end,25)=[0;1;0;3];%r4*r6^3

terms2(1,26)=2*(at13*at14*(2*at14^2*mt1+mt31+mt6+at24^2*(mt2+mt7))+at23*at24*(mt12+mt32+at14^2*(mt2+mt7)+2*at24^2*mt8));
terms2(2:end,26)=[0;0;1;3];%r5*r6^3

terms2(1,27)=(at14^4*mt1+at24^2*(mt12+mt32)+mt36+at14^2*(mt31+mt6+at24^2*(mt2+mt7))+at24^4*mt8);
terms2(2:end,27)=[0;0;0;4];%r6^4

terms2(1,28)=(-at13^2*(1+mvt1)-at23^2*mvt2-mvt5);
terms2(2:end,28)=[0;0;2;0];%r5^2

terms2(1,29)=2*(at11*(6*at13^2*at14*mt1+2*at13*at23*at24*(mt2+mt7)+at14*(mt25+mt5+at23^2*(mt2+mt7)))+at21*(2*at13*at14*at23*(mt2+mt7)+at24*(mt11+mt26+at13^2*(mt2+mt7)+6*at23^2*mt8)));
terms2(2:end,29)=[1;0;2;1];%r3*r5^2*r6

terms2(1,30)=2*(at12*(6*at13^2*at14*mt1+2*at13*at23*at24*(mt2+mt7)+at14*(mt25+mt5+at23^2*(mt2+mt7)))+at22*(2*at13*at14*at23*(mt2+mt7)+at24*(mt11+mt26+at13^2*(mt2+mt7)+6*at23^2*mt8)));
terms2(2:end,30)=[0;1;2;1];%r4*r5^2*r6

terms2(1,31)=(-at12^2*(1+mvt1)-at22^2*mvt2-mvt4);
terms2(2:end,31)=[0;2;0;0];%r4^2

terms2(1,32)=2*(at11*(6*at12^2*at13*mt1+2*at12*at22*at23*(mt2+mt7)+at13*(mt19+mt4+at22^2*(mt2+mt7)))+at21*(2*at12*at13*at22*(mt2+mt7)+at23*(mt10+mt20+at12^2*(mt2+mt7)+6*at22^2*mt8)));
terms2(2:end,32)=[1;2;1;0];%r3*r4^2*r5

terms2(1,33)=2*(at11*(6*at12^2*at14*mt1+2*at12*at22*at24*(mt2+mt7)+at14*(mt19+mt4+at22^2*(mt2+mt7)))+at21*(2*at12*at14*at22*(mt2+mt7)+at24*(mt10+mt20+at12^2*(mt2+mt7)+6*at22^2*mt8)));
terms2(2:end,33)=[1;2;0;1];%r3*r4^2*r6

terms2(1,34)=2*(2*at12*at22*(at14*at23+at13*at24)*(mt2+mt7)+at13*at14*(mt19+mt4+at22^2*(mt2+mt7))+at12^2*(6*at13*at14*mt1+at23*at24*(mt2+mt7))+at23*at24*(mt10+mt20+6*at22^2*mt8));
terms2(2:end,34)=[0;2;1;1];%r4^2*r5*r6

terms2(1,35)=(-at11^2*(1+mvt1)-at21^2*mvt2-mvt3);
terms2(2:end,35)=[2;0;0;0];%r3^2

terms2(1,36)=2*(2*at11*at21*(at13*at22+at12*at23)*(mt2+mt7)+at12*at13*(mt13+mt3+at21^2*(mt2+mt7))+at11^2*(6*at12*at13*mt1+at22*at23*(mt2+mt7))+at22*at23*(mt14+6*at21^2*mt8+mt9));
terms2(2:end,36)=[2;1;1;0];%r3^2*r4*r5

terms2(1,37)=2*(2*at11*at21*(at14*at22+at12*at24)*(mt2+mt7)+at12*at14*(mt13+mt3+at21^2*(mt2+mt7))+at11^2*(6*at12*at14*mt1+at22*at24*(mt2+mt7))+at22*at24*(mt14+6*at21^2*mt8+mt9));
terms2(2:end,37)=[2;1;0;1];%r3^2*r4*r6

terms2(1,38)=2*(2*at11*at21*(at14*at23+at13*at24)*(mt2+mt7)+at13*at14*(mt13+mt3+at21^2*(mt2+mt7))+at11^2*(6*at13*at14*mt1+at23*at24*(mt2+mt7))+at23*at24*(mt14+6*at21^2*mt8+mt9));
terms2(2:end,38)=[2;0;1;1];%r3^2*r5*r6

terms2(1,39)=-2*(at11*at12*(1+mvt1)+at21*at22*mvt2);
terms2(2:end,39)=[1;1;0;0];%r3*r4

terms2(1,40)=2*(at11*(2*at13*at22*at23*(mt2+mt7)+at12*(6*at13^2*mt1+mt25+mt5+at23^2*(mt2+mt7)))+at21*(2*at12*at13*at23*(mt2+mt7)+at22*(mt11+mt26+at13^2*(mt2+mt7)+6*at23^2*mt8)));
terms2(2:end,40)=[1;1;2;0];%r3*r4*r5^2

terms2(1,41)=4*(at21*(at13*at14*at22+at12*at14*at23+at12*at13*at24)*(mt2+mt7)+at11*(at22*(at14*at23+at13*at24)*(mt2+mt7)+at12*(6*at13*at14*mt1+at23*at24*(mt2+mt7)))+6*at21*at22*at23*at24*mt8);
terms2(2:end,41)=[1;1;1;1];%r3*r4*r5*r6

terms2(1,42)=2*(at11*(2*at14*at22*at24*(mt2+mt7)+at12*(6*at14^2*mt1+mt31+mt6+at24^2*(mt2+mt7)))+at21*(2*at12*at14*at24*(mt2+mt7)+at22*(mt12+mt32+at14^2*(mt2+mt7)+6*at24^2*mt8)));
terms2(2:end,42)=[1;1;0;2];%r3*r4*r6^2

terms2(1,43)=-2*(at11*at13*(1+mvt1)+at21*at23*mvt2);
terms2(2:end,43)=[1;0;1;0];%r3*r5

terms2(1,44)=2*(at11*(2*at14*at23*at24*(mt2+mt7)+at13*(6*at14^2*mt1+mt31+mt6+at24^2*(mt2+mt7)))+at21*(2*at13*at14*at24*(mt2+mt7)+at23*(mt12+mt32+at14^2*(mt2+mt7)+6*at24^2*mt8)));
terms2(2:end,44)=[1;0;1;2];%r3*r5*r6^2

terms2(1,45)=-2*(at12*at13*(1+mvt1)+at22*at23*mvt2);
terms2(2:end,45)=[0;1;1;0];%r4*r5

terms2(1,46)=2*(at12*(2*at14*at23*at24*(mt2+mt7)+at13*(6*at14^2*mt1+mt31+mt6+at24^2*(mt2+mt7)))+at22*(2*at13*at14*at24*(mt2+mt7)+at23*(mt12+mt32+at14^2*(mt2+mt7)+6*at24^2*mt8)));
terms2(2:end,46)=[0;1;1;2];%r4*r5*r6^2

%Scale the coefficients
terms2(1,:)=terms2(1,:)/max(abs(terms2(1,:)));

%The third polynomial in Equation 23:
%A vector term in the third formula in Equation 23.
mVec3=(u2-u3)'*A;

%Extract the elements
mvtt1=mVec3(1);
mvtt2=mVec3(2);
mvtt3=mVec3(3);
mvtt4=mVec3(4);
mvtt5=mVec3(5);
mvtt6=mVec3(6);

terms3=zeros(5,11);

terms3(1,1)=(-u3'*u3+u2'*u2-(u2-u3)'*A*n);
terms3(2:end,1)=[0;0;0;0];%Constant term

terms3(1,2)=1+at11^2*mvtt1+at21^2*(-1+mvtt2)+mvtt3;
terms3(2:end,2)=[2;0;0;0];%r3^2

terms3(1,3)=2*(at11*at12*mvtt1+at21*at22*(-1+mvtt2));
terms3(2:end,3)=[1;1;0;0];%r3*r4

terms3(1,4)=(at12^2*mvtt1+at22^2*(-1+mvtt2)+mvtt4);
terms3(2:end,4)=[0;2;0;0];%r4^2

terms3(1,5)=2*(at11*at13*mvtt1+at21*at23*(-1+mvtt2));
terms3(2:end,5)=[1;0;1;0];%r3*r5

terms3(1,6)=2*(at12*at13*mvtt1+at22*at23*(-1+mvtt2));
terms3(2:end,6)=[0;1;1;0];%r4*r5

terms3(1,7)=(at13^2*mvtt1+at23^2*(-1+mvtt2)+mvtt5);
terms3(2:end,7)=[0;0;2;0];%r5^2

terms3(1,8)=2*(at11*at14*mvtt1+at21*at24*(-1+mvtt2));
terms3(2:end,8)=[1;0;0;1];%r3*r6

terms3(1,9)=2*(at12*at14*mvtt1+at22*at24*(-1+mvtt2));
terms3(2:end,9)=[0;1;0;1];%r4*r6

terms3(1,10)=2*(at13*at14*mvtt1+at23*at24*(-1+mvtt2));
terms3(2:end,10)=[0;0;1;1];%r5*r6

terms3(1,11)=(at14^2*mvtt1+at24^2*(-1+mvtt2)+mvtt6);
terms3(2:end,11)=[0;0;0;2];%r6^2

%Scale the coefficients
terms3(1,:)=terms3(1,:)/max(abs(terms3(1,:)));

%The fourth polynomial in Equation 23:
mVec4=(u2-u4)'*A;
%Extract the elements
mvtt1=mVec4(1);
mvtt2=mVec4(2);
mvtt3=mVec4(3);
mvtt4=mVec4(4);
mvtt5=mVec4(5);
mvtt6=mVec4(6);

terms4=zeros(5,11);

terms4(1,1)=(-u4'*u4+u2'*u2-(u2-u4)'*A*n);
terms4(2:end,1)=[0;0;0;0];%Constant term

terms4(1,2)=(at11^2*mvtt1 + at21^2*(-1 + mvtt2) + mvtt3) ;
terms4(2:end,2)=[2;0;0;0];%r3^2

terms4(1,3)=2*(at11*at12*mvtt1 + at21*at22*(-1 + mvtt2)) ;
terms4(2:end,3)=[1;1;0;0];%r3*r4

terms4(1,4)=(1 + at12^2*mvtt1 + at22^2*(-1 + mvtt2) + mvtt4);
terms4(2:end,4)=[0;2;0;0];%r4^2

terms4(1,5)=2*(at11*at13*mvtt1 + at21*at23*(-1 + mvtt2));
terms4(2:end,5)=[1;0;1;0];%r3*r5

terms4(1,6)=2*(at12*at13*mvtt1 + at22*at23*(-1 + mvtt2));
terms4(2:end,6)=[0;1;1;0];%r4*r5

terms4(1,7)=(at13^2*mvtt1 + at23^2*(-1 + mvtt2) + mvtt5);
terms4(2:end,7)=[0;0;2;0];%r5^2

terms4(1,8)=2*(at11*at14*mvtt1 + at21*at24*(-1 + mvtt2));
terms4(2:end,8)=[1;0;0;1];%r3*r6

terms4(1,9)=2*(at12*at14*mvtt1 + at22*at24*(-1 + mvtt2));
terms4(2:end,9)=[0;1;0;1];%r4*r6

terms4(1,10)=2*(at13*at14*mvtt1 + at23*at24*(-1 + mvtt2));
terms4(2:end,10)=[0;0;1;1];%r5*r6

terms4(1,11)=(at14^2*mvtt1 + at24^2*(-1 + mvtt2) + mvtt6);
terms4(2:end,11)=[0;0;0;2];%r6^2

%Scale the coefficients
terms4(1,:)=terms4(1,:)/max(abs(terms4(1,:)));

%Now, put the term matrices into formats that can be used by the
%multivariate polynomial solvers.
xPolys=cell(4,1);
xPolys{1}=terms1;
xPolys{2}=terms2;
xPolys{3}=terms3;
xPolys{4}=terms4;

varNames={'rc','rd','re','rf'};

for curPoly=1:4
    termMat=xPolys{curPoly};

    numTerms=size(termMat,2);

    thePoly=[];

    for curTerm=1:numTerms
        curCoeff=num2str(termMat(1,curTerm),16);

        curMonomial=[];

        for curVar=1:4
            if(termMat(curVar+1,curTerm)~=0)
                if(~isempty(curMonomial))
                   curMonomial=[curMonomial,'*'];
                end

                if(termMat(curVar+1,curTerm)==1)
                    curMonomial=[curMonomial,varNames{curVar}];
                else
                    curMonomial=[curMonomial,varNames{curVar},'^',num2str(termMat(curVar+1,curTerm))];
                end
            end
        end

        if(isempty(curMonomial))%If it is a constant term.
            %The constant term (if there is one) should be the first
            %term, so we can just set thePoly to it.
            thePoly=curCoeff;
        elseif(~isempty(thePoly))
            thePoly=[thePoly,'+(',curCoeff,')*',curMonomial];
        else%If there is no constant term.
            thePoly=['(',curCoeff,')*',curMonomial];
        end
    end

    xPolys{curPoly}=thePoly;
end

rEst=solvePolySysWithExtProg(xPolys,varNames,algorithm,opts,scratchFolderPath,execPath);

if(isempty(rEst))
    %The solver failed.
    xEst=[];
    return;
end
%Throw out complex solutions. Solutions are deemed complex if the
%imaginary part exceeds AbsTol in magnitude.
sel=all(abs(imag(rEst))<AbsTol,1);
rEst=real(rEst(:,sel));

%Only positive range values are valid.
sel=all(rEst>0,1);
rEst=rEst(:,sel);

numSol=size(rEst,2);
xEst=zeros(6,numSol);

%Given solutions to the r variables, we need to extract the state.
numAdded=0;
for curSol=1:numSol
    %From Equation 17
    r1r2=r12Transform*rEst(:,curSol);

    %Only positive ranges are valid.
    if(all(r1r2)>0)
        r=[r1r2;rEst(:,curSol)];

        %Equation 8
        uDot=A*(-RDot*r);

        %Equation 9
        u=(1/2)*A*(n-r.*r);

        numAdded=numAdded+1;
        xEst(:,numAdded)=[u;uDot];
    end
end

%Shrink to fit the actual number of solutions added.
xEst=xEst(:,1:numAdded);

%Undo the effects of scaling:
xEst=scalFactor*xEst;

%Add back in the offset that was present due to centering the coordinate
%system around the sensors.
xEst(1:3,:)=bsxfun(@plus,xEst(1:3,:),centerOfRegion);
end

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


% nufft_tune_kaiser
% tune the shape parameter for kaiser-bessel interpolation in 1D NUFFT
% to minimize the worst-case error

% explore scaling factors
if ~isvar('N') && 0
	N = 2^8; K = 2*N;
	J = 6; alpha = 2.34 * J; kb_m = 0;
	n = [0:(N-1)]'-(N-1)/2;
	kernel = kaiser_bessel('inline', [], alpha, kb_m, K/N);
	sn_zn = reale(1 ./ nufft_interp_zn(0, N, J, K, kernel)); % discrete way
	sn_ft = 1 ./ kaiser_bessel_ft(n/K, J, alpha, kb_m, 1); % cont. FT way
	clf, subplot(121)
	plot(n, sn_zn, 'b.', n, sn_ft, 'r-'), legend('zn', 'FT')
	subplot(122), plot(n, sn_zn ./ sn_ft - 1)
prompt
end

if ~isvar('err.zn')
	N = 2^8;			% large N to reduce effect of N
	n = [0:(N-1)]'-(N-1)/2;
	K_N = 2;			% oversampling factor
	Jlist = [3:16]';
%	Jlist = [2]';			% for m=0, a=2.5 is best
%	mlist = linspace(-2,2,21)';	% J=7 with this list showed m=0
	mlist = 0;
	alist = linspace(2.2,2.4,21)';
	rlist = [0:20]'/40;		% fractions of gamma

	% range of useful alpha values for each J
%	alf_min = sqrt((pi*Jlist/4).^2 - (5.764)^2);
%	alf_min(Jlist < 8) = 0;
%	alf_max = sqrt((pi*Jlist).^2 - (5.764)^2);

	[aa mm] = ndgrid(alist, mlist);
	err.zn = zeros([length(rlist) length(Jlist) numel(aa)]);
	err.ft = zeros([length(rlist) length(Jlist) numel(aa)]);

	for jj=1:length(Jlist)
		J = Jlist(jj);
		printf('J=%d', J)

		K = K_N * N;
		gam = 2*pi/K;
		om = gam * rlist;

		% kaiser-bessel with various shapes
		for ii=1:numel(aa)
			alf = aa(ii) * J;
			kb_m = mm(ii);
			kernel = kaiser_bessel('inline', [], alf, kb_m, K/N);
			kernel_ft = kaiser_bessel_ft('inline', J, alf, kb_m, 1);

			% interpolator worst-case error
		        err.zn(:,jj,ii) = nufft1_error(om, N, J, K, kernel);
		        err.ft(:,jj,ii) = nufft1_error(om, N, J, K, kernel, ...
				kernel_ft);
%				1 ./ kaiser_bessel_ft(n/K, J, alf, mm(ii), 1));
		end

		if 1
			subplot(4,4,jj)
			tmp = reshape(max(err.zn(:,jj,:), [], 1), size(aa));
			semilogy(alist, tmp)
			title(sprintf('J=%d', J)), axis tight
%			prompt
		end
	end
prompt
end

if 1
	emax.zn = max(err.zn, [], 1);			% [M J AO] -> [1 J AO]
	emax.zn = reshape(emax.zn, length(Jlist), numel(aa)); % [J AO]
	[ebest.zn, ibest.zn] = min(emax.zn, [], 2);	% [J AO] -> J
	[ia_best.zn, im_best.zn] = ind2sub(size(aa), ibest.zn);
	if any(ia_best.zn == 1 | ia_best.zn == length(alist))
		warning 'zn end point F'
	end
	if any(im_best.zn == 1 | im_best.zn == length(mlist)) && length(mlist)>1
		warning 'zn end point m'
	end
	abest.zn = alist(ia_best.zn);
	m_best.zn = mlist(im_best.zn);
end

if 1
	emax.ft = max(err.ft, [], 1);		% [M J AO] -> [1 J AO]
	emax.ft = reshape(emax.ft, length(Jlist), numel(aa)); % [J AO]
	[ebest.ft, ibest.ft] = min(emax.ft, [], 2); % [J AO] -> J
	[ia_best.ft, im_best.ft] = ind2sub(size(aa), ibest.ft);
	if any(ia_best.ft == 1 | ibest.ft == length(alist))
		warning 'ft end point F'
	end
	if any(im_best.ft == 1 | im_best.ft == length(mlist)) && length(mlist)>1
		warning 'ft end point m'
	end
	abest.ft = alist(ia_best.ft);
	m_best.ft = mlist(im_best.ft);
end

if 0
	err.m2 = err.zn(:,:,mm == 2);		% just where m=2
	emax.m2 = max(err.m2, [], 1);		% [M J A] -> [1 J A]
	emax.m2 = reshape(emax.m2, length(Jlist), length(alist)); % [J A]
	[ebest.m2, ia_best.m2] = min(emax.m2, [], 2); % [J A] -> J
	if any(ia_best.m2 == 1 | ia_best.m2 == length(alist))
		warning 'm2 end point F'
	end
	abest.m2 = alist(ia_best.m2);
end

% plot vs m (or f).  suprisingly, m=0 seems best!
if 0
	tmp = reshape(emax.zn, [length(Jlist) size(aa)]);	% [J A O]
%	tmp = permute(tmp, [1 3 2]);	% [J O A]
%	tmp = permute(tmp, [3 2 1]);	% [O A J]
	tmp = permute(tmp, [2 3 1]);	% [A O J]
	clf
	semilogy(mlist, 1*tmp(:,:,1), 'y-o'), xlabel m, axis tight
	semilogy(alist, 1*tmp(:,:,1), 'g-^'), xlabel f, axis tight
return
	semilogy(alist, 1*tmp(:,:,1), 'g-^', ...
		alist, 0*tmp(:,:,2), 'y-o')
end

% plot best "fwhm" vs J
clf
if 1
	subplot(131)
	plot(Jlist, abest.zn, 'c-x', Jlist, abest.ft, 'y-o')
	xlabel J, ylabel \alpha, grid, legend('zn', 'ft')
end

% plot ebest vs J
if 1
	subplot(132)
	semilogy(Jlist, ebest.zn, 'c-x', Jlist, ebest.ft, 'y-o')
	xlabel J, ylabel E_{max}, legend('zn', 'ft')
	titlef('Maximum KB error for $K/N=%g$', K/N)

% ir_savefig c 'fig_?'
end

if 1
	subplot(133)
	plot(Jlist, ebest.zn ./ ebest.ft, '-o'), axis tight
	xlabel J, ylabel 'zn / ft'
return
end

% save the tuned parameter to file
% using trick to deal with J=2
if 1
	file = sprintf('kaiser,m=%d', mlist)
	Jlist = [2; Jlist];
	abest.zn = [2.5; abest.zn];
	abest.ft = [2.5; abest.ft];
	save(file, 'Jlist', 'abest')
end


function cheby_u_poly_values_test ( )

%*****************************************************************************80
%
%% CHEBY_U_POLY_VALUES_TEST demonstrates the use of CHEBY_U_POLY_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 February 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CHEBY_U_POLY_VALUES_TEST:\n' );
  fprintf ( 1, '  CHEBY_U_POLY_VALUES returns values of\n' );
  fprintf ( 1, '  the Chebyshev U polynomials.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     N       X      U(N)(X)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, n, x, fx ] = cheby_u_poly_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %4d  %8f  %24.16f\n', n, x, fx );

  end

  return
end


%% Dummy test
lp = [1 1 1 1
      1 2 2 1
      1 3 3 1
      1 1 1 1];
ms = [2 3 4
      1 4 5];

% Get all pairs of neighboring leave regions
[~, idx_neighbors] = seg2gridbmap(lp);
K = max(idx_neighbors.matrix_max(:)) + 1;
neigh_pairs = unique(idx_neighbors.matrix_min+K*idx_neighbors.matrix_max);
neigh_pairs(neigh_pairs==0) = [];
neigh_pairs_min = mod(neigh_pairs,K);
neigh_pairs_max = (neigh_pairs-neigh_pairs_min)/K;

if isrow(neigh_pairs_min)
    neigh_pairs_min = neigh_pairs_min';
end
if isrow(neigh_pairs_max)
    neigh_pairs_max = neigh_pairs_max';
end

n_pairs    = 100;
n_triplets = 100;
curr_cands = mex_get_tree_cands(double(lp)-1, double(ms)-1,...
                                 neigh_pairs_min-1, neigh_pairs_max-1,...
                                 [n_pairs, n_triplets]);

assert(isequal(curr_cands{1},[1 2
                              1 3]))
assert(isempty(curr_cands{2}))

%% Simple test 1
lp = [1 2 3 4 5];
ms = [2 3 6
      4 5 7
      6 1 8
      7 8 9];

% Get all pairs of neighboring leave regions
[~, idx_neighbors] = seg2gridbmap(lp);
K = max(idx_neighbors.matrix_max(:)) + 1;
neigh_pairs = unique(idx_neighbors.matrix_min+K*idx_neighbors.matrix_max);
neigh_pairs(neigh_pairs==0) = [];
neigh_pairs_min = mod(neigh_pairs,K);
neigh_pairs_max = (neigh_pairs-neigh_pairs_min)/K;

if isrow(neigh_pairs_min)
    neigh_pairs_min = neigh_pairs_min';
end
if isrow(neigh_pairs_max)
    neigh_pairs_max = neigh_pairs_max';
end

n_pairs    = 100;
n_triplets = 100;
curr_cands = mex_get_tree_cands(double(lp)-1, double(ms)-1,...
                                 neigh_pairs_min-1, neigh_pairs_max-1,...
                                 [n_pairs, n_triplets]);
assert(isequal(curr_cands{1},[6 7
                              4 8
                              4 6
                              1 2
                              3 7
                              3 4]))
assert(isempty(curr_cands{2}))

%%  Simple test 2
lp = [1 2 3 4];
ms = [1 2 5
      3 4 6
      5 6 7];

% Get all pairs of neighboring leave regions
[~, idx_neighbors] = seg2gridbmap(lp);
K = max(idx_neighbors.matrix_max(:)) + 1;
neigh_pairs = unique(idx_neighbors.matrix_min+K*idx_neighbors.matrix_max);
neigh_pairs(neigh_pairs==0) = [];
neigh_pairs_min = mod(neigh_pairs,K);
neigh_pairs_max = (neigh_pairs-neigh_pairs_min)/K;

if isrow(neigh_pairs_min)
    neigh_pairs_min = neigh_pairs_min';
end
if isrow(neigh_pairs_max)
    neigh_pairs_max = neigh_pairs_max';
end

n_pairs    = 100;
n_triplets = 100;
curr_cands = mex_get_tree_cands(double(lp)-1, double(ms)-1,...
                                neigh_pairs_min-1, neigh_pairs_max-1,...
                                [n_pairs, n_triplets]);
assert(isequal(curr_cands{1},[3 5
                              2 6
                              2 3]))
assert(isempty(curr_cands{2}))

%% Simple with 4-tuples
lp = [1 2 3 4 5
      1 3 3 4 5
      1 4 4 4 5
      1 5 5 5 5];
ms = [1 2 6
      3 6 7
      4 7 8
      5 8 9];

% Get all pairs of neighboring leave regions
[~, idx_neighbors] = seg2gridbmap(lp);
K = max(idx_neighbors.matrix_max(:)) + 1;
neigh_pairs = unique(idx_neighbors.matrix_min+K*idx_neighbors.matrix_max);
neigh_pairs(neigh_pairs==0) = [];
neigh_pairs_min = mod(neigh_pairs,K);
neigh_pairs_max = (neigh_pairs-neigh_pairs_min)/K;

if isrow(neigh_pairs_min)
    neigh_pairs_min = neigh_pairs_min';
end
if isrow(neigh_pairs_max)
    neigh_pairs_max = neigh_pairs_max';
end

n_cands    = [100, 100, 100, 100];
curr_cands = mex_get_tree_cands(double(lp)-1, double(ms)-1,...
                                neigh_pairs_min-1, neigh_pairs_max-1,...
                                n_cands);
assert(isequal(curr_cands{1},[4 5
                              5 7
                              3 4
                              5 6
                              4 6
                              1 5
                              1 4
                              1 3
                              2 3]))
assert(isequal(curr_cands{2},[3 4 5
                              4 5 6
                              1 4 5
                              1 3 5
                              1 3 4
                              2 3 4]))
assert(isequal(curr_cands{3}, [1 3 4 5
                               2 3 4 5]))
assert(isempty(curr_cands{4}))


function bd = blk_diag(A,n)
%BLK_DIAG Make or extract a sparse block diagonal matrix
% function bd = blk_diag(A,n);
% If A is not sparse, then
% returns a sparse block diagonal "bd", diagonalized from the
% elements in "A".
% "A" is ma x na, comprising bdn=(na/"n") blocks of submatrices.
% Each submatrix is ma x "n", and these submatrices are
% placed down the diagonal of the matrix.
%
% If A is already sparse, then the operation is reversed, yielding a block
% row matrix, where each set of n columns corresponds to a block element
% from the block diagonal.
%
% Routine uses NO for-loops for speed considerations.

% Copyright (c) 1993-1995, The Regents of the University of California.
% This software was produced under a U.S. Government contract
% (W-7405-ENG-36) by Los Alamos National Laboratory, which is operated
% by the University of California for the U.S. Department of Energy,
% and was funded in part by NIH grant R01-MH53213 through the University
% of Southern California to Los Alamos National Laboratory,
% and was funded in part by NIH grant R01-EY08610 to Los Alamos
% National Laboratory.
% The U.S. Government is licensed to use, reproduce, and distribute this
% software.  Permission is granted to the public to copy and use this
% software without charge, provided that this Notice and any statement
% of authorship are reproduced on all copies.  Neither the Government
% nor the University makes any warranty, express or implied, or assumes
% any liability or responsibility for the use of this software.
%
% Author: John C. Mosher, Ph.D.
% Los Alamos National Laboratory
% Group ESA-MT, MS J580
% Los Alamos, NM 87545
% email: mosher@LANL.Gov

% July 29, 1993 Author
% September 28, 1993 JCM Conversion to sparse
% July 27, 1995 JCM inverse block diagonal added

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@

if(~issparse(A)),		% then make block sparse
    [ma,na] = size(A);
    bdn = na/n; 			% number of submatrices

    if(bdn - fix(bdn)),
        error('Width of matrix must be even multiple of n');
    end

    if(0)
        i = [1:ma]';
        i = i(:,ones(1,n));
        i = i(:); 			% row indices first submatrix

        ml = length(i); 		% ma*n

        % ndx = [0:(bdn-1)]*ma; 	% row offsets per submatrix
        ndx = [0:ma:(ma*(bdn-1))]; 	% row offsets per submatrix

        i = i(:,ones(1,bdn)) + ndx(ones(ml,1),:);
    else
        tmp = reshape([1:(ma*bdn)]',ma,bdn);
        i = zeros(ma*n,bdn);
        for iblock = 1:n,
            i((iblock-1)*ma+[1:ma],:) = tmp;
        end
    end

    i = i(:); 			% row indices foreach sparse bd

    j = [1:na];
    j = j(ones(ma,1),:);
    j = j(:); 			% column indices foreach sparse bd

    bd = sparse(i,j,A(:));

else 				% already is sparse, unblock it

    [mA,na] = size(A);		% matrix always has na columns
    % how many entries in the first column?
    bdn = na/n;			% number of blocks
    ma = mA/bdn;			% rows in first block

    % blocks may themselves contain zero entries.  Build indexing as above
    if(0)
        i = [1:ma]';
        i = i(:,ones(1,n));
        i = i(:); 			% row indices first submatrix

        ml = length(i); 		% ma*n

        % ndx = [0:(bdn-1)]*ma; 	% row offsets per submatrix
        ndx = [0:ma:(ma*(bdn-1))]; 	% row offsets per submatrix

        i = i(:,ones(1,bdn)) + ndx(ones(ml,1),:);
    else
        tmp = reshape([1:(ma*bdn)]',ma,bdn);
        i = zeros(ma*n,bdn);
        for iblock = 1:n,
            i((iblock-1)*ma+[1:ma],:) = tmp;
        end
    end

    i = i(:); 			% row indices foreach sparse bd

    if(0)
        j = [1:na];
        j = j(ones(ma,1),:);
        j = j(:); 			% column indices foreach sparse bd

        % so now we have the complete two dimensional indexing. Convert to
        % one dimensional

        i = i + (j-1)*mA;
    else
        j = [0:mA:(mA*(na-1))];
        j = j(ones(ma,1),:);
        j = j(:);

        i = i + j;
    end

    bd = full(A(i)); 	% column vector
    bd = reshape(bd,ma,na);	% full matrix
end


function [gx] = g_BSL(x,phi,u,in)
% observation function for a Bayesian sequence-learner (BSL)
% function [gx] = g_BSL(x,theta,u,in)
% BSL is guessing the next outcome y_t based upon P(y_t=1|y_{t-1}), ie her
% bet actually depends upon the past sequence of outcomes.
% IN:
%   - x: sufficient statistics of log-odds of P(o=1):
%       x(1:2^K)= E[log-odds]
%       x((2^K)+1:2^(K+1))= log V[log-odds]
%   - phi: phi(1) = log-temperature and phi(2) = bias
%   - u: u(1:K)= sequence of K past outcomes
%   - in: depth of sequence learning
% OUT:
%   - gx: P(y_t=1|y_{t-1})

if VBA_isWeird (u) % e.g., 1st trial
    gx = 0.5;
    return
end

a = 0.36; % for E[s(x)] when x~n(mu,Sig)
K = in.K; % sequence depth
% yb = u(2:K+1); % previous outcomes
yb = u(1:K); % previous outcomes
if K >0
    indSeq = bin2dec(num2str(yb'))+1; % index of sequence of previous outcomes
else
    indSeq = 1;
end
m = x(indSeq);
v = exp(x((2^K)+indSeq));
gx = VBA_sigmoid(phi(2)+exp(phi(1)).*m./sqrt(1+a*v)); % E[sigm(log-odds of P(y))]


function [Ytest d C] = kmeanscov(Ctest,Ctrain,Nclass,varargin)

    if isempty(varargin)
        method_mean = 'riemann';
        method_dist = 'riemann';
    else
        method_mean = varargin{1};
        method_dist = varargin{2};
    end

    % initialisation
    Ntrial = size(Ctrain,3);

    Y_old = zeros(Ntrial,1);
    ix = randperm(Ntrial);
    C = cell(Nclass,1);
    for i=1:Nclass
        C{i} = Ctrain(:,:,ix(i));
    end

    d = zeros(Ntrial,Nclass);
    for j=1:Ntrial
        for i=1:Nclass
            d(j,i) = distance(Ctrain(:,:,j),C{i},method_dist);
        end
    end

    [~,Y] = min(d,[],2);

    % iteration
    while sum(Y~=Y_old)>(Ntrial*0.01)
        Y_old = Y;
        [Y,~,C] = mdm(Ctrain,Ctrain,Y,method_mean,method_dist);
    end

    % classification of test data
    Ntesttrial = size(Ctest,3);
    d = zeros(Ntesttrial,Nclass);
    for j=1:Ntesttrial
        for i=1:Nclass
            d(j,i) = distance(Ctest(:,:,j),C{i},method_dist);
        end
    end

    [~,Ytest] = min(d,[],2);


function [ cdf ] = gsp_erdos_renyi_warp( ss , N , p )

cutoff=4; % parameter to approximate the non-compactly supported distribution (the free convolution) by a compactly supported one, just for numerical purposes
% To do: pass a parameter

num_pts=length(ss);
if num_pts > 2
    delta=min(ss(2:num_pts)-ss(1:(num_pts-1)));
else
    delta=.1;
end

cdf=zeros(size(ss));

for k=1:num_pts
    if ss(k) > (p*N-cutoff*sqrt(p*(1-p)*N)-delta)
        if ss(k) <= (p*N+cutoff*sqrt(p*(1-p)*N)+delta)
            xx=(p*N-cutoff*sqrt(p*(1-p)*N)-2*delta):delta:ss(k);
            cdf(k)=trapz(xx,sqrt(1/((1-p)*N*p))*gsp_free_conv_norm_semi((xx-p*N)/(sqrt(p*(1-p)*N))));
        else
            cdf(k)=1;
        end
    end
end

end