function out = Dstar(R, n, e, fun)
%finds excess distortion for a given rate at a given blocklength for
%Gaussian source with unit variance
%R - rate
%n - block length (scalar)
%e - excess probability
%fun - which function to use for calculation

%
%   Created in 2012 by Victoria Kostina (vkostina@caltech.edu)
%

%starting points - 'persistent' to make optimization faster
persistent x0;
persistent y0;

try

    switch lower(fun)
        case 'shannon'
            %achievability via Shannon's upper bound:
            tol = 1e-4;
            qglb = [2^(-2*R); 1];
            qgub = [1; Inf];
            options = optimset('TolX',tol, 'MaxFunEvals', 500, 'Algorithm', 'active-set');

            if isempty(x0) && isempty (y0)
                x0 = [2^(-2*R) 1];
                y0 = [(2^(-2*R)+1)/2 1]; %q gamma
            end
            out = DShannon();
        case 'normal'
            %normal approximation
            out = DNormal();
        case 'spherecoveringc'
            %converse via sphere covering
            out = DSphereCovering();
        case 'spherecoveringa'
            %achievability via sphere covering
            if isempty(x0) && isempty (y0)
                x0 = [2^(-2*R) .5]; %distortion
             %   y0 = [0 1];        %auxiliary parameter q
            end
            out = DSphereCoveringA();
        otherwise
            disp('Unknown type.')
    end

catch ME
    fprintf('Error: %s\n', ME.message);
    out = NaN;
end

%--------------------------------------------------------------------------
    function out = DShannon()
        %achievability via Shannon's upper bound:
        out = fzero(@(x)Pexcess(x) - e,x0);
        x0 = out;
        function out = Pexcess(x)
            D = x;
            [y0, Pe] = fmincon(@optqg, y0,[],[],[],[],qglb, qgub, [], options);
            out = Pe;

            function out = optqg(y)
                q = y(1);
                g = log2(y(2));
                out = PeubShannon(R, n, D, q, g);
            end
        end

    end

%--------------------------------------------------------------------------
    function out = DNormal()
        out = 2^(-2*R)*(1 + sqrt(2/n)*Qinv(e));
    end

%--------------------------------------------------------------------------
    function out = DSphereCovering()
        %converse via sphere covering

        %find rstar:
        factor = 10;
        rstar = fzero(@(x)chi2cdf(x^2*n, n) - chi2cdf(factor*n, n) + e, 1);
        out = fminbnd(@(x) abs(R - rate(x)),2^(-2*R),1);

        function out = rate(x)
            out = 1/2*log2(rstar^2/x);
        end
    end

%--------------------------------------------------------------------------
    function out = DSphereCoveringA()
        %achievability via sphere covering
        M = 2.^(n*R);
        out = fzero(@(x)Pexcess(x) - e, x0);
    function out = Pexcess(x)
        d = x;
        s0 = 1; %variance of source
        r0 = sqrt(s0^2 - d); %distance from the center of the representation sphere

        out = quad(@density, r0 - sqrt(d), r0 + sqrt(d))...
            + 1 - chi2cdf((r0 + sqrt(d))^2*n,n)...
            + chi2cdf((r0 - sqrt(d))^2*n,n);
        out = real(out);

        function out = density(r)
          cosa = (r0^2 + r.^2 - d)./(2*r0.*r);
          sina = (1 - cosa.^2).^(1/2);
          out = ub(sina).*chi2pdf(n*r.^2, n).*n*2.*r;
        end

        function out = ub(sina)
            if (n < 50)
                A = 1/sqrt(pi).*gamma(n/2+1)./n./gamma((n-1)/2+1);
                out = (1 - A.*(sina).^(n-1)).^M;
            else
                out = exp(-M*sina.^(n-1)./sqrt(2*pi*n));
            end
        end
    end

    end

end


function H = comp_nyquistfilt(wintype,fs,chan_max,freqtoscale,scaletofreq,bwmul,bins,Ls)
%COMP_NYQUISTFILT high-pass filter for warped filter banks

    kk = chan_max;
    while scaletofreq(kk-bwmul) < fs/2;
      kk = kk+1/bins;
    end
    Maxfilt = kk;

    Minpos = ceil(Ls/fs*scaletofreq(chan_max+1/bins-bwmul));
    samples = freqtoscale((Minpos-1:floor(Ls/2))*fs/Ls);

    FILTS = zeros(round(bins*(Maxfilt-chan_max)),numel(samples));
    for kk = 1:size(FILTS,1)
       FILTS(kk,:) = firwin(wintype,(samples-(chan_max+kk/bins))/(2*bwmul));
    end
    H = zeros(2*numel(samples)-1,1);
    H(1:numel(samples)) = sqrt(sum(abs(FILTS.^2),1));
    H(numel(samples)+1:end) = H(numel(samples)-1:-1:1);


function H = elec_meshplot(X,Y,Z,interp,grid_res)

% elec_meshplot - Plot a mesh and points for 3D electrode positions.
%
% Usage: H = elec_meshplot(X,Y,Z [,interp] [,grid_resolution])
%
% Uses an interpolated mesh at 0.25 cm resolution, unless
% user specifies alternative grid resolution (optional).
%
% Interpolation can be 'linear' or 'cubic'.  Unless specified,
% cubic is the default.
%
% Returns a handle to the figure created.
%

% $Revision: 1.1 $ $Date: 2009-04-28 22:13:55 $

% Licence: Gnu GPL
% Author: Darren.Weber_at_radiology.ucsf.edu
% Created:  18/05/00
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  H = figure('NumberTitle','off','Name','Electrode Mesh Plot','Position',[200 200 650 500],'PaperUnits','centimeters','PaperType','A4','Units','centimeters');

  colormap(gray);

  if ~exist( 'grid_res', 'var' )  grid_res = 0.25;  end

  xi =   (min(X) - 2):grid_res:(max(X) + 2);
  yi =  [(min(Y) - 2):grid_res:(max(Y) + 2)]';

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Use 'linear' for a linear grid or 'cubic' for a polynomial grid

  if ~exist( 'interp', 'var' )  interp = 'cubic';  end

  [Xi,Yi,Zi] = griddata(X,Y,Z,xi,yi,interp);

  surf(Xi,Yi,Zi);

  brighten(0.75);

  mesh(Xi,Yi,Zi);

  hold on;

  plot3(X,Y,Z,'ro');

  rotate3d;

  clear xi yi Xi Yi Zi X Y Z;

%  view(2);  % top view
%  view(   0,0);  % back view
%  view(-180,0);  % front view
%  view(-90, 0);  % left side view
%  view( 90, 0);  % right side view


%ComputeMarginal Computes the marginal over a set of given variables
%   M = ComputeMarginal(V, F, E) computes the marginal over variables V
%   in the distribution induced by the set of factors F, given evidence E
%
%   M is a factor containing the marginal over variables V
%   V is a vector containing the variables in the marginal e.g. [1 2 3] for
%     X_1, X_2 and X_3.
%   F is a vector of factors (struct array) containing the factors
%     defining the distribution
%   E is an N-by-2 matrix, each row being a variable/value pair.
%     Variables are in the first column and values are in the second column.
%
% Copyright (C) Daphne Koller, Stanford Univerity, 2012

function M = ComputeMarginal(V, F, E)

% Check for empty factor list
assert(numel(F) ~= 0, 'Error: empty factor list');

  F = ObserveEvidence(F, E);
  Joint = ComputeJointDistribution(F);
  Joint.val = Joint.val ./ sum(Joint.val);
  M = FactorMarginalization(Joint, setdiff(Joint.var, V));
end


