function [pbest,perror,nchi2]=cnonlin(mfunc,x,y,sy,pt,v);
% function [pbest,perror,nchi2]=cnonlin(mfunc,x,y,sy,pt,v);
%
% Levenberg-Marquardt non-linear regression for complex functions.
% mfunc=name of the function file that calculates the function to be
%    fitted to the set of data. mfunc is a string variable, so
%    the name of the file must be put inside quotes, eg. 'model'.
%    The function file written to calculate your function should
%    be of the form:
%
%    function yfit=model(x,p)
%    yfit=p(1)+p(2)*x;
%
%    The example above implements y=a+bx.
%    The vector x contains the values of the independent variable.
%    The vector p contains whatever parameters you are using.
%
% x=vector of the independent variable (e.g. Time).
% y=vector of the dependent variable (e.g. Counts recorded).
% sy=vector of standard error in y values.
% pt=initial estimate of parameters to be fitted. All parameters
%   must be non-zero, and are assumed to be real.
% v=vector indicating which parameters are to be varied (fitted), and
%   which are to be held fixed. In most cases we will want all parameters
%   to be varied, but in some circumstances it is useful to hold certain
%   parameters fixed while the others are varied. v is a vector of the same
%   length as p, and should contain only ones and zeros; a one indicates that
%   the corresponding parameter should be varied (fitted), and a zero
%   indicates that the corresponding parameter should be held constant.
%   eg. [1,0,1,1] would be used to keep the second parameter fixed while
%   varying the rest (in the case of four parameters).
%
% OUTPUT VARIABLES
% pbest=fitted parameters
% perror=standard error in fitted parameters
% nchi2=normalised chi-squared parameter (=chi-Squared/degrees of freedom). This
%    is expected to be close to 1 for a good fit.

% Written 4/12/95 by Michael Fleming, University Of Auckland

chiCut=0.01;  lambda=0.001;  stepSize=0.001;  chiOld=Inf;
y=y(:);  x=x(:);  sy=sy(:);  pt=pt(:);  v=v(:);  maxIter=50;
m=length(pt); n=length(x);

evalstr=[mfunc '(x,['];      % Create string containing function call
count=0;
for i=1:m
 if v(i)==1         % Extract vector p of parameters which will be varied
  count=count+1;    % from vector pt which contains all input parameters
  p(count)=pt(i);
  evalstr=[evalstr 'p(' int2str(count) ') '];
 else
  evalstr=[evalstr num2str(pt(i)) ' '];
 end;
 if (pt(i)==0)   % Check all parameters are non-zero
  error('You have entered a zero parameter.All parameters must be nonzero');
 end;
end;
evalstr=[evalstr '])'];

dof=n-count;    p=p(:);   delta=p*stepSize;
% delta is the size of the small increment used for calculating numerical derivatives
% dof is the number of degrees of freedom

chiSqr=sum((abs(y-eval(evalstr))./sy).^2);     % Calc ChiSqr from initial parameters
if chiSqr/dof>5000;
 disp('You have made a bad choice of initial parameters');
end;

iter=0;
while (abs(chiOld-chiSqr)>chiCut)&(iter<maxIter);
 iter=iter+1;
 chiOld=chiSqr;
 [alpha,beta]=cnonlin2(evalstr,x,y,sy,p,delta,lambda);
 if det(alpha)==0;
  error('No convergence - try a different set of parameters');
 end;
 dp=alpha\beta;    % Evaluate parameter increments
 p=p+dp;
 chiSqr=sum((abs(y-eval(evalstr))./sy).^2);
 while (chiSqr>(chiOld+chiCut));
  p=p-dp;
  iter=iter+1;  lambda=lambda*10;
  [alpha,beta]=cnonlin2(evalstr,x,y,sy,p,delta,lambda);
  if det(alpha)==0;
   error('No convergence - try a different set of parameters');
  end;
  dp=alpha\beta;    % Evaluate parameter increments
  p=p+dp;
  chiSqr=sum((abs(y-eval(evalstr))./sy).^2);
 end;
 lambda=0.1*lambda;
end;

if iter==maxIter,
 disp('Maximum number of iterations exceeded - convergence not achieved');
end;

%xfine=linspace(x(1),x(n),201);    % Print graph of function
%yfitted=model(xfine,p);
%errorbar(x,y,sy);
% Calculate correlation coefficicent R^2
%f=eval(evalstr);
%r=corrcoef(y,f);
%r2=r(1,2).^2;

% Calculate standard errors in parameters
[alpha,beta]=cnonlin2(evalstr,x,y,sy,p,delta,0);	% Calculate final alpha matrix
sp=sqrt(diag(inv(alpha)));   % Evaluate standard errors in parameters.
disp('Parameters');
count=0;
for i=1:m
 if v(i)==1
  count=count+1;
  pbest(i)=p(count); perror(i)=sp(count);
  disp(['p(', int2str(i), ') = ' sprintf('%5.3e',p(count)), '+-' sprintf('%5.3e',sp(count))]);
 else
  disp(['p(', int2str(i), ') = ' sprintf('%5.3e',pt(i)), '+-0']);
  pbest(i)=pt(i); perror(i)=0;
 end;
end;

format compact
%disp('Correlation Coefficient R^2');
%disp(r2);
disp('Normalised chi-squared value');
nchi2=chiSqr/dof;
disp(nchi2);
format;


function [ p, l, u ] = maxij_plu ( n )

%*****************************************************************************80
%
%% MAXIJ_PLU returns the PLU factors of the MAXIJ matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 June 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Output, real P(N,N), L(N,N), U(N,N), the PLU factors.
%
  p = zeros ( n, n );

  for j = 1 : n
    for i = 1 : n
      if ( i4_wrap ( j - i, 1, n ) == 1 )
        p(i,j) = 1.0;
      end
    end
  end

  l = zeros ( n, n );

  i = 1;
  j = 1;
  l(i,j) = 1.0;

  j = 1;
  for i = 2 : n
    l(i,j) = ( i - 1 ) / n;
  end

  for j = 2 : n
    l(j,j) = 1.0;
  end

  u = zeros ( n, n );

  i = 1;
  for j = 1 : n
    u(i,j) = n;
  end

  for i = 2 : n
    for j = i : n
      u(i,j) = j + 1 - i;
    end
  end

  return
end


function [f,g]=ycfun2(x);
f=[x(1)+2*x(1)^2+x(2)+2*x(2)^2+x(3)-10
   x(1)+x(1)^2+x(2)+x(2)^2-x(3)-50
   2*x(1)+x(1)^2+2*x(2)+x(3)-40];
g=x(1)^2+x(3)-2;


% Test file for chebtech1/coeffs2vals.m

function pass = test_coeffs2vals(varargin)

