function [u,v,px,py] = perform_tv_hilbert_projection(f,kernel,lam,options)

% Aujol & Chambolle projection for solving TV-K regularization
%
%   [u,v,px,py] = perform_tv_hilbert_projection(f,kernel,lam,options);
%
%   Solve the image separation problem :
%       u = argmin_u |f-u|_K^2 + lam*|u|_TV
%
%   f = u+v
%   u is the cartoon part.
%   v is the texture part.
%
%   where |.|_K is an hilber space norm defined by some operator K
%       <f,g>_K = <f,K*g>
%       |f|_K^2 = <f,f>_K
%
%   f is the input image.
%   kernel is a callback function of the kernel operator, should be like
%       f = kernel(f, options);
%
%	Set options.use_gabor==1 to setup the Gabor Kernel automatically
%	and options.use_gabor==-1 to set up the L2 identity kernel.
%
% Original code by Guy Gilboa, modified by Gabriel Peyre
%
%   Copyright (c) 2006 Gabriel Peyre

[ny,nx]=size(f);

options.null = 0;
if isfield(options, 'niter')
    niter = options.niter;
else
    niter = 30;
end
if isfield(options, 'dt')
    dt = options.dt;
else
    dt = 0.01;
end
if isfield(options, 'p0x') && ~isempty(options.p0x)
    p0x = options.p0x;
else
    p0x=zeros(ny,nx);
end
if isfield(options, 'p0y') && ~isempty(options.p0y)
    p0y = options.p0y;
else
    p0y=p0x;
end

use_gabor = 1;
if isfield(options, 'use_gabor')
    use_gabor = options.use_gabor;
end

px=p0x; py=p0y;
lami=1/lam;  % here the algorithm works with inverse of lambda

if use_gabor==1
    sig_k=1; freq=0.25; bias=0;
    n=11; %kernel size
    x=ones(n,1)*(-(n-1)/2:(n-1)/2); y=x';
    gs=exp(-((x/sig_k).^2+(y/sig_k).^2)/2);  % 2D gaussian
    gs=gs/sum(sum(gs)); % normalize
    r=sqrt(x.^2+y.^2); cs=cos(2*pi*r*freq); %radially symmetric
    iKx = cs.*gs; %% "Gabor filter" for inverse K
    iKx=iKx-mean(mean(iKx)); % zero mean filter
    cp=(n+1)/2; % center of kernel
    iKx(cp,cp) = iKx(cp,cp)-bias;
elseif use_gabor==-1
    iKx = 1;
end

%% perform iterations
if use_gabor~=0
    for i=1:niter,  %% do niterations
        progressbar(i,niter);
        %% compute projection
        km1div = filter2(iKx,div(px,py))-f*lam;
        %[Gx,Gy] = grad(km1div);
        Gx=gradx(km1div); Gy=grady(km1div);
        aG = sqrt(Gx.^2+Gy.^2); %abs(G)
        px = (px + dt*Gx)./(1+dt*aG);
        py = (py + dt*Gy)./(1+dt*aG);
    end % for i
    v = filter2(iKx,div(px,py))/lam;
else
    for i=1:niter,  %% do niterations
        progressbar(i,niter);
        % compute projection
        km1div = feval( kernel, div(px,py), options ) - f*lam;
        % compute gradient
        Gx=gradx(km1div); Gy=grady(km1div);
%        [Gx,Gy] = grad(km1div);
        aG = sqrt(Gx.^2+Gy.^2); %abs(G)
        px = (px + dt*Gx)./(1+dt*aG);
        py = (py + dt*Gy)./(1+dt*aG);
    end % for i
    v = lami*feval( kernel, div(px,py), options );
end

u=f-v;

%% additional functions
%%%%%%%%%%%%%%%%%%%%%%%%%%% Gradient (forward difference)
function [fx,fy] = grad(P)
error('Should not be used.');
fx = P(:,[2:end end])-P;
fy = P([2:end end],:)-P;
%%%%%%%%%%%%%%%%%%%%%%%%%%% Divergence (backward difference)
function M=div(px,py)
[m,n]=size(px);
M=zeros(m,n);
Mx=M; My=M;
Mx(2:m-1,1:n)=px(2:m-1,1:n)-px(1:m-2,1:n);
Mx(1,:)=px(1,:);
Mx(m,:)=-px(m-1,:);
My(1:m,2:n-1)=py(1:m,2:n-1)-py(1:m,1:n-2);
My(:,1)=py(:,1);
My(:,n)=-py(:,n-1);
M=Mx+My;
%%%%%%%%%%%%%%%%%%%%%%%%%%% Laplacian (central difference)
function fl = lap(P)
[gx,gy]=grad(P);
fl=div(gx,gy);
%%%%%%%%%%%%%%%%%%%%%%%%%%% Gradient on X (forward difference)
function M=gradx(I)
[m,n]=size(I);
M=zeros(m,n);
M(1:m-1,1:n)=-I(1:m-1,:)+I(2:m,:);
M(m,1:n)=zeros(1,n);
%%%%%%%%%%%%%%%%%%%%%%%%%%% Gradient on Y (forward difference)
function M=grady(I)
[m,n]=size(I);
M=zeros(m,n);
M(1:m,1:n-1)=-I(:,1:n-1)+I(:,2:n);
M(1:m,n)=zeros(m,1);


function [month, day, year] = gdate (jdate)

% convert Julian date to Gregorian (calendar) date

% input

%  jdate = julian day

% output

%  month = calendar month [1 - 12]
%  day   = calendar day [1 - 31]
%  year  = calendar year [yyyy]

%  note: day may include fractional part

% Orbital Mechanics with MATLAB

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

jd = jdate;

z = fix(jd + .5);
fday = jd + .5 - z;

if (fday < 0)
   fday = fday + 1;
   z = z - 1;
end

if (z < 2299161)
   a = z;
else
   alpha = floor((z - 1867216.25) / 36524.25);
   a = z + 1 + alpha - floor(alpha / 4);
end

b = a + 1524;
c = fix((b - 122.1) / 365.25);
d = fix(365.25 * c);
e = fix((b - d) / 30.6001);
day = b - d - fix(30.6001 * e) + fday;

if (e < 14)
   month = e - 1;
else
   month = e - 13;
end

if (month > 2)
   year = c - 4716;
else
   year = c - 4715;
end


function [m] = um2m(um)
% Convert length from micrometers (or microns) to meters.
% Chad A. Greene 2012
m = um*1e-6;


function sigmaVal = fwhm2sigma(fwhmVal)
%FWHM2SIGMA
%   Conversion factor proved here:
%   https://brainder.org/2011/08/20/gaussian-kernels-convert-fwhm-to-sigma/

    conversionFactor = sqrt(8*log(2));

    sigmaVal = fwhmVal./conversionFactor;
end


function [maxFreqDev,omega,freqs] = calcFreqDev(stimList,conditions,freqConditions)
% function [maxFreqDev,omega,freqs] = calcFreqDev(stimList,conditions,freqConditions)
%
%
% conditions is integer vector of conditions of interest
% freqConditions is vector of propotions of all conditions

			nc = size(conditions,2);

            for i = 1:nc
         		freqs(i) = sum(stimList == conditions(i));
      	    end

			% not necessary if forcing sum to 1
			% freqs =  freqs ./ size(stimList,1);

			% force frequencies to sum to 1 b/c otherwise rest intervals will influence frequencies
            freqs = freqs ./ sum(freqs);

			maxFreqDev = max(abs(freqConditions(1:nc) - freqs));

			omega = 1 - maxFreqDev;

return


function [var_prc,pci,Nneeded,pcitarget] = var_prctile(p,n_in_sample,varargin)
% :Usage:
% ::
%
%     [var_prc,pci,Nneeded,pcitarget] = var_prctile(p,n_in_sample,['nboot',nboot],['x',data])
%
%     [var_prc,pci,Nneeded,pcitarget] = var_prctile(p,50,'nboot',5000)
%
% :Inputs:
%
%   **p:**
%        is desired prctile of data (threshold)
%
%   **nboot:**
%        is number of bootstrap samples (if bootstrapping)
%
%   **n_in_sample:**
%        is number of obs. in original sample
%
%   **x:**
%        is data sample of distribution of interest (empirical pdf based on
%        this)
%
%        Note: using empirical PDF/CDF depends a great deal on choice of h (see
%        code).
%
% :Examples:
% ::
%
%    Nneeded = [];
%    for p = [.05:-.001:.001]
%        [var_prc,pci,Nneeded(end+1),pcitarget] = var_prctile(x,p);
%    end
%    figure;
%    plot([.05:-.001:.001],Nneeded)
%
% ..
%    tor wager, jan 2007
% ..

nboot = Inf;
norm_model = 1;
x = [];
for i = 1:length(varargin)
    if ischar(varargin{i})
        switch varargin{i}
            case 'nboot', nboot = varargin{i+1};                    % # bootstrap samples
            case {'x','data'}, x = varargin{i+1}; norm_model = 0;   % use empirical PDF

            otherwise
                error('Unknown string option');
        end
    end
end

% ------------------------------------------------------------
% * get PDF
% ------------------------------------------------------------

if norm_model
    % get normal PDF at p-th percentile
    % -------------------------------------------
    prc = norminv(p);
    pdfval = normpdf(prc);
else
    % empirical estimate of PDF from x (data)
    % -------------------------------------------
    % prc is x-score at prctile of interest
    prc = prctile(x,100*p);

    % now we need pdf at that prctile.
    % pick some unit h, and differentiate around prc

    h = max(.01,1000 ./ length(x));  % enough so we have reasonable idea
    h = min(h,p);
    h = max(h,1000 ./ length(x));

    pdfval = 0;

    while pdfval == 0
        pdfval = emppdf(x,prc,h);

        h = h + .01;
    end
end

% ------------------------------------------------------------
% * get variance
% ------------------------------------------------------------

%for normal: fit = ( 1./ (normpdf(norminv(p)).^2) ) .* (p *(1-p)./N);
% from Brown and Wolfe, asymptotic
%var_prc = ( 1./ (pdfval.^2) ) .* (p *(1-p)./N);

% from Martin's bootstrap book, Ch 19, Eq. 19.7, p. 275
var_prc = ( 1./ (pdfval.^2) ) .* (p *(1-p)) .* (1./nboot + 1./n_in_sample);

if nargout < 2, return, end

% ------------------------------------------------------------
% * get confidence interval
% ------------------------------------------------------------

halfci_prc = 1.96 .* sqrt(var_prc);

% lower and upper bounds on p-value derived from distribution of x
pci = get_pval_ci(x,prc,halfci_prc);

if nargout < 3, return, end

% now get nboot needed to achieve target
% ---------------------------------------

% expression for tolerance: if upper bound of p-value is within 10% of p-value
maxN = 50000;               % maximum number of iterations
Nneeded = 500;              % starting estimate for nboot needed (should be fairly low)
Nstep = 500;                % increase Nneeded in units of Nstep until satisfied
p_upper_bound = p + .1 * p; % upper bound for p desired; now 10% larger than p
pcitarget = Inf;            % target conf. interval for p-values with Nneeded boot samples

% computations we don't have to repeat.  Divide by sqrt(N) to get halfci of prc
halfci_squared_determiner = 1.96 .* sqrt( ( 1./ (pdfval.^2) ) .* p *(1-p) );

while ( max(pcitarget) > p_upper_bound ) && ( Nneeded < maxN )

    Nneeded = Nneeded + Nstep;
    halfci_prc = halfci_squared_determiner ./ sqrt(Nneeded);
    pcitarget = get_pval_ci(x,prc,halfci_prc);

end

end

function pci = get_pval_ci(x,prc,halfci_prc)
% get confidence interval for p-value, based on data x, x-axis-value prc,
% and confidence half-interval for prc

if isempty(x)
    % normal CDF
    pci = [max(0,normcdf(prc - halfci_prc)) min(1,normcdf(prc + halfci_prc))];
else
    % empirical CDF
    pci = [max(0,empcdf(x,prc - halfci_prc)) min(1,empcdf(x,prc + halfci_prc))];
end

end

function pdfval = emppdf(x,prc,h)
% empirical PDF of data numerically differentiated in a window h
pdfval = ( sum(x <= prc + h) - sum(x <= prc - h) ) ./ (length(x) * 2 * h);

end

function p = empcdf(x,prc)
% empirical CDF of data x at prctile prc

p = sum(x <= prc) ./ length(x);
end


function [output, fs] = sp_johansson_impl_n3(inp_fs, input, time_skew)
% Reconstruction of a signal from non-uniform samples
% Based on Sindhi and Prabhu's implementation of Johansson's
% method

% index_mapping is used to reorded filters in case the first input
% signal is actually delayed comparing to the second and not vice versa
N = length(time_skew); % number of samplers
fs = inp_fs * N; % full sampling frequency

T_inp = 1/inp_fs;

