 function r1d = ir_mri_dce_r1d(r10, Ct, varargin)
%function r1d = ir_mri_dce_r1d(r10, Ct, varargin)
%|
%| Make time series of R1=1/T1 values given baseline value(s) R10
%| and contrast agent time-concentration curves Ct.
%|
%| in
%|	r10	[1] | [Nc]	[1/s] baseline R1 values
%|				for Nc tissue classes
%|	Ct	[Nc Nt]		[mMmol] tissue constrast concentration curves
%|				for Nt time points
%|
%| option
%|	'r1'	[1]		relaxivity [1/mMol 1/s]; default: 4.5 (for 3T)
%|
%| out
%|	r1d	[Nc Nt]		[1/s] dynamic R1=1/T1 curves
%|
%| 2014-08-21 Jeff Fessler and Mai Le, University of Michigan

if nargin == 1 && streq(r10, 'test'), ir_mri_dce_r1d_test, return, end
if nargin < 2, ir_usage, end

arg.r1 = 4.5; % [mMol^-1 sec^-1] for Gd at 3T, Table 2 of sasaki:05:eea
arg = vararg_pair(arg, varargin);

if numel(r10) ~= 1 && numel(r10) ~= nrow(Ct)
	fail 'Nc mismatch'
end
r1d = ir_mri_dce_r1d_do(r10(:), Ct, arg.r1);

function r1d = ir_mri_dce_r1d_do(r10, Ct, r1)

if numel(r10) ~= 1
	r10 = repmat(r10, [1 ncol(Ct)]);
end
r1d = r10 + r1 * Ct;

% ir_mri_dce_r1d_test()
function ir_mri_dce_r1d_test

TR = 5e-3; % 5 msec
duration = 4; % [min]
ti = linspace(0, duration, 401); % [min]
Ktrans = [3.0 0.6 0.2 0];
kep = [6.0 2.0 1.3 1];
leg = {'rapid', 'moderate', 'slow', 'none'};
Ct = ir_mri_dce_aif1(ti, Ktrans, kep);

f = mri_brainweb_params('grey-matter');
t10 = f.t1 / 1000; % [s]
r10 = 1 / t10; % [1/s]

r1d = ir_mri_dce_r1d(r10, Ct);
if im
	clf
	subplot(121)
	plot(ti, r1d, '.-'), ylabelf 'R1 [1/s]'
	legend(leg{:})
	t1d = 1 ./ r1d;
	subplot(122)
	plot(ti, 1000*t1d, '.-'), ylabelf 'T1 [ms]'
	xlabel 't [min]'
end