function MV = ffmInteractionsSparseHF(X,Xbox,Y,Ybox,V,Ibox,green,k,edg,tol)
%+========================================================================+
%|                                                                        |
%|         OPENFFM - LIBRARY FOR FAST AND FREE MEMORY CONVOLUTION         |
%|           openFfm is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2019.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : ffmInteractionsSparseHF.m                     |
%|    #    |   VERSION    : 0.6                                           |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 05.09.2019                                    |
%| ( === ) |   SYNOPSIS   : Sparse product for high-frequency compressible|
%|  `---'  |                leaves                                        |
%+========================================================================+

% Initialisation du produit Matrice-Vecteur
MV = zeros(size(X,1),1,class(V));

% Quadrature spherique de Geggenbauer
[Xq,Wq,l] = ffmQuadratureHF(k,edg,tol);

% Unicite des vecteurs de translation
XY        = Ybox.ctr(Ibox(:,2),:) - Xbox.ctr(Ibox(:,1),:);
[~,Il,It] = unique(floor(XY*1e6),'rows','stable');
Nt        = length(Il);

% Fonctions de transfert
Txy = cell(Nt,1);
for i = 1:Nt
    Txy{i} = ffmTransfertHF(Xq,Wq,XY(Il(i),:),k,l);
end

% Convolution en Y
Vy = cell(size(Ybox.ind));
for i = unique(Ibox(:,2)')
    % Boite centree en Y
    iy = Ybox.ind{i};
    ny = length(iy);
    Ym = Y(iy,:) - ones(ny,1)*Ybox.ctr(i,:);

    % Transformee de Fourier inverse (Ym->Xq)
    Vy{i} = ffmInterpHF(Xq,Ym,V(iy),-1,tol);

    % Derivation du noyau en Y
    if strcmp(green(1:end-1),'grady[exp(ikr)/r]')
        j     = str2double(green(end));
        Vy{i} = -1i*Xq(:,j) .* Vy{i};
    end
end

% Translations
TVy = cell(size(Xbox.ind));
for i = 1:length(TVy)
    TVy{i} = 0;
end
for i = 1:size(Ibox,1)
    TVy{Ibox(i,1)} = TVy{Ibox(i,1)} + Txy{It(i)}.*Vy{Ibox(i,2)};
end

% Convolutions en X
for i = unique(Ibox(:,1)')
    % Boite centree en X
    ix = Xbox.ind{i};
    nx = length(ix);
    Xm = X(ix,:) - ones(nx,1)*Xbox.ctr(i,:);

    % Derivation du noyau en X
    if strcmp(green(1:end-1),'gradx[exp(ikr)/r]')
        j      = str2double(green(end));
        TVy{i} = 1i*Xq(:,j) .* TVy{i};
    end

    % Transformee de Fourier (Xq->X)
    MV(ix) = ffmInterpHF(Xm,Xq,TVy{i},+1,tol);
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Xq,Wq,l] = ffmQuadratureHF(k,edg,tol)
% Ordre harmonique (E. Darve)
l = floor( abs(k)*sqrt(3)*edg - log(tol) );

% Quadrature de Gauss sur [-1,1]
u     = 1:l;
u     = u ./ sqrt(4*u.^2 - 1);
[V,x] = eig( diag(u,-1) + diag(u,+1) );
[x,I] = sort(diag(x));
w     = 2*V(1,I)'.^2;

% Quadrature de Gauss sur [0,1] par transformation lineaire
a = 0;   b = 1;
x = 0.5*(b-a)*x + 0.5*(a+b);
w = 0.5*(b-a)*w;

% Quadrature de Gauss en elevation (phi)
phi = acos(2*x(end:-1:1)-1) - pi/2;
Wp  = 2*w(end:-1:1)';

% Quadrature reguliere en azimut (theta)
Ntheta = 2*(l+1);
theta  = 2*pi/Ntheta*(0:Ntheta-1)';
Wt     = 2*pi/Ntheta*ones(Ntheta,1);

% Quadrature spherique par produit tensoriel
[theta,phi] = ndgrid(theta,phi);
theta       = theta(:);
phi         = phi(:);
Wq          = Wt * Wp;
Wq          = Wq(:);

% Coordonnees carthesienne et produit par le nombre d'onde
[x,y,z] = sph2cart(theta,phi,1);
Xq      = k*[x,y,z];

% Securite
if (l>3)
    % Harmoniques spheriques jusqu'a l'ordre 3
    n   = 1;
    Ylm = zeros(length(theta),16);
    for ll = 0:3
        Plm = sqrt((2*ll+1)/(4*pi)) * legendre(ll,cos(pi/2-phi),'sch').';
        for m = -ll:ll
            if m<0
                Ylm(:,n) = Plm(:,abs(m)+1) .* sin(abs(m).*theta);
            else
                Ylm(:,n) = Plm(:,abs(m)+1) .* cos(abs(m).*theta);
            end
            n = n+1;
        end
    end

    % Test de l'integration spherique des harmoniques (produit scalaire)
    [m,n] = size(Ylm);
    if norm( (Ylm' * spdiags(Wq,0,m,m) * Ylm) - eye(n) , 'inf') > 1e-5
        error('ffmQuadratureHF.m - error 1');
    end
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function T = ffmTransfertHF(Xq,ws,xy,k,l)
% Fonctions de hankel spherique 1ere espece
besselhs = @(n,z) sqrt(pi./(2.*z)) .* besselh(n+0.5,1,z);

% Distance
kr = k*norm(xy);

% cosinus(angle incidence)
x = Xq*(-xy/kr)';

% Initialisation de la recursion sur (besselhs(kr) * Pl(cos(phi))
P0 = ones(size(x));
P1 = x;
T  = besselhs(0,kr).*P0 + 3i*besselhs(1,kr).*P1;

% Construction recursive des polynomes de Legendre
for ll = 2:l
    P2 = 1/ll .* ( (2*(ll-1)+1).*x.*P1 - (ll-1).*P0 );
    T  = T + (2*ll+1) * 1i^ll * besselhs(ll,kr) .* P2;
    P0 = P1;
    P1 = P2;
end

% Operateur de Translation
T = 1i*abs(k)/(4*pi) .* ws .* T;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function MV = ffmInterpHF(X,Y,V,iflag,tol)
% Dimensions
Nx   = size(X,1);
Ny   = size(Y,1);
Nmax = max(Nx,Ny);
Nmin = min(Nx,Ny);

% DFT non uniforme
if (Nmin<100) || (Nmin<log(Nmax))
    MV = exp(1i*iflag*X*Y') * V;

% FFT non uniforme
else
    % Conversion de type
    if isa(X,'single') || isa(Y,'single') || isa(V,'single')
        sgl = 1;
        X = double(X); Y = double(Y); V = double(V);
    else
        sgl = 0;
    end

    % Initialisation produit Matrice-Vecteur
    MV = zeros(Nx,1) + 1i*zeros(Nx,1);

    % Prevention de coplanarite en X
    Nx         = Nx + 1;
    X(end+1,:) = X(end,:) + tol;
    MV(end+1)  = 0;

    % Prevention de coplanarite en X
    Ny         = Ny + 1;
    Y(end+1,:) = Y(end,:) + tol;
    V(end+1)   = 0;

    % Transformee de Fourier
    ier    = 0;
    mex_id = 'nufft3d3f90(i int[x], i double[], i double[], i double[], i dcomplex[], i int[x], i double[x], i int[x], i double[], i double[], i double[], io dcomplex[], io int[x])';
    MV     = nufft3d(mex_id,Ny,Y(:,1),Y(:,2),Y(:,3),V, iflag, tol, ...
        Nx,X(:,1),X(:,2),X(:,3),MV, ier, 1, 1, 1, 1, 1);

    % Supression dernier point
    MV = MV(1:end-1);

    % Conversion de type
    if sgl
        MV = single(MV);
    end
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function [mMedModF, mStdL, loopout] = brutebootloglike_a2(time_as, time_asf, bootloops,fT1, nMod)
    % BRUTEBOOTLOGLIKE_A2 Bootstrap analysis of Omori parameters calculated by brute force
    % (p1,p2,c1,c2,k1,k2)-pair is mean of the bootstrap values by determining the mean cumulative
    % number modeled a end of the learning period
    % Standard deviations are calculated as the 2nd moment, not to rely fully on normal distributions
    %
    % [mMedModF, mStdL, loopout] = BRUTEBOOTLOGLIKE_A2(time_as, time_asf, bootloops,fT1, nMod);
    % -------------------------------------------------------------------------------
    %
    % Input parameters:
    %   time_as     Delay times [days] of learning period
    %   time_asf    Delay times [days] until end of forecast period
    %   bootloops   Number of bootstraps
    %   fT1         Time of biggest aftershock in learning period
    %   nMod        Model to fit data, three models including a secondary aftershock sequence.
    %               Different models have varying amount of free parameters
    %               before (p1,c1,k1) and after (p2,c2,k2) the aftershock occurence
    %               1: modified Omori law (MOL): 3 free parameters
    %                  p1=p2,c1=c2,k1=k2
    %               2: MOL with one secondary aftershock sequence:4 free parameters
    %                  p1=p2,c1=c2,k1~=k2
    %               3: MOL with one secondary aftershock sequence:5 free parameters
    %                  p1~=p2,c1=c2,k1~=k2
    %               4: MOL with one secondary aftershock sequence:6 free parameters
    %                  p1~=p2,c1~=c2,k1~=k2
    %
    % Output parameters:
    %  mMedModF :  Result matrix including the values for the mean forecast at end of forecast period
    %  mStdL    :  Uncertainties of fit to the data in learning period
    %  loopout     contains all results
    %
    % Samuel Neukomm / S. Wiemer / J. Woessner
    % updated: 05.08.03

    time_as = sort(time_as);
    %bootloops = 50; % number of bootstrap samples
    n = length(time_as);
    loopout = nan(bootloops,9); % 8 from bruteforceloglike_a2, plus variate column.
    % Initialize random seed
    rng('shuffle');

    % i = (1:n)';
    for j = 1:bootloops
        newtas = sort(datasample(time_as, n, 'Replace',true));
        [pv1, pv2, cv1, cv2, kv1, kv2, fAIC, fL] = bruteforceloglike_a2(newtas, fT1, nMod);
        loopout(j,1:8) = [pv1, pv2, cv1, cv2, kv1, kv2, fAIC, fL];
    end

    % New version: Choose mean (p,c,k)-variables by modelling the cumulative number at end of
    % the learning period

    % 2nd moment i.e. Standard deviations
    [pstd1] = std(loopout(:,1),1,'omitnan');
    [pstd2] = std(loopout(:,2),1,'omitnan');
    [cstd1] = std(loopout(:,3),1,'omitnan');
    [cstd2] = std(loopout(:,4),1,'omitnan');
    [kstd1] = std(loopout(:,5),1,'omitnan');
    [kstd2] = std(loopout(:,6),1,'omitnan');

    % Uncertainties of fit
    mStdL = [pstd1 pstd2 cstd1 cstd2 kstd1 kstd2];

    %% Compute best fitting pair of variates
    % TODO (maybe) vectorize this
    n_time_asf = length(time_asf);

    pv1=loopout(:,1);
    pv2=loopout(:,2);
    cv1=loopout(:,3);
    cv2=loopout(:,4);
    kv1=loopout(:,5);
    kv2=loopout(:,6);

    cumnr_model = OmoriModel.doForecast(nMod,time_asf, pv1, cv1, kv1, fT1, kv2, pv2, cv2);
    loopout(:,9) = max(cumnr_model);

    %%
    [Y, in] = sort(loopout(:,9));
    loops = loopout(in,:);
    % % Median values: Old version
    % vMedian = abs(loops(:,9)-median(loops(:,9)));
    % nMedian = (find(vMedian == min(vMedian)));
    %
    % if length(nMedian(:,1)) > 1
    %     nMedian = nMedian(1,1);
    % end
    % pmedian1 = loops(nMedian,1);
    % pmedian2 = loops(nMedian,2);
    % cmedian1 = loops(nMedian,3);
    % cmedian2 = loops(nMedian,4);
    % kmedian1 = loops(nMedian,5);
    % kmedian2 = loops(nMedian,6);
    %
    % mMedModF = [pmedian1, pstd1, pmedian2, pstd2, cmedian1, cstd1, cmedian2, cstd2, kmedian1, kstd1, kmedian2, kstd2];

    % Mean values
    vMean = abs(loops(:,9)-mean(loops(:,9)));
    nMean = (find(vMean == min(vMean)));

    if length(nMean(:,1)) > 1
        nMean = nMean(1,1);
    end
    pMean1 = loops(nMean,1);
    pMean2 = loops(nMean,2);
    cMean1 = loops(nMean,3);
    cMean2 = loops(nMean,4);
    kMean1 = loops(nMean,5);
    kMean2 = loops(nMean,6);

    mMedModF = [pMean1, pstd1, pMean2, pstd2, cMean1, cstd1, cMean2, cstd2, kMean1, kstd1, kMean2, kstd2];


clear all; close all; clc;
addpath('./toolbox');

% load feature trajectories
load('./data/running.mat');

% define some variables
dtrng			= 20:0.5:40;
arng			= 0.8:0.02:1.2;
projmodel = 'affine';
a_con			= [];						% set to a value to constrain a

% define noise parameters
nnoise		= 21;
nits			= 20;
noisevec	= linspace(0,5,nnoise);
dts				= zeros(nits,nnoise);

% offset W1 by 30 frames
W1		= W1(31:end-30,:);

for n = 1:nnoise
	for it = 1:nits
		W1n = W1 + noisevec(n)*randn(size(W1));
		W2n	= W2 + noisevec(n)*randn(size(W2));

		% run synchronization script
		[a,dt] = sync(W1n,W2n,a_con,projmodel);

		dts(it,n)	= dt;
	end
end

% this took a while so save to a temporary file just in case
save('temp.mat','dts');

mn	= mean(dts,1);
sd	= 3*std(dts,[],1);

figure;
	plot([0,5],[30,30],'r--',...
				noisevec,dts,'b.');
	axis('equal',[minmax(noisevec),28,32]);
	xlabel('\sigma_n (pixels)'); ylabel('Recovered offset (frames)');


%% housekeeping
gentle_clear()
close all
clc

%% load and transform the data
db=xlsread('usmacro2.xlsx');

db=ts('1955Q1',db(:,4:end),{'Y','P','R','C','N','I','E'});

db=pages2struct(db);

%% Rise the madel

m=rise('model10');

%% priors
priors=struct();
priors.beta={0.98,0.1,0.999};
priors.kappa={0.1,0.001,5};
priors.psi={0.5,0.001,1};
priors.rhou={0.5,-0.999,0.999};
priors.rhog={0.5,-0.999,0.999};
priors.rhoe={0.5,-0.999,0.999};
priors.sigu={0.1,0.001,15};
priors.sigg={0.1,0.001,15};
priors.sige={0.1,0.001,15};
%% estimate model

mest=estimate(m,'data_demean',true,'data',db,'priors',priors,...
    'estim_start_date','1973Q2','estim_end_date','2015Q4');


function p_ = ViewRealizedVol(X,p)

[J,K]=size(X);

% constrain probabilities to sum to one...
Aeq = ones(1,J);
beq=1;

% ...constrain the median...
V = abs(X(:,1));

[V_Sort I_Sort]=sort(V);
F=cumsum(p(I_Sort));

I_Reference=max(find(F<=3/5));
V_Reference=V_Sort(I_Reference);

I_Select=find(V<=V_Reference);

a=zeros(1,J);
a(I_Select)=1;

A = a;
b = .5;

% ...compute posterior probabilities
p_ = EntropyProg(p,A,b,Aeq ,beq);


classdef (Abstract) AbstractSimulator < handle
% Abstract class for movement simulation.
% Author: Yu Jiang
% Contact: yu.jiang@nyu.edu
% Copyright 2015 Yu Jiang

    properties (Constant)
        % Constant simulation parameters
        tau = 0.05;   % Time constant
        m1 = 2;       % mass on x direction
        m2 = 2;       % mass on y direction

        c1 = 0.15/2;  % noise scale
        c2 = 0.05/2;  % noise scale
        dt_ = 0.005;   % sample time for learning

        Q0 = [500 0; 0 1000];   % Initial weighting matrices
        R = diag([0.01,0.01]);  % Initial weighting matrices

        % The null filed dynamics
        A0 = [zeros(2) eye(2) zeros(2);
              zeros(2,4) diag([1/AbstractSimulator.m1 1/AbstractSimulator.m2])
              zeros(2,4) -diag([1/AbstractSimulator.tau 1/AbstractSimulator.tau])];
        B =  [zeros(4,2); diag([1/AbstractSimulator.tau 1/AbstractSimulator.tau])];
    end

    properties
        % Variables
        dt % Sample time for simulation
        A  % Dynamics with Force field
    end

    properties
        % Others
        fig1;
        fig2;
        fig3;
    end
end


function fv= proc_rSquareSigned(fv, varargin)
%PROC_RSQUARESIGNED - computes signed r^2 values (measure for discriminance)
%
%Synopsis:
% 	fv = proc_rSquareSigned(fv, <opt>)
%
%Returns:
% FV_RVAL - data structure of signed squared biserial correlation coefficients
%  .x     - signed squared biserial correlation between each featur and the
%           class label
%  .se    - contains the standard error of atanh(r), if opt.Stats==1
%  .p     - contains the p value of null hypothesis that there is zero
%           correlation between feature and class-label, if opt.Stats==1
%  .sgnlogp - contains the signed log10 p-value, if opt.Stats==1
%             if opt.Bonferroni==1, the p-value is multiplied by
%             fv_rval.corrfac
%  .sgnlogp - contains the signed log10 p-value, if opt.Stats==1
%           if opt.Bonferroni==1, the p-value is multiplied by
%           fv_rval.corrfac and then logarithmized
%  .sigmask - binary array indicating significance at alpha level
%             opt.Alphalevel, if opt.Stats==1 and opt.Alphalevel > 0
%  .corrfac - Bonferroni correction factor (number of simultaneous tests),
%             if opt.Bonferroni==1
%
%Properties:
% 'TolerateNans': observations with NaN value are skipped
%    (nanmean/nanstd are used instead of mean/std). Deafult: 0
% 'ValueForConst': constant feauture dimensions are assigned this
%    value. Default: NaN.
% 'MulticlassPolicy': possible options: 'pairwise' (default),
%    'all-against-last', 'each-against-rest', or provide specified
%    pairs as an [nPairs x 2] sized matrix. ('specified_pairs' is obsolete)
% 'Stats' - if true, additional statistics are calculated, including the
%           standard error of atanh(r), the p-value for the null
%           Hypothesis that the correlation is zero,
%           and the "signed log p-value"
% 'Bonferroni' - if true, Bonferroni corrected is used to adjust p-values
%                and their logarithms
% 'Alphalevel' - if provided, a binary indicator of the significance to the
%                alpha level is returned for each feature in fv_rval.sigmask
%
%Description:
% Computes the r^2 value for each feature, multiplied by the sign of
% of r value. The r^2 value is a measure
% of how much variance of the joint distribution can be explained by
% class membership.
%
% Example:
%  [cnt, mrk]= file_readBV(some_file);   %load EEG-data in BV-format
%  mrk= mrk_defineClasses(mrk, {1, 2; 'target','nontarget'});
%  epo= proc_segmentation(cnt, mrk, [-200 800], 'CLab', {'Fz','Cz','Pz'});
%  epo_r = proc_rSquareSigned(epo);
%
% See also proc_classmeanDiff, proc_rValues, proc_rSquare
%
% 03-03 Benjamin Blankertz
% 09-2012 stefan.haufe@tu-berlin.de

if nargin==0,
  fv=proc_rValues; return
end

fv= proc_rValues(fv, varargin{:});
fv.x= fv.x .* abs(fv.x);
for cc= 1:length(fv.className),
  fv.className{cc}= ['sgn r^2' fv.className{cc}(2:end)];
end
fv.yUnit= 'sgn r^2';


function [rRelmTest] = relm_NTest(vRatesH, vRatesN, nNumberSimulation, fMagThreshold, bOptimized, bDrawFigure)
% function [rRelmTest] = relm_NTest(vRatesH, vRatesN, nNumberSimulation, fMagThreshold, bOptimized, bDrawFigure)
% --------------------------------------------------------------------------------------------------------------
% Computation of the N-test for the RELM framework
%
% Input parameters:
%   vRatesH                       Matrix with rates of the test hypothesis
%   vRatesN                       Matrix with rates of the null hypothesis
%   nNumberSimulation             Number of random simulations
%   fMagThreshold                 Magnitude threshold (Use only bins with magnitude >= threshold
%   bOptimized                    0 (default): use a for loop, 1: matrix-wise calculation (needs a lot of memory)
%   bDrawFigure                   Draw the cumulative density plot after testing (default: off)
%
% Output paramters:
%   rRelmTest.fAlpha              Alpha-value of the cumulative density
%   rRelmTest.fBeta               Beta-value of the cumulative density
%   rRelmTest.vSimValues_H        Vector containing the sorted simulated numbers of events for the test hypothesis
%   rRelmTest.vSimValues_N        Vector containing the sorted simulated numbers of events for the null hypothesis
%   rRelmTest.nNumberSimulation   Number of random simulations
%   rRelmTest.fObservedData       Observed total number of events
%
% Danijel Schorlemmer
% October 4, 2002

% Exit on empty rate matrices
if isempty(vRatesH)  ||  isempty(vRatesN)
  rRelmTest.fAlpha = nan;
  rRelmTest.fBeta = nan;
  rRelmTest.vSimValues_H = nan;
  rRelmTest.vSimValues_N = nan;
  rRelmTest.nNumberSimulation = nan;
  rRelmTest.fObservedData = nan;
  return;
end

if ~exist('bDrawFigure')
  bDrawFigure = 0;
end

if ~exist('bOptimized')
  bOptimized = 0;
end

% Randomize
rand('state',sum(100*clock));

% Get the necessary data from the rate matrices and weight them properly
[vLambdaH, vLambdaN, vNumberQuake] = relm_PrepareData(vRatesH, vRatesN, fMagThreshold);
nNumberQuake = sum(vNumberQuake);

% Get the number of bins (rows)
[nRow, nColumn] = size(vLambdaH);

if bOptimized
  % Create the random numbers for the simulation
  vRandom = rand(nRow, nNumberSimulation);

  % Replicate the rate vectors
  vLambdaH = repmat(vLambdaH, 1, nNumberSimulation);
  vLambdaN = repmat(vLambdaN, 1, nNumberSimulation);

  % Compute the simulated number of events and sum them up
  vNum_H = poissinv(vRandom, vLambdaH);
  vNum_N = poissinv(vRandom, vLambdaN);
  vSimNum_H = sum(vNum_H);
  vSimNum_N = sum(vNum_N);
else
  % Create empty vectors for the total number of events
  vSimNum_H = [];
  vSimNum_N = [];

  % Loop over the simulations
  for nCnt = 1:nNumberSimulation
    % Create the random numbers for the simulation
    vRandom = rand(nRow, 1);

    % Compute the simulated number of events and sum them up
    vNum_H = poissinv(vRandom, vLambdaH);
    vNum_N = poissinv(vRandom, vLambdaN);
    vSimNum_H = [vSimNum_H; nansum(vNum_H)];
    vSimNum_N = [vSimNum_N; nansum(vNum_N)];
  end
end

% Sort them for the cumulative density plot
rRelmTest.vSimValues_H = sort(vSimNum_H);
rRelmTest.vSimValues_N = sort(vSimNum_N);

% Compute Alpha and Beta and store the important parameters
rRelmTest.fAlpha = sum(rRelmTest.vSimValues_N > nNumberQuake)/nNumberSimulation;
rRelmTest.fBeta = sum(rRelmTest.vSimValues_H < nNumberQuake)/nNumberSimulation;
rRelmTest.nNumberSimulation = nNumberSimulation;
rRelmTest.fObservedData = nNumberQuake;

if bDrawFigure
  relm_PaintCumPlot(rRelmTest, 'Number of earthquakes');
end


function [occGrad,occlusionErrors] = occlusionGrad(S,state)

occGrad = zeros(size(S));
numpoints = size(S,1);

mask = double(state.mask);
[Y,X]=size(mask);
mkp = mean(state.kps,2);% we had shifted everything by this when computing projected points so shift back now

%% Finding points projected outside the image
points2d = state.cameraScale*state.cameraRot*S'; %% Verify this later
points2d = points2d' + repmat(mkp',size(S,1),1);
points2d = round(points2d(:,1:2)); %% Verify correctness later

badX = double(points2d(:,1)>X) + double(points2d(:,1)<1);
badY = double(points2d(:,2)>Y) + double(points2d(:,2)<1);

%% Visualization
%imagesc(mask);hold on;
%plot(points2d(:,1),points2d(:,2));pause();
%close;

%% Restructuring to ease computation to determine of projected points inside silhoutte
points2d = points2d*[Y;1]-Y;
points2d(points2d>X*Y)=X*Y;
points2d(points2d<1)=1;

mask = double(mask(:)==0);
occlusionErrors = double((mask(points2d)+badX+badY)>0);
occGrad = repmat(occlusionErrors,1,3).*(repmat(mean(S,1),numpoints,1)-S);

end


% Script demonstrating usage of the cbpdndlms function.
%
% Author: Brendt Wohlberg <brendt@lanl.gov>  Modified: 2017-04-29
%
% This file is part of the SPORCO library. Details of the copyright
% and user license can be found in the 'Copyright' and 'License' files
% distributed with the library.

% Training images
S0 = zeros(512, 512, 2, 'single');
S0(:,:,1) = single(stdimage('lena.grey')) / 255;
S0(:,:,2) = single(stdimage('barbara.grey')) / 255;

%Reduce images size to speed up demo script
tmp = zeros(128, 128, 2, 'single');
for k = 1:size(S0,3),
  tmp(:,:,k) = imresize(S0(:,:,k), 0.25);
end
S0 = tmp;

% Filter input images and compute highpass images
npd = 16;
fltlmbd = 5;
[Sl, Sh] = lowpass(S0, fltlmbd, npd);

% Construct weight matrix and padded test image set
Shp = padarray(Sh, [7 7], 'post');
t = 0.5;
W = randn(size(Sh));
W(abs(W) > t) = 1;
W(abs(W) < t) = 0;
W = padarray(W, [7 7], 'post');
ShW = W .* Shp;

% Construct initial dictionary
D0 = zeros(8,8,32, 'single');
D0(3:6,3:6,:) = single(randn(4,4,32));

% Set up cbpdndl parameters
lambda = 0.05;
opt = [];
opt.Verbose = 1;
opt.MaxMainIter = 500;
opt.rho = 50*lambda + 0.5;
opt.sigma = size(Sh,3);
opt.AutoRho = 1;
opt.AutoRhoPeriod = 10;
opt.AutoSigma = 1;
opt.AutoSigmaPeriod = 10;
opt.XRelaxParam = 1.8;
opt.DRelaxParam = 1.8;

% Do standard dictionary learning and reconstruct
[D1, X1, optinf1] = cbpdndl(D0, ShW, lambda, opt);
DX1 = ifft2(bsxfun(@times, fft2(D1, size(X1,1), size(X1,2)), fft2(X1)), ...
           'symmetric');
Sr1 = squeeze(sum(DX1,3)) + padarray(Sl, [7 7], 'post');

% Do dictionary learning with additive mask simulation and reconstruct
opt.W = W;
[D2, X2, optinf2] = cbpdndlms(D0, ShW, lambda, opt);
DX2 = ifft2(bsxfun(@times, fft2(D2, size(X2,1), size(X2,2)), fft2(X2)), ...
           'symmetric');
Sr2 = squeeze(sum(DX2,3)) + padarray(Sl, [7 7], 'post');

% Display dictionaries
figure;
subplot(1,2,1);
imdisp(tiledict(D1));
title('Standard DL');
subplot(1,2,2);
imdisp(tiledict(D2));
title('DL with additive mask simulation');

% Display reconstructions
figure;
subplot(2,2,1);
imdisp(Sr1(:,:,1));
title('Standard DL');
subplot(2,2,2);
imdisp(Sr2(:,:,1));
title('DL with additive mask simulation');
subplot(2,2,3);
imdisp(Sr1(:,:,2));
title('Standard DL');
subplot(2,2,4);
imdisp(Sr2(:,:,2));
title('DL with additive mask simulation');

% Plot functional value evolution
figure;
subplot(1,2,1);
semilogx(optinf1.itstat(:,2), 'LineWidth', 2);
ylim([15, 45]);
xlabel('Iterations');
ylabel('Functional value');
title('Standard DL');
subplot(1,2,2);
semilogx(optinf2.itstat(:,2), 'LineWidth', 2);
ylim([15, 45]);
xlabel('Iterations');
ylabel('Functional value');
title('DL with additive mask simulation');


%[2018]-"Tree growth algorithm (TGA): A novel approach for solving
%optimization problems"

function TGA = jTreeGrowthAlgorithm(feat,label,opts)
% Parameters
lb         = 0;
ub         = 1;
thres      = 0.5;
num_tree1  = 3;    % size of first group
num_tree2  = 5;    % size of second group
num_tree4  = 3;    % size of fourth group
theta      = 0.8;  % tree reduction rate of power
lambda     = 0.5;  % control nearest tree

if isfield(opts,'T'), max_Iter = opts.T; end
if isfield(opts,'N'), N = opts.N; end
if isfield(opts,'N1'), num_tree1 = opts.N1; end
if isfield(opts,'N2'), num_tree2 = opts.N2; end
if isfield(opts,'N4'), num_tree4 = opts.N4; end
if isfield(opts,'theta'), theta = opts.theta; end
if isfield(opts,'lambda'), lambda = opts.lambda; end
if isfield(opts,'thres'), thres = opts.thres; end

% Limit number of N4 to N1
if num_tree4 > num_tree1 + num_tree2
  num_tree4 = num_tree1 + num_tree2;
end
% Objective function
fun = @jFitnessFunction;
% Number of dimensions
dim = size(feat,2);
% Initial
X   = zeros(N,dim);
for i = 1:N
	for d = 1:dim
    X(i,d) = lb + (ub - lb) * rand();
	end
end
% Fitness
fit  = zeros(1,N);
fitG = inf;
for i = 1:N
  fit(i) = fun(feat,label,(X(i,:) > thres),opts);
  % Best
  if fit(i) < fitG
    fitG = fit(i);
    Xgb  = X(i,:);
  end
end
% Sort tree from best to worst
[fit, idx] = sort(fit,'ascend');
X          = X(idx,:);
% Initial
dist = zeros(1,num_tree1 + num_tree2);
X1   = zeros(num_tree1,dim);
Xnew = zeros(num_tree4,dim);
Fnew = zeros(1,num_tree4);

curve = zeros(1,max_Iter);
curve(1) = fitG;
t = 2;
% Iterations
while t <= max_Iter
	% {1} Best trees group
  for i = 1:num_tree1
    r1 = rand();
    for d = 1:dim
      % Local search (1)
      X1(i,d) = (X(i,d) / theta) + r1 * X(i,d);
    end
  	% Boundary
    XB = X1(i,:); XB(XB > ub) = ub; XB(XB < lb) = lb;
    X1(i,:) = XB;
    % Fitness
    fitT = fun(feat,label,(X1(i,:) > thres),opts);
    % Greedy selection
    if fitT <= fit(i)
      X(i,:) = X1(i,:);
      fit(i) = fitT;
    end
  end
  % {2} Competitive for light tree group
  X_ori = X;
  for i = num_tree1 + 1 : num_tree1 + num_tree2
    % Neighbor tree
    for j = 1 : num_tree1 + num_tree2
      if j ~= i
        % Compute Euclidean distance (2)
        dist(j) = sqrt(sum((X_ori(j,:) - X_ori(i,:)) .^ 2));
      else
        % Solve same tree problem
        dist(j) = inf;
      end
    end
    % Find 2 trees with shorter distance
    [~, idx] = sort(dist,'ascend');
    T1       = X_ori(idx(1),:);
    T2       = X_ori(idx(2),:);
    % Alpha in [0,1]
    alpha    = rand();
    for d = 1:dim
      % Compute linear combination between 2 shorter tree (3)
      y = lambda * T1(d) + (1 - lambda) * T2(d);
      % Move tree i between 2 adjacent trees (4)
      X(i,d) = X(i,d) + alpha * y;
    end
    % Boundary
    XB = X(i,:); XB(XB > ub) = ub; XB(XB < lb) = lb;
    X(i,:) = XB;
    % Fitness
    fit(i) = fun(feat,label,(X(i,:) > thres),opts);
  end
  % {3} Remove and replace group
  for i = num_tree1 + num_tree2 + 1 : N
    for d = 1:dim
      % Generate new tree by remove worst tree
      X(i,d) = lb + (ub - lb) * rand();
    end
    % Fitness
    fit(i) = fun(feat,label,(X(i,:) > thres),opts);
  end
  % {4} Reproduction group
  for i = 1:num_tree4
    % Random a best tree
    r     = randi([1,num_tree1]);
    Xbest = X(r,:);
    % Mask operator
    mask  = randi([0,1],1,dim);
    % Mask opration between new & best trees
    for d = 1:dim
      % Generate new solution
      Xn = lb + (ub - lb) * rand();
      if mask(d) == 1
        Xnew(i,d) = Xbest(d);
      elseif mask(d) == 0
        % Generate new tree
        Xnew(i,d) = Xn;
      end
    end
    % Fitness
    Fnew(i) = fun(feat,label,(Xnew(i,:) > thres),opts);
  end
  % Sort population get best nPop trees
  XX        = [X; Xnew];
  FF        = [fit, Fnew];
  [FF, idx] = sort(FF,'ascend');
  X         = XX(idx(1:N),:);
  fit       = FF(1:N);
  % Global best
  if fit(1) < fitG
    fitG = fit(1);
    Xgb  = X(1,:);
  end
  curve(t) = fitG;
  fprintf('\nIteration %d Best (TGA)= %f',t,curve(t))
  t = t + 1;
end
% Select features
Pos   = 1:dim;
Sf    = Pos((Xgb > thres) == 1);
sFeat = feat(:,Sf);
% Store results
TGA.sf = Sf;
TGA.ff = sFeat;
TGA.nf = length(Sf);
TGA.c  = curve;
TGA.f  = feat;
TGA.l  = label;
end


function marginal = marginal_family(engine, i, t)
% MARGINAL_FAMILY Compute the marginal on the specified family (ff)
% marginal = marginal_family(engine, i, t)

if nargin < 3, t = 1; end

% The method is similar to the following HMM equation:
% xi(i,j,t) = normalise( alpha(i,t) * transmat(i,j) * obsmat(j,t+1) * beta(j,t+1) )
% where xi(i,j,t) = Pr(Q(t)=i, Q(t+1)=j | y(1:T))

bnet = bnet_from_engine(engine);

if myismember(i, engine.onodes)
  ps = parents(bnet.dag, i);
  p = ps(1);
  marginal = pot_to_marginal(engine.marginals{p,t});
  marginal.domain = [p i];
  return;
end

if t==1
  marginal = pot_to_marginal(engine.marginals{i,t});
  return;
end

bnet = bnet_from_engine(engine);
ss = length(bnet.intra);
pot = engine.CPDpot{i,t};
c = engine.obschild(i);
pot = multiply_by_pot(pot, engine.CPDpot{c,t});
pot = multiply_by_pot(pot, engine.back{i,t});
ps = parents(bnet.dag, i+ss);
for p=ps(:)'
  pot = multiply_by_pot(pot, engine.fwd{p,t-1});
end
marginal = pot_to_marginal(normalize_pot(pot));


% SYNTAX:
% intensity = hmrR_PreprocessIntensity_MedianFilter( intensity )
%
% UI NAME:
% hmrR_PreprocessIntensity_MedianFilter
%
% DESCRIPTION:
% Applies a median filter to data to remove huge spikes.
%
% INPUT:
% intensity - SNIRF data type where the d matrix is intensity
%
% OUTPUT:
% intensity - SNIRF data type where the d matrix is intensity
%
% USAGE OPTIONS:
% Intensity_to_Intensity: d = hmrR_PreprocessIntensity_MedianFilter(data)

function d = hmrR_PreprocessIntensity_MedianFilter( intensity)

for ii=1:length(intensity)
    d = intensity(ii).GetDataTimeSeries();

    for j = 1:size(d,2)
        foo = d(:,j);

        xdata = (1:length( foo))';
        d(:,j) = interp1(xdata(~isnan( foo)), foo(~isnan( foo)),xdata,'spline');

        new_signal = zeros(size(foo));
        new_signal(1) = foo(1);
        new_signal(end) = foo(end);
        for tp = 2:length(foo)-1
            values = foo([tp-1,tp,tp+1]);
            median_value = median(values);
            new_signal(tp) = median_value;
        end
        d(:,j) = new_signal;

    end

    intensity(ii).SetDataTimeSeries(d);
end


function www=trajt3(para,chrom)

x0=para(1);
y0=para(2);
z0=para(3);
vx0=0;
vy0=0;
vz0=0;
ax0=0;
ay0=0;
az0=0;
x2=para(4);
y2=para(5);
z2=para(6);
vx2=0;
vy2=0;
vz2=0;
ax2=0;
ay2=0;
az2=0;
x1=chrom(1);
y1=chrom(2);
z1=chrom(3);
vx1=chrom(4);
vy1=chrom(5);
vz1=chrom(6);
t1=chrom(7);
t2=chrom(8);

a00=x0;
a01=vx0;
a02=ax0/2;
a03=(4*x1-vx1*t1-4*x0-3*vx0*t1-ax0*t1^2)/t1^3;
a04=(vx1*t1-3*x1+3*x0+2*vx0*t1+ax0*t1^2/2)/t1^4;
ax1=2*a02+6*a03*t1+12*a04*t1^2;

c00=y0;
c01=vy0;
c02=ay0/2;
c03=(4*y1-vy1*t1-4*y0-3*vy0*t1-ay0*t1^2)/t1^3;
c04=(vy1*t1-3*y1+3*y0+2*vy0*t1+ay0*t1^2/2)/t1^4;
ay1=2*c02+6*c03*t1+12*c04*t1^2;

e00=z0;
e01=vz0;
e02=az0/2;
e03=(4*z1-vz1*t1-4*z0-3*vz0*t1-az0*t1^2)/t1^3;
e04=(vz1*t1-3*z1+3*z0+2*vz0*t1+az0*t1^2/2)/t1^4;
az1=2*e02+6*e03*t1+12*e04*t1^2;
%--------------------------------------------------------------------------
b10=x1;
b11=vx1;
b12=ax1/2;
b13=(20*x2-20*x1-(8*vx2+12*vx1)*t2-(3*ax1-ax2)*t2^2)/(2*t2^3);
b14=(30*x1-30*x2+(14*vx2+16*vx1)*t2+(3*ax1-2*ax2)*t2^2)/(2*t2^4);
b15=(12*x2-12*x1-(6*vx2+6*vx1)*t2-(ax1-ax2)*t2^2)/(2*t2^5);

d10=y1;
d11=vy1;
d12=ay1/2;
d13=(20*y2-20*y1-(8*vy2+12*vy1)*t2-(3*ay1-ay2)*t2^2)/(2*t2^3);
d14=(30*y1-30*y2+(14*vy2+16*vy1)*t2+(3*ay1-2*ay2)*t2^2)/(2*t2^4);
d15=(12*y2-12*y1-(6*vy2+6*vy1)*t2-(ay1-ay2)*t2^2)/(2*t2^5);

f10=z1;
f11=vz1;
f12=az1/2;
f13=(20*z2-20*z1-(8*vz2+12*vz1)*t2-(3*az1-az2)*t2^2)/(2*t2^3);
f14=(30*z1-30*z2+(14*vz2+16*vz1)*t2+(3*az1-2*az2)*t2^2)/(2*t2^4);
f15=(12*z2-12*z1-(6*vz2+6*vz1)*t2-(az1-az2)*t2^2)/(2*t2^5);
%--------------------------------------------------------------------------

t=linspace(0,t1,20);

x01=a00+a01*t+a02*t.^2+a03*t.^3+a04*t.^4;
vx01=a01+2*a02*t+3*a03*t.^2+4*a04*t.^3;
ax01=2*a02+6*a03*t+12*a04*t.^2;

y01=c00+c01*t+c02*t.^2+c03*t.^3+c04*t.^4;
vy01=c01+2*c02*t+3*c03*t.^2+4*c04*t.^3;
ay01=2*c02+6*c03*t+12*c04*t.^2;

z01=e00+e01*t+e02*t.^2+e03*t.^3+e04*t.^4;
vz01=e01+2*e02*t+3*e03*t.^2+4*e04*t.^3;
az01=2*e02+6*e03*t+12*e04*t.^2;
%--------------------------------------------------------------------------
t=linspace(0,t2,20);

x12=b10+b11*t+b12*t.^2+b13*t.^3+b14*t.^4+b15*t.^5;
vx12=b11+2*b12*t+3*b13*t.^2+4*b14*t.^3+5*b15*t.^4;
ax12=2*b12+6*b13*t+12*b14*t.^2+20*b15*t.^3;

y12=d10+d11*t+d12*t.^2+d13*t.^3+d14*t.^4+d15*t.^5;
vy12=d11+2*d12*t+3*d13*t.^2+4*d14*t.^3+5*d15*t.^4;
ay12=2*d12+6*d13*t+12*d14*t.^2+20*d15*t.^3;

z12=f10+f11*t+f12*t.^2+f13*t.^3+f14*t.^4+f15*t.^5;
vz12=f11+2*f12*t+3*f13*t.^2+4*f14*t.^3+5*f15*t.^4;
az12=2*f12+6*f13*t+12*f14*t.^2+20*f15*t.^3;
%--------------------------------------------------------------------------
ppx=[x01 x12];ppy=[y01 y12];ppz=[z01 z12];
q1=ppx;q2=ppy;q3=ppz;

vvx=[vx01 vx12];vvy=[vy01 vy12];vvz=[vz01 vz12];
vq1=vvx;vq2=vvy;vq3=vvz;

aax=[ax01 ax12];aay=[ay01 ay12];aaz=[az01 az12];
aq1=aax;aq2=aay;aq3=aaz;

www=[q1;q2;q3;vq1;vq2;vq3;aq1;aq2;aq3];


function setG(OCP,G)
    % assert
    if OCP.dim.u + OCP.dim.x < 30
        Gux = jacobian(G,[OCP.u;OCP.x]);
        hasUX = has(Gux(:),[OCP.u;OCP.x]);
        if sum(hasUX(:)) ~= 0
            error('G must be a linear function of u and x!');
        end
    end
    OCP.G = G;
    % Global variable
    global ParNMPCGlobalVariable
    G_formula = formula(OCP.G);
    [zDim,~] = size(G_formula);
    ParNMPCGlobalVariable.dim.z = zDim;
    ParNMPCGlobalVariable.solutionInitGuess.z = zeros(zDim,ParNMPCGlobalVariable.N);
    OCP.dim.z      = zDim;
    OCP.z          = sym('z',[OCP.dim.z,1]);
    if size(OCP.z,1) ~= OCP.dim.z
       OCP.z = OCP.z.';
    end
    OCP.LBarrier   = sym(0);
    %% barrier term
    for i = 1:zDim
        OCP.LBarrier = OCP.LBarrier - log(G_formula(i));
    end
    %% linear damping term
    for i = 1:zDim
        OCP.LBarrier = OCP.LBarrier + 1e-4*G_formula(i);
    end
end


function [y] = cellcrossproduct(x)

% [Y] = CELLSUBSELECT(X, BOOLVEC, DIM) outputs a cell-arry Y with the same dimension as X
% but for each input cell the n(n+1)/2 cross products of the rows are computed.
% the cross-products are ordered along the lower-triangle of the pairwise
% matrix.

nx = size(x);
if ~iscell(x) || length(nx)>2 || all(nx>1),
  error('incorrect input for cellcrossproduct');
end

n = cellfun('size', x, 1);
if ~all(n==n(1))
  error('each cell should have the same number of rows');
end
%[ix{1}(:,1),ix{1}(:,2)] = find(tril(ones(n(1))));

%y = cellfun(@crossprod, x, repmat(ix, nx), 'UniformOutput', 0);
y = cellfun(@crossprod, x, 'UniformOutput', 0);

function [y] = crossprod(x)

% FIXME only works in case of dim=2
n    = size(x);
indx = tril(ones(n(1)))==1;
y = zeros(0.5*n(1)*(n(1)+1), n(2));
for k = 1:n(2)
  tmp = tril(x(:,k)*x(:,k)');
  y(:,k) = tmp(indx);
end

% function [y] = crossprod(x, ix)
%
% %FIXME works only in case of dim=2
% n = size(x);
% y = zeros(0.5*n(1)*(n(1)+1), n(2));
% for k = 1:size(ix,1)
%   y(k,:) = x(ix(k,1),:).*x(ix(k,2),:);
% end


% TIMEFDETAILS - details of the TIMEF function for time/frequency analysis
%                  of multiple epochs of single-channel event-related data.
%
% Global Description:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% TIMEF performs normalized time/frequency averaging using either
% FFT-, wavelet-, or multitaper DFT estimates. The wavelets are N-cycle
% Hanning-windowed sinusoids. (Note: To substitute for HANNING windowing
% GAUSS or other windowing, replace the timef.m reference to HANNING).
%
% By default, the two image panels of the output plot show, respectively, the
% event-related spectral perturbation (ERSP) and inter-trial coherence (ITC)
% of the input data.
%
% The ERSP:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% The ERSP (S. Makeig, Electroencephalogr Clin Neurophysiol 86:283-93, 1993)
% shows mean log event-loced deviations from epoch-mean (or baseline-mean) power
% at each frequency. If bootstrap statistics are computed (via the 'alpha'
% probability input parameter), (time, freq) points with non-significant
% differences from 0 are colored green in the image (but not in the 'ersp'
% output variable - use the output 'erspboot' variable to re-mask the 'ersp'
% output if desired). The baseline mean spectrum removed from each epoch
% is available as output parameter "powbase". Note that log(power) differences
% are equivalent to log(power ratios) between baseline and observed spectra,
% meaning the implicit ERSP model is one of (multiplicative) amplitude modulation
% of the EEG/MEG spectrum by e.g. subcortical and/or intra-cortical influences.
%
% In the default view, the thin bottom panel below the upper (ERSP) image shows
% the ERSP envelope (the most positive and most negative values at each output
% time point). The thin left panel shows the mean (or baseline) log spectrum
% (blue trace). When bootstrap statistics are computed (via the "alpha" argument),
% the left panel (green trace) also shows the bootstrap significant levels (+/-)
% at each frequency.
%
% The ITC:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% (Inter-trial Coherence, cf. Tallon-Baudry et al., "Phase-locking factor")
% The lower panel shows the degree of tendency for spectral phase at each
% (time, freq) point to repeat across the data epochs. If bootstrap statistics
% are computed (as per the 'alpha' input parameter), non-significant points
% are colored green (but again not in the 'itc' output variable - use the
% itcboot output to re-mask if desired in later plotting).
%
% The lower thin panel shows the time-domain average (ERP) of the input data
% (blue) plus a zero-line (green). The average (ERP) is created principally by
% partial phase resetting of the EEG as measured by the ITC. (While phase resetting
% dominates, event-related spectral power changes (as measured by the ERSP)
% may also play a minor role). The thin left panel shows the frequency-mean ITC
% (blue trace) and, if bootstrap statistics are computed, the ITC significance
% limits at each frequency (green trace).
%
% ITC Math Derivation:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% By definition, linear coherence is
%       R=mean(Fxy)/sqrt(mean(abs(Fxx))*mean(abs(Fyy)));
% where Fxy is the cross-spectrum (FxFy*) and Fxx and Fyy the autospectra of
% processes x and y.  We define the phase coherence to be
%       R=mean(Fxy/(abs(Fxx))*abs(Fyy));  % mean of individually normed Fxy
% To derive the ITC, we consider y to be a stimulus-locked process
% such that, at each time and frequency, angle(Fyy) = 0 and abs(Fyy) = 1.
% Thus Pxy = Pxx, and the (complex) inter-trial phase coherence between x and
% the constant stimulus-locked process y is
%       ITC=mean(Pxx/abs(Pxx));
%
% USAGE:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%   >> [ersp,itc,powbase,times,freqs,erspboot,itcboot]  ...
%                = timef(data,frames,tlimits,srate,cycles,...
%                              'key1',value1,'key2',value2, ... );
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% NOTE:
% * Left-click on subplots to view and zoom in separate windows (uses AXCOPY).
%
% Required Inputs:
%   data        = Single-channel data vector (1,frames*ntrials) (required)
%
%   frames      = Frames per trial                        {750}
%  Here, a frame is a data point (one channel at one time point) and the data
%  is assumed to be composed of concatenated epochs of the same length (frames).
%
%   tlimits     = [mintime maxtime] (ms) Epoch time limits {[-1000 2000]}
%  These should be the starting and ending times of the input epochs.
%
%   srate       = data sampling rate (Hz)                 {250}
%  This is sample rate per channel (i.e., data frames per second).
%
%   cycles      = >0 -> Number of cycles in each analysis wavelet
%                 =0 -> Use FFTs (with constant window length) {0}
%                 If [wavecycles factor] -> wavelet cycles increase with frequency
%                 beginning at wavecyles (0<factor<1; factor=1 -> no increase,
%                 standard wavelets; factor=0 -> fixed epoch length, as in FFT.
%                 OR multitaper decomposition (with 'mtaper').
%  Here, the user chooses either to use the FFT method (fixed window size
%  for all frequencies) or the wavelet DFT method (the data window length
%  depends inversely on the frequency, e.g. '3' means that the data
%  windows will each be three cycles wide (at each frequency). A higher
%  number here (e.g., '5') will narrow the frequency band and widen the
%  time window.
%
% Optional Inter-Irial Coherence Type:
%   'type'      = ['coher'|'phasecoher'] Compute either linear coherence
%                  ('coher') or phase coherence ('phasecoher') also known
%                  as the phase coupling factor           {'phasecoher'}.
%
% Optional Detrending:
%   'detret'    = ['on'|'off'], Detrend data in time.       {'off'}
%   'detrep'    = ['on'|'off'], Detrend data across trials (at each time point
%                 compute the linear trend across the set of ordered trials and
%                 remove it; not that this also subtract the ERP) {'off'}
%
% Optional FFT/DFT Parameters:
%   'winsize'   = If cycles==0: data subwindow length (fastest is 2^n < frames);
%                 If cycles >0: *longest* window length to use. This determines
%                 the lowest output frequency  {default: ~frames/8}
%  When cycles>0, winsize determines the lowest computed frequency. For example,
%  with srate=100 and cycles=3, a winsize of 100 means that 3 cycles must fit
%  within a 1-sec ( 100-sample) window. So, the lowest output frequency is 3 Hz.
%
%  When cycles=0, winsize is the length of data in each FFT window. This may be
%  extended with zeroes (to give more output frequencies) using padratio (below).
%
%  'timesout'  = Number of output times (int<frames-winframes) {200}
%  The number of FFTs or wavelet DFTs computed and plotted.
%
%   'padratio'  = FFT-length/winframes (2^k)                    {2}
%                  Multiplies the number of output frequencies by
%                  dividing their spacing. When cycles==0, frequency
%                  spacing is (low_freq/padratio).
%  This factor multiplies the number of output frequencies. In the FFT method
%  (cycles=0), this is done by zero-padding each analysis window. In the wavelet
%  DFT method (cycles>0), this gives the number of frequencies per Hz.
%
%   'maxfreq'   = Maximum frequency (Hz) to plot (& to output, if cycles>0)
%                  If cycles==0, all FFT frequencies are output. {50}
%   'baseline'  = Spectral baseline end-time (in ms). Use NaN for no baseline
%                 removal{0}
%   'powbase'   = Baseline spectrum to log-subtract. 'baseline' parameter is
%                 ignored if this parameter is used {def|NaN->from data}
%  This is useful only when you want to use a known baseline spectrum (e.g. from
%  another condition) instead of using the actual mean baseline spectrum of the data.
%  Otherwise, leave this out or specify as 'NaN' (not a number).
%
% Optional Multitaper Parameters:
%   'mtaper'    = If [N W], performs multitaper decomposition.
%                  (N is the time resolution and W the frequency resolution;
%                  maximum taper number is 2NW-1). Overwrites 'winsize' and
%                  'padratio'.
%                  If [N W K], forces the use of K Slepian tapers (if possible).
%                  Phase is calculated using standard methods.
%                  The use of mutitaper with wavelets (cycles>0) is not
%                  recommended (as multiwavelets are not implemented).
%                  Uses Matlab functions DPSS, PMTM.   {no multitaper}
%
% Optional Bootstrap Parameters:
%   'alpha'     = If non-0, compute two-tailed bootstrap significance prob.
%                  level. Show non-signif. output values as green.   {0}
%  This optional parameter lengthens the computation time but gives bootstrap
%  estimates of which ERSP and ITC values are significantly different from 0,
%  by setting to 0 (green) all non-significant values in the ERSP and ITC images.
%  Normal values for alpha are 0 ([], or none) -> no bootstrap computation, or
%  0.01 (which should allow about 1% of random images to appear "significant"
%
%   'naccu'     = Number of bootstrap replications to accumulate     {200}
%   'baseboot'  = Bootstrap baseline subtract (0 -> use 'baseline';
%                                                  1 -> use whole trial) {0}
% Optional Scalp Map Plotting Parameters:
%   'topovec'   = Scalp topography (map) to plot                     {none}
%   'elocs'     = Electrode location file for scalp map   {no default}
%                     File should be ascii in format of  >> topoplot example
%  This is an optional map-plotting feature. Given an input map vector
%  (one weight at each channel, and a electrode location file, TIMEF plots
%  a TOPOPLOT-style 2-d scalp map on the left side of the figure. See
%  >> topoplot example % for the format of the electrode location file.
%
% Other Optional Plotting Parameters:
%   'vert'      = [vector of ms times] -> plot vertical dashed lines  {0 only}
%  Use this to add extra vertical dashed lines at significant epoch times.
%  Time 0 is marked by default.
%
%   'plotersp'  = ['on'|'off'] Plot power spectral perturbations    {'on'}
%   'plotitc'   = ['on'|'off'] Plot inter trial coherence            {'on'}
%   'title'     = Optional figure title                              {none}
%
%   'pboot'     = Bootstrap power limits (e.g., from TIMEF)   {from data}
%   'rboot'     = Bootstrap ITC limits (e.g., from TIMEF)     {from data}
%  These are useful if you want to apply significance limits from another condition
%  to new data. {default|NaN, compute from data}
%
%   'linewidth' = Line width for 'marktimes' traces (thick=2, thin=1) {2}
%   'axesfont'  = Axes text font size                                {10}
%   'titlefont' = Title text font size                               {8}
%
% Outputs:
%        ersp   = Matrix (nfreqs,timesout) of log spectral diffs. from baseline (dB)
%        itc    = Matrix (nfreqs,timesout) of inter-trial phase coherence (range: [0 1])
%   Note that when cycles=0, nfreqs is total number of FFT frequencies, which
%   typically include frequencies higher than maxfreq. When cycles>0, *no* extra
%   (higher) frequencies are computed.
%
%      powbase  = Baseline power spectrum (removed to compute the ERSP)
%        times  = Vector of output times (subwindow centers) (in ms).
%        freqs  = Vector of frequency bin centers (in Hz).
%
%     erspboot  = Matrix (2,nfreqs) of [lower;upper] ERSP significance diffs.
%      itcboot  = Matrix (2,nfreqs) of [lower;upper] ITC thresholds (abs., not diffs)
%  Note that the itcboot lower bound is practically meaningless.
%
%  Plot description:
%    Assuming both 'plotersp' and 'plotitc' options are 'on' (= default). The upper panel
%    presents the data ERSP (Event-Related Spectral Perturbation) in dB, with mean baseline
%    spectral activity (in dB) subtracted. Use "'baseline', NaN" to prevent TIMEF from
%    removing the baseline. The lower panel presents the data ITC (Inter-Trial Coherence).
%    Click on any plot axes to pop up a new window (using 'AXCOPY')
%    -- Upper left marginal panel presents the mean spectrum during the baseline period
%       (blue), and when significance is set, the significance threshold at each frequency
%       (dotted green-black trace).
%    -- The marginal panel under the ERSP image shows the maximum (green) and minimum
%       (blue) ERSP values relative to baseline power at each frequency.
%    -- The lower left marginal panel shows mean ITC across the imaged time range (blue),
%       and when significance is set, the significance threshold (dotted green-black).
%    -- The marginal panel under the ITC image shows the ERP (which is produced by ITC
%       across the data spectral pass band).
%
% Author: Sigurd Enghoff, Arnaud Delorme & Scott Makeig
%          CNL / Salk Institute 1998- | SCCN/INC, UCSD 2002-
%
% See also: CROSSF - event-related cross-spectral coherence between two input
%                      time series.
%
% History:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% TIMEF was coded by Sigurd Enghoff and Scott Makeig at The Salk
% Institute, La Jolla CA in August, 1998, using methods developed
% in Makeig, 1993. Arno Delorme added the multitaper option, recoded
% the function to use 'keyword','parameter' argument pairs, and added the
% 'type' argument with advice from Joern Anemueller at SCCN/Institute for
% Neural Computation, UCSD in early 2002.

% Copyright (C) 8/01/00 Scott Makeig, SCCN/INC/UCSD, scott@sccn.ucsd.edu
%
% This file is part of EEGLAB, see http://www.eeglab.org
% for the documentation and details.
%
% 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.
%
% 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 COPYRIGHT HOLDER 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.

% 01-25-02 reformated help & license -ad


function asa266_test06 ( )

%*****************************************************************************80
%
%% TEST06 tests GAMAIN, GAMMDS, GAMMAD.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 August 2010
%
%  Author:
%
%    John Burkardt
%
  ntest = 10;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST06\n' );
  fprintf ( 1, '  GAMAIN,\n' );
  fprintf ( 1, '  GAMMDS and \n' );
  fprintf ( 1, '  GAMMAD compute the incomplete Gamma integral.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  X  P  GAMMDS  GAMMAD  GAMAIN\n' );
  fprintf ( 1, '\n' );

  for i = 1 : ntest

    x = i / ntest;

    fprintf ( 1, '\n' );

    for j = 1 : ntest

      p = j / ntest;
      [ g1, ifault ] = gammds ( x, p );
      if ( ifault ~= 0 )
        g1 = -99.0;
      end

      [ g2, ifault ] = gammad ( x, p );
      if ( ifault ~= 0 )
        g2 = -99.0;
      end

      [ g3, ifault ] = gamain ( x, p );
      if ( ifault ~= 0 )
        g3 = - 99.0;
      end

      fprintf ( 1, '  %12f  %12f  %12f  %12f  %12f\n', x, p, g1, g2, g3 );

    end

  end

  return
end


function [gradient, delta] = gradchek(w, func, grad, varargin)
%GRADCHEK Checks a user-defined gradient function using finite differences.
%
%	Description
%	This function is intended as a utility for other netlab functions
%	(particularly optimisation functions) to use.  It enables the user to
%	check whether a gradient calculation has been correctly implmented
%	for a given function. GRADCHEK(W, FUNC, GRAD) checks how accurate the
%	gradient  GRAD of a function FUNC is at a parameter vector X.   A
%	central difference formula with step size 1.0e-6 is used, and the
%	results for both gradient function and finite difference
%	approximation are printed. The optional return value GRADIENT is the
%	gradient calculated using the function GRAD and the return value
%	DELTA is the difference between the functional and finite difference
%	methods of calculating the graident.
%
%	GRADCHEK(X, FUNC, GRAD, P1, P2, ...) allows additional arguments to
%	be passed to FUNC and GRAD.
%
%	See also
%	CONJGRAD, GRADDESC, HMC, OLGD, QUASINEW, SCG
%

%	Copyright (c) Ian T Nabney (1996-2001)

% Reasonable value for step size
epsilon = 1.0e-6;

func = fcnchk(func, length(varargin));
grad = fcnchk(grad, length(varargin));

% Treat
nparams = length(w);
deltaf = zeros(1, nparams);
step = zeros(1, nparams);
for i = 1:nparams
  % Move a small way in the ith coordinate of w
  step(i) = 1.0;
  fplus  = feval('linef', epsilon, func, w, step, varargin{:});
  fminus = feval('linef', -epsilon, func, w, step, varargin{:});
  % Use central difference formula for approximation
  deltaf(i) = 0.5*(fplus - fminus)/epsilon;
  step(i) = 0.0;
end
gradient = feval(grad, w, varargin{:});
fprintf(1, 'Checking gradient ...\n\n');
delta = gradient - deltaf;
fprintf(1, '   analytic   diffs     delta\n\n');
disp([gradient', deltaf', delta'])


%%  kmeans algorithm for an image
%---input---------------------------------------------------------
%   Y: 3D image
%   k: number of clusters
%   g: number of GMM components
%---output--------------------------------------------------------
%   X: 3D labels
%   GMM: Gaussian mixture model parameters

function [X GMM]=image_kmeans(Y,k,g)
[m n l]=size(Y);
y=Y(:);
x=kmeans(y,k);
X=reshape(x,[m n l]);

GMM=get_GMM(X,Y,g);


%features
% load 'facefeature_lw6lr1_512.mat'
% features1=features;

%mirror feature
% load 'Mfacefeature_lw6lr1_512.mat'
% features=[features,features1];

tgt_method='facefeature_512'
tgtDir='./LFWTest/';

load pair_lfw
features=double(feature)';
features= bsxfun(@rdivide, features, sqrt(sum(features.^2,2)));

image_path = list;

for i=1:10
	intra_pairs=pair_lfw.IntraPersonPair{i};
	extra_pairs=pair_lfw.ExtraPersonPair{i};

        train_features=features;
        train_features([intra_pairs(:);extra_pairs(:)],:)=[];
	[coeff,~,latent] = pca(double(train_features)');
        m_feature=mean(double(train_features));
        accuracies = zeros(10,1);

%test the result with the range of PCA components
%	for numDim=64:64:1024
    for numDim=256:256
    %	for numDim=79:104

        test_features1=double(features([intra_pairs(:,1);extra_pairs(:,1)],:));
        test_features2=double(features([intra_pairs(:,2);extra_pairs(:,2)],:));

        test_features1=(test_features1-ones([size(test_features1,1) 1])*m_feature)*coeff(:,1:numDim);
        test_features2=(test_features2-ones([size(test_features2,1) 1])*m_feature)*coeff(:,1:numDim);

        test_features1= bsxfun(@rdivide, test_features1, sqrt(sum(test_features1.^2,2)));
        test_features2= bsxfun(@rdivide, test_features2, sqrt(sum(test_features2.^2,2)));

        scores=diag(test_features1*test_features2');

        test_path1=image_path([intra_pairs(:,1);extra_pairs(:,1)]);
        test_path2=image_path([intra_pairs(:,2);extra_pairs(:,2)]);
        [tgtDir, '/',tgt_method, '_', num2str(numDim),  '/TR',num2str(i)]
        mkdir([tgtDir, '/',tgt_method, '_', num2str(numDim)]);
        mkdir([tgtDir, '/',tgt_method, '_', num2str(numDim),  '/TR',num2str(i)]);

        fid=fopen([tgtDir, '/' ,tgt_method, '_', num2str(numDim), '/TR',num2str(i),'/NCMNCM52evaC.dat'],'w');
        for j=1:length(scores)
            idx=0;
            if ~isempty(strmatch(test_path1{j}(1:end-9),test_path2{j}(1:end-9)))
                idx=1;
            end
            fprintf(fid,'%s\t%s\t1\t%d\t%f\n',test_path1{j}(1:end-4),test_path2{j}(1:end-4),idx,-scores(j));
        end
        fclose(fid);
        accuracy = Match_measure([tgtDir, '/' ,tgt_method, '_', num2str(numDim), '/TR',num2str(i),'/NCMNCM52evaC.dat'])
        accuracies(i,1) = accuracy;
    end
end
mean(accuracy)


classdef LossSmoothL1 < dagnn.Loss
%LossSmoothL1  Smooth L1 loss
%  `LossSmoothL1.forward({x, x0, w})` computes the smooth L1 distance
%  between `x` and `x0`, weighting the elements by `w`.
%
%  Here the smooth L1 loss between two vectors is defined as:
%
%     Loss = sum_i f(x_i - x0_i) w_i.
%
%  where f is the function (following the Faster R-CNN definition):
%
%              { 0.5 * sigma^2 * delta^2,         if |delta| < 1 / sigma^2,
%   f(delta) = {
%              { |delta| - 0.5 / sigma^2,         otherwise.
%
%  In practice, `x` and `x0` can pack multiple instances as 1 x 1 x C
%  x N arrays (or simply C x N arrays).

  properties
    sigma = 1.
  end

  methods
    function outputs = forward(obj, inputs, params)
      sigma2 = obj.sigma^2 ;
      delta = inputs{1} - inputs{2} ;
      absDelta = abs(delta) ;

      linearRegion = (absDelta > 1. / sigma2) ;
      absDelta(linearRegion) = absDelta(linearRegion) - 0.5/sigma2 ;
      absDelta(~linearRegion) = 0.5 * sigma2 * absDelta(~linearRegion).^2 ;

      % Mutliply by instance weights and sum.
      outputs{1} = inputs{3}(:)' * absDelta(:) ;

      % Accumulate loss statistics.
      n = obj.numAveraged ;
      m = n + gather(sum(inputs{3}(:))) + 1e-9 ;
      obj.average = (n * obj.average + gather(outputs{1})) / m ;
      obj.numAveraged = m ;
    end

    function [derInputs, derParams] = backward(obj, inputs, params, derOutputs)
    % Function derivative:
    %
    %          { sigma^2 * x,             if |x| < 1 / sigma^2,
    %  f'(x) = {
    %          { sign(x),                 otherwise.

      sigma2 = obj.sigma^2 ;
      delta = inputs{1} - inputs{2} ;
      absDelta = abs(delta) ;

      linearRegion = (absDelta > 1. / sigma2) ;
      delta(linearRegion) = sign(delta(linearRegion));
      delta(~linearRegion) = sigma2 * delta(~linearRegion) ;

      derInputs = {inputs{3} .* delta .* derOutputs{1}, [], []} ;
      derParams = {} ;
    end

    function obj = LossSmoothL1(varargin)
      obj.load(varargin) ;
      obj.loss = 'smoothl1';
    end
  end
end


function [row_sum,col_sum]=calc_sum(Image,rowsize,z)
col_sum=sum(Image)'/z;
for i=1:rowsize,
row_sum(i)=sum(Image(i,:))/z;
end


%MDL_TWOLINK_MDH Create model of a 2-link mechanism using modified DH convention
%
% MDL_TWOLINK_MDH is a script that the workspace variable tl which
% describes the kinematic and dynamic characteristics of a simple planar
% 2-link mechanism using modified Denavit-Hartenberg conventions.
%
% Also defines the vector:
%   qz   corresponds to the zero joint angle configuration.
%
% Notes::
% - SI units of metres are used.
% - It is a planar mechanism operating in the XY (horizontal) plane and is
%   therefore not affected by gravity.
%
% References::
%  - Based on Fig 3.8 (p71) of Craig (3rd edition).
%
% See also SerialLink, mdl_onelink, mdl_twolink, mdl_planar2.

% MODEL: generic, planar, 2DOF, modified_DH

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

a1 = 1;
a2 = 1;

% for MDH parameters we need to implement the second link as a tool
% transform

twolink = SerialLink([
        RevoluteMDH('d', 0, 'a', 0,  'alpha', 0)
        RevoluteMDH('d', 0, 'a', a1, 'alpha', 0)
    ], ...
    'tool', transl(a2, 0, 0), ...
    'name', 'two link', ...
    'comment', 'from Craig');
qz = [0 0];
qn = [pi/6, -pi/6];


function plot_mls1p(vPar, vMlsprob, nType)
    % Plot maximum likelihod score (MLS) versus parameter variation
    %  plot_mls1p(vPar, vMlsprob, nType)
    % -----------------------------------------------
    % Plot maximum likelihod score (MLS) versus parameter variation
    % vPar is e.g. a shift, a stretch or a rate change
    %
    % Incoming variable
    % vPar          : Vector of parameter used in search
    % vMlsprob      : Maximum likelihood score for parameter
    % nType    : Parameter identification
    %            1 = dM (Simple shift)
    %            2 = dS (Simple stretch)
    %            3 = Rf (Rate factor)
    %
    % See also plot_mls2p, plot_mls3p
    %
    % Author: J. Woessner, woessner@seismo.ifg,.ethz.ch
    % updated: 28.10.02

    switch nType
        case 1
            sTitle = 'Simple magnitude shift';
            sX = 'dM';
        case 2
            sTitle = 'Simple stretch';
            sX = 'dS';
        case 3
            sTitle = 'Rate factor';
            sX = 'R_f';
        otherwise
            return;
    end
    if exist('mls2_fig','var') &  ishandle(mls2_fig)
        set(0,'Currentfigure',mls2_fig);
    else
        mls2_fig=figure_w_normalized_uicontrolunits('tag','mls2','Name','Max. Likelikehood score','Units','normalized','Nextplot','add',...
            'Numbertitle','off');
        mls2_axs=axes('tag','ax_mls2','Nextplot','add','box','off');
    end

    set(gcf,'tag','mls2');
    set(gca,'tag','ax_mls2','Nextplot','replace','box','off','visible','off');
    plot(vPar, log10(vMlsprob), 'r-^');
    xlabel(sX);
    ylabel('log10(Likelihood score)');
    title(sTitle);
end


function [T, score_mat] = learn_struct_mwst(data, discrete, node_sizes, node_type, scoring_fn, root)
% LEARN_STRUCT_MWST Learn an oriented tree using the MSWT algorithm
% T = learn_struct_mwst(data, discrete, node_sizes, node_type, scoring_fn, root)
%
% Input :
%   data(i,m) is the node i in the case m,
%   discrete = [ 1 if discret-node 0 if not ],
%   node_sizes = 1 if gaussian node,
%   node_type = {'tabular','gaussian',...},
%   score = 'bic' (for complete data and any node types) or 'mutual_info' (tabular nodes),
%   root is the futur root-node of the tree T.
%
% Output :
%	T = adjacency matrix of the tree
%
% V1.2 : 17 feb 2003 (O. Francois - francois.olivier.c.h@gmail.com, Ph. Leray - philippe.leray@univ-nantes.fr)
%
%
% See Chow&Liu 1968 for the original algorithm using Mutual Information scoring.
% Or Heckerman 1994.

if nargin <4
    error('Requires at least 4 arguments.')
end

if nargin == 4
    scoring_fn='bic'; root=1;
end;

if nargin == 5
    root=1;
end;

N=size(data,1);
score_mat=zeros(N,N);

switch scoring_fn
case 'bic',
    for i=1:(N-1)
            score2 = score_family(i, [], node_type{i}, scoring_fn, node_sizes, discrete, data,[]);
        for j=(i+1):N
            score1 = score_family(i, [j], node_type{i}, scoring_fn, node_sizes, discrete, data,[]);
            score = score2-score1;
            score_mat(i,j)=score;
            score_mat(j,i)=score;
        end
    end
case 'mutual_info',
    for i=1:(N-1)
        for j=(i+1):N
            score_mat(i,j)= -mutual_info_score(i,node_sizes(i),j,node_sizes(j),data);
            score_mat(j,i)=score_mat(i,j);
        end
    end
otherwise,
    error(['unrecognized scoring fn ' scoring_fn]);
end

G = minimum_spanning_tree(score_mat);
T = mk_rooted_tree(G, root);
T=full(T);


% Digital circuit sizing for an inverter chain (GP)
% Boyd, Kim, Patil, and Horowitz, "Digital circuit optimization
% via geometric programming"
% Written for CVX by Almir Mutapcic 02/08/06
%
% We consider a chain of N inverters driving a load capacitance CL.
% The problem is to find optimal scale factors for the inverter
% that minimize the sum of them (area), while obeying constraints
% on the maximum delay through the circuit, and minimum and maximum
% limits on scale factors. There are no limits on the total power.
% (For more details about the inverter chain see sec. 2.1.11 in the paper.)
%
%   minimize   sum(x)
%       s.t.   T_j <= Dmax          for j an output gate
%              T_j + d_i <= T_i     for j in FI(i)
%              x_min <= x <= x_max
%
% where variables are x and T.
% Here we use data structures and digital circuit models from the
% referenced paper.

%********************************************************************
% problem data
%********************************************************************
N  = 8;      % number of inverters
CL = 20;     % capacitance load
Dmax = 20;   % maximum delay through the circuit
x_min = 1;   % minimum scale factor
x_max = 20;  % maximum scale factor

% circuit labeling convention:
% label primary input (input to the first inverter in the chain) with N+1
% label primary output (output of the last inverter in the chain) with N+2
% label inverters in the chain with 1,2,...,N based on their location

% primary input and primary output labels (start with N+1)
primary_inputs  = [N+1];
primary_outputs = [N+2];
M = N + length( primary_inputs ) + length( primary_outputs );

% fan-in cell array for a straight chain of inverters
FI{1} = [N+1];   % fan-in of the first inverter is the primary input
for k = 2:N
  FI{k} = [k-1]; % fan-in of other inverters is the inverter feeding into them
end
FI{N+2} = [N];   % fan-in of the primary output is the last inverter in the chain

% fan-out cell array
% (will be computed from the fan-in cell array, no need to modify)
FO = cell(M,1);
for gate = [1:N primary_outputs]
  preds = FI{gate};
  for k = 1:length(preds)
    FO{preds(k)}(end+1) = gate;
  end
end

% input and internal capacitance of gates and the driving resistance
Cin_norm  = ones(N,1);
Cint_norm = ones(N,1);
Rdrv_norm = ones(N,1);

% place extra capacitance before the input of the 5th inverter
Cin_norm(5) = 80;

% primary output has Cin capacitance (but has no Cload)
Cin_po = sparse(M,1);
Cin_po(primary_outputs) = CL;

% primary input has Cload capacitance (but has no Cin)
Cload_pi = sparse(M,1);
Cload_pi(primary_inputs) = 1;

%********************************************************************
% optimization
%********************************************************************
cvx_begin gp
  % optimization variables
  variable x(N)                 % sizes
  variable T(N)                 % arrival times

  % minimize the sum of scale factors subject to above constraints
  minimize( sum(x) )
  subject to

    % input capacitance is an affine function of sizes
    Cin  = Cin_norm.*x;
    Cint = Cint_norm.*x;

    % driving resistance is inversily proportional to sizes
    R = Rdrv_norm./x;

    % gate delay is the product of its driving resistance and load cap.
    Cload = cvx( zeros(N,1) );
    for gate = 1:N
      if ~ismember( FO{gate}, primary_outputs )
        Cload(gate) = sum( Cin(FO{gate}) );
      else
        Cload(gate) = Cin_po( FO{gate} );
      end
    end

    % delay
    D = 0.69*ones(N,1).*R.*( Cint + Cload );

    % create timing constraints
    for gate = 1:N
      if ~ismember( FI{gate}, primary_inputs )
        for j = FI{gate}
          % enforce T_j + D_j <= T_i over all gates j that drive i
          D(gate) + T(j) <= T(gate);
        end
      else
        % enforce D_i <= T_i for gates i connected to primary inputs
        D(gate) <= T(gate);
      end
    end

    % circuit delay is the max of arrival times for output gates
    output_gates = [FI{primary_outputs}];
    circuit_delay = max( T(output_gates) );

    % collect all the constraints
    circuit_delay <= Dmax;
    x_min <= x <= x_max;
cvx_end

% message about extra capacitance and result display
disp(' ')
disp(['Note: there is an extra capacitance between the 4th and 5th inverter'...
     ' in the chain.'])
fprintf(1,'\nOptimal scale factors are: \n'), x

% plot scale factors and maximum delay for inverter i
close all;
subplot(2,1,1); plot([1:N],T,'g--',[1:N],T,'bo');
ylabel('maximum delay T')
subplot(2,1,2); stem([1:N],x);
ylabel('scale factor x')
xlabel('inverter stage')


classdef VehicleSimpleLinear < VehicleDynamicsLateral.VehicleSimple
    % VehicleSimpleLinear Linear simple vehicle model.
    %
    % It inherits properties from VehicleSimple.

    methods
        function self = VehicleSimpleLinear()
            % Constructor for the vehicle
            self.mF0 = 700;
            self.mR0 = 600;
            self.IT = 10000;
            self.lT = 3.5;
            self.nF = 2;
            self.nR = 2;
            self.wT = 2;
            self.muy = .8;
            self.deltaf = 0;
            self.Fxf = 0;
            self.Fxr = 0;
        end

        %% Model
        % Function with the model
        function dx = Model(self, t, states,tspan)
            % Data
            mT = self.mT;
            IT = self.IT;
            a = self.a;
            b = self.b;
            nF = self.nF;
            nR = self.nR;
            muy = self.muy;

            g = 9.81;                 % Gravity [m/s^2]

            FzF = self.mF0 * g;       % Vertical load @ F [N]
            FzR = self.mR0 * g;       % Vertical load @ R [N]

            v0 = 20;                  % [m/s]

            % State variables
            X = states(1,1);         % Not used
            Y = states(2,1);         % Not used
            PSI     = states(3,1);
            VT       = states(4,1);
            ALPHAT  = states(5,1);
            dPSI    = states(6,1);

            if isa(self.deltaf,'function_handle')
                deltaf = self.deltaf([X;Y;PSI;VT;ALPHAT;dPSI],t);
            elseif length(self.deltaf)>1
                deltaf = interp1(tspan,self.deltaf,t);
            else
                deltaf = self.deltaf;
            end

            % Slip angles
            ALPHAF = ALPHAT + a/v0*dPSI - deltaf;
            ALPHAR = ALPHAT - b/v0*dPSI;

            % Longitudinal forces
            if isa(self.Fxf,'function_handle')
                FxF = self.Fxf([X;Y;PSI;VT;ALPHAT;dPSI],t);
            elseif length(self.Fxf)>1
                FxF = interp1(tspan,self.Fxf,t);
            else
                FxF = self.Fxf;
            end

            if isa(self.Fxr,'function_handle')
                FxR = self.Fxr([X;Y;PSI;VT;ALPHAT;dPSI],t);
            elseif length(self.Fxr)>1
                FxR = interp1(tspan,self.Fxr,t);
            else
                FxR = self.Fxr;
            end

            % Lateral force
            FyF = nF * self.tire.Characteristic(ALPHAF, FzF / nF, muy);
            FyR = nR * self.tire.Characteristic(ALPHAR, FzR / nR, muy);

            % State equations
            dx(1,1) = VT;
            dx(2,1) = v0*(PSI + ALPHAT);
            dx(3,1) = dPSI;
            dx(4,1) = (FxF + FxR)/mT;
            dx(5,1) = (FyF + FyR)/(mT*v0) - dPSI;
            dx(6,1) = (a*FyF - b*FyR)/IT;

        end
    end
end

%% See Also
%
% <../../index.html Home>
%


function [t,x] = unrz(bits, bitrate)
% UNRZ Encode bit string using unipolar NRZ code.
%   [T, X] = UNRZ(BITS, BITRATE) encodes BITS array using unipolar NRZ
%   code with given BITRATE. Outputs are time T and encoded signal
%   values X.

% Copyright (c) 2013 Yuriy Skalko <yuriy.skalko@gmail.com>

T = length(bits)/bitrate; % full time of bit sequence
n = 200;
N = n*length(bits);
dt = T/N;
t = 0:dt:T;
x = zeros(1,length(t)); % output signal

for i = 0:length(bits)-1
  if bits(i+1) == 1
    x(i*n+1:(i+1)*n) = 1;
  else
    x(i*n+1:(i+1)*n) = 0;
  end
end


function test_optimization_test03 ( )

%*****************************************************************************80
%
%% TEST_OPTIMIZATION_TEST03 tries Compass Search on each problem.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 January 2012
%
%  Author:
%
%    John Burkardt
%
  m = 2;
  n = 1000;
  delta_tol = 0.000001;
  k_max = 20000;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST_OPTIMIZATION_TEST03\n' );
  fprintf ( 1, '  For each problem, using dimension M = 2\n' );
  fprintf ( 1, '  try compass search.\n' );
%
%  Get the number of problems.
%
  problem_num = p00_problem_num ( );

  for problem = 1 : problem_num

    seed = 123456789;

    [ a, b ] = p00_ab ( problem, m );
    [ x0, seed ] = r8col_uniform ( m, 1, a, b, seed );
    fx = p00_f ( problem, m, 1, x0 );
    delta_init = 0.3 * norm ( x0 ) / m;
    delta_init = max ( delta_init, 1000.0 * delta_tol );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Problem %2d  DELTA_INIT = %14g\n', problem, delta_init );
    fprintf ( 1, '  Initial:  %14g  %14g  %14g\n', x0, fx );
    [ x, fx, k ] = p00_compass_search ( problem, m, x0, delta_tol, delta_init, ...
      k_max );
    fprintf ( 1, '  Final:    %14g  %14g  %14g  Steps = %d\n', x, fx, k );

    know = 0;
    while ( 1 )
      [ know, x ] = p00_sol ( problem, m, know );
      if ( know == 0 )
        break
      end
      fx = p00_f ( problem, m, 1, x );
      fprintf ( 1, '  Exact:    %14g  %14g  %14g\n', x, fx );
    end

  end

  return
end


function ranks=cosmo_tiedrank(data, dim)
% Compute ranks for the input along the specified dimension
%
% ranks=cosmo_tiedrank(data[, dim])
%
% Inputs:
%   data                        numeric N-dimensional array
%   dim                         optional dimension along which the ranks
%                               are computed (default: 1)
%
% Output:
%   ranks                       numeric N-dimensional array with the same
%                               size as the input containing the rank of
%                               each vector along the dim-th dimension.
%                               Equal values have the same rank, which is
%                               the average of the rank the values would
%                               have if they differed by a minimal amount.
%                               NaN values in the input result in a NaN
%                               values in the output at the corresponding
%                               locations.
%                               If dim is greater than the number of
%                               dimensions in data, then all values in rank
%                               are one (or NaN of the corresponding value
%                               in data is NaN).
%
% Examples:
%     cosmo_tiedrank([1 2 2],2)
%     %|| [ 1 2.5 2.5]
%
%     cosmo_tiedrank([NaN 2 2;3 NaN 4],1)
%     %|| [ NaN     1     1;
%     %||     1   NaN     2];
%
%     cosmo_tiedrank([NaN 2 2;3 NaN 4],2)
%     %|| [ NaN   1.5   1.5;
%     %||     1   NaN     2];
%
%     cosmo_tiedrank([2 4 3 3 3 3 5 5 5],2)
%     %|| [ 1.0 6.0 3.5 3.5 3.5 3.5 8.0 8.0 8.0 ]
%
% Notes:
% - Unlike the Matlab builtin function 'tiedrank' (part of the statistics
%   toolbox), the meaning of the second argument is the dimension along
%   which the ranks are computed.
%
% #   For CoSMoMVPA's copyright information and license terms,   #
% #   see the COPYING file distributed with CoSMoMVPA.           #

    if nargin<2
        dim=1;
    end

    check_inputs(data, dim);
    orig_size=size(data);

    if numel(orig_size)<dim
        % if the input data does not have enough data, then the output
        % consists of an array with only ones (or NaNs, if present)
        ranks=singleton_ranks(data);
        return;
    end

    [values,idx]=sort(data,dim);

    data_is_vector=numel(orig_size)<=2 && orig_size(3-dim)==1;
    if data_is_vector
        ranks=vector_tied_rank(values(:), idx(:));
        if orig_size(1)==1
            % transpose to turn it back into a row vector
            ranks=ranks';
        end
        return
    end

    % make the dim-th dimension the first dimension
    values_sh=shiftdim(values,dim-1);
    idx_sh=shiftdim(idx,dim-1);
    sh_size=size(values_sh);

    count_along_dim=size(values_sh,1);

    % reshape into a matrix
    values_mat=reshape(values_sh,count_along_dim,[]);
    idx_mat=reshape(idx_sh,count_along_dim,[]);

    % space for output
    ranks_mat=zeros(size(idx_mat));

    % compute for each column vector
    n_col=size(ranks_mat,2);
    for k=1:n_col
        ranks_mat(:,k)=vector_tied_rank(values_mat(:,k),idx_mat(:,k));
    end

    % put back in shape after shiftdim
    ranks_sh=reshape(ranks_mat,sh_size);

    % undo shiftdim
    unshift_count=numel(orig_size)-dim+1;
    ranks=reshape(shiftdim(ranks_sh,unshift_count),orig_size);

function ranks=vector_tied_rank(sorted_values, sort_idx)
% sorted_values and sort_idx are the output from 'sort'
% it is assumes that sorted_values is a vector
    n_values=numel(sorted_values);
    nan_msk=isnan(sorted_values);
    nan_count=sum(nan_msk);
    non_nan_count=numel(sorted_values)-nan_count;

    % first set ranks for values without ties
    ranks=sort_idx+NaN;
    ranks(sort_idx(1:non_nan_count))=1:non_nan_count;

    % now deal with ties
    tie_msk=sorted_values(2:end)==sorted_values(1:(end-1));
    tie_idx=find(tie_msk);
    tie_count=numel(tie_idx);

    k=0;
    while k<tie_count
        k=k+1;

        tie_start=tie_idx(k);
        tie_end=tie_start+1;

        while tie_end<n_values ...
                && sorted_values(tie_end)==sorted_values(tie_end+1)
            tie_end=tie_end+1;
            k=k+1;
        end

        tie_value=(tie_start+tie_end)/2;
        pos=tie_start+(0:(tie_end-tie_start));
        ranks(sort_idx(pos))=tie_value;
    end

function ranks=singleton_ranks(data)
    % all ranks are either NaN or 1
    ranks=ones(size(data));
    ranks(isnan(data))=NaN;

function check_inputs(data, dim)
    if ~isnumeric(data)
        error('First input must be numeric')
    end

    if ~(isnumeric(dim) ...
            && isscalar(dim) ...
            && round(dim)==dim ...
            && dim>0)
        error('Second argument must be numeric integer');
    end


% Test code for irntv.m
%
% Legal:
%   irnTest.m is part of NUMIPAD (http://numipad.sf.net).
%
%   The NUMIPAD library is being developed under U.S. Government contract
%   W-7405-ENG-36 for Los Alamos National Laboratory.
%
% Authors
%   Paul Rodriguez    prodrig@pucp.edu.pe
%   Brendt Wohlberg   brendt@tmail.lanl.gov

%  example = 'l1deconv';
%  example = 'l2deconv';
%  example = 'l1denoise';
example = 'l2denoise';

%  Img = 'lena';
%  Img = 'peppers';
%  Img = 'goldhill';
Img = 'lena';
%  nmppath;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%       Input images        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

switch lower(Img)

  case{'lena'}
    Ig = double( imread('lena_gray_512.png') ) / 255;
    Ic = double( imread('lena_color_512.png') ) / 255;

  case{'peppers'}
    Ig = double( imread('peppers_gray.png') ) / 255;
    Ic = double( imread('peppers_color.png') ) / 255;

  case{'goldhill'}
    Ig = double( imread('goldhill_gray.png') ) / 255;
    Ic = double( imread('goldhill_color.png') ) / 255;

  case{'none'}
    disp(' ');
    disp('Select a test image (see code for an example)...');
    disp(' ');
    disp('Exiting irnTest code...');
    return;

  otherwise
    error('Not a valid image\n');

end

%%%%%%%%%%%%%%%%%%%%%%%%
%     Normalize        %
%%%%%%%%%%%%%%%%%%%%%%%%

Normalize = @(x) (x - min(x(:)))/(max(x(:)) - min(x(:)));

%%%%%%%%%%%%%%%%%%%%%%%%
%       kernels        %
%%%%%%%%%%%%%%%%%%%%%%%%

kernel = fspecial('disk',3.2);

K = @(x) imfilter(x, kernel, 'symmetric','conv');
KT = @(x) K(x);
KC = {K, KT};

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%       Blurred & noisy images        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

switch lower(example)

  case{'l1deconv'}
    IgBlur = K(Ig);
    Ig_01L1 = imnoise(IgBlur, 'salt & pepper', 0.1);
    Ig_03L1 = imnoise(IgBlur, 'salt & pepper', 0.3);

    IcBlur = K(Ic);
    Ic_01L1 = imnoise(IcBlur, 'salt & pepper', 0.1);
    Ic_03L1 = imnoise(IcBlur, 'salt & pepper', 0.3);

  case{'l1denoise'}

    Ig_01L1 = imnoise(Ig, 'salt & pepper', 0.1);
    Ig_03L1 = imnoise(Ig, 'salt & pepper', 0.3);

    Ic_01L1 = imnoise(Ic, 'salt & pepper', 0.1);
    Ic_03L1 = imnoise(Ic, 'salt & pepper', 0.3);

  case{'l2deconv'}

    IgBlur = K(Ig);
                                             %NOTE sigma^2 in imnoise
    Ig_01L2 = imnoise(IgBlur, 'gaussian', 0, 0.01*max(IgBlur(:)) );
    Ig_001L2 = imnoise(IgBlur,'gaussian', 0, 0.0001*max(IgBlur(:)) );

    IcBlur = K(Ic);
    Ic_01L2 = imnoise(IcBlur,'gaussian', 0, 0.01*max(IgBlur(:)) );
    Ic_001L2 = imnoise(IcBlur,'gaussian', 0, 0.0001*max(IgBlur(:)) );

  case{'l2denoise'}
    Ig_01L2 = imnoise(Ig,'gaussian', 0, 0.01*max(Ig(:)) );
    Ic_01L2 = imnoise(Ic,'gaussian', 0, 0.01*max(Ig(:)) );

    Ig_005L2 = imnoise(Ig,'gaussian', 0, 0.0025*max(Ig(:)) );
    Ic_005L2 = imnoise(Ic,'gaussian', 0, 0.0025*max(Ig(:)) );

end

%-----------------------------------------------------------------------------

if( strcmp(example,'l1deconv') || strcmp(example,'all') )

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%       L1 Deconvolved        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%noise level: 0.1

lambda = 0.45;

pars = irntvInputPars('l1tv');

%  pars.pcgtol_ini   = 1e-4;
pars.adapt_epsR   = 1;
pars.epsR_cutoff  = 0.01;
pars.adapt_epsF   = 1;
pars.epsF_cutoff  = 0.05;
pars.loops        = 4;

%-----------------%
% -- Grayscale -- %

pars.U0           = Ig_01L1;  % initial solution

% >> Deconvolution via IRN <<
IRN_Ig_01L1 = irntv(Ig_01L1, KC, lambda, pars);

figure; imagesc( Normalize(IRN_Ig_01L1) );
colormap gray; axis image; axis off;
title(sprintf('Deconvolved Image - Scalar IRN. \nSNR: %4.1fdB (noisy SNR: %4.1fdB).\n ', ...
               snr(Ig, IRN_Ig_01L1), snr(Ig, Ig_01L1)));

%-------------%
% -- Color -- %

pars.U0           = Ic_01L1;  % initial solution

% >> Deconvolution via IRN <<
IRN_Ic_01L1 = irntv(Ic_01L1, KC, lambda, pars);

figure; imagesc( Normalize(IRN_Ic_01L1) ); axis image; axis off;
title(sprintf('Deconvolved Image - Vector IRN. \nSNR: %4.1fdB (noisy SNR: %4.1fdB).\n ', ...
               snr(Ic, IRN_Ic_01L1), snr(Ic, Ic_01L1)));

%------------------
%------------------

%noise level: 0.3

lambda = 0.95;

pars = irntvInputPars('l1tv');

%  pars.pcgtol_ini   = 1e-4;
pars.adapt_epsR   = 1;
pars.epsR_cutoff  = 0.01;
pars.adapt_epsF   = 1;
pars.epsF_cutoff  = 0.05;
pars.loops        = 4;

%-----------------%
% -- Grayscale -- %

pars.U0           = Ig_03L1;  % initial solution

% >> Deconvolution via IRN <<
IRN_Ig_03L1 = irntv(Ig_03L1, KC, lambda, pars);

figure; imagesc( Normalize(IRN_Ig_03L1) );
colormap gray; axis image; axis off;
title(sprintf('Deconvolved Image - Vector IRN. \nSNR: %4.1fdB (noisy SNR: %4.1fdB).\n', ...
               snr(Ig, IRN_Ig_03L1), snr(Ig, Ig_03L1)));

%-------------%
% -- Color -- %

pars.U0           = Ic_03L1;  % initial solution

% >> Deconvolution via IRN <<
IRN_Ic_03L1 = irntv(Ic_03L1, KC, lambda, pars);

figure; imagesc( Normalize(IRN_Ic_03L1) ); axis image; axis off;
title(sprintf('Deconvolved Image - Vector IRN. \nSNR: %4.1fdB (noisy SNR: %4.1fdB). \n', ...
               snr(Ic, IRN_Ic_03L1), snr(Ic, Ic_03L1)));

end % _END_ if( strcmp(example,'l1deconv') || strcmp(example,'all') )

%-----------------------------------------------------------------------------

if( strcmp(example,'l2deconv') || strcmp(example,'all') )

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%       L2 Deconvolved        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% sigma (noise level) : 0.01

lambda  = 0.001;

pars = irntvInputPars('l2tv');

pars.pcgtol_ini   = 1e-4;
pars.adapt_epsR   = 1;
pars.epsR_cutoff  = 0.01;
pars.adapt_epsF   = 1;
pars.epsF_cutoff  = 0.05;
pars.loops        = 3;

%-----------------%
% -- Grayscale -- %

pars.U0           = Ig_001L2;  % initial solution

% >> Deconvolution via IRN <<
IRN_Ig_001L2 = irntv(Ig_001L2, KC, lambda, pars);

figure; imagesc( Normalize(IRN_Ig_001L2) );
colormap gray; axis image; axis off;
title(sprintf('Deconvolved Image - Vector IRN. \nSNR: %4.1fdB (noisy SNR: %4.1fdB).\n ', ...
               snr(Ig, IRN_Ig_001L2), snr(Ig, Ig_001L2)));

%-------------%
% -- Color -- %

pars.U0           = Ic_001L2;  % initial solution

% >> Deconvolution via IRN <<
IRN_Ic_001L2 = irntv(Ic_001L2, KC, lambda, pars);

figure; imagesc( Normalize(IRN_Ic_001L2) ); axis image; axis off;
title(sprintf('Deconvolved Image - Vector IRN. \nSNR: %4.1fdB (noisy SNR: %4.1fdB).\n ', ...
               snr(Ic, IRN_Ic_001L2), snr(Ic, Ic_001L2)));

% sigma (noise level) : 0.1

lambda  = 0.05;

pars = irntvInputPars('l2tv');

pars.pcgtol_ini   = 1e-4;
pars.adapt_epsR   = 1;
pars.epsR_cutoff  = 0.05;
pars.adapt_epsF   = 1;
pars.epsF_cutoff  = 0.01;
pars.loops        = 3;

%-----------------%
% -- Grayscale -- %

pars.U0           = Ig_01L2;  % initial solution

% >> Deconvolution via IRN <<
IRN_Ig_01L2 = irntv(Ig_01L2, KC, lambda, pars);

figure; imagesc( Normalize(IRN_Ig_01L2) );
colormap gray; axis image; axis off;
title(sprintf('Deconvolved Image - Vector IRN. \nSNR: %4.1fdB (noisy SNR: %4.1fdB).\n ', ...
               snr(Ig, IRN_Ig_01L2), snr(Ig, Ig_01L2)));

%-------------%
% -- Color -- %

pars.U0           = Ic_01L2;  % initial solution

% >> Deconvolution via IRN <<
IRN_Ic_01L2 = irntv(Ic_01L2, KC, lambda, pars);

figure; imagesc( Normalize(IRN_Ic_01L2) );
colormap gray; axis image; axis off;
title(sprintf('Deconvolved Image - Vector IRN. \nSNR: %4.1fdB (noisy SNR: %4.1fdB).\n ', ...
               snr(Ic, IRN_Ic_01L2), snr(Ic, Ic_01L2)));

end % _END_ if( strcmp(example,'l2deconv') || strcmp(example,'all') )

%-----------------------------------------------------------------------------

if( strcmp(example,'l1denoise') || strcmp(example,'all') )

%%%%%%%%%%%%%%%%%%%%%%%%%%%
%       L1 Denoise        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%

%noise level: 10%

%-- IRN
lambda  = 1.1;

pars = irntvInputPars('l1tv');

pars.pcgtol_ini = 1e-4;
pars.epsf       = 1e-2;
pars.epsr       = 1e-4;
pars.loops      = 2;

%-----------------%
% -- Grayscale -- %

% >> Denoising via IRN <<
IRN_Ig_01L1 = irntv(Ig_01L1, [], lambda, pars);

figure; imagesc( Normalize(IRN_Ig_01L1) );
colormap gray; axis image; axis off;
title(sprintf('Denoised Image - Vector IRN. \nSNR: %4.1fdB (noisy SNR: %4.1fdB).\n ', ...
               snr(Ig, IRN_Ig_01L1), snr(Ig, Ig_01L1)));

%-------------%
% -- Color -- %

% >> Denoising via IRN <<
IRN_Ic_01L1 = irntv(Ic_01L1, [], lambda, pars);

figure; imagesc( Normalize(IRN_Ic_01L1) );
colormap gray; axis image; axis off;
title(sprintf('Denoised Image - Vector IRN. \nSNR: %4.1fdB (noisy SNR: %4.1fdB).\n ', ...
               snr(Ic, IRN_Ic_01L1), snr(Ic, Ic_01L1)));

%-----------------
%-----------------

%noise level: 30%

lambda  = 1.2;

pars = irntvInputPars('l1tv');

pars.pcgtol_ini   = 1e-4;
pars.adapt_epsR   = 1;
pars.epsR_cutoff  = 0.01;
pars.adapt_epsF   = 1;
pars.epsF_cutoff  = 0.05;
pars.loops        = 3;

%-----------------%
% -- Grayscale -- %

% >> Denoising via IRN <<
IRN_Ig_03L1 = irntv(Ig_03L1, [], lambda, pars);

figure; imagesc( Normalize(IRN_Ig_03L1) );
colormap gray; axis image; axis off;
title(sprintf('Denoised Image - Vector IRN. \nSNR: %4.1fdB (noisy SNR: %4.1fdB).\n ', ...
               snr(Ig, IRN_Ig_03L1), snr(Ig, Ig_03L1)));

%-------------%
% -- Color -- %

% >> Denoising via IRN <<
IRN_Ic_03L1 = irntv(Ic_03L1, [], lambda, pars);

figure; imagesc( Normalize(IRN_Ic_03L1) ); axis image; axis off;
title(sprintf('Denoised Image - Vector IRN. \nSNR: %4.1fdB (noisy SNR: %4.1fdB).\n ', ...
               snr(Ic, IRN_Ic_03L1), snr(Ic, Ic_03L1)));

end % _END_ if( strcmp(example,'l1denoise') || strcmp(example,'all') )

%-----------------------------------------------------------------------------

if( strcmp(example,'l2denoise') || strcmp(example,'all') )

%%%%%%%%%%%%%%%%%%%%%%%%%%%
%       L2 Denoise        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%

% sigma (noise level) : 0.05

lambda  = 0.05;

pars = irntvInputPars('l2tv');

pars.pcgtol_ini = 1e-4;
pars.epsf       = 1e-2;
pars.epsr       = 1e-5;
pars.loops      = 2;

%-----------------%
% -- Grayscale -- %

% >> Denoising via IRN <<
IRN_Ig_005L2 = irntv(Ig_005L2, [], lambda, pars);

figure; imagesc( Normalize(IRN_Ig_005L2) );
colormap gray; axis image; axis off;
title(sprintf('Denoised Image - Vector IRN. \nSNR: %4.1fdB (noisy SNR: %4.1fdB).\n ', ...
               snr(Ig, IRN_Ig_005L2), snr(Ig, Ig_005L2)));

%-------------%
% -- Color -- %

% >> Denoising via IRN <<
IRN_Ic_005L2 = irntv(Ic_005L2, [], lambda, pars);

figure; imagesc( Normalize(IRN_Ic_005L2) ); axis image; axis off;
title(sprintf('Denoised Image - Vector IRN. \nSNR: %4.1fdB (noisy SNR: %4.1fdB).\n ', ...
               snr(Ic, IRN_Ic_005L2), snr(Ic, Ic_005L2)));

%-----------------
%-----------------

% sigma (noise level) : 0.1

lambda  = 0.1;

pars = irntvInputPars('l2tv');

pars.pcgtol_ini = 1e-4;
pars.epsf       = 1e-2;
pars.epsr       = 1e-5;
pars.loops      = 2;

%-----------------%
% -- Grayscale -- %

% >> Denoising via IRN <<
IRN_Ig_01L2 = irntv(Ig_01L2, [], lambda, pars);

figure; imagesc( Normalize(IRN_Ig_01L2) );
colormap gray; axis image; axis off;
title(sprintf('Denoised Image - Vector IRN. \nSNR: %4.1fdB (noisy SNR: %4.1fdB).\n ', ...
               snr(Ig, IRN_Ig_01L2), snr(Ig, Ig_01L2)));

%-------------%
% -- Color -- %

% >> Denoising via IRN <<
IRN_Ic_01L2 = irntv(Ic_01L2, [], lambda, pars);

figure; imagesc( Normalize(IRN_Ic_01L2) ); axis image; axis off;
title(sprintf('Denoised Image - Vector IRN. \nSNR: %4.1fdB (noisy SNR: %4.1fdB).\n ', ...
               snr(Ic, IRN_Ic_01L2), snr(Ic, Ic_01L2)));

end % _END_ if( strcmp(example,'l2denoise') || strcmp(example,'all') )

%-----------------------------------------------------------------------------


function hess = cstepHess(fun,x,isTril,h)
%CSTEPHESS  Complex Step Hessian of a Scalar Function
%
%  cstepHess uses a complex step approach to finite difference to ensure
%  robust estimate of the Hessian. Note your function MUST be
%  written in Matlab code only and must not contain any if/else statements.
%
%  If it does not work - ensure you are using .' (transpose) and not ' (complex conjugate).
%
%   hess = cstepHess(fun,x) calculates the Hessian of fun at the point x.
%
%   hess = cstepHess(fun,x,isTril) specifies if the returned Hessian
%   should be Symmetric Lower Triangular.
%
%   hess = cstepHess(fun,x,isTril,h) specifies the step-size. This defaults to
%   1e-3.

%   Copyright (C) 2013 Jonathan Currie (IPL)
%
%   This code follows ideas from "New Complex-Step Derivative Approximations
%   with Application to Second-Order Kalman Filtering", by Kok-Lam Lai, John
%   L. Crassidis and Yang Cheng.

if(nargin < 4), h = 1e-3; end
if(nargin < 3 || isempty(isTril)), isTril = false; end

%Constants from paper
I = sqrt(2)/2*(1i + 1);
n = length(x); hess = zeros(n,n);
%Diagonal Elements
for k = 1:n
    xu = x; xu(k) = xu(k) + I*h;
    xl = x; xl(k) = xl(k) - I*h;
    hess(k,k) = imag((fun(xu) + fun(xl))/h^2);
end
%Lower Triangular Elements
lam = 1; k = n-1;
while(k > 0)
    for phi = 1:k
        xu = x; xu(phi:phi+lam) = xu(phi:phi+lam) + I*h;
        xl = x; xl(phi:phi+lam) = xl(phi:phi+lam) - I*h;
        Fsum = sum(sum(hess(phi:phi+lam,phi:phi+lam)));
        hess(phi+lam,phi) = (imag((fun(xu) + fun(xl))/h^2) - Fsum)/2;
        hess(phi,phi+lam) = hess(phi+lam,phi); %copy to upper tri
    end
    k = k - 1;
    lam = lam + 1;
end
%Make tril if requested
if(isTril), hess = tril(hess); end