% index_mapping is used to reorded filters in case the first input
% signal is actually delayed comparing to the second and not vice versa
[time_skew, index_mapping] = sort(time_skew);
x11 = cell2mat(input')';
x11 = x11(index_mapping, :);

TQ = 1; % Nyquist sampling period

% Decimation Periods - in our case all ADCs sample with the same rate
T = [3*TQ 3*TQ 3*TQ];

K = 0.5*lcm(2*T(1), 2*T(2))/TQ; % number of samples in recurrent period
K = 0.5*lcm(2*K, 2*T(3))/TQ;
capT = K*TQ; % the full sampling period - of all samplers
M = capT./T;
capM = lcm(M(1), M(2));
capM = lcm(capM, M(3));
excess = ceil((K-1)/capM);

maxf = K/(excess*capM+1);
K1 = K/maxf;

ML = min(cellfun('length', input)); % number of slices
w_c = 0.85;

LF = capM*2*K1+1;  %% min length of LF should be capM*2*K1
n = -(LF-1)/2:1:(LF-1)/2;

taus = time_skew / T_inp * capT; % ADC delays in seconds
tausI = sort([taus(1) taus(2) taus(3)]);
tauI = zeros(K,ML);
for p = 1:K
    tauI(p,:) = tausI(p)+(0:ML-1)*capT;
end;

r = tausI-TQ*(0:K-1);
r = r.';
w_o = w_c*pi*TQ;
hJ = zeros(K,LF);
C = zeros(1,LF);
Nt = (LF-1)/2;
for i = 1:K
    C = -2*sin(w_o*(n-r(1+(mod(i-1-n,K)))'))./(pi*(n-r(1+(mod(i-1-n,K)))'));
    C(isnan(C))=-2*w_o/pi;
    C = C.';
    S = zeros(LF,LF);
    for k = 1:LF
        S(k,:) = sin(w_o*(-Nt+k-1-r(1+(mod(i-1-(-Nt+k-1),K)))-(n-r(1+(mod(i-1-n,K)))')))./(pi*(-Nt+k-1-r(1+(mod(i-1-(-Nt+k-1),K)))-(n-r(1+(mod(i-1-n,K)))')));
    end;
    S(isnan(S))=w_o/pi;
    hJ(i,:) = -0.5*S\C;
end;

x1 = reshape(x11,1,size(x11,2)*K);
y1 = zeros(K,length(x1));
for j=1:K
    y1(j,:) = filter(hJ(j,:),1,x1);
    y1(j,:) = upsample(downsample(y1(j,:),K,j-1),K)/K;
    y1(j,:) = filter([zeros(1,j-1),1],1,y1(j,:));
end;
y = K*0.25*sum(y1,1);
delayJ = (size(hJ,2)-1)/2;
y = real(y(1+delayJ:end));

output = y;
end


%%********************************************************************
%% minEpts: minimum volume ellipsoid containing given
%%          points, V(:,1),...,V(:,m).
%%
%% max log(det(B))
%% s.t. || Bx + d || <= 1, for all x = V(:,1),...,V(:,m)
%%
%% [blk,At,C,b,OPTIONS,B,d] = minEpts(V,solve)
%%
%% For problem formulation, see
%% Vandenberghe, Boyd, Wu, Determinant maximization with linear
%% matrix inequalities, SIAM J. Matrix Analysis and Applications,
%% 19 (1998), pp. 499--533.
%%
%%********************************************************************

  function  [blk,At,C,b,OPTIONS,B,d] = minEpts(V,solve)

  if (nargin < 2); solve = 0; end;
%%
%% form data matrices
%%
  [p,m] =  size(V); N = (p+1)*m;
  blk{1,1} = 's'; blk{1,2} = (p+1)*ones(1,m);
  blk{2,1} = 's'; blk{2,2} = p;
%%
  count = 0;
  for j = 1:p
     s1 = V(j,:)'; i1 = [j:(p+1):(p+1)*(m-1)+j]; j1 = [p+1:p+1:(p+1)*m];
     tmp = sparse(i1,j1,s1,N,N);
     tmp = tmp + tmp';
     count = count + 1;
     F{1,count} = -tmp;
     F{2,count} = sparse(j,j,-1,p,p);
  end
  for j = 2:p
     for k = 1:j-1
        s1 = V(k,:)'; i1 = [j:(p+1):(p+1)*(m-1)+j]; j1 = [p+1:p+1:(p+1)*m];
        s2 = V(j,:)'; i2 = [k:(p+1):(p+1)*(m-1)+k]; j2 = [p+1:p+1:(p+1)*m];
        tmp  = sparse(i1,j1,s1,N,N);
        tmp  = tmp + sparse(i2,j2,s2,N,N);
        tmp = tmp + tmp';
        count = count + 1;
        F{1,count} = -tmp;
        F{2,count} = sparse([j k],[k j],[-1 -1],p,p);
     end
  end
  for j = 1:p
     s1 = ones(m,1);
     i1 = [j:(p+1):(p+1)*(m-1)+j]; j1 = [p+1:p+1:(p+1)*m];
     tmp = sparse(i1,j1,s1,N,N);
     tmp = tmp + tmp';
     count = count + 1;
     F{1,count} = -tmp;
     F{2,count} = sparse(p,p);
  end
  At = svec(blk,F,ones(2,1));
  C{1,1} = speye(N);
  C{2,1} = zeros(p);
  b = zeros(p*(p+1)/2+p,1);
  parbarrier{1,1} = 0;
  parbarrier{2,1} = 1;
  OPTIONS.parbarrier = parbarrier;
%%
%% || Bx + d || <= 1.
%% x'*(B'*B)*x + 2(B'*d)'*x  + d'*d <= 1.
%%
  if (solve)
     [obj,X,y,Z,info] = sqlp(blk,At,C,b,OPTIONS);
     if (length(y) ~= p*(p+3)/2)
        error('length of y not compatible with p');
     end
     B = diag(y(1:p));
     tmp = p;
     for k = 1:p-1
         B(k+1:p,k) = y(tmp + [1:p-k]);
         B(k,k+1:p) = B(k+1:p,k)';
         tmp = tmp + p-k;
     end;
     d = y(tmp+1:length(y));
  else
     B = []; d = [];
  end
%%********************************************************************


%VGG_SOLVELIN_BLKSYM  Solves M*x==y where M is (typically huge sparse) symmetric 4-block matrix.
%   It solves the system much more efficiently than a general (sparse) linear system solver.
%   Typical usage to solve normal equations in Levenberg-Marquardt in bundle adjustment.
%
%   X = VGG_SOLVELIN_BLKSYM(A,B,C,p,q [,'nocheck']) solves M*x==Y where
%   M=[A B; B' C] and y=[p;q].
%
%   X = VGG_SOLVELIN_BLKSYM(M,y,sideA [,'nocheck']) does the same but takes M and splits it,
%   M=[A B; B' C] and size(A)==[sideA sideA].
%
%   The function checks whether C is really near to diagonal; if not, a warning is printed.
%   Parmeter 'nocheck' switches off this test.

% (c) {awf,werner}@robots.ox.ac.uk, March 2002

function x = vgg_solvelin_blksym(varargin)

switch nargin
 case 6
  [A,B,C,p,q,check] = deal(varargin{:});
 case 5
  [A,B,C,p,q] = deal(varargin{:});
  check = '';
 case 4
  [M,y,sideA,check] = deal(varargin{:});
 case 3
  [M,y,sideA] = deal(varargin{:});
  check = '';
 otherwise
  error('Bad number of parameters');
end

if nargin<5
  Rtop = 1:sideA;
  Rbot = sideA+1:size(M,1);
  C = M(Rbot, Rbot);
  B = M(Rtop, Rbot);
  A = M(Rtop, Rtop);
  p = y(Rtop);
  q = y(Rbot);
end

if ~strcmp(check,'nocheck') & nnz(C(1,:))/size(C,1) > .1
  warning('It is likely that M was splitted incorrectly. Check diagonality of C and/or value of sideA.');
end

%tic

% Surprisingly, branch 1 is 2x slower than branch 2. We don't know why.
switch 2
 case 1
  invC_Btq = C \ [B' q];
 case 2
  invC_Btq = inv(C) * [B' q];
end

invC_Bt = invC_Btq(:,1:end-1);
invC_q = invC_Btq(:,end);

u = (A - B * invC_Bt) \ (p - B * invC_q);
v = invC_q - invC_Bt*u;

x = [u;v];

%toc

return


% Computes gain using the covariance matrix.
% Copyrights Farhat Masood (NUST) Pakistan

function [k,s] = kfilter(A,C,V1,V2,V12)%function [k,s] = kfilter(A,C,V1,V2,V12)%KFILTER can have arguments: (A,C,V1,V2) if there are no cross% products, V12=0.%     KFILTER calculates the kalman gain, k, and the stationary%     covariance matrix, s, using the Kalman filter for:%  %		x[t+1] = Ax[t] + Bu[t] + w1[t+1]%               y[t] = Cx[t] + Du[t] + w2[t]%%               E [w1(t+1)] [w1(t+1)]' =  [V1   V12;%                 [ w2(t) ] [ w2(t) ]      V12' V2 ]%%  where x is the mx1 vector of states, u is the nx1 vector of controls, y is%  the px1 vector of observables, A is mxm, B is mxn, C is pxm, V1 is mxm,%  V2 is pxp, V12 is mxp.%m=max(size(A));[rc,cc]=size(C);if nargin==4; V12=zeros(m,rc); end;if (rank(V2)==rc);  A=A-(V12/V2)*C;  V1=V1-V12*(V2\V12');  [k,s]=doubleo(A,C,V1,V2);  k=k+(V12/V2);else;  s0=.01*eye(m);  dd=1;  it=1;  maxit=1000;  while (dd>1e-8 & it<=maxit);    k0= (A*s0*C'+V12)/(V2+C*s0*C');    s1= A*s0*A' + V1 -(A*s0*C'+V12)*k0';    k1= (A*s1*C'+V12)/(V2+C*s1*C');    dd=max(max(abs(k1-k0)));    it=it+1;    s0=s1;  end;  k=k1;s=s0;  if it>=maxit;     disp('WARNING: Iteration limit of 1000 reached in KFILTER.M');   end;end;


function [a,c]=HermiteInterpMultiPoly(x,y,numDims)
%%HERMITEINTERPMULTIPOLYS Given a multidimensional function y(x) evaluated
%             a certain values of x (control points) along with an
%             arbitrary number of derivatives of y(x) at those points, this
%             function returns the coefficients of the Hermite
%             interpolating polynomials fitting those points across all
%             dimensions. This function differs from
%             findHermiteInterpPolySet in that this function tries to find
%             one high-order Hermite interpolation polynomial for each
%             dimension that matches all of the points given, whereas
%             findHermiteInterpPolySet finds a set of low-order
%             polynomials, which is more practical for large regions as
%             finite precision errors can dominate this function. Compare
%             this to the function HermiteInterpPoly, which can only handle
%             scalar values.
%
%INPUTS: x An NpX1 or 1XNp vector of real, scalar values at which the
%          function y and its derivative are given. The values are assumed
%          to be given in increasing order. Np>=2.
%        y A (numMoments*numDims)XNp matrix of values of the
%          multidimensional function y(x) and its derivatives evaluated at
%          the points in x. All values for a particular derivative are
%          given before any of the next derivative. For example, to
%          interpolate in 3D position and velocity, the ordering would be
%          [x;y;z;xDot;yDot;zDot].
%  numDims The scalar number of dimensions present. This will typically be
%          2 or 3 when dealing with target states. If this parameter is
%          omitted or an empty matrix is passed, then numDims=1 is used.
%
%OUTPUTS: a The numDimsXnumCoeff matrix of the interpolating polynomial
%           coefficients for each dimension and moment. This and the
%           next output can be used in the function polyValNewton to
%           interpolate values.
%         c The numDimsX(numCoeff-1) matrix of control points for the
%           interpolating polynomials.
%
%This function calls HermiteInterpPoly for each of the dimensions present
%and stacks the results by dimension.
%
%May 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<3||isempty(numDims))
    numDims=1;
end

if(numDims==1)
    [a,c]=HermiteInterpPoly(x,y);
    return
end

Np=length(x);
stateSize=size(y,1);

numMatch=stateSize/numDims;
numCoeff=numMatch*Np;

a=zeros(numDims,numCoeff);
c=zeros(numDims,numCoeff-1);

for curDim=1:numDims
    yCur=y(curDim:numDims:stateSize,:);
    [a(curDim,:),c(curDim,:)]=HermiteInterpPoly(x,yCur);
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.


function fourthorder
%%
%     laplace^2 u = f;   [0,1]^2
%               u = g_D;
%       Du\cdot n = g_N.
%
%more information , you can check fourorderdata.m,biharmonicP1.m,biharmonicP2.m
%biharmonicP3.m.
% Created by Jie Zhou.
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.
close all
%% Parameters
%maxN = 2e3;     theta = 0.5;
maxIt = 6;
N = zeros(maxIt,1);     errL2 = zeros(maxIt,1);     errH1 = zeros(maxIt,1);

%%  Generate an initial mesh
node = [0,0; 0,1; 1,0;1,1];        % nodes
elem = [1,3,2; 4,2,3];                 % elements
elem = label(node,elem);               % label the mesh
bdEdge = setboundary(node,elem,'Dirichlet');    % Dirichlet boundary condition

% showmesh(node,elem);                            % plot mesh
% findelem(node,elem,'all','index','color','g');  % plot element indices
% findnode(node,'all','index','color','r');       % plot node indices

%%  Get a fine mesh by uniform bisection
for k = 1:2
     [node,elem,bdEdge] = uniformbisect(node,elem,bdEdge);
%    [node,elem,bdEdge] = bisect(node,elem,'all',bdEdge);
end

showmesh(node,elem);
findnode(node,'all','index','color','r');
findelem(node,elem,'all','index','color','r');

%% Set up PDE data
pde = fourorderdata;

%%       Adaptive Finite Element Method
% *SOLVE* -> *ESTIMATE* -> *MARK* -> *REFINE*
 for k = 1:maxIt
%                   [w,u] = biharmonicP1(node,elem,pde,bdEdge);
                    [w,u] = biharmonicP2(node,elem,pde,bdEdge);
%                   [w,u] = biharmonicP3(node,elem,pde,bdEdge);
               errL2(k) = getL2error(node,elem,pde.exactu,u);
               errH1(k) = getH1error(node,elem,pde.Du,u);
                   N(k) = size(w,1)+size(u,1);
     [node,elem,bdEdge] = uniformbisect(node,elem,bdEdge);
 end

%  Plot convergence rates
N= N(1:k);  errH1 = errH1(1:k);     errL2 = errL2(1:k);
figure;
showrate2(N,errH1,3,'-*','||Du-Du_h||',...
          N,errL2,3,'k-+','||u-u_h||');


function fe2d_r_fast_test ( )

%*****************************************************************************80
%
%% FE2D_R_FAST_TEST tests the FE2D_R_FAST code.
%
%  Discussion:
%
%    This function sets all parameter values and initial condition information
%    necessary to execute the "fast" version of the fe2d_r algorithm.
%
%  Licensing:
%
%    Copyright (C) 2014 Marcus R. Garvie.
%    See 'mycopyright.txt' for details.
%
%  Modified:
%
%    28 April 2014
%
%  Author:
%
%    Marcus R. Garvie.
%
%  Reference:
%
%    Marcus R Garvie, John Burkardt, Jeff Morgan,
%    Simple Finite Element Methods for Approximating Predator-Prey Dynamics
%    in Two Dimensions using MATLAB,
%    Submitted to Bulletin of Mathematical Biology, 2014.
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FE2D_R_FAST_TEST:\n' );
  fprintf ( 1, '  Test the FE2D_R_FAST function\n' );
  fprintf ( 1, '  which applies Robin boundary conditions as it\n' );
  fprintf ( 1, '  approximates a solution to a predator-prey system.\n' );
%
%  Set the parameters.
%
  alpha = 0.4;
  beta = 2.0;
  gamma = 0.6;
  delta = 1.0;
%
%  Use T=150.0 for normal run.
%  Use T=0.50 for a "quick" run that might take 15 minutes of computing.
%
% T = 150.0;
  T = 0.50;
  delt = 1.0 / 384.0;
  k1 = 0.01;
  k2 = 0.01;

  t = tic;
  fe2d_r_fast ( alpha, beta, gamma, delta, T, delt, @u0f, @v0f, k1, k2 );
  t = toc ( t );

  fprintf ( 1, '  Execution took %10.2g minutes \n', t / 60.0 );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FE2D_R_FAST_TEST:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end

function value = u0f ( x, y )

%*****************************************************************************80
%
%% U0F evaluates the initial condition for U.
%
%  Licensing:
%
%    Copyright (C) 2014 Marcus R. Garvie.
%    See 'mycopyright.txt' for details.
%
%  Modified:
%
%    26 April 2014
%
%  Author:
%
%    Marcus R. Garvie.
%
%  Parameters:
%
%    Input, real X, Y, a location in the region.
%
%    Output, real VALUE, the initial condition for U at (X,Y).
%
  value = 6.0 / 35.0 - 2.0E-07 * ( x - 0.1 * y - 225.0 ) * ( x - 0.1 * y - 675.0 );

  return
end

function value = v0f ( x, y )

%*****************************************************************************80
%
%% V0F evaluates the initial condition for V.
%
%  Licensing:
%
%    Copyright (C) 2014 Marcus R. Garvie.
%    See 'mycopyright.txt' for details.
%
%  Modified:
%
%    26 April 2014
%
%  Author:
%
%    Marcus R. Garvie.
%
%  Parameters:
%
%    Input, real X, Y, a location in the region.
%
%    Output, real VALUE, the initial condition for V at (X,Y).
%
  value = 116.0 / 245.0 - 3.0E-05 * ( x - 450.0 ) - 1.2E-04 * ( y - 150.0 );

  return
end


function [Btu] = Nm2Btu(Nm)
% Convert energy or work from newton-meters to British thermal units.
% Chad A. Greene 2012
Btu = Nm*0.00094781707775;


function [P, p] = prob_node(CPD, self_ev, pev)
% PROB_NODE Compute prod_m P(x(i,m)| x(pi_i,m), theta_i) for node i (discrete)
% [P, p] = prob_node(CPD, self_ev, pev)
%
% self_ev(m) is the evidence on this node in case m.
% pev(i,m) is the evidence on the i'th parent in case m (if there are any parents).
% (These may also be cell arrays.)
%
% p(m) = P(x(i,m)| x(pi_i,m), theta_i)
% P = prod p(m)

if iscell(self_ev), usecell = 1; else usecell = 0; end

ncases = length(self_ev);
sz = dom_sizes(CPD);

nparents = length(sz)-1;
if nparents == 0
  assert(isempty(pev));
else
  assert(isequal(size(pev), [nparents ncases]));
end

n = length(sz);
dom = 1:n;
p = zeros(1, ncases);
if nparents == 0
  for m=1:ncases
    if usecell
      evidence = {self_ev{m}};
    else
      evidence = num2cell(self_ev(m));
    end
    T = convert_to_table(CPD, dom, evidence);
    p(m) = T;
  end
else
  for m=1:ncases
    if usecell
      evidence = cell(1,n);
      evidence(1:n-1) = pev(:,m);
      evidence(n) = self_ev(m);
    else
      evidence = num2cell([pev(:,m)', self_ev(m)]);
    end
    T = convert_to_table(CPD, dom, evidence);
    p(m) = T;
  end
end
P = prod(p);


function [W,Idx] = barycentricWeights(X,xBnd,N)
% [W,Idx] = barycentricWeights(X,xBnd,N)
%
% This function computes the weights and indicies such that
%
% INPUTS:
%   X = [d , n] = n points of interest, each is d-dimensional
%   xBnd = [d , 2] = [min,max] along each dimension
%   N = [1, d] = number of grid points along each dimension
%
% OUTPUTS:
%   W = [d+1, n] weight to apply to each index
%   Idx = [d+1, n] linear index corresponding to each weight
%
% NOTES:
%   --> y = f(x), where x is d-dimensional, and y is scalar
%   --> Y = Y(x1,x2,...,xd)
%   --> n = size(Y);
%   --> y(X(:,i)) = dot(W(:,i)*Y(Idx(:,i));
%
% ASSUME:
%   --> all data is uniformly spaced:
%       {xi} = linspace(xBnd(i,1), xBnd(i,2), n(i));
%
% REFERENCE:
%
% Based on paper: "Multidimensional Triangulation and Interpolation for
% Reinforcement Learning" by Scott Davies, NIPS 1996
%
% See Also: barycentricInterpolate

[d, n] = size(X);
W = zeros(d+1,n);
Idx = zeros(d+1,n);
for i=1:n
    [W(:,i),Idx(:,i)] = barycentricWeightsCore(X(:,i),xBnd,N);
end

end

function [w,idx] = barycentricWeightsCore(x,xBnd,N)
% [w,idx] = barycentricWeights(x,xBnd,n)
%
% This function computes the weights and indicies such that y = Y(idx)*w,
% where Y is some data, stored on a length(N) dimensional grid.
%
% INPUTS:
%   x = [d , 1] point of interest
%   xBnd = [d , 2] = [min,max] along each dimension
%   N = [1, d] = number of grid points along each dimension
%
% OUTPUTS:
%   w = [d+1,1] weight to apply to each index
%   idx = [d+1,1] linear index corresponding to each weight
%
% NOTES:
%   --> y = f(x), where x is d-dimensional, and y is scalar
%   --> Y = Y(x1,x2,...,xd)
%   --> n = size(Y);
%   --> y = dot(Y(idx),w);
%
% ASSUME:
%   --> all data is uniformly spaced:
%       {xi} = linspace(xBnd(i,1), xBnd(i,2), n(i));
%
% REFERENCE:
%
% Based on paper: "Multidimensional Triangulation and Interpolation for
% Reinforcement Learning" by Scott Davies, NIPS 1996

% Coerce any data that is out of range:
checkLow = x<xBnd(:,1); x(checkLow) = xBnd(checkLow,1);
checkUpp = x>xBnd(:,2); x(checkUpp) = xBnd(checkUpp,2);

% Translate and scale such that each grid cell is a unit cube:
x = x-xBnd(:,1);
x = x./(xBnd(:,2)- xBnd(:,1));
x = x.*(N-1);

% Seperate out into bin number and fractional length:
xBaseIdx = floor(x);  %Figure out the lower index (bin) number
checkCeil = xBaseIdx==(N-1); newCeil = N-2;
xBaseIdx(checkCeil) = newCeil(checkCeil);  %Fix special case for points on upper grid
x = x-xBaseIdx; %Keep the fraction along each dimension:

% Figure out which dimensions to build simplex along:
[~,I] = sort(x); %x must be a column vector!

% Build simplex by walking along each successive dimension
% The matrix X stores the verticies of the simplex, where each vertex is on
% the unit hypercube. The first vertex is always zeros(n,1) and the last is
% always ones(n,1). The intermediate verticies are selected based on the
% sort step. Since X will be triangular, and all entries are unity, it is
% trivial to solve the system: x = X*w for the unknown weights w by row
% reduction.
n = length(N);
X = zeros(n,n+1);
w = zeros(n+1,1);
wSum = 0;
for i=1:n
    X(I(i), (n+2-i):(n+1)) = 1;
    w(n+2-i) = x(I(i)) - wSum;
    wSum = wSum + w(n+2-i);
end
w(1) = 1-wSum;  %Final convex combination

% Compute the linear indices:
idx = zeros(n+1,1);
for i=1:(n+1)
    idx(i) = sub2idx(N, X(:,i)+xBaseIdx+1, n);
end

end

%%%% Custom version of sub2ind, that takes a vector input for the desired
%%%% input dimension, rather than a comma seperated list.
function idx = sub2idx(n,idxVec,d)
%
% Based on Matlab's sub2ind command
% Modified by Matthew Kelly, April 8, 2015
%
% CHANGES:
%   - must explicitly enter all dimensions: length(n) == length(idxVec)
%   - pass through for scalar: length(n)==1 is valid input
%   - must pass desired subscripts as a vector (not a comma-seperated-list)
%   - disabled most error checking
%
% INPUTS:
%   n = size(dataMatrix)
%   idxVec = list of desired indices. For example A(2,1) -> [2,1]
%
% OUTPUTS:
%   idx = linear index, such that:
%
%       A(idx) = A(idxVec(1), ... , idxVec(length(n));
%

if d == 1
    idx = idxVec;
elseif d ==2
    idx = idxVec(1) + (idxVec(2) - 1).*n(1);
else
    %Compute linear indices
    k = [1 cumprod(n(1:end-1))'];
    idx = 1;
    for i = 1:d
        v = idxVec(i);
        idx = idx + (v-1)*k(i);
    end
end

end


name = 'david50kf';
name = 'hand';
name = 'elephant-50kv';
name = 'bunny';
options.name = name;

path(path, '../toolbox_graph_data/off/');

rep = ['results/curvature/' name '/'];
if not(exist(rep))
    mkdir(rep);
end

[vertex,face] = read_mesh(name);
n = size(vertex,2);

options.symmetrize = 0;
options.normalize = 1;
type = 'combinatorial';
type = 'conformal';
if not(exist('L'))
    L = compute_mesh_laplacian(vertex,face,type,options);
end

% initial conditions
sigma = .015;
if strcmp(name, 'bunny')
    sigma = .002;
end
npoints = 10; m =0;
f = zeros(n,1);
while m<npoints
    clf;
    options.face_vertex_color = f;
    plot_mesh(vertex,face,options);
    colormap jet(256);
    disp('Click on mesh and then hit enter.');
    pause; p = select3d;
    if not(isempty(p))
        d = compute_distance_to_points(vertex,p)';
        f = f + (-1)^mod(m,2) *exp( -d/(2*sigma.^2) );
        m = m+1;
    end
end

Tmax = 80;
dt = .5;

rep = 'results/wave-equation/';
if not(exist(rep))
    mkdir(rep);
end

fprev = f;
m = 1;
for i=1:round(Tmax/dt)
    f1 = f;
    f = 2*f - fprev - dt^2 * L*f;
    fprev = f1;
    if mod(i,round(4/dt))==1
        a = f; a(a==max(a)) = max(abs(f));
        a(a==min(a)) = -max(abs(f));
        options.face_vertex_color = a;
        clf;
        plot_mesh(vertex,face,options);
        colormap jet(256);
        saveas(gcf, [rep name '-wave-eq-' num2string_fixeddigit(m,2) '.png'], 'png');
        m = m+1;
    end
end


function [Xhat] = aprxMAPGMM(Y,patchSize,noiseSD,imsize,GS,excludeList,SigmaNoise)
% approximate GMM MAP estimation - a single iteration of the "hard version"
% EM MAP procedure (see paper for a reference)
%
% Inputs:
%   Y - the noisy patches (in columns)
%   noiseSD - noise standard deviation
%   imsize - size of the original image (not used in this case, but may be
%   used for non local priors)
%   GS - the gaussian mixture model structure
%   excludeList - used only for inpainting, misleading name - it's a list
%   of patch indices to use for estimation, the rest are just ignored
%   SigmaNoise - if the noise is non-white, this is the noise covariance
%   matrix
%
% Outputs:
%   Xhat - the restore patches

% handle exclusion list - used for inpainting
if ~exist('excludeList','var')
    excludeList = [];
end

% Supports general noise covariance matrices
if (~exist('SigmaNoise','var'))
    SigmaNoise = noiseSD^2*eye(patchSize^2);
end

if ~isempty(excludeList)
    T = Y;
    Y = Y(:,excludeList);
end

% remove DC component
meanY = mean(Y);
Y = bsxfun(@minus,Y,meanY);

% calculate assignment probabilities for each mixture component for all
% patches
GS2 = GS;
PYZ = zeros(GS.nmodels,size(Y,2));
for i=1:GS.nmodels
    GS2.covs(:,:,i) = GS.covs(:,:,i) + SigmaNoise;
    PYZ(i,:) = log(GS.mixweights(i)) + loggausspdf2(Y,GS2.covs(:,:,i));
end

% find the most likely component for each patch
[~,ks] = max(PYZ);

% and now perform weiner filtering
Xhat = zeros(size(Y));
for i=1:GS.nmodels
    inds = find(ks==i);
    Xhat(:,inds) = ((GS.covs(:,:,i)+SigmaNoise)\(GS.covs(:,:,i)*Y(:,inds) + SigmaNoise*repmat(GS.means(:,i),1,length(inds))));
end

% handle exclusion list stuff (inpainting only)
if ~isempty(excludeList)
    tt = T;
    tt(:,excludeList) = bsxfun(@plus,Xhat,meanY);
    Xhat = tt;
else
    Xhat = bsxfun(@plus,Xhat,meanY);
end


%BCSDRAWDOWN BlueChipStock drawdown analysis

addpath ./source

load BlueChipBacktest

RefIndex = 45;

[NumMonths, NumAssets] = size(RetHistory);
NumStocks = NumAssets - 3;

NumPortfolios = 40;
Periodicity = 12;

NumPeriods = floor(NumMonths/Periodicity);
StartIndex = NumMonths - NumPeriods * Periodicity;
if StartIndex < 1
	NumPeriods = NumPeriods - 1;
	StartIndex = StartIndex + Periodicity;
end
EndIndex = NumMonths;

iend = StartIndex;

PortRet = ones(NumPortfolios, NumPeriods);
IndexRet = ones(1,NumPeriods);

for k = 1:NumPeriods
	istart = iend;
	iend = istart + Periodicity;

	% calculate asset returns at specified periodicity

	A = ones(NumAssets,1);
	for i = (istart+1):iend
		for j = 1:NumAssets;
			A(j) = A(j) * (1.0 + RetHistory(i,j));
		end
	end

	for j = 1:NumAssets
		if isnan(A(j))
			A(j) = 0.0;
		end
	end

	% calculate portfolio returns at specified periodicity

	H = PortHistory{istart};
	P = H * A(1:NumStocks);

	if (k > 1)
		for i = 1:NumPortfolios
			PortRet(i,k) = PortRet(i,k - 1) * P(i);
		end
		IndexRet(k) = IndexRet(k - 1) * A(RefIndex);
	end
end

MaxDD = maxdrawdown(PortRet');
IDD = maxdrawdown(IndexRet');
IDD = repmat(IDD,1,NumPortfolios);

figure(1);
plot(MaxDD);
hold all
plot(IDD)
set(gca,'ylim',[-1 0]);
ylabel('\bfMaximum Drawdown');
xlabel('\bfPortfolio Number on Efficient Frontier');
title('\bfDrawdown of Portfolio Sequence');
hold off


function precisions = precision_plot(positions, ground_truth, video_name, show)
%PRECISION_PLOT
%   Calculates precision for a series of distance thresholds (percentage of
%   frames where the distance to the ground truth is within the threshold).
%   The results are shown in a new figure if SHOW is true.
%
%   Accepts positions and ground truth as Nx2 matrices (for N frames), and
%   a title string.
%
%   Joao F. Henriques, 2014
%   http://www.isr.uc.pt/~henriques/

	max_threshold = 50;  %used for graphs in the paper

	precisions = zeros(max_threshold, 1);

	if size(positions,1) ~= size(ground_truth,1)
% 		fprintf('%12s - Number of ground truth frames does not match number of tracked frames.\n', title)

		%just ignore any extra frames, in either results or ground truth
		n = min(size(positions,1), size(ground_truth,1));
		positions(n+1:end,:) = [];
		ground_truth(n+1:end,:) = [];
	end

	%calculate distances to ground truth over all frames
    gt_center = [ground_truth(:,1)+(ground_truth(:,3)-1)/2 ground_truth(:,2)+(ground_truth(:,4)-1)/2];
    positions_center = [positions(:,1)+(positions(:,3)-1)/2 positions(:,2)+(positions(:,4)-1)/2];

    distances = sqrt(sum((positions_center-gt_center).^2,2));

    index = ground_truth>0;
    ind = (sum(index,2)==4);

    distances(~ind) = -1;

    %compute precisions
    for p = 1:max_threshold
        precisions(p) = nnz(distances <= p) / numel(distances);
    end

    %get annotation of plot
    score = precisions(20);
    tmp = sprintf('%.3f', score);
    tmpName = ['TrackerName' ' [' tmp ']'];
    titleName = 'Precision plots of error';

	%plot the precisions
	if show == 1
		figure('NumberTitle','off', 'Name',['Precisions - ' video_name])
		plot(precisions, 'r-', 'LineWidth',2)
        legend1 = legend(tmpName);
		xlabel('Threshold'), ylabel('Precision')
        title(titleName)
        saveas(gca,'PrecisionPlot','jpg')
	end

end


function err = getHdiverrorBDM1(node,elem,divSigma,sigmah,markedElem)
%% GETHDIVERRORBDM1 Hdiv norm of the approximation error for BDM1.
%
% The input divSigma is a function bundle, and sigma_h is a vector array
% whose component including two parts, the first part (i.e., sigmah(1:NE))
% is the line integral of flux in norm direction, i.e.
% sigmah_i = \int_{e_i} sigma\cdot n.
% the second part (i.e., sigmah(NE+1:2*NE)) is the dual basis written as
% sigmah_i = 3\int_{e_i} (\lambda1-\lambda2) sigma\cdot n  (ei = < 1 , 2 >)
%
% err = getHdiverrorBDM1(node,elem,divSigma,sigmah)
%
% Example
%
%     maxIt = 5;
%     node = [1,0; 1,1; 0,1; -1,1; -1,0; -1,-1; 0,-1; 0,0]; % nodes
%     elem = [1,2,8; 3,8,2; 8,3,5; 4,5,3; 7,8,6; 5,6,8];    % elements
%     bdEdge = setboundary(node,elem,'Dirichlet');
%     pde = mixBCdata;
%     err = zeros(maxIt,1); N = zeros(maxIt,1);
%     for i =1:maxIt
%         [node,elem,bdEdge] = uniformrefine(node,elem,bdEdge);
%         [u,sigma,NULL] = PoissonBDM1(node,elem,pde,bdEdge);
%         err(i) = getHdiverrorBDM1(node,elem,pde.f,-sigma,[]);
%         N(i) = size(u,1);
%     end
%     r1 = showrate(N,err,2);
%     legend('||\sigma - \sigma_h||_{H(div)}',['N^{' num2str(r1) '}'],...
%            'LOCATION','Best');
%
% See also getHdiverror3RT0, getL2errorRT0.
%
% Created by Ming Wang at Jan 17, 2011, M-lint modified at May 15, 2011.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

%% Construct Data Structure
[elem2dof,~,elem2edgeSign] = dofedge(elem);
NT = size(elem,1);
% compute div phi
[Dlambda,area] = gradbasis(node,elem);
% div phi = 2*(Dlambda_i,Rot_j);
rotMat = [0 -1; 1 0]; % rotation matrix for computing rotLambda.
divPhi(:,3) = 2*dot(Dlambda(:,:,1),Dlambda(:,:,2)*rotMat,2);
divPhi(:,1) = 2*dot(Dlambda(:,:,2),Dlambda(:,:,3)*rotMat,2);
divPhi(:,2) = 2*dot(Dlambda(:,:,3),Dlambda(:,:,1)*rotMat,2);
divSigmahp = zeros(NT,1);
for k = 1:3
    divSigmahp = divSigmahp + elem2edgeSign(:,k).*sigmah(elem2dof(:,k)).*divPhi(:,k);
end

%% compute Hdiv error element-wise
[lambda,w] = quadpts(2);
nQuad = size(lambda,1);
err = zeros(NT,1);
for p = 1:nQuad
    % quadrature points in the x-y-z coordinate
    pxy = lambda(p,1)*node(elem(:,1),:) ...
        + lambda(p,2)*node(elem(:,2),:) ...
        + lambda(p,3)*node(elem(:,3),:);
    divSigmap = divSigma(pxy);
    % compute divSigmahp at quadrature points
    err = err + w(p)*sum((divSigmap - divSigmahp).^2,2);
end
err = err.*area;
% modify the error
err(isnan(err)) = 0; % remove the singular part
if (nargin == 5) && ~isempty(markedElem)
    err = err(markedElem); % error on some marked region
end
err = sqrt(sum(err));


%DTRANSFORM Distance transform
%
% DT = DTRANSFORM(IM, OPTIONS) is the distance transform of the
% binary image IM. The value of each output pixel is the distance (pixels)
% to the closest set pixel.
%
% Options::
% 'Euclidean'   use Euclidean distance (default)
% 'cityblock'   use cityblock (Manhattan) distance
% 'show',T      display the evolving distance transform, with a delay of T
%               seconds between frames
%
% See also IMORPH, DISTANCEXFORM, DXform.

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

function d = dtransform(world, varargin)

    opt.metric = {'Euclidean', 'cityblock'};
    opt.show = [];

    [opt,args] = tb_optparse(opt, varargin);
    if length(args) > 0 && isnumeric(args{1})
        opt.show = args{1};
    end

    if strcmpi(opt.metric, 'cityblock')
        m = ones(3,3);
        m(2,2) = 0;
    elseif strcmpi(opt.metric, 'Euclidean')
        r2 = sqrt(2);
        m = [r2 1 r2; 1 0 1; r2 1 r2];
    end

    world(world==0) = Inf;
    world(world==1) = 0;

    count = 0;
    while 1
        world = imorph(world, m, 'plusmin');
        count = count+1;
        if opt.show
            cmap = gray(256);
            cmap = [1 0 0; cmap];
            colormap(cmap)
            image(world+1, 'CDataMapping', 'direct');
            set(gca, 'Ydir', 'normal');
            xlabel('x');
            ylabel('y');
            pause(opt.show);
        end

        if length(find(world(:)==Inf)) == 0
            break;
        end
    end

    if opt.show
        fprintf('%d iterations\n', count);
    end

    d = world;


function m = hlp_minor(X, row, col)
%
% calculate the ijth minor of matrix X by returning the determinant of X
% after removing the ith row and jth column
%
% Input:
%
%   X:      2- or 3-dimensional matrix
%   row,col to remove
%
% output:
%
%   m:      if X is 2-D, m = minor(X)
%           if X is 3-D, m(i) = minor(X(:,:,i))
%
%
% References:
%
% [1] Mullen T (2010) The Source Information Flow Toolbox (SIFT):
%   Theoretical Handbook and User Manual.
%   Available at: http://www.sccn.ucsd.edu/wiki/Sift/
%
%
% Author: Tim Mullen Dec 1st, 2010, SCCN/INC, UCSD.
% Email:  tim@sccn.ucsd.edu

% This function is part of the Source Information Flow Toolbox (SIFT)
%
% 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, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

nd = ndims(X);
if nd < 2 || nd > 3
    error('X must have 2 or 3 dimensions');
end

X(row,:,:) = [];
X(:,col,:) = [];

if nd==2
    m = det(X);
else
    m=zeros(size(X,3),1);
    for k=1:size(X,3)
        m(k) = det(X(:,:,k));
    end
end


function y = sigmoid(y, fac)
% SIGMOID nonlinear funcion for cochlear model
%	y = sigmoid(y, fac);
%	fac: non-linear factor
%	 -- fac > 0, transister-like function
%	 -- fac = 0, hard-limiter
%	 -- fac = -1, half-wave rectifier
%	 -- else, no operation, i.e., linear
%
%	SIGMOID is a monotonic increasing function which simulates
%	hair cell nonlinearity.
%	See also: WAV2AUD, AUD2WAV

% Auther: Powen Ru (powen@isr.umd.edu), NSL, UMD
% v1.00: 01-Jun-97

if fac > 0,
	%y = exp(y/fac); y = 1/(1+.1)-1./(1+.1*y);
	y = exp(-y/fac); y = 1./(1+y);
elseif fac == 0,
	y = (y > 0);	% hard-limiter
elseif fac == -1,
	y = max(y, 0);	% half-wave rectifier
elseif fac == -3,
	y = halfregu(y);
end;


%  INTERNAL FUNCTION: Prediction step for Kalman filter
%
%  ::
%
%    [a,P,Tt,Rt,Record]=prediction_step(T,R,att,Ptt)
%    [a,P,Tt,Rt,Record]=prediction_step(T,R,att,Ptt,MUt,OMGt,DPHI,DT,Record,ExpandedFlag)
%
%  Args:
%
%     - **T** [matrix] : m x m state matrix (autoregressive part)
%     - **R** [matrix] : m x n state matrix (shock impacts)
%     - **att** [vector] : m x 1 state update
%     - **Ptt** [matrix] : m x m covariance matrix of state update
%     - **MUt** [matrix] : k x l matrix with forward information to match
%     - **OMGt** [matrix|{[]}] : kl x kl covariance matrix of forward
%       information
%     - **DPHI** [matrix] : Matrix representing the restrictions on future
%       shocks
%     - **DT** [matrix] : Convoluted impact of initial conditions
%     - **Record** [] : Holder of invariant information
%     - **ExpandedFlag** [true|false] : if true, returns the expanded state
%       vector including the future shocks. If false, returns only the
%       endogenous variables
%
%  Returns:
%     :
%
%     - **a** [vector] : m x 1 or mm x 1 vector of predictions
%     - **P** [matrix] : m x m or mm x mm covariance of predictions
%     - **Tt** [matrix] : m x m or mm x mm time-varying matrix, modifying the
%       impact of lagged endogenous (T)
%     - **Rt** [] : m x n or mm x nn matrix of impact of shocks
%     - **Record** [] : Holder of invariant information
%
%


function epsilon = CauchyStrain(F)
% Cauchy strain or small strain
%
% Syntax
%   epsilon = CauchyStrain(F)
%
% Input
%  F - @deformationTensor
%
% Output
%  epsilon - @strainTensor
%

epsilon = strainTensor(F.sym - tensor(eye(3),'rank',2));


function [ basis, filt_sig, param ] = gsp_eigenspace_estimation( G, k, param )
%GSP_EIGENSPACE_ESTIMATION Estimation of first eigenvectors of any graph Laplacian
%   Usage:  basis = gsp_eigenspace_estimation(G,k);
%           [basis, approx_U, param] = gsp_eigenspace_estimation(G,k,param);
%
%   Input parameters :
%         G          : Graph structure.
%         k          : Dimension of the subspace.
%         param      : Optional parameters
%   Output parameters:
%         basis      : Approximated basis of k first eigenvectors
%         filt_sig   : Filtered random signal
%         param      : Optional parameters (with new entries)
%
%   'gsp_eigenspace_estimation(G,k)' computes an estimation of the first
%   $k$ eigenvectors of the Laplacian of G using Gaussian random signal
%   filtering, following the FEARS method described in paratte2017fast.
%
%
%   Example:::
%
%         G = gsp_sensor(256);
%         G = gsp_estimate_lmax(G);
%         k = 8;
%         param.order = 100;
%         Uk_est = gsp_eigenspace_estimation(G, k, param);
%         G = gsp_compute_fourier_basis(G);
%         proj_energy = norm(Uk_est' * G.U(:, 1:k), 'fro');
%
%
%   Additional parameters
%   ---------------------
%
%   * *param.filter*  : Select the filter to be used for the computation.
%     * 'lp-ch'   : Chebyshev polynomial approximation
%     * 'lp-jch'  : Jackson-Chebyshev polynomial approximation
%     * 'expwin'  : Exponentially decreasing polynomial approximation Default: 'lp-jch'
%   * *param.order* : Degree of the polynomial approximation (default=50).
%   * *param.lk_est_method* : Select the version of lk estimation.
%     * 'fast'      : Accelerated method using local uniformity assumption
%     * 'std'  : Usual method using dichotomy all the time Default: 'fast'
%   * *param.R* : Random matrix to use (of size N > d, d >= k)
%     (default: Gaussian(0, 1/k) of size Nxk)
%   * *param.pcoef* : Polynomial coefficients if already known.
%   * *param.lk* : Estimated value of lambda_k if already known.
%   * *param.verbose* : Verbosity level (0 no log - 1 display warnings) (default 1).
%
%
%   References: paratte2016fast
%

% Author: Johan Paratte, Lionel Martin
% Date: 3 November 2016

if nargin < 3, param = struct; end
if ~isfield(param, 'filter'), param.filter = 'lp-jch'; end
if ~isfield(param, 'order'), param.order = 50; end
if ~isfield(param, 'lk_est_method'), param.lk_est_method = 'fast'; end
if ~isfield(param, 'R'), param.R = randn(G.N, k)/sqrt(k); end
if ~isfield(param, 'verbose'), param.verbose = 1; end

assert(size(param.R, 1) == G.N && size(param.R, 2) >= k, 'The optional parameter R has wrong size.');

if ~isfield(param, 'pcoefs')
    if param.verbose, disp('Polynomial filtering required. Computing polynomial coefficients...'); end

    if ~isfield(G, 'lmax')
        G = gsp_estimate_lmax(G);
        warning(['GSP_EIGENSPACE_ESTIMATION: The variable lmax is not available.', ...
            'The function will compute it for you. However, if you apply ', ...
            'many time this function, you should precompute it using the ', ...
            'function: gsp_estimate_lmax.']);
    end

    if ~isfield(param, 'lk')
        if param.verbose, fprintf('Estimation of lambda_k');
        end

        tic;
        switch param.lk_est_method
            case 'fast'
                [param.lk, info] = gsp_fast_estimate_lk(G, k, param);
                if param.verbose, disp('using our accelerated method.'); end

            case 'std'
                [~, param.lk, ~, ~, nb_iter_lk, k_est_lk] = gsp_estimate_lk(G, k, param);
                if param.verbose, disp('using the standard method.'); end

            otherwise
                error('Unknown method for lk_est_method.');
        end
        t = toc;

        if param.verbose
            fprintf(['* Estimated lk: %d\n', ...
            '* Time to estimate lk: %f sec\n', ...
            '* in %d iterations with k_est=%d (target=%d)\n'], ...
            param.lk, t, mean(info.calls), mean(info.k_est), k);
        end
    else
        warning('lambda_k was provided to the method from param.');
    end

    tic;
    switch param.filter
        case 'lp-ch'
            [param.pcoefs, ~] = jackson_cheby_poly_coefficients(0, param.lk, [0, G.lmax], param.order);

        case 'lp-jch'
            [~, param.pcoefs] = jackson_cheby_poly_coefficients(0, param.lk, [0, G.lmax], param.order);

        case 'expwin'
            ew = gsp_design_expwin(G, param.lk/G.lmax);
            param.pcoefs = gsp_cheby_coeff(G, ew, param.order);

        otherwise
            error('Unknown filter type!');
    end

    t = toc;
    if param.verbose, fprintf('* Time to compute polynomial coefficients: %f sec.\n', t); end

else
    if param.verbose, warning('pcoef was provided to the method from param.'); end
end

if param.verbose, disp('Filtering random signals...'); end
tic;
filt_sig = gsp_cheby_op(G, param.pcoefs, param.R);
t = toc;
if param.verbose, fprintf('* Time to filter random signals: %f sec.\n', t); end

if param.verbose, disp('Computing the SVD for eigenspace recovery...'); end
tic;
[basis, ~, ~] = svd(filt_sig, 'econ');
t = toc;
if param.verbose, fprintf('* Time to compute SVD: %f sec.\n', t); end

end


function [G, cliques, fill_ins] = triangulate(G, order)
% TRIANGULATE Ensure G is triangulated (chordal), i.e., every cycle of length > 3 has a chord.
% [G, cliques, fill_ins, cliques_containing_node] = triangulate(G, order)
%
% cliques{i} is the i'th maximal complete subgraph of the triangulated graph.
% fill_ins(i,j) = 1 iff we add a fill-in arc between i and j.
%
% To find the maximal cliques, we save each induced cluster (created by adding connecting
% neighbors) that is not a subset of any previously saved cluster. (A cluster is a complete,
% but not necessarily maximal, set of nodes.)

MG = G;
n = length(G);
eliminated = zeros(1,n);
cliques = {};
for i=1:n
  u = order(i);
  U = find(~eliminated); % uneliminated
  nodes = myintersect(neighbors(G,u), U); % look up neighbors in the partially filled-in graph
  nodes = myunion(nodes, u); % the clique will always contain at least u
  G(nodes,nodes) = 1; % make them all connected to each other
  G = setdiag(G,0);
  eliminated(u) = 1;

  exclude = 0;
  for c=1:length(cliques)
    if mysubset(nodes,cliques{c}) % not maximal
      exclude = 1;
      break;
    end
  end
  if ~exclude
    cnum = length(cliques)+1;
    cliques{cnum} = nodes;
  end
end

fill_ins = sparse(triu(max(0, G - MG), 1));

%assert(check_triangulated(G)); % takes 72% of the time!


function [kHz] = Hz2kHz(Hz)
% Convert frequency from hertz to kilohertz.
% Chad A. Greene 2012
kHz = Hz*1e-3;


function r = bisect_characteristic ( x0, theta, characteristic )

%*****************************************************************************80
%
%% BISECT_CHARACTERISTIC: characteristic function transition surface.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 May 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X0(M,1), the coordinates of the base point inside the surface.
%
%    Input, real THETA, the coordinates of a direction from the base point.
%
%    Input, real y = CHARACTERISTIC ( m, n, x ), the handle for a function which
%    evaluates the characteristic for the object at N M-dimensional points X,
%    returning a 1 for points inside the object, and 0 for points outside.
%
%    Output, real R, the distance from X0 to the surface in direction THETA.
%

  x0 = x0(:);

  m = length ( x0 );
  n = 1;
%
%  Initially, X1 = X0, and so Y1 should be 1.
%
  x1(1:m,1) = x0(1:m,1);
  y1 = characteristic ( m, n, x1 );
%
%  Seek an exterior point.
%
  x2 = exterior_point_characteristic ( m, x0, theta, characteristic );
%
%  Carry out the bisection search in the interval [X1,X2].
%
  it_num = 0;

  while ( 1.0E-3 < sqrt ( norm ( x2 - x1 ) ) )

    x3 = ( x1 + x2 ) / 2.0;
    y3 = characteristic ( m, n, x3 );

    if ( y3 == 0 )
      x2 = x3;
    else
      x1 = x3;
    end

    it_num = it_num + 1;

    if ( 1000 < it_num )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'BISECT_CHARACTERISTIC - Fatal error!\n' );
      fprintf ( 1, '  Too many iterations.\n' );
      error ( 'BISECT_CHARACTERISTIC - Fatal error!' )
    end

  end
%
%  Measure the distance from X0 to the transition.
%  We estimate the transition to occur at the average of X1 and X2.
%
  x3 = ( x1 + x2 ) / 2.0;
  r = norm ( x3 - x0 );

  return
end


function [net] = net_init_char_qrnn(opts)

rng('default');
rng(0) ;

f=1/100 ;

n_hidden_nodes=opts.parameters.n_hidden_nodes;
n_input_nodes=opts.parameters.n_input_nodes;
n_output_nodes=opts.parameters.n_output_nodes;
n_gates=opts.parameters.n_gates;

net{1}.type='Gates';% update gates
net{1}.layers = {} ;
net{1}.layers{end+1} = struct('type', 'mlp', ...
                           'weights', {{f*randn(n_gates*n_hidden_nodes,n_input_nodes, 'single'), zeros(n_gates*n_hidden_nodes,1,'single')}}) ;
net{1}.layers{end+1} = struct('type', 'sigmoid') ;

%generate the adjustments of the hidden nodes for the current time frame
net{2}.type='InputTransform';
net{2}.layers = {} ;
net{2}.layers{end+1} = struct('type', 'mlp', ...
                           'weights', {{f*randn(n_hidden_nodes,n_input_nodes, 'single'), zeros(n_hidden_nodes,1,'single')}}) ;
net{2}.layers{end+1} = struct('type', 'relu') ;

net{3}.type='Fit';
net{3}.layers = {};
net{3}.layers{end+1} = struct('type', 'mlp', ...
                           'weights', {{f*randn(n_output_nodes,n_hidden_nodes, 'single'), zeros(n_output_nodes,1,'single')}}) ;
net{3}.layers{end+1} = struct('type', 'softmaxloss');


function [K] = spm_perm_mtx(n)
% Returns a matrix of indices permuted over n
% FORMAT [K] = spm_perm_mtx(n)
%    n   - (scalar) number of indices
%    K   - (2^n x n) permutation matrix
%    n   - (vector) indices
%    K   - (length(n)! x n) permutation matrix
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_perm_mtx.m 5657 2013-09-26 16:53:40Z karl $

% get permutations
%==========================================================================

% permute zeros and ones
%--------------------------------------------------------------------------
if isscalar(n)

    N  = 2^n;
    K  = sparse(N,n);
    x  = sparse(1,1,1,2,1);
    for i = 1:n
        y      = ones(N/length(x),1);
        K(:,i) = kron(x,y);
        x      = [x;x];
    end

% permute indices
%--------------------------------------------------------------------------
elseif isvector(n)

    n  = n(:);
    K  = n;
    while size(K,2) < length(n)
        x     = [];
        for i = 1:size(K,1)
            d = K(i,:);
            r = n;
            for j = 1:length(d)
                r(r == d(j)) = [];
            end
            x = [x; [kron(ones(length(r),1),d) r]];
        end
        K = x;
    end
end

% make logical
%--------------------------------------------------------------------------
K  = logical(K);


% funSimLogNormProbCov returns SINR-based k-coverage probability
% under log-normal shadowing based on repeated simulations of
% of model outlined in [1]
%
% simPCovk=funSimLogNormProbCov(tValues,betaConst,K,lambda,sigma,W,diskRadius,simNumb,k)
% simPCovk is the k-coverage probability
% tValues are the SINR threshold values. tValues can be a vector
% betaConst is the pathloss exponent.
% K = path-loss constant
% lambda = density of base station/nodes of cellular network
% sigma = standard deviation of log-normal shadowing (not in dB)
% W = noise constant
% diskRadius = radius of simulation region (Warning: if too small, edge
% effects will not be sufficiently included and disagreement with analytic
% results may occurr)
% simNumb = number of simulations
% k = coverage number (often set to one)
% All input values (except tValues) are scalars
%
% Author: H.P. Keeler, Inria Paris/ENS, 2013
%
% References
% [1] H.P. Keeler, B. Błaszczyszyn and M. Karray,
% 'SINR-based k-coverage probability in cellular networks with arbitrary
% shadowing', accepted at ISIT, 2013

function simPCovk=funSimLogNormProbCov(tValues,betaConst,K,lambda,sigma,W,diskRadius,simNumb,k)

if nargin==8
    k=1;
end

tNumb=length(tValues);

%%% Simulation Section %%%
%(uniformly) randomly places nodes on a disk of radius diskRadius
diskArea=pi*diskRadius^2;
coveredNumbk=zeros(size(tValues));
ESTwoBeta=exp(sigma^2*(2-betaConst)/betaConst^2);
%rescale lambda - see foot note 5 in [1]
lambdaSim=lambda*ESTwoBeta;
for i=1:simNumb
    randNumb=poissrnd(lambdaSim*diskArea);
    %shadowing distribution can be constant if lambda is rescaled - see [1]
    shadowRand=ones(randNumb,1);
    %random distances from the typical node
    rRand=diskRadius*sqrt(rand(randNumb,1)); %uniform in cartesion, not polar coordinates

    signalRand=shadowRand.*(K*rRand).^(-betaConst);
    interferTotal=sum(signalRand); %total inteference in network
    SINR=signalRand./((interferTotal-signalRand)+W); %calculate SINR for each node in the network

    for j=1:tNumb
        T=tValues(j);
        %counts how many nodes are exactly k or more connected/covered
        if sum(SINR>=T)>=k
            coveredNumbk(j)=coveredNumbk(j)+1;
        end
    end

end

simPCovk=coveredNumbk/simNumb;


function H = higuchi(sequence,isplot)
%
% 'higuchi' estimate the hurst parameter of a given sequence with
%     higuchi's method.
%
% Inputs:
%     sequence: the input sequence for estimate
%     isplot: whether display the plot. without a plot if isplot equal to 0
% Outputs:
%     H: the estimated hurst coeffeient of the input sequence

%  Author: Chu Chen
%  Version 1.0,  03/10/2008
%  chen-chu@163.com
%

if nargin == 1
    isplot = 0;
end

sequence = cumsum(sequence);
N = length(sequence);
mlarge = floor(N/5);
M = [floor(logspace(0,log10(mlarge),50))];
M = unique(M(M>1));
n = length(M);
cut_min = ceil(n/10);
cut_max = floor(6*n/10);

curve_length = zeros(1,n);
for h = 1:n
    m = M(h);
    k = floor((N-m)/m);
    temp_length = zeros(m,k);

    for i = 1:m
        for j = 1:k
            temp_length(i,j) = abs(sequence(i+j*m)-sequence(i+(j-1)*m));
        end
    end

    curve_length(h) = sum(mean(temp_length,2)) * ((N-1)/m^3);
end

x = log(M);
y = log(curve_length);
X = x(cut_min:cut_max);
Y = y(cut_min:cut_max);
p1 = polyfit(X,Y,1);
Yfit = polyval(p1,X);
yfit = polyval(p1,x);
H = 2 + (Yfit(end)-Yfit(1))/(X(end)-X(1));

if isplot ~= 0
    figure,hold on;
    plot(x,y,'b*');
    plot(X,Yfit,'r-','LineWidth',2);
    plot(x(1:cut_min),yfit(1:cut_min),'r:','LineWidth',2);
    plot(x(cut_max:end),yfit(cut_max:end),'r:','LineWidth',2);
    xlabel('Log10(Aggregate Level)'),ylabel('Log10(Curve Legnth)'),title('Higuchi Method');
end


function [g,a]=ar1nv(x)
% AR1NV - Estimate the parameters for an AR(1) model
% Syntax: [g,a]=ar1nv(x);
%
% Input: x - a time series.
%
% Output: g - estimate of the lag-one autocorrelation.
%         a - estimate of the noise variance.

% (c) Eric Breitenberger

x=x(:);
N=length(x);
m=mean(x);
x=x-m;

% Lag zero and one covariance estimates:
c0=x'*x/N;
c1=x(1:N-1)'*x(2:N)/(N-1);

g=c1/c0;
a=sqrt((1-g^2)*c0);


function [phi] = tdoa(arraySignals, fs, d, c)

x1 = arraySignals(:,1);
x2 = arraySignals(:,2);

[acor,lag] = xcorr(x2,x1);
[~,I] = max(abs(acor));
t = lag(I);

phi = acos(t/fs*c/d) * 180 / pi;

end

function [phi] = gcc_phat(arraySpectrum, fs, d, c)

X1 = arraySpectrum(:,1);
X2 = arraySpectrum(:,2);

G = X1 .* conj(X2);
corr = ifft(G ./ abs(G));

[~,I] = max(abs(corr));
tdoa = lag(I);

phi = acos(tdoa/fs*c/d) * 180 / pi;

end


function [prices, alphas] = Heston1993KahlJaeckelLordRev3(PC, S,K,T,t,r,q,v0,theta,rho,kappa,sigma, alphas)
% Heston pricing function based on the implementation suggested by
% Roger Lord and Chrisitan Kahl in "Optimal Fourier inversion in
% semi-analytical option pricing"
%
%
% Input: (PC till q can be vectorized)
%       PC: 1 for Calls, 2 for Puts
%       S: Spot
%       K: Strike
%       T: Maturity
%       t: start date
%       r: interest rate
%       q: dividend
%       v0: initial variance
%       theta: long run mean variance
%       kappa: mean reversion speed of  volatility
%       sigma: volatility of volatility
%       rho: correlation between returns volatility
%       alpha: alpha can be a vector supplied by the user, otherwise the
%       function attempts to find a payoff-dependent optimal alpha
%
%   Output: Price for each option, optionally generated alphas
%
%   Usage: Heston1993KahlJaeckelLordRev3(1, 100, 100, 20,0, 0.05, 0.0,
%           0.00003, 0.00003,-0.3, 0.5, 0.0008)
%
%   Author: Jonathan Frei, 2015
%

    % force column vector
    PC=PC(:);
    S=S(:);
    K=K(:);
    T=T(:);
    t=t(:);
    r=r(:);
    q=q(:);

    nos = numel(S);
    prices=NaN(nos,1);
    tau=T-t;
    mu=(r-q);
    F = S.*exp(mu.*tau);

    if(~exist('alphas','var'))
        alphas = NaN(numel(S),1);
    elseif(numel(alphas)==1)
        alphas = repmat(alphas,numel(S),1);
    end

    alpha0=0.75;

    for(ind=1:nos)
        if(isnan(alphas(ind)))
            try
                % using fzero here instead of fminsearch
               alphas(ind) = fzero( @(a) psi(a,K(ind), F(ind), kappa, theta, rho, sigma, tau(ind), v0), alpha0);
            catch
               alphas(ind) = alpha0;
            end
        end
        prices(ind) =  Ralpha(F(ind), K(ind), alphas(ind))+1/pi*integral(@(x) phi(x, K(ind), alphas(ind), F(ind), kappa, theta, rho, sigma, tau(ind), v0) , 0, Inf);
        if (PC(ind)==2)
            prices(ind) = prices(ind) + K(ind)*exp(-r(ind)*tau(ind))-S(ind)*exp(-q(ind)*tau(ind));
        end
    end

end

function p = psi(alpha, K, F, kappa, theta, rho, sigma, tau, v0)
    k = log(K);
    p = -alpha*k+0.5*log(phi(-(alpha+1)*1i, K, alpha, F, kappa, theta, rho, sigma, tau, v0)^2);
end

function r = Ralpha(F, K, alpha)
    r = F*(alpha<=0)-K*(alpha<=-1)-0.5*(F*(alpha==0)-K*(alpha==-1));
end

function y = phi(v, K, alpha, F, kappa, theta, rho, sigma, tau, v0)
    k = log(K);
    y = real(exp(-1i*(v-1i*alpha)*k).*( cf(v-1i*(alpha+1), F, kappa, theta, rho, sigma, tau, v0)./(-(v-1i*(alpha+1)).*(v-1i*alpha))));
end

function c = cf(u, F, kappa, theta, rho, sigma, tau, v0)
    f = log(F);
    c = exp(1i*u*f+ A(u, kappa, theta, rho, sigma, tau)+Bv(u, rho, sigma, kappa, tau)*v0);
end

function b = Bv(u, rho, sigma, kappa, tau)
    b = ((beta(u,rho,sigma,kappa)-D(u, rho, sigma, kappa)).*(1-exp(-D(u, rho, sigma, kappa)*tau)))./(sigma.^2*(1-G(u, rho, sigma, kappa).*exp(-D(u, rho, sigma, kappa)*tau)));
end

function a = A(u, kappa, theta, rho, sigma, tau)
    a = (kappa*theta*((beta(u,rho,sigma,kappa)-D(u, rho, sigma, kappa))*tau-2*log(phi2(u, rho, sigma, kappa, tau))))/sigma.^2;
end

function p = phi2(u, rho, sigma, kappa, tau)
    p = (G(u, rho, sigma, kappa).*exp(-D(u, rho, sigma, kappa)*tau)-1)./(G(u, rho, sigma, kappa)-1);
end

function g = G(u, rho, sigma, kappa)
    g = (beta(u,rho,sigma,kappa)-D(u, rho, sigma, kappa))./(beta(u,rho,sigma,kappa)+D(u, rho, sigma, kappa));
end

function d = D(u, rho, sigma, kappa)
    d = sqrt(beta(u,rho,sigma,kappa).^2-4*alphahat(u)*gamma(sigma));
end

function a = alphahat(u)
    a = -0.5*u.*(1i+u);
end

function b = beta(u,rho,sigma,kappa)
    b = kappa-rho*sigma*u*1i;
end

function y = gamma(sigma)
    y = 0.5*sigma.^2;
end


%  Figure 10.67      Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%
% fig10_67.m is a script to generate Fig. 10.67 the linear RTP response
% PI controller
% RTP chamber demo Example
clf;
a=[-0.068208813989939 0.014929776357245 0.000000065782442;...
   0.045809672136430 -0.118134773528570 0.021802006306129;...
   0.000000433637498 0.046839194867347 -0.100884399149872];
b3=[0.37873508304055 0.110575403544586 0.022912893467038;...
   0.000000002974222 0.449046982554739 0.073572900808161;...
   0.000000027324116 0.000702342292121 0.417797228783230];
c3=eye(3,3);
d3=0*eye(3,3);

% Combine 3 lamps into a single actuator
b=b3(:,1)+b3(:,2)+b3(:,3);
% Select center temperature
c=c3(2,:)
%
d=[0];
% PI controller
ac=[0];
bc=[1.0];
cc=[0.0527];
dc=[1];
sysG=ss(a,b,c,d);
sysD=ss(ac,bc,cc,dc);
sysL=series(sysD,sysG);
sysH=tf(1,1);
sysCL=feedback(sysL,sysH);

%CL Step Response
t=0:.1:100;
R=[0:.1:25, 25*ones(1,500), 0*ones(1,250)];
R11=[R'];
[yy,t]=lsim(sysCL,R11,t);
plot(t,yy,'--');
grid;
hold on;
plot(t,R11,'-');
xlabel('Time (sec)');
ylabel('Temperature (K)');
title('Fig. 10.67 (a) PI controller: temperature tracking response');
%legend('y')
pause
hold off
% Control effort
sysCLu=feedback(sysD,sysG);
[uuu,t]=lsim(sysCLu,R11,t);
plot(t(1:753,:),uuu(1:753,:));
hold on;
plot(t(752:818,:),0*ones(67,3));
hold on;
plot(t(753:818,:),uuu(753:818,:),'--');
hold on;
plot(t(819:1001,:),uuu(819:1001,:));
xlabel('Time (sec)');
ylabel('Lamp voltage');
title('Fig. 10.67 (b) PI controller :control effort');
legend('u')
grid;
hold off;


function box_display_test01 ( )

%*****************************************************************************80
%
%% BOX_DISPLAY_TEST01 plots total degree index sets.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 March 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BOX_DISPLAY_TEST01:\n' );
  fprintf ( 1, '  Plot total degree index sets.\n' );
  fprintf ( 1, '\n' );

  m = 12;
  n = 12;
  title_string = 'Total Degree <= 1';
  box_display ( m, n, @(x,y)x+y<=0, @(x,y)x+y<=1, title_string );
  print ( '-dpng', 'td1.png' );
  fprintf ( 1, '  Created "td1.png".\n' );

  m = 12;
  n = 12;
  title_string = 'Total Degree <= 2';
  box_display ( m, n, @(x,y)x+y<=1, @(x,y)x+y<=2, title_string );
  print ( '-dpng', 'td2.png' );
  fprintf ( 1, '  Created "td2.png".\n' );

  m = 12;
  n = 12;
  title_string = 'Total Degree <= 3';
  box_display ( m, n, @(x,y)x+y<=2, @(x,y)x+y<=3, title_string );
  print ( '-dpng', 'td3.png' );
  fprintf ( 1, '  Created "td3.png".\n' );

  m = 12;
  n = 12;
  title_string = 'Total Degree <= 4';
  box_display ( m, n, @(x,y)x+y<=3, @(x,y)x+y<=4, title_string );
  print ( '-dpng', 'td4.png' );
  fprintf ( 1, '  Created "td4.png".\n' );

  m = 12;
  n = 12;
  title_string = 'Total Degree <= 5';
  box_display ( m, n, @(x,y)x+y<=4, @(x,y)x+y<=5, title_string );
  print ( '-dpng', 'td5.png' );
  fprintf ( 1, '  Created "td5.png".\n' );

  m = 12;
  n = 12;
  title_string = 'Total Degree <= 6';
  box_display ( m, n, @(x,y)x+y<=5, @(x,y)x+y<=6, title_string );
  print ( '-dpng', 'td6.png' );
  fprintf ( 1, '  Created "td6.png".\n' );

  m = 12;
  n = 12;
  title_string = 'Total Degree <= 7';
  box_display ( m, n, @(x,y)x+y<=6, @(x,y)x+y<=7, title_string );
  print ( '-dpng', 'td7.png' );
  fprintf ( 1, '  Created "td7.png".\n' );

  m = 12;
  n = 12;
  title_string = 'Total Degree <= 8';
  box_display ( m, n, @(x,y)x+y<=7, @(x,y)x+y<=8, title_string );
  print ( '-dpng', 'td8.png' );
  fprintf ( 1, '  Created "td8.png".\n' );

  m = 12;
  n = 12;
  title_string = 'Total Degree <= 9';
  box_display ( m, n, @(x,y)x+y<=8, @(x,y)x+y<=9, title_string );
  print ( '-dpng', 'td9.png' );
  fprintf ( 1, '  Created "td9.png".\n' );

  return
end


function value = r4_tan ( x )

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

  pi2rec = 0.0116197723675813430;

  if ( isempty ( nterms ) )
    tancs = [ ...
      0.226279327631293578, ...
      0.0430179131465489618, ...
      0.0006854461068256508, ...
      0.0000110453269475970, ...
      0.0000001781747790392, ...
      0.0000000028744968582, ...
      0.0000000000463748541, ...
      0.0000000000007481760, ...
      0.0000000000000120704, ...
      0.0000000000000001947, ...
      0.0000000000000000031 ]';
    nterms = r4_inits ( tancs, 11, 0.1 * r4_mach ( 3 ) );
    xmax = 1.0 / r4_mach ( 4 );
    xsml = sqrt ( 3.0 * r4_mach ( 3 ) );
    sqeps = sqrt ( r4_mach ( 4 ) );
  end

  y = abs ( x );

  if ( xmax < y )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_TAN - Warning!\n' );
    fprintf ( 1, '  No precision because |X| is big.\n' );
    value = 0.0;
    return
  end
%
%  Carefully compute y * (2/pi) = (aint(y) + rem(y)) * (.625 + pi2rec)
%  = aint(.625*y) + rem(.625*y) + y*pi2rec  =  aint(.625*y) + z
%  = aint(.625*y) + aint(z) + rem(z)
%
  ainty = floor ( y );
  yrem = y - ainty;
  prodbg = 0.625 * ainty;
  ainty = floor ( prodbg );
  y = ( prodbg - ainty ) + 0.625 * yrem + y * pi2rec;
  ainty2 = floor ( y );
  ainty = ainty + ainty2;
  y = y - ainty2;

  ifn = floor ( mod ( ainty, 2.0 ) );

  if ( ifn == 1 )
    y = 1.0 - y;
  end

  if ( 1.0 - y < abs ( x ) * sqeps )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_TAN - Warning!\n' );
    fprintf ( 1, '  Answer < half precision.\n' );
    fprintf ( 1, '  |X| big or X near pi/2 or 3*pi/2.\n' );
  end

  if ( y == 1.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_TAN - Fatal error!\n' );
    fprintf ( 1, '  X is pi/2 or 3*pi/2.\n' );
    error ( 'R4_TAN - Fatal error!' )
  end

  if ( y <= 0.25 )

    value = y;
    if ( xsml < y )
      value = y * ( 1.5 ...
        + r4_csevl ( 32.0 * y * y - 1.0, tancs, nterms ) );
    end

  elseif ( y <= 0.5 )

    value = 0.5 * y * ( 1.5 ...
      + r4_csevl ( 8.0 * y * y - 1.0, tancs, nterms ) );

    value = 2.0 * value / ( 1.0 - value * value );

  else

    value = 0.25 * y * ( 1.5 ...
      + r4_csevl ( 2.0 * y * y - 1.0, tancs, nterms ) );
    value = 2.0 * value / ( 1.0 - value * value );
    value = 2.0 * value / ( 1.0 - value * value );

  end

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

  if ( ifn == 1 )
    value = - value;
  end

  return
end


function b = r83t_mv ( m, n, a, x )

%*****************************************************************************80
%
%% R83T_MV multiplies an R83T matrix times an R8VEC.
%
%  Discussion:
%
%    The R83T storage format is used for a tridiagonal matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, real A(M,3), the matrix.
%
%    Input, real X(N), the vector to be multiplied by A.
%
%    Output, real B(M), the product A * x.
%
  b = zeros ( m, 1 );
  x = x(:);

  mn = min ( m, n );

  if ( n == 1 )
    b(1) = a(1,2) * x(1);
    if ( 1 < m )
      b(2) = a(2,1) * x(1);
    end
    return
  end

  b(1)      = a(1,2)       * x(1) ...
            + a(1,3)       * x(2);

  b(2:mn-1) = a(2:mn-1,1) .* x(1:mn-2) ...
            + a(2:mn-1,2) .* x(2:mn-1) ...
            + a(2:mn-1,3) .* x(3:mn);

  b(mn)     = a(mn,1)      * x(mn-1) ...
            + a(mn,2)      * x(mn);

  if ( n < m )
    b(mn+1) = b(mn+1) + a(mn+1,1) * x(mn);
  elseif ( m < n )
    b(mn) = b(mn) + a(mn,3) * x(mn+1);
  end

  return
end


clear all, close all, clc

% J = @(u,t)(25-(5-(u-t))^2);

J = @(u,t)(25-(5-(u)-sin(t)).^2);
u = 0;
y0 = J(u,0);

% Extremum Seeking Control Parameters
freq = 100; % sample frequency
dt = 1/freq;
T = 100; % total period of simulation (in seconds)
A = .2;  % amplitude
omega = 10*2*pi; % 10 Hz
phase = 0;
K = 5;   % integration gain

% high pass filter
butterorder=1;
butterfreq=2;  % in Hz for 'high'
[b,a] = butter(butterorder,butterfreq*dt*2,'high')
ys = zeros(1,butterorder+1)+y0;
HPF=zeros(1,butterorder+1);

uhat=u;
for i=1:T/dt
    t = (i-1)*dt;
    time(i) = t;
    yvals(i)=J(u,t);

    for k=1:butterorder
        ys(k) = ys(k+1);
        HPF(k) = HPF(k+1);
    end
    ys(butterorder+1) = yvals(i);
    HPFnew = 0;
    for k=1:butterorder+1
        HPFnew = HPFnew + b(k)*ys(butterorder+2-k);
    end
    for k=2:butterorder+1
        HPFnew = HPFnew - a(k)*HPF(butterorder+2-k);
    end
    HPF(butterorder+1) = HPFnew;

    xi = HPFnew*sin(omega*t + phase);
    uhat = uhat + xi*K*dt;
    u = uhat + A*sin(omega*t + phase);
    uhats(i) = uhat;
    uvals(i) = u;
end

%%
figure
subplot(2,1,1)
plot(time,uvals,time,uhats,'LineWidth',1.2)
l1=legend('$u$','$\hat{u}$')
set(l1,'interpreter','latex','Location','SouthEast')
grid on
subplot(2,1,2)
plot(time,yvals,'LineWidth',1.2)
ylim([-1 26])
grid on

set(gcf,'Position',[100 100 500 350])
set(gcf,'PaperPositionMode','auto')
print('-depsc2', '-loose', '../../../figures/ESC_ResponseVarying');


% test of hufmann coding by concatening several token

% probability of having 0
t = .12;
n = 4096*2;
x = (rand(n,1)>t)+1;

% entropy lower bound
p = [t 1-t];
e =  -sum(p.*log2(p));

% create a new vector by lifting
q = 3;
n1 = ceil(n/q)*q;
x1 = x;
x1(end+1:n1) = 1;
x1 = reshape(x1,[q n1/q]);
[Y,X] = meshgrid(1:n1/q,0:q-1);
x1 = sum( (x1-1) .* (2.^X), 1 )' + 1;

% generate probability table
P = p(:); p = p(:);
for i=1:q-1
    Pold = P;
    P = [];
    for i=1:length(p)
        P = [P; Pold*p(i)];
    end
end

% compute the tree
T = compute_hufftree(P);
% do the coding
y = perform_huffcoding(x1,T,+1);
% average number of bits
e1 = length(y)/length(x);

disp(['Entropy=' num2str(e) ', Huffman(block size ' num2str(q) ')=' num2str(e1)]);


function f = tone2freq(T)
% MUSIC.TONE2FREQ converts a musical semitone to a frequency.
%    F = MUSIC.TONE2FREQ(T) converts the musical semitones in T to frequencies.
%
%    Example
%       f = music.tone2freq(0:2);  % returns [261.63  277.19  293.67]
%
%    See also music.tone2interval, music.tone2note, music.freq2tone.

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

fC4 = 261.625565300599;  % Middle C (C4) is 261.63 Hz

f = fC4 .* 2 .^ (T / 12);


function [W,H] = nmf_cjlin(V,Winit,Hinit,tol,timelimit,maxiter)
%
% Copyright (c) 2005-2006 Chih-Jen Lin
% All rights reserved.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions
% are met:
%
% 1. Redistributions of source code must retain the above copyright
% notice, this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright
% notice, this list of conditions and the following disclaimer in the
% documentation and/or other materials provided with the distribution.
%
% 3. Neither name of copyright holders nor the names of its contributors
% may be used to endorse or promote products derived from this software
% without specific prior written permission.
%
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
% ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
% LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
% A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR
% CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
% EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
% PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
% PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
% LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
% NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
% SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
%
%
% NMF by alternative non-negative least squares using projected gradients
% Author: Chih-Jen Lin, National Taiwan University
%
% W,H: output solution
% Winit,Hinit: initial solution
% tol: tolerance for a relative stopping condition
% timelimit, maxiter: limit of time and iterations

W = Winit; H = Hinit; initt = cputime;

gradW = W*(H*H') - V*H'; gradH = (W'*W)*H - W'*V;
initgrad = norm([gradW; gradH'],'fro');
fprintf('Init gradient norm %f\n', initgrad);
tolW = max(0.001,tol)*initgrad; tolH = tolW;

for iter=1:maxiter,
  % stopping condition
  projnorm = norm([gradW(gradW<0 | W>0); gradH(gradH<0 | H>0)]);
  if projnorm < tol*initgrad | cputime-initt > timelimit,
    break;
  end

  [W,gradW,iterW] = nlssubprob(V',H',W',tolW,1000); W = W'; gradW = gradW';
  if iterW==1,
    tolW = 0.1 * tolW;
  end

  [H,gradH,iterH] = nlssubprob(V,W,H,tolH,1000);
  if iterH==1,
    tolH = 0.1 * tolH;
  end

  if (iterW==1 & iterH==1 & tolH + tolW < tol*initgrad),
    fprintf('Failed to move\n'); break;
  end
  if rem(iter,10)==0, fprintf('.'); end
end
fprintf('\nIter = %d Final proj-grad norm %f\n', iter, projnorm);

function [H,grad,iter] = nlssubprob(V,W,Hinit,tol,maxiter)
% H, grad: output solution and gradient
% iter: #iterations used
% V, W: constant matrices
% Hinit: initial solution
% tol: stopping tolerance
% maxiter: limit of iterations

H = Hinit;
WtV = W'*V;
WtW = W'*W;

alpha = 1; beta = 0.1;
for iter=1:maxiter,
  grad = WtW*H - WtV;
  projgrad = norm(grad(grad < 0 | H >0));
  if projgrad < tol,
    break
  end

  % search step size
  Hn = max(H - alpha*grad, 0); d = Hn-H;
  gradd=sum(sum(grad.*d)); dQd = sum(sum((WtW*d).*d));
  if gradd + 0.5*dQd > 0.01*gradd,
    % decrease alpha
    while 1,
      alpha = alpha*beta;
      Hn = max(H - alpha*grad, 0); d = Hn-H;
      gradd=sum(sum(grad.*d)); dQd = sum(sum((WtW*d).*d));
      if gradd + 0.5*dQd <= 0.01*gradd | alpha < 1e-20,
        H = Hn; break;
      end
    end
  else
    % increase alpha
    while 1,
      Hp = Hn;
      alpha = alpha/beta;
      Hn = max(H - alpha*grad, 0); d = Hn-H;
      gradd=sum(sum(grad.*d)); dQd = sum(sum((WtW*d).*d));
      if gradd + 0.5*dQd > 0.01*gradd | Hn == Hp | alpha > 1e10,
        H = Hp; alpha = alpha*beta; break;
      end
    end
  end
end

if iter==maxiter,
  fprintf('Max iter in nlssubprob\n');
end


function Z=SoftThreshold_GS( A, thres )

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Soft-thresholding for group lasso
%
% Reference:
% Yuan, Ming, and Yi Lin.
% "Model selection and estimation in regression with grouped variables."
% Journal of the Royal Statistical Society: Series B
% (Statistical Methodology) 68.1 (2006): 49-67.
%
% Provider:
% Hongteng Xu @ Georgia Tech
% June 12, 2017
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Z = zeros(size(A));
for u = 1:size(A, 3)
    for v = 1:size(A, 1)
        tmp = 1 - thres/norm(A(v,:,u));
        if tmp>0
            Z(v,:,u) = tmp*A(v,:,u);
        end
    end
end


function [d pred] = shortest_paths(A,u,varargin)
% SHORTEST_PATHS Compute the weighted single source shortest path problem.
%
% [d pred] = shortest_paths(A,u) returns the distance (d) and the predecessor
% (pred) for each of the vertices along the shortest path from u to every
% other vertex in the graph.
%
% ... = shortest_paths(A,u,...) takes a set of
% key-value pairs or an options structure.  See set_matlab_bgl_options
% for the standard options.
%   options.algname: the algorithm to use
%       [{'auto'} | 'dijkstra' | 'bellman_ford' | 'dag']
%   options.inf: the value to use for unreachable vertices
%       [double > 0 | {Inf}]
%   options.target: a special vertex that will stop the search when hit
%       [{'none'} | any vertex number besides the u]; target is ignored if
%       visitor is set.
%   options.visitor: a structure with visitor callbacks.  This option only
%       applies to dijkstra or bellman_ford algorithms.  See dijkstra_sp or
%       bellman_ford_sp for details on the visitors.
%   options.edge_weight: a double array over the edges with an edge
%       weight for each edge, see EDGE_INDEX and EXAMPLES/REWEIGHTED_GRAPHS
%       for information on how to use this option correctly
%       [{'matrix'} | length(nnz(A)) double vector]
%
% Note: if you need to compute shortest paths with 0 weight edges, you must
% use an edge_weight vector, see the examples for details.
%
% Note: 'auto' cannot be used with 'nocheck' = 1.  The 'auto' algorithm
% checks if the graph has negative edges and uses bellman_ford in that
% case, otherwise, it uses 'dijkstra'.  In the future, it may check if the
% graph is a dag and use 'dag'.
%
% Example:
%    load graphs/clr-25-2.mat
%    shortest_paths(A,1)
%    shortest_paths(A,1,struct('algname','bellman_ford'))
%
% See also DIJKSTRA_SP, BELLMAN_FORD_SP, DAG_SP

% David Gleich
% Copyright, Stanford University, 2006-2008

%% History
%  2006-04-19: Initial coding
%  2007-04-18: Added edge_weight option.
%  2007-04-19: Added target option.
%    Added additional error checks.
%  2007-07-12: Fixed edge_weight documentation
%%

[trans check full2sparse] = get_matlab_bgl_options(varargin{:});
if full2sparse && ~issparse(A), A = sparse(A); end

options = struct('algname', 'auto', 'inf', Inf, 'edge_weight', 'matrix', ...
    'target', 'none');
options = merge_options(options,varargin{:});

% edge_weights is an indicator that is 1 if we are using edge_weights
% passed on the command line or 0 if we are using the matrix.
edge_weights = 0;
edge_weight_opt = 'matrix';

if strcmp(options.edge_weight, 'matrix')
    % do nothing if we are using the matrix weights
else
    edge_weights = 1;
    edge_weight_opt = options.edge_weight;
end

if strcmp(options.target,'none')
    target = 0; % a flag used to denote "no target" to the mex
elseif isa(options.target, 'double')
    target = options.target;
else
    error('matlab_bgl:invalidParameter', ...
        'options.target is not ''none'' or a vertex number.');
end

if check
    % check the values of the matrix
    check_matlab_bgl(A,struct('values',edge_weights ~= 1));

    if edge_weights && nnz(A) ~= length(edge_weight_opt)
        error('matlab_bgl:invalidParameter', 'the vector of edge weights must have length nnz(A)');
    end

    % set the algname
    if (strcmpi(options.algname, 'auto'))
        if edge_weights
            mv = min(edge_weights);
        else
            mv = min(min(A));
        end

        if (mv < 0)
            options.algname = 'bellman_ford';
        else
            options.algname = 'dijkstra';
        end
    else
        % check the data provided to match the algorithm
        if strcmpi(options.algname, 'dijkstra')
            if edge_weights
                mv = min(edge_weight_opt);
            else
                mv = min(min(A));
            end
            if mv < 0
                error('matlab_bgl:invalidParameter', ...
                    'dijkstra''s algorithm cannot be used with negative edge weights.');
            end
        end
    end

else
    if (strcmpi(options.algname, 'auto'))
        error('shortest_paths:invalidParameter', ...
            'algname auto is not compatible with no check');
    end
end

if options.inf < 0, error('options.inf must be larger than 0'); end

if trans, A = A'; end

if isfield(options,'visitor')
    [d pred] = matlab_bgl_sp_mex(A,u,target,lower(options.algname),options.inf,...
        edge_weight_opt, options.visitor);
else
    [d pred] = matlab_bgl_sp_mex(A,u,target,lower(options.algname),options.inf,...
        edge_weight_opt);
end


function p = mahdist(x, m, S, V)
% mahalanobis distance from x to m by S
% mahdist([x(:)';y(:)';z(:)'],mu,[],S)
%   See NORMPDF for argument description.
% [d, n] = size(x)
% [d, 1] = size(m)
% S = []
% V = cov
[d, n] = size(x);
if nargin == 1
  dx = x;
elseif isempty(m)
  dx = x;
else
  % m specified
  sz = size(m);
  if sz(1) ~= d
    error('rows(m) ~= rows(x)')
  end
  nm = sz(2);
  if nm == 1
    dx = x - repmat(m,1,n);
  elseif n == 1
    dx = repmat(x,1,nm) - m;
  elseif nm == n
    dx = x - m;
  else
    error('incompatible number of columns in x and m')
  end
end
if nargin < 3
  % unit variance
  p = col_sum(dx.*dx);
  return
end
have_inv = 0;
if nargin == 3
  % standard deviation given
  if d == 1
    dx = dx./S;
    p = dx.*dx;
    return;
  end
  if S(2,1) ~= 0
    error('S is not upper triangular')
  end
  if any(size(S) ~= [d d])
    error('S is not the right size')
  end
else
  if ischar(V)
    if strcmp(V,'inv')
      % inverse stddev given
      iS = S;
      have_inv = 1;
    else
      error('unknown directive')
    end
  elseif ischar(S)
    if strcmp(S,'inv')
      % inverse variance given
      if d == 1
	iS = sqrt(V);
      else
	iS = chol(V);
      end
      have_inv = 1;
    else
      error('unknown directive')
    end
  else
    % variance given
    if d == 1
      S = sqrt(V);
    else
      S = chol(V);
    end
  end
end
if have_inv
  if d == 1
    dx = iS .* dx;
  else
    dx = iS*dx;
  end
else
  if d == 1
    dx = dx./S;
  else
    dx = solve_tril(S',dx);
  end
end
p = sum(dx.*dx,1);


function ttf = sin_test()

d = 10;
r = 2;
n = 2;
N = 20000;
x = 2*pi*rand(d,N);
y = sin(sum(x));
tt_rank = [1 ; r*ones(d-1,1) ; 1];
n_basis = n*ones(d,1);
basis_ps = cumsum([1 ; n_basis*N]);
basis_cr = zeros(basis_ps(d+1) - basis_ps(1), 1);
for dim = 1: d
    t = zeros(n, N);
    t(1,:) = sin(x(dim,:));
    t(2,:) = cos(x(dim,:));
	basis_cr(basis_ps(dim):basis_ps(dim+1)-1) = reshape(t, [n*N 1]);
end
coeff = reg_als(basis_cr, y, tt_rank, n_basis);
ttf = tt_function(@fun, coeff);

end

function fx = fun(~, j, x)
% i-th dimension, j-th basis function at x

if j == 1
    fx = sin(x);
elseif j == 2
    fx = cos(x);
end

end


function Kc = kernelCenter(K)

% KERNELCENTER Attempts to Center Kernel Matrix
% FORMAT
% DESC returns a centered kernel matrix
% ARG kernel matrix
% RETURN Kc : The centered kernel
%
% SEEALSO :
%
% COPYRIGHT : Carl Henrik Ek, 2008

% KERN

if(nargin<1)
  error('To Few Arguments');
end
if(size(K,1)~=size(K,2))
  error('Kernel Not Square');
end

Kc = (eye(size(K)) - 1/size(K,1)*ones(size(K)))*K*(eye(size(K))-1/size(K,1)*ones(size(K)));

return;


function [convRVTOut, rvtOut, verbose] = tapas_physio_create_rvt_regressors(...
    ons_secs, sqpar, model_rvt, verbose)
% computes respiratory response function regressor and respiratory volume per time
%
%    [convRVTOut, rvtOut, verbose] = tapas_physio_create_rvt_regressors(...
%                                   ons_secs, sqpar, model_rvt, verbose)
% References:
%
%   Birn, R.M., Smith, M.A., Jones, T.B., Bandettini, P.A., 2008.
%       The respiration response function: The temporal dynamics of
%       fMRI signal fluctuations related to changes in respiration.
%       NeuroImage 40, 644-654.
%
%   Harrison, S.J., Bianchi, S., Heinzle, J., Stephan, K.E., Iglesias, S.,
%   Kasper L., 2021.
%   A Hilbert-based method for processing respiratory timeseries.
%   NeuroImage, 117787. https://doi.org/10.1016/j.neuroimage.2021.117787

% IN
%   ons_secs.
%       ons_secs            ons_secs structure with variable `fr`
%                           (filtered respiratory signal time series)
%       sqpar               scan timing information (sequence parameters)
%                           slice onsets etc.
%       model_rvt           rvt modeling parameter structure. e.g.
%                           model_rvt.method
%                           'hilbert' (default, [Harrison2021]) or
%                           'peaks' [Birn2006]
%
% OUT
%   convRVTOut          [nScans, nDelays, nSampleSlices]
%                       respiratory response function regressor after
%                       convolution for specified delays and downsampled
%                       to given slices.
% EXAMPLE
%   [convHRV, hr] = tapas_physio_create_hrv_regressor(physio_out.ons_secs, physio_out.sqpar);
%
%   See also tapas_physio_rvt_hilbert tapas_physio_rvt_peaks tapas_physio_rrf

% Author: Lars Kasper
% Created: 2014-01-20
% Copyright (C) 2014 TNU, Institute for Biomedical Engineering,
%               University of Zurich and ETH Zurich.
%
% This file is part of the physIO toolbox, which is released under the
% terms of the GNU General Public Licence (GPL), version 3. You can
% redistribute it and/or modify it under the terms of the GPL (either
% version 3 or, at your option, any later version). For further details,
% see the file COPYING or <http://www.gnu.org/licenses/>.

if nargin < 3
    physio = tapas_physio_new;
    model_rvt = physio.model.rvt;
end

delays = model_rvt.delays;

if nargin < 4
    verbose.level = [];
    verbose.fig_handles = [];
end

slicenum = 1:sqpar.Nslices;

% Calculate RVT
sample_points  = tapas_physio_get_sample_points(ons_secs, sqpar, slicenum);
switch lower(model_rvt.method)
    case 'peaks'
        [rvt, ~, ~, verbose] = tapas_physio_rvt_peaks(ons_secs.fr, ons_secs.t, sample_points, verbose);
    case 'hilbert'
        [rvt, verbose] = tapas_physio_rvt_hilbert(ons_secs.fr, ons_secs.t, sample_points, verbose);
    otherwise
        error('Unrecognised value for ''rvt.method'' (%s)!', model_rvt.method)
end
rvt = rvt / max(abs(rvt)); % normalize for reasonable range of regressor

if verbose.level >=2
    verbose.fig_handles(end+1) = tapas_physio_get_default_fig_params();
    set(gcf, 'Name', 'Model: Convolution Respiration RVT X RRF');
    subplot(2,2,1)
    plot(sample_points,rvt, 'g');xlabel('time (seconds)');
    title('Respiratory volume per time');
    ylabel('a.u.');
end

% Generate RRF
dt = sqpar.TR / sqpar.Nslices;
t = 0:dt:60;  % seconds
rrf = tapas_physio_rrf(t);
rrf = rrf / max(abs(rrf));

if verbose.level >= 2
    subplot(2,2,2)
    plot(t, rrf,'g'); xlabel('time (seconds)');
    title('Respiratory response function');
end

% Convolve and rescale for display purposes
convRVT = tapas_physio_conv(rvt, rrf, 'causal');
convRVT = convRVT / max(abs(convRVT));

if verbose.level >= 2
    subplot(2,2,3)
    plot(sample_points, convRVT,'g');xlabel('time (seconds)');
    title('Resp vol time X resp response function');
end

% Create shifted regressors convolved time series, which is equivalent to
% delayed response functions according to Wikipedia (convolution)
%
% "Translation invariance[edit]
% The convolution commutes with translations, meaning that
%
% \tau_x ({f}*g) = (\tau_x f)*g = {f}*(\tau_x g)\,
% where \tau_x is the translation of the function f by x defined by
% (\tau_x f)(y) = f(y-x).

% remove mean and linear trend to fulfill periodicity condition for
% shifting
convRVT = detrend(convRVT);

% TODO: what happens at the end/beginning of shifted convolutions?
nDelays = numel(delays);
nShiftSamples = ceil(delays/dt);

% resample to slices needed
nSampleSlices = numel(sqpar.onset_slice);
nScans = numel(sample_points(sqpar.onset_slice:sqpar.Nslices:end));

rvtOut = zeros(nScans,nSampleSlices);
convRVTOut = zeros(nScans,nDelays,nSampleSlices);
samplePointsOut = zeros(nScans,nSampleSlices);

for iDelay = 1:nDelays
    convRVTShifted = circshift(convRVT, nShiftSamples(iDelay));
    for iSlice = 1:nSampleSlices
        onset_slice = sqpar.onset_slice(iSlice);
        rvtOut(:,iSlice) = rvt(onset_slice:sqpar.Nslices:end)';
        convRVTOut(:,iDelay,iSlice) = convRVTShifted(onset_slice:sqpar.Nslices:end);
        samplePointsOut(:,iSlice) = sample_points(onset_slice:sqpar.Nslices:end);
    end
end

if verbose.level >= 2
    subplot(2,2,4)
    [tmp, iShiftMin] = min(abs(delays));
    hp{1} = plot(samplePointsOut, rvtOut,'k--');hold all;
    hp{2} = plot(samplePointsOut, squeeze(convRVTOut(:,iShiftMin,:)),'g');
    xlabel('time (seconds)');
    title('RVT regessor');
    legend([hp{1}(1), hp{2}(1)], 'respiratory volume / time (a. u.)', ...
        'respiratory response regressor');
end

end


function d=disteusq(x,y,mode,w)
%DISTEUSQ calculate euclidean, squared euclidean or mahanalobis distance D=(X,Y,MODE,W)
%
% Inputs: X,Y         Vector sets to be compared. Each row contains a data vector.
%                     X and Y must have the same number of columns.
%
%         MODE        Character string selecting the following options:
%                         'x'  Calculate the full distance matrix from every row of X to every row of Y
%                         'd'  Calculate only the distance between corresponding rows of X and Y
%                              The default is 'd' if X and Y have the same number of rows otherwise 'x'.
%                         's'  take the square-root of the result to give the euclidean distance.
%
%         W           Optional weighting matrix: the distance calculated is (x-y)*W*(x-y)'
%                     If W is a vector, then the matrix diag(W) is used.
%
% Output: D           If MODE='d' then D is a column vector with the same number of rows as the shorter of X and Y.
%                     If MODE='x' then D is a matrix with the same number of rows as X and the same number of columns as Y'.
%

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: disteusq.m 713 2011-10-16 14:45:43Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[nx,p]=size(x); ny=size(y,1);
if nargin<3 | isempty(mode) mode='0'; end
if any(mode=='d') | (mode~='x' & nx==ny)

    % Do pairwise distance calculation

    nx=min(nx,ny);
    z=double(x(1:nx,:))-double(y(1:nx,:));
    if nargin<4
        d=sum(z.*conj(z),2);
    elseif min(size(w))==1
        wv=w(:).';
        d=sum(z.*wv(ones(size(z,1),1),:).*conj(z),2);
    else
        d=sum(z*w.*conj(z),2);
    end
else

    % Calculate full distance matrix

    if p>1

        % x and y are matrices

        if nargin<4
            z=permute(double(x(:,:,ones(1,ny))),[1 3 2])-permute(double(y(:,:,ones(1,nx))),[3 1 2]);
            d=sum(z.*conj(z),3);
        else
            nxy=nx*ny;
            z=reshape(permute(double(x(:,:,ones(1,ny))),[1 3 2])-permute(double(y(:,:,ones(1,nx))),[3 1 2]),nxy,p);
            if min(size(w))==1
                wv=w(:).';
                d=reshape(sum(z.*wv(ones(nxy,1),:).*conj(z),2),nx,ny);
            else
                d=reshape(sum(z*w.*conj(z),2),nx,ny);
            end
        end
    else

        % x and y are vectors

        z=double(x(:,ones(1,ny)))-double(y(:,ones(1,nx))).';
        if nargin<4
            d=z.*conj(z);
        else
            d=w*z.*conj(z);
        end
    end
end
if any(mode=='s')
    d=sqrt(d);
end


addpath('../src/')
addpath('../src/utils/')

% fix random seed
rng(0);

% make your own discrete linear system with disturbance
A = [1 1; 0 1];
B = [0.5; 1];
Q = diag([1, 1]);
R = 0.1;

W_vertex = [0.15, 0.15; 0.15, -0.15; -0.15, -0.15; -0.15, 0.15]; % construct a convex set of disturbance (2dim here)
W = Polyhedron(W_vertex);

% construct disturbance Linear system
disturbance_system = DisturbanceLinearSystem(A, B, Q, R, W);

% constraints on state Xc and input Uc
Xc_vertex = [2, -2; 2 2; -10 2; -10 -2];
Uc_vertex = [1; -1];
Xc = Polyhedron(Xc_vertex);
Uc = Polyhedron(Uc_vertex);

% create a tube_mpc simulater
% if N_horizon is too small, the path will never reach inside the robust MPI-set X_mpi_robust in time step N_horizon, then the problem becomes infeasible.
N_horizon = 10;
w_min = [0; -0.10];
w_max = [0; 0.10];
mpc = TubeModelPredictiveControl(disturbance_system, Xc, Uc, N_horizon);

% The robust MPC guidances the path inside the robust MPI-set so that the path will reach the robust MPI-set in N_horizon.
x = [-7; -2];
savedir_name = './results/';
mkdir(savedir_name);

for i = 1:15
    disp(i)
    u_next = mpc.solve(x);
    x = disturbance_system.propagate(x, u_next); % additive disturbance is considered inside the method
    mpc.show_prediction();
    filename = strcat(savedir_name, 'tmpc_seq', number2string(i), '.png')
    saveas(gcf, char(filename)); % removing this line makes the code much faster
    clf;
end