% Set a tolerance (pref.chebfuneps doesn't matter)
tol = 100*eps;

%%
% Test that a single coefficient is converted correctly
c = sqrt(2);
v = chebtech1.coeffs2vals(c);
pass(1) = (c == v);

%%
% Simple data (even case)
c = (6:-1:1).';
% Exact values
vTrue = [ -3*sqrt(6)/2-5/sqrt(2)+2*sqrt(3)+7 ; 4 - sqrt(2)/2 ; -3*sqrt(6)/2+5/sqrt(2)-2*sqrt(3)+7 ; 3*sqrt(6)/2-5/sqrt(2)-2*sqrt(3)+7 ; 4 + sqrt(2)/2 ; 3*sqrt(6)/2+5/sqrt(2)+2*sqrt(3)+7];

%%
% Test real branch
v = chebtech1.coeffs2vals(c);
pass(2) = norm(v - vTrue, inf) < tol;
pass(3) = ~any(imag(v));

%%
% Test imaginary branch
v = chebtech1.coeffs2vals(1i*c);
pass(4) = norm(v - 1i*vTrue, inf) < tol;
pass(5) = ~any(real(v));

%%
% Test general branch
v = chebtech1.coeffs2vals((1+1i)*c);
pass(6) = norm(v - (1+1i)*vTrue, inf) < tol;

%%
% Test for array input
v = chebtech1.coeffs2vals([c, -c]);
pass(7) = norm(v(:,1) - vTrue, inf) < tol && ...
          norm(v(:,2) + vTrue, inf) < tol;

%%
% Simple data (odd case)
c = (5:-1:1).';
% Exact values
vTrue = [ 11/2+sqrt(5)-2*sqrt((5+sqrt(5))/2)-sqrt((5-sqrt(5))/2) ; 11/2-sqrt(5)-2*sqrt((5-sqrt(5))/2)+sqrt((5+sqrt(5))/2) ; 3 ; 11/2-sqrt(5)+2*sqrt((5-sqrt(5))/2)-sqrt((5+sqrt(5))/2) ; 11/2+sqrt(5)+2*sqrt((5+sqrt(5))/2)+sqrt((5-sqrt(5))/2) ];

%%
% Test real branch
v = chebtech1.coeffs2vals(c);
pass(8) = norm(v - vTrue, inf) < tol;
pass(9) = ~any(imag(v));

%%
% Test imaginary branch
v = chebtech1.coeffs2vals(1i*c);
pass(10) = norm(v - 1i*vTrue, inf) < tol;
pass(11) = ~any(real(v));

%%
% Test general branch
v = chebtech1.coeffs2vals((1+1i)*c);
pass(12) = norm(v - (1+1i)*vTrue, inf) < tol;

%%
% Test for array input
v = chebtech1.coeffs2vals([c, -c]);
pass(13) = norm(v(:,1) - vTrue, inf) < tol && ...
          norm(v(:,2) + vTrue, inf) < tol;

%%
% Test for symmetry preservation
c = kron(ones(10,1),eye(2));
v = chebtech1.coeffs2vals(c);
pass(14) = norm(v(:,1) - flipud(v(:,1)), inf) == 0 && ...
          norm(v(:,2) + flipud(v(:,2)), inf) == 0;

end


clc; clear all; close all;

meshPath = '../data/woody.obj';
[V,F] = readOBJ(meshPath);
V = V(:,1:2); % the input mesh contains redundant zero third column

% get handles
fig = tsurf(F,V);
axis equal;
fprintf( ...
    ['Point Handle Selection: \n' ...
    '- CLICK the mesh to add point handls \n', ...
    '- BACKSPACE to remvoe the previous selection\n', ...
    '- ENTER to finish selection\n'] ...
    );
try
  [Cx,Cy] = getpts;
catch e
  return  % quit early, stop script
end

C = [Cx,Cy]; % handle locations

% compute pairwise distance
D = zeros(size(V,1), size(C,1));
for ii = 1:size(C,1)
    D(:,ii) = sqrt(sum((V - C(ii,:)).^2,2));
end

% geet handle indices b
[~,b] = min(D);

% TODO: (unbounded) biharmonic weights
W = compute_skinning_weight(V,F,b);

% TODO: linear blended skinning
deform_GUI(V,F,C,W);


function V = solveV_eig(U, Us, Vs, Ss, reg_smooth, reg_l2)
    % solve S analytically using eigen-decomposition
    %
    % TODO: check with optimization form.

    n = length(Ss);

    UtU = U' * U;

    [Q1, Lu] = eig(UtU);

    V = cell(n, 1);

    for ii = 1:n
        V{ii} = solveVi(U, Us, Q1, diag(Lu) + reg_l2, Vs{ii}, Ss{ii}, reg_smooth);
    end

end

function Vi = solveVi (U, Us, Q1, Lu, Vsi, Ssi,  reg_smooth)

ti = size(Ssi, 2);

iy  = [1: (ti - 1), 1: (ti - 1)];
ix  = [1: (ti - 1), 2: ti];
val = [ones(1, ti-1), -1 * ones(1, ti-1)];
Ri  = sparse(ix, iy, val, ti, ti-1);

%[Q2, Lr] = eig(reg_smooth * (Ri * Ri'), ti);
[Q2, Lr] = eig(reg_smooth * full(Ri * Ri'));
Lr = diag(Lr);

UtSi = Q1' * (U' * Us) * Vsi * Q2 + Q1' * (U' * Ssi) * Q2;

V_hat = zeros(size(UtSi));

for i = 1:size(V_hat, 1)
    for j = 1: size(V_hat, 2)
        V_hat(i, j) = UtSi(i, j) / (Lu(i) + Lr(j));
    end
end

Vi = Q1 * V_hat * Q2';

end


%%%%%script to generate figure 7 from the paper 'Invariant Scattering Convolution Networks'
%%%%%feb 2012%%%%%%%
%%%%%%Joan Bruna and Stephane Mallat$$$$$$$$$

%%%%%This figure displays the scattering coefficients of
%%%%%% a digit taken from the MNIST dataset.

Nim=32;
copts.renorm_process=0;
copts.l2_renorm=1;
foptions.J=3;
foptions.L=8;
soptions.M=2;
soptions.oversampling = 0;
[Wop,filters]=wavelet_factory_2d([Nim Nim], foptions, soptions);

dirac=zeros(Nim);
dirac(1)=1;
dirac=fftshift(dirac);
[scdirac]=scat(dirac,Wop);

tmp=load('mnist_sample.mat');
im=tmp.im;

%compute scattering coefficients
[sc_digit]=scat(im,Wop);

sc=format_scat(sc_digit);
S=size(sc);

%construct the grid with the scattering displays

drawing1=[];
drawing2=[];
for s1=1:S(2)
	row1=[];
	row2=[];
	for s2=1:S(3)
		tempo=scat_display(sc_digit,scdirac,copts,sc(:,s1,s2));
		row1=[row1 tempo{1}];
		row2=[row2 tempo{2}];
	end
	drawing1=[drawing1 ; row1];
	drawing2=[drawing2 ; row2];
end

[N,M]=size(drawing1);

digit_bg=imresize(im,[N M]);
digit_bg=min(255,max(0,digit_bg));

atten=0.0;

fact1=atten* max(drawing1(:)) / max(digit_bg(:));
fact2=atten* max(drawing2(:)) / max(digit_bg(:));

gg=colormap(gray);
invgg=1-gg;
figure
imagesc(drawing1)
set(gca,'XTick',[]);
set(gca,'YTick',[]);
axis square

figure
imagesc(drawing2)
set(gca,'XTick',[]);
set(gca,'YTick',[]);
axis square

figure
imagesc(digit_bg);
colormap(invgg);
set(gca,'XTick',[]);
set(gca,'YTick',[]);
axis square


function [summa, index] = max_sum(v, n)
    [~, len] = size(v);
    summa = -inf;
    if n > len
        summa = 0;
    end
    index = -1;
    for i = 1:(len - n + 1)
        s = sum(v(i : i + n - 1));
        if s > summa
            summa = s;
            index = i;
        end
    end
end


%This Matlab script can be used to reproduce Figure 7.27 in the monograph:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.1 (Last edited: 2018-10-10)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

%Empty workspace and close figures
close all;
clear;

%% Define parameters
S = [1, 16, 256]; %Number of subarrays
M = 256./S; %Number of antennas per subarray
d0 = 10; %Guard distance to the closest subarray
W = 350; %Cell radii
numberOfSetups = 5000; %Number of random subarray drops

%Propagation model parameters as in (2.3)
alpha = 3.76;
Upsilon = -148.1;
sigma_sf = 8;
pathlossdB = @(d) Upsilon-10*alpha*log10(d/1000);
addShadowing = @(pldB) 10.^((pldB +  sigma_sf*randn(size(pldB)))/10);

%% Monte Carlo simulations
betaValues = zeros(numel(S), numberOfSetups);

%Go through the number of subarrays
for s = 1:numel(S)

    %Go through all drops of subarrays
    for it1 = 1:numberOfSetups

        d = sort(sqrt(rand(S(s),1)*(W^2-d0^2) + d0^2)); %The squared distances of uniformly distributed points between two circles are uniformly distributed
        pldB = pathlossdB(d); %Compute pathloss without shadow fading
        pl = addShadowing(pldB); %Add random shadow fading

        %Compute average channel gain
        betaValues(s,it1) = sum(pl)/S(s);

    end

end

%% Plot the simulation results
figure;
hold on; box on;
plot(sort(10*log10(betaValues(1,:))),linspace(0,1,numberOfSetups), 'LineStyle', '-', 'Color', 'k', 'LineWidth', 1);
plot(sort(10*log10(betaValues(2,:))),linspace(0,1,numberOfSetups), 'LineStyle', '--', 'Color', 'r',  'LineWidth', 1);
plot(sort(10*log10(betaValues(3,:))),linspace(0,1,numberOfSetups), 'LineStyle', '-.', 'Color', 'b',  'LineWidth', 1);

xlabel('Average channel gain [dB]')
ylabel('CDF')
legend({['S=', num2str(S(1))],['S=', num2str(S(2))],['S=', num2str(S(3))]}, 'Location', 'NorthWest')
xlim([-160 -60]);


function [ x, r, z, p, q, job ] = cg_rc ( n, b, x, r, z, p, q, job )

%*****************************************************************************80
%
%% CG_RC is a reverse communication conjugate gradient routine.
%
%  Discussion:
%
%    This routine seeks a solution of the linear system A*x=b
%    where b is a given right hand side vector, A is an n by n
%    symmetric positive definite matrix, and x is an unknown vector
%    to be determined.
%
%    Under the assumptions that the matrix A is large and sparse,
%    the conjugate gradient method may provide a solution when
%    a direct approach would be impractical because of excessive
%    requirements of storage or even of time.
%
%    The conjugate gradient method presented here does not require the
%    user to store the matrix A in a particular way.  Instead, it only
%    supposes that the user has a way of calculating
%      y = alpha * A * x + b * y
%    and of solving the preconditioned linear system
%      M * x = b
%    where M is some preconditioning matrix, which might be merely
%    the identity matrix, or a diagonal matrix containing the
%    diagonal entries of A.
%
%    This routine was extracted from the "templates" package.
%    There, it was not intended for direct access by a user;
%    instead, a higher routine called "cg()" was called once by
%    the user.  The cg() routine then made repeated calls to
%    cgrevcom() before returning the result to the user.
%
%    The reverse communication feature of cgrevcom() makes it, by itself,
%    a very powerful function.  It allows the user to handle issues of
%    storage and implementation that would otherwise have to be
%    mediated in a fixed way by the function argument list.  Therefore,
%    this version of cgrecom() has been extracted from the templates
%    library and documented as a stand-alone procedure.
%
%    The user sets the value of JOB to 1 before the first call,
%    indicating the beginning of the computation, and to the value of
%    2 thereafter, indicating a continuation call.
%    The output value of JOB is set by cgrevcom(), which
%    will return with an output value of JOB that requests a particular
%    new action from the user.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 January 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Richard Barrett, Michael Berry, Tony Chan, James Demmel,
%    June Donato, Jack Dongarra, Victor Eijkhout, Roidan Pozo,
%    Charles Romine, Henk van der Vorst,
%    Templates for the Solution of Linear Systems:
%    Building Blocks for Iterative Methods,
%    SIAM, 1994,
%    ISBN: 0898714710,
%    LC: QA297.8.T45.
%
%  Parameters:
%
%    Input, integer N, the dimension of the matrix.
%
%    Input, real B(N), the right hand side vector.
%
%    Input, real X(N).  On first call, the user
%    should store an initial guess for the solution in X.
%
%    Input, real R(N), Z(N), P(N), Q(N), work arrays.  The user should
%    create each of these before the first call, using the zeros() command.
%    On subsequent calls, the user may be asked to assign a value to one
%    of these vectors.
%
%    Input, integer JOB, communicates the task to be done.
%    The user needs to set the input value of JOB to 1, before the first call,
%    and then to 2 for every subsequent call for the given problem.
%
%    Output, real X(N), the current solution estimate.
%    Each time JOB is returned as 4, X has been updated.
%
%    Output, real R(N), Z(N), P(N), Q(N), work arrays.  Depending on the
%    output value of JOB, the user may be asked to carry out a computation
%    involving some of these vectors.
%
%    Output, integer JOB, communicates the task to be done.
%    * JOB = 1, compute Q = A * P;
%    * JOB = 2: solve M*Z=R, where M is the preconditioning matrix;
%    * JOB = 3: compute R = R - A * X;
%    * JOB = 4: check the residual R for convergence.
%               If satisfactory, terminate the iteration.
%               If too many iterations were taken, terminate the iteration.
%

%
%  Local variables should be SAVED between calls.
%
  persistent iter
  persistent rho
  persistent rho_old
  persistent rlbl
%
%  Initialization.
%  Ask the user to compute the initial residual.
%
  if ( job == 1 )

    r(1:n) = b(1:n);

    job = 3;
    rlbl = 2;
%
%  Begin first conjugate gradient loop.
%  Ask the user for a preconditioner solve.
%
  elseif ( rlbl == 2 )

    iter = 1;

    job = 2;
    rlbl = 3;
%
%  Compute the direction.
%  Ask the user to compute ALPHA.
%  Save A*P to Q.
%
  elseif ( rlbl == 3 )

    rho = r(:)' * z(:);

    if ( 1 < iter )
      beta = rho / rho_old;
      z(1:n) = z(1:n) + beta * p(1:n);
    end

    p(1:n) = z(1:n);

    job = 1;
    rlbl = 4;
%
%  Compute current solution vector.
%  Ask the user to check the stopping criterion.
%
  elseif ( rlbl == 4 )

    pdotq = p(:)' * q(:);
    alpha = rho / pdotq;
    x(1:n) = x(1:n) + alpha * p(1:n);
    r(1:n) = r(1:n) - alpha * q(1:n);

    job = 4;
    rlbl = 5;
%
%  Begin the next step.
%  Ask for a preconditioner solve.
%
  elseif ( rlbl == 5 )

    rho_old = rho;
    iter = iter + 1;

    job = 2;
    rlbl = 3;

  end

  return
end


function [A, b, x, ProbInfo] = PRblurgauss(varargin)
% PRblurgauss Image deblurring problem with a Gaussian point spread function
%
% [A, b, x, ProbInfo] = PRblurgauss
% [A, b, x, ProbInfo] = PRblurgauss(n)
% [A, b, x, ProbInfo] = PRblurgauss(n, options)
% [A, b, x, ProbInfo] = PRblurgauss(options)
%
% Generates data for use in image deblurring problems with a Gaussian
% point spread function.
%
% Input:
%  n      -  size of the image. Can be either a scalar n (in this case
%            the size is n x n) or a vector [nrow, ncol] (in this case the
%            size is (nrow x ncol).
%            Default: n = 256.
%  options - Structure containing the following optional fields:
%    trueImage  : test image of size n, of type numeric, 2-D only,
%                 or character string indicating
%                 'pattern1'  : geometrical image
%                 'pattern2'  : geometrical image
%                 'sppattern' : sparse (edges of ) a geometrical image
%                 'ppower'    : random image with patterns of nonzero pixels
%                 'smooth'    : very smooth image
%                 'dot2'      : two small Gaussian shaped dots, e.g., a
%                               binary star
%                 'dotk'      : n/2 small Gaussian shaped dots, e.g., stars
%                               (placement is random, reset using rng(0))
%                 'satellite' : satellite test image
%                 'hst'       : image of the Hubble space telescope
%                 Default: 'hst'.
%                 This image is then stored in the output vector x.
%    BlurLevel  : If choosing one of the built-in PSFs, this sets the
%                 severity of the blur to one of the following:
%                 'mild'
%                 'medium'
%                 'severe'
%                 Default is 'medium'
%    BC         : Specify boundary condition:
%                 'zero'
%                 'periodic'
%                 'reflective' (or 'neumann' or 'reflexive')
%                 Default: 'reflective'
%                 Note that in this case an extended (or padded) test image
%                 is blurred using 'zero' boundary conditions, and then the
%                 central subimage of size n is extracted from the exact and
%                 the blurred image. No inverse crime is committed,
%                 i.e., A*x ~= b.
%    CommitCrime: To get an exact system Ax = b (i.e., commit the inverse
%                 crime), set this to:
%                 'on'
%                 Default is 'off' (do not commit the inverse crime).
%
% Output:
%  A        - blurring matrix, which is a either a psfMatrix object in
%             the case of spatically invariant blur, or a sparse matrix
%             in the case of spatially variant blur
%  b        - blurred vector (i.e., blurred image with stacked columns)
%  x        - image vector, i.e., exact (unknown) image with stacked columns
%  ProbInfo - structure whose fields contain information about problem:
%               problemType : kind of test problem generated
%                             (in this case: 'deblurring')
%               xType       : solution type (in this case 'image2D')
%               bType       : data type (in this case 'image2D')
%               xSize       : size of image x
%               bSize       : size of image b
%               psf         : point spread function
%
% See also: PRblur, PRblurdefocus, PRblurmotion, PRblurrotation,
% PRblurshake, PRblurspeckle, PRdiffusion, PRinvinterp2, PRnmr,
% PRseismic, PRspherical, PRtomo, PRnoise, PRshowb, PRshowx, fspecial

% Silvia Gazzola, University of Bath
% Per Christian Hansen, Technical University of Denmark
% James G. Nagy, Emory University
% April, 2018.

% This file is part of the IR Tools package and is distributed under the
% 3-Clause BSD License. A separate license file should be provided as part
% of the package.

% Set default values for options.
defaultopt = struct('trueImage', 'hst', 'BlurLevel', 'medium', ...
    'BC', 'reflective', 'CommitCrime', 'off');

% If input is 'defaults,' return the default options in X
if nargin == 1 && nargout <= 1 && strcmp(varargin,'defaults')
    A = defaultopt;
    return;
end

% Check for acceptable number of optional input arguments
switch length(varargin)
    case 0
        n = []; options = [];
    case 1
        if isa(varargin{1}, 'double')
            n = varargin{1}; options = [];
        else
            n = []; options = varargin{1};
        end
    case 2
        if isa(varargin{1}, 'double')
            n = varargin{1}; options = varargin{2};
        else
            n = varargin{2}; options = varargin{1};
        end
    otherwise
        error('Too many input parameters')
end

if isempty(options)
    options = defaultopt;
end

options = PRset(defaultopt, options);
options = PRset(options, 'PSF', 'gauss');
[A, b, x, ProbInfo] = PRblur(n, options);


function x = discretesample(p, n,varargin)
% Samples from a discrete distribution
%
%   x = discretesample(p, n)
%       independently draws n samples (with replacement) from the
%       distribution specified by p, where p is a probability array
%       whose elements sum to 1.
%
%       Suppose the sample space comprises K distinct objects, then
%       p should be an array with K elements. In the output, x(i) = k
%       means that the k-th object is drawn at the i-th trial.
%
%   Remarks
%   -------
%       - This function is mainly for efficient sampling in non-uniform
%         distribution, which can be either parametric or non-parametric.
%
%       - The function is implemented based on histc, which has been
%         highly optimized by mathworks. The basic idea is to divide
%         the range [0, 1] into K bins, with the length of each bin
%         proportional to the probability mass. And then, n values are
%         drawn from a uniform distribution in [0, 1], and the bins that
%         these values fall into are picked as results.
%
%       - This function can also be employed for continuous distribution
%         in 1D/2D dimensional space, where the distribution can be
%         effectively discretized.
%
%       - This function can also be useful for sampling from distributions
%         which can be considered as weighted sum of "modes".
%         In this type of applications, you can first randomly choose
%         a mode, and then sample from that mode. The process of choosing
%         a mode according to the weights can be accomplished with this
%         function.
%
%   Examples
%   --------
%       % sample from a uniform distribution for K objects.
%       p = ones(1, K) / K;
%       x = discretesample(p, n);
%
%       % sample from a non-uniform distribution given by user
%       x = discretesample([0.6 0.3 0.1], n);
%
%       % sample from a parametric discrete distribution with
%       % probability mass function given by f.
%       p = f(1:K);
%       x = discretesample(p, n);
%

%   Created by Dahua Lin, On Oct 27, 2008
%

%% parse and verify input arguments

assert(isfloat(p), 'discretesample:invalidarg', ...
  'p should be an array with floating-point value type.');

assert(isnumeric(n) && isscalar(n) && n >= 0 && n == fix(n), ...
  'discretesample:invalidarg', ...
  'n should be a nonnegative integer scalar.');

%% main

if numel(p) == 1 && p <= 1 && nargin == 2

  x = ones(1,n);
  return

elseif numel(p) == 1

  if nargin == 2 % without replacement
    if 4*n > p
      rp = randperm(p);
      x = rp(1:n);

    else
      f = zeros(1,p); % flags
      sumf = 0;
      while sumf < n
        f(ceil(p * rand(1,n-sumf))) = 1; % sample w/replacement
        sumf = sum(f); % count how many unique elements so far
      end
      x = find(f > 0);
      x = x(randperm(n));
    end
  else % with replacement
    x = ceil(p * rand(1,n));
  end

  return
end

% process p if necessary
K = numel(p);
if ~isequal(size(p), [1, K])
    p = reshape(p, [1, K]);
end

% construct the bins
edges = [0, cumsum(p)];
s = edges(end);
if abs(s - 1) > eps, edges = edges * (1 / s); end

% draw bins
rv = rand(1, n);
c = histcounts(rv, edges);

% extract samples
xv = find(c);

if numel(xv) == n  % each value is sampled at most once
  x = xv;
else                % some values are sampled more than once
  xc = c(xv);
  d = zeros(1, n);
  dv = [xv(1), diff(xv)];
  dp = [1, 1 + cumsum(xc(1:end-1))];
  d(dp) = dv;
  x = cumsum(d);
end

% randomly permute the sample's order
x = x(randperm(n));


% ========================================================================
% USAGE: [Coeff]=LLC_coding_appr(B,X,knn,lambda)
% Approximated Locality-constraint Linear Coding
%
% Inputs
%       B       -M x d codebook, M entries in a d-dim space
%       X       -N x d matrix, N data points in a d-dim space
%       knn     -number of nearest neighboring
%       lambda  -regulerization to improve condition
%
% Outputs
%       Coeff   -N x M matrix, each row is a code for corresponding X
%
% Jinjun Wang, march 19, 2010
% URL: http://www.ifp.illinois.edu/~jyang29/codes/CVPR10-LLC.rar
%
% Minor edits by Aditya Khosla
% ========================================================================

function [Coeff] = LLC_coding_appr(B, X, knn, beta)

if ~exist('knn', 'var') || isempty(knn),
    knn = 5;
end

if ~exist('beta', 'var') || isempty(beta),
    beta = 3e-2;
end

nframe=size(X,1);
nbase=size(B,1);

% find k nearest neighbors
D = sp_dist2(X, B);
[~, sort_idx] = sort(D, 2, 'ascend');
IDX = sort_idx(:, 1:knn);

% llc approximation coding
II = eye(knn, knn);
Coeff = zeros(nframe, nbase);
for i=1:nframe
   idx = IDX(i,:);
   z = bsxfun(@minus, B(idx,:), X(i,:));           % shift ith pt to origin
   C = z*z';                                        % local covariance
   C = C + II*beta*trace(C);                        % regularlization (K>D)
   w = C\ones(knn,1);
   w = w/sum(w);                                    % enforce sum(w)=1
   Coeff(i,idx) = w';
end


function score = IGDp(Population,optimum)
% <min> <multi/many> <real/integer/label/binary/permutation> <large/none> <constrained/none> <expensive/none> <multimodal/none> <sparse/none> <dynamic/none>
% Inverted generational distance plus (IGD+)

%------------------------------- Reference --------------------------------
% H. Ishibuchi, H. Masuda, Y. Tanigaki, and Y. Nojima. Modified distance
% calculation in generational distance and inverted generational distance,
% Proceedings of the International Conference on Evolutionary
% Multi-Criterion Optimization, 2015, 110-125.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    PopObj = Population.best.objs;
    if size(PopObj,2) ~= size(optimum,2)
        score = nan;
    else
        [Nr,M] = size(optimum);
        [N,~]  = size(PopObj);
        delta  = zeros(Nr,1);
        for i = 1 : Nr
            delta(i) = min(sqrt(sum(max(PopObj - repmat(optimum(i,:),N,1),zeros(N,M)).^2,2)));
        end
        score = mean(delta);
    end
end


function [ ft ] = gsp_time_translate(G, f,tau,param)
%GSP_TIME_TRANSLATE Generalized time-vertex translation of the signal f to the node i and time t
%   Usage: ft = gsp_time_translate(G, f,tau,param);
%
%   Input parameters
%       G   : Time-Vertex Graph structure
%       f   : Time-Vertex signal
%       tau : Time location
%   Output parameters
%       ft  : Translated signal
%
%   This function translate the time-vertex signal *f* at time tau.
%
%   Additional parameters
%   ---------------------
%   * *param.boundary*  : Time boundary condition for the translation:
%                                                'periodic' (fft),  (default)
%                                                'reflecting' (dct),
%                                                'absorbing' (zero-padding).
%
%
% Author :  Francesco Grassi

if nargin<5
    param=struct;
end

if ~isfield(param,'boundary'), param.boundary='periodic'; end

[N,T] = size(f);

error('Change the boundary!')

switch param.boundary
    case 'periodic'
        delta = gsp_delta(G.jtv.T,tau).';
        fhat = fft(f,[],2);
        operator = repmat(fft(delta,[],2),N,1);
        ft = sqrt(G.N)*ifft(fhat .* operator,[],2);

    case 'symmetric'
        delta = gsp_delta(G.jtv.T,tau).';
        fhat = dct(f.').';
        operator = repmat(dct(delta.').',N,1);
        ft = sqrt(G.N)*idct( (fhat .* operator).' ).';

    case 'absorbing'

        ind = round(-tau*G.jtv.fs);
        ft = circshift(f,[0 ind]);

        if tau >0

            ft = [ft(:,1:end+ind) zeros(N,-ind)];

        else

            ft = [zeros(N,ind-1) ft(:,ind+1:end)];

        end

    otherwise
        error('Unknown boundary condition');

end

end


function [out] = saturation_3(S,Smax,p1,In)
%saturation_3

% Copyright (C) 2019, 2021 Wouter J.M. Knoben, Luca Trotter
% This file is part of the Modular Assessment of Rainfall-Runoff Models
% Toolbox (MARRMoT).
% MARRMoT is a free software (GNU GPL v3) and distributed WITHOUT ANY
% WARRANTY. See <https://www.gnu.org/licenses/> for details.

% Flux function
% ------------------
% Description:  Saturation excess from a store with different degrees of
%               saturation (exponential variant)
% Constraints:  -
% @(Inputs):    S    - current storage [mm]
%               Smax - maximum contributing storage [mm]
%               p1   - linear scaling parameter [-]
%               In   - incoming flux [mm/d]

out = (1-(1/(1+exp((S/Smax + 0.5)/p1)))).*In;

end


A=magic(4)
sum(A)
sum(A')
sum(diag(A))
sum(diag(fliplr(A)))


function X = tdma(A,B,C,D)
%TriDiagonal Matrix Algorithm (TDMA) or Thomas Algorithm
% A_i*X_(i-1) + B_i*X_i + C_i*X_(i+1) = D_i (where A_1 = 0, C_n = 0)
% A,B,C,D are input vectors. X is the solution, also a vector.

% Copyright 2013 The MathWorks, Inc.

Cp = C;
Dp = D;
n = length(A);
X = zeros(n,1);
% Performs Gaussian elimination
Cp(1) = C(1)/B(1);
Dp(1) = D(1)/B(1);
for i = 2:n
Cp(i) = C(i)/(B(i)-Cp(i-1)*A(i));
Dp(i) = (D(i)-Dp(i-1)*A(i))/(B(i)-Cp(i-1)*A(i));
end
% Backward substitution, since X(n) is known first.
X(n) = Dp(n);
for i = n-1:-1:1
X(i) = Dp(i)-Cp(i)*X(i+1);
end


function [in] = mm2in(mm)
% Convert length from millimeters to inches.
% Chad A. Greene 2012
in = mm/25.4;


clear, clc
        % Definirea variabilelor simbolice
syms ud uq Rs Ls Rr Lr id iD iq iQ M w
syms diddt diqdt diDdt diQdt
        % Variabilele utilizate care nu corespund exact cu simbolurile utilizate
        % - w - omega
        % - diddt - d(id)/dt  si  diqdt - d(iq)/dt
        % - diDdt - d(iD)/dt  si  diQdt - d(iQ)/dt
        % Incarcarea celor patru ecuatii
eq1=-ud+Rs*id+Ls*diddt+M*diddt+M*diDdt;
eq2=-uq+Rs*iq+Ls*diqdt+M*diqdt+M*diQdt;
eq3=Rr*iD+Lr*diDdt+M*diddt+M*diDdt+w*Lr*iQ+w*M*(iq+iQ);
eq4=Rr*iQ+Lr*diQdt+M*diqdt+M*diQdt-w*Lr*iD-w*M*(id+iD);
        % Rezolvarea sistemului de ecuatii
        % avand ca variabile independente cele 4 derivate ale curentilor
        % Rezultatele obtinute sunt intocmai derivatele curentilor
SOL=solve(eq1,eq2,eq3,eq4,'diddt,diqdt,diDdt,diQdt');
        % Extragerea solutiei din structura returnata
diddt=SOL.diddt;
diqdt=SOL.diqdt;
diDdt=SOL.diDdt;
diQdt=SOL.diQdt;
        % Afisarea solutiei
disp('did/dt = '),pretty(diddt)
disp('diq/dt = '),pretty(diqdt)
disp('diD/dt = '),pretty(diDdt)
disp('diQ/dt = '),pretty(diQdt)
        % Verificarea solutiei prin inlocuirea rezultatelor in sistemul de ecuatii
Verif1=simple(-ud+Rs*id+Ls*diddt+M*diddt+M*diDdt);
Verif2=simple(-uq+Rs*iq+Ls*diqdt+M*diqdt+M*diQdt);
Verif3=simple(Rr*iD+Lr*diDdt+M*diddt+M*diDdt+w*Lr*iQ+w*M*(iq+iQ));
Verif4=simple(Rr*iQ+Lr*diQdt+M*diqdt+M*diQdt-w*Lr*iD-w*M*(id+iD));
        % Formarea unei vectori cu erorile obtinute
        % (daca toate elementele sale sunt nule, solutia este buna)
Eroare=[Verif1,Verif2,Verif3,Verif4]


close all
zebra = imread('zebra.bmp');
figure; imagesc(zebra); title('original image');

[Zebra1] = Nonlinear_Diffusion(double(zebra),1,1e-2,1,20,0, 0);
title('total variation flow(p=1, tau=1), 20 time steps');
[Zebra2] = Nonlinear_Diffusion(double(zebra),1,1e-3,1.2,20, 0,0);
title('edge enhacing flow(p=1.2, tau=1), 20 time steps');
[Zebra3] = Nonlinear_Diffusion(double(zebra),3,1e-3,1.2,30, 0,0);
title('edge enhacing flow(p=1.2, tau=3), 30 time steps');
[Zebra4] = Nonlinear_Diffusion(double(zebra),3,1e-3,1.3,30, 0,0);
title('edge enhacing flow(p=1.3, tau=3), 30 time steps');

house = imread('house_noisy.bmp');
figure; imagesc(house); title('original image');

[House1] = Nonlinear_Diffusion(double(house),1,1e-3,1,50, 0,0);
title('total variation flow(p=1, tau=1) , 50 time steps');

[House2] = Nonlinear_Diffusion(double(house),5,1e-3,1,10, 0,0);
title('total variation flow(p=1, tau=5) , 10 time steps');

[House4] = Nonlinear_Diffusion(double(house),5,1e-3,1,20, 0,0.5);
title('total variation flow(p=1, tau=5), 20 time steps, regularization sigma=0.5');

[House5] = Nonlinear_Diffusion(double(house),5,1e-3,1,20, 0,1);
title('total variation flow(p=1, tau=5), 20 time steps, regularization sigma=1');

[House6] = Nonlinear_Diffusion(double(house),5,1e-3,1.2,40, 0,0);
title('edge ehnacing flow(p=1.2, tau=5), 40 time steps');

[House7] = Nonlinear_Diffusion(double(house),5,1e-3,1.4,60, 0,0.4);
title('edge ehnacing flow(p=1.4, tau=5), 60 time steps, regularization sigma=0.4');

figure;
subplot(2,2,1); imagesc(zebra); title('original image');
subplot(2,2,2); imagesc(Zebra4/255); title('filtered image');
subplot(2,2,3); imagesc(house); title('original image');
subplot(2,2,4); imagesc(House7/255); title('filtered image');


%==============================================================================
% This code is part of the Matlab-based toolbox
% LagLDDDM - A Lagrangian Gauss--Newton--Krylov Solver for Mass- and
%                        Intensity-Preserving Diffeomorphic Image Registration
%
% For details and license info see
% - https://github.com/C4IR/FAIR.m/tree/master/add-ons/LagLDDMM
%
% function [Sc,dS,d2S] = diffusionST(vc,omega,m,varargin)
%
% Matrix-free spatio-temporal diffusion regularization energy for vc, where
% vc is cell-centered
%
% S(v) = 0.5 * \int_{\omega} alpha(1)*v(x)'*A*v(x)+ alpha(2)*v(x)'*B*v(x) dx,
%
% where A is the spatial gradient operator and B the time derivative operator.
%
% Input:
%
%   vc          instationary velocity field (cell-centered)
%   omega       spatial domain
%   m           number of discretization points
%   varargin    optional parameters (see below)
%
% Optional Input:
%
%   tspan       time span (default: [0 1])
%   nt          number of time points for velocity (default: computed from input)
%
%
% Output:
%
%   Sc          current value   (0.5 * hd * vc'* A *vc + 0.5*dt*vc'*B*vc)
%   dS          derivative      (hd * vc'*A )
%   d2S         Hessian, struct A
% ==================================================================================

function [Sc,dS,d2S] = mfDiffusionST(vc,omega,m,varargin)
if nargin == 0
    help(mfilename);
    return;
end

if strcmp(vc,'para')
    Sc = 'cell-centered';       % grid
    dS = 1;                     % matrixFree
    d2S = @spectralPrecondPCG;  % solver
    return;
end

alpha       = [1 1e-3];
tspan       = [0 1];
nt          = [];
for k=1:2:length(varargin) % overwrites default parameter
    eval([varargin{k},'=varargin{',int2str(k+1),'};']);
end;

dim = numel(omega)/2;

if isempty(nt) % roughly estimate nt
    nt = round(numel(vc)/(prod(m)*dim))-1;
end

d2S.regularizer = regularizer;
d2S.alpha  = alpha;
d2S.B      = @(omega,m)     getDiffusionMatrixST(omega,tspan,m,nt,alpha);
d2S.d2S    = @(u,omega,m)   diffusionOperatorST(u,omega,tspan,m,nt,alpha);
d2S.diag   = @(omega,m)     diag(omega,tspan,m,nt,alpha);
d2S.solver = @spectralPrecondPCG;
d2S.res    = vc;
dS         = d2S.d2S(vc,omega,m)';
Sc         = .5*dS*vc;

function B = getDiffusionMatrixST(omega,tspan,m,nt,alpha)

h   = (omega(2:2:end)-omega(1:2:end))./m;
hd  = prod(h);
% compute time-stepsize
dt = abs(tspan(2)-tspan(1))/nt;

% build gradient matrix for one transformation
Bx =  getSpaceGradientMatrix(omega,m);
% a = sqrt(alpha(1).*hd.*dt)*ones(nt+1,1);
a = sqrt(alpha(1).*hd.*dt*[1/2;ones(nt-1,1);1/2]);
% apply spatial regularization to all transformations and sum up in
% time
Bx =  kron(sdiag(a(:)),Bx);

% get time regularization matrix
b = sqrt(alpha(2)*hd.*dt);
Bt = b * getTimeDiffusionMatrix(nt,dt,prod(m),length(omega)/2);

% build B
B = [Bx;Bt];

function D = sdiag(v)
	D = diag(sparse(v(:)));

% get diagonal of d2S (interesting in matrix free mode)
function D = diag(omega,tspan,m,nt,alpha)
dim   = numel(omega)/2;
one   = @(i,j) One(omega,m,i,j);
hd    = prod((omega(2:2:end)-omega(1:2:end))./m);
% compute time stepsize
dt = abs(tspan(1)-tspan(2))/nt;

if dim == 2
    Dx = [ one(1,1) + one(1,2);
           one(2,1) + one(2,2) ];
    Dx = kron(hd*dt*ones(nt+1,1),Dx);

    Dt = [1/2;ones(nt-1,1);1/2]/dt^2;

    Dt = kron(hd*dt*Dt , ones(prod(m)*dim,1));

    D  = alpha(1)*Dx + alpha(2)*Dt;

else
    Dx   = [ ...
        one(1,1)+one(1,2)+one(1,3);
        one(2,2)+one(2,1)+one(2,3);
        one(3,3)+one(3,1)+one(3,2)];
    Dx   = kron(hd*dts(:),Dx);

    Dt          = zeros(mTime+1,1);
    Dt(1:end-1) = (1./dt(:));
    Dt(2:end  ) = Dt(2:end) + (1./dt(:));

    Dt = kron(hd*Dt, ones(prod(mSpace)*dim,1));

    D  = alpha(1)*Dx + alpha(2)*Dt;

end;

% helper for computation of diag(d2S)
function o = One(omega,m,i,j)
h = (omega(2:2:end)-omega(1:2:end))./m;
o = ones(m)/h(j)^2;
switch j
    case 1, o(2:end-1,:,:) = 2*o(2:end-1,:,:);
    case 2, o(:,2:end-1,:) = 2*o(:,2:end-1,:);
    case 3, o(:,:,2:end-1) = 2*o(:,:,2:end-1);
end;
o = o(:);

% matrix free implementation of spatio-temporal diffusion operator
function Ay = diffusionOperatorST(vc,omega,tspan,m,nt,alpha)
dim = numel(omega)/2;
h   = (omega(2:2:end)-omega(1:2:end))./m;
hd  = prod(h(1:dim));
dt  = abs(tspan(1)-tspan(2))/nt;
w   = dt*[1/2;ones(nt-1,1);1/2];

switch dim
    case 2
        d1 = @(Y) (Y(2:end,:)-Y(1:end-1,:))/h(1);
        d2 = @(Y) (Y(:,2:end)-Y(:,1:end-1))/h(2);

        d1T = @(Y) reshape([-Y(1,:);Y(1:end-1,:)-Y(2:end,:);Y(end,:)],[],1)/h(1);
        d2T = @(Y) reshape([-Y(:,1),Y(:,1:end-1)-Y(:,2:end),Y(:,end)],[],1)/h(2);

        vc   = reshape(vc,[],nt+1);
        Ay = zeros(prod(m)*dim,nt+1);
        % spatial diffusion
        for k=1:nt+1
            vct = reshape(vc(:,k),[m dim]);
            Ay(:,k) = w(k) * hd*  alpha(1) * ....
                [ (d1T(d1(vct(:,:,1))) + d2T(d2(vct(:,:,1)))); ...
                (d1T(d1(vct(:,:,2))) + d2T(d2(vct(:,:,2))))];
        end
        Ay = Ay(:);

    case 3
        d1 = @(Y) (Y(2:end,:,:)-Y(1:end-1,:,:))/h(1);
        d2 = @(Y) (Y(:,2:end,:)-Y(:,1:end-1,:))/h(2);
        d3 = @(Y) (Y(:,:,2:end)-Y(:,:,1:end-1))/h(3);

        d1T = @(Y) reshape(d1t(Y),[],1)/h(1);
        d2T = @(Y) reshape(d2t(Y),[],1)/h(2);
        d3T = @(Y) reshape(d3t(Y),[],1)/h(3);

        vc   = reshape(vc,[],nt+1);
        Ay = zeros(prod(m)*dim,nt+1);
        % spatial diffusion
        for k=1:nt+1
            vct = reshape(vc(:,k),[m dim]);
            Ay(:,k) = w(k) * hd*  alpha(1) * ....
                [ ...
                (d1T(d1(vct(:,:,:,1))) + d2T(d2(vct(:,:,:,1))) + d3T(d3(vct(:,:,:,1)))); ...
                (d1T(d1(vct(:,:,:,2))) + d2T(d2(vct(:,:,:,2))) + d3T(d3(vct(:,:,:,2)))); ...
                (d1T(d1(vct(:,:,:,3))) + d2T(d2(vct(:,:,:,3))) + d3T(d3(vct(:,:,:,3)))); ...
                ];
        end
        Ay = Ay(:);
    otherwise
        error('%s - dimension %d not supported.',mfilename,dim);
end
% time diffusion
if alpha(2)>0
    Dt  = @(Y) (Y(:,2:end)-Y(:,1:end-1))/dt;
    DtT = @(Y) ([-Y(:,1),Y(:,1:end-1)-Y(:,2:end),Y(:,end)])/dt;
    At  = DtT(Dt(vc));
    Ay  = Ay + (alpha(2)* hd*dt)*At(:);
end

% partial derivative operator for x1 (mf)
function y = d1t(Y)
m = size(Y);
y = zeros(m+[1,0,0]);
y(1,:,:) = -Y(1,:,:);
y(2:end-1,:,:) = Y(1:end-1,:,:)-Y(2:end,:,:);
y(end,:,:) = Y(end,:,:);
% partial derivative operator for x2 (mf)
function y = d2t(Y)
m = size(Y);
y = zeros(m+[0,1,0]);
y(:,1,:) = -Y(:,1,:);
y(:,2:end-1,:) = Y(:,1:end-1,:)-Y(:,2:end,:);
y(:,end,:) = Y(:,end,:);
% partial derivative operator for x3 (mf)
function y = d3t(Y)
m = size(Y); if length(m) == 2, m = [m,1]; end;
y = zeros(m+[0,0,1]);
y(:,:,1) = -Y(:,:,1);
y(:,:,2:end-1) = Y(:,:,1:end-1)-Y(:,:,2:end);
y(:,:,end) = Y(:,:,end);

function A = getSpaceGradientMatrix(omega,m)
dim = length(omega)/2;
h   = (omega(2:2:end)- omega(1:2:end)) ./m ;

I = @(i)  speye(m(i));

% setup regularizer for y
%
% S(y) =  INT |Dy(.,t)|^2 + INT |d/dt y(x,.)|^2 dx
switch dim
    case 2
        % build discrete derivative operators
        D1=spdiags(ones(m(1),1)*[-1 1],0:1,m(1)-1,m(1)); D1=D1/(h(1));
        D2=spdiags(ones(m(2),1)*[-1 1],0:1,m(2)-1,m(2)); D2=D2/(h(2));

        % spatial regularization
        A = [kron(I(2),D1); kron(D2,I(1))];
    case 3
        % build discrete derivative operators
        D1=spdiags(ones(m(1),1)*[-1 1],0:1,m(1)-1,m(1)); D1=D1/(h(1));
        D2=spdiags(ones(m(2),1)*[-1 1],0:1,m(2)-1,m(2)); D2=D2/(h(2));
        D3=spdiags(ones(m(3),1)*[-1 1],0:1,m(3)-1,m(3)); D3=D3/(h(3));
        % build gradient (scalar)
        A = [kron(I(3),kron(I(2),D1)); ...
             kron(I(3),kron(D2,I(1))); ...
             kron(D3,kron(I(2),I(1)))];

end
A = kron(speye(dim), A);                  % nD gradient

function A = getTimeDiffusionMatrix(nt,dt,n,dim)
Dt=spdiags(ones(nt+1,1)*[-1/dt 1/dt],0:1,nt,nt+1);
A = kron(Dt, speye(n*dim)); % 2nd deriveative over time for two components


function xdot = Equation(t,x)
% Set the input values in order to pass onto ode45
%
n = length(x) ;
thita = x(1) ;          % Angular Position of the bead
dthita = x(2) ;         % Angular Velocity of the bead
g = x(3) ;              % Acceleration due to gravity
M = x(4) ;              % Mass of the bead
R = x(5) ;              % Radius of the hoop
V = x(6) ;              % Frictional coefficient of the bead on the hoop
w0 = x(7) ;             % Frequency of rotation of the bead
%
xdot=zeros(n,1);
xdot(1) = dthita;
xdot(2) = -sin(thita)*(g/R-w0^2*cos(thita))-V/M*dthita ;


function[H,P,M1,M2,N1,N2] = countsig(data1,data2,T1,T2,parametric,p,quiet)
% Give the program two spike data sets and one
% or two time intervals and it will decide if
% the counts are significantly different.
% this is either with a non-parametric method
% or with a sqrt transformation followed by a
% t-test
% Usage: [H,P,M1,M2,N1,N2] = countsig(data1,data2,T1,T2,parametric,p,quiet)
%
% Input:
% Note that all times have to be consistent. If data
% is in seconds, so must be sig and t. If data is in
% samples, so must sig and t. The default is seconds.
%
% data1      - structure array of spike times (required)
% data2      - structure array of spike times (required)
% T1         - time interval (default all)
% T2         - time interval (default T1)
% parametric - 0 = non-parametric (Wilcoxon)
%            - 1 = ttest on sqrt of counts
%            - 2 = Poisson assumption
%              (default = 0)
% p          - significance level (0.05)
% quiet      - 1 = no display 0 = display
%
% Output:
%
% H          - 1 if different 0 if not
% P          - prob of result if same
% M1         - mean count for data1
% M2         - mean count for data2
% N1         - counts for data1
% N2         - counts for data2

if nargin < 2;error('I need 2 sets of spike data');end
data1=padNaN(data1); % create a zero padded data matrix from input structural array
data2=padNaN(data2); % create a zero padded data matrix from input structural array
data1=data1'; data2=data2'; % transpose data to get it into a form acceptable to Murray's routine
if nargin < 3,
   T1 = [min(data1(:,1)) max(max(data1))];
end
if nargin < 4,
   T2 = T1;
end
if nargin < 5,
   parametric = 0;
end
if nargin < 6; p = 0.05;end
if nargin < 7; quiet = 0; end

if isempty(T1),
   T1 = [min(data1(:,1)) max(max(data1))];
end
if isempty(T2)
   T2 = T1;
end
if isempty(parametric),
   parametric = 0;
end
if isempty(p)
   p = 0.05;
end
if isempty(quiet),
    quiet = 0;
end

NT1 = length(data1(:,1));
NT2 = length(data2(:,2));

if (NT1 < 4 || NT2 < 4) && parametric ~= 2,
  disp('Low number of trials : switch to Poisson test')
  parametric = 2;
end

if abs((T1(2)-T1(1)) - (T1(2)-T1(1))) > 10.^-6
  error('Time intervals for analysis are different')
end

% get counts...
N1=zeros(1,NT1);
for n=1:NT1
  N1(n) = length(find(data1(n,:) >=  T1(1) & ...
          data1(n,:) <=  T1(2) & ~isnan(data1(n,:))));
end
N2=zeros(1,NT2);
for n=1:NT2
  N2(n) = length(find(data2(n,:) >=  T2(1) & ...
          data2(n,:) <=  T2(2) & ~isnan(data1(n,:))));
end
M1 = mean(N1);
M2 = mean(N2);

% do non parametric test...

if parametric == 0
  [P H] = ranksum(N1,N2,p);
end

% parametric test (with stabilizing transform)...

%  use sqrt transformation from
%  Cox and Lewis to make data more Gaussian
%  the statistical analysis of series of events pg 44

if parametric == 1
  X = sqrt(N1 +0.25);
  Y = sqrt(N2 +0.25);
  [H,P] = ttest2(X,Y,p,0);
end

%  Poisson test.  Use method from Zar
%  pg 580 Ed 3.  Z bahaves as a normal variate under
%  null of same process mean and Poisson processes

if parametric == 2
  X = sum(N1);
  Y = sum(N2);
  Z = abs(X-Y)./sqrt(X+Y);
  P = 2*(1-normcdf(Z));
  if P < p; H = 1;else H = 0;end
end

if quiet == 0
  if H == 1
    disp('Counts are signifcantly different')
  else
    disp('Counts are not signifcantly different')
  end
  disp(['Mean count for data1 = ' num2str(M1)])
  disp(['Mean count for data2 = ' num2str(M2)])
  disp(['P value = ' num2str(P)])
end


function gmm = mixtureOfGaussian(data, K, niter, fullCov, varargin)
% Mixture of K gaussians
% data(ndata, nvariables) - the data
% K - the number of gaussians in the mixture
% niter - the number of iterations
% fullCov - 1 if a full covariance matrix should be used, otherwise
%                   a diagonal matrix will be used
% varargin{1} - weights for the data (optional)
% gmm.mu{K} - the means of the mixtures
% gmm.sigma{K} - the variance matrices
% gmm.priors(K) - the mixture priors

[ndata, ndim] = size(data);

if ndata == 0
    disp('warning no data')
    gmm.mu = {};
    gmm.sigma = {};
    gmm.priors = [];
    return;
end

if length(varargin)==0
    w = ones(ndata, 1);
else
    w = varargin{1}(:);
end

w = w / sum(w) * ndata;

cumw = cumsum(w / sum(w));

% initialize mu by randomly selected data values
r = rand(K, 1);
for k = 1:K
    for i = 1:ndata
        if cumw(i)>=r(k)
            mu{k} = data(i, :);
            break;
        end
    end
end

% initialize all covariances to be covariance of data
%if fullCov
%    sigma(1:K) = {cov(data)};
%else
%    sigma(1:K) = {diag(var(data))};
%end
sigma(1:K) = {diag(ones(1, ndim))};

priors = ones(1, K) / K;

for iter = 1:niter

    % get posteriors: P(k | data) = P(data|k)P(k) / sum_k(P(data|k))
    pC = zeros(ndata, K);
    for k = 1:K
        invSigma = inv(sigma{k});
        for i = 1:ndata
            pC(i, k) = 1 / (sqrt(2*pi)*sqrt(det(sigma{k}))) * ...
                exp(-1/2 * (data(i, :)-mu{k}) * invSigma * (data(i, :)-mu{k})') ...
                * priors(k);
        end
    end

    if 0
    for k = 1:K
        disp(['k: ' num2str(k)])
        disp(['mu: ' num2str(mu{k})])
        disp(['sigma: ' num2str(det(sigma{k}))])
        disp(['prior: ' num2str(priors(k))])
        disp(['min: ' num2str(min(pC(:, k)))])
        disp(['max: ' num2str(max(pC(:, k)))])
    end
    end

    %disp(num2str(priors))

    pC = pC + eps; % avoid divide by zero error

    sumPc = sum(pC, 2);
    for k = 1:K
        pC(:, k) = pC(:, k) ./ sumPc .* w;
    end
    kweight = sum(pC, 1);

    priors = kweight / sum(kweight);

    % mean estimation
    for k = 1:K
        sumkX = pC(:, k)' * data;
        mu{k} = sumkX / kweight(k);
    end

    % covariance matrix estimation
    if fullCov
        for k = 1:K
            diffs = data - (ones(ndata, 1) * mu{k});
            diffs = diffs .* (sqrt(pC(:,k))*ones(1, ndim));
            sigma{k} = (diffs' * diffs) / kweight(k);
        end
    else
        for k = 1:K
            diffs = data - (ones(ndata, 1) * mu{k});
            sigma{k} = diag(sum((diffs.*diffs).*(pC(:, k)*ones(1, ndim)), 1) ./ kweight(k));
        end
    end
end

gmm.mu = mu;
gmm.sigma = sigma;
gmm.priors = priors;


function [ y, m, d, f ] = jed_to_ymdf_ethiopian ( jed )

%*****************************************************************************80
%
%% JED_TO_YMDF_ETHIOPIAN converts a JED to an Ethiopian YMDF date.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Edward Richards,
%    Algorithm F,
%    Mapping Time, The Calendar and Its History,
%    Oxford, 1999, pages 324-325.
%
%  Parameters:
%
%    Input, real JED, the Julian Ephemeris Date.
%
%    Output, integer Y, M, D, real F, the YMDF date.
%

%
%  Determine the computational date (Y'/M'/D').
%
  j = floor ( jed + 0.5 );
  f = ( jed + 0.5 ) - j;

  j_prime = j + 124;

  y_prime = floor ( ( 4 * j_prime + 3 ) / 1461 );
  t_prime = floor ( mod ( 4 * j_prime + 3, 1461 ) / 4 );
  m_prime = floor ( t_prime / 30 );
  d_prime = mod ( t_prime, 30 );
%
%  Convert the computational date to a calendar date.
%
  d = d_prime + 1;
  m = mod ( m_prime, 13 ) + 1;
  y = y_prime - 4720 + floor ( ( 13 - m ) / 13 );

  return
end


function [y,deriv] = brier_obj(w,T,weights,logit_prior)
% This is an MV2DF. See MV2DF_API_DEFINITION.readme.
%
% Weighted binary classifier cross-entropy objective, based on 'Brier'
% quadratic proper scoring rule. This rule places less emphasis on extreme scores,
% than the logariothmic scoring rule.
%
%  Differentiable inputs:
%   w: is vector of N detection scores (in log-likelihood-ratio format)
%
%  Fixed parameters:
%   T: is vector of N labels: 1 for target and -1 for non-target.
%   weights: is N-vector of objective function weights, one per trial.
%   logit_prior: is logit(prior), this controls the region of interest

if nargin==0
    test_this();
    return;
end

if isempty(w)
    y = @(w)brier_obj(w,T,weights,logit_prior);
    return;
end

if isa(w,'function_handle')
    outer = brier_obj([],T,weights,logit_prior);
    y = compose_mv(outer,w,[]);
    return;
end

w = w(:);
scores = w.';

arg = bsxfun(@plus,scores,logit_prior).*T;
logp2 = -neglogsigmoid(-arg);
wobj = 0.5*exp(2*logp2).*weights; % 1*N
y = sum(wobj);

if nargout>1
    logp1 = -neglogsigmoid(arg);
    deriv = @(dy) deriv_this(dy,weights(:),T(:),logp1(:),logp2(:));
end

function [g,hess,linear] = deriv_this(dy,weights,T,logp1,logp2)
g0 = -exp(logp1+2*logp2).*weights.*T;
g = dy*g0;
linear = false;
hess = @(d) hessianprod(d,dy,g0,weights,logp1,logp2);

function [h,Jv] = hessianprod(d,dy,g0,weights,logp1,logp2)

ddx = -exp(logp1+2*logp2);
h = dy*(ddx.*(1-3*exp(logp1))).*weights.*d(:);

if nargout>1
    Jv = d.'*g0;
end

function test_this()
N = 30;
T = [ones(1,N/3),-ones(1,N/3),zeros(1,N/3)];
scores = randn(1,N);
weights = [rand(1,2*N/3),zeros(1,N/3)];
f = @(w) brier_obj(w,T,weights,-2.23);
test_MV2DF(f,scores(:));


function [ G ] = gsp_nn_hypergraph( Xin, param )
%GSP_NN_HYPERGRAPH Create a nearest neighbors hypergraph from a point cloud
%   Usage :  G = gsp_nn_hypergraph( Xin );
%            G = gsp_nn_hypergraph( Xin, param );
%
%   Input parameters:
%       Xin         : Input points
%       param       : Structure of optional parameters
%
%   Output parameters:
%       G           : Resulting graph
%
%   Example:::
%
%           P = rand(100,2);
%           G = gsp_nn_hypergraph(P)
%
%   Additional parameters
%   ---------------------
%
%   * *param.use_flann* : [0, 1]              use the FLANN library
%   * *param.center*    : [0, 1]              center the data
%   * *param.rescale*   : [0, 1]              rescale the data (in a 1-ball)
%   * *param.sigma*     : float               the variance of the distance kernel
%   * *param.k*         : int                 number of neighbors for knn
%
%   See also: gsp_nn_graph
%

% Author: Nathanael Perraudin
% Date: 21 October 2015
% Testing: test_rmse

%   * *param.type*      : ['knn', 'radius']   the type of graph (default 'knn')
%   * *param.epsilon*   : float               the radius for the range search
%   * *param.use_l1*    : [0, 1]              use the l1 distance

    if nargin < 2
    % Define parameters
        param = {};
    end

    %Parameters
%     if ~isfield(param, 'type'), param.type = 'knn'; end
    if ~isfield(param, 'use_flann'), param.use_flann = 0; end
    if ~isfield(param, 'center'), param.center = 0; end
    if ~isfield(param, 'rescale'), param.rescale = 1; end
    if ~isfield(param, 'k'), param.k = 10; end

    param.type = 'knn';
%     if ~isfield(param, 'epsilon'), param.epsilon = 0.01; end
%     if ~isfield(param, 'use_l1'), param.use_l1 = 0; end
%     if ~isfield(param, 'target_degree'), param.target_degree = 0; end;
    paramnn = param;
%     paramnn.k = param.k +1;
    [indx, ~, dist] = gsp_nn_distanz(Xin',Xin',paramnn);

%     switch param.type
%         case 'knn'
%             if param.use_l1
%                 if ~isfield(param, 'sigma'), param.sigma = mean(dist); end
%             else
                if ~isfield(param, 'sigma'), param.sigma = mean(dist)^2; end
%             end
%         case 'radius'
%             if param.use_l1
%                 if ~isfield(param, 'sigma'), param.sigma = epsilon/2; end
%             else
%                 if ~isfield(param, 'sigma'), param.sigma = epsilon.^2/2; end
%             end
%         otherwise
%             error('Unknown graph type')
%     end

    w = exp(-dist.^2/param.sigma);
    G.N = size(Xin,1);
    G.Ne = G.N;
    G.W = sparse(G.N,G.Ne);
    G.E = cell(G.Ne,1);
    k = param.k;
    for ii = 1:G.Ne
        edge = indx((1:k)+(ii-1)*k);
        G.E{ii} = edge;
        % Here we use H for HW...
        G.W(edge,ii) = sqrt(sum(w(edge)));
    end
    G.hypergraph = 1;
    G.directed = 0;

    %Fill in the graph structure
    G.coords = Xin;

    G.type = 'Nearest neighboors hypergraph';
    G.lap_type = 'normalized';
    G.sigma = param.sigma;
    G = gsp_graph_default_parameters(G);
end


function L=GetLevel(i,Ms,n)
%return the level of i-th coefficient
col = floor(i/n);
row = rem(i,n);

dlevels = length(Ms);

p=n/(2^(dlevels-1));

for j=1:dlevels
    if (col<=p) && (row<=p)
        L=j;
        break;
    else
        p=2*p;
    end
end


function vo = ch_qmulv(q, vi)
% 向量通过四元数做3D旋转
%
% Inputs: q - Qb2n
%            vi - 需要旋转的向量
% Output: vout - output vector, such that vout = q*vin*conjugation(q)
%
% See also  q2mat, qconj, qmul.

%     qi = [0; vi];
%     qo = ch_qmul(ch_qmul(q,qi),ch_qconj(q));
%     vo = qo(2:4,1);

    qo1 =              - q(2) * vi(1) - q(3) * vi(2) - q(4) * vi(3);
    qo2 = q(1) * vi(1)                + q(3) * vi(3) - q(4) * vi(2);
    qo3 = q(1) * vi(2)                + q(4) * vi(1) - q(2) * vi(3);
    qo4 = q(1) * vi(3)                + q(2) * vi(2) - q(3) * vi(1);
    vo = vi;
    vo(1) = -qo1 * q(2) + qo2 * q(1) - qo3 * q(4) + qo4 * q(3);
    vo(2) = -qo1 * q(3) + qo3 * q(1) - qo4 * q(2) + qo2 * q(4);
    vo(3) = -qo1 * q(4) + qo4 * q(1) - qo2 * q(3) + qo3 * q(2);


function [pitch, yaw] = ueci2angles(reci, veci, ueci)

% convect eci unit vector to rtn angles

% input

%  reci = eci position vector (kilometers)
%  veci = eci velocity vector (kilometers/second)
%  ueci = eci unit vector

% output

%  pitch = pitch angle (radians)
%          positive above the local horizon
%  yaw   = yaw angle (radians)
%          positive in the direction of the angular momentum vector

% Orbital Mechanics with MATLAB

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

% compute radial frame unit vectors

rmag = norm(reci);

xrdl = reci / rmag;

zrdl = cross(reci, veci);

hmag = norm(zrdl);

zrdl = zrdl / hmag;

yrdl = cross(zrdl, xrdl);

% unit vector in radial-tangential-normal frame

umee(1) = dot(ueci, xrdl);

umee(2) = dot(ueci, yrdl);

umee(3) = dot(ueci, zrdl);

% pitch angle (radians)

pitch = asin(umee(1));

% yaw angle (radians)

yaw = atan2(umee(3), umee(2));


function sF = radon(sF, delta)
% Radon transform of a spherical function
%
% Syntax
%   sF = radon(sF)
%
% Input
%  sF  - @S2FunHarmonic
%
% Output
%  sF - @S2FunHarmonic
%

% Description
% The spherical Radon transform is simply a convolution operator with
% a kernel function with Legendre coefficients given by
%
%         A(n) = (-1)^(n/2) * (n-1)!! / n!!     if n is even
%         A(n) = 0                              if n is odd
%
% where
% (n-1)!! = 1 * 3 * 5 * ... (n-1)
% n!!     = 2 * 4 * 6 * ... n
%

if nargin == 2

  A = sum(legendre0(sF.bandwidth,cos(pi/2+delta)),2);

else
  A = zeros(sF.bandwidth+1,1);
  A(1) = 1;
  for n = 2:2:sF.bandwidth
    A(n+1) = -A(n-1) * (n-1)/n;
  end

end

sF = conv(sF,A);


N = 3;
x = rand(N,2); % each row is a feature vector
m = mean(x,1);
xc = x-repmat(m, N, 1);

C = eye(N) - (1/N)*ones(N,N);
xc2 = C*x;
assert(approxeq(xc, xc2))


% ESTIMATION OF ZERO COUPON YIELDS FROM COUPON BOND PRICES USING THE
% NELSON-SIEGEL AND SVENSSON MODEL
%
% Model = 'NS' (Nelson-Siegel) | 'Svensson'
%
% Optimization Structure
%
% .Method = 'price' | 'ytm'
%  'price' fits (weighted)) cupon bond prices
%  'ytm' fits yields-to-maturity
%
% .Weights  = 'MD' | 'LA' | empty (Weights are irrelevant for the YTM method.)
%  'MD' weights use modified duration of bonds
%  'LA' weights work as the correct linear approaximation of ytm;
%  LA weights equal MD weights divideded by observed cupon bond prices.
%  Note that weights are irrelevant for the YTM method.
%  Further note that short-rates (LIBOR rates) are underweighted, see the
%  NSobjP.m for details and "The Czech Treasury Yield Curve from 1999 to the
%  Present" for a discussion.
%
% .Algorithm = 'lsqnonlin' | 'fminsearch'
%  Supports two standard Matlab optimization routines. The nonlinear least
%  squares, lsqnonlin, which is tha part of Optimization Tbx is recommended.
%
% .Multistart = 'yes' | 'no'
%  The estimation algorothm creates a grid of starting values for optimization, which
%  gives better hope for finding global optima. Recommended.
%
%
% Kamil Kladivko
% email: kladivko@gmail.com
%
% Cite as:
% Kladivko Kamil (2010). The Czech Treasury Yield Curve from 1999 to the Present,
% Czech Journal of Economics and Finance, 60(4): 307-335
% =========================================================================

clc
close all
clear variables

% 1) Set model
Model = 'NS';
Optimization.Method = 'price';
Optimization.Weights = 'LA';
Optimization.Algorithm = 'lsqnonlin';
Optimization.MultiStart = 'yes';

% 2) Read data from EXCEL
% The EXCEL file contains Czech Governemnt bond prices and PRIBOR rates (short rates) from March 2, 2007
% There were 13 Czech Government bonds available on March 2, 2007. Furter, 4 PRIBOR rates are used
% to fix the short-end of the yield curve. See Section 5 of "The Czech Treasury Yield Curve from 1999 to the Present"

[B Btext] = xlsread('bonddata.xls', 'data', 'B6:F18');
[junk Settle] = xlsread('bonddata.xls', 'data', 'C2:C2');
[SR SRtext] = xlsread('bonddata.xls', 'data', 'J6:L9');

% The DATE format in EXCEL depends on the "national settings". Needs to be
% set accordingly!
Bonds.Issue = datenum(Btext(:,1), 'dd.mm.yyyy');
Bonds.Maturity = datenum(Btext(:,2), 'dd.mm.yyyy');
Bonds.Settle = datenum(Settle, 'dd.mm.yyyy');
Bonds.Prices = B(:,2);
% Make sure that bond ID (isseu number) was read as text! If not adjust
% either this code or EXCEL.
Bonds.Emission = Btext(:,5);
Bonds.Coupon = B(:,1)./100;
% Assumes that coupon is paid once a year. The first cupon can be payed
% later than after one year. See czbondcf.m and czbondfuturecf.m for
% details
Bonds.Notional = 100;
Bonds.Basis = 1;
% Bonds.Basis = '1' | '2'
% Bond.Basis is Day-Count basis for calculation coupon cash flows
% 1: 30E/360 (default), 2: Actual/360
% Go to czbondcf.m and czbondfuturecf.m to adjust for your basis

ShortRates.IR = SR(:,1)'./100; % This must be as row vector opposed to the bond data
if ~isempty(ShortRates), ShorRates.IR = log(1+ShortRates.IR); end % Transform SR to continuous compounding
ShortRates.TimeFactor = SR(:,2)';
ShortRates.Emission = SRtext';

% If Short Rates are not to be used then supply "ShotRates = []" into the
% estimation routine below.
%ShortRates = [];

% 3) Nelson-Siegel Parameters Estimation (calls the main estimation routine)
[Params Fval MaxTime2Mat ParamsMS FvalMS] = NSest(Bonds, ShortRates, Model, Optimization);

% 4) Estimation Error;
IncludeSRinError = 'no';
[PriceE YTME] = NSerror(Bonds, ShortRates, Model, Params, IncludeSRinError);

% 5) Plot
Plots1D(Params, Model, YTME, ShortRates);


function [L,S,obj,err,iter] = rpca(X,lambda,opts)

% Solve the Robust Principal Component Analysis minimization problem by M-ADMM
%
% min_{L,S} ||L||_*+lambda*loss(S), s.t. X=L+S
% loss(S) = ||S||_1 or ||S||_{2,1}
%
% ---------------------------------------------
% Input:
%       X       -    d*n matrix
%       lambda  -    >0, parameter
%       opts    -    Structure value in Matlab. The fields are
%           opts.loss       -   'l1' (default): loss(S) = ||S||_1
%                               'l21': loss(S) = ||S||_{2,1}
%           opts.tol        -   termination tolerance
%           opts.max_iter   -   maximum number of iterations
%           opts.mu         -   stepsize for dual variable updating in ADMM
%           opts.max_mu     -   maximum stepsize
%           opts.rho        -   rho>=1, ratio used to increase mu
%           opts.DEBUG      -   0 or 1
%
% Output:
%       L       -    d*n matrix
%       S       -    d*n matrix
%       obj     -    objective function value
%       err     -    residual
%       iter    -    number of iterations
%
% version 1.0 - 19/06/2016
%
% Written by Canyi Lu (canyilu@gmail.com)
%

tol = 1e-8;
max_iter = 500;
rho = 1.1;
mu = 1e-4;
max_mu = 1e10;
DEBUG = 0;
loss = 'l1';

if ~exist('opts', 'var')
    opts = [];
end
if isfield(opts, 'loss');        loss = opts.loss;            end
if isfield(opts, 'tol');         tol = opts.tol;              end
if isfield(opts, 'max_iter');    max_iter = opts.max_iter;    end
if isfield(opts, 'rho');         rho = opts.rho;              end
if isfield(opts, 'mu');          mu = opts.mu;                end
if isfield(opts, 'max_mu');      max_mu = opts.max_mu;        end
if isfield(opts, 'DEBUG');       DEBUG = opts.DEBUG;          end

[d,n] = size(X);

L = zeros(d,n);
S = L;
Y = L;

iter = 0;
for iter = 1 : max_iter
    Lk = L;
    Sk = S;
    % update L
    [L,nuclearnormL] = prox_nuclear(-S+X-Y/mu,1/mu);
    % update S
    if strcmp(loss,'l1')
        S = prox_l1(-L+X-Y/mu,lambda/mu);
    elseif strcmp(loss,'l21')
        S = prox_l21(-L+X-Y/mu,lambda/mu);
    else
        error('not supported loss function');
    end

    dY = L+S-X;
    chgL = max(max(abs(Lk-L)));
    chgS = max(max(abs(Sk-S)));
    chg = max([chgL chgS max(abs(dY(:)))]);
    if DEBUG
        if iter == 1 || mod(iter, 10) == 0
            obj = nuclearnormL+lambda*comp_loss(S,loss);
            err = norm(dY,'fro');
            disp(['iter ' num2str(iter) ', mu=' num2str(mu) ...
                    ', obj=' num2str(obj) ', err=' num2str(err)]);
        end
    end

    if chg < tol
        break;
    end
    Y = Y + mu*dY;
    mu = min(rho*mu,max_mu);
end
obj = nuclearnormL+lambda*comp_loss(S,loss);
err = norm(dY,'fro');

function out = comp_loss(E,loss)

switch loss
    case 'l1'
        out = norm(E(:),1);
    case 'l21'
        out = 0;
        for i = 1 : size(E,2)
            out = out + norm(E(:,i));
        end
end


function vl_demo_kdtree_ann
% VL_DEMO_KDTREE
%   Demonstrates the use of a kd-tree for approximate nearest neighbor
%   (ANN) queries.

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

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

% Generate some 2D data and a query point
X = rand(2, 100) ;
Q = rand(2,1) ;

% Buld a kd-tree
kdtree = vl_kdtreebuild(X) ;

% Query with increasing accuracy
maxNumComparisonRange = [1 10 20 30] ;
for t = [1 2 3 4]
  figure(t) ; clf ;
  vl_plotframe(X, 'ro') ;
  hold on ;
  xl = [.2, .8] ;
  yl = [.1, .7] ;
  xlim(xl) ;
  ylim(yl) ;

  %  vl_demo_kdtree_plot(kdtree, 1, xl, yl) ;

  [i, d] = vl_kdtreequery (kdtree, X, Q, ...
                           'NumNeighbors', 10, ...
                           'MaxComparisons', maxNumComparisonRange(t), ...
                           'Verbose') ;

  vl_plotframe(Q,'b*','markersize',10) ;
  for k=1:length(i)
    if i(k) == 0, continue ; end
    vl_plotframe([Q ; sqrt(d(k))],'b-','linewidth',1) ;
    vl_plotframe(X(:, i(k)), 'bx','markersize',15) ;
  end
  title(sprintf('10 ANNs with at most %d comparisions', maxNumComparisonRange(t))) ;

  axis square ;
  set(gca,'xtick',[],'ytick',[]) ;
  vl_demo_print(t, sprintf('kdtree_ann_%d', t)) ;
end


function s=frac2bin(d,n,m)
%FRAC2BIN Convert an column vector to binary S=(D,N,M)
%  Inputs:  D   scalar or column vector to convert
%           N   minimum number of integer bits to output [default 1]
%           M   number of places after binary point [default 0]
%
%  Outputs: S   String matrix with one value per row. A binary point is included
%               if M>0. The values in D are rounded to the number of displayed bits.
%               If N is negative then leading zeros will be output as spaces if they are to the
%               left of the |N|'th integer column (i.e. N digits will always be output)
%               If M is negative, then values will be truncated rather than rounded.
%
% Bug: doesn't yet cope with negative numbers

%      Copyright (C) Mike Brookes 2005
%      Version: $Id: frac2bin.m,v 1.2 2007/05/04 07:01:38 dmb Exp $
%
%   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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin<3
    m=0;
    if nargin<2
        n=1;
    end
end
l=abs(n);
r=abs(m);
[f,e]=log2(max(d));
if m<0
    v=floor(pow2(d(:),r));
else
    v=round(pow2(d(:),r));
end
s=setstr(rem(floor(v*pow2(1-max(l,e)-r:0)),2)+'0');
c=size(s,2)+1;  % size including binary point (even if not present)
b=c-r;          % position of binary point
if r>0
    s(1,c)='0'; % make s bigger
    s(:,b+1:c)=s(:,b:c-1);  % shift binary places to the right
    s(:,b)='.';
end
q=cumsum(s~='0',2);
if n<0
    t=s(:,1:b-l-1);
    t(~q(:,1:b-l-1))=' ';
    s(:,1:b-l-1)=t;
end


% -----------------------------------------------------------------  %
% Matlab Programs included the Appendix B in the book:               %
%  Xin-She Yang, Engineering Optimization: An Introduction           %
%                with Metaheuristic Applications                     %
%  Published by John Wiley & Sons, USA, July 2010                    %
%  ISBN: 978-0-470-58246-6,   Hardcover, 347 pages                   %
% -----------------------------------------------------------------  %
% Citation detail:                                                   %
% X.-S. Yang, Engineering Optimization: An Introduction with         %
% Metaheuristic Application, Wiley, USA, (2010).                     %
%                                                                    %
% http://www.wiley.com/WileyCDA/WileyTitle/productCd-0470582464.html %
% http://eu.wiley.com/WileyCDA/WileyTitle/productCd-0470582464.html  %
% -----------------------------------------------------------------  %
% ===== ftp://  ===== ftp://   ===== ftp:// =======================  %
% Matlab files ftp site at Wiley                                     %
% ftp://ftp.wiley.com/public/sci_tech_med/engineering_optimization   %
% ----------------------------------------------------------------   %

% Genetic Algorithm (Simple Demo) Matlab/Octave Program              %
% Written by X S Yang (Cambridge University) 2008                    %
% This is a simple demo, there are more efficient packages available %
% both commercial and/or open source. For actual applications,       %
% more sophicated implementation is needed. So search the web to     %
% to find a suitable package concerning genetic algorithms           %

% Usage: B1_ga   or  B1_ga(`x*exp(-x)');
function [bestsol, bestfun, count]=B1_ga(funstr)
global solnew sol pop popnew fitness fitold f range;
if nargin<1,
  % Easom Function with fmax=1 at x=pi
  funstr='-cos(x)*exp(-(x-3.1415926)^2)';
end
range=[-10 10];       % Range/Domain
% Converting to an inline function
f=vectorize(inline(funstr));
% Generating the initil population
rand('state',0');     % Reset the random generator
popsize=20;           % Population size
MaxGen=100;           % Max number of generations
count=0;              % counter
nsite=2;              % number of mutation sites
pc=0.95;              % Crossover probability
pm=0.05;              % Mutation probability
nsbit=16;             % String length (bits)
% Generating initial population
popnew=init_gen(popsize,nsbit);
fitness=zeros(1,popsize);    % fitness array
% Display the shape of the function
x=range(1):0.1:range(2); plot(x,f(x));
% Initialize solution <- initial population
for i=1:popsize,
   solnew(i)=bintodec(popnew(i,:));
end
% Start the evolution loop
for i=1:MaxGen,
   % Record as the history
   fitold=fitness; pop=popnew; sol=solnew;
 for j=1:popsize,
   % Crossover pair
   ii=floor(popsize*rand)+1; jj=floor(popsize*rand)+1;
   % Cross over
   if pc>rand,
      [popnew(ii,:),popnew(jj,:)]=...
                    crossover(pop(ii,:),pop(jj,:));
   % Evaluate the new pairs
   count=count+2;
   evolve(ii); evolve(jj);
   end
   % Mutation at n sites
   if pm>rand,
    kk=floor(popsize*rand)+1;   count=count+1;
    popnew(kk,:)=mutate(pop(kk,:),nsite);
    evolve(kk);
   end
 end  % end for j
   % Record the current best
   bestfun(i)=max(fitness);
   bestsol(i)=mean(sol(bestfun(i)==fitness));
end
% Display results
subplot(2,1,1); plot(bestsol); title('Best estimates');
subplot(2,1,2); plot(bestfun); title('Fitness');
% ------------- All sub functions ----------
% generation of initial population
function pop=init_gen(np,nsbit)
% String length=nsbit+1 with pop(:,1) for the Sign
pop=rand(np,nsbit+1)>0.5;
% Evolving the new generation
function evolve(j)
global solnew popnew fitness fitold pop sol f;
   solnew(j)=bintodec(popnew(j,:));
   fitness(j)=f(solnew(j));
   if fitness(j)>fitold(j),
      pop(j,:)=popnew(j,:);
      sol(j)=solnew(j);
   end
% Convert a binary string into a decimal number
function [dec]=bintodec(bin)
global range;
% Length of the string without sign
nn=length(bin)-1;
num=bin(2:end);   % get the binary
% Sign=+1 if bin(1)=0; Sign=-1 if bin(1)=1.
Sign=1-2*bin(1);
dec=0;
% floating point.decimal place in the binary
dp=floor(log2(max(abs(range))));
for i=1:nn,
   dec=dec+num(i)*2^(dp-i);
end
dec=dec*Sign;
% Crossover operator
function [c,d]=crossover(a,b)
nn=length(a)-1;
% generating random crossover point
cpoint=floor(nn*rand)+1;
c=[a(1:cpoint) b(cpoint+1:end)];
d=[b(1:cpoint) a(cpoint+1:end)];
% Mutatation operator
function anew=mutate(a,nsite)
nn=length(a); anew=a;
for i=1:nsite,
   j=floor(rand*nn)+1;
   anew(j)=mod(a(j)+1,2);
end


function [ n_data, nu, x, fx ] = bessel_ix_values ( n_data )

%*****************************************************************************80
%
%% BESSEL_IX_VALUES returns some values of the Ix Bessel function.
%
%  Discussion:
%
%    This set of data considers the less common case in which the
%    index of the Bessel function In is actually not an integer.
%    We may suggest this case by occasionally replacing the symbol
%    "In" by "Ix".
%
%    The modified Bessel functions In(Z) and Kn(Z) are solutions of
%    the differential equation
%
%      Z^2 W'' + Z * W' - ( Z^2 + N^2 ) * W = 0.
%
%    In Mathematica, the function can be evaluated by:
%
%      BesselI[n,x]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 March 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz, Irene Stegun,
%    Handbook of Mathematical Functions,
%    National Bureau of Standards, 1964,
%    ISBN: 0-486-61272-4,
%    LC: QA47.A34.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Cambridge University Press, 1999,
%    ISBN: 0-521-64314-7,
%    LC: QA76.95.W65.
%
%  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 NU, the order of the function.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 28;

  fx_vec = [ ...
    0.3592084175833614E+00,  ...
    0.9376748882454876E+00,  ...
    2.046236863089055E+00,   ...
    3.053093538196718E+00,   ...
    4.614822903407601E+00,   ...
    26.47754749755907E+00,   ...
    2778.784603874571E+00,   ...
    4.327974627242893E+07,   ...
    0.2935253263474798E+00,  ...
    1.099473188633110E+00,   ...
    21.18444226479414E+00,   ...
    2500.906154942118E+00,   ...
    2.866653715931464E+20,   ...
    0.05709890920304825E+00, ...
    0.3970270801393905E+00,  ...
    13.76688213868258E+00,   ...
    2028.512757391936E+00,   ...
    2.753157630035402E+20,   ...
    0.4139416015642352E+00,  ...
    1.340196758982897E+00,   ...
    22.85715510364670E+00,   ...
    2593.006763432002E+00,   ...
    2.886630075077766E+20,   ...
    0.03590910483251082E+00, ...
    0.2931108636266483E+00,  ...
    11.99397010023068E+00,   ...
    1894.575731562383E+00,   ...
    2.716911375760483E+20 ];

  nu_vec = [ ...
    0.50E+00, ...
    0.50E+00, ...
    0.50E+00, ...
    0.50E+00, ...
    0.50E+00, ...
    0.50E+00, ...
    0.50E+00, ...
    0.50E+00, ...
    1.50E+00, ...
    1.50E+00, ...
    1.50E+00, ...
    1.50E+00, ...
    1.50E+00, ...
    2.50E+00, ...
    2.50E+00, ...
    2.50E+00, ...
    2.50E+00, ...
    2.50E+00, ...
    1.25E+00, ...
    1.25E+00, ...
    1.25E+00, ...
    1.25E+00, ...
    1.25E+00, ...
    2.75E+00, ...
    2.75E+00, ...
    2.75E+00, ...
    2.75E+00, ...
    2.75E+00 ];

  x_vec = [ ...
      0.2E+00, ...
      1.0E+00, ...
      2.0E+00, ...
      2.5E+00, ...
      3.0E+00, ...
      5.0E+00, ...
     10.0E+00, ...
     20.0E+00, ...
      1.0E+00, ...
      2.0E+00, ...
      5.0E+00, ...
     10.0E+00, ...
     50.0E+00, ...
      1.0E+00, ...
      2.0E+00, ...
      5.0E+00, ...
     10.0E+00, ...
     50.0E+00, ...
      1.0E+00, ...
      2.0E+00, ...
      5.0E+00, ...
     10.0E+00, ...
     50.0E+00, ...
      1.0E+00, ...
      2.0E+00, ...
      5.0E+00, ...
     10.0E+00, ...
     50.0E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

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

  return
end


function [ grid_num, point_num ] = cc_grids_minmax_size ( dim_num, q_min, ...
  q_max )

%*****************************************************************************80
%
%% CC_GRIDS_MINMAX_SIZE returns sizes for the CC_GRIDS_MINMAX.
%
%  Discussion:
%
%    This routine can be used to determine the necessary size to be
%    allocated to arrays GRID_ORDER and GRID_POINT in a call to
%    CC_GRIDS_MINMAX.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 October 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer Q_MIN, Q_MAX, the minimum and maximum values of
%    Q, the sum of the orders in each spatial coordinate.
%
%    Output, integer GRID_NUM, the number of Clenshaw Curtis
%    grids whose Q value is between Q_MIN and Q_MAX.
%
%    Output, integer POINT_NUM, the total number of points in the grids.
%

%
%  Determine the total number of points that will be generated
%  by "going through the motions".
%
  point_num = 0;
  grid_num = 0;

  for q = q_min : q_max

    more = 0;
    order_1d = [];

    while ( 1 )

      [ order_1d, more ] = compnz_next ( q, dim_num, order_1d, more );

      order_nd = prod ( order_1d(1:dim_num) );

      point_num = point_num + order_nd;

      grid_num = grid_num + 1;

      if ( ~more )
        break
      end

    end

  end

  return
end


function quadrule_test10 ( )

%*****************************************************************************80
%
%% TEST10 tests FEJER2_RULE_COMPUTE and FEJER2_RULE_SET.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 March 2007
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST10\n' );
  fprintf ( 1, '  FEJER2_RULE_COMPUTE computes\n' );
  fprintf ( 1, '  a Fejer type 2 quadrature rule.\n' );
  fprintf ( 1, '  FEJER2_RULE_SET sets\n' );
  fprintf ( 1, '  a Fejer type 2 quadrature rule.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Compare:\n' );
  fprintf ( 1, '    (X1,W1) from FEJER2_RULE_SET\n' );
  fprintf ( 1, '    (X2,W2) from FEJER2_RULE_COMPUTE\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, ...
    '     Order        W1            W2            X1            X2\n' );
  fprintf ( 1, '\n' );

  for order = 1 : 3 : 10

    [ x1, w1 ] = fejer2_rule_set ( order );
    [ x2, w2 ] = fejer2_rule_compute ( order );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  %8d\n', order );

    for i = 1 : order
      fprintf ( 1, '            %12f  %12f  %12f  %12f\n', ...
        w1(i), w2(i), x1(i), x2(i) );
    end

  end

  return
end


function [V,F] = engraving(im,w,t,s,l)
  % ENGRAVING Create an engraving of an image
  %
  % [V,F] = engraving(im,w,t,s)
  %
  % Inputs:
  %   im  width by height grayscale image
  %   w   desired width of engraving model (in mm)
  %   t  desired thickness of engraving model (in mm)
  %   s  desired span of thickness devoted to levels (in mm)
  %   l  number of levels
  % Outputs:
  %   V  #V by 3 list of vertex positions
  %   F  #F by 3 list of triangle indices
  %
  % Example:
  %  % load image
  %  im = imresize(rgb2gray(im2double(imread('hans-hass.jpg'))),0.5);
  %  % pad image by 10% of width
  %  im = padarray(im,ceil(0.1*repmat(size(im,2),1,2)));
  %  % engrave: 50mm wide, 5mm thick, 1mm devoted to 4 layers
  %  [V,F] = engraving(im,50,5,1,4);
  %

  assert(isfloat(im));
  [V,F] = create_regular_grid(size(im,2),size(im,1),0,0);
  V(:,1) = V(:,1)*w;
  V(:,2) = (1-V(:,2))*size(im,1)/size(im,2)*w;
  [V,F] = extrude(V,F);
  V(:,3) = V(:,3)*t;
  V(1:numel(im),3) = V(1:numel(im),3)-s*round(im(:)*(l-1))/(l-1);

end


function imgBlend = sc_poisson_blend(imgTrg, imgSrc, holeMask)
% SC_POISSON_BLEND: Blend the source and target image using a simple
% discrete Poisson solver
%
% Input:
%   - imgTrg: original image with hole
%   - imgSrc: synthesized image
%   - holeMask: specify the hole pixels
% Output:
%   - imgBlend: blended image

[imgH, imgW, nCh] = size(imgSrc);

% Initialize the reconstructed image
imgRecon = zeros(imgH, imgW, nCh);

% Independently process each channel
for ch = 1: nCh
    % Source and target image
    imgS = imgSrc(:,:,ch);
    imgT = imgTrg(:,:,ch);

    % prepare discrete Poisson equation
    [A, b] = solvePoisson(holeMask, imgS, imgT);

    % solve Poisson equation
    x = A\b;
    imgRecon(:,:,ch) = reshape(x, [imgH, imgW]);
end

% Combined with the known region in the target
holeMaskC = cat(3, holeMask, holeMask, holeMask);
imgBlend = holeMaskC.*imgRecon + ~holeMaskC.*imgTrg;

end

function [A, b] = solvePoisson(holeMask, imgS, imgT)

% Prepare the linear system of equations for Poisson blending

[imgH, imgW] = size(holeMask);
N = imgH*imgW;

% Number of unknown variables
numUnknownPix = sum(holeMask(:));

maxNumInd = 8*numUnknownPix; % Max number of indices for constructing the sparse matrix
maxNumEqn = 4*numUnknownPix; % Max number of equations (4-neighbor)

% 4-neighbors: dx and dy
dx = [1, 0, -1,  0];    dy = [0, 1,  0, -1];

% Initialize (I, J, S), for sparse matrix A where A(I(k), J(k)) = S(k)
I = zeros(maxNumInd, 1);
J = zeros(maxNumInd, 1);
S = zeros(maxNumInd, 1);

% Initialize b
b = zeros(maxNumEqn, 1);

% Precompute unkonwn pixel position
[pi, pj] = find(holeMask == 1);
pind = sub2ind([imgH, imgW], pi, pj);

% Precompute the 4-neighbor of the unkonwn pixel positions
qi = bsxfun(@plus, pi, dy);
qj = bsxfun(@plus, pj, dx);

% Handling cases at image borders
validN = (qi >= 1) & (qi <= imgH) & (qj >= 1) & (qj <= imgW);
qind = zeros(size(validN));
qind(validN) = sub2ind([imgH, imgW], qi(validN), qj(validN));

c = 1; % index counter for matrix A
e = 1; % equation counter

% Set up the matrix A and the vector b
for k = 1: numUnknownPix
    pind_cur = pind(k);
    for n = 1: 4 % 4-neighbor
        if(validN(k, n)) % if the neighbor pixel q lies in the image
            qind_cur = qind(k, n);
            if(holeMask(qind_cur))
                % A(e, pind_cur) = 1;
                I(c) = e;   J(c) = pind_cur;    S(c) = 1;   c = c + 1;
                % A(e, qind_cur) = 1;
                I(c) = e;   J(c) = qind_cur;    S(c) = -1;  c = c + 1;

                % gradient constraint
                b(e) = imgS(pind_cur) - imgS(qind_cur);
                e = e + 1;
            else
                % A(e, pind_cur) = 1;
                I(c) = e;   J(c) = pind_cur;    S(c) = 1;   c = c + 1;

                % boundary constraint
                b(e) = imgS(pind_cur) - imgS(qind_cur) + imgT(qind_cur);
                e = e + 1;
            end
        end
    end
end

% Clean up unused entries
nEqn = e - 1;
b = b(1:e-1);

nInd = c - 1;
I = I(1:nInd);  J = J(1:nInd);  S = S(1:nInd);

% Construct the sparse matrix A
A = sparse(I, J, S, nEqn, N);

end


function RHS = SliceMultiProd(M, X)
% function RHS = SliceMultiProd(M, X)
%
% PURPOSE: Multiple matrix product. This function performs the same
%          task as MULTIPROD but works on different shaping convention
%          for input/output matrices
%
% INPUT
%   M  : 3D array (m x n x p)
%   X  : 3D array (n x q x p)
% OUTPUT
%   RHS: 3D array (m x q x p)
%
% Compute p matrix products:
%   M(:,:,k) * X(:,:,k) = RHS(:,:,k) for all k=1,2,...,p
%
% NOTE -- Special call with X: common right matrix
%   Input argument X can be contracted as (n x q), and automatically
%   expanded by the function
%       M   is 3D array (m x n x p)
%       X   is 2D array (n x q)
%       RHS is 3D array (m x q x p)
%       M(:,:,k) * X = RHS(:,:,k) for all k=1,2,...,p
%
% See also: MultiProd, SliceMultiSolver
%
% Author: Bruno Luong <brunoluong@yahoo.com>
% History: original 11-August-2009

X = permute(X, [1 3 2]);
RHS = MultiProd(M, X);
if size(M,3)>1
    RHS = permute(RHS, [1 3 2]);
end


function idx=diagElIdx(N,minorDiag)
%%DIAGELIDX For an NXN matrix, determine the linear indices of the elements
%           on the main or minor diagonal of the matrix.
%
%INPUTS:  N The size of the NXN matrix.
% minorDiag Optionally, this can specify whether the main or minor diagonal
%           of the matrix is desired. minorDiag=false (the default if
%           omitted or an empty matrix is passed) chooses the main
%           diagonal, and minorDiag=true chooses the minor diagonal of the
%           matrix.
%
%OUTPUTS: idx A 1XN vector of the indices on the main (or minor) diagonal
%             of the matrix. Minor diagonal elements go from the lower-left
%             corner of the matrix to the upper-right corner.
%
%The major diagonal is also known as the principal diagonal, the primary
%diagonal, the major diagonal and the leading diagonal. The minor diagonal
%of a matrix is also known as the antidiagonal, the counterdiagonal, the
%trailing diagonal, and the secondary diagonal.
%
%EXAMPLE:
% N=8;
% M=magic(N)
% mainDiagEls=M(diagElIdx(N))
% minorDiagEls=M(diagElIdx(N,true))
%One will see that mainDiagEls contains the main diagonal elements of M,
%and minorDiagEls contains the minor diagonal elements, which increase
%sequentialy.
%
%September 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

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

if(minorDiag==false)
    idx=1:(N+1):(N^2);
else
    idx=N:(N-1):(N^2-N+1);
end
end

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


% Add a landmark to the UKF.
% We have to compute the uncertainty of the landmark given the current state
% (and its uncertainty) of the newly observed landmark. To this end, we also
% employ the unscented transform to propagate Q (sensor noise) through the
% current state

function [mu, sigma, map] = add_landmark_to_map(mu, sigma, z, map, Q);

% For computing sigma
global scale;

landmarkId = z.id;

%add landmark to the map
map = [map; landmarkId];
% TODO: Initialize its pose according to the measurement and add it to mu

% Append the measurement to the state vector
mu = [mu; z.range(); z.bearing()];
% Initialize its uncertainty and add it to sigma
sigma = blkdiag(sigma, Q);

% Transform from [range, bearing] to the x/y location of the landmark
% This operation intializes the uncertainty in the position of the landmark
% Sample sigma points
sig_pnts_new = compute_sigma_points(mu, sigma);
% Normalize!
sig_pnts_new(3,:) = normalize_angle(sig_pnts_new(3,:));
% Compute the xy location of the new landmark according to each sigma point
newX = sig_pnts_new(1,:) + sig_pnts_new(end-1,:).*cos(sig_pnts_new(3,:) + sig_pnts_new(end,:));
newY = sig_pnts_new(2,:) + sig_pnts_new(end-1,:).*sin(sig_pnts_new(3,:) + sig_pnts_new(end,:));
% The last 2 components of the sigma points can now be replaced by the xy pose of the landmark
sig_pnts_new(end-1,:) = newX;
sig_pnts_new(end,:) = newY;

% Recover mu and sigma
n = length(mu);
lambda = scale - n;
w0 = lambda/scale;
wm = [w0, repmat(1/(2*scale),1,2*n)];
wc = wm;
% Theta should be recovered by summing up the sines and cosines
cosines = sum(cos(sig_pnts_new(3,:)).*wm);
sines = sum(sin(sig_pnts_new(3,:)).*wm);
% recompute the angle and normalize it
mu_theta = normalize_angle(atan2(sines, cosines));
mu = sum(sig_pnts_new .* repmat(wm, rows(sig_pnts_new), 1), 2);
mu(3) = mu_theta;

diff = sig_pnts_new - repmat(mu,1,size(sig_pnts_new,2));
% Normalize!
diff(3,:) = normalize_angle(diff(3,:));
sigma = (repmat(wc, rows(diff), 1) .* diff) * diff';

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% DISCRETE VARIANCE SWAP/OPTION PRICER
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Descritpion: Script to Price Barrier options in Levy Models
%              using the PROJ method
% Author:      Justin Kirkby
% References:  (1) A General Framework for discretely sampled realized
%              variance derivatives in stocahstic volatility models with
%              jumps, EJOR, 2017
%              (2) Efficient Option Pricing By Frame Duality with The Fast
%              Fourier Transform, SIAM J. Financial Math., 2015
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[folder, name, ext] = fileparts(which( mfilename('fullpath')));
cd(folder);
addpath('../RN_CHF')
addpath('../Helper_Functions')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  Step 1) CHOOSE CONTRACT/GENERAL PARAMETERS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
K    = 0.01;  %Strike            %NOTE: no error handling in place for extreme values of W (increase grid if strike falls outside)
r    = .05;  %Interest rate
q    = .00;  %dividend yield
T    = 1;    %Time (in years)
M    = 252;  %number of discrete monitoring points

contract = 1;  %1 for for variance swap, 3 for variance call  (other contracts not yet coded)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  Step 2) CHOOSE MODEL PARAMETERS (Levy Models)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

model = 1;   % (CHOOSE) See Models Below (e.g. model 1 is Black Scholes), and choose specific params
params = {};

if model == 1 %BSM (Black Scholes Merton)
    params.sigmaBSM = 0.18;    %CHOOSE

elseif model == 2 %CGMY
    params.C  = 0.02;
    params.G  = 5;
    params.MM = 15;
    params.Y  = 1.2;

elseif model == 3 %NIG
    params.alpha = 15;
    params.beta  = -5;
    params.delta = 0.5;

elseif model == 4 %MJD (Merton Jump Diffusion)
    params.sigma  = 0.12;
    params.lam    = 0.4;
    params.muj    = -0.12;
    params.sigmaj = 0.18;

elseif model == 5 %Kou Double Expo
    params.sigma = 0.15;
    params.lam   = 3;
    params.p_up  = 0.2;
    params.eta1  = 25;
    params.eta2  = 10;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  Step 3) CHOOSE PROJ PARAMETERS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
UseCumulant = 1;  %Set to 1 to use the cumulant base rule (Approach 1) to determine gridwidth, else used fixed witdth (Approach 2)

%---------------------
% APPROACH 1: Cumulant Based approach for grid width
% (see "Robust Option Pricing with Characteritics Functions and the BSpline Order of Density Projection")
%---------------------
if UseCumulant ==1  %With cumulant based rule, choose N and Alpha (N = 2^(P+Pbar) based on second approach)
    logN  = 10;   %Uses N = 2^logN  gridpoint
    L1 = 14;  % determines grid witdth (usually set L1 = 8 to 15 for Levy, or 18 for Heston)
%---------------------
% APPROACH 2: Manual GridWidth approach
%---------------------
else %Manually specify resolution and Pbar
    P     = 7;  % resolution is 2^P
    Pbar  = 3;  % Determines density truncation grid with, 2^Pbar
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% PRICE
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  Note: rnCHF is the risk netural CHF, c1,c2,c4 are the cumulants
modelInput = getModelInput(model, T/M, r, q, params);

if UseCumulant ==1  % Choose density truncation width based on cumulants
    alpha = getTruncationAlpha(T, L1, modelInput, model);
else    % Manually supply density truncation width above
    logN = P + Pbar;
    alpha = 2^Pbar/2;
end
N = 2^logN;

tic
price = PROJ_DiscreteVariance_Swaps_Options( N,alpha,M,r,T,K,modelInput.rnCHF,contract);
toc

fprintf('price: %.8f\n', price)

if contract == 1  % In swap case, the price is known analytically (for Levy process)
    c2 = modelInput.c2; c1 = modelInput.c1;
    analytical = c2 + c1^2*T/M;
    fprintf('analytical SWAP price: %.8f \n', analytical)
    fprintf('----------------------------\n')
    fprintf('Error: %.2e\n', analytical-price)
end


function k = WalravenValeton_k(Lwa, wv_sigma)
%
%       k = WalravenValeton_k(Lwa, wv_sigma)
%
%
%        Input:
%           -Lwa: world adaptation luminance in cd/m^2
%           -wv_sigma: cd/m^2
%
%        Output:
%           -k: WalravenValeton's k value
%
%     Copyright (C) 2016 Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%     The paper describing this technique is:
%     "A Model of Visual Adaptation for Realistic Image Synthesis"
% 	  by James A. Ferwerda, Sumanta N. Pattanaik, Peter Shirley, Donald P. Greenberg
%     in Proceedings of SIGGRAPH 1996
%

if(~exist('wv_sigma', 'var'))
    wv_sigma = 100; %cd/m^2
end

k = (wv_sigma - Lwa / 4) ./ (wv_sigma + Lwa);

k(k < 0.0) = 0.0;

end


%computes the spectral crest from the magnitude spectrum
%> called by ::ComputeFeature
%>
%> @param X: spectrogram (dimension FFTLength X Observations)
%> @param f_s: sample rate of audio data (unused)
%>
%> @retval vtsc spectral crest factor
% ======================================================================
function [vtsc] = FeatureSpectralCrestFactor (X, f_s)

    % get maximum
    vtsc = max(X, [], 1) ./ sum(X, 1);

    % avoid NaN for silence frames
    vtsc (sum(X, 1) == 0) = 0;
end


function [huff entropy avglength redundancy]=huffman(alpha,prob)
s=sum(prob(:));
s=roundn(s,-4);
% Calculate length of source and probability
la=length(alpha);
lp=length(prob);
if (la==lp & s==1)
       %Calculate the Entropy
         entropy=prob.*log2(prob);
         entropy=-sum(entropy(:));
         pos=1:lp;
         [prs idx]=sort(prob,'descend');
         npos=pos(idx);
         %Minimum Variance
         idx=find(prs==min(prs(:)));
         tp=npos(idx);
         tp=sort(tp,'descend');
         npos(idx)=tp;
         %
         codebook(1:lp)={''};
         ps=npos;
         np=lp;
         cb=zeros([lp-1 3]);
         cnt=lp+1;
         prb=prs;
         for i=1:lp-1
              fst=ps(np-1);
              sec=ps(np);

                if fst<=lp
                   codebook(fst)=strcat('0',codebook(fst));
                else
                  codebook=encod(fst,cb,codebook,'0',lp);

                end

                if sec<=lp
                    codebook(sec)=strcat('1',codebook(sec));
                else
                   codebook=encod(sec,cb,codebook,'1',lp);
                end
              cb(i,1)=cnt;
              cb(i,2)=fst;
              cb(i,3)=sec;
              if np>2
                  ps=ps(1:np-2);
                  ps(np-1)=cnt;
                  cnt=cnt+1;
                  prbt=prb(1:np-2);
                  prbt(np-1)=prb(np-1)+prb(np);
                  prb=prbt;
                  [prb idx]=sort(prb,'descend');
                  ps=ps(idx);
                  %Minimum Variance
                  idx=find(prb==prbt(np-1));
                  tp=ps(idx);
                  tp=sort(tp,'descend');
                  ps(idx)=tp;
                  %
                  np=np-1;
              end

         end
         for i=1:lp
            huff(i).sym=alpha(i);
            huff(i).prob=prob(i);
            huff(i).code=codebook(i);
         end
           avglength=0;
           for i=1:lp
               avglength=avglength+huff(i).prob*length(cell2mat(huff(i).code));
           end
           redundancy=((avglength-entropy)/entropy)*100;
else
    display('Error in input.....');
    huff=[];
end

function codebook=encod(fs,cb,codebook,str,lp)
idx=find(cb(:,1)==fs);
x=cb(idx,2);
y=cb(idx,3);
 if x<=lp & y<=lp
     codebook(x)=strcat(str,codebook(x));
     codebook(y)=strcat(str,codebook(y));
     return
 elseif x<=lp
     codebook(x)=strcat(str,codebook(x));
     codebook=encod(y,cb,codebook,str,lp);
 elseif y<=lp
     codebook(y)=strcat(str,codebook(y));
     codebook=encod(x,cb,codebook,str,lp);
 else
     codebook=encod(x,cb,codebook,str,lp);
     codebook=encod(y,cb,codebook,str,lp);
 end