function [A,B,D,E,I]=E7x(R,C)
% Subprogram for 7th order Elliptical LPF
N=7;U=11;M=1;Y=11;
% create space for and zero A1 & B2 matrices
A1=zeros(U+N);B2=zeros(U+N,N+M);
%
% R=[R1 R2 R3 R4 R5 R6 R7 R8 R9 R10 R11 R12 R13 R14 R15]
%    1  2  3  4  5  6  7  8  9  10  11  12  13  14  15
%
% C = [C1 C2 C3 C4 C5 C6 C7];
%      1  2  3  4  5  6  7
%
% For column alignment:
%
% V1 V2 V3 V4 V5 V6 V7 V8 V9 V10 V11   iC1 iC2 iC3 iC4 iC5 iC6 iC7
% 1  2  3  4  5  6  7  8  9  10  11    12  13  14  15  16  17  18
%
% Create A1
%
%       -V1/R2      +iC1
A1(1,1)=-1/R(2);A1(1,12)=1; % -Ein/R1 - V1/R2 + iC1 = 0
%
G2=1/R(3)+1/R(5); % For row 2 below
%   +V1/R3        -V2*G2       +iC2
A1(2,1)=1/R(3);A1(2,2)=-G2;A1(2,13)=1; % V1/R3 -V2*G2 + iC2 = 0
%
A1(3,3)=-1/R(6);A1(3,4)=1/R(6);A1(3,13)=-1;A1(3,14)=1; % -V3/R6 + V4/R6 - iC2 + iC3 = 0
%
G4=1/R(6)+1/R(9)+1/R(10); % For row 4 below
%
% V3/R6        +V6/R9        +V8/R10         -V4*G4       +iC4 = 0
A1(4,3)=1/R(6);A1(4,6)=1/R(9);A1(4,8)=1/R(10);A1(4,4)=-G4;A1(4,15)=1;
A1(5,1)=1/R(4);A1(5,5)=-1/R(4);A1(5,15)=-1;A1(5,14)=-1;
A1(6,5)=1/R(7);A1(6,6)=-1/R(7);A1(6,14)=-1;
A1(7,6)=1/R(8);A1(7,7)=-1/R(8);A1(7,17)=-1;A1(7,18)=-1;
A1(8,4)=1/R(10);A1(8,8)=-(1/R(10)+1/R(11));A1(8,16)=1;
A1(9,10)=1/R(12);A1(9,9)=-1/R(12);A1(9,17)=1;A1(9,16)=-1;
G10=1/R(12)+1/R(14)+1/R(15); % For row 10 below
A1(10,18)=1;A1(10,10)=-G10;A1(10,11)=1/R(14);A1(10,9)=1/R(12);
A1(11,7)=1/R(13);A1(11,11)=-1/R(13);A1(11,17)=-1;
A1(12,1)=-1; % 0-V1=E1
A1(13,3)=1;A1(13,2)=-1; % V3-V2=E2
A1(14,6)=1;A1(14,3)=-1; % V6-V3=E3
A1(15,5)=1;A1(15,4)=-1; % V5-V4=E4
A1(16,9)=1;A1(16,8)=-1; % V9-V8=E5
A1(17,11)=1;A1(17,9)=-1; % V11-V9=E6
A1(18,7)=1;A1(18,10)=-1; % V7-V10=E7
%
% B2
%
E1=1;E2=1;E3=1;E4=1;E5=1;E6=1;E7=1;Ein=1;
% E1 E2 E3 E4 E5 E6 E7 Ein  Source
% 1  2  3  4  5  6  7  8    Column of B2
%
B2(12,1)=E1;
B2(13,2)=E2;
B2(14,3)=E3;
B2(15,4)=E4;
B2(16,5)=E5;
B2(17,6)=E6;
B2(18,7)=E7;
B2(1,8)=Ein/R(1);

%
P=diag([C(1) C(2) C(3) C(4) C(5) C(6) C(7)]);
%
% Template "canned" statements
%
V=A1\B2;H=V(U+1:U+N,1:N+M);I=eye(N);
AB=P\H;A=AB(1:N,1:N);B=AB(1:N,N+1:N+M);
D=V(Y:Y,1:N);E=V(Y:Y,N+1:N+M);


function yearly_tmax_correction(filenameout)

% this program is design to adjust the daily Tmax generated by WG using
% new yearly Tmax series produced by FFT.

% load the yearly averaged Tmax after FFT
load('Pnew_tmax');
tmax_FFT=Pnew_tmax';

% load WG generated daily Tmax
load(filenameout);
[n,m]=size(gTmax);
gTmax=gTmax';
tmax_WG=reshape(gTmax,[],1);

n=length(tmax_FFT); % years
m=length(tmax_WG);  % days

% calculate yearly Tmax generated by WG, namely (Y)
j=1;
Z=zeros(n,1);
for i=1:365:m
    Z(j,1)=mean(tmax_WG(i:i+365-1,1));
    j=j+1;
end

% calculate the ratio of yearly Tmax between those after FFT and
% generated by weather generator
for i=1:n
    tmax_ratio(i,1)=tmax_FFT(i,1)-Z(i,1);
end

% extend the yearly Tmax ratio to daily scale,the data in each
% year are the same
tmax_extent=zeros(m,1);
j=1;
for i=1:365:m
    tmax_extent(i:i+365-1,1)=tmax_ratio(j,1);
    j=j+1;
end

% adjust the daily Tmax generated by WG using above ratios
tmax_adjust=zeros(size(tmax_WG));
for i=1:m
    tmax_adjust(i,1)=tmax_WG(i,1)+tmax_extent(i,1);
end
yearly_corrected_tmax=tmax_adjust;
save('yearly_corrected_tmax','yearly_corrected_tmax')


function [W,H] = NNDSVD(A, k)
nSample = size(A, 1);
nFeature = size(A, 2);
k = k + 1;
% --------------------------------------------
[U, S, V] = svds(A, k);

% --------------------------------------------
W = zeros(nSample, k);
H = zeros(k, nFeature);
W(:,1) = sqrt(S(1,1))*U(:,1);
H(1,:) = sqrt(S(1,1))*V(:,1)';
for i = 2:k
    x = U(:,i);
    y = V(:,i);
    xp = pos(x); xn = neg(x);
    yp = pos(y); yn = neg(y);
    xpnorm = norm(xp); ypnorm = norm(yp); mp = xpnorm*ypnorm;
    xnnorm = norm(xn); ynnorm = norm(yn); mn = xnnorm*ynnorm;
    if mp > mn
        u = xp/xpnorm;
        v = yp/ypnorm;
        sigma = mp;
    else
        u = xn/xnnorm;
        v = yn/ynnorm;
        sigma = mn;
    end
    W(:,i) = sqrt(S(i,i)*sigma)*u;
    H(i,:) = sqrt(S(i,i)*sigma)*v';
end
W = W(:, 2:k);
H = H(2:k, :);
end

function [xp] = pos(x)
xp = (x >= 0).*x;
end

function [xn] = neg(x)
xn = (x < 0).*(-x);
end


clc;
clear;
close all;
restoredefaultpath;

sdpnalpath  = '../../SDPNAL+v1.0';
pgdpath     = '../STRIDE';
utilspath   = '../utils';

addpath(genpath(pwd));
addpath(genpath(utilspath))
%% construct space of n1 x n2 hankel matrices (n1 <= n2)
n1 = 20;
n2 = 20;
[S,k,Scell] = hankel_struct(n1,n2);
% generate random hankel matrix
u1 = randn(k,1);
U1 = applyAffineMapCell(Scell,u1);

%% Convert the nearest hankel matrix problem to SDP
addpath(genpath(pgdpath));
SDP     = nearest_hankel_sdp(S,u1);
fprintf('SDP size: n = %d, m = %d.\n\n\n',SDP.n,SDP.m);
rmpath(genpath(pgdpath));