function [xMin,fMin,exitCode]=conjugateGradMethod(f,x0,epsilon,deltaTestDist,delta,lineSearchParams,resetEvery,gammaVal,maxIter)
%%CONJUGATEGRADMETHOD Perform unconstrained nonlinear optimization using
%              the conjugate gradient algorithm. The algorithm performs
%              unconstrained minimization of a nonlinear function without
%              having to provide a Hessian matrix. On large problems, this
%              algorithm can be faster than Newton's method, which has a
%              large matrix inversion, and steepest ascent, which usually
%              converges more slowly.
%
%INPUTS: f A handle to the function (and its gradient) over which the
%          minimization is to be performed. The function [fVal,gVal]=f(x)
%          takes the NX1 x vector and returns the real scalar function
%          value fVal and NX1 gradient gVal at the point x.
%       x0 The NX1-dimensional point from which the minimization starts.
%  epsilon The parameter determining the accuracy of the desired solution
%          in terms of the gradient. The function terminates when
%          norm(g) < epsilon*max([1, norm(x)])
%          where g is the gradient. The default if omitted or an empty
%          matrix is passed is 1e-6.
% deltaTestDist The number of iterations back to use to compute the
%          decrease of the objective function if a delta-based convergence
%          test is performed. If zero, then no delta-based convergence
%          testing is done. The default if omitted or an empty matrix is
%          passed is zero.
%    delta The delta for the delta convergence test. This determines the
%          minimum rate of decrease of the objective function. Convergence
%          is determined if (f'-f)<=delta*f, where f' is the value of the
%          objective function f deltaTestDist iterations ago,and f is the
%          current objective function value. The default if this parameter
%          is omitted or an empty matrix is passed is 0.
% lineSearchParams An optional structure whose members specify tolerances
%          for the line search. The parameters are described as in the
%          lineSearch function, except if -1 is passed instead of a
%          parameter structure, then no line search is performed.
% resetEvery In Chapter 2.1 of [1], when handling non-quadratic function,
%          it is suggested that the subgradients be reset every N
%          iterations. By reset, this means that the past direction is
%          discarded and the next step is a steepest descent step. This is
%          the number of iterations after which a reset will occur.
%          resetEvery>=1. The default if omitted or an empty matrix is
%          passed is N.
% gammaVal In Chapter 2.1 of [1], it is suggested that the descent
%          direction algorithm be reset if
%          abs(g'*gPrev)>gammaVal*norm(gPrev)^2
%          where 0<gammaVal<1. The notion is that the previous and next
%          gradient directions should be nearly orthogonal, so if they are
%          not, within some tolerance, then a steepest descent step is
%          taken. The default if this parameter is omitted or an empty
%          matrix is passed is 0.1.
%  maxIter The maximum number of iterations to use for the algorithm. The
%          default if this parameter is omitted or an empty matrix is
%          passed is 4*N.
%
%OUTPUTS: xMin The value of x at the minimum point found. If exitCode is
%              negative, then this might be an empty matrix.
%         fMin The cost function value at the minimum point found. If
%              exitCode is negative, then this might be an empty matrix.
%     exitCode A value indicating the termination condition of the
%              algorithm. Nonnegative values indicate success; negative
%              values indicate some type of failure. Possible values are:
%                  0 The algorithm termiated successfully based on the
%                    gradient criterion.
%                  1 The algorithm terminated successfully based on the
%                    accuracy criterion.
%                 -1 The maximum number of overall iterations was reached.
%                 -2 A non-finite value was encoutered outside the line
%                    search.
%              Other negative values correspond to a failure in lineSearch
%              and correspond to the exitCode returned by the lineSearch
%              function.
%
%The algorithm is implemented based on the description in Chapter 2.1 of
%[1].
%
%EXAMPLE:
%The example is that used in the lineSearch file.
% f=@(x)deal((x(1)+x(2)-3)*(x(1)+x(2))^3*(x(1)+x(2)-6)^4,... %The function
%            [(-6+x(1)+x(2))^3*(x(1)+x(2))^2*(54+8*x(1)^2+x(2)*(-45+8*x(2))+x(1)*(-45+16*x(2)));
%            (-6+x(1)+x(2))^3*(x(1)+x(2))^2*(54+8*x(1)^2+x(2)*(-45+8*x(2))+x(1)*(-45+16*x(2)))]);%And the gradient as the second return.
% %Note that the deal function is used to make an anonymous function have
% %two outputs.
% x0=[0.5;0.25];
% [xMin,fMin,exitCode]=conjugateGradMethod(f,x0)
%The optimum point found is such that sum(xMin) is approximately
%1.73539450 with a minimum function value of approximately -2.1860756.
%
%REFERENCES:
%[1] D. P. Bertsekas, Nonlinear Programming, 3rd ed. Belmont, MA: Athena
%    Science, 2016.
%
%January 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release

    if(nargin<3||isempty(epsilon))
        epsilon=1e-6;
    end

    if(nargin<4||isempty(deltaTestDist))
        deltaTestDist=0;
    end

    if(nargin<5||isempty(delta))
        delta=0;
    end

    %Take any line search parameters given.
    useLineSearch=true;
    if(nargin>=6&&~isempty(lineSearchParams))
        if(isnumeric(lineSearchParams)&&lineSearchParams==-1)
            %If one just wishes to blindly take steps.
            useLineSearch=false;
        elseif(~isstruct(lineSearchParams)||(isnumeric(lineSearchParams)&&lineSearchParams~=-1))
            error('Unknown lineSearchParams value given')
        end
    else
        lineSearchParams=[];
    end

    N=size(x0,1);

    if(nargin<7||isempty(resetEvery))
       resetEvery=N;
    end

    if(nargin<8||isempty(gammaVal))
       gammaVal=0.1;
    end

    if(nargin<9||isempty(maxIter))
        maxIter=4*N;
    end

    x=x0;
    [fVal,gradF]=f(x0);

    %The previous values are not yet set.
    gradFPrev=[];
    dPrev=[];

    if(deltaTestDist>0)
        pastFVals=zeros(deltaTestDist,1);
        pastFVals(1)=fVal;
    end

    stepsSinceReset=0;

    for cutIter=1:maxIter
        if(stepsSinceReset>0)
            if(stepsSinceReset>=resetEvery)
                stepsSinceReset=0;
            else
                gradFPrevMag2=gradFPrev'*gradFPrev;

                if(abs(gradF'*gradFPrev)>gammaVal*gradFPrevMag2)
                    stepsSinceReset=0;
                end
            end
        end

        if(stepsSinceReset>0)
            %If it hasn't just been reset.

            %Equation 2.35
            beta=gradF'*(gradF-gradFPrev)/gradFPrevMag2;

            %Equation 2.34 for the descent direction.
            d=-gradF+beta*dPrev;
        else
            %Steepest descent direction.
            d=-gradF;
        end

        if(useLineSearch)
            %Perform a line search in the given descent direction.
            [xCur,fValCur,gradFCur,~,~,exitCode]=lineSearch(f,x,d,[],[],lineSearchParams);
            if(isempty(xCur))
                xMin=[];
                fMin=[];
                return;
            end
        else%Take a step without a line search.
            xCur=xPrev+d;
        end

        if(any(~isfinite(xCur(:))))
            exitCode=-2;
            xMin=[];
            fMin=[];
            return;
        end

        %Check for convergence based on the gradient.
        if(norm(gradFCur)<epsilon*max([1,norm(xCur)]))
            xMin=xCur;
            fMin=fValCur;
            exitCode=0;
            return;
        end

        %Check for convergence based on the actual function value.
        if(deltaTestDist~=0)
            if(pastFVals(end)-fValCur<=delta*fValCur)
                xMin=xCur;
                fMin=fValCur;
                exitCode=1;
                return;
            end
            pastFVals=circshift(pastFVals,[1,0]);
            pastFVals(1)=fVal;
        end

        x=xCur;
        gradFPrev=gradF;
        gradF=gradFCur;
        dPrev=d;
        stepsSinceReset=stepsSinceReset+1;
    end

    %The maximum number of iterations elapsed without convergence
    xMin=xCur;
    fMin=fValCur;
    exitCode=-1;
end

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


function dsq=define_distances_btw_control_points()

% Copyright (C) <2007>  <Francesc Moreno-Noguer, Vincent Lepetit, Pascal Fua>
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the version 3 of the GNU General Public License
% as published by the Free Software Foundation.
%
% This program is distributed in the hope that it will be useful, but
% WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
% General Public License for more details.
% You should have received a copy of the GNU General Public License
% along with this program. If not, see <http://www.gnu.org/licenses/>.
%
% Francesc Moreno-Noguer, CVLab-EPFL, September 2007.
% fmorenoguer@gmail.com, http://cvlab.epfl.ch/~fmoreno/

%relative coordinates of the control points
c1=[1,0,0];
c2=[0,1,0];
c3=[0,0,1];
c4=[0,0,0];

d12=(c1(1)-c2(1))^2 + (c1(2)-c2(2))^2 + (c1(3)-c2(3))^2;
d13=(c1(1)-c3(1))^2 + (c1(2)-c3(2))^2 + (c1(3)-c3(3))^2;
d14=(c1(1)-c4(1))^2 + (c1(2)-c4(2))^2 + (c1(3)-c4(3))^2;
d23=(c2(1)-c3(1))^2 + (c2(2)-c3(2))^2 + (c2(3)-c3(3))^2;
d24=(c2(1)-c4(1))^2 + (c2(2)-c4(2))^2 + (c2(3)-c4(3))^2;
d34=(c3(1)-c4(1))^2 + (c3(2)-c4(2))^2 + (c3(3)-c4(3))^2;

dsq=[d12,d13,d14,d23,d24,d34]';


function pdepetrans
% 1D transport - modelling with extensions for decay and fast sorption
%    using MATLAB pdepe
%
%   $Ekkehard Holzbecher  $Date: 2006/03/16 $
%--------------------------------------------------------------------------
T = 1;                     % maximum time [s]
L = 1;                     % length [m]
D = 1;                     % diffusivity [m*m/s]
v = 1;                     % velocity [m/s]
lambda = 0.0;              % decay constant [1/s]
sorption = 2;              % sorption-model: no sorption (0), linear (1),
                           %                 Freundlich (2), Langmuir (3)
k1 = 0.0004;               % sorption parameter 1 (R=0 for linear isotherm with Kd, else k1=R)
k2 = 0.5;                  % sorption parameter 2 (Kd for linear isotherm with Kd)
rhob = 1300;               % porous medium bulk density [kg/m*m*m]
theta = 0.2;               % porosity [-]
c0 = 0.0;                  % initial concentration [kg/m*m*m]
cin = 1;                   % boundary concentration [kg/m*m*m]

M = 40;                    % number of timesteps
N = 40;                    % number of nodes
%-------------------------- output parameters
gplot = 0;                 % =1: breakthrough curves; =2: profiles
gsurf = 0;                 % surface
gcont = 0;                 % =1: contours; =2: filled contours
ganim = 2;                 % animation of profiles; =1: single line; =2: all lines

t = linspace (T/M,T,M);    % time discretization
x = linspace (0,L,N);      % space discretization

%----------------------execution-------------------------------------------
if sorption == 1 && k1 <=0
    k1 = 1+k2*rhob/theta;
else
    if sorption > 1 k1 = rhob*k1/theta; end
end
options = odeset; if (c0 == 0) c0 = 1.e-20; end
c = pdepe(0,@transfun,@ictransfun,@bctransfun,x,[0 t],options,D,v,lambda,sorption,k1,k2,c0,cin);

%---------------------- graphical output ----------------------------------
switch gplot
    case 1
        plot ([0 t],c)        % breakthrough curves
        xlabel ('time'); ylabel ('concentration');
    case 2
        plot (x,c','--')      % profiles
        xlabel ('space'); ylabel ('concentration');
end
if gsurf                      % surface plot
    figure; surf (x,[0 t],c);
    xlabel ('space'); ylabel ('time'); zlabel('concentration');
end
if gcont figure; end
switch gcont
    case 1
        contour (x,[0 t],c)   % contours
        grid on; xlabel ('space'); ylabel ('time');
    case 2
        contourf(x,[0 t],c)   % filled contours
        colorbar; xlabel ('space'); ylabel ('time');
end
if (ganim)
    [FileName,PathName] = uiputfile('*.mpg');
    figure; if (ganim > 1) hold on; end
    for j = 1:size(c,1)
        axis manual;  plot (x,c(j,:),'r','LineWidth',2);
        ylim ([min(c0,cin) max(c0,cin)]);
        legend (['t=' num2str(T*(j-1)/M)]);
        Anim(j) = getframe;
        plot (x,c(j,:),'b','LineWidth',2);
    end
    mpgwrite (Anim,colormap,[PathName '/' FileName]);     % mgwrite not standard MATLAB
    movie (Anim,0);   % play animation
end

%----------------------functions------------------------------
function [c,f,s] = transfun(x,t,u,DuDx,D,v,lambda,sorption,k1,k2,c0,cin)
switch sorption
    case 0
        R = 1;
    case 1
        R = k1;
    case 2
        R = 1+k1*k2*u^(k2-1);
    case 3
        R = 1+k1*k2*u/(k2+u)/(k2+u);
end
c = R;
f = D*DuDx;
s = -v*DuDx -lambda*R*u;
% --------------------------------------------------------------
function u0 = ictransfun(x,D,v,lambda,sorption,k1,k2,c0,cin)
u0 = c0;
% --------------------------------------------------------------
function [pl,ql,pr,qr] = bctransfun(xl,ul,xr,ur,t,D,v,lambda,sorption,k1,k2,c0,cin)
pl = ul-cin;
ql = 0;
pr = 0;
qr = 1;


function Kc = knCenter(kn, X, X1, X2)
% Centerize the data in the kernel space
% Input:
%   kn: kernel function
%   X: d x n data matrix of which the center in the kernel space is computed
%   X1, X2: d x n1 and d x n2 data matrix. the kernel k(x1,x2) is computed
%       where the origin of the kernel space is the center of phi(X)
% Ouput:
%   Kc: n1 x n2 kernel matrix between X1 and X2 in kernel space centered by
%       center of phi(X)
% Written by Mo Chen (sth4nth@gmail.com).
K = kn(X,X);
mK = mean(K);
mmK = mean(mK);
if nargin == 2     % compute the pairwise centerized version of the kernel of X. eq knCenter(kn,X,X,X)
    Kc = K+mmK-bsxfun(@plus,mK',mK);        % Kc = K-M*K-K*M+M*K*M; where M = ones(n,n)/n;
elseif nargin == 3  % compute the norms (k(x,x)) of X1 w.r.t. the center of X as the origin. eq diag(knCenter(kn,X,X1,X1))
    Kc = kn(X1)+mmK-2*mean(kn(X,X1));
elseif nargin == 4  % compute the kernel of X1 and X2 w.r.t. the center of X as the origin
    Kc = kn(X1,X2)+mmK-bsxfun(@plus,mean(kn(X,X1))',mean(kn(X,X2)));
end


% clear all;
% close all;
% clc;

display('Nth (2) order Reconstruction');

N = 2;                  % Nth order nonuniform sampling
TQ = 1;Fs = 1/TQ;                 % Nyquist Period

T = [1.5*TQ 3*TQ];      % Decimation Periods
K = 0.5*lcm(2*T(1), 2*T(2))/TQ; capT = K*TQ; M = capT./T;
ML = (100:100:2500); % number of slices
w_c = 0.85;
NS = 100;  % Number of Sinusoids

LF = lcm(M(1),M(2))*2*K+1;      % length of LF should be Multiple of LCM{M(p)}*2*K
n = -(LF-1)/2:1:(LF-1)/2;

std = 1e-1;
taus = [0 1+std*randn]*TQ;
tausI = sort([taus(1) taus(2) T(1)+taus(1)]);
display(tausI);

Frq = rand(1,NS)*w_c/2;
Amp = rand(1,NS)/(sqrt(NS)*2);
Phi = rand(1,NS)*2*pi;

timeP = zeros(size(ML));
timeE = zeros(size(ML));
timeI = zeros(size(ML));
timeV = zeros(size(ML));
timePr = zeros(size(ML));
timeJ = zeros(size(ML));

MC_runs = 1;
for rrr = 1:length(ML)
display(rrr);
input = zeros(1,ML(rrr)*K);
for k = 1:NS
  input = input + Amp(k)*sin(2*pi*Frq(k)*(0:ML(rrr)*K-1)*TQ+Phi(k));
end;
for tt = 1:MC_runs

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% A realization of Digital Filter Banks for Reconstruction of Uniformly
% sampled signals from nonuniform samples
% Authors: Itami, Watanabe, Nishihara
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
tauI = zeros(K,ML(rrr));
for p = 1:K
    tauI(p,:) = tausI(p)+(0:ML(rrr)-1)*capT;
end;

x11 = zeros(K,ML(rrr));
for k = 1:NS
    x11 = x11 + Amp(k)*sin(2*pi*Frq(k)*tauI+Phi(k));
end;

tic
a = zeros(1,K);
for p = 1:K
    a(p) = 1;
    for q = 1:K
        if q ~= p
                a(p) = a(p)/sin(pi*(tausI(p)-tausI(q))/capT);
        end;
    end;
end;
% c = sin(pi*(tausI(0+1))/capT);
% s = cos(pi*(tausI(0+1))/capT);
% b(1,1) = 0.5*(c+1i*s);
% c = sin(pi*(tausI(1+1))/capT);
% s = cos(pi*(tausI(1+1))/capT);
% b(1,2) = 0.5*(c+1i*s);
% b(3,:) = conj(b(1,:));
% b(2,:) = 0;

b = zeros(2*K-1,K);
c = -0.5*cos(pi*(tausI(1+1)+tausI(2+1))/capT);
s = -0.5*sin(pi*(tausI(1+1)+tausI(2+1))/capT);
b(1,1) = 0.5*(c+1i*s);
c = -0.5*cos(pi*(tausI(0+1)+tausI(2+1))/capT);
s = -0.5*sin(pi*(tausI(0+1)+tausI(2+1))/capT);
b(1,2) = 0.5*(c+1i*s);
c = -0.5*cos(pi*(tausI(0+1)+tausI(1+1))/capT);
s = -0.5*sin(pi*(tausI(0+1)+tausI(1+1))/capT);
b(1,3) = 0.5*(c+1i*s);
b(5,:) = conj(b(1,:));
b(2,:) = 0;
b(4,:) = conj(b(2,:));
c = 0.5*cos(pi*(tausI(2+1)-tausI(1+1))/capT);
b(3,1) = c;
c = 0.5*cos(pi*(tausI(2+1)-tausI(0+1))/capT);
b(3,2) = c;
c = 0.5*cos(pi*(tausI(1+1)-tausI(0+1))/capT);
b(3,3) = c;

y1=upsample(x11.',K).';

k = -(K-1):1:(K-1);
m = (0:1:(2*K-1))';
F = exp(1i*(pi/K).*kron(m,k));

y = zeros(K,size(y1,2));
for r = 1:K
    tempI = a(r)*y1(r,:);
    tempI2 = F*b(:,r);
    y2 = tempI2*tempI;
%     h = sinc((n/K)-tausI(r)/capT).*knab(LF,4,-tausI(r)/TQ).';
    h = sinc((n/K)-tausI(r)/capT).*kaiser_mine1(LF,3,-tausI(r)/TQ);
    for i=1:2*K
            h1 = upsample(downsample(h,2*K,i-1),2*K);
            temp = filter([zeros(1,i-1),1],1,h1);
%             y = y + filter(temp,1,y2(i,:));
            y2(i,:) = filter(temp,1,y2(i,:));
%             y2(i,:) = filter(h1,1,y2(i,:));
%             y2(i,:) = filter([zeros(1,i-1),1],1,y2(i,:));%,zeros(1,2*K-i)
    end;
    y(r,:) = sum(y2,1);
end;
y = (sum(y,1));
timeI(rrr) = timeI(rrr)+toc;
delayI = (length(n)-1)/2;
x=input(1:end-delayI);
y=y(1+delayI:end);
y = y(160:end);
x = x(160:end);
serI = 20*log10(norm(x,2)/norm(y-x,2));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Reconstruction of Nonuniformly Sampled Band-Limited Signals
% Using a Differentiator-Multiplier Cascade
% Authors: Stefan Tertinek and Christian Vogel
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
r = tausI-TQ*(0:K-1);
rr = r(mod((0:ML(rrr)*K-1),K)+1);
x1 = reshape(x11,1,K*size(x11,2));
% x1 = x11;% LF = (0:11); Fs = 1;
% Differentiator Design
% figure();
% NFFT = 2^nextpow2(length(Hd)); % Next power of 2 from length of Hd
% HD = fftshift(fft(Hd,NFFT))/length(Hd);
% f = Fs*linspace(-1,1,NFFT);
% % Plot double-sided amplitude spectrum.
% plot(f,2*abs(HD(1:NFFT)))
% title('Double-Sided Amplitude Spectrum of Hd(n)')
% xlabel('Frequency (Hz)')
% ylabel('|HD(f)|')
tic
Hd = firpm(LF-1,[0 w_c],[0 w_c*pi],'differentiator');
delayV = (LF-1)/2;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
y1 = filter(Hd,1,x1);
x1 = filter([zeros(1,delayV),1],1,x1);
r2 = filter([zeros(1,delayV),1],1,rr);
e = y1.*r2;
y1 = x1-e;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
y2 = filter(Hd,1,y1);
temp = filter([zeros(1,delayV),1],1,y2);
x1 = filter([zeros(1,2*delayV),1],1,x1);
r2 = filter([zeros(1,2*delayV),1],1,r2);
e1 = temp.*r2;
y2 = filter(Hd,1,y2);
e2 = 0.5*y2.*r2.^2;
y2 = x1-e1-e2;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
y3 = filter(Hd,1,y2);
temp = filter([zeros(1,2*delayV),1],1,y3);
x1 = filter([zeros(1,3*delayV),1],1,x1);
r2 = filter([zeros(1,3*delayV),1],1,r2);
e1 = temp.*r2;
y3 = filter(Hd,1,y3);
temp = filter([zeros(1,delayV),1],1,y3);
e2 = 0.5*temp.*r2.^2;
y3 = filter(Hd,1,y3);
e3 = (y3.*r2.^3)/6;
y3 = x1-e1-e2-e3;
timeV(rrr) = timeV(rrr)+toc;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
y = real(y3(1+6*delayV:end));
x = input(1:end-6*delayV);
serV = 20*log10(norm(x,2)/norm(y-x,2));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Reconstruction of Band-Limited Periodic Nonuniformly Sampled Signals
% Through Multirate Filter Banks
% Ryan S Prendergast, Bernard C Levy, Paul J Hurst
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
tauPr = zeros(K,ML(rrr));
for p = 1:K
    tauPr(p,:) = -tausI(p)+(0:ML(rrr)-1)*capT;
end;
x1 = zeros(K,ML(rrr));
for k = 1:NS
    x1 = x1 + Amp(k)*sin(2*pi*Frq(k)*tauPr+Phi(k));
end;
tic
H = zeros(K,LF);
for i=1:K
%     H(i,:) = sinc(n-tausI(i)).*conv(sinc(n-tausI(i)),kaiser(LF,34).','same');
%     H(i,:) = sinc(n-tausI(i)).*knab(LF,34,-tausI(i)).';
    H(i,:) = sinc(n-tausI(i)).*kaiser_mine1(LF,3,-tausI(i));
end;
H = H(:,1:end-1);
EP = reshape(H.',K,length(H(1,:))/K,K);
capE = [];
for k=1:K
    temp = [];
    for i=1:K
        temp=[temp,toeplitz([EP(i,1,k),zeros(1,(length(H(1,:))/K)-1)],[EP(i,:,k),zeros(1,(length(H(1,:))/K)-1)])];
    end
    capE = [capE;temp];
end
d = ceil(size(capE,2)/(2*K));
P = kron(eye(K),[zeros(1,d),1,zeros(1,(size(capE,2)/K)-d-1)]);
R = P/capE;
% size(capE)
% size(zeros(LF-1,2*LF-2-K))
% size(R)
% size(zeros(K,LF-1))
% size(P)
% size(zeros(K,2*LF-2-K))
R = upsample(R.',K).';
rt = size(R,2)/K;
FR = zeros(K,rt);
for j=1:K
    for i=1:K
        temp = filter([zeros(1,K-i),1],1,R(i,(j-1)*rt+1:j*rt));
        FR(j,:) = FR(j,:)+temp;
    end
end
yb = upsample(x1.',K).';
for i = 1:K
    yb(i,:) = filter(FR(i,:),1,yb(i,:));
end;
y = sum(yb,1);
timePr(rrr) = timePr(rrr)+toc;
delayPr = (size(FR,2))/2+K-1;
x=input(1:end-delayPr);
y=y(1+delayPr:end);
serPr = 20*log10(norm(x,2)/norm(y-x,2));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Reconstruction of Periodically Nonuniformly Sampled Bandlimited Signals
% Using Time-Varying FIR Filters
% Authors: H. Johansson and Per Lowenborg
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
r = r.';
x1 = reshape(x11,1,size(x11,2)*K);
tic
w_o = w_c*pi*TQ;
hJ = zeros(K,LF);
C = zeros(1,LF);
Nt = (LF-1)/2;
for i = 1:K
    C = -2*sin(w_o*(n-r(1+(mod(i-1-n,K)))'))./(pi*(n-r(1+(mod(i-1-n,K)))'));
    C(isnan(C)==1)=-2*w_o/pi;
    C = C.';
    S = zeros(LF,LF);
    for k = 1:LF
        S(k,:) = sin(w_o*(-Nt+k-1-r(1+(mod(i-1-(-Nt+k-1),K)))-(n-r(1+(mod(i-1-n,K)))')))./(pi*(-Nt+k-1-r(1+(mod(i-1-(-Nt+k-1),K)))-(n-r(1+(mod(i-1-n,K)))')));
    end;
    S(isnan(S)==1)=w_o/pi;
    hJ(i,:) = -0.5*S\C;
end;

y1 = zeros(K,length(x1));
for j=1:K
    y1(j,:) = filter(hJ(j,:),1,x1);
    y1(j,:) = upsample(downsample(y1(j,:),K,j-1),K)/K;
    y1(j,:) = filter([zeros(1,j-1),1],1,y1(j,:));
end;
y = K*0.25*sum(y1,1);
timeJ(rrr) = timeJ(rrr)+toc;
delayJ = (size(hJ,2)-1)/2;
y = real(y(1+delayJ:end));
x = input(1:end-delayJ);
serJ = 20*log10(norm(x,2)/norm(y-x,2));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Filterbank Reconstruction of Bandlimited Signals from Nonuniform and
% Generalized Samples
% Authors: Y C Eldar and A V Oppenheim
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

y = zeros(N,ML(rrr)*K);
for p = 1:N
    tau = taus(p)+(0:ML(rrr)*M(p)-1)*T(p);
    x1 = zeros(1,ML(rrr)*M(p));
    for k = 1:NS
        x1 = x1 + Amp(k)*sin(2*pi*Frq(k)*tau+Phi(k));
    end;
    tic
    y1 = upsample(x1,K);

    LFE = M(p)*lcm(M(1),M(2))*2*K+1;      % length of LF should be Multiple of LCM{M(p)}*2*K
    nE = -(LFE-1)/2:1:(LFE-1)/2;
    h = sinc((nE/K)-(taus(p)/T(p))).*kaiser_mine1(LFE,3,-K*(taus(p)/T(p)));

%%%%%%%%%%%%%%%%%%%%%%%%%%% Implementation 1 %%%%%%%%%%%%%%%%%%%%%%%%%%%
%     bb = zeros(2*(K-M(p))+1,M(p));
%     aaa = ones(1,M(p));
%     bbvl = zeros(M(p),LFE);
%     for l = 1:M(p)
%         for q = 1:N
%             if q ~= p
%                 aaa(l) = aaa(l)*sin(pi*(taus(p)-taus(q)+(l-1)*T(p))/T(q));
%                 c = sin(pi*M(q)*(-taus(q)+(l-1)*T(p))/capT);
%                 s = cos(pi*M(q)*(taus(q)-(l-1)*T(p))/capT);
%                 bb(2*M(q)+1,l) = 0.5*(c-1i*s);
%                 bb(1,l) = conj(bb(2*M(q)+1,l));
%             end;
%         end;
%         bbv = zeros(2*(K-M(p))+1,LFE);
%         for v = -(K-M(p)):K-M(p)
%             bbv(K-M(p)+1+v,:) = bb(K-M(p)+1+v,l)*exp(1i*(pi/(K*M(p)))*v*nE);
%         end;
%         bbvl(l,:) = sum(bbv,1)/aaa(l);
%     end;
%     bbb = zeros(M(p),LFE);
%     bbn = zeros(M(p),LFE);
%     for m = 1:M(p)
%         for l = 1:M(p)
%             bbb(l,:) = bbvl(l,:)*exp(1i*(2*pi/M(p))*(m-1)*(l-1));
%         end
%         bbb = sum(bbb,1);
%         bbn(m,:) = bbb.*exp(1i*(2*pi/M(p))*(m-1)*nE);
%     end;
%     bbn = sum(bbn,1);
%     bbn = bbn.*h/M(p);

%%%%%%%%%%%%%%%%%%%%%%%%%%% Implementation 2 %%%%%%%%%%%%%%%%%%%%%%%%%%%
%     bb = zeros(2*(K-M(p))+1,M(p));
%     aaa = ones(1,M(p));
%     for l = 1:M(p)
%         for q = 1:N
%             if q ~= p
%                 aaa(l) = aaa(l)/sin(pi*(taus(p)-taus(q)+(l-1)*T(p))/T(q));
%                 c = sin(pi*M(q)*(-taus(q)+(l-1)*T(p))/capT);
%                 s = cos(pi*M(q)*(taus(q)-(l-1)*T(p))/capT);
%                 bb(2*M(q)+1,l) = 0.5*(c-1i*s);
%                 bb(1,l) = conj(bb(2*M(q)+1,l));
%             end;
%         end;
%     end;
%     AE = diag(aaa);
%     BE = bb.';
%
%     dim = K-M(p); w = -dim:1:dim;
%     FE = exp(1i*(pi/(K*M(p))).*kron(nE,w'));
%
%     E1E = exp(1i*(2*pi/M(p)).*kron((0:M(p)-1),(0:M(p)-1)'))/M(p);
%
%     E2E = exp(1i*(2*pi/M(p)).*kron(nE,(0:M(p)-1)'));
%
%     temp = E1E*AE*BE*FE;
%     bbn = h.*sum(E2E.*temp,1);

%%%%%%%%%%%%%%%%%%%%%%%%%%% Implementation 3 %%%%%%%%%%%%%%%%%%%%%%%%%%%
    aaa = ones(1,M(p));
    bb = ones(M(p),LFE);
    for l = 1:M(p)
        for q = 1:N
            if q ~= p
                aaa(l) = aaa(l)*sin(pi*(taus(p)-taus(q)+(l-1)*T(p))/T(q));
                bb(l,:) = bb(l,:).*sin(pi*((nE*TQ/M(p))-taus(q)+(l-1)*T(p))/T(q));
            end;
        end;
        bb(l,:) = bb(l,:)/aaa(l);
    end;

    bbb = zeros(M(p),LFE);
    bbn = zeros(M(p),LFE);
    for m = 1:M(p)
        for l = 1:M(p)
            bbb(l,:) = bb(l,:)*exp(1i*(2*pi/M(p))*(m-1)*(l-1));
        end
        bbb = sum(bbb,1);
        bbn(m,:) = bbb.*exp(1i*(2*pi/M(p))*(m-1)*nE);
    end;
    bbn = sum(bbn,1);
    bbn = bbn.*h/M(p);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    y1 = conv(y1,bbn);
    delayE = (length(h)-1)/2;
    y(p,:) = y1(1+delayE:M(p):end-delayE);
    timeE(rrr) = timeE(rrr)+toc;
end;
y = sum(real(y),1);
x = input;
serE = 20*log10(norm(x,2)/norm(y-x,2));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Reconstruction of N-th order nonuniformly sampled bandlimited signals
% using digital filter banks
% Authors: S. K. Sindhi, K. M. M. Prabhu
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
xp = zeros(N,ML(rrr)*K);
for p = 1:N
    tau = taus(p)+(0:ML(rrr)*M(p)-1)*T(p);
    x1 = zeros(1,ML(rrr)*M(p));
    for k = 1:NS
        x1 = x1 + Amp(k)*sin(2*pi*Frq(k)*tau+Phi(k));
    end;

    tic
    m = (0:1:M(p)-1)'; lemda = 0:1:M(p)-1;
    W = exp(1i*(2*pi/M(p)).*kron(m,lemda)); % m*lemda

    bb = zeros(2*(K-M(p))+1,M(p));
    aaa = ones(1,M(p));
    for l = 1:M(p)
        for q = 1:N
            if q ~= p
                    aaa(l) = aaa(l)/sin(pi*M(q)*(taus(p)-taus(q)+(l-1)*T(p))/capT);
                    c = sin(pi*M(q)*(-taus(q)+(l-1)*T(p))/capT);
                    s = cos(pi*M(q)*(taus(q)-(l-1)*T(p))/capT);
                    bb(2*M(q)+1,l) = 0.5*(c-1i*s);
                    bb(1,l) = conj(bb(2*M(q)+1,l));
            end;
        end;
    end;
    A = diag(aaa); % display(A);
    B = bb; % display(B);

    y1 = upsample(x1,K);
    y1 = reshape(y1,M(p),length(y1)/M(p));

    if M(p)>1
        y1(2:end,:) = flipud(y1(2:end,:));
        for i = 1:M(p)-1
            y1(i+1,:) = filter([0,1],1,y1(i+1,:));
        end;
    end;

    dim = K-M(p); w = -dim:1:dim;

    xlemda = zeros(M(p),size(y1,2));
    for lemda = 0:M(p)-1

        rP = (lemda/M(p))+(0:1:(2*K-1))';
        Fshift = exp(1i*(pi/K).*kron(rP,w));   % r*w

%         h = sinc((n*TQ/T(p))+(lemda/K)-(taus(p)/T(p))).*knab(LF,4,(lemda/M(p))-(taus(p)/TQ)).';
        h = sinc((n*TQ/T(p))+(lemda/K)-(taus(p)/T(p))).*kaiser_mine1(LF,3,(lemda/M(p))-(taus(p)/TQ));
        mtemp = A*W;
        mtemp = B*mtemp;
        mtemp = Fshift*mtemp;
%         mtemp = mtemp*W(:,lemda+1);
%         y2 = mtemp*y1(lemda+1,:);
%         for i=1:2*K
%             h1 = upsample(downsample(h,2*K,i-1),2*K);
% %             y2(i,:) = filter(h1,1,y2(i,:));
% %             y2(i,:) = filter([zeros(1,i-1),1],1,y2(i,:));%,zeros(1,2*K-i)
%             temp = filter([zeros(1,i-1),1],1,h1);
%             y2(i,:) = filter(temp,1,y2(i,:));
%         end;
%         xlemda(lemda+1,:) = sum(y2,1);

        Htemp = mtemp*W(:,lemda+1);
        h1 = zeros(2*K, length(h)+2*K-1);
        for i = 2:2*K
            h1(i,:) = [filter([zeros(1,i-1),1],1,upsample(downsample(h,2*K,i-1),2*K)) zeros(1,2*K)]*Htemp(i);
        end;
        h1(1,:) = upsample(downsample(h,2*K),2*K)*Htemp(1);
        h1 = sum(h1,1);
        xlemda(lemda+1,:) = filter(h1,1,y1(lemda+1,:));
    end;
    xp(p,:) = sum(xlemda,1)/M(p);
    clear bb;
    timeP(rrr) = timeP(rrr)+toc;
end;
y = real(sum(xp,1));
delayP = (length(h)-1)/2;
y = y(1+delayP:end);
x = input(1:end-delayP);
serP = 20*log10(norm(x,2)/norm(y-x,2));
end;
end;
timeP = timeP/MC_runs;
timeE = timeE/MC_runs;
timeI = timeI/MC_runs;
timeV = timeV/MC_runs;
timePr = timePr/MC_runs;
timeJ = timeJ/MC_runs;

figure();hold on;
plot(ML,timeJ,'kp-','LineWidth',2);
plot(ML,timePr,'ko-','LineWidth',2);
plot(ML,timeV,'ks-','LineWidth',2);
plot(ML,timeP,'kd-','LineWidth',2);
plot(ML,timeI,'k>-','LineWidth',2);
plot(ML,timeE,'k+-','LineWidth',2);
legend('Johansson','Prendergast','Tertinek','Proposed','Itami','Eldar');
xlabel('Input signal length','fontsize',14,'fontweight','b');
ylabel('Time in seconds','fontsize',14,'fontweight','b');
grid on;box on;
set(gca,'fontsize',14,'fontweight','b')

display(serI)
display(serV)
display(serPr)
display(serJ)
display(serP)
display(serE)
% figure();
% subplot(2,1,1);
% plot(([x' y']));
% title('input / output signals');
% xlabel('sample');
% ylabel('signal value');
% grid on;
% subplot(2,1,2);
% plot((x'-y'));
% xlabel('time (sample)');
% ylabel('error value');
% grid on;


function [x, funVal, ValueL]=glLeastR(A, y, z, opts)
%
%%
% Function glLeastR
%      Least Squares Loss with the (group) L1/Lq-norm Regularization
%
%% Problem
%
%  min  1/2 || A x - y||^2 + z * sum_j gWeight_j ||x^j||_q
%
%  x is grouped into k groups according to opts.ind
%  The indices of x_j in x is (ind(j)+1):ind(j+1)
%
%% Input parameters:
%
%  A-         Matrix of size m x n
%                A can be a dense matrix
%                         a sparse matrix
%                         or a DCT matrix
%  y -        Response vector (of size mx1)
%  z -        L1/Lq norm regularization parameter (z >=0)
%  opts-      Optional inputs (default value: opts=[])
%
%% Output parameters:
%
%  x-         Solution
%  funVal-    Function value during iterations
%
%% Copyright (C) 2009-2010 Jun Liu, and Jieping Ye
%
% You are suggested to first read the Manual.
%
% For any problem, please contact with Jun Liu via j.liu@asu.edu
%
% Last modified on February 19, 2010.
%
%% Related papers
%
% [1]  Jun Liu, Shuiwang Ji, and Jieping Ye, Multi-Task Feature Learning
%      Via Efficient L2,1-Norm Minimization, UAI, 2009
%
% [2]  Jun Liu, Lei Yuan, Songcan Chen and Jieping Ye, Multi-Task Feature Learning
%      Via Efficient L2,1-Norm Minimization, Technical Report ASU, 2009.
%
%% Related functions:
%
%  sll_opts, initFactor, pathSolutionLeast
%  glLogisticR, eppVectorR
%
%%

%% Verify and initialize the parameters
%%
if (nargin <4)
    error('\n Inputs: A, y, z, and opts.ind should be specified!\n');
end

[m,n]=size(A);

if (length(y) ~=m)
    error('\n Check the length of y!\n');
end

if (z<=0)
    error('\n z should be positive!\n');
end

opts=sll_opts(opts); % run sll_opts to set default values (flags)

%% Detailed initialization
%% Normalization

% Please refer to sll_opts for the definitions of mu, nu and nFlag
%
% If .nFlag =1, the input matrix A is normalized to
%                     A= ( A- repmat(mu, m,1) ) * diag(nu)^{-1}
%
% If .nFlag =2, the input matrix A is normalized to
%                     A= diag(nu)^{-1} * ( A- repmat(mu, m,1) )
%
% Such normalization is done implicitly
%     This implicit normalization is suggested for the sparse matrix
%                                    but not for the dense matrix
%

if (opts.nFlag~=0)
    if (isfield(opts,'mu'))
        mu=opts.mu;
        if(size(mu,2)~=n)
            error('\n Check the input .mu');
        end
    else
        mu=mean(A,1);
    end

    if (opts.nFlag==1)
        if (isfield(opts,'nu'))
            nu=opts.nu;
            if(size(nu,1)~=n)
                error('\n Check the input .nu!');
            end
        else
            nu=(sum(A.^2,1)/m).^(0.5); nu=nu';
        end
    else % .nFlag=2
        if (isfield(opts,'nu'))
            nu=opts.nu;
            if(size(nu,1)~=m)
                error('\n Check the input .nu!');
            end
        else
            nu=(sum(A.^2,2)/n).^(0.5);
        end
    end

    ind_zero=find(abs(nu)<= 1e-10);    nu(ind_zero)=1;
    % If some values in nu is typically small, it might be that,
    % the entries in a given row or column in A are all close to zero.
    % For numerical stability, we set the corresponding value to 1.
end

if (~issparse(A)) && (opts.nFlag~=0)
    fprintf('\n -----------------------------------------------------');
    fprintf('\n The data is not sparse or not stored in sparse format');
    fprintf('\n The code still works.');
    fprintf('\n But we suggest you to normalize the data directly,');
    fprintf('\n for achieving better efficiency.');
    fprintf('\n -----------------------------------------------------');
end

%% Group & Others

% Initialize ind and q
if (~isfield(opts,'ind'))
    error('\n In glLeastR, the field .ind should be specified');
else
    ind=opts.ind;

    k=length(ind)-1; % the number of groups

    if (ind(k+1)~=n)
        error('\n Check opts.ind');
    end
end

if (~isfield(opts,'q'))
    q=2; opts.q=2;
else
    q=opts.q;
    if (q<1)
        error('\n q should be larger than 1');
    end
end

% gWeight: the weigtht for each group
if (isfield(opts,'gWeight'))
    gWeight=opts.gWeight;
    if (size(gWeight,1)~=k)
        error('\n opts.gWeight should a %d x 1 vector',k);
    end

    if (min(gWeight)<=0)
        error('\n .gWeight should be positive');
    end
else
    gWeight=ones(k,1);
end

%% Starting point initialization

% compute AT y
if (opts.nFlag==0)
    ATy =A'*y;
elseif (opts.nFlag==1)
    ATy= A'*y - sum(y) * mu';  ATy=ATy./nu;
else
    invNu=y./nu;              ATy=A'*invNu-sum(invNu)*mu';
end

% process the regularization parameter
if (opts.rFlag==0)
    lambda=z;
else % z here is the scaling factor lying in [0,1]
    if (z<0 || z>1)
        error('\n opts.rFlag=1, and z should be in [0,1]');
    end

    if q==1
        q_bar=Inf;
    elseif q>=1e6
        q_bar=1;
    else
        q_bar=q/(q-1);
    end

    % compute the norm of ATy corresponding to each group
    norm_ATy=zeros(k,1);
    for i=1:k
        norm_ATy(i,1)=norm(  ATy( (1+ind(i)):ind(i+1) ), q_bar );
    end

    % incorporate the gWeight
    norm_ATy=norm_ATy./gWeight;

    % compute lambda_max
    lambda_max=max(norm_ATy);

    % As .rFlag=1, we set lambda as a ratio of lambda_max
    lambda=z*lambda_max;
end

% initialize a starting point
if opts.init==2
    x=zeros(n,1);
else
    if isfield(opts,'x0')
        x=opts.x0;
        if (length(x)~=n)
            error('\n Check the input .x0');
        end
    else
        x=ATy;  % if .x0 is not specified, we use ratio*ATy,
        % where ratio is a positive value
    end
end

% compute A x
if (opts.nFlag==0)
    Ax=A* x;
elseif (opts.nFlag==1)
    invNu=x./nu; mu_invNu=mu * invNu;
    Ax=A*invNu -repmat(mu_invNu, m, 1);
else
    Ax=A*x-repmat(mu*x, m, 1);     Ax=Ax./nu;
end

if (opts.init==0) % If .init=0, we set x=ratio*x by "initFactor"
    % Please refer to the function initFactor for detail

    % the q-norm of x
    x_norm=zeros(k,1);
    for i=1:k
        x_norm(i,1)=norm(  x( (1+ind(i)):ind(i+1) ), q);
    end

    sum_x_norm=x_norm'*gWeight;

    if x_norm>=1e-10
        ratio=initFactor(sum_x_norm, Ax, y, lambda,'glLeastR');
        x=ratio*x;    Ax=ratio*Ax;
    end
end

%% The main program
% The Armijo Goldstein line search schemes + accelearted gradient descent

bFlag=0; % this flag tests whether the gradient step only changes a little

if (opts.mFlag==0 && opts.lFlag==0)
    L=1;
    % We assume that the maximum eigenvalue of A'A is over 1

    % assign xp with x, and Axp with Ax
    xp=x; Axp=Ax; xxp=zeros(n,1);

    alphap=0; alpha=1;

    for iterStep=1:opts.maxIter
        % --------------------------- step 1 ---------------------------
        % compute search point s based on xp and x (with beta)
        beta=(alphap-1)/alpha;    s=x + beta* xxp;

        % --------------------------- step 2 ---------------------------
        % line search for L and compute the new approximate solution x

        % compute the gradient (g) at s
        As=Ax + beta* (Ax-Axp);

        % compute AT As
        if (opts.nFlag==0)
            ATAs=A'*As;
        elseif (opts.nFlag==1)
            ATAs=A'*As - sum(As) * mu';  ATAs=ATAs./nu;
        else
            invNu=As./nu;                ATAs=A'*invNu-sum(invNu)*mu';
        end

        % obtain the gradient g
        g=ATAs-ATy;

        % copy x and Ax to xp and Axp
        xp=x;    Axp=Ax;

        while (1)
            % let s walk in a step in the antigradient of s to get v
            % and then do the L1/Lq-norm regularized projection
            v=s-g/L;

            % L1/Lq-norm regularized projection
            if (q<1e6)
                x=eppVector(v, ind, k, n, lambda/ L * gWeight, q);
            else % when q>=1e6, we treat q as inf
                x=eppVector(v, ind, k, n, lambda/ L * gWeight, 1e6);
            end

            v=x-s;  % the difference between the new approximate solution x
            % and the search point s

            % compute A x
            if (opts.nFlag==0)
                Ax=A* x;
            elseif (opts.nFlag==1)
                invNu=x./nu; mu_invNu=mu * invNu;
                Ax=A*invNu -repmat(mu_invNu, m, 1);
            else
                Ax=A*x-repmat(mu*x, m, 1);     Ax=Ax./nu;
            end

            Av=Ax -As;
            r_sum=v'*v; l_sum=Av'*Av;

            if (r_sum <=1e-20)
                bFlag=1; % this shows that, the gradient step makes little improvement
                break;
            end

            % the condition is ||Av||_2^2 <= L * ||v||_2^2
            if(l_sum <= r_sum * L)
                break;
            else
                L=max(2*L, l_sum/r_sum);
                %fprintf('\n L=%5.6f',L);
            end
        end

        % --------------------------- step 3 ---------------------------
        % update alpha and alphap, and check whether converge
        alphap=alpha; alpha= (1+ sqrt(4*alpha*alpha +1))/2;

        xxp=x-xp;   Axy=Ax-y;

        ValueL(iterStep)=L;

        % the q-norm of x
        x_norm=zeros(k,1);
        for i=1:k
            x_norm(i,1)=norm(  x( (1+ind(i)):ind(i+1) ), q);
        end

        % function value = loss + regularizatioin
        funVal(iterStep)=Axy'* Axy/2 + lambda * x_norm'* gWeight;

        if (bFlag)
            % fprintf('\n The program terminates as the gradient step changes the solution very small.');
            break;
        end

        switch(opts.tFlag)
            case 0
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <= opts.tol)
                        break;
                    end
                end
            case 1
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <=...
                            opts.tol* funVal(iterStep-1))
                        break;
                    end
                end
            case 2
                if ( funVal(iterStep)<= opts.tol)
                    break;
                end
            case 3
                norm_xxp=sqrt(xxp'*xxp);
                if ( norm_xxp <=opts.tol)
                    break;
                end
            case 4
                norm_xp=sqrt(xp'*xp);    norm_xxp=sqrt(xxp'*xxp);
                if ( norm_xxp <=opts.tol * max(norm_xp,1))
                    break;
                end
            case 5
                if iterStep>=opts.maxIter
                    break;
                end
        end
    end
end

%% Reformulated problem + Nemirovski's line search
%
%  Original problem:
%  min  1/2 || A x - y||^2 + z * sum_j ||x^j||_q
%
%  Reformulated problem
%  min  1/2 || A x - y||^2  + z * gWeight' * t
%  ||x^j||_q <=t_j
%
%  We only deal with q=2

if (opts.mFlag==1 && opts.lFlag==0 && opts.q==2)

    L=1;
    % We assume that the maximum eigenvalue of A'A is over 1

    % assign xp with x, and Axp with Ax
    xp=x; Axp=Ax; xxp=zeros(n,1);

    t=zeros(k,1);
    for i=1:k
        t(i,1)=norm(  x( (1+ind(i)):ind(i+1) ), 2);
    end
    tp=t;

    alphap=0; alpha=1;

    for iterStep=1:opts.maxIter
        % --------------------------- step 1 ---------------------------
        % compute search point s based on xp and x (with beta)
        beta=(alphap-1)/alpha;    s=x + beta* xxp; s_t=t + beta * (t-tp);

        % --------------------------- step 2 ---------------------------
        % line search for L and compute the new approximate solution x

        % compute the gradient (g) at s
        As=Ax + beta* (Ax-Axp);

        % compute AT As
        if (opts.nFlag==0)
            ATAs=A'*As;
        elseif (opts.nFlag==1)
            ATAs=A'*As - sum(As) * mu';  ATAs=ATAs./nu;
        else
            invNu=As./nu;                ATAs=A'*invNu-sum(invNu)*mu';
        end

        % obtain the gradient g
        g=ATAs-ATy;

        % copy x and Ax to xp and Axp
        xp=x;    Axp=Ax;
        tp=t;

        while (1)
            % let s walk in a step in the antigradient of s to get v
            % and then do the L1/Lq-norm regularized projection
            u=s-g/L; v= s_t - lambda * gWeight / L;

            % projection
            [x, t] = eppVectorR(u, v, ind, n, k);

            v=x-s;  % the difference between the new approximate solution x
            % and the search point s

            % compute A x
            if (opts.nFlag==0)
                Ax=A* x;
            elseif (opts.nFlag==1)
                invNu=x./nu; mu_invNu=mu * invNu;
                Ax=A*invNu -repmat(mu_invNu, m, 1);
            else
                Ax=A*x-repmat(mu*x, m, 1);     Ax=Ax./nu;
            end

            Av=Ax -As;
            r_sum=v'*v + norm(t-s_t)^2; l_sum=Av'*Av;

            if (r_sum <=1e-20)
                bFlag=1; % this shows that, the gradient step makes little improvement
                break;
            end

            % the condition is ||Av||_2^2 <= L * ||v||_2^2
            if(l_sum <= r_sum * L)
                break;
            else
                L=max(2*L, l_sum/r_sum);
                %fprintf('\n L=%5.6f',L);
            end
        end

        % --------------------------- step 3 ---------------------------
        % update alpha and alphap, and check whether converge
        alphap=alpha; alpha= (1+ sqrt(4*alpha*alpha +1))/2;

        ValueL(iterStep)=L;

        xxp=x-xp;   Axy=Ax-y;

        % function value = loss + regularizatioin
        funVal(iterStep)=Axy'* Axy/2 + lambda * t'* gWeight;

        if (bFlag)
            % fprintf('\n The program terminates as the gradient step changes the solution very small.');
            break;
        end

        switch(opts.tFlag)
            case 0
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <= opts.tol)
                        break;
                    end
                end
            case 1
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <=...
                            opts.tol* funVal(iterStep-1))
                        break;
                    end
                end
            case 2
                if ( funVal(iterStep)<= opts.tol)
                    break;
                end
            case 3
                norm_xxp=sqrt(xxp'*xxp);
                if ( norm_xxp <=opts.tol)
                    break;
                end
            case 4
                norm_xp=sqrt(xp'*xp);    norm_xxp=sqrt(xxp'*xxp);
                if ( norm_xxp <=opts.tol * max(norm_xp,1))
                    break;
                end
            case 5
                if iterStep>=opts.maxIter
                    break;
                end
        end
    end
end

%% adaptive line search

% .mFlag=1, and .lFlag=1
%  refomulate the problem as the constrained convex optimization
%  problem, and then apply adaptive line search scheme

% Problem:
%    min  1/2 || A x - y||^2 + z * t' * gWeight
%    s.t.   |x| <= t

if(opts.mFlag==1 && opts.lFlag==1 && opts.q==2)

    L=1;
    % We assume that the maximum eigenvalue of A'A is over 1

    gamma=1;
    % we shall set the value of gamma = L,
    % where L is appropriate for the starting point

    xp=x; Axp=Ax;
    % store x and Ax
    xxp=zeros(n,1);
    % the difference of x and xp

    t=zeros(k,1);
    for i=1:k
        t(i,1)=norm(  x( (1+ind(i)):ind(i+1) ), 2);
    end
    tp=t;
    % t is the upper bound of the 2-norm of x

    % compute AT Ax
    if (opts.nFlag==0)
        ATAx=A'*Ax;
    elseif (opts.nFlag==1)
        ATAx=A'*Ax - sum(Ax) * mu';  ATAx=ATAx./nu;
    else
        invNu=Ax./nu;                ATAx=A'*invNu-sum(invNu)*mu';
    end

    % We begin the adaptive line search in the following
    %
    % Note that, in the line search, L and beta are changing

    for iterStep=1:opts.maxIter

        ATAxp=ATAx;
        % store ATAx to ATAxp

        if (iterStep~=1)
            % compute AT Ax
            if (opts.nFlag==0)
                ATAx=A'*Ax;
            elseif (opts.nFlag==1)
                ATAx=A'*Ax - sum(Ax) * mu';  ATAx=ATAx./nu;
            else
                invNu=Ax./nu;                ATAx=A'*invNu-sum(invNu)*mu';
            end
        end

        %--------- Line Search for L begins
        while (1)
            if (iterStep~=1)
                alpha= (-gamma+ sqrt(gamma*gamma + 4* L * gamma)) / (2*L);
                beta= (gamma - gamma* alphap) / (alphap * gamma + alphap* L * alpha);
                % beta is the coefficient for generating search point s

                s=x + beta* xxp;   s_t= t + beta * (t -tp);
                As=Ax + beta* (Ax-Axp);
                ATAs=ATAx + beta * (ATAx- ATAxp);
                % compute the search point s, A * s, and A' * A * s
            else
                alpha= (-1+ sqrt(5)) / 2;
                beta=0; s=x; s_t=t; As=Ax; ATAs=ATAx;
            end

            g=ATAs-ATy;
            % compute the gradient g

            % let s walk in a step in the antigradient of s
            u=s-g/L; v= s_t - lambda * gWeight / L;

            % projection
            [xnew, tnew] = eppVectorR(u, v, ind, n, k);

            v=xnew-s;  % the difference between the new approximate solution x
                            % and the search point s
            v_t=tnew-s_t;

            % compute A xnew
            if (opts.nFlag==0)
                Axnew=A* xnew;
            elseif (opts.nFlag==1)
                invNu=xnew./nu; mu_invNu=mu * invNu;
                Axnew=A*invNu -repmat(mu_invNu, m, 1);
            else
                Axnew=A*xnew-repmat(mu*xnew, m, 1);     Axnew=Axnew./nu;
            end

            Av=Axnew -As;
            r_sum=v'*v + v_t'*v_t; l_sum=Av'*Av;

            if (r_sum <=1e-20)
                bFlag=1; % this shows that, the gradient step makes little improvement
                break;
            end

            % the condition is ||Av||_2^2
            %                       <= L * (||v||_2^2 + ||v_t|| _2^2 )
            if(l_sum <= r_sum * L)
                break;
            else
                L=max(2*L, l_sum/r_sum);
                % fprintf('\n L=%5.6f',L);
            end
        end
        %--------- Line Search for L ends

        gamma=L* alpha* alpha;    alphap=alpha;
        % update gamma, and alphap

        ValueL(iterStep)=L;

        tao=L * r_sum / l_sum;
        if (tao >=5)
            L=L*0.8;
        end
        % decrease the value of L

        xp=x;    x=xnew; xxp=x-xp;
        Axp=Ax;  Ax=Axnew;
        % update x and Ax with xnew and Axnew
        tp=t; t=tnew;
        % update tp and t

        Axy=Ax-y;
        funVal(iterStep)=Axy'* Axy/2 + lambda * t'* gWeight;
        % compute function value

        if (bFlag)
            % fprintf('\n The program terminates as the gradient step changes the solution very small.');
            break;
        end

        switch(opts.tFlag)
            case 0
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <= opts.tol)
                        break;
                    end
                end
            case 1
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <=...
                            opts.tol* funVal(iterStep-1))
                        break;
                    end
                end
            case 2
                if ( funVal(iterStep)<= opts.tol)
                    break;
                end
            case 3
                norm_xxp=sqrt(xxp'*xxp+ norm(t-tp)^2);
                if ( norm_xxp <=opts.tol)
                    break;
                end
            case 4
                norm_xp=sqrt(xp'*xp + tp'*tp);    norm_xxp=sqrt(xxp'*xxp+ norm(t-tp)^2);
                if ( norm_xxp <=opts.tol * max(norm_xp,1))
                    break;
                end
            case 5
                if iterStep>=opts.maxIter
                    break;
                end
        end
    end
end

%%
if(opts.mFlag==0 && opts.lFlag==1)
    error('\n The function does not support opts.mFlag=0 & opts.lFlag=1!');
end


function nP=spacing2numVertices(F,V,pointSpacing)

A=sum(patchArea(F,V)); %Total area
At=(pointSpacing.^2*sqrt(3))/4; %Theoretical area of equilateral triangle
NF=(A/At);

E=patchEdges(F,1); %Edges
nE=size(E,1); %Number of edges
nF=size(F,1); %Number of faces
nV=size(V,1); %Number of vertices

%Compute Euler characteristic
X=size(V,1)-size(E,1)+size(F,1);

nRefScalar=(log(NF)-log(nF))/log(4);

if nRefScalar<0
    nRef=floor(nRefScalar);
    nRange=0:-1:nRef;
else
    nRef=ceil(nRefScalar);
    nRange=0:1:nRef;
end

nvR=nV.*ones(numel(nRange),1);
neR=nE.*ones(numel(nRange),1);

nfR=nF*4.^nRange';
for q=2:1:numel(nRange)
    if nRef>0
        nvR(q)=nvR(q-1)+neR(q-1);
    elseif nRef<0
        nvR(q)=(nvR(q-1)+X-nfR(q))/2;
    end
    neR(q)=-X+nfR(q)+nvR(q);
end

% [nvR -neR nfR]
% sum([nvR -neR nfR],2)

nP=ceil(interp1(nfR(:),nvR(:),NF,'pchip'));
%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function [alg] = learnDisturbanceQuantile(input_signal, alg)

%__________________________________________________________________________
%% Documentation
%
% Authors:      Alexander Wischnewski (alexander.wischnewski@tum.de)
%
% Start Date:   21.11.2019
% Last Update:  12.03.2020
%
% Description:
%   learns a quantile estimate for the given input signal using a batch algorithm (Wischnewski, 2020)
%
% Inputs:
%   input_signal    Input signal used for quantile estimation
%   alg             algorithm intermediate results and parameters
%
% Algorithm intermediate results and parameters
%   alg.r_est               Estimate for the q-quantile
%   alg.q_target            Target percentage of the quantile
%   alg.lambda              Learning rate
%   alg.N                   Samples for one batch
%   alg.count_lower         Count of samples in this batch lower than the quantile estimate
%   alg.count               Count of samples in this batch
%__________________________________________________________________________
%% Algorithm

% if the input signal is smaller than the current quantile estimate,
% increase counter of signals lower than quantile estimate. This is used to estimate the
% percentage of the data covered by the estimate.
if(input_signal < alg.r_est)
    alg.count_lower = alg.count_lower + 1;
end
alg.count = alg.count + 1;
%
% if the batch size is reached, update the estimate
if(alg.count >= alg.N)
    alg.r_est = alg.r_est - alg.lambda*(alg.count_lower/alg.count - alg.q_target);
    alg.count_lower = 0;
    alg.count = 0;
end


% THE QUATERNION DIFFERENCE
function [dq] = qDifference(q1,q2)
% This function gets the quaternion that will rotate from the
% q1 orientation to the q2 orientation [q1->q2].

assert(size(q1,1) == 4,'The initial quaternion (q1) must be a 4x1 column vector')
assert(size(q1,1) == 4,'The terminal quaternion (q2) must be a 4x1 column vector')

% Get the quaternion inverse
[q1_inv] = qInverse(q1);
% Calculate difference
dq = qMultiply(q1_inv,q2);
% Re-normalise
dq = unit(dq);
end


function out = ICC(cse,typ,dat)
%function to work out ICCs according to shrout & fleiss' schema (Shrout PE,
%Fleiss JL. Intraclass correlations: uses in assessing rater reliability.
%Psychol Bull. 1979;86:420-428).
%
% 'dat' is data whose rows represent different ratings or raters & whose
% columns represent different cases or targets being measured. Each target
% is assumed to be a random sample from a population of targets.
%
% 'cse' is either 1,2,3. 'cse' is: 1 if each target is measured by a
% different set of raters from a population of raters, 2 if each target is
% measured by the same raters, but that these raters are sampled from a
% population of raters, 3 if each target is measured by the same raters and
% these raters are the only raters of interest.
%
% 'typ' is either 'single' or 'k' & denotes whether the ICC is based on a
% single measurement or on an average of k measurements, where k = the
% number of ratings/raters.
%
% This has been tested using the example data in the paper by shrout & fleiss.
%
% Example: out = ICC(3,'k',S_Fdata)
% returns ICC(3,k) of data 'S_Fdata' to double 'out'.
%
% Kevin Brownhill, Imaging Sciences, KCL, London kevin.brownhill@kcl.ac.uk
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

%number of raters/ratings
k = size(dat,2);
%number of targets
n = size(dat,1);
%mean per target
mpt = mean(dat,2);
%mean per rater/rating
mpr = mean(dat);
%get total mean
tm = mean(mpt);
%within target sum sqrs
WSS = sum(sum(bsxfun(@minus,dat,mpt).^2));
%within target mean sqrs
WMS = WSS / (n * (k - 1));
%between rater sum sqrs
RSS = sum((mpr - tm).^2) * n;
%between rater mean sqrs
RMS = RSS / (k - 1);
% %get total sum sqrs
% TSS = sum(sum((dat - tm).^2));
%between target sum sqrs
BSS = sum((mpt - tm).^2) * k;
%between targets mean squares
BMS = BSS / (n - 1);
%residual sum of squares
ESS = WSS - RSS;
%residual mean sqrs
EMS = ESS / ((k - 1) * (n - 1));
switch cse
    case 1
        switch typ
            case 'single'
                out = (BMS - WMS) / (BMS + (k - 1) * WMS);
            case 'k'
                out = (BMS - WMS) / BMS;
            otherwise
               error('Wrong value for input typ')
        end
    case 2
        switch typ
            case 'single'
                out = (BMS - EMS) / (BMS + (k - 1) * EMS + k * (RMS - EMS) / n);
            case 'k'
                out = (BMS - EMS) / (BMS + (RMS - EMS) / n);
            otherwise
               error('Wrong value for input typ')
        end
    case 3
        switch typ
            case 'single'
                out = (BMS - EMS) / (BMS + (k - 1) * EMS);
            case 'k'
                out = (BMS - EMS) / BMS;
            otherwise
               error('Wrong value for input typ')
        end
    otherwise
        error('Wrong value for input cse')
end


%this is a simple example, demonstrating delay estimation. Estimates the
%delay of yn1 with respect to yn0 in the presence of noise.

N_p=256;%number of points;

d=5*rand;%delay

y0=randn(1,N_p);%reference signal

y1=fft_circshift(y0,d);%perform circular shift

yn0=0.05*randn(1,N_p);%noise
yn1=0.05*randn(1,N_p);%noise

y0n=y0+yn0;%add noise to signals
y1n=y1+yn1;

alpha=-1;%tuning parameter for modified gaussian method
%(only used in the case of negative xc if alpha is negative, otherwise
%always use it. See delayest_3point.m)

%estimate delays
d_hat_parabola=delayest_3point(y1n,y0n,'parabola','xc');
d_hat_Gaussian=real(delayest_3point(y1n,y0n,'Gaussian','xc'));
d_hat_modGaussian=delayest_3point(y1n,y0n,'modGaussian','xc',alpha);
d_hat_cosine=real(delayest_3point(y1n,y0n,'cosine','xc'));
d_hat_phase=delayest_fft(y1n,y0n);
d_hat_iterative=delayest_iterative(y1n,y0n);

fprintf('Delay=%f samples\n',d);
fprintf('Estimates (samples):\nparabola %f\nGaussian %f\nmodified Gaussian %f\ncosine %f\nphase %f\niterative %f\n',...
    d_hat_parabola,d_hat_Gaussian,d_hat_modGaussian,d_hat_cosine,d_hat_phase,d_hat_iterative);

figure(1)
plot([y0n;y1n]','.-')


function lm = me_multin(Ns, epsil, A);
% Returns ldpc achevability data for a given (n,epsil,A)
cap = log2(1+A^2)/2;
Pdb = 20*log10(A);

lm = [];
for n = Ns;
	% these are absolute bounds on rate
	Rmin = 25/74; Rmax = 25/32;
	Rmax = min(Rmax, cap);
	% Step 1: find minimal acceptable rate
	Ptry = +Inf;
	Rnext = Rmin; Rstep = 0.1;
	while ~(Ptry <= Pdb)
		Rtry = Rnext;
		Ktry = floor(n*Rtry);
		[a b] = compute_ab(Ktry, Rtry);
		Ptry = (-norminv(epsil))/a + b;
		%% disp(sprintf('   Rtry = %.3g, Ktry = %d, Ptry = %.3g', Rtry, Ktry, Ptry));
		Rnext = Rnext + Rstep;
		if(Rnext > Rmax)
			if (Rstep == 0.1)
				Rnext = Rmin;
				Rstep = 0.01;
			else
				disp(sprintf(['me_multin(n = %d, epsil = %g, P = %.2g dB:' ...
						' could not find minrate'], n, epsil, Pdb));
				Rtry = Inf;
				break;
			end
		end
	end
	if(Rtry == Inf)
		lm = [lm NaN];
		continue;
	end
	Rmin = Rtry;
	Kmin = Ktry;
	Pmin = Ptry;
	% Step 2: find maximal acceptable rate

	Ptry = -Inf;
	Rnext = Rmax; Rstep = 0.1;
	while ~(Ptry >= Pdb)
		Rtry = Rnext;
		Ktry = ceil(n*Rtry);
		[a b] = compute_ab(Ktry, Rtry);
		Ptry = (-norminv(epsil))/a + b;
		Rnext = Rnext - Rstep;
		if(Rnext < Rmin)
			if (Rstep == 0.1)
				Rnext = Rmax;
				Rstep = 0.01;
			else
				disp(sprintf(['me_multin(n = %d, epsil = %g, P = %.2g dB:' ...
						' could not find maxrate'], n, epsil, Pdb));
				Rtry = Inf;
				break;
			end
		end
	end
	if(Rtry == Inf)
		lm = [lm NaN];
		continue;
	end
	Rmax = Rtry;
	Kmax = Ktry;
	Pmax = Ptry;
	% Step 3: proceed by binary division

	disp(sprintf(['me_multin(n = %d, eps = %g, P = %.2g dB: R = (%.3g, %.3g), '...
			'K = (%d, %d), P = (%.3g, %.3g)'], ...
			n, epsil, Pdb, Rmin, Rmax, Kmin, Kmax, Pmin, Pmax));

	while (Kmax - Kmin) > 1
		Rtry = (Rmin + Rmax)/2;
		Ktry = floor(Rtry*n);
		[a b] = compute_ab(Ktry, Rtry);
		Ptry = (-norminv(epsil))/a + b;
		%% disp(sprintf('   Rtry = %.3g, Ktry = %d, Ptry = %.3g', Rtry, Ktry, Ptry));
		if(Ptry == NaN) break; end
		if(Ptry > Pdb)
			Rmax = Rtry; Kmax = Ktry;
		else
			Rmin = Rtry; Kmin = Ktry;
		end
	end
	reason = 'PREC OK:';
	if(Ptry == NaN)
		reason = 'NAN BREAK:';
	end
	disp(sprintf('     %s lm = %d', reason, Kmin));
	lm = [lm Kmin];
end


function x = r83_cr_sl ( n, a_cr, b )

%*****************************************************************************80
%
%% R83_CR_SL solves a real linear system factored by R83_CR_FA.
%
%  Discussion:
%
%    The matrix A must be tridiagonal.  R83_CR_FA is called to compute the
%    LU factors of A.  It does so using a form of cyclic reduction.  If
%    the factors computed by R83_CR_FA are passed to R83_CR_SL, then one or many
%    linear systems involving the matrix A may be solved.
%
%    Note that R83_CR_FA does not perform pivoting, and so the solution
%    produced by R83_CR_SL may be less accurate than a solution produced
%    by a standard Gauss algorithm.  However, such problems can be
%    guaranteed not to occur if the matrix A is strictly diagonally
%    dominant, that is, if the absolute value of the diagonal coefficient
%    is greater than the sum of the absolute values of the two off diagonal
%    coefficients, for each row of the matrix.
%
%  Example:
%
%    Here is how a R83 matrix of order 5 would be stored:
%
%       *  A12 A23 A34 A45
%      A11 A22 A33 A44 A55
%      A21 A32 A43 A54  *
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 March 2004
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Roger Hockney,
%    A fast direct solution of Poisson's equation using Fourier Analysis,
%    Journal of the ACM,
%    Volume 12, Number 1, pages 95-113, January 1965.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Input, real A_CR(3,2*N+1), factorization information computed by R83_CR_FA.
%
%    Input, real B(N), the right hand side vector.
%
%    Output, real X(N), the solution of the linear system.
%
  if ( n <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R83_CR_SL - Fatal error!\n' );
    fprintf ( 1, '  Nonpositive N = %d\n', n );
    return
  end

  x = zeros ( n, 1 );

  if ( n == 1 )
    x(1) = a_cr(2,2) * b(1);
    return
  end
%
%  Set up RHS.
%
  rhs = zeros ( 2 * n + 1, 1 );

  rhs(1) = 0.0;
  rhs(2:n+1) = b(1:n);
  rhs(n+2:2*n+1) = 0.0;

  il = n;
  ndiv = 1;
  ipntp = 0;

  while ( 1 < il )

    ipnt = ipntp;
    ipntp = ipntp + il;
    il = floor ( il / 2 );
    ndiv = ndiv * 2;
    ihaf = ipntp;

    for iful = ipnt + 2 : 2 : ipntp
      ihaf = ihaf + 1;
      rhs(ihaf+1) = rhs(iful+1) - a_cr(3,iful) * rhs(iful) ...
        - a_cr(1,iful+1) * rhs(iful+2);
    end

  end

  rhs(ihaf+1) = rhs(ihaf+1) * a_cr(2,ihaf+1);
  ipnt = ipntp;

  while ( 0 < ipnt )

    ipntp = ipnt;
    ndiv = floor ( ndiv / 2 );
    il = floor ( n / ndiv );
    ipnt = ipnt - il;
    ihaf = ipntp;

    for ifulm = ipnt + 1 : 2 : ipntp
      iful = ifulm + 1;
      ihaf = ihaf + 1;
      rhs(iful+1) = rhs(ihaf+1);
      rhs(ifulm+1) = a_cr(2,ifulm+1) * ( rhs(ifulm+1) ...
        - a_cr(3,ifulm) * rhs(ifulm) ...
        - a_cr(1,ifulm+1) * rhs(iful+1) );
    end

  end

  x(1:n) = rhs(2:n+1);

  return
end


function [H,V] = lp_arn_m(name,Bf,Kf,k,r)
%
%  Arnoldi method w.r.t. inv(F), where F = A-Bf*Kf'.
%
%  Calling sequence:
%
%    [H,V] = lp_arn_m(name,Bf,Kf,k)
%    [H,V] = lp_arn_m(name,Bf,Kf,k,r)
%
%  Input:
%
%    name      basis name of the m-file which generates matrix
%              operations with A, e.g., 'as';
%    Bf        matrix Bf;
%              Set Bf = [] if not existing or zero!
%    Kf        matrix Kf;
%              Set Kf = [] if not existing or zero!
%    k         number of Arnoldi steps (usually k << n, where
%              n is the order of the system);
%    r         initial n-vector
%              (optional - chosen by random, if omitted).
%
%  Output:
%
%    H         matrix H ((k+1)-x-k matrix, upper Hessenberg);
%    V         matrix V (n-x-(k+1) matrix, orthogonal columns).
%
%  User-supplied functions called by this function:
%
%    '[name]_m', '[name]_i'
%
%  Method:
%
%    The ("inverse") Arnoldi method produces matrices V and H such that
%
%      V(:,1) in span{r},
%      V'*V = eye(k+1),
%      inv(F)*V(:,1:k) = V*H.
%
%  Remark:
%
%    This implementation does not check for (near-)breakdown!
%
%
%  LYAPACK 1.0 (Thilo Penzl, May 1999)

% Input data not completely checked!

na = nargin;

with_BK = length(Bf)>0;

eval(lp_e( 'n = ',name,'_m;' ));                    % Get system order.
if k >= n-1, error('k must be smaller than the order of A!'); end
if na<5, r = randn(n,1); end

V = zeros(n,k+1);
H = zeros(k+1,k);

V(:,1) = (1.0/norm(r))*r;

beta = 0;

if with_BK,      % SM = inv(F)*Bf*inv(I-Kf'*inv(F)*Bf)
                 % (This is the main part of the term needed for the low
                 % rank correction in the Sherman-Morrison formula.)
  Im = eye(size(Bf,2));
  TM = Bf;
  eval(lp_e( 'TM = ',name,'_l(''N'',TM);' ));
  SM = TM/(Im-Kf'*TM);
end

for j = 1:k

  if j > 1
    H(j,j-1) = beta;
    V(:,j) = (1.0/beta)*r;
  end

  w = V(:,j);
  eval(lp_e( 'w = ',name,'_l(''N'',w);' ));
  if with_BK, w = w + SM*(Kf'*w); end     % LR correction by SM formula
  r = w;

  for i = 1:j
    H(i,j) = V(:,i)'*w;
    r = r-H(i,j)*V(:,i);
  end

  beta = norm(r);
  H(j+1,j) = beta;

end

V(:,k+1) = (1.0/beta)*r;


function [Cnorm, Curv, Mu, Sigma] = dtiComputeFiberRadiusofCurvatureDistribution(fg,displayDist)

% Calculate the mean curvature of individual fiber first, and then make a
% summary across fibers.
%
% [Cnorm, Curv, Mu, Sigma]= dtiComputeFiberRadiusofCurvatureDistribution(fg,displayDist)
%
% INPUTS:
% fibers   = A fiber group.
% displayDist = 1: Display the distribution plot. 0: Not display.
%
% Outputs:
% Cnorm    = Normalized radius of curvature of each fiber, in units of z-score.
% Curvature      = The mean radius of curvature of each fiber.
%
% Written by Hiromasa Takemura (c) Stanford University 2014

if notDefined('fg'), error('Fiber group required'); end

% Compute the curvature of fibers in individual nodes.
[fibercurvature] = dtiComputeFiberCurvature(fg);

% Calculate the mean of curvature in individual fibers.

for i = 1:length(fibercurvature)
   Curv(i) = 1./mean(fibercurvature{i});

end

% Z-score the curvature
% Here we take the log first so that the distribution of Curv is 'more'
% gaussian and because it it not clipped at 0.
[Cnorm, Mu, Sigma] = zscore(log10(Curv));

% Show a histagram
if displayDist==1
    mrvNewGraphWin('Distibutions of fiber lengths');
    hold on;
    [y, x] = hist(Curv,round(length(fg.fibers)*0.1));
    bar(x,y,'FaceColor','k','EdgeColor','k');
    axis([min(x) max(x) 0 max(y)]);
    xlabel('Curvature');
    plot(10.^[Mu Mu],[0 max(y)],'r','linewidth',2);
    plot(10.^[Mu-Sigma   Mu-Sigma],[0 max(y)],'--r');
    plot(10.^[Mu+Sigma   Mu+Sigma],[0 max(y)],'--r');
    plot(10.^[Mu-2*Sigma Mu-2*Sigma],[0 max(y)],'--r');
    plot(10.^[Mu+2*Sigma Mu+2*Sigma],[0 max(y)],'--r');
end

return


function TV = compute_total_variation(y, options)

% compute_total_variation - compute the total variation of an image
%
%   TV = compute_total_variation(y, options);
%
%   See also: perform_tv_projection, grad, div.
%
%   Copyright (c) 2007 Gabriel Peyre

options.null = 0;

nbdims = 2;
if size(y,1)==1 || size(y,2)==1
    nbdims = 1;
end
if size(y,1)>1 && size(y,2)>1 && size(y,3)>1
    nbdims = 3;
end

if nbdims==1
    TV = sum( abs(diff(y)) );
    return;
end

weight_tv = getoptions(options, 'weight_tv', ones(size(y)) );
tv_norm = getoptions(options,'tv_norm', 'l2');

% options.bound = 'per';
g = grad(y, options);

switch tv_norm
    case 'l2'
        TV = sqrt( sum(g.^2,nbdims+1) );
    case 'l1'
        TV = sum( abs(g),nbdims+1);
    case 'linf'
        TV = max( abs(g),[],nbdims+1);
    otherwise
        error('Unknown norm.');
end
TV = TV .* weight_tv;
TV = sum(TV(:));


function K = covLINone(hyp, x, z, i)

% Linear covariance function with a single hyperparameter. The covariance
% function is parameterized as:
%
% k(x^p,x^q) = (x^p'*x^q + 1)/t^2;
%
% where the P matrix is t2 times the unit matrix. The second term plays the
% role of the bias. The hyperparameter is:
%
% hyp = [ log(t) ]
%
% Copyright (c) by Carl Edward Rasmussen and Hannes Nickisch, 2010-09-10.
%
% See also COVFUNCTIONS.M.

if nargin<2, K = '1'; return; end                  % report number of parameters
if nargin<3, z = []; end                                   % make sure, z exists
xeqz = isempty(z); dg = strcmp(z,'diag');                       % determine mode

it2 = exp(-2*hyp);                                                  % t2 inverse

% precompute inner products
if dg                                                               % vector kxx
  K = sum(x.*x,2);
else
  if xeqz                                                 % symmetric matrix Kxx
    K = x*x';
  else                                                   % cross covariances Kxz
    K = x*z';
  end
end

if nargin<4                                                        % covariances
  K = it2*(1+K);
else                                                               % derivatives
  if i==1
    K = -2*it2*(1+K);
  else
    error('Unknown hyperparameter')
  end
end


function theta=Theta_ADMM(Y,W,H,X,L,alfa,beta,theta,rho,maxiter)

%%
%  Solve the JPLAY's subproblem: theta, using ADMM

%% Initializing Setting
epsilon = 1e-6;
iter=0;

G=zeros(size(theta));
Q=zeros(size(theta*X));
P=zeros(size(theta*X));
% M=zeros(size(theta*X));
lamda1=zeros(size(theta*X));
lamda2=zeros(size(theta));
lamda3=zeros(size(theta*X));
lamda4=zeros(size(theta*X));
% lamda5=zeros(size(theta*X));

stop = false;
mu=1e-3;
% rho=2;%1.4:Houston2018 2:Houston2013
mu_bar=1e+6;

GL=(X*L*X');%Graph Laplacian

  while ~stop && iter < maxiter+1

      iter=iter+1;
      %solve theta
      theta=(mu*(H*X')+lamda1*X'+mu*G+lamda2+mu*(Q*X')+lamda3*X'+mu*(P*X')+lamda4*X')/(3*mu*(X*X')+beta*GL+mu*eye(size(X*X')));
      %solve H
      H=(alfa*(W'*W)+(G*G')+mu*eye(size(W'*W)))\(alfa*(W'*Y)+(G*X)+mu*(theta*X)-lamda1);
      %solve G
      G=((H*H')+mu*eye(size(H*H')))\(mu*theta-lamda2+(H*X'));
      %solve Q
      Q=max(theta*X-(lamda3/mu),0);
      %solve P
      MidV=theta*X-lamda4/mu;
      for i=1:size(MidV,2)
          if norm(MidV(:,i))<=1
             P(:,i)=MidV(:,i);
          else
             P(:,i)=MidV(:,i)/norm(MidV(:,i));
          end
      end
%       M=max(abs(theta*(X-XW)-lamda5/mu)-(beta*0.01/mu),0).*sign(theta*(X-XW)-lamda5/mu);
     %update Lagrange multipliers
     lamda1=lamda1+mu*(H-theta*X);
     lamda2=lamda2+mu*(G-theta);
     lamda3=lamda3+mu*(Q-theta*X);
     lamda4=lamda4+mu*(P-theta*X);
%      lamda5=lamda5+mu*(M-theta*(X-XW));
     %update penalty parameter
     mu=min(mu*rho,mu_bar);
     %computer errors
     r_H=norm(H-theta*X,'fro');
     r_G=norm(G-theta,'fro');
     r_Q=norm(Q-theta*X,'fro');
     r_P=norm(P-theta*X,'fro');
%      r_M=norm(M-theta*(X-XW),'fro');
     %check the convergence conditions
     if r_H<epsilon&&r_G<epsilon&&r_Q<epsilon&&r_P<epsilon%&&r_M<epsilon
         stop = true;
         break;
     end
  end
end


%  Core QP Solver. Use qpng.m instead.
%
% This routine solves the following optimization problem:
%
% min_x .5x' H x + q' x
% s.t.  Aeq x =  beq
%       Ain x <= bin
%
% note that x0 is a feasible starting point.
%
% (C) Nicolo Giorgetti, 2006.
function [x, lam, k, status]=qpsolng(H, q, Aeq, beq, Ain, bin, x0)

nmax=200; % max number of iterations
tol=1e-6; % tolerance

neq=length(beq); % number of eqs
nin=length(bin); % number of ineqs

x=x0;
n=size(x);
Wact=[];
nact=0; % number of rows in Wact (only active inequalities)

status=1; % problem feasible
lam=zeros(neq+nin,1);
k=[];

for k=1:nmax
   % Construct KKT
   K=H;
   r=-q-H*x;
   if neq > 0
      % Add equality constraints
      A=Aeq;
   end
   if nin > 0
      % Add active inequality constraints
      for j=1:nact
         i=Wact(j);
         if j+neq==1
            A=Ain(i,:);
         else
            A=[A; Ain(i,:)];
         end
      end
   end
   nneq=neq+nact;
   if nneq>0
      K=[K, A'; A, zeros(nneq,nneq)];
      r=[r; zeros(nneq,1)];
   end

   y=K\r; %%%%%%%% Check this: we should use pinv instead. Possible numerical problems
   p=y(1:n);

   if norm(p)<tol
      % check optimality or add to work set
      if nact==0
         x=x+p;
         status=1;
         return % successfully
      else
         lam=y(n+neq+1:n+neq+nact);
         [lmin,arg]=min(lam);
         if lmin >= 0
            x=x+p;
            status=1;
            return; % successfully
         else
            % remove constraints from W
            nact=nact-1;
            for j=arg:nact
               Wact(j)=Wact(j+1);
            end
         end
      end
   else % x is not the minimizer in W
      count=nin+1;
      val(1:count)=1.1;
      for j=1:nin
         if Ain(j,1:n)*p > max(tol^2,1e-15)
            val(j)=(bin(j)-Ain(j,1:n)*x)/(Ain(j,1:n)*p);
         end
      end
      val(count)=1;

      [alpha,ind]=min(val); % this is the allowed step size

      x=x+alpha*p;

      if ind < count % there are blocking constraints, add one to W
         nact=nact+1;
         Wact(nact)=ind;
      end
   end
end

eq_infeas=0;
in_infeas=0;

if neq>0
   eq_infeas = (norm(Aeq*x-beq) > rtol*(1+norm(beq)));
end
if nin>0
   in_infeas = (any(Ain*x-bin > -rtol*(1+norm(bin))));
end

if eq_infeas | in_infeas
   status=2; % Max iterations reached, no solution found.
else
   status=3; % Max iterations reached but a feasible solution found.
end

return;


classdef StudentT2C
%%STUDENTT2C A collection of functions for the bivariate Student-t copulae.
%           Implemented functions include: PDF, tau.
%
%REFERENCES:
%[1] H. Joe and D. Kurowicka, Dependence Modeling: Vine Copula Handbook.
%    World Scientific, 2011.
%
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    methods(Static)
        function jprob = PDF(u,R,nu)
            %PDF Generates a joint probability for column vector u or
            %    a row vector of joint probabilities for matrix u
            %    with each column as the input vector.
            %
            %INPUTS:
            % u: A 2-by-n matrix where each column represents a
            %    2-dimensional random vector and n is the number of
            %    random vectors. Entries must be in [0,1].
            % R: A 2-by-2 correlation matrix, or a scalar which is the
            %    correlation.
            % nu: Scalar degrees of freedom.
            %
            %OUTPUTS: jprob: A 1-by-n vector of joint probabilities.
            %
            %EXAMPLE 1: Make a contour plot of the density.
            % x = linspace(0,1,1e2);
            % [X,Y] = meshgrid(x);
            % Z = zeros(length(X),length(Y));
            % for idx = 1:length(X)
            %     for iidx = 1:length(Y)
            %         Z(idx,iidx) = StudentT2C.PDF([X(idx,iidx);Y(idx,iidx)],0.5,3);
            %     end
            % end
            % contourf(X,Y,Z)
            %
            %EXAMPLE 2: Generate an exact joint density plot.
            % x = linspace(0,1,1e2);
            % [X,Y] = meshgrid(x);
            % Z = zeros(length(X),length(Y));
            % for idx = 1:length(X)
            % for iidx = 1:length(Y)
            % Z(idx,iidx) = StudentT2C.PDF([X(idx,iidx);Y(idx,iidx)],0.95,3);
            % X(idx,iidx) = GumbelD.invCDF(X(idx,iidx),1,2);
            % Y(idx,iidx) = GaussianD.invCDF(Y(idx,iidx));
            % Z(idx,iidx) = Z(idx,iidx).*GumbelD.PDF(X(idx,iidx),1,2).*GaussianD.PDF(Y(idx,iidx));
            % end
            % end
            % surf(X,Y,Z)
            %
            %October 2020 Codie T. Lewis, Naval Research Laboratory, Washington D.C.
            %
            if ~exist('u','var') || isempty(u)
                jprob = [];
                return
            end
            if ~exist('R','var') || isempty(R)
                rho = 0;
            elseif ~isscalar(R)
                rho = R(2,1);
            else
                rho = R;
            end

            a = StudentTD.invCDF(u(1,:),nu);
            b = StudentTD.invCDF(u(2,:),nu);
            c = (a.^2+b.^2-2*rho*a.*b)/(nu*(1-rho^2));

            jprob = (gamma(0.5*nu+1)/(gamma(nu/2)*nu*pi*sqrt(1-rho^2)))...
                    ./(StudentTD.PDF(a,0,1,nu).*StudentTD.PDF(b,0,1,nu).*(1+c).^(0.5*nu+1));

            %NaN results from 0*inf in the calculation.
            %These should be 0s.
            nanIdxs = isnan(jprob);
            jprob(nanIdxs) = 0;

        end

        function t = tau(R)
            %%TAU Compute Kendall's tau for the copula.
            %
            %INPUTS:
            % R: A 2-by-2 correlation matrix, or a scalar which is the
            %    correlation.
            %
            %OUTPUTS:
            % t: The scalar value of Kendall's tau.
            %
            %Note: This does not depend on the degrees of freedom nu.
            %
            %October 2020 Codie T. Lewis, Naval Research Laboratory, Washington D.C.
            %
            if ~exist('R','var') || isempty(R)
                rho = 0;
            elseif ~isscalar(R)
                rho = R(2,1);
            else
                rho = R;
            end

            t = 2*arcsin(rho)/pi;
        end
    end
end

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


function [H]=freqresp(b,a,w)
% Frequency response function from difference equation
% [H] = freqresp(b,a,w)
% H = frequency response array evaluated a w frequencies
% b = numerator coefficient array
% a = denominator coefficient array (a(1)=1)
% w = frequency location array
m = 0:length(b)-1;
l = 0:length(a)-1;
num = b*exp(-j*m'*w);
den = a*exp(-j*l'*w);
H = num./den;


function [func, grad, Hess] = FuncGradHessSub(x, y, F, R, kin, rho)
% `SubGradHess` provides the function value, the gradient, and the Hessian
% of the subproblem of DCA and BDCA to be used in `fminunc`.
%
% USAGE:
%
%    [func, grad, Hess] = FuncGradHessSub(x, y, F, R, kin, rho)
%
% INPUTS:
%    x,y:     points
%    F:       Forward stoichiometric matrix
%    kin:     kinetics parameter in :math:`R^{2n}`
%    rho:     strongly comvex modulus
%
%
% OUTPUTS:
%    f:       function value
%    grad:    gradient
%    H:       Hessian

m            = size(F,1);
FR           = [F,R];
RF           = [R,F];
FR_plus_RF   = FR+RF;
exp_x        = exp(kin+FR'*x);
exp_y        = exp(kin+FR'*y);
ux           = FR*exp_x;
wx           = RF*exp_x;
uy_plus_wy   = FR_plus_RF*exp_y;
FRdiag_x     = FR*diag(exp_x);
dux          = FRdiag_x*FR';
dwx          = FRdiag_x*RF';
FRdiag_y     = FR*diag(exp_y);
duy_plus_dwy = FRdiag_y*FR_plus_RF';
gx           = 2*(norm(ux)^2+norm(wx)^2)+rho/2*norm(x)^2;
dgx          = 4*(dux*ux+dwx*wx)+rho*x;
dhy          = 2*duy_plus_dwy*uy_plus_wy+rho*y;

func = gx-dhy'*x;
grad = dgx-dhy;
H    = dux*dux.'+dwx*dwx.';
for j = 1:m
    H = H+ux(j)*FR*diag(exp_x.*FR(j,:)')*FR'+ ...
                                     wx(j)*FR*diag(exp_x.*RF(j,:)')*FR';
end
Hess = 4*H+rho*eye(m);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%% End of FuncGradHessSub.m %%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% Tutorial for FAIR: elastic regularizer, matrix based and matrix free
%
% illustrates the tools for L2-norm based regularization
%
%  S(y) = alpha/2 * norm(B*(y-yRef)^2,
%
% where
%  alpha regularization parameter, weights regularization versus
%        distance in the joint objective function, alpha = 1 here
%  yRef  is a reference configuration, e.g. yRef = x or a
%        pre-registration result
%  B     a discretized partial differential operator either in explicit
%        matrix form or as a structure containing the necessary
%        parameters to compute B*y, here B = elastic
%==============================================================================

clear, close all, help(mfilename);

%% 2D example
omega  = [0,1,0,1]; % physical domin
m      = [16,12];   % number of discretization points
hd     = prod((omega(2:2:end)-omega(1:2:end))./m);

alpha  = 1;         % regularization parameter, irrelevant in this tutorial
mu     = 1;         % Lame constants, control elasticity properties
lambda = 0;         % Youngs modulus and Poisson ratio
para   = {'alpha',alpha,'mu',mu,'lambda',lambda};

yRef   = getStaggeredGrid(omega,m); % reference for regularization
yc     = rand(size(yRef));          % random transformation

%% 1. build elasticity operator on a staggered grid
%  - explicitly
%  - via regularizer.m
%  - via A(:,j) = B(e_j), B is the matrix free version

B = getElasticMatrixStg(omega,m,mu,lambda);
Amb = hd*alpha*(B'*B);

regularizer('reset','regularizer','mbElastic',para{:});
% regularizer('disp');
[Smb,dSmb,d2Smb] = regularizer(yc-yRef,omega,m);

regularizer('reset','regularizer','mfElastic',para{:});
% regularizer('disp');
[Scmf, dSmf, d2Smf] = regularizer(yc,omega,m);

% note: B*y can also be computed without storing B. The following loop
% computes A(:,j) = mfBy(e(j)), where mfBy is the matrix free operation B*y
% and e(j) is the j-th unit vector; hence A == B,

e = @(j) ((1:size(B,2))' == j); % j-th unit vector
A = sparse(size(B,1),size(B,2));
for j=1:size(B,2), A(:,j) = d2Smf.By(e(j),omega,m); end;

% now the transpose
e = @(j) ((1:size(B,1))' == j); % j-th unit vector
C = sparse(size(B,2),size(B,1));
for j=1:size(B,1), C(:,j) = d2Smf.BTy(e(j),omega,m); end;

% visualize matrices and transpose
FAIRfigure(1); clf;
subplot(2,3,1); spy(B);    title('B =elastic operator on staggered grid')
subplot(2,3,2); spy(A);    title('B from mfBy');
subplot(2,3,3); spy(B-A);  title('difference |B-mfBy()|');
subplot(2,3,4); spy(B');   title('B''')
subplot(2,3,5); spy(C);    title('B'' from mfBy');
subplot(2,3,6); spy(B'-C); title('difference |B''-mfBy(...,''BTy'')|');

%% 3D example
omega  = [0,4,0,2,0,1]; % physical domin
m      = [16,12,8];     % number of discretization points
hd     = prod((omega(2:2:end)-omega(1:2:end))./m);

yRef   = getStaggeredGrid(omega,m); % reference for regularization
yc     = rand(size(yRef));          % random transformation

% build elasticity operator on a staggered grid
B = getElasticMatrixStg(omega,m,mu,lambda);
Amb = hd*alpha*(B'*B);

regularizer('reset','regularizer','mbElastic',para{:});
% regularizer('disp');
[Smb,dSmb,d2Smb] = regularizer(yc-yRef,omega,m);

regularizer('reset','regularizer','mfElastic',para{:});
% regularizer('disp');
[Smf, dSmf, d2Smf] = regularizer(yc-yRef,omega,m);

zc     = randn(size(B,1),1);        % random for the adjoint

FAIRfigure(2); clf;
subplot(2,2,1); spy(B);          title('B elastic on staggered grid')
subplot(2,2,3); spy(Amb);        title('mb: hd\alpha B''*B')
subplot(2,2,4); spy(d2Smb);      title('mb: d2S')
subplot(2,2,2); spy(Amb-d2Smb);  title('difference')

Splain = 0.5*(yc-yRef)'*Amb*(yc-yRef);

fprintf('Splain = %-10s\n',num2str(Splain))
fprintf('Smb    = %-10s, Splain-Smb =%-10s\n',num2str(Smb),num2str(Splain-Smb))
fprintf('Smf    = %-10s, Splain-Smf =%-10s\n',num2str(Smf),num2str(Splain-Smf))

compare = @(s,l,r) fprintf('%-25s = %s\n',s,num2str(norm(l-r)));
compare('||B*yc-mfBy(y)||',B*yc,d2Smf.By(yc,omega,m));
compare('||B''*z-mfBy(z,''BTy'')||',B'*zc,d2Smf.BTy(zc,omega,m));

%% check elasticity

%% 3D example
omega  = [0,4,0,2,0,1]; % physical domin
m      = [32,16,8];     % number of discretization points
hd     = prod((omega(2:2:end)-omega(1:2:end))./m);

yRef   = getStaggeredGrid(omega,m); % reference for regularization
yc     = rand(size(yRef));          % random transformation

% build elasticity operator on a staggered grid
B = getElasticMatrixStg(omega,m,mu,lambda);

regularizer('reset','regularizer','mfElastic',para{:});
% regularizer('disp');
[Smf, dSmf, d2Smf] = regularizer(yc-yRef,omega,m);

H.d2S = d2Smf;
H.omega  = omega;
H.m      = m;
H.alpha  = regularizer('get','alpha');
H.mu     = regularizer('get','mu');
H.lambda = regularizer('get','lambda');
H.regularizer = regularizer;

By = B*yc;
testMF  = norm(By-d2Smf.By(yc,omega,m));
testMFT = norm(B'*By-d2Smf.BTy(By,omega,m));
fprintf('|By-mfBy|     = %s\n',num2str(testMF));
fprintf('|B''*By-mfBy''| = %s\n',num2str(testMFT));
% ---------------------------------------------------------------------
% using multigrid to solve A*uc = fc, where A = I + alpha*hd*B'*B

xc  = getStaggeredGrid(omega,m);
M   = speye(length(xc),length(xc)); % idenity matrix
A   = M + hd*alpha*B'*B;
fc  = A*xc;
u0  = zeros(size(xc));

% prepare for multigrid
H.MGlevel      = log2(m(1))+1;
H.MGcycle      = 4;
H.MGomega      = 2/3;   %% !!! 0.5 should be better
H.MGsmoother   = 'mfJacobi';
H.MGpresmooth  = 10;
H.MGpostsmooth = 10;
H.d2D.M        = full(diag(M));

uMG = mfvcycle(H,u0,fc,1e-12,H.MGlevel,5);

testMG = norm( A\fc - uMG);
fprintf('|A\\fc-uMG|=%s\n',num2str(testMG));

%==============================================================================

%==============================================================================
FAIRmessage(sprintf('<%s> done',mfilename));
%==============================================================================


function [reg_spectrum, chi2_regNNLS] = do_regNNLS(decay_matrix, measurements, sigma, mu)

%-------------------------------------------------------------------
% function [s_NNLS, chi2_NNLS] = do_regNNLS(num_t2_vals, decay_matrix, measurements, sigma, mu)
%
% * Runs through regularized NNLS (energy)
%
% ~~~ Charmaine Chia (May 3, 2005) ~~~
%-------------------------------------------------------------------

% generate minimum energy regularization
num_t2_vals = size(decay_matrix,2);
H = eye(num_t2_vals);
H_temp = mu*H;
this_decay_matrix = [decay_matrix; H_temp];
this_data = [measurements'; zeros(num_t2_vals,1)];

%-------------------------------------------------------------------
% --- EAO: addition to fit 2D spectrum (T2* times, with freq. offsets)
%-------------------------------------------------------------------
fit_delf = 1;

if fit_delf == 1

    % compute default tolerance used by lsqnonneg (as per help lsqnonneg )
    default_tolx = 10*max(size(this_decay_matrix))*norm(this_decay_matrix,1)*eps;
    % initialize fitting options structure
    opts = optimset('TolX',default_tolx);

    [reg_spectrum, bla, blo, exitflag, output] = lsqnonneg(this_decay_matrix,  this_data);

    % if num of iterations exceeded
    while ~exitflag
        % increase the tolerance
        opts = optimset(opts,'TolX',opts.TolX*10);
        % re-fit
        [reg_spectrum, bla, blo, exitflag] = lsqnonneg(this_decay_matrix, this_data, opts);
    end

else
    reg_spectrum = lsqnonneg(this_decay_matrix, this_data);
end
%-------------------------------------------------------------------

chi2_regNNLS = sum((this_decay_matrix*reg_spectrum - this_data).^2)/sigma^2;


function res=rot2eulZYX(R)
% calculates ZYX euler rotation angles from
% the rotation matrix R

% Matlab implementation of the method descirped in:
% "Computing Euler angles from a rotation matrix"
% Author: Gregory G. Slabaugh

% Copyright, Mohammd SAFEEA, 9th August 2018

% test:
% R=rotz(45)*roty(22)*rotx(30);
% res=rot2eulZYX(R)*180/pi

% res=rot2eulZYX(eye(3))

    [res]=eulerAngles(R) ;
end

function y=closeEnough(a,b)
    epsilon=0.000001;
    if (epsilon>abs(a-b))
        y=1;
    else
        y=0;
    end
end

function [res]=eulerAngles(R)
    %% check for gimbal lock
    R=R';
    if (closeEnough(R(1,3), -1.0))
        b = 0; %% gimbal lock, value of x doesn't matter
        c = pi / 2;
        a = b + atan2(R(2,1), R(3,1));
    elseif (closeEnough(R(1,3), 1.0))
        b = 0;
        c = -pi / 2;
        a = -b + atan2(-R(2,1), -R(3,1));
    else  %% two solutions exist
        b1 = -asin(R(1,3));
        b2 = pi - b1;

        c1 = atan2(R(2,3) / cos(b1), R(3,3) / cos(b1));
        c2 = atan2(R(2,3) / cos(b2), R(3,3) / cos(b2));

        a1 = atan2(R(1,2) / cos(b1), R(1,1)/ cos(b1));
        a2 = atan2(R(1,2) / cos(b2), R(1,1) / cos(b2));

        %% choose one solution to return
        %% for example the "shortest" rotation
        condition=((abs(b1) + abs(c1) + abs(a1)) <= (abs(b2) + abs(c2) + abs(a2)));
        if condition
            a=a1;
            b=b1;
            c=c1;
        else
            a=a2;
            b=b2;
            c=c2;
        end
    end
    res=[a,b,c];
end


%% Bingham Distribution
%
%% Theory
%
% The Bingham distribution has the density function
%
% $$ f(g;K,U) = _1\!F_1 \left(\frac{1}{2},2,K \right)^{-1} \exp
% \left\{ g^T UKU  g \right\},\qquad g\in S^3, $$
%
% where $U$ is an $4 \times 4$ orthogonal matrix with unit quaternions
% $u_{1,..,4}\in S^3$ in the columns and $K$ is a $4 \times 4$ diagonal matrix
% with the entries $k_1,..,k_4$ describing the shape of the distribution.
% $_1F_1(\cdot,\cdot,\cdot)$ is the hypergeometric function with matrix
% argument normalizing the density.
%
% The shape parameters $k_1 \ge k_2 \ge k_3 \ge k_4$ give
%
% * a _bipolar_   distribution, if $k_1 + k_4 > k_2 + k_3$,
% * a _circular_  distribution, if $k_1 + k_4 = k_2 + k_3$,
% * a _spherical_ distribution, if $k_1 + k_4 < k_2 + k_3$,
% * a _uniform_  distribution, if $k_1 = k_2 = k_3 = k_4$,
%
%%
% The general setup of the Bingham distribution in MTEX is done as follows

cs = crystalSymmetry('1');

kappa = [100 90 80 0];   % shape parameters
U     = eye(4);          % orthogonal matrix

odf = BinghamODF(kappa,U,cs)

%%
%

h = [Miller(0,0,1,cs) Miller(1,0,0,cs) Miller(1,1,1,cs)];
plotPDF(odf,h,'antipodal','silent');

%%
%
plot(odf,'sections',6)

%% The bipolar case and unimodal distribution
% First, we define some unimodal odf

odf_spherical = unimodalODF(orientation.rand(cs),'halfwidth',20*degree)

%%
%

plotPDF(odf_spherical,h,'antipodal','silent')

%%
% Next, we simulate individual orientations from this odf, in a scattered
% axis/angle plot in which the simulated data looks like a sphere

ori_spherical = discreteSample(odf_spherical,1000);
close all
scatter(ori_spherical)

%%
% From this simulated EBSD data, we can estimate the parameters of the
% Bingham distribution,

odf_est = calcBinghamODF(ori_spherical)

plotPDF(odf_est,h,'antipodal','silent')

%% TODO
%
% where |U| is the orthogonal matrix of eigenvectors of the orientation
% tensor and |kappa| the shape parameters associated with the |U|.
%
% next, we test the different cases of the distribution on rejection

%T_spherical = bingham_test(ori_spherical,'spherical','approximated');
%T_oblate    = bingham_test(ori_spherical,'prolate',  'approximated');
%T_prolate   = bingham_test(ori_spherical,'oblate',   'approximated');

%t = [T_spherical T_oblate T_prolate]

%%
% The spherical test case failed to reject for some level of
% significance, hence we would dismiss the hypothesis prolate and oblate.

%df_spherical = BinghamODF(kappa,U,crystalSymmetry,specimenSymmetry)

%%
%

%plotPDF(odf_spherical,h,'antipodal','silent')

%% Prolate case and fibre distribution
% The prolate case correspondes to a fibre.

odf_prolate = fibreODF(Miller(0,0,1,crystalSymmetry('1')),zvector,...
  'halfwidth',20*degree)

%%
%

plotPDF(odf_prolate,h,'upper','silent')

%%
% As before, we generate some random orientations from a model odf. The
% shape in an axis/angle scatter plot reminds of a cigar

ori_prolate = discreteSample(odf_prolate,1000);
close all
scatter(ori_prolate)

%%
% We estimate the parameters of the Bingham distribution

calcBinghamODF(ori_prolate)

%%
% and test on the three cases

%T_spherical = bingham_test(ori_prolate,'spherical','approximated');
%T_oblate    = bingham_test(ori_prolate,'prolate',  'approximated');
%T_prolate   = bingham_test(ori_prolate,'oblate',   'approximated');

%t = [T_spherical T_oblate T_prolate]

%%
% The test clearly rejects the spherical and prolate case, but not the
% prolate. We construct the Bingham distribution from the parameters, it
% might show some skewness

odf_prolate = BinghamODF(kappa,U,crystalSymmetry,specimenSymmetry)

%%
%

plotPDF(odf_prolate,h,'antipodal','silent')

%% Oblate case
% The oblate case of the Bingham distribution has no direct counterpart in
% terms of texture components, thus we can construct it straightforward

odf_oblate = BinghamODF([50 50 50 0],eye(4),crystalSymmetry,specimenSymmetry)

%%
%

plotPDF(odf_oblate,h,'antipodal','silent')

  %%
% The oblate cases in axis/angle space remind on a disk

ori_oblate = discreteSample(odf_oblate,1000);
close all
scatter(ori_oblate)

%%
% We estimate the parameters again

calcBinghamODF(ori_oblate)

%%
% and do the tests

%T_spherical = bingham_test(ori_oblate,'spherical','approximated');
%T_oblate    = bingham_test(ori_oblate,'prolate',  'approximated');
%T_prolate   = bingham_test(ori_oblate,'oblate',   'approximated');

%t = [T_spherical T_oblate T_prolate]

%%
% the spherical and oblate case are clearly rejected, the prolate case
% failed to reject for some level of significance

odf_oblate = BinghamODF(kappa, U,crystalSymmetry,specimenSymmetry)

%%
%

plotPDF(odf_oblate,h,'antipodal','silent')

%%
% *Bingham unimodal ODF*

% a modal orientation
cs = crystalSymmetry('-3m');
mod = orientation.byEuler(45*degree,0*degree,0*degree,cs);

% the corresponding Bingham ODF
odf = BinghamODF(20,mod)

plot(odf,'sections',6,'silent','contourf','sigma')

%%
% *Bingham fibre ODF*

odf = BinghamODF([-10,-10,10,10],quaternion(eye(4)),cs)

plot(odf,'sections',6,'silent','sigma')

%%
% *Bingham spherical ODF*

odf = BinghamODF([-10,10,10,10],quaternion(eye(4)),cs)

plot(odf,'sections',6,'silent','sigma');


function W = threshold_proportional(W, p)
%THRESHOLD_PROPORTIONAL     Proportional thresholding
%
%   W_thr = threshold_proportional(W, p);
%
%   This function "thresholds" the connectivity matrix by preserving a
%   proportion p (0<p<1) of the strongest weights. All other weights, and
%   all weights on the main diagonal (self-self connections) are set to 0.
%
%   Inputs: W,      weighted or binary connectivity matrix
%           p,      proportion of weights to preserve
%                       range:  p=1 (all weights preserved) to
%                               p=0 (no weights preserved)
%
%   Output: W_thr,  thresholded connectivity matrix
%
%
%   Mika Rubinov, U Cambridge,
%   Roan LaPlante, Martinos Center, MGH
%   Zitong Zhang, Penn Engineering

%   Modification history:
%   2010: Original (MR)
%   2012: Bug fix for symmetric matrices (RLP)
%   2015: Improved symmetricity test (ZZ)

n=size(W,1);                                %number of nodes
W(1:n+1:end)=0;                             %clear diagonal

if max(max(abs(W-W.'))) < 1e-10;            %if symmetric matrix
    W=triu(W);                              %ensure symmetry is preserved
    ud=2;                                   %halve number of removed links
else
    ud=1;
end

ind=find(W);                                %find all links
E=sortrows([ind W(ind)], -2);               %sort by magnitude
en=round((n^2-n)*p/ud);                     %number of links to be preserved

W(E(en+1:end,1))=0;                         %apply threshold

if ud==2                                    %if symmetric matrix
    W=W+W.';                                %reconstruct symmetry
end


function [Dx,Dy] = PhysDmatrices2D(x1, y1, interp)

% function [Dr,Ds] = PhysDmatrices2D(x1, y1, interp)
% Purpose : Initialize the (x,y) differentiation matrices on the simplex
%	        evaluated at (x1,y1) at order N

Globals2D;
IDr = interp*Dr; IDs = interp*Ds;

[rx1,sx1,ry1,sy1,J1] = GeometricFactors2D(x1, y1, IDr, IDs);

n = size(interp, 1);
Dx = spdiags(rx1, 0, n, n)*IDr + spdiags(sx1, 0, n, n)*IDs;
Dy = spdiags(ry1, 0, n, n)*IDr + spdiags(sy1, 0, n, n)*IDs;
return;