%% Optimistic initialization using SLRA
s.m             = n1;
s.n             = n2;
tol             = 0;
[zhtls,Hu]      = htls(s.m-1,u1,'linear'); % good initialization
opts.Rini       = zhtls';
opts.maxiter    = 5000;
opts.tol        = tol;
opts.epsrel     = tol;
opts.epsabs     = tol;
opts.epsgrad    = tol;
opts.method     = 'll'; % 'll': Levenberg–Marquardt, 'qb': BFGS
[uslra, info]   = slra(u1, s, s.m-1, opts);
zslra           = info.Rh'; zslra = zslra/norm(zslra);
Uslra           = applyAffineMapCell(Scell,uslra);
ztUnorm         = norm(zslra'*Uslra); % measure of rank deficientness
fprintf('SLRA: norm(zt*U) = %3.2e.\n',ztUnorm);
% lift to SDP solution
xtld            = kron([uslra;1],zslra);
X0              = {xtld * xtld'};

%% STRIDE
addpath(genpath(pgdpath))
pgdopts.pgdStepSize     = 10;
pgdopts.maxiterSGS      = 300;
pgdopts.maxiterLBFGS    = 1000;
pgdopts.SDPNALpath      = sdpnalpath;
pgdopts.tolADMM         = 5e-5;
pgdopts.phase1          = 1;
pgdopts.rrOpt           = 1:3;
pgdopts.lbfgsmemory     = 10;
pgdopts.tolPGD          = 1e-8;
pgdopts.rrFunName       = 'rr_hankel';
rrPar.m = n1; rrPar.n = n2; rrPar.k = k; rrPar.theta = u1; rrPar.S = S;
pgdopts.rrPar           = rrPar;
[outPGD,Xopt,yopt,Sopt] = PGDSDP(SDP.blk, SDP.At, SDP.b, SDP.C, X0, pgdopts);
f_sdp                   = outPGD.pobj;
time_pgd                = outPGD.totaltime;

[z,u,f_est]     = recover_solution(Xopt,SDP.C,n1,k);
eta             = abs(f_est-f_sdp) / (1+abs(f_est)+abs(f_sdp)); % relative suboptimality gap
U               = applyAffineMapCell(Scell,u);
ztUnorm         = norm(z'*U); % measure of rank deficientness
fprintf('norm(zt*U) = %3.2e, eta = %3.2e.\n',ztUnorm,eta);

rmpath(genpath(pgdpath))
fprintf('\n\n\n\n\n')


%%
% Test for recovering a 2-D metric field over the a*b* plane of the L*a*b*
% domain that fits the ciede2000 perceptual metric.

addpath('../toolbox/');
addpath('../colors_functions/');
addpath('../image_blur/');
addpath('../blur_functions//');
addpath('../convolutional_wasserstein/');
% addpath('../../data/images/colors/'); % low-res images
addpath('../../data/images/colors-big/'); % high-res images

rep = '../results/color-metric/';
if not(exist(rep))
    mkdir(rep);
end

%%
% L in [0,100], A in [-85,100], B in [-107,95]
% base fixed luminance for the metric
L = 50;

%%
% helpers

mmin = @(x)min(x(:));
mmax = @(x)max(x(:));
%
eta = .001;
DiffDir = @(ab,h)deltaE2000([L ab],[L ab]+eta*[0 h])/eta;

%%
% test

ab = [3 -80];
h = [1 1];
d = DiffDir(ab,h);

%%
% fit a single metric

M = fit_metric(@(h)DiffDir(ab,h));

%%
% Display a comparison between metric and fit.

P = 100;
tlist = linspace(0,2*pi,P+1)'; tlist(end)=[];
EuclMetric = @(M,h)sqrt( h*M*h' );

D = arrayfun(@(h1,h2)DiffDir(ab,[h1 h2]), cos(tlist), sin(tlist));
D1 = arrayfun(@(h1,h2)EuclMetric(M,[h1 h2]), cos(tlist), sin(tlist));

clf; hold on;
plot( cos(tlist).*D, sin(tlist).*D, 'b' );
plot( cos(tlist).*D1, sin(tlist).*D1, 'r:' );
axis equal;

%%
% compute some image histogram

name = 'blue-7';
n = 512;
f = rescale( load_image(name, n) );
A = colorspace(['RGB->LAB'], f);
% compute histo
N = 60;
H = compute_histogram_2d(A(:,:,2),A(:,:,3),N);
delta = .0001;
% display
clf;
imageplot(log(H+delta));

%%
% Compute a field of metrics
% A in [-85,100], B in [-107,95]

% background color image
v = 100;
a = linspace(-v,v,N);
b = linspace(-v,v,N);
[B,A] = meshgrid(b,a);
L = 80*ones(N,N);
CM = colorspace('LAB->RGB', cat(3,L,A,B));
% metric
F = fit_metric_field(DiffDir, arange,brange,N);
% inverse metric
[e1,e2,l1,l2] = perform_tensor_decomp(F);
G = perform_tensor_recomp(e1,e2,1./l1,1./l2);

options.sub = 2;
clf;
h = plot_tensor_field(F, CM, options);
saveas(gcf, [rep name '-cie-metric.png'], 'png');
clf;
h = plot_tensor_field(G, CM, options);
saveas(gcf, [rep name '-cie-inverse.png'], 'png');


clear all
n = 0:40;
h = exp(-0.1*n);
x = exp(-0.2*n);
y = conv(h,x);
subplot(3,1,1);stem(h);title("h")
subplot(3,1,2);stem(x);title("x")
subplot(3,1,3);stem(y);title("y")


% Example of fixed lag smoothing

rand('state', 1);
S = 2;
O = 2;
T = 7;
data = sample_discrete([0.5 0.5], 1, T);
transmat = mk_stochastic(rand(S,S));
obsmat = mk_stochastic(rand(S,O));
obslik = multinomial_prob(data, obsmat);
prior = [0.5 0.5]';

[alpha0, beta0, gamma0, ll0, xi0] = fwdback(prior, transmat, obslik);

w = 3;
alpha1 = zeros(S, T);
gamma1 = zeros(S, T);
xi1 = zeros(S, S, T-1);
t = 1;
b = obsmat(:, data(t));
olik_win = b; % window of conditional observation likelihoods
alpha_win = normalise(prior .* b);
alpha1(:,t) = alpha_win;
for t=2:T
  [alpha_win, olik_win, gamma_win, xi_win] = ...
      fixed_lag_smoother(w, alpha_win, olik_win, obsmat(:, data(t)), transmat);
  alpha1(:,max(1,t-w+1):t) = alpha_win;
  gamma1(:,max(1,t-w+1):t) = gamma_win;
  xi1(:,:,max(1,t-w+1):t-1) = xi_win;
end

e = 1e-1;
%assert(approxeq(alpha0, alpha1, e));
assert(approxeq(gamma0(:, T-w+1:end), gamma1(:, T-w+1:end), e));
assert(approxeq(xi0(:,:,T-w+1:end), xi1(:,:,T-w+1:end), e));


function test_tbgppca_subset

data = tbload_temperature;
data = tbpreprocess(data);
data = tbsubset(data, 1:1:79, 1:10:89000);

data

% $$$ figure
% $$$ plot(data.observations');

inW = data.coordinates(:,1:2)';
inX = data.time';

[M,N] = size(data.observations);
D = 2;

% Covariance matrices
logthetaW = {[log(1);log(5e1)], [log(1);log(1e1)]}
logthetaX = {log(2000), log(100)};

% Covariance functions
gpcovW = {@gpcovScale, @(logtheta,x1,x2) gpcov(logtheta,x1,x2,@sqdistEarth)};
% $$$ pseudoW = cell(D,1);%zeros([dimW,Mp,D]);
% $$$ Mp = ceil(1*M);
% $$$ for d=1:D
% $$$   permM = randperm(M);
% $$$   pseudoW{d} = inW(:,permM(1:Mp));% + randn(2,Mp);
% $$$ end

gpcovX = @gpcov;

% Generate latent variables
for d=1:D
  covfunW{d} = gpcovW;
  covfunX{d} = gpcovX;
end

Y = data.observations;

% Learn GP PCA
maxiter = 3e1;
Q = vbgppcamv(Y,D,inW,inX,covfunW,logthetaW,covfunX,logthetaX, ...
              'maxiter',maxiter, 'pseudodensityx', 0.01, 'pseudodensityw', ...
              0.4, 'loglikelihood', true, 'updatehyper', 5, 'maxsearchx', ...
              3, 'maxsearchw', 50);

% $$$ % Learn other models
% $$$ if ppca
% $$$   Qppca = pca_full(Y,D,'maxiters',maxiter,'rotate2pca',true, ...
% $$$                    'algorithm','ppca');
% $$$   Yh_ppca = Qppca.A * Qppca.S + repmat(Qppca.Mu,1,N);
% $$$   testrmse_ppca = rmse(Ytest, Yh_ppca)
% $$$   noiselessrmse_ppca = rmse(Y, Yh_ppca)
% $$$ end
% $$$ if vbpca
% $$$   Qvbpca = vbpcamv(Ynm,M-1,'maxiters',maxiter);
% $$$   Yh_vbpca = Qvbpca.W * Qvbpca.X + repmat(Qvbpca.mu,1,N);
% $$$   testrmse_vbpca = rmse(Ytest, Yh_vbpca)
% $$$   noiselessrmse_vbpca = rmse(Y, Yh_vbpca)
% $$$ end

Yh_gppca = Q.W * Q.X;
rmse_gppca = rmse(Y, Yh_gppca)
%testrmse_gppca = rmse(Ytest, Yh_gppca)
%noiselessrmse_gppca = rmse(Y, Yh_gppca)

figure
subplot(5,1,1);
plot(Y');
title('Original data')
% $$$ subplot(5,1,2);
% $$$ plot(Ynm');
% $$$ title('Observed data')
subplot(5,1,3);
plot(Yh_gppca');
title('Reconstruction of GP VB PCA')
% $$$ if vbpca
% $$$   subplot(5,1,4);
% $$$   plot(Yh_vbpca');
% $$$   title('Reconstruction of VB PCA')
% $$$ end
% $$$ if ppca
% $$$   subplot(5,1,5);
% $$$   plot(Yh_ppca');
% $$$   title('Reconstruction of PPCA')
% $$$ end

vX = Q.varX;%diag(Q.CovX);
eX = 2*sqrt(vX);%sqrt( vX(reshape(1:(N*D), D, N)) );
vW = Q.varW;%diag(Q.CovW);
eW = 2*sqrt(vW);%sqrt( vW(reshape(1:(M*D), M, D)) );
tsgpplot(inX, Q.X', eX', 'pseudoinputs', {Q.pseudoX});

%tsgpplot(inW, Q.W, eW, 'pseudoinputs', {Q.pseudoW});
figure
for d=1:D
  subplot(D,1,d);
  mapproj('testbed');
  mapplot(inW(1,:),inW(2,:),'ro');
  hold on
  mapplot(Q.pseudoW{d}(1,:),Q.pseudoW{d}(2,:),'k+');
  mapcoast
end

figure
mapproj('testbed');
gpmapcolor(Q.pseudoW, Q.Wp, Q.CovWp, 22:0.1:27, 59:0.1:61, Q.logthetaW, covfunW);


function T_hat = run_tc(params)
  T = params.T;
  Omega = params.Idx;

  %Omega = find(Idx);
  %Ak = T(Omega);

  %% ====================== Load data ==============================
  normalize              =        max(T(:))                     ;
  Xn                     =        T/normalize                   ;
  [n1,n2,n3]             =        size(Xn)                      ;

  %p                      =        0.3                           ;
  %Omega                  =        zeros(size(Xn))               ;
  %chosen                 =        randperm(n1*n2*n3,...
  %                                       round(p*n1*n2*n3))     ;
  %Omega(chosen)          =        1                             ;

  alpha                  =        1.05                             ;
  maxItr                 =        100                          ; % maximum iteration
  rho                    =        0.01                          ;

  A                      =        diag(sparse(double(Omega(:)))); % sampling operator
  b                      =        A * Xn(:)                     ; % available data
  bb                     =        reshape(b,[n1,n2,n3]);

  %% ================ main process of completion =======================
  [X] = LtSVD_TC(A ,b,rho,alpha ,[n1,n2,n3],maxItr, Xn(:), false, false);
  X                      =        X * normalize                 ;
  T_hat                  =        reshape(X,[n1,n2,n3])         ;

  %X_dif                  =        T-X                           ;
  %RSE                    =        norm(X_dif(:))/norm(T(:))     ;
end


function [filt, B, A] = ft_preproc_bandpassfilter(dat, Fs, Fbp, order, type, dir, instabilityfix, df, wintype, dev, plotfiltresp, usefftfilt)

% FT_PREPROC_BANDPASSFILTER applies a band-pass filter to the data and thereby
% removes the spectral components in the data except for the ones in the
% specified frequency band.
%
% Use as
%   [filt] = ft_preproc_bandpassfilter(dat, Fs, Fbp, order, type, dir, instabilityfix, df, wintype, dev, plotfiltresp, usefftfilt)
% where
%   dat             data matrix (Nchans X Ntime)
%   Fs              sampling frequency in Hz
%   Fbp             frequency band, specified as [Fhp Flp] in Hz
%   order           optional filter order, default is 4 (but) or dependent on frequency band and data length (fir/firls)
%   type            optional filter type, can be
%                     'but'       Butterworth IIR filter (default)
%                     'firws'     FIR filter with windowed sinc
%                     'fir'       FIR filter using MATLAB fir1 function
%                     'firls'     FIR filter using MATLAB firls function (requires MATLAB Signal Processing Toolbox)
%                     'brickwall' frequency-domain filter using forward and inverse FFT
%   dir             optional filter direction, can be
%                     'onepass'                   forward filter only
%                     'onepass-reverse'           reverse filter only, i.e. backward in time
%                     'onepass-zerophase'         zero-phase forward filter with delay compensation (default for firws, linear-phase symmetric FIR only)
%                     'onepass-reverse-zerophase' zero-phase reverse filter with delay compensation
%                     'onepass-minphase'          minimum-phase converted forward filter (non-linear, only for firws)
%                     'twopass'                   zero-phase forward and reverse filter (default, except for firws)
%                     'twopass-reverse'           zero-phase reverse and forward filter
%                     'twopass-average'           average of the twopass and the twopass-reverse
%   instabilityfix  optional method to deal with filter instabilities
%                     'no'       only detect and give error (default)
%                     'reduce'   reduce the filter order
%                     'split'    split the filter in two lower-order filters, apply sequentially
%   df              optional transition width (only for firws)
%   wintype         optional window type (only for firws), can be
%                     'hamming' (default)    maximum passband deviation 0.0022 [0.22%], stopband attenuation -53dB
%                     'hann'                 maximum passband deviation 0.0063 [0.63%], stopband attenuation -44dB
%                     'blackman'             maximum passband deviation 0.0002 [0.02%], stopband attenuation -74dB
%                     'kaiser'
%   dev             optional max passband deviation/stopband attenuation (only for firws with kaiser window, default = 0.001 [0.1%, -60 dB])
%   plotfiltresp    optional, 'yes' or 'no', plot filter responses (only for firws, default = 'no')
%   usefftfilt      optional, 'yes' or 'no', use fftfilt instead of filter (only for firws, default = 'no')
%
% Note that a one- or two-pass filter has consequences for the strength of the
% filter, i.e. a two-pass filter with the same filter order will attenuate the signal
% twice as strong.
%
% Further note that the filter type 'brickwall' filters in the frequency domain,
% but may have severe issues. For instance, it has the implication that the time
% domain signal is periodic. Another issue pertains to that frequencies are
% not well defined over short time intervals; particularly for low frequencies.
%
% If the data contains NaNs, these will affect the output. With an IIR
% filter, and/or with FFT-filtering, local NaNs will spread to the whole
% time series. With a FIR filter, local NaNs will spread locally, depending
% on the filter order.
%
% See also PREPROC

% Copyright (c) 2003-2022, Robert Oostenveld, Arjen Stolk, Andreas Widmann,
% Jan-Mathijs Schoffelen
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

% set the default filter order later
if nargin<4 || isempty(order)
  order = [];
end

% set the default filter type
if nargin<5 || isempty(type)
  type = 'but';
end

% set the default filter direction
if nargin<6 || isempty(dir)
  if strcmp(type, 'firws')
    dir = 'onepass-zerophase';
  else
    dir = 'twopass';
  end
end

% set the default method to deal with filter instabilities
if nargin<7|| isempty(instabilityfix)
  instabilityfix = 'no';
end

% Set default transition width later
if nargin < 8 || isempty(df)
  df = [];
end

% Set default window type
if nargin < 9 || isempty(wintype)
  wintype = 'hamming';
end

% Set default passband deviation/stopband attenuation for Kaiser window
if nargin < 10 || isempty(dev)
  if strcmp(wintype, 'kaiser')
    dev = 0.001;
  else
    dev = [];
  end
end

% Set default passband deviation/stopband attenuation for Kaiser window
if nargin < 11 || isempty(plotfiltresp)
  plotfiltresp = 'no';
end

% Set default filter function
if nargin < 12 || isempty(usefftfilt)
  usefftfilt = false;
else
  % convert to boolean value
  usefftfilt = istrue(usefftfilt);
end

% Filtering does not work on integer data
if ~isa(dat, 'double') && ~isa(dat, 'single')
  dat = cast(dat, 'double');
end

% preprocessing fails on channels that contain NaN
if any(isnan(dat(:)))
  ft_warning('FieldTrip:dataContainsNaN', 'data contains NaN values');
end

% Nyquist frequency
Fn = Fs/2;

% demean the data before filtering
meandat = nanmean(dat,2);
dat = bsxfun(@minus, dat, meandat);

% compute filter coefficients
switch type
  case 'but'
    if isempty(order)
      order = 4;
    end
    [B, A] = butter(order, [min(Fbp)/Fn max(Fbp)/Fn]);

  case 'firws'
    % Input arguments
    if length(Fbp) ~= 2
      ft_error('Two cutoff frequencies required.')
    end

    % Filter order AND transition width set?
    if ~isempty(order) && ~isempty(df)
      ft_warning('firws:dfOverridesN', 'Filter order AND transition width set - transition width setting will override filter order.')
    elseif isempty(order) && isempty(df) % Default transition width heuristic
      df = fir_df(Fbp, Fs);
    end

    % Compute filter order from transition width
    [foo, maxDf] = fir_df(Fbp, Fs); %#ok<ASGLU>
    isOrderLow = false;
    if ~isempty(df)
      if df > maxDf
        ft_error('Transition band too wide. Maximum transition width is %.2f Hz.', maxDf)
      end
      [order, dev] = firwsord(wintype, Fs, df, dev);
    else % Check filter order otherwise
      [df, dev] = invfirwsord(wintype, Fs, order, dev);
      if df > maxDf
        nOpt = firwsord(wintype, Fs, maxDf, dev);
        ft_warning('firws:filterOrderLow', 'Filter order too low. For better results a minimum filter order of %d is recommended. Effective cutoff frequency might deviate from requested cutoff frequency.', nOpt)
        isOrderLow = true;
      end
    end

    % Window
    if strcmp(wintype, 'kaiser')
      beta = kaiserbeta(dev);
      win = windows('kaiser', order + 1, beta);
    else
      win = windows(wintype, order + 1);
    end

    % Impulse response
    Fbp = sort(Fbp);
    B = firws(order, sort(Fbp) / Fn, 'band', win);
    A = 1;

    % Convert to minimum phase
    if strcmp(dir, 'onepass-minphase')
      B = minphaserceps(B);
    end

    % Twopass filtering
    if strncmp(dir, 'twopass', 7)
      pbDev = (dev + 1)^2 - 1;
      sbAtt = 20 * log10(dev^2);
      order = 2 * (length(B) - 1);
      isTwopass = true;
    else
      pbDev = dev;
      sbAtt = 20 * log10(dev);
      order = length(B) - 1;
      isTwopass = false;
    end

    % Reporting
    ft_info once
    ft_info('Bandpass filtering data: %s, order %d, %s-windowed sinc FIR\n', dir, order, wintype);
    if ~isTwopass && ~isOrderLow % Do not report shifted cutoffs
      ft_info('  cutoff (-6 dB) %g Hz and %g Hz\n', Fbp(1), Fbp(2));
      tb = [max([Fbp(1) - df / 2 0]), Fbp(1) + df / 2, Fbp(2) - df / 2, min([Fbp(2) + df / 2 Fn])]; % Transition band edges
      ft_info('  transition width %.1f Hz, stopband 0-%.1f Hz, passband %.1f-%.1f Hz, stopband %.1f-%.0f Hz\n', df, tb, Fn);
    end
    if ~isOrderLow
      ft_info('  maximum passband deviation %.4f (%.2f%%), stopband attenuation %.0f dB\n', pbDev, pbDev * 100, sbAtt);
    end

  case 'fir'
    if isempty(order)
      order = 3*fix(Fs / Fbp(1));
    end
    if order > floor( (size(dat,2) - 1) / 3)
      order=floor(size(dat,2)/3) - 1;
    end
    B = fir1(order, [min(Fbp)/Fn max(Fbp)/Fn]);
    A = 1;

  case 'firls' % from NUTMEG's implementation
    % Deprecated: see bug 2453
    ft_warning('The filter type you requested is not recommended for neural signals, only proceed if you know what you are doing.')
    if isempty(order)
      order = 3*fix(Fs / Fbp(1));
    end
    if order > floor( (size(dat,2) - 1) / 3)
      order=floor(size(dat,2)/3) - 1;
    end
    f = 0:0.001:1;
    if rem(length(f),2)~=0
      f(end)=[];
    end
    z = zeros(1,length(f));
    if(isfinite(min(Fbp)))
      [val,pos1] = min(abs(Fs*f/2 - min(Fbp)));
    else
      [val,pos2] = min(abs(Fs*f/2 - max(Fbp)));
      pos1=pos2;
    end
    if(isfinite(max(Fbp)))
      [val,pos2] = min(abs(Fs*f/2 - max(Fbp)));
    else
      pos2 = length(f);
    end
    z(pos1:pos2) = 1;
    A = 1;
    B = firls(order,f,z); % requires MATLAB signal processing toolbox

  case 'brickwall'
    ix = round((0:Fs) ./ (Fs ./ size(dat,2)));
    ax = ix ./ (size(dat,2)./Fs); % frequency axis, including the other end of the spectrum

    a    = ones(1, size(dat,2));
    fbin1 = nearest(ax, [min(Fbp)    max(Fbp)]);
    fbin2 = nearest(ax, [Fs-max(Fbp) Fs-min(Fbp)]); % same band at the other end of the spectrum

    a(1:(fbin1(1)-1))            = 0;
    a((fbin1(2)+1):(fbin2(1)-1)) = 0;
    a((fbin2(2)+1):end)          = 0;

    f    = fft(dat,[],2);             % FFT
    f    = f.*a(ones(size(dat,1),1),:); % brickwall
    filt = real(ifft(f,[],2));        % iFFT

  otherwise
    ft_error('unsupported filter type "%s"', type);
end

% Plot filter responses
if strcmp(plotfiltresp, 'yes')
  plotfresp(B, A, [], Fs, dir)
end

if ~isequal(type, 'brickwall')
  try
    filt = filter_with_correction(B,A,dat,dir,usefftfilt);
  catch
    switch instabilityfix
      case 'no'
        rethrow(lasterror);
      case 'reduce'
        ft_warning('off','backtrace');
        ft_warning('instability detected - reducing the %dth order filter to an %dth order filter', order, order-1);
        ft_warning('on','backtrace');
        filt = ft_preproc_bandpassfilter(dat,Fs,Fbp,order-1,type,dir,instabilityfix,df,wintype,dev,plotfiltresp,usefftfilt);
      case 'split'
        N1 = ceil(order/2);
        N2 = floor(order/2);
        ft_warning('off','backtrace');
        ft_warning('instability detected - splitting the %dth order filter in a sequential %dth and a %dth order filter', order, N1, N2);
        ft_warning('on','backtrace');
        filt = ft_preproc_bandpassfilter(dat ,Fs,Fbp,N1,type,dir,instabilityfix,df,wintype,dev,plotfiltresp,usefftfilt);
        filt = ft_preproc_bandpassfilter(filt,Fs,Fbp,N2,type,dir,instabilityfix,df,wintype,dev,plotfiltresp,usefftfilt);
      otherwise
        ft_error('incorrect specification of instabilityfix');
    end % switch
  end
end


function demoami
% DEMOAMI demos function ami for bivariate time series data
%
% USAGE:
%       demoami
%
% INPUT:
%
% OUTPUT:
%      amis:   vector of average mutual information for time lags of 0:nLags
%     corrs:   vector of correlation (or autocorrelation for univariate) for
%              time lags of 0:nLags
%
% EXAMPLES:
%
% See also AMI, PROBXY, RHIST

% Copyright 2004-2005 by Durga Lal Shrestha.
% eMail: durgals@hotmail.com
% $Date: 2005/06/27
% $Revision: 1.1.0 $ $Date: 2005/07/01 $

% ***********************************************************************
clear all
close all
clc
mydata = load('data.txt')';
lag = 25;
nBins = [15 15];
[iy ry] = ami(mydata,nBins,lag);


function [Y,Xpad] = kuwahara(X,WINSZ,progress)
% [Y,Xpad] = KUWAHARA(X[,WINSZ][,progress])
% perform kuwahara nonlinear edge-preserving filtering on an intensity
% image
%
% * If no window size WINSZ is specified, the default is 5.
% * Setting progress to a nonzero value causes KUWAHARA to display
%   the current row it is processing.
%
% Description:
% The Kuwahara filter works on a window divided into 4 overlapping
% subwindows (typically 5x5 pixels, see below). In each subwindow, the mean and
% variance are computed. The output value (located at the center of the
% window) is set to the mean of the subwindow with the smallest variance.
%
%    ( a  a  ab   b  b)
%    ( a  a  ab   b  b)
%    (ac ac abcd bd bd)
%    ( c  c  cd   d  d)
%    ( c  c  cd   d  d)
%
% Notes:
% Image is converted to double format for processing.
%
% References:
% http://www.incx.nec.co.jp/imap-vision/library/wouter/kuwahara.html
%
% Copyright Art Barnes, 2005 artbarnes<at>ieee<dot>org

if nargin >= 3
    verboseFlag = true;
else
    verboseFlag = false;
end

if nargin < 2
    WINSZ = 5;
end

if ~isa(X,'double')
    X = im2double(X);
end

PADDING = floor(WINSZ/2);

Xpad = padarray(X,[PADDING PADDING],'replicate');
[padRows,padCols] = size(Xpad);
Y = zeros(size(X));

nRowIters = length((PADDING+1):(padRows-PADDING));
count = 1;
for i = (PADDING+1):(padRows-PADDING)
    for j = (PADDING+1):(padCols-PADDING)
        % window & subwindows
        W = Xpad((i-PADDING):(i+PADDING),(j-PADDING):(j+PADDING));
        Wnw = W(1:(PADDING+1),1:(PADDING+1));
        Wne = W(1:(PADDING+1),(PADDING+1):WINSZ);
        Wsw = W((PADDING+1):WINSZ,1:(PADDING+1));
        Wse = W((PADDING+1):WINSZ,(PADDING+1):WINSZ);

        % find the variances
        s = var([Wnw(:) Wne(:) Wsw(:) Wse(:)]);
        m = mean([Wnw(:) Wne(:) Wsw(:) Wse(:)]);
        [y,k] = min(s);

        % assign the mean of the subwindow with the least variance to
        % the center pixel
        Y(i,j) = m(k);
    end

    if verboseFlag
        fprintf('Kuwahara: %d/%d\n',count,nRowIters);
        count = count + 1;
    end
end


function [ a, ifact ] = i4vec_red ( n, a, incx )

%*****************************************************************************80
%
%% I4VEC_RED divides out common factors in an I4VEC.
%
%  Discussion:
%
%    If A is a simple vector, then it has dimension N.
%
%    If A is a row of a matrix, then INCX will not be 1, and
%    the actual dimension of A is at least 1+(N-1)*INCX.
%
%    On output, the entries of A have no common factor
%    greater than 1.
%
%    If A is a simple vector, then INCX is 1, and we simply
%    check the first N entries of A.
%
%    If A is a row of a matrix, then INCX will be the number
%    of rows declared in the matrix, in order to allow us to
%    "skip" along the row.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Input, integer A(*), the vector to be reduced.
%
%    Input, integer INCX, the distance between successive
%    entries of A that are to be checked.
%
%    Output, integer A(*), the reduced vector.
%
%    Output, integer IFACT, the common factor that was divided out.
%

%
%  Find the smallest nonzero value.
%
  ifact = 0;
  indx = 1;

  for i = 1 : n

    if ( a(indx) ~= 0 )

      if ( ifact == 0 )
        ifact = abs ( a(indx) );
      else
        ifact = min ( ifact, abs ( a(indx) ) );
      end

    end

    indx = indx + incx;

  end

  if ( ifact == 0 )
    return
  end
%
%  Find the greatest common factor of the entire vector.
%
  indx = 1;
  for i = 1 : n
    ifact = i4_gcd ( a(indx), ifact );
    indx = indx + incx;
  end

  if ( ifact == 1 )
    return
  end
%
%  Divide out the common factor.
%
  indx = 1;
  for i = 1 : n
    a(indx) = a(indx) / ifact;
    indx = indx + incx;
  end

  return
end


%% Toolox Image - A Toolbox for General Purpose Image Processing
%
% Copyright (c) 2008 Gabriel Peyre
%

%%
% The toolbox can be downloaded from Matlab Central
% http://www.mathworks.com/matlabcentral/fileexchange/loadFile.do?objectId=16201&objectType=FILE

%%
% This includes in the path some additional useful scripts.
path(path, 'toolbox/');

%% Image loading and displaying

%%
% The |load_image| function contains several synthetic examples and can
% also load arbitrary images. You can provide the size of the image, and
% you can load batch of images using cell arrays. Optional parameters for
% the synthetic images are given using the |options| field.

options.radius = .3;
namelist = {'lena' 'disk' 'chessboard' 'phantom'};
Mlist = load_image(namelist, 256, options);

%%
% The |imageplot| function allows to display sets of images together and
% link the axes. It handles color images.

clf;
imageplot(Mlist,namelist, 2,2);

%% Image Manipulation

%%
%  You can rotate an image.

M = load_image('lena', 256);
Mlist = {};
for i=1:6
    Mlist{end+1} = perform_image_rotation(M,i*pi/8);
end
imageplot(Mlist);

%%
% You can (gaussian) blur an image.

M = load_image('lena', 256);
Mlist = {};
for i=1:6
    Mlist{end+1} = perform_blurring(M,i*4);
end
imageplot(Mlist);

%%
% You can crop and resize images.

M = load_image('lena');
M = crop(M,32);
Mi = perform_image_resize(M,[256 256]);
clf; imageplot({M Mi}, {'Original' 'Interpolated'});

%% Image Registration

%%
% You can perform image registration by first selecting two points on the image
% (here using clicking).

M = load_image('barb');
% extract the eyes
clf; imagesc([0,1],[0,1],M);
title('Click on each eye');
colormap gray(256);
axis image; axis off;
[x,y,b] = ginput(2);

%%
% Then a similitude is applied to register the eye position.

% position of the eye
u = [x(1) y(1)];
v = [x(2) y(2)];
% target position
u1 = [0.3,0.3];
v1 = [0.7,0.3];
% registration
M1 = perform_image_similitude(M,u,u1,v,v1);
% display
clf; imageplot({M M1}, {'Original' 'Registered'});

%% Spacially varying filter
% You can provide a set of filters, and for each pixel decide which filter
% to use (for instance according to local image information). The code is implemented with
% a mex C code, so it is quite fast. Here we show
% a simple foveated filtering, where the blurring is stronger in the center
% of the image.

n = 256;
M = load_image('lena', n);

%%
% We first create the set of filters
m = 41; % width of filers
p = 20; % number of filters
sigma = linspace(0.05,10,p);
H = zeros(m,m,p);
for i=1:p
    H(:,:,i) = compute_gaussian_filter([m m],sigma(i)/n,[n n]);
end

%%
% Then we compute an index map that tells which filter to use (foveation effect).
x = linspace(-1,1,n);
[Y,X] = meshgrid(x,x);
R = sqrt(X.^2 + Y.^2);
I = round(rescale(R,1,p));

%%
% At last, we launch the filtering, and display the result.

M1 = perform_adaptive_filtering(M,H,I);
imageplot({M M1},{'Original' 'Foveated'});

%% Median Filtering
% Median filter is effective to remove salt and pepper impulse noise.

n = 256;
M = load_image('lena');
M = rescale(crop(M, n));
% add noise at random locations
r = .3;  % 30% location corrupted
I = randperm(n^2); I = I(1:round(r*end));
M(I) = rand(size(I));
% do the filtering
M1 = perform_median_filtering(M,2);
% display
imageplot({M M1}, {'Original' 'Filtered'});

%% Line integral convolution
% The LIC algorithm filters an image along a vector field.

%%
% create a vector field (this is done using the functions of the diffc
% toolbox).

n = 256;    % size of the image
sigma = 60; % regularity of the vector field
options.bound = 'sym'; % boundary handling
v = perform_blurring(randn(n,n,2), sigma, options);
v = perform_vf_normalization(v);

%%
% Now we perform the LIC of an initial noise image along the flow.

M = randn(n);
% parameters for the LIC
options.histogram = 'linear'; % keep contrast fixed
options.verb = 0;
options.dt = 1.5; % time steping
% size of the features
options.flow_correction = 1;
options.niter_lic = 2; % several iterations gives better results
% iterated lic
Mlist = {};
for i=1:4
    options.M0 = M;
    Mlist{end+1} = perform_lic(v, i*4, options);
end
% display
clf; imageplot(Mlist,'',2,2);

%% TV denoising
% The Sparsity Toolbox implements TV denoising, that minimizes the total variation of
% the image.

%%
% |argmin_f |F-f|^2 + 2*lambda*TV(f)|

%%
% Where |F| is the noisy image and |f| is the optimized denoised image.
% |TV(f)| is the discrete total variation of the image. |lambda| controls
% how much denoising you want to perform.

%%
% It corresponds to the Rudin/Osher/Fatemi image denoiser
% (lagrangian formulation), and works well for images with strong edges
% (otherwise, wavelets performs better). The TV minimization is performed
% using the iterative fixed point algorihtm of Chambolle (published in
% JMIV 2002). A nice feature of the algorithm is that you can give him
% the target residual value (norm of the noise) in |options.etgt| and it will track it for
% you during the iterations, and will automatically select the correct |lambda| value.

%%
% First we load and image and make some noise.

n = 128;
M0 = load_image('lena',256);
M0 = rescale(crop(M0,n));
sigma = .1;
M = M0 + randn(size(M0))*sigma;
% display
clf; imageplot({clamp(M0) clamp(M)},{'Original' 'Noisy'});

%%
% We set the target residual error |options.etgt| just little larger than the noise level
% |n*sigma|, and run the algorithm.

% some parameter for the algorithm
options.verb = 0;
options.display = 0;
options.niter = 300;    % number of iterations
options.etgt = 1.1*sigma*n;
options.lambda = .3; % initial regularization
% now we perform the denoising
[Mtv,err,tv,lambda] = perform_tv_denoising(M,options);

%%
% We display the result.

clf; imageplot({clamp(M) clamp(Mtv)},{'Noisy' 'TV Denoised'});

%%
% We can keep track of the decay of |lambda| during the iteration and the
% Lagrangian decay.

clf;
subplot(2,1,1);
h = plot(lambda); axis tight;
set(h, 'LineWidth', 2);
set(gca, 'FontSize', 20);
title('Evolution of \lambda');
subplot(2,1,2);
lagr = .5*err.^2 + lambda(end)*tv;
h = plot(lagr); axis tight;
set(h, 'LineWidth', 2);
set(gca, 'FontSize', 20);
title('Evolution of |y-U x|^2+\lambda |x|_1');
axis([1 options.niter min(lagr) min(lagr)*1.3]);


function [ value_min, value_mean, value_max, value_var ] = ...
  tet_mesh_quality2 ( node_num, node_xyz, tetra_order, tetra_num, ...
  tetra_node )

%*****************************************************************************80
%
%% TET_MESH_QUALITY2 returns a tet mesh quality factor.
%
%  Discussion:
%
%    The tet mesh quality measure is the minimum of the
%    corresponding tetrahedron quality measure, over all tetrahedrons in the
%    tet mesh.
%
%    This routine is designed for an order-4 tet mesh.  Order 10 tet meshes
%    may be input, but the extra nodes are ignored.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 October 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, real NODE_XYZ(3,NODE_NUM), the nodes.
%
%    Input, integer TETRA_ORDER, the order of the tetrahedrons.
%
%    Input, integer TETRA_NUM, the number of tetrahedrons.
%
%    Input, integer TETRA_NODE(TETRA_ORDER,TETRA_NUM), the indices of the nodes
%    that make up the tetrahedrons.
%
%    Output, real VALUE_MIN, VALUE_MEAN, VALUE_MAX, VALUE_VAR,
%    the minimum, mean, maximum and variance of the quality measure.
%
  dim_num = 3;

  for tetra = 1 : tetra_num

    tetrahedron(1:dim_num,1:4) = node_xyz(1:dim_num,tetra_node(1:4,tetra));

    tetrahedron_quality(tetra) = tetrahedron_quality2_3d ( tetrahedron );

  end

  value_max  = max  ( tetrahedron_quality(1:tetra_num) );
  value_min  = min  ( tetrahedron_quality(1:tetra_num) );
  value_mean = mean ( tetrahedron_quality(1:tetra_num) );
  value_var  = var  ( tetrahedron_quality(1:tetra_num) );

  return
end


function [numRows numColumns area] = BoxSize(bbox)
% [numRows numColumns Surface] = BoxSize(bbox)
%
% Retrieves number of rows, columns, and surface area from bounding box
%
% bbox:         4 x N Bounding box as [rowBegin colBegin rowEnd colEnd]
%
% numRows:      Number of rows of boxes
% numColumns:   Number of columns of boxes
% area:         Area of boxes
%
%     Jasper Uijlings - 2013

% Box is empty
if isempty(bbox)
    numRows = 0;
    numColumns = 0;
    area = 0;
    return
end

numRows = bbox(:,3) - bbox(:,1) + 1;
numColumns = bbox(:,4) - bbox(:,2) + 1;
area = numRows .* numColumns;


% VL_QUICKSHIFT Quick shift image segmentation
%   Quick shift is a mode seeking algorithm which links each pixel to
%   its nearest neighbor which has an increase in the estimate of the
%   density. These links form a tree, where the root of the tree is
%   the pixel which correspond to the highest mode in the image.
%
%   [MAP,GAPS] = VL_QUICKSHIFT(I, KERNELSIZE, MAXDIST) computes quick shift on the
%   image I. KERNELSIZE is the bandwidth of the Parzen window estimator of
%   the density. Since searching over all pixels for the nearest
%   neighbor which increases the density would be prohibitively
%   expensive, MAXDIST controls the maximum L2 distance between neighbors
%   that should be linked. MAP and GAP represent the resulting forest
%   of trees. They are array of the same size of I.  Each element
%   (pixel) of MAP is and index to the parent elemen in the forest and
%   GAP contains the corresponding branch length. Pixels which are at
%   the root of their respective tree have MAP(x) = x and GAPS(x) =
%   inf.
%
%   [MAP,GAPS,E] = VL_QUICKSHIFT(I, KERNELSIZE, MAXDIST) also returns the estimate
%   of the density E.
%
%   [MAP,GAPS] = VL_QUICKSHIFT(I, KERNELSIZE) uses a default MAXDIST of 3 * KERNELSIZE.
%
%   Notes::
%     The distance between pixels is always measured in image
%     coordinates (not normalized), so the importance of the color
%     component should be weighted accordingly before calling this
%     function.
%
%   Options:
%
%   Verbose::
%     Toggles verbose output.
%
%   Medoid::
%     Run medoid shift instead of quick shift.
%
%   See also: VL_HELP().

% Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).


function p = bspeval(d,c,k,u)
% BSPEVAL  Evaluate B-Spline at parametric points
% -------------------------------------------------------------------------
% ADAPTATION of BSPEVAL from C Routine
% -------------------------------------------------------------------------
%
% Calling Sequence:
%
%   p = bspeval(d,c,k,u)
%
%    INPUT:
%
%       d - Degree of the B-Spline.
%       c - Control Points, matrix of size (dim,nc).
%       k - Knot sequence, row vector of size nk.
%       u - Parametric evaluation points, row vector of size nu.
%
%    OUTPUT:
%
%       p - Evaluated points, matrix of size (dim,nu)
%

nu = numel(u);
[mc,nc] = size(c);
                                                %   int bspeval(int d, double *c, int mc, int nc, double *k, int nk, double *u,int nu, double *p){
                                                %   int ierr = 0;
                                                %   int i, s, tmp1, row, col;
                                                %   double tmp2;
                                                %
                                                %   // Construct the control points
                                                %   double **ctrl = vec2mat(c,mc,nc);
                                                %
                                                %   // Contruct the evaluated points
p = zeros(mc,nu);                               %   double **pnt = vec2mat(p,mc,nu);
                                                %
                                                %   // space for the basis functions
N = zeros(d+1,1);                               %   double *N = (double*) mxMalloc((d+1)*sizeof(double));
                                                %
                                                %   // for each parametric point i
for col=1:nu                                    %   for (col = 0; col < nu; col++) {
                                                %     // find the span of u[col]
    s = findspan(nc-1, d, u(col), k);           %     s = findspan(nc-1, d, u[col], k);
    N = basisfun(s,u(col),d,k);                 %     basisfun(s, u[col], d, k, N);
                                                %
    tmp1 = s - d + 1;                           %     tmp1 = s - d;
    for row=1:mc                                %     for (row = 0; row < mc; row++)  {
        tmp2 = 0;                               %       tmp2 = 0.0;
        for i=0:d                               %       for (i = 0; i <= d; i++)
           tmp2 = tmp2 + N(i+1)*c(row,tmp1+i);  % 	tmp2 += N[i] * ctrl[tmp1+i][row];
        end                                     %
        p(row,col) = tmp2;                      %       pnt[col][row] = tmp2;
    end                                         %     }
end                                             %   }
                                                %
                                                %   mxFree(N);
                                                %   freevec2mat(pnt);
                                                %   freevec2mat(ctrl);
                                                %
                                                %   return ierr;
                                                %   }


function y = tapas_sgm(x, a)
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2012-2013 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

y = a./(1+exp(-x));

return;


function [ fea, out ] = ex_swirl_flow2( varargin )
%EX_SWIRL_FLOW2 2D Axisymmetric swirl flow in step domain.
%
%   [ FEA, OUT ] = EX_SWIRL_FLOW2( VARARGIN ) Axisymmetric swirl for in tubular step
%   region where the inner cylindrical wall is rotating.
%
%   Accepts the following property/value pairs.
%
%       Input       Value/{Default}        Description
%       -----------------------------------------------------------------------------------
%       omega       scalar {100}           Angular rotational velocity (of inner wall)
%       sf_u        string {sflag1}        Shape function for velocity
%       sf_p        string {sflag1}        Shape function for pressure
%       iplot       scalar 0/{1}           Plot solution (=1)
%                                                                                .
%       Output      Value/(Size)           Description
%       -----------------------------------------------------------------------------------
%       fea         struct                 Problem definition struct
%       out         struct                 Output struct

% Copyright 2013-2022 Precise Simulation, Ltd.

cOptDef = { 'omega',    100;
            'sf_u',     'sflag1';
            'sf_p',     'sflag1';
            'iphys',    1;
            'iplot',    1;
            'tol',      [];
            'fid',      1 };
[got,opt] = parseopt(cOptDef,varargin{:});
fid       = opt.fid;

% Geometry and grid generation.
fea.sdim = {'r' 'z'};

fea.geom.objects = { gobj_rectangle(0.5,1.5,0,3) gobj_rectangle(1.0,1.5,1.5,3,'R2') };
fea = geom_apply_formula( fea, 'R1-R2' );

fea.grid = gridgen( fea, 'hmax', 0.1, 'fid', fid );

% Equation definition.
if ( opt.iphys==1 )

  fea = addphys(fea,@swirlflow);
  fea.phys.sw.eqn.coef{1,end} = { 1 };
  fea.phys.sw.eqn.coef{2,end} = { 1 };
  fea.phys.sw.sfun            = [ repmat( {opt.sf_u}, 1, 3 ) {opt.sf_p} ];
  fea.phys.sw.bdr.sel = [1 1 5 2 1 1];
  fea.phys.sw.bdr.coef{2,end}{2,4} = opt.omega;
  fea.phys.sw.prop.artstab.ps = isequal(opt.sf_u,opt.sf_p);

  fea = parsephys(fea);
  if( isfield(fea,'constr') )
    fea = rmfield(fea,'constr');
  end

else

  opt.sf_u = 'sflag2';
  opt.sf_p = 'sflag1';

  fea.dvar = { 'u', 'v', 'w', 'p' };

  fea.sfun = [ repmat( {opt.sf_u}, 1, 3 ) {opt.sf_p} ];
  c_eqn    = { 'r*rho*u'' - r*miu*(2*ur_r + uz_z  +   wr_z) + r*rho*(u*ur_t + w*uz_t) + r*p_r     = r*Fr - 2*miu/r*u_t + p_t + rho*v*v_t';
               'r*rho*v'' - r*miu*(  vr_r + vz_z) + miu*v_r + r*rho*(u*vr_t + w*vz_t) + rho*u*v_t = r*Fth + miu*(v_r - 1/r*v_t)';
               'r*rho*w'' - r*miu*(  wr_r + uz_r  + 2*wz_z) + r*rho*(u*wr_t + w*wz_t) + r*p_z     = r*Fz';
               'r*ur_t + r*wz_t + u_t = 0' };
  fea.eqn = parseeqn( c_eqn, fea.dvar, fea.sdim );

  fea.coef = { 'rho', 1 ;
               'miu', 1 ;
               'Fr',  0 ;
               'Fth', 0 ;
               'Fz',  0 };

  % Boundary conditions.
  fea.bdr.d = { 0  0  [] 0  0  0 ;
                0  0  [] opt.omega 0 0 ;
                0  0  0  0  0  0 ;
                [] [] [] [] [] [] };
  fea.bdr.n = cell(size(fea.bdr.d));

end

% Fix pressure at p([r,z]=[ro,h/2]) = 0.
[~,ix_p] = min( sqrt( (fea.grid.p(1,:)-1.5).^2 + (fea.grid.p(2,:)-1.5).^2) );
fea.pnt = struct( 'type',  'constr', ...
                  'index', ix_p, ...
                  'dvar',  'p', ...
                  'expr',  '0' );

% Parse and solve problem.
fea = parseprob( fea );
fea.sol.u = solvestat( fea, 'maxnit', 50, 'fid', fid );

% Postprocessing.
if( opt.iplot )
  postplot( fea, 'surfexpr', 'sqrt(u^2+v^2+w^2)', 'isoexpr', 'v' )
end

% Error checking.
out.ref  = [ -6.1 10.5 73 1.25 ];
if( ~got.tol )
  if( opt.sf_u(end) == '2' )
    opt.tol = 0.05;
  else
    opt.tol = 0.3;
  end
end
[u_min,u_max] = minmaxsubd( 'u', fea );
out.val  = [ u_min u_max intsubd('v',fea) intsubd('w',fea) ];
out.pass = mean(abs(out.val-out.ref)./abs(out.ref)) < opt.tol;

if( nargout==0 )
  clear fea out
end


function [edge, isInside] = clipRay(ray, bb)
%CLIPRAY Clip a ray with a box.
%
%   EDGE = clipRay(RAY, BOX);
%   RAY is a straight ray given as a 4 element row vector: [x0 y0 dx dy],
%   with (x0 y0) being the origin of the ray and (dx dy) its direction
%   vector, BOX is the clipping box, given by its extreme coordinates:
%   [xmin xmax ymin ymax].
%   The result is given as an edge, defined by the coordinates of its 2
%   extreme points: [x1 y1 x2 y2].
%   If the ray does not intersect the box, [NaN NaN NaN NaN] is returned.
%
%   Function works also if RAY is a N-by-4 array, if BOX is a Nx4 array, or
%   if both RAY and BOX are N-by-4 arrays. In these cases, EDGE is a N-by-4
%   array.
%
%   See also
%     rays2d, boxes2d, edges2d, clipLine, drawRay
%

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2010-05-13, using Matlab 7.4.0.287 (R2007a)
% Copyright 2010-2022 INRA - Cepia Software Platform

% adjust size of two input arguments
if size(ray, 1) == 1
    ray = repmat(ray, size(bb, 1), 1);
elseif size(bb, 1) == 1
    bb = repmat(bb, size(ray, 1), 1);
elseif size(ray, 1) ~= size(bb, 1)
    error('bad sizes for input');
end

% first compute clipping of supporting line
edge = clipLine(ray, bb);

% detects valid edges (edges outside box are all NaN)
inds = find(isfinite(edge(:, 1)));

% compute position of edge extremities relative to the ray
pos1 = linePosition(edge(inds,1:2), ray(inds,:), 'diag');
pos2 = linePosition(edge(inds,3:4), ray(inds,:), 'diag');

% if first point is before ray origin, replace by origin
edge(inds(pos1 < 0), 1:2) = ray(inds(pos1 < 0), 1:2);

% if last point of edge is before origin, set all edge to NaN
edge(inds(pos2 < 0), :) = NaN;

% eventually returns result about inside or outside
if nargout > 1
    isInside = isfinite(edge(:,1));
end


function J=calcRuvConvJacob(zRUV,useHalfRange,lTx,lRx,M)
%%CALCRUVCONVJACOB Calculate the Jacobian for a monostatic or bistatic
%            range and direction cosines measurement in 3D, ignoring
%            atmospheric effects, with respect to Cartesian position.
%            This type of Jacobian is useful when performing tracking using
%            Cartesian-converted measurements where the clutter density is
%            specified in the measurement coordinate system, not the
%            converted measurement coordinate system.
%
%INPUTS: zRUV A 3X1 point in bistatic range and u and v coordinates in the
%           format [bistatic range;u;v].
% useHalfRange A boolean va0lue specifying whether the bistatic range value
%           should be divided by two. This normally comes up when operating
%           in monostatic mode, so that the range reported is a one-way
%           range. The default if this parameter is not provided, or an
%           empty matrix is passed, is false.
%       lTx The 3X1 [x;y;z] location vector of the transmitter in global
%           Cartesian coordinates. If this parameter is omitted or an
%           empty matrix is passed, then the transmitter is assumed to be
%           at the origin.
%       lRx The 3X1 [x;y;z] location vector of the receiver in Cartesian
%           coordinates. If this parameter is omitted or an empty matrix
%           is passed, then the receiver is assumed to be at the origin.
%         M A 3X3 rotation matrix to go from the alignment of the global
%           coordinate system to that at the receiver. The z-axis of the
%           local coordinate system of the receiver is the pointing
%           direction of the receiver. If omitted or an empty matrix is
%           passed, then it is assumed that the local coordinate system is
%           aligned with the global and M=eye(3) --the identity matrix is
%           used.
%
%OUTPUTS: J The 3X3 Jacobian matrix. Each row is a components of
%           [range;u;v] in that order with derivatives taken with respect
%           to [x,y,z] by column.
%
%This function converts the measurement into Cartesian coordinates and then
%calls rangeGradient and uvGradient.
%
%February 2017 David F.Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<5||isempty(M))
    M=eye(3,3);
end

if(nargin<4||isempty(lRx))
    lRx=zeros(3,1);
end

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

if(nargin<2||isempty(useHalfRange))
    useHalfRange=false;
end

x=ruv2Cart(zRUV,useHalfRange,lTx,lRx,M);

J=zeros(3,3);
J(1,:)=rangeGradient(x,useHalfRange,lTx,lRx);
J(2:3,:)=uvGradient(x,lRx,M);

end

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


function value = r8vec_i4vec_dot_product ( n, r8vec, i4vec )

%*****************************************************************************80
%
%% R8VEC_I4VEC_DOT_PRODUCT finds the dot product of an R8VEC and an I4VEC.
%
%  Discussion:
%
%    An R8VEC is a vector of R8's.
%
%    An I4VEC is a vector of I4's.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 June 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the dimension of the vectors.
%
%    Input, real R8VEC(N), the first vector.
%
%    Input, integer I4VEC(N), the second vector.
%
%    Output, real R8VEC_I4VEC_DOT_PRODUCT, the dot product.
%
  value = r8vec * i4vec';

  return
end


function [ vX ] = ConsensusAdmm( cProxFun, numElements, paramRho, numIterations, stopThr )
% ----------------------------------------------------------------------------------------------- %
% [ vX ] = ConsensusAdmm( cProxFun, numElements, paramRho, numIterations, stopThr )
%   Solves the Consensus ADMM problem given the set of Proximal Operators.
% Input:
%   - cProxFun      -   Set of Proximal Operators.
%                       Each cell elemnts is the i-th proximal operator.
%                       Each function handler input is a (vV, paramLambda).
%                       The implementation is for the prox in its lambda
%                       form: 0.5 * || x - v ||^2 + paramLamba * g(x).
%                       Structure: Cell Array (numSets x 1).
%                       Type: Function Handler.
%                       Range: NA.
% Output:
%   - vX            -   Solution Vector.
%                       The solution to the optimization problem..
%                       Structure: Vector (m x 1).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
% References
%   1.  ELE 522 - Large Scale Optimization for Data Science - Alternating Direction Method of Multipliers.
% Remarks:
%   1.  Pay attention that the references are using (1 / paramLambda) form
%       of the Prox. Hence the adaptation in the code.
%   2.  This is ultra efficient way to solve the Orthogonal Projection
%       Problem in case having projection onto sets which the intersection
%       is the objective set.
% TODO:
%   1.  C
% Release Notes:
%   -   1.0.000     25/03/2020  Royi Avital
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

FALSE   = 0;
TRUE    = 1;

OFF     = 0;
ON      = 1;

numSets     = size(cProxFun, 1);
% numElements = size(vX, 1);

paramRhoInv = 1 / paramRho;

mX      = zeros(numElements, numSets);
vZ      = mean(mX, 2);
mLambda = zeros(numElements, numSets);

for ii = 1:numIterations
    for jj = 1:numSets
        mX(:, jj) = cProxFun{jj}(vZ - (paramRhoInv * mLambda(:, jj)), paramRhoInv);
    end

    vZ = mean(mX + (paramRhoInv * mLambda), 2);

    for jj = 1:numSets
        mLambda(:, jj) = mLambda(:, jj) + (paramRho * (mX(:, jj) - vZ)); %<! Can be done mLambda = mLambda + (paramRho * (mX - vZ));
    end

    % To calculate the difference from the previous iteration.
    stopCond = max(abs(mX - vZ), [], 'all') < stopThr;

    if(stopCond)
        break;
    end
end

vX = mean(mX, 2);

end


% compute matrix multiply X * Y
%
function [grad] = B_matrix_multiply(input_layers, curr_layer, future_layers)
X = input_layers{1}.a;
Y = input_layers{2}.a;

% input1 and input2

[Dx,Tx,Nx] = size(X);
[Dy,Ty,Ny] = size(Y);
if Tx ~=Dy
    fprintf('Error: matrix size not match each other\n');
end

future_grad = GetFutureGrad(future_layers, curr_layer);

if Nx==1 && Ny==1
    gradX = conj(future_grad * Y');
    gradY = X' * future_grad;
else
    [validFrameMask, variableLength] = getValidFrameMask(curr_layer);
    if variableLength
        future_grad = PadShortTrajectory(future_grad, validFrameMask, 0);
    end

    if IsInGPU(X(1))
        gradX = gpuArray.zeros(size(X));
        gradY = gpuArray.zeros(size(Y));
    else
        gradX = zeros(size(X));
        gradY = zeros(size(Y));
    end

    if Nx==1 && Ny>1    %use the same X matrix for all Y matrices
        Y = PadShortTrajectory(Y, validFrameMask, 0);
        for i = 1:Ny
            gradX = gradX + conj(future_grad(:,:,i) * Y(:,:,i)');
            gradY(:,:,i) = X' * future_grad(:,:,i);
        end
    elseif Nx>1 && Ny==1    %use the same Y matrix for all X matrices
        X = PadShortTrajectory(X, validFrameMask, 0);
        for i = 1:Nx
            gradX(:,:,i) = conj(future_grad(:,:,i) * Y');
            gradY = gradY + X(:,:,i)' * future_grad(:,:,i);
        end
    elseif Nx>1 && Ny>1
        Y = PadShortTrajectory(Y, validFrameMask, 0);
        X = PadShortTrajectory(X, validFrameMask, 0);

        for i = 1:N
            gradX(:,:,i) = conj(future_grad(:,:,i) * Y(:,:,i)');
            gradY(:,:,i) = X(:,:,i)' * future_grad(:,:,i);
        end
    end
end
grad{1} = gradX;
grad{2} = gradY;
end


function [r,v] = keplerUniversal(r0,v0,t,mu)
%Purpose:
%Most effecient way to propagate any type of two body orbit using kepler's
%equations.
%-------------------------------------------------------------------------%
%                                                                         %
% Inputs:                                                                 %
%--------
%r_ECI                  [3 x N]                         Position Vector in
%                                                       ECI coordinate
%                                                       frame of reference
%
%v_ECI                  [3 x N]                         Velocity vector in
%                                                       ECI coordinate
%                                                       frame of reference
%
%t                      [1 x N]                         time vector in
%                                                       seconds
%
%mu                     double                          Gravitational Constant
%                                                       Defaults to Earth if
%                                                       not specified
% Outputs:
%---------                                                                %
%r_ECI                  [3 x N]                         Final position
%                                                       vector in ECI
%
%v_ECI                  [3 x N]                         Final velocity
%                                                       vector in ECI
%--------------------------------------------------------------------------
% Programmed by Darin Koblick 03-04-2012                                  %
%--------------------------------------------------------------------------
if ~exist('mu','var'); mu = 398600.4418; end
tol = 1e-9;
v0Mag = sqrt(sum(v0.^2,1));  r0Mag = sqrt(sum(r0.^2,1));
alpha = -(v0Mag.^2)./mu + 2./r0Mag;
%% Compute initial guess (X0) for Newton's Method
X0 = NaN(size(t));
%Check if there are any Eliptic/Circular orbits
idx = alpha > 0.000001;
if any(idx)
    X0(idx) = sqrt(mu).*t(idx).*alpha(idx);
end
%Check if there are any Parabolic orbits
idx = abs(alpha) < 0.000001;
if any(idx)
   h = cross(r0(:,idx),v0(:,idx)); hMag = sqrt(sum(h.^2,1));
   p = (hMag.^2)./mu; s = acot(3.*sqrt(mu./(p.^3)).*t(idx))./2;
   w = atan(tan(s).^(1/3)); X0(idx) = sqrt(p).*2.*cot(2.*w);
end
%Check if there are any Hyperbolic orbits
idx = alpha < -0.000001;
if any(idx)
   a = 1./alpha(idx);
   X0(idx) = sign(t(idx)).*sqrt(-a).*...
       log(-2.*mu.*alpha(idx).*t(idx)./ ...
       (dot(r0(:,idx),v0(:,idx))+sign(t(idx)).*sqrt(-mu.*a).*...
       (1-r0Mag(idx).*alpha(idx))));
end
%% Newton's Method to converge on solution
% Declare Constants that do not need to be computed within the while loop
err = Inf;
dr0v0Smu = dot(r0,v0)./sqrt(mu);
Smut = sqrt(mu).*t;
while any(abs(err) > tol)
    X02 = X0.^2;
    X03 = X02.*X0;
    psi = X02.*alpha;
    [c2,c3] = c2c3(psi);
    X0tOmPsiC3 = X0.*(1-psi.*c3);
    X02tC2 = X02.*c2;
    r = X02tC2 + dr0v0Smu.*X0tOmPsiC3 + r0Mag.*(1-psi.*c2);
    Xn = X0 + (Smut-X03.*c3-dr0v0Smu.*X02tC2-r0Mag.*X0tOmPsiC3)./r;
    err = Xn-X0; X0 = Xn;
end
f = 1 - (Xn.^2).*c2./r0Mag; g = t - (Xn.^3).*c3./sqrt(mu);
gdot = 1 - c2.*(Xn.^2)./r; fdot = Xn.*(psi.*c3-1).*sqrt(mu)./(r.*r0Mag);
r = bsxfun(@times,f,r0) + bsxfun(@times,g,v0);
v = bsxfun(@times,fdot,r0) + bsxfun(@times,gdot,v0);
%% Ensure Solution Integrity
%idx = round((f.*gdot - fdot.*g)./tol).*tol ~= 1; r(:,idx) = NaN; v(:,idx) = NaN;
end

function [c2,c3] = c2c3(psi)
%Vallado pg. 71 Algorithm 1
c2 = NaN(size(psi));
c3 = NaN(size(psi));
idx = psi > 1e-6;
if any(idx)
    c2(idx) = (1-cos(sqrt(psi(idx))))./psi(idx);
    c3(idx) = (sqrt(psi(idx))-sin(sqrt(psi(idx))))./sqrt(psi(idx).^3);
end
idx = psi < -1e-6;
if any(idx)
    c2(idx) = (1 - cosh(sqrt(-psi(idx))))./psi(idx);
    c3(idx) = (sinh(sqrt(-psi(idx)))-sqrt(-psi(idx)))./sqrt(-psi(idx).^3);
end
idx = abs(psi) <= 1e-6;
if any(idx)
    c2(idx) = 0.5;
    c3(idx) = 1/6;
end
end


%DEMO_HURDLE Demonstration of Logit Negative-binomial hurdle model
%            using Gaussian process prior
%
%  Description
%    Hurdle models can be used to model excess number of zeros
%    compared to usual Poisson and negative binomial count models.
%    Hurdle models assume a two-stage process, where the first
%    process determines whether the count is larger than zero, and
%    the second process determines the non-zero count. Both
%    processes are given a zero mean Gaussian process prior. The
%    two stage model formulation makes it possible to make
%    inference for the two latent processes separately.
%
%    In this demo we construct logit negative binomial hurdle model
%    by using logit and zero truncated negative binomial models.
%    The posterior inference is made with parallel-EP
%    approximation.
%
%    See also  DEMO_SPATIAL2, DEMO_CLASSIFIC1

% Copyright (c) 2008-2010 Jarno Vanhatalo
% Copyright (c) 2010-2011 Aki Vehtari
% Copyright (c) 2011 Jaakko Riihimäki

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

% load the data
S = which('demo_spatial1');
data = load(strrep(S,'demo_spatial1.m','demodata/spatial1.txt'));

x = data(:,1:2);
ye = data(:,3);
y = data(:,4);

x0=x;
x=bsxfun(@rdivide,bsxfun(@minus,x0,mean(x0)),std(x0));

% for zero-process
yz=double(y>0)*2-1;
% for count-process
ci=find(y>0);
yc=y(ci);
xc=x(ci,:);
yec=ye(ci,:);

% Create the covariance functions
pl = prior_t('s2',10);
pm = prior_sqrtunif();
cf = gpcf_matern32('lengthScale', 5, 'magnSigma2', 0.05, ...
                   'lengthScale_prior', pl, 'magnSigma2_prior', pm);

% Create the zero part
likz=lik_probit();
gpz=gp_set('lik',likz,'cf',cf,'jitterSigma2',1e-6,'latent_method','EP','latent_opt',struct('parallel','on'));
% Create the count part
likc=lik_negbinztr();
gpc=gp_set('lik',likc,'cf',cf,'jitterSigma2',1e-6,'latent_method','EP','latent_opt',struct('parallel','on'));
% Note that although both parts use 'cf', they have separate hyperparameters

% Set the options for the quasi-Newton optimization
opt=optimset('TolFun',1e-2,'TolX',1e-2,'Display','iter');
gpz=gp_optim(gpz,x,yz,'opt',opt,'optimf',@fminlbfgs);
gpc=gp_optim(gpc,xc,yc,'z',yec,'opt',opt,'optimf',@fminlbfgs);

% make prediction to the data points
[Efz, Varfz] = gp_pred(gpz, x, yz, x);
[Efc, Varfc] = gp_pred(gpc, xc, yc, xc, 'z', yec);

% Define help parameters for plotting
xii=sub2ind([60 35],x0(:,2),x0(:,1));
[X1,X2]=meshgrid(1:35,1:60);

% Plot the figures
figure
subplot(1,2,1)
G=repmat(NaN,size(X1));
G(xii)=Efz(:);
pcolor(X1,X2,G),shading flat
colorbar
axis equal
axis([0 35 0 60])
title('Posterior mean of latent zero process')

subplot(1,2,2)
G=repmat(NaN,size(X1));
G(xii(ci))=Efc(:);
pcolor(X1,X2,G),shading flat
colorbar
axis equal
axis([0 35 0 60])
title('Posterior mean of latent count process')


% test for texture synthesis

path(path, 'images/');
path(path, '../images/');

name = 'grass';
name = 'turbulence1';
name = 'fingerprint3';
name = 'dunes';

n = 128;
M = load_image(name, n);
M = crop(M,n);

s = size(M,3);
n1 = 128;
M1 = randn(n1,n1,s);

options.niter_synthesis = 5;
options.verb = 1;
disp('--> Steerable synthesis.');
options.synthesis_method = 'steerable';
Msteer = perform_wavelet_matching(M1,M,options);
disp('--> Wavelet orthogonal synthesis.');
options.synthesis_method = 'wavelets-ortho';
Morth = perform_wavelet_matching(M1,M,options);
disp('--> Quincunx synthesis.');
options.synthesis_method = 'quincunx-ti';
Mquin = perform_wavelet_matching(M1,M,options);
disp('--> Wavelets TI synthesis.');
options.synthesis_method = 'wavelets-ti';
Mwavti = perform_wavelet_matching(M1,M,options);

clf;
imageplot({M Msteer Morth Mquin Mwavti},{'Original', 'Synth. steerable', 'Synth. wav. ortho', 'Synth. quincunx', 'Synth. wav. ti'});


function t = bartlett_unit_sample ( m, df )

%*****************************************************************************80
%
%% BARTLETT_UNIT_SAMPLE samples the unit Bartlett distribution.
%
%  Discussion:
%
%    If the matrix T is sampled from the unit Bartlett distribution, then
%    the matrix W = T' * T is a sample from the unit Wishart distribution.
%
%    This function requires functions from the PDFLIB and RNGLIB libraries.
%
%    The "initialize()" function from RNGLIB must be called before using
%    this function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 October 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Patrick Odell, Alan Feiveson,
%    A numerical procedure to generate a sample covariance matrix,
%    Journal of the American Statistical Association,
%    Volume 61, Number 313, March 1966, pages 199-203.
%
%    Stanley Sawyer,
%    Wishart Distributions and Inverse-Wishart Sampling,
%    Washington University,
%    30 April 2007, 12 pages.
%
%  Parameters:
%
%    Input, integer M, the order of the matrix.
%
%    Input, integer DF, the number of degrees of freedom.
%    M <= DF.
%
%    Output, real T(M,M), the sample matrix from the unit Bartlett distribution.
%
  if ( df < m )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'BARTLETT_UNIT_SAMPLE - Fatal error!\n' );
    fprintf ( 1, '  DF = %d < M = %d.\n', df, m );
    error ( 'BARTLETT_UNIT_SAMPLE - Fatal error!\n' );
  end

  t = zeros ( m, m );

  for i = 1 : m - 1
    df_chi = df - i + 1;
    t(i,i) = sqrt ( r8_chi_sample ( df_chi ) );
    for j = i + 1 : m
      t(i,j) = r8_normal_01_sample ( );
    end
  end

  return
end


function yv = dif_vals ( nd, xd, yd, nv, xv )

%*****************************************************************************80
%
%% DIF_VALS evaluates a divided difference polynomial at a set of points.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 May 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Carl deBoor,
%    A Practical Guide to Splines,
%    Springer, 2001,
%    ISBN: 0387953663,
%    LC: QA1.A647.v27.
%
%  Parameters:
%
%    Input, integer ND, the order of the difference table.
%
%    Input, real XD(ND), the X values of the difference table.
%
%    Input, real YD(ND), the divided differences.
%
%    Input, integer NV, the number of evaluation points.
%
%    Input, real XV(NV), the evaluation points.
%
%    Output, real YV(NV), the value of the divided difference
%    polynomial at the evaluation points.
%

%
%  Deal with the blasted curse of row vectors.
%
  yd = yd ( : );
  xd = xd ( : );
  xv = xv ( : );

  yv = zeros ( nv, 1 );

  yv(1:nv) = yd(nd);

  for i = 1 : nd - 1
    yv(1:nv) = yd(nd-i) + ( xv(1:nv) - xd(nd-i) ) .* yv(1:nv);
  end

  return
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   demo script to convert a closed surface to a binary image
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% preparation
% user must add the path of iso2mesh to matlab path list
% addpath('../');

%% load the sample data
load rat_head.mat

% first, generate a surface from the original image
% similar to demo_shortcuts_ex1.m

[node,face,regions,holes]=v2s(volimage,0.5,3);

node=sms(node,face(:,1:3),3,0.5); % apply 3 mesh smoothing

mdim=ceil(max(node)+1);
dstep=0.25;
zslice=15;
xrange=0:dstep:mdim(1);
yrange=0:dstep:mdim(2);
zrange=0:dstep:mdim(3);
img=surf2vol(node,face(:,1:3),xrange,yrange,zrange);

imagesc(squeeze(img(:,:,zslice))); % z=10

hold on

z0=zslice*dstep;
plane=[min(node(:,1)) min(node(:,2)) z0
       min(node(:,1)) max(node(:,2)) z0
       max(node(:,1)) min(node(:,2)) z0];

% run qmeshcut to get the cross-section information at z=mean(node(:,1))
% use the x-coordinates as the nodal values

[bcutpos,bcutvalue,bcutedges]=qmeshcut(face(:,1:3),node,node(:,1),plane);
[bcutpos,bcutedges]=removedupnodes(bcutpos,bcutedges);
bcutloop=extractloops(bcutedges);
bcutloop(isnan(bcutloop))=[]; % there can be multiple loops, remove the separators
plot(bcutpos(bcutloop,2)*(1/dstep),bcutpos(bcutloop,1)*(1/dstep),'w');

if(isoctavemesh)
  if(~exist('bwfill'))
    error('you need to install octave-image toolbox first');
  end
  img2=zeros(size(img),'uint8');
  for i=1:size(img,3)
    img2(:,:,i)=bwfill(img(:,:,i),'holes');
  end
  img2=img2+img;
else
  img2=imfill(img,'holes')+img;
end
figure;
imagesc(squeeze(img2(:,:,zslice))); % z=10
hold on;
plot(bcutpos(bcutloop,2)*(1/dstep),bcutpos(bcutloop,1)*(1/dstep),'y--');


function [clrmap,clrIX_x,clrIX_y,clrIX_z] = MapXYto3Dcolormap(gIX_in,X,Y,Z,Xrange,Yrange,Zrange,cmap3D)
% gIX_in corresponds to the 2D values X and Y.
% The output clrmap is a linear colormap of the same length as numK
% (=max(gIX_in)), and same as clrIX_x and clrIX_y.
% clrIX_x and clrIX_y are the coordinates in the 2D colormap for the range of values in gIX_in.

if nargin < 5
    Xrange = [min(X),max(X)];
end
if nargin < 6
    Yrange = [min(Y),max(Y)];
end
if nargin < 7
    Zrange = [min(Z),max(Z)];
end

if nargin < 8
    cmap3D = MakeDiagonal3Dcolormap;
end
res = size(cmap3D,1);

% check input dimensions
if size(X,2)>1
    X = X';
end
if size(Y,2)>1
    Y = Y';
end
if size(Z,2)>1
    Z = Z';
end
% assert(isequal(size(X),size(Y)));
% assert(isequal(size(X),size(Z)));

% set data range
X(X<Xrange(1)) = Xrange(1);
Y(Y<Yrange(1)) = Yrange(1);
Z(Z<Zrange(1)) = Zrange(1);
X(X>Xrange(2)) = Xrange(2);
Y(Y>Yrange(2)) = Yrange(2);
Z(Z<Zrange(1)) = Zrange(1);

clrIX_x = round((X-Xrange(1))/(Xrange(2)-Xrange(1))*(res-1))+1;
clrIX_y = round((Y-Yrange(1))/(Yrange(2)-Yrange(1))*(res-1))+1;
clrIX_z = round((Z-Zrange(1))/(Zrange(2)-Zrange(1))*(res-1))+1;

%%
clrmap = zeros(length(clrIX_x),3);
cmap3D_flat = reshape(cmap3D,res*res*res,3); % for efficient indexing

U = unique(gIX_in);

ix = sub2ind([res,res,res],clrIX_x(U),clrIX_y(U),clrIX_z(U));
% ix = (clrIX_x(U)-1)*res+clrIX_y(U);
clrmap(U,:) = cmap3D_flat(ix,:);
% for i = 1:length(U)
% %     ix = sub2ind([res,res],clrIX_y(U(i))',clrIX_x(U(i))');
%     ix = (clrIX_x(U(i))'-1)*res+clrIX_y(U(i))';
% %     center_ix = (M_xyz(j,2)-1)*dimv(1)+M_xyz(j,1); % linear pixel index, faster equivalent of:
%         %     center_ix = sub2ind([dimv(1),dimv(2)],M_xyz(j,1),M_xyz(j,2));
%
%
%     clrmap(U(i),:) = clrmap_2D_flat(ix,:);
% end
end


% MAIN  --  Quad-Rotor  --  Minimal-Acceleration trajectory
%
% Fin the minimal acceleration-squared trajectory to move the quad-rotor from an
% arbitrary state to the origin.
%
% NOTES:
%   X = [x;y;q] = [x pos, y pos, angle] = configuration
%  dX = [dx;dy;dq] = [x vel, y vel, angle rate] = rate
% ddX = [ddx;ddy;ddq] = acceleration
%
% PROBLEM:
%
% ddX = f(X,dX,u);     <-- dynamics
%
% cost = integral(  ddX^2  );     <-- cost function
%
% subject to:
%   X(0) = X0;
%   X(1) = XF;
%   dX(0) = dX0;
%   dX(1) = dXF;
%
% How to pose as a standard trajectory optimization problem?
%
% dX = V1;
% dV1 = f(X,V1,U1)
%
% V2 == V1;   % <-- Key line.
% dV2 = U2;
% cost = integral(  U2^2  );
%
% z = [X;V1;V2]
% u = [U1;U2]
%

clc; clear;

addpath ../../

% Dynamics paramters
p.g = 9.81; % (m/s^2) gravity
p.d = 0.3;  % (m) half-width of quad rotor
p.m = 0.2;  % (m) half-mass of the quad rotor

% Trajectory Parameters:
duration = 1;
uMax = 5*p.g*p.m;

% Initial State:
X0 = [1;0;0];   %  initial configuration
dX0 = zeros(3,1);  % initial rates
z0 = [X0; dX0; dX0];  % initial state

XF = [0;0;0];   % final configuration
dXF = zeros(3,1);  % final rates
zF = [XF; dXF; dXF];  % final state

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                     Set up function handles                             %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
w = 1./[1,1,1];  %weighting vector for path objective

problem.func.dynamics = @(t,z,u)( dynAcc(z,u,p) );
problem.func.pathObj = @(t,z,u)( pathObj(u,w) );  %accel-squared cost function
problem.func.pathCst = @(t,z,u)( pathCst(z) );

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                     Set up problem bounds                               %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

problem.bounds.initialTime.low = 0;
problem.bounds.initialTime.upp = 0;
problem.bounds.finalTime.low = duration;
problem.bounds.finalTime.upp = duration;

problem.bounds.initialState.low = z0;
problem.bounds.initialState.upp = z0;
problem.bounds.finalState.low = zF;
problem.bounds.finalState.upp = zF;

problem.bounds.control.low = [-uMax*[1;1];  -inf(3,1)];   %[torque, accel]
problem.bounds.control.upp = [uMax*[1;1];  inf(3,1)];

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                    Initial guess at trajectory                          %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

problem.guess.time = [0,duration];
problem.guess.state = [z0, zF];
problem.guess.control = [p.g*p.m*ones(2,2); zeros(3,2)];

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                         Solver options                                  %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

problem.options.nlpOpt = optimset(...
    'Display','iter',...
    'MaxFunEvals',1e5);

% problem.options.method = 'trapezoid';
% problem.options.trapezoid.nGrid = 40;
%
% problem.options.method = 'hermiteSimpson';
% problem.options.hermiteSimpson.nSegment = 10;

problem.options.method = 'chebyshev';
problem.options.chebyshev.nColPts = 15;

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                            Solve!                                       %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

soln = optimTraj(problem);

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                        Display Solution                                 %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

%%%% Unpack the simulation
t = linspace(soln(end).grid.time(1), soln(end).grid.time(end), 150);

z = soln(end).interp.state(t);
x = z(1,:);
y = z(2,:);
q = z(3,:);
dx = z(4,:);
dy = z(5,:);
dq = z(6,:);

X1 = z(1:3,:);
V1 = z(4:6,:);
V2 = z(7:9,:);

u = soln(end).interp.control(t);
u1 = u(1,:);
u2 = u(2,:);
A2 = u(3:5,:);

[dObj,uStar] = pathObj(u,w);

%%%% Plots:

%%%% Plots:

figure(2); clf;

subplot(2,2,1)
plot(t,X1);
legend('x','y','q')
title('configuration')

subplot(2,2,3)
plot(t,V2);
legend('x','y','q')
title('rates')

subplot(2,2,2)
plot(t,A2);
legend('x','y','q')
title('acceleration')

subplot(2,2,4); hold on;
plot(t,u1);  plot(t,u2);
title('actuators')
legend('u1','u2');

% Configuration trajectories
figure(1); clf;

subplot(2,2,1); hold on;
plot(t,x);
xlabel('t')
ylabel('x')
title('Minimum acceleration-squared trajectory')

subplot(2,2,2); hold on;
plot(t,y);
xlabel('t')
ylabel('y')

subplot(2,2,3); hold on;
plot(t,q);
xlabel('t')
ylabel('q')

subplot(2,2,4); hold on;
plot(t,u1);  plot(t,u2);
xlabel('t')
ylabel('u')
legend('u1','u2');


function [Updated_Grid] = compute_transformation(i,Grid,H,E,Theta)
if (Grid.x(i) < E)
    Grid.y_s(i) = 0;
    Grid.h(i) = H;
else
    Grid.y_s(i) = (-Grid.x(i)*tan(Theta)) + (E*tan(Theta));
    Grid.h(i) = H + (Grid.x(i)*tan(Theta)) - (E*tan(Theta));
end
for j = 1:401,
    if (Grid.x(i) < E)
        Grid.m1(j,i) = 0;
    else
        Grid.m1(j,i) = (tan(Theta)/Grid.h(i)) - (Grid.y_t(j)*(tan(Theta)/Grid.h(i)));
    end
    Grid.y(j,i) = (Grid.y_t(j)*Grid.h(i)) + Grid.y_s(i);
    Grid.m2(j,i) = 1/Grid.h(i);
end
Updated_Grid = Grid;
end


%otyp==0->cont, otyp==1->disc
function [A N]=sys_bd_dcm(llh, vel_b, Cbn, gyro, otyp, dt)
% position   (1-3)
% velocity   (4-6)
% attitude   (7-9)

%system disturbance coefs
N=zeros(9,6);
N(7:9,4:6)=-Cbn; %attitude
N(4:6,1:3)=eye(3); %velocity
N(4:6,4:6)=skew(vel_b); %velocity

%system matrix
A=zeros(9);
[Rn, Re, g, sL, cL, WIE_E]=geoparam_v000(llh);
tL=sL/cL;
Rn_h=Rn+llh(3);
Re_h=Re+llh(3);
R=diag([1/Rn_h,1/Re_h,-1]);
O=[0 1/Re_h 0;-1/Rn_h 0 0;0 -tL/Re_h 0];
vel_n=Cbn*vel_b;
wen_n=[vel_n(2)/Re_h; -vel_n(1)/Rn_h; -vel_n(2)*tL/Re_h];
wie_n=[WIE_E*cL; 0; -WIE_E*sL];
win_n=wen_n+wie_n;

%Position
A(1:3,4:6)=R*Cbn;
A(1:3,7:9)=R*skew(vel_n);
A(1,3)=-vel_n(1)/Rn_h^2;
A(2,1)=vel_n(2)*tL/Re_h/cL;
A(2,3)=-vel_n(2)/Re_h^2/cL;

%Velocity
A(4:6,4:6)=-skew(Cbn'*wie_n+gyro);
A(4:6,7:9)=Cbn'*skew([0;0;-g])+skew(vel_b)*Cbn'*skew(wie_n);
A(4:6,1)=skew(vel_b)*Cbn'*[-WIE_E*sL;0;-WIE_E*cL];

%Attitude
A(7,1)=-WIE_E*sL;
A(7,3)=-vel_n(2)/Re_h^2;
A(8,3)=vel_n(1)/Rn_h^2;
A(9,1)=-(WIE_E*cL+vel_n(2)/Re_h/cL/cL);
A(9,3)=vel_n(2)*tL/Re_h^2;
A(7:9,4:6)=O*Cbn;
A(7:9,7:9)=-skew(win_n)+O*skew(vel_n);

%discretize A;
if (otyp==0)
    A=A;
elseif(otyp==1)
    A=expm(A*dt);
end


function [gd,nlen]=ptpfundual(w,a,M,L,varargin)
%PTPFUNDUAL Sampled, periodized dual TP function of finite type
%   Usage: gd=ptpfundual(w,a,M,L)
%          gd=ptpfundual({w,width},a,M,L)
%          gd=ptpfundual(...,inc)
%          [gd,nlen]=ptpfundual(...)
%
%   Input parameters:
%         w      : Vector of reciprocals $w_j=1/\delta_j$ in Fourier representation of *g*
%         width  : Integer stretching factor for the essential support
%         a      : Length of time shift.
%         M      : Number of channels.
%         L      : Window length.
%         inc    : Extension parameter
%
%   Output parameters:
%         gd    : The periodized totally positive function dual window.
%         nlen  : Number of non-zero elements in gd.
%
%   `ptpfundual(w,a,M,L)` computes samples of a dual window of totally
%   positive function of finite type >=2 with weights *w*. Please see
%   |ptpfun| for definition of totally positive functions.
%   The lattice parameters $a,M$ must satisfy $M > a$ to ensure the
%   system is a frame.
%
%   `ptpfundual({w,width},a,M,L)` works as above but in addition the *width*
%   parameter determines the integer stretching factor of the original TP
%   function. For explanation see help of |ptpfun|.
%
%   `ptpfundual(...,inc)` or `ptpfundual(...,'inc',inc)` works as above,
%   but integer *inc* denotes number of additional columns to compute window
%   function *gd*. 'inc'-many are added at each side. It should be smaller
%   than 100 to have comfortable execution-time. The higher the number the
%   closer *gd* is to the canonical dual window.
%   The default value is 10.
%
%   `[gd,nlen]=ptpfundual(...)` as *gd* might have a compact support,
%   *nlen* contains a number of non-zero elements in *gd*. This is the case
%   when *gd* is symmetric. If *gd* is not symmetric, *nlen* is extended
%   to twice the length of the longer tail.
%
%   If $nlen = L$, *gd* has a 'full' support meaning it is a periodization
%   of a dual TP function.
%
%   If $nlen < L$, additional zeros can be removed by calling
%   `gd=middlepad(gd,nlen)`.
%
%   Examples:
%   ---------
%
%   The following example compares dual windows computed using 2 different
%   approaches.:::
%
%     w = [-3,-1,1,3];a = 25; M = 31; inc = 10;
%     L = 1e6; L = dgtlength(L,a,M);
%     width = M;
%
%     % Create the window
%     g = ptpfun(L,w,width);
%
%     % Compute a dual window using pebfundual
%     tic
%     [gd,nlen] = ptpfundual({w,width},a,M,L,inc);
%     ttpfundual=toc;
%
%     % We know that gd has only nlen nonzero samples, lets shrink it.
%     gd = middlepad(gd,nlen);
%
%     % Compute the canonical window using gabdual
%     tic
%     gdLTFAT = gabdual(g,a,M,L);
%     tgabdual=toc;
%
%     fprintf('PTPFUNDUAL elapsed time %f s\n',ttpfundual);
%     fprintf('GABDUAL elapsed time    %f s\n',tgabdual);
%
%     % Test on random signal
%     f = randn(L,1);
%
%     fr = idgt(dgt(f,g,a,M),gd,a,numel(f));
%     fprintf('Reconstruction error PTPFUNDUAL: %e\n',norm(f-fr)/norm(f));
%
%     fr = idgt(dgt(f,g,a,M),gdLTFAT,a,numel(f));
%     fprintf('Reconstruction error GABDUAL:    %e\n',norm(f-fr)/norm(f));
%
%   See also: dgt, idgt, ptpfun
%
%   References: grst13 kl12 bagrst14 klst14

% AUTHORS: Joachim Stoeckler, Tobias Kloos, 2012-2014

complainif_notenoughargs(nargin,4,upper(mfilename));
complainif_notposint(L,'L',upper(mfilename));
complainif_notposint(a,'a',upper(mfilename));
complainif_notposint(M,'M',upper(mfilename));

% Check lattice
if M<=a
    error('%s: Lattice parameters must satisfy M>a.',upper(mfilename));
end

% Check w
if iscell(w)
    if numel(w)~=2
        error('%s: w must be a 2 element cell array.',upper(mfilename));
    end
    width = w{2};
    w = w{1};
    complainif_notposint(width,'width',upper(mfilename));
else
    width = floor(sqrt(L));
end

if isempty(w) || ~isnumeric(w) || numel(w)<2
    error(['%s: w must be a nonempty numeric vector with at least',...
    ' 2 elements.'], upper(mfilename));
end

if any(w==0)
    error('%s: All weights w must be nonzero.', upper(mfilename));
    % TO DO: Also add a warning if w is very small or big?
end

% Define initial value for flags and key/value pairs.
%definput.import={'setnorm'};
definput.keyvals.inc = 10;
%definput.flags.scale = {'nomatchscale','matchscale'};
[flags,~,inc]=ltfatarghelper({'inc'},definput,varargin);
complainif_notnonnegint(inc,'inc',upper(mfilename));

% TP functions are scale invariant so we do scaling directly on w.
wloc = w/width;
% Converting a, M to alpha, beta
alpha = a;
beta = 1/M;

% check alpha beta
if (alpha<=0) || (beta<=0)
    error('lattice parameters alpha, beta must be positive')
end
if (width*beta > 10)
    warning('width/M should be smaller than 10: numerical instability may occur')
end

% compute m n and check that a has nonzero entries
if all(wloc<0)
    wloc = -wloc;
    case0 = 2;
else
    case0 = 0;
end
m = length(find(wloc>0));
n = length(find(wloc<0));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% preparations specially for computation of gd
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
r = floor(1/(1-alpha*beta)+eps);

% check special cases according to n and m
if n == 0
    if m >= 2
        k2 = (m-1)*(r+1)-1;
        k1 = -k2;
        if case0 == 0
            case0 = 1;
        end
    end
elseif n == 1
    N = m*(r+1)+1;       % minimal column size
    k1 = -m*(r+1)+1;     % column index k1 from the paper
    k2 = k1+N-1;         % column index k2 from the paper
else
    N = (m+n-1)*(r+1);   % minimal column size
    k1 = -m*(r+1)+1;     % column index k1 from the paper
    k2 = k1+N-1;         % column index k2 from the paper
end

k1 = k1-inc;
k2 = k2+inc;

% minimal values for x and y
varl = floor((k1+m-1)/(alpha*beta))-1;
varr = ceil((k2-n+1)/(alpha*beta))+1;
x = varl*alpha:alpha:varr*alpha;
i0 = abs(varl)+1; % index of "central" row of P(x)
y = (k1-1)/beta:(1/beta):(k2+1)/beta;
k0 = abs(k1-1)+1; % index of "central" column of P(x)

[yy,xx] = meshgrid(y,x);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% discretization
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
t = 0:(a-1); % for stepping through the interval [0,alpha)
tt = varl*alpha:varr*alpha; % choose same stepsize for t and tt
% left and right bounds large enough for the support of gamma
tt0 = abs(varl*a)+1; % index for tt == 0
gd = zeros(1,length(tt)); % dual window

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% computation of gamma
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

k1 = k1+k0;
k2 = k2+k0;

for k=1:length(t)
    % step through the interval [0,alpha)
    x0 = t(k); % compute dual window at points (x+j*alpha)

    % row indices for rectangular P0
    i1 = floor((k1-k0+m-1)/alpha/beta-x0/alpha)+1+i0;
    i2 = ceil((k2-k0-n+1)/alpha/beta-x0/alpha)-1+i0;

    % Computation of P0(x0)
    % z0 is the matrix of the abscissa x0+j*alpha-k/beta, j=i1:i2, k=k1:k2,
    % z1 puts all these abscissae into a row vector.
    % The computation of g(z1) is done as described above for the
    % vector tt.
    z0 = x0+xx(:,k1:k2)-yy(:,k1:k2);
    z1 = z0(:)';

    Y = zeros((n+m)-1,length(z1));
    for q = 1:(n+m)-1
        if wloc(q) == wloc(q+1)
            Y(q,:) = abs(wloc(q))^2*abs((z1.*((wloc(q)*z1)>=0))).*exp(-wloc(q)*(z1.*((wloc(q)*z1)>=0))).*((wloc(q)*z1)>=0);
        else
            if wloc(q)*wloc(q+1) < 0
                Y(q,:) = abs(wloc(q)*wloc(q+1))/(abs(wloc(q))+abs(wloc(q+1)))*(exp(-wloc(q)*(z1.*((wloc(q)*z1)>=0))).*((wloc(q)*z1)>=0) + ...
                exp(-wloc(q+1)*(z1.*((wloc(q+1)*z1)>=0))).*((wloc(q+1)*z1)>0));
            else
                Y(q,:) = wloc(q)*wloc(q+1)/(abs(wloc(q+1))-abs(wloc(q)))*(exp(-wloc(q)*(z1.*((wloc(q)*z1)>=0)))-exp(-wloc(q+1)*(z1.*((wloc(q)*z1)>=0)))).*((wloc(q)*z1)>=0);
            end
        end
    end

    for q = 2:(n+m)-1
        for j = 1:(n+m)-q
            if wloc(j) == wloc(j+q)
                Y(j,:) = Y(j,:).*abs(z1)/q*abs(wloc(j));
            else
                Y(j,:) = (wloc(j)*Y(j+1,:)-wloc(j+q)*Y(j,:))/(wloc(j)-wloc(j+q));
            end
        end
    end

    if (n+m) == 1
        A0 = abs(wloc)*exp(-wloc*(z1.*((wloc*z1)>=0))).*((wloc*z1)>=0);
    else
        A0 = Y(1,:);
    end

    A0 = reshape(A0,size(z0))*sqrt(width);%*L^(1/4);
    P0 = A0(i1:i2,:);

    % computation of pseudo-inverse matrix of P0
    P0inv = pinv(P0);
    gd(k-1+tt0-a*(i0-i1):a:k-1+tt0+a*(i2-i0)) = beta*P0inv(k0-k1+1,:); % row index k0-k1a+1
    % points to the "j=0" row of P0inv
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% periodization of gamma
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nlen = length(gd);
nr = ceil(nlen/L);
v = zeros(1,nr*L);
v(1:length(gd)) = gd;
v = [v(tt0:end),v(1:tt0-1)];

gd = sum(reshape(v,L,nr),2);
gd = gd(:);
if case0 == 2
    gd =  flipud(gd);
    gd = [gd(end);gd(1:end-1)];
end

% Determine nlen
if nlen<L
    negsupp = tt0-1;
    possupp = nlen-tt0; % excluding zero pos.
    nlen = 2*max([negsupp,possupp])+1;
end
nlen = min([L,nlen]);

% if flags.do_matchscale
%    g = ptpfun(L,w,width,flags.norm);
%    [scal,err] = gabdualnorm(g,gd,a,M,L);
%     assert(err<1e-10,sprintf(['%s: Assertion failed. This is not a valid ',...
%                               ' dual window.'],upper(mfilename)));
%    gd = gd/scal;
% else
%    gd = setnorm(gd,flags.norm);
% end

gd = gd(:);

end


function indx = multigrid_index_one ( dim_num, order_1d, order_nd )

%*****************************************************************************80
%
%% MULTIGRID_INDEX_ONE returns an indexed multidimensional grid.
%
%  Discussion:
%
%    For dimension DIM, the number of points is ORDER_1D(DIM).
%
%    We index the points as
%      1,   2,   3,   ...,  ORDER_1D(DIM).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    A Sparse Grid Stochastic Collocation Method for Partial Differential
%    Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2309-2345.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension of the points.
%
%    Input, integer ORDER_1D(DIM_NUM), the order of the
%    rule in each dimension.
%
%    Input, integer ORDER_ND, the product of the entries of ORDER_1D.
%
%    Output, integer INDX(DIM_NUM,ORDER_ND), the indices of the points in
%    the grid.  The second dimension of this array is equal to the
%    product of the entries of ORDER_1D.
%
  a = [];
  more = 0;
  p = 0;

  while ( 1 )

    [ a, more ] = vec_colex_next2 ( dim_num, order_1d, a, more );

    if ( ~more )
      break
    end

    p = p + 1;
%
%  The values of A(DIM) are between 0 and ORDER_1D(DIM)-1 = N - 1 = 2 * M.
%  Subtracting M sets the range to -M to +M, as we wish.
%
    indx(1:dim_num,p) = a(1:dim_num) + 1;

  end

  return
end


function qr=v_rotqc2qr(qc)
%V_ROTQC2QR converts a matrix of complex quaternion row vectors into real form
%
% Inputs:
%
%     QC(2m,...)   array of complex-valued quaternions
%
% Outputs:
%
%     QR(4m,...)   array of real-valued quaternions
%
% The complex-valued quaternion [r+j*b  a+j*c] becomes [r a b c]

%
%      Copyright (C) Mike Brookes 2000-2018
%      Version: $Id: v_rotqc2qr.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
persistent a b
if isempty(a)
    a=[1 3 2 4];
end
s=size(qc);
s(1)=2*s(1); % each complex number needs two reals
qr=reshape([real(qc(:).'); imag(qc(:).')],4,[]);
qr=reshape(qr(a,:),s);
if ~nargout
    qr=qr(1:4); % just select the first one
    v_rotqr2ro(qr(:)); % plot a rotated cube
end


function point_num = sparse_grid_gl_size ( dim_num, level_max )

%*****************************************************************************80
%
%% SPARSE_GRID_GL_SIZE sizes a sparse grid of Gauss-Legendre points.
%
%  Discussion:
%
%    The grid is defined as the sum of the product rules whose LEVEL
%    satisfies:
%
%      LEVEL_MIN <= LEVEL <= LEVEL_MAX.
%
%    where LEVEL_MAX is user specified, and
%
%      LEVEL_MIN = max ( 0, LEVEL_MAX + 1 - DIM_NUM ).
%
%    The grids are only very weakly nested, since Gauss-Legendre rules
%    only have the origin in common.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 July 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    A Sparse Grid Stochastic Collocation Method for Partial Differential
%    Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2309-2345.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer LEVEL_MAX, the maximum value of LEVEL.
%
%    Output, integer POINT_NUM, the number of points in the grid.
%

%
%  Special case.
%
  if ( level_max == 0 )
    point_num = 1;
    return;
  end
%
%  The outer loop generates LEVELs from LEVEL_MIN to LEVEL_MAX.
%
  point_num = 0;

  level_min = max ( 0, level_max + 1 - dim_num );

  for level = level_min : level_max
%
%  The middle loop generates the next partition that adds up to LEVEL.
%
    level_1d = [];
    more = false;
    h = 0;
    t = 0;

    while ( 1 )

      [ level_1d, more, h, t ] = comp_next ( level, dim_num, level_1d, more, h, t );
%
%  Transform each 1D level to a corresponding 1D order.
%
      order_1d = level_to_order_open ( dim_num, level_1d );

      for dim = 1 : dim_num
%
%  If we can reduce the level in this dimension by 1 and
%  still not go below LEVEL_MIN.
%
        if ( level_min < level & 1 < order_1d(dim) )
          order_1d(dim) = order_1d(dim) - 1;
        end

      end

      point_num = point_num + prod ( order_1d(1:dim_num) );

      if ( ~more )
        break
      end

    end
  end

  return
end


function pass = test_univariate(~)
%TEST_UNIVARIATE   Test various univariate TREEVAR methods.
%   This methods generates a number of anonymous functions that we convert to
%   first order format. We then evaluate them, and check that they match the
%   expected results.

%% Setup
% Initialize a TREEVAR variable.
dom = [0 2];

% Used for setting up anonymous functions we convert to first order form.
rhs = 3.2;
alp = 2;

% Arguments we eventually evaluate the functions at:
tArg = 1;
uArg = [.4 .2];

% List of methods that we want to test:
testMethods = {@abs, @acos, @acosd, @acot, @acoth, @acsc, @acscd, @acsch, ...
    @airy, @asec, @asecd, @asech, @asin, @asind, @asinh, @atan, @atand, ...
    @atanh, @conj, @cos, @cosd, @cosh, @cot, @cotd, @coth, @csc, @cscd, ...
    @csch, @exp, @expm1, @imag, @log, @log10, @log1p, @log2, @pow2, @real, ...
    @sec, @secd, @sech, @sin, @sind, @sinh, @sqrt, @tan, @tand, @tanh, ...
    @uminus, @uplus};

% Store comparison errors:
errors = zeros(length(testMethods), 1);
for mCounter = 1:length(testMethods)
    % Current method we're testing:
    method = testMethods{mCounter};

    % Construct an anonymous function that calls the current method of interest:
    myFun = @(u) diff(u, 2) + diff(u) +  alp*method(u);

    % Convert MYFUN to first order system:
    anonFun = treeVar.toFirstOrder(myFun, rhs, dom);

    % The correct first order reformulation of MYFUN:
    correctFun = @(t,u) [u(2); rhs - u(2) - alp*method(u(1))];

    % Compare the result of evaluating the automatically converted ANONFUN and
    % the manually constructed CORRECTFUN. The difference in the outputs should
    % be very small (if any):
    errors(mCounter) = norm(anonFun(tArg, uArg) - correctFun(tArg, uArg));
end

pass = errors < 10*eps;


function [ n_data, x, fx ] = goodwin_values ( n_data )

%*****************************************************************************80
%
%% GOODWIN_VALUES returns some values of the Goodwin and Staton function.
%
%  Discussion:
%
%    The function is defined by:
%
%      GOODWIN(x) = Integral ( 0 <= t < infinity ) exp ( -t^2 ) / ( t + x ) dt
%
%    The data was reported by McLeod.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Allan McLeod,
%    Algorithm 757, MISCFUN: A software package to compute uncommon
%      special functions,
%    ACM Transactions on Mathematical Software,
%    Volume 22, Number 3, September 1996, pages 288-301.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 20;

  fx_vec = [ ...
      0.59531540040441651584E+01, ...
      0.45769601268624494109E+01, ...
      0.32288921331902217638E+01, ...
      0.19746110873568719362E+01, ...
      0.96356046208697728563E+00, ...
      0.60513365250334458174E+00, ...
      0.51305506459532198016E+00, ...
      0.44598602820946133091E+00, ...
      0.37344458206879749357E+00, ...
      0.35433592884953063055E+00, ...
      0.33712156518881920994E+00, ...
      0.29436170729362979176E+00, ...
      0.25193499644897222840E+00, ...
      0.22028778222123939276E+00, ...
      0.19575258237698917033E+00, ...
      0.17616303166670699424E+00, ...
      0.16015469479664778673E+00, ...
      0.14096116876193391066E+00, ...
      0.13554987191049066274E+00, ...
      0.11751605060085098084E+00 ];

  x_vec = [ ...
      0.0019531250E+00, ...
      0.0078125000E+00, ...
      0.0312500000E+00, ...
      0.1250000000E+00, ...
      0.5000000000E+00, ...
      1.0000000000E+00, ...
      1.2500000000E+00, ...
      1.5000000000E+00, ...
      1.8750000000E+00, ...
      2.0000000000E+00, ...
      2.1250000000E+00, ...
      2.5000000000E+00, ...
      3.0000000000E+00, ...
      3.5000000000E+00, ...
      4.0000000000E+00, ...
      4.5000000000E+00, ...
      5.0000000000E+00, ...
      5.7500000000E+00, ...
      6.0000000000E+00, ...
      7.0000000000E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    x = 0.0;
    fx = 0.0;
  else
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end