function [ f,resL2,qualMeasOut] = AwPCSD(proj,geo,angles,maxiter,varargin)
%AwPCSD solves the reconstruction problem using adaptive-weighted
%projection-controlled steepest descent method
%
%   AwPCSD(PROJ,GEO,ALPHA,NITER) solves the reconstruction problem using
%   the projection data PROJ taken over ALPHA angles, corresponding to the
%   geometry described in GEO, using NITER iterations.
%
%   AwPCSD(PROJ,GEO,ALPHA,NITER,OPT,VAL,...) uses options and values for solving. The
%   possible options in OPT are:
%
%
%   'lambda':      Sets the value of the hyperparameter for the SART iterations.
%                  Default is 1
%
%   'lambdared':   Reduction of lambda.Every iteration
%                  lambda=lambdared*lambda. Default is 0.99
%
%       'init':    Describes diferent initialization techniques.
%                   •  'none'     : Initializes the image to zeros(default)

%                   •  'FDK'      : intializes image to FDK reconstrucition
%
%   'TViter':      Defines the amount of TV iterations performed per SART
%                  iteration. Default is 20
%
%   'maxL2err'     Maximum L2 error to accept an image as valid. This
%                  parameter is crucial for the algorithm, determines at
%                  what point an image should not be updated further.
%                  Default is 20% of the FDK L2 norm.
%   'Verbose'      1 or 0. Default is 1. Gives information about the
%                  progress of the algorithm.
%   'delta'        Defines Parameter to control the amount of smoothing
%                  for pixels at the edges. A large 'delta' is not able to
%                  differentiate image gradients at different pixels. A
%                  small 'delta' give low weights to almost every pixels,
%                  making the algorithm inefficient in removing noise or
%                  straking artifacts. Default is -0.00055
% 'redundancy_weighting': true or false. Default is true. Applies data
%                         redundancy weighting to projections in the update step
%                         (relevant for offset detector geometry)
%  'groundTruth'  an image as grounf truth, to be used if quality measures
%                 are requested, to plot their change w.r.t. this known
%                 data.
%--------------------------------------------------------------------------
%--------------------------------------------------------------------------
% This file is part of the TIGRE Toolbox
%
% Copyright (c) 2015, University of Bath and
%                     CERN-European Organization for Nuclear Research
%                     All rights reserved.
%
% License:            Open Source under BSD.
%                     See the full license at
%                     https://github.com/CERN/TIGRE/blob/master/LICENSE
%
% Contact:            tigre.toolbox@gmail.com
% Codes:              https://github.com/CERN/TIGRE/
% Coded by:           Ander Biguri and Manasavee Lohvithee
%--------------------------------------------------------------------------
%% parse inputs
[beta,beta_red,f,ng,verbose,epsilon,delta,QualMeasOpts,nonneg,gpuids,redundancy_weights,gt]=parse_inputs(proj,geo,angles,varargin);
measurequality=~isempty(QualMeasOpts) | ~any(isnan(gt(:)));
if ~any(isnan(gt(:)))
    QualMeasOpts{end+1}='error_norm';
    res_prev=gt;
    clear gt
end
if nargout<3 && measurequality
    warning("Image metrics requested but none catched as output. Call the algorithm with 3 outputs to store them")
    measurequality=false;
end
qualMeasOut=zeros(length(QualMeasOpts),niter);

if nargout>1
    computeL2=true;
else
    computeL2=false;
end
resL2=zeros(1,niter);

% [alphablocks,orig_index]=order_subsets(angles,blocksize,OrderStrategy);
%
% angles=cell2mat(alphablocks);
% index_angles=cell2mat(orig_index);

% does detector rotation exists?
if ~isfield(geo,'rotDetector')
    geo.rotDetector=[0;0;0];
end

%% Create weigthing matrices for the SART step
% the reason we do this, instead of calling the SART fucntion is not to
% recompute the weigths every AwASD-POCS iteration, thus effectively doubling
% the computational time
% Projection weigth, W
W=computeW(geo,angles,gpuids);

% Back-Projection weigth, V
V=computeV(geo,angles,num2cell(angles),num2cell(1:length(angles)),'gpuids',gpuids);

if redundancy_weights
    % Data redundancy weighting, W_r implemented using Wang weighting
    % reference: https://iopscience.iop.org/article/10.1088/1361-6560/ac16bc

    num_frames = size(proj,3);
    W_r = redundancy_weighting(geo);
    W_r = repmat(W_r,[1,1,num_frames]);
    % disp('Size of redundancy weighting matrix');
    % disp(size(W_r));
    W = W.*W_r; % include redundancy weighting in W
end
%Initialize image.
%f=zeros(geo.nVoxel','single');

iter=0;
offOrigin=geo.offOrigin;
offDetector=geo.offDetector;
rotDetector=geo.rotDetector;
stop_criteria=0;
DSD=geo.DSD;
DSO=geo.DSO;
while ~stop_criteria %POCS
    % If quality is going to be measured, then we need to save previous image
    if measurequality && ~strcmp(QualMeasOpts,'error_norm')
        res_prev = f; % only store if necesary
    end
    f0=f;
    if (iter==0 && verbose==1);tic;end
    iter=iter+1;

    %Estimation error in the projection domain
    est_proj=Ax(f,geo,angles,'interpolated');
    delta_p=im3Dnorm(est_proj-proj,'L2');

    %Enforcing ART along all projections if squared delta_p > epsilon
    if (delta_p^2)>epsilon
        for jj=1:size(angles,2)
            if size(offOrigin,2)==size(angles,2)
                geo.offOrigin=offOrigin(:,jj);
            end
            if size(offDetector,2)==size(angles,2)
                geo.offDetector=offDetector(:,jj);
            end
            if size(rotDetector,2)==size(angles,2)
                geo.rotDetector=rotDetector(:,jj);
            end
            if size(DSD,2)==size(angles,2)
                geo.DSD=DSD(jj);
            end
            if size(DSO,2)==size(angles,2)
                geo.DSO=DSO(jj);
            end

            f=f+beta* bsxfun(@times,1./sum(V(:,:,jj),3),Atb(W(:,:,jj).*(proj(:,:,jj)-Ax(f,geo,angles(:,jj),'gpuids',gpuids)),geo,angles(:,jj),'gpuids',gpuids));

        end
    end

    %Non-negativity projection on all pixels
    if nonneg
        f=max(f,0);
    end

    geo.offDetector=offDetector;
    geo.offOrigin=offOrigin;

    if measurequality
        qualMeasOut(:,iter)=Measure_Quality(res_prev,f,QualMeasOpts);
    end

    % Compute L2 error of actual image. Ax-b
    dd=im3Dnorm(Ax(f,geo,angles,'gpuids',gpuids)-proj,'L2');

    % Compute change in the image after last SART iteration
    dp_vec=(f-f0);

    if iter==1
        step=1;
    else
        step=delta_p/delta_p_first;
    end
    f0=f;
    %  TV MINIMIZATION
    % =========================================================================
    %  Call GPU to minimize AwTV
    f=minimizeAwTV(f0,step,ng,delta,'gpuids',gpuids);    %   This is the MATLAB CODE, the functions are sill in the library, but CUDA is used nowadays
    %                                             for ii=1:ng
    %                                                 %delta=-0.00038 for thorax phantom
    %                                                 df=weighted_gradientTVnorm(f,delta);
    %                                                 df=df./im3Dnorm(df,'L2');
    %                                                 f=f-(step.*df);
    %                                             end
    % Compute change by TV min
    dg_vec=(f-f0);

    if iter==1
        delta_p_first=im3Dnorm((Ax(f0,geo,angles,'interpolated','gpuids',gpuids))-proj,'L2');
    end

    % Reduce SART step
    beta=beta*beta_red;

    % Check convergence criteria
    % ==========================================================================

    %Define c_alpha as in equation 21 in the journal
    c=dot(dg_vec(:),dp_vec(:))/(norm(dg_vec(:),2)*norm(dp_vec(:),2));
    %This c is examined to see if it is close to -1.0

    if (c<-0.99 && dd<=epsilon) || beta<0.005|| iter>maxiter
        if verbose
            disp(['Stopping criteria met']);
            disp(['   c    = ' num2str(c)]);
            disp(['   beta = ' num2str(beta)]);
            disp(['   iter = ' num2str(iter)]);
        end
        stop_criteria=true;
    end

    if computeL2
        geo.offOrigin=offOrigin;
        geo.offDetector=offDetector;
        errornow=im3Dnorm(proj-Ax(f,geo,angles,'gpuids',gpuids),'L2');                       % Compute error norm2 of b-Ax
        % If the error is not minimized.
        if  iter~=1 && errornow>errorL2(end)
            if verbose
                disp(['Convergence criteria met, exiting on iteration number:', num2str(iter)]);
            end
            return;
        end
        errorL2=[errorL2 errornow];
    end

    if (iter==1 && verbose==1)
        expected_time=toc*maxiter;
        disp('AwPCSD');
        disp(['Expected duration  :    ',secs2hms(expected_time)]);
        disp(['Expected finish time:    ',datestr(datetime('now')+seconds(expected_time))]);
        disp('');
    end

end
end

function [beta,beta_red,f0,ng,verbose,epsilon,delta,QualMeasOpts,nonneg,gpuids,redundancy_weights,gt]=parse_inputs(proj,geo,angles,argin)
opts=     {'lambda','lambda_red','init','tviter','verbose','maxl2err','delta','qualmeas','nonneg','gpuids','redundancy_weighting','groundtruth'};
defaults=ones(length(opts),1);
% Check inputs
nVarargs = length(argin);
if mod(nVarargs,2)
    error('TIGRE:AwPCSD:InvalidInput','Invalid number of inputs')
end

% check if option has been passed as input
for ii=1:2:nVarargs
    ind=find(ismember(opts,lower(argin{ii})));
    if ~isempty(ind)
        defaults(ind)=0;
    else
        error('TIGRE:AwPCSD:InvalidInput',['Optional parameter "' argin{ii} '" does not exist' ]);
    end
end

for ii=1:length(opts)
    opt=opts{ii};
    default=defaults(ii);
    % if one option isnot default, then extract value from input
    if default==0
        ind=double.empty(0,1);jj=1;
        while isempty(ind)
            ind=find(isequal(opt,lower(argin{jj})));
            jj=jj+1;
        end
        if isempty(ind)
            error('TIGRE:AwPCSD:InvalidInput',['Optional parameter "' argin{jj} '" does not exist' ]);
        end
        val=argin{jj};
    end
    % parse inputs
    switch opt
        % Verbose
        %  =========================================================================
        case 'verbose'
            if default
                verbose=1;
            else
                verbose=val;
            end
            if ~is2014bOrNewer
                warning('TIGRE:Verbose mode not available for older versions than MATLAB R2014b');
                verbose=false;
            end
            % Lambda
            %  =========================================================================
        case 'lambda'
            if default
                beta=1;
            else
                if length(val)>1 || ~isnumeric( val)
                    error('TIGRE:AwPCSD:InvalidInput','Invalid lambda')
                end
                beta=val;
            end
            % Lambda reduction
            %  =========================================================================
        case 'lambda_red'
            if default
                beta_red=0.99;
            else
                if length(val)>1 || ~isnumeric( val)
                    error('TIGRE:AwPCSD:InvalidInput','Invalid lambda')
                end
                beta_red=val;
            end
            % Initial image
            %  =========================================================================
        case 'init'
            if default || strcmp(val,'none')
                f0=zeros(geo.nVoxel','single');

            else
                if strcmp(val,'FDK')
                    f0=FDK(proj, geo, angles);
                else
                    error('TIGRE:AwPCSD:InvalidInput','Invalid init')
                end
            end
            % Number of iterations of TV
            %  =========================================================================
        case 'tviter'
            if default
                ng=20;
            else
                ng=val;
            end
            %  Maximum L2 error to have a "good image"
            %  =========================================================================
        case 'maxl2err'
            if default
                epsilon=im3Dnorm(FDK(proj,geo,angles))*0.2; %heuristic
            else
                epsilon=val;
            end
            %Parameter to control the amount of smoothing for pixels at the
            %edges
            %  =========================================================================
        case 'delta'
            if default
                delta=-0.005;
            else
                delta=val;
            end
            % Image Quality Measure
            %  =========================================================================
        case 'qualmeas'
            if default
                QualMeasOpts={};
            else
                if iscellstr(val)
                    QualMeasOpts=val;
                else
                    error('TIGRE:AwPCSD:InvalidInput','Invalid quality measurement parameters');
                end
            end
            %  Non negative
            %  =========================================================================
        case 'nonneg'
            if default
                nonneg=true;
            else
                nonneg=val;
            end
            %  GPU Ids
            %  =========================================================================
        case 'gpuids'
            if default
                gpuids = GpuIds();
            else
                gpuids = val;
            end
        case 'redundancy_weighting'
            if default
                redundancy_weights = true;
            else
                redundancy_weights = val;
            end
                case 'groundtruth'
            if default
                gt=nan;
            else
                gt=val;
            end
        otherwise
            error('TIGRE:AwPCSD:InvalidInput',['Invalid input name:', num2str(opt),'\n No such option in AwPCSD()']);

    end
end

end


%MDL_STANFORD Create model of Stanford arm
%
%      mdl_stanford
%
% Script creates the workspace variable stanf which describes the
% kinematic and dynamic characteristics of the Stanford (Scheinman) arm.
%
% Also defines the vectors:
%   qz   zero joint angle configuration.
%
% Note::
% - SI units are used.
% - Gear ratios not currently known, though reflected armature inertia
%   is known, so gear ratios are set to 1.
%
% References::
% - Kinematic data from "Modelling, Trajectory calculation and Servoing of
%   a computer controlled arm".  Stanford AIM-177.  Figure 2.3
% - Dynamic data from "Robot manipulators: mathematics, programming and control"
%   Paul 1981, Tables 6.5, 6.6
% - Dobrotin & Scheinman, "Design of a computer controlled manipulator for
%   robot research", IJCAI, 1973.
%
% See also SerialLink, mdl_puma560, mdl_puma560akb.

% MODEL: Stanford, Stanford Arm, prismatic, 6DOF, standard_DH

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

clear L
%             th    d       a    alpha
L(1) = Link([ 0     0.412   0   -pi/2     0]);
L(2) = Link([ 0     0.154   0    pi/2     0]);
L(3) = Link([ -pi/2 0       0    0        1]);  % PRISMATIC link
L(4) = Link([ 0     0       0   -pi/2     0]);
L(5) = Link([ 0     0       0    pi/2     0]);
L(6) = Link([ 0     0.263   0    0        0]);

% guestimates of some parameters
%
% According to the IJCAI paper the rack is 38in and the maximum reach is 52in
% From the image http://infolab.stanford.edu/pub/voy/museum/pictures/display/robots/IMG_2408ArmCenter.JPG
% and scaled by the rack length (38in) it looks like the minimum stroke is 12in.
%
L(3).qlim = [12 12+38] * 0.0254;

% According to the IJCAI paper
L(1).qlim = [-170 170]*pi/180;
L(2).qlim = [-170 170]*pi/180;
L(4).qlim = [-170 170]*pi/180;
L(5).qlim = [-90 90]*pi/180;
L(6).qlim = [-170 170]*pi/180;

L(1).m = 9.29;
L(2).m = 5.01;
L(3).m = 4.25;
L(4).m = 1.08;
L(5).m = 0.630;
L(6).m = 0.51;

L(1).Jm = 0.953;
L(2).Jm = 2.193;
L(3).Jm = 0.782;
L(4).Jm = 0.106;
L(5).Jm = 0.097;
L(6).Jm = 0.020;

L(1).G = 1;
L(2).G = 1;
L(3).G = 1;
L(4).G = 1;
L(5).G = 1;
L(6).G = 1;

L(1).I = [0.276   0.255   0.071   0   0   0];
L(2).I = [0.108   0.018   0.100   0   0   0];
L(3).I = [2.51    2.51    0.006   0   0   0 ];
L(4).I = [0.002   0.001   0.001   0   0   0 ];
L(5).I = [0.003   0.0004  0       0   0   0];
L(6).I = [0.013   0.013   0.0003  0   0   0];

L(1).r = [0    0.0175 -0.1105];
L(2).r = [0   -1.054  0];
L(3).r = [0    0      -6.447];
L(4).r = [0    0.092  -0.054];
L(5).r = [0    0.566   0.003];
L(6).r = [0    0       1.554];

qz = [0 0 0 0 0 0];

stanf = SerialLink(L, 'name', 'Stanford arm');
stanf.plotopt = {'workspace', [-2 2 -2 2 -2 2]};
stanf.model3d = 'example/stanford';


function MOV = PQavgMOVB (MOVC, Nchan, Nwup)
% Time average MOV precursors

% P. Kabal $Revision: 1.1 $  $Date: 2003/12/07 13:34:46 $

Fs = 48000;
NF = 2048;
Nadv = NF / 2;
Fss = Fs / Nadv;
tdel = 0.5;
tex = 0.050;

% BandwidthRefB, BandwidthTestB
[MOV(0+1), MOV(1+1)] = PQ_avgBW (MOVC.BW);

% Total NMRB, RelDistFramesB
[MOV(2+1), MOV(10+1)] = PQ_avgNMRB (MOVC.NMR);

% WinModDiff1B, AvgModDiff1B, AvgModDiff2B
N500ms = ceil (tdel * Fss);
Ndel = max (0, N500ms - Nwup);
[MOV(3+1), MOV(6+1), MOV(7+1)] = PQ_avgModDiffB (Ndel, MOVC.MDiff);

% RmsNoiseLoudB
N50ms = ceil (tex * Fss);
Nloud = PQloudTest (MOVC.Loud);
Ndel = max (Nloud + N50ms, Ndel);
MOV(8+1) = PQ_avgNLoudB (Ndel, MOVC.NLoud);

% ADBB, MFPDB
[MOV(4+1), MOV(9+1)] = PQ_avgPD (MOVC.PD);

% EHSB
MOV(5+1) = PQ_avgEHS (MOVC.EHS);

%-----------------------------------------
function EHSB = PQ_avgEHS (EHS)

[Nchan, Np] = size (EHS.EHS);

s = 0;
for (j = 0:Nchan-1)
    s = s + PQ_LinPosAvg (EHS.EHS(j+1,:));
end
EHSB = 1000 * s / Nchan;

%-----------------------------------------
function [ADBB, MFPDB] = PQ_avgPD (PD)

global PQopt

c0 = 0.9;
if (isempty (PQopt))
    c1 = 1;
else
    c1 = PQopt.PDfactor;
end

N = length (PD.Pc);
Phc = 0;
Pcmax = 0;
Qsum = 0;
nd = 0;
for (i = 0:N-1)
    Phc = c0 * Phc + (1 - c0) * PD.Pc(i+1);
    Pcmax = max (Pcmax * c1, Phc);

    if (PD.Pc(i+1) > 0.5)
        nd = nd + 1;
        Qsum = Qsum + PD.Qc(i+1);
    end
end

if (nd == 0)
    ADBB = 0;
elseif (Qsum > 0)
    ADBB = log10 (Qsum / nd);
else
    ADBB = -0.5;
end

MFPDB = Pcmax;

%-----------------------------------------
function [TotalNMRB, RelDistFramesB] = PQ_avgNMRB (NMR)

[Nchan, Np] = size (NMR.NMRavg);
Thr = 10^(1.5 / 10);

s = 0;
for (j = 0:Nchan-1)
    s = s + 10 * log10 (PQ_LinAvg (NMR.NMRavg(j+1,:)));
end
TotalNMRB = s / Nchan;

s = 0;
for (j = 0:Nchan-1)
    s = s + PQ_FractThr (Thr, NMR.NMRmax(j+1,:));
end
RelDistFramesB = s / Nchan;

%-----------------------------------------
function [BandwidthRefB, BandwidthTestB] = PQ_avgBW (BW)

[Nchan, Np] = size (BW.BWRef);

sR = 0;
sT = 0;
for (j = 0:Nchan-1)
    sR = sR + PQ_LinPosAvg (BW.BWRef(j+1,:));
    sT = sT + PQ_LinPosAvg (BW.BWTest(j+1,:));
end
BandwidthRefB  = sR / Nchan;
BandwidthTestB = sT / Nchan;

%-----------------------------------------
function [WinModDiff1B, AvgModDiff1B, AvgModDiff2B] = PQ_avgModDiffB (Ndel, MDiff)

NF = 2048;
Nadv = NF / 2;
Fs = 48000;

Fss = Fs / Nadv;
tavg = 0.1;

[Nchan, Np] = size (MDiff.Mt1B);

% Sliding window average - delayed average
L = floor (tavg * Fss);     % 100 ms sliding window length
s = 0;
for (j = 0:Nchan-1)
    s = s + PQ_WinAvg (L, MDiff.Mt1B(j+1,Ndel+1:Np-1+1));
end
WinModDiff1B = s / Nchan;

% Weighted linear average - delayed average
s = 0;
for (j = 0:Nchan-1)
    s = s + PQ_WtAvg (MDiff.Mt1B(j+1,Ndel+1:Np-1+1), MDiff.Wt(j+1,Ndel+1:Np-1+1));
end
AvgModDiff1B = s / Nchan;

% Weighted linear average - delayed average
s = 0;
for (j = 0:Nchan-1)
    s = s + PQ_WtAvg (MDiff.Mt2B(j+1,Ndel+1:Np-1+1), MDiff.Wt(j+1,Ndel+1:Np-1+1));
end
AvgModDiff2B = s / Nchan;

%-----------------------------------------
function RmsNoiseLoudB = PQ_avgNLoudB (Ndel, NLoud)

[Nchan, Np] = size (NLoud.NL);

% RMS average - delayed average and loudness threshold
s = 0;
for (j = 0:Nchan-1)
    s = s + PQ_RMSAvg (NLoud.NL(j+1,Ndel+1:Np-1+1));
end
RmsNoiseLoudB = s / Nchan;

%-----------------------------------
% Average values values, omitting values which are negative
function s = PQ_LinPosAvg (x)

N = length(x);

Nv = 0;
s = 0;
for (i = 0:N-1)
    if (x(i+1) >= 0)
        s = s + x(i+1);
        Nv = Nv + 1;
    end
end

if (Nv > 0)
    s = s / Nv;
end

%----------
% Fraction of values above a threshold
function Fd = PQ_FractThr (Thr, x)

N = length (x);

Nv = 0;
for (i = 0:N-1)
    if (x(i+1) > Thr)
        Nv = Nv + 1;
    end
end

if (N > 0)
    Fd = Nv / N;
else
    Fd = 0;
end

%-----------
% Sliding window (L samples) average
function s = PQ_WinAvg (L, x)

N = length (x);

s = 0;
for (i = L-1:N-1)
    t = 0;
    for (m = 0:L-1)
        t = t + sqrt (x(i-m+1));
    end
    s = s + (t / L)^4;
end

if (N >= L)
    s = sqrt (s / (N - L + 1));
end

%----------
% Weighted average
function s = PQ_WtAvg (x, W)

N = length (x);

s = 0;
sW = 0;
for (i = 0:N-1)
    s = s + W(i+1) * x(i+1);
    sW = sW + W(i+1);
end

if (N > 0)
    s = s / sW;
end

%----------
% Linear average
function LinAvg = PQ_LinAvg (x)

N = length (x);
s = 0;
for (i = 0:N-1)
    s = s + x(i+1);
end

LinAvg = s / N;

%----------
% Square root of average of squared values
function RMSAvg = PQ_RMSAvg (x)

N = length (x);
s = 0;
for (i = 0:N-1)
    s = s + x(i+1)^2;
end

if (N > 0)
    RMSAvg = sqrt(s / N);
else
    RMSAvg = 0;
end


function [ywarp,dwarp_dt,dwarp_dtheta,d2warp_dthetadt] = outwarp_negscaledpow(hyp,y,invflag)
%GPLITE_NOISEFUN Noise function for lite Gaussian Process regression.
%   SN2 = GPLITE_NOISEFUN(HYP,X,NOISEFUN) computes the GP noise function
%   NOISEFUN, that is the variance of observation noise evaluated at test
%   points X. HYP is a single column vector of noise function
%   hyperparameters. NOISEFUN is a numeric array whose elements specify
%   features of the noise function, as follows:
%
%   See also GPLITE_COVFUN, GPLITE_MEANFUN.

if nargin < 2; y = []; end
if nargin < 3 || isempty(invflag); invflag = false; else; invflag = true; end

if invflag && nargout > 1
    error('outwarp_fun:InverseOnly', ...
        ['When calling for the inverse output warping function, only one function output is expected.']);
end

%--------------------------------------------------------------------------
% CUSTOM: Number of hyperparameters
Noutwarp = 3;       % # hyperparameters of the output warping function
%--------------------------------------------------------------------------

N = size(y,1);      % Number of training points

% Return number of output warping function hyperparameters and additional info
if ischar(hyp)
    ywarp = Noutwarp;
    if nargout > 1

        % Initialize bounds for all hyperparameters
        outwarp_info.LB = -Inf(1,Noutwarp);
        outwarp_info.UB = Inf(1,Noutwarp);
        outwarp_info.PLB = -Inf(1,Noutwarp);
        outwarp_info.PUB = Inf(1,Noutwarp);
        outwarp_info.x0 = NaN(1,Noutwarp);

        %------------------------------------------------------------------
        % CUSTOM: Initialize hyperparameter bounds and other details

        % Threshold parameter
        outwarp_info.LB(1) = min(y);
        outwarp_info.UB(1) = max(y);
        outwarp_info.PLB(1) = min(y);
        outwarp_info.PUB(1) = max(y);
        outwarp_info.x0(1) = NaN;

        % Scaling parameter a (log space)
        outwarp_info.LB(2) = -Inf;
        outwarp_info.UB(2) = Inf;
        outwarp_info.PLB(2) = -2;
        outwarp_info.PUB(2) = 2;
        outwarp_info.x0(2) = 0;

        % Power exponent k (log space)
        outwarp_info.LB(3) = -Inf;
        outwarp_info.UB(3) = Inf;
        outwarp_info.PLB(3) = -3;
        outwarp_info.PUB(3) = 3;
        outwarp_info.x0(3) = 0;

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

        % Assign handle of current output warping function
        outwarp_info.outwarpfun = str2func(mfilename);

        % Plausible starting point
        idx_nan = isnan(outwarp_info.x0);
        outwarp_info.x0(idx_nan) = 0.5*(outwarp_info.PLB(idx_nan) + outwarp_info.PUB(idx_nan));

        dwarp_dt = outwarp_info;

    end

    return;
end

[Nhyp,Ns] = size(hyp);      % Hyperparameters and samples

if Nhyp ~= Noutwarp
    error('outwarp_fun:WrongLikHyp', ...
        ['Expected ' num2str(Noutwarp) ' output warping function hyperparameters, ' num2str(Nhyp) ' passed instead.']);
end
if Ns > 1
    error('outwarp_fun:nosampling', ...
        'Output warping function output is available only for one-sample hyperparameter inputs.');
end

%--------------------------------------------------------------------------
% CUSTOM: Compute output warping function and gradients

% Read hyperparameters
y0 = hyp(1);
a = exp(hyp(2));
k = exp(hyp(3));

% Compute output warping or inverse warping
ywarp = y;
idx = y < y0;
if invflag      % Inverse output warping
    ywarp(idx) = y0 - ((y0 - y(idx)).^(1/k))/a;
else            % Direct output warping
    adelta = a*(y0 - y(idx));
    adeltak = adelta.^k;
    ywarp(idx) = y0 - adeltak;
end

if nargout > 1
    % First-order derivative of output warping function in output space
    dwarp_dt = ones(size(y));
    adeltakm1 = adelta.^(k-1);

    dwarp_dt(idx) = a*k*adeltakm1;

    if nargout > 2
        % Gradient of output warping function wrt hyperparameters
        dwarp_dtheta = zeros(N,Noutwarp);

        dwarp_dtheta(idx,1) = 1 - a*k*adeltakm1;            % y0
        dwarp_dtheta(idx,2) = -k*adeltak;                   % log(a)
        dwarp_dtheta(idx,3) = -k*adeltak.*log(adelta);      % log(k)

        if nargout > 3
            % Gradient of derivative of output warping function
            d2warp_dthetadt = zeros(N,Noutwarp);

            d2warp_dthetadt(idx,1) = a^2*k*(k-1)*adelta.^(k-2);                % y0
            d2warp_dthetadt(idx,2) = a*k^2*adeltakm1;                           % log(a)
            d2warp_dthetadt(idx,3) = a*k*adeltakm1 + a*k^2*adeltakm1.*log(adelta);  % log(k)

        end

    end
end

end


% NMFLAB for Signal Processing written by A. Cichocki and R. Zdunek
% in cooperation with other members of Laboratory for Advanced Brain Signal
% Processing, BSI, RIKEN, Saitama, JAPAN

function [A,X,Distance_output]=nmf_newton(Y,r,Index_norm,A_true,S, mc_on, NoAlts,restart_mc_on,AL,Y_true,type_alg_A,type_alg_X,max_restart,no_iter,alphaS,alpha,CostFun,lambda,alpha0,tau,niter_selected)
%
% Non-negative Matrix Factorization (NMF) with the quasi-Newton method
%
%
% [A,X]=nmf_newton(Y,r,Index_norm,A_true,S,mc_on,NoAlts,restart_mc_on,AL,Y_true,type_alg_A,type_alg_X,max_restart,no_iter,alphaS,alpha,L_type,lambda,alpha0,tau)
%
%       produces mixing matrix A of dimension [m by r],
%       and source matrix X of dimension [r by T], for the linear mixing model: AX = Y,
%       where Y is an observation matrix [m by T].
% Note: > m: number of sensors,
%       > r: number of sources,
%       > T: number of samples,
%
% INPUTS:
%       > Index_norm:    vector of 13 binary entries indicating which number
%                        divergence measures are turned on in the View Options,
%       > A_true:        true mixing matrix (only for synthetic data),
%       > S:             true source matrix (only for synthetic data),
%       > mc_on:         1 - Monte Carlo analysis enabled, 0 - otherwise,
%       > NoAlts:        number of alternating steps (only for Monte Carlo
%                        analysis and the option "Fixed Alternatings" is selected)
%       > restart_mc_on: 1 - restarts in Monte Carlo analysis are enabled,
%                        0 - otherwise,
%       > AL:            mixing matrix estimaed from the preceeding layer,
%       > Y_true:        the first layer mixed signals (mixtures),
%       > type_alg_A:    indicates the selected algorithm for computation
%                        of the mixing matrix,
%       > type_alg_X:    indicates the selected algorithm for computation of the sources,
%       > no_iter:       number of inner iterations,
%       > alphaS:        parameter of non-linear projection in computation
%                        of the sources,
%       > alpha:         parameter "alpha" in the alpha-divergence,
%       > CostFun        cost function: (1) - Euclidean,
%                                       (2) - alpha-divergence
%       > lambda         Tikhonov regularization parameter in QP-NMF
%       > alpha0:        initial magnitude in the exponential model for regularization parameter,
%       > tau:           damping factor in the exponential model for regularization parameter,
%
% OUTPUTS:
%       > A:               estimated mixing matrix,
%       > X:               estimated source matrix,
%       > Distance_output: structures of different divergences measured between "Y" and estimated "AX" versus iterations,
%
% #########################################################################
A = [];
X = [];
if (nargin < 21) | isempty(niter_selected) | max(size(niter_selected) > 1)
   disp('Default number of alternating steps');
   niter_selected = 1000;
end
if (nargin < 20) | isempty(tau) | max(size(tau) > 1)
   disp('Incorrect the damping factor in the exponential model');
   return
end
if (nargin < 19) | isempty(alpha0) | max(size(alpha0) > 1)
   disp('Incorrect the initial magnitude in the exponential model');
   return
end
if (nargin < 18) | isempty(lambda) | max(size(lambda) > 1)
   disp('Incorrect regularization parameter');
   return
end
if (nargin < 17) | isempty(CostFun) | (CostFun < 1) | max(size(CostFun) > 1)
   disp('Incorrect cost function');
   return
end
if (nargin < 16) | isempty(alpha) | max(size(alpha) > 1)
   disp('Incorrect parameter alpha in alpha-divergence');
   return
end
if (nargin < 15) | isempty(alphaS) | max(size(alphaS) > 1)
   disp('Incorrect parameter alphaS');
   return
end
if (nargin < 14) | isempty(no_iter)  | (no_iter < 0) | max(size(no_iter) > 1)
   disp('Incorrect number of inner iterations');
   return
end
if (nargin < 13) | isempty(max_restart)  | (max_restart < 0) | max(size(max_restart) > 1)
   disp('Number of restarts must be given correctly');
   return
end
if (nargin < 12) | isempty(type_alg_X) | (type_alg_X < 1) | max(size(type_alg_X) > 1)
   disp('Incorrect algorithm for X');
   return
end
if (nargin < 11) | isempty(type_alg_A) | (type_alg_A < 1) | max(size(type_alg_A) > 1)
   disp('Incorrect algorithm for A');
   return
end
if (nargin < 10) | isempty(Y_true)
   disp('The first layer mixed signals are unknown');
   Y_true = zeros(size(Y_true));
end
if (nargin < 9) | isempty(AL)
   disp('Mixing matrix from the preceeding layer unknown');
   AL = eye(size(Y,1));
end
if (nargin < 8) | isempty(restart_mc_on) | max(size(restart_mc_on) > 1)
   disp('Index od restarts in MC analysis unknown');
   restart_mc_on = 0;
end
if (nargin < 7) | isempty(NoAlts) | max(size(NoAlts) > 1)
   disp('Adjustable number of alternatings');
   NoAlts = [];
end
if (nargin < 6) | isempty(mc_on) | max(size(mc_on) > 1)
   disp('No Monte Carlo Analysis');
   mc_on = 0;
end
if (nargin < 5) | isempty(S)
   disp('X_true not given');
end
if (nargin < 4) | isempty(A_true)
   disp('A_true not given');
   index_fixed_A = 1;
else
   index_fixed_A = 0;
end
if (nargin < 3) | isempty(Index_norm)
   '"Index_norm" must be specified'
   return
end
if (nargin < 2) | isempty(r)
   'Rank of factorization must be given'
   return
end
if isempty(Y) | isnan(Y)
   error('No data');
   return
end
% test for negative values in Y
if min(min(Y)) < 0
    disp('Some matrix entries are changed from negative to small positive');
    Y(Y< 0) = eps;
end
if min(sum(Y,2)) == 0
    disp('Not all entries in a row can be zero');
    return
end

Y = Y + eps;

[m,T]=size(Y);
%niter_selected = 1000;     % maximum number of iterations for the selected sample (can be adjusted)
niter_sample = 10; % maximum number of iterations for each random sample
epsil_normA = 1E-8; % tolerance for alternating

if (alpha == 0) & (type_alg_A == 6)
   type_alg_A = 5;
end

if (type_alg_A == 8) & (size(A_true,1) ~= size(Y,1))
   disp('Multilayer technique cannot be used with A fixed');
   Distance_output = [];
   return
end

% Monte Carlo and alternatings adjustment
if mc_on & ~restart_mc_on
    max_restart = 0;
end
if ~isempty(NoAlts)
    niter_selected = NoAlts;
end

% Parameters for QP-NMF
rho = 1E-3;
eta = 1E-4;
epsil_A = 1E-6;

% Declaration for A and X
A=zeros(m,r);
Ap = A;
X=zeros(r,T);
Ainit = A;
Xinit = X;
Z = zeros(m,T);
KL_outer_temp = 0;
Z_outer_temp = 0;
nr = 0; restart_on = 0; norm_A = 10; nr_best = -1;
m_sx = 1:m; r_sx = 1:r; T_sx = 1:T;
s_dist = 0;

% Newton
HX = spalloc(r*T,r*T,T*r^2);
HA = spalloc(m*r,m*r,m*r^2);
E = ones(m,T);
I = eye(r);
Zx = zeros(m,T);
alpha_h_init = 1E-13;

while (nr <= max_restart)

   % Initialize random A and X
       if ~nr & (~mc_on | restart_mc_on)
          Ainit(m_sx',r_sx) = abs(repmat(.1*sin(2*pi*.1*m_sx'),1,r) + repmat(.1*cos(2*pi*.1*r_sx),m,1) + repmat(cos(2*pi*.471*m_sx'),1,r) + repmat(sin(2*pi*.471*r_sx),m,1));
          Ainit = Ainit/max(max(Ainit));

          Xinit(r_sx',T_sx) = abs(repmat(.1*sin(2*pi*.1*r_sx'),1,T) + repmat(.1*cos(2*pi*.1*T_sx),r,1) + repmat(cos(2*pi*.471*r_sx'),1,T) + repmat(sin(2*pi*.471*T_sx),r,1));
          Xinit = Xinit/max(max(Xinit));
       else
          Ainit=rand(m,r);
          Xinit=rand(r,T);
       end

        % Normalization of initial guess
        Ainit = Ainit*diag(1./sum(Ainit,1));

        if (nr == max_restart)&(max_restart > 0)
           A = A_best;
           X = X_best;
        else
           A = Ainit;
           X = Xinit;
        end % initial guess assignment

    Yx = zeros(m,T);
    n = 0; k = 0;

while ((k <= niter_sample)&(nr < max_restart)) | ((k <= niter_selected)&(nr == max_restart)&(norm_A > epsil_normA)& isempty(NoAlts)) | ((k <= niter_selected)&(nr == max_restart)& (NoAlts > 0))

k = k + 1;

    switch type_alg_X

        case 1 % ALS

            X = max(1E6*eps,pinv(A'*A)*A'*Y);

        case 2 % HALS

                 for j = 1:r
                     Ys = Y - A*X + A(:,j)*X(j,:);
                     X(j,:) = max(1E6*eps,(A(:,j)'*Ys)/(A(:,j)'*A(:,j)));
                 end

        case 3 % Regularized ALS

            alpha_reg = alpha0*exp(-k/tau);
            if isnan(A)  disp('Matrix A is too much ill-conditioned. Try again.'); break; end
            if cond(A) > 1E6 alphaX = 1E-6; else alphaX = 0; end
            X = max(1E6*eps,pinv(A'*A + alpha_reg + alphaX*I)*A'*Y); %Updating of X

        case 4 % GPCG

            X = gpcg_nmf(Y,A,X,no_iter,CostFun,alpha);

        case 5 % Kullback-Leibler (EMML)

             for t = 1:no_iter
                 X = (X.*(A'*(Y./(A*X + eps)))).^(1 + alphaS);
             end
             X = X + 100*eps;

        case 6 % Frobenius (ISRA)

             for t = 1:no_iter
                 X = X.*((A'*Y)./(A'*A*X + eps));
             end
             X = X + 100*eps;

        case 7 % Newton applied to Frobenius

            hX = A'*A;
            GX = A'*Y - hX*X;
            HX = kron(speye(T),-hX); % Hessian
            hk = 0;
            alpha_h = alpha_h_init;
            while condest(HX) > 1E7
                  hk = hk + 1;
                  alpha_h = 10*alpha_h;
                  HX = HX + alpha_h*speye(r*T);
            end
            [QX,RX] = qr(HX,GX(:));
            X(:) = X(:) - .9*RX\QX;
            X(X <= 0) = 100*eps;

        case 8  % Newton applied to KL

            Zx = A*X+1E5*eps;
            GX = A'*(E - Y./Zx);
            Zxx = (Y+100*eps)./Zx.^2;
            for t = 1:T
                HX(((t-1)*r+1):t*r,((t-1)*r+1):t*r) = A'*diag(Zxx(:,t))*A;
            end
            hk = 0;
            alpha_h = alpha_h_init;
            while condest(HX) > 1E7
                  hk = hk + 1;
                  alpha_h = 10*alpha_h;
                  HX = HX + alpha_h*speye(r*T);
            end
            [QX,RX] = qr(HX,GX(:));
            X(:) = X(:) + .9*RX\QX;
            X(X <= 0) = 100*eps;

        case 9 % Newton applied to dual KL (KL2)

            Zx = A*X+10*eps;
            Zxx = 1./Zx + eps;
            GX = A'*log(Zx./(Y + 10*eps));
            for t = 1:T
                HX(((t-1)*r+1):t*r,((t-1)*r+1):t*r) = A'*diag(Zxx(:,t))*A;
            end
            hk = 0;
            alpha_h = alpha_h_init;
            while condest(HX) > 1E7
                  hk = hk + 1;
                  alpha_h = 10*alpha_h;
                  HX = HX + alpha_h*speye(r*T);
            end
            [QX,RX] = qr(HX,GX(:));
            X(:) = X(:) - .9*RX\QX;
            X(X <= 0) = 100*eps;

        case 10 % QP with barrier function

            X = iptrnr(Y',A',no_iter,lambda,rho,eta,epsil_A);
            X = X';

        case 11 % Fixed X

            X = S + eps;
            niter_selected = 1000;     % maximum number of iterations for the selected sample (can be adjusted)

    end % switch for X

    switch type_alg_A

        case 1 % QP with barrier function

            Ap = A;
            A = iptrnr(Y,X,no_iter,lambda,rho,eta,epsil_A);
            A = A*diag(1./(sum(A,1)+eps));

        case 2 % GPCG

            Ap = A;
            A = gpcg_nmf(Y',X',A',no_iter,CostFun,alpha);
            A = A';

        case 3 % Newton applied to Frobenius

            Ap = A;
            hA = X*X';
            hA = hA + (1E-12 + 1E8*exp(-k))*eye(r);  % Levenberg-Marquardt regularization
            GA = A*hA - Y*X';  % Gradient
            A = A - .9*GA*pinv(hA);
            A(A <= 0) = 1E2*eps;
            A = A*diag(1./sum(A,1));

        case 4 % Newton applied to KL

            Ap = A;
            Zx = A*X+100*eps;
            Zxx = (Y+eps)./Zx.^2;
            for i = 1:m
                HA(((i-1)*r+1):i*r,((i-1)*r+1):i*r) = (X.*repmat(Zxx(i,:),r,1))*X'; % Hessian
            end
            HA = HA + (1E-12 + 1E15*exp(-2*k))*speye(m*r);
            GA = X*(1 - (Y+eps)./Zx)'; % Gradient
        %    [QA,R] = qr(HA,GA(:)); % Q-less QR factorization
            cond_HA = condest(HA);
            if isinf(cond_HA)
               fprintf(1,'Hessian is singular in %d iteration(s). Restart is needed.\n',k)
               break;
            end
            A = A';
            A(:) = A(:) - .9*inv(HA)*GA(:);
          %  A(:) = A(:) - .9*R\QA;
            A(A < 0) = 100*eps;
            A = A';
            A = A*diag(1./sum(A,1));

        case 5 % Newton applied to dual KL (KL2)

            Zx = A*X+100*eps; Zxx = 1./Zx;
            GA = X*(log(Zx./(Y+eps)))'; % Gradient
            for i = 1:m
                HA(((i-1)*r+1):i*r,((i-1)*r+1):i*r)...
                = (X.*repmat(Zxx(i,:),r,1))*X';  % Hessian
            end
            HA = HA + (1E-12 + 1E15*exp(-2*k))*speye(m*r);
           %    [QA,R] = qr(HA,GA(:)); % Q-less QR factorization
            cond_HA = condest(HA);
            if isinf(cond_HA)
               fprintf(1,'Hessian is singular in %d iteration(s). Restart is needed.\n',k)
               break;
            end
            A = A';
            A(:) = A(:) - .9*inv(HA)*GA(:);
          %  A(:) = A(:) - .9*R\QA;
            A(A < 0) = 100*eps;
            A = A';
            A = A*diag(1./sum(A,1));

        case 6 % Newton applied to alpha-divergence

            Ap = A;
            Zx = A*X+100*eps;
            Zxx = ((Y+eps).^alpha)./(Zx.^(alpha + 1));
            for i = 1:m
                HA(((i-1)*r+1):i*r,((i-1)*r+1):i*r) =...
                    (X.*repmat(Zxx(i,:),r,1))*X'; % Hessian
            end
            GA = (1/alpha)*X*(1 - ((Y+eps)./Zx).^alpha)'; % Gradient
            HA = HA + (1E-12 + 1E15*exp(-2*k))*speye(m*r);
       %    [QA,R] = qr(HA,GA(:)); % Q-less QR factorization
            cond_HA = condest(HA);
            if isinf(cond_HA)
               fprintf(1,'Hessian is singular in %d iteration(s). Restart is needed.\n',k)
               break;
            end
            A = A';
            A(:) = A(:) - .9*inv(HA)*GA(:);
         %  A(:) = A(:) - .9*R\QA;
            A(A < 0) = 100*eps;
            A = A';
            A = A*diag(1./sum(A,1));

        case 7 % Regularized ALS

            Ap = A;
            alpha_reg = alpha0*exp(-k/tau);
            A = max(1E6*eps, Y*X'*pinv(X*X' + alpha_reg));
            A = A*diag(1./sum(A,1));

        case 8 % Fixed A

            niter_selected = 1000;     % maximum number of iterations for the selected sample (can be adjusted)
            A = A_true + eps;
            A = A*diag(1./(sum(A,1) + eps));

    end % switch for A

                if (nr == max_restart)&(mod(k,50)==0)& (~mc_on | restart_mc_on)
                    norm_A = norm(abs(A - Ap),'fro');
                    fprintf(1, 'Restart %d,  %d-th alternating step\n',nr_best+1,k);
                end

                if sum(Index_norm)
                   if (nr == max_restart) & (((k < 50) & (mod(k,5)==0)) | ((k>49) & ((mod(k,50)==0))))

                       s_dist = s_dist + 1;
                       k_select(s_dist) = k;
                       Z = A*X + eps;
                       Z = diag(1./sqrt(var(Z')))*Z;

                        dist_Fro(s_dist) = norm(Y - Z,'fro');
                        dist_KL(s_dist) = sum(sum(Y.*log(Y./Z + eps) - Y + Z));
                        dist_KL2(s_dist) = sum(sum(Z.*log(Z./Y + eps) + Y - Z));
                        dist_Pearson(s_dist) = sum(sum( ((Y - Z).^2)./Z ));
                        dist_Hellinger(s_dist) = sum(sum( (sqrt(Z) - sqrt(Y)).^2 ));
                        dist_JS_rel(s_dist) = sum(sum(2*Y.*log(2*Y./(Y + Z) + eps) + Z - Y));
                        dist_JS_rel2(s_dist) = sum(sum(2*Z.*log(2*Z./(Y + Z) + eps) - Z + Y));
                        Zy = Y + Z;
                        dist_JS(s_dist) = sum(sum(Y.*log(2*Y./Zy + eps) + Z.*log(2*Z./Zy + eps) ));
                        dist_AG_rel(s_dist) = sum(sum(Zy.*log(.5*Zy./Y + eps) + Y - Z));
                        dist_AG(s_dist) = sum(sum(.5*Zy.*log(.5*Zy./sqrt(Y.*Z) + eps)));
                        dist_J(s_dist) = sum(sum( .5*(Y - Z).*log(Y./Z + eps) ));
                        dist_Chi(s_dist) = sum(sum( ((Y + Z).*(Y - Z).^2)./(Y.*Z) ));
                        dist_Tria(s_dist) = sum(sum( ((Y - Z).^2)./(Y + Z) ));
                    end % if multiple
                end % if sum

end % while (k)

% Outer KL divergence
Z = AL*A*X;
Z_outer = norm(Z,'fro') + eps;
KL_outer = sum(sum(Y_true.*log((Y_true + eps)./(Z + eps)) - Y_true + Z))/Z_outer;

           if (nr == 0) | (KL_outer < KL_outer_temp)
              A_best = A; X_best = X; KL_outer_temp = KL_outer; nr_best = nr;
           end % multi-conditions

   nr = nr + 1;

   if nr <=max_restart
      fprintf(1, 'Restart %d, Kullback-Leibler divergence = %e\n',	nr, KL_outer);
   end

end % while (restarts)

% One-Variance scaling
X(X <= 0) = eps;

Distance_output = cell(length(s_dist),1);
Distance_output(1) = {[]};
Distance_output(2) = {[]};
Distance_output(3) = {[]};
Distance_output(4) = {[]};
Distance_output(5) = {[]};
Distance_output(6) = {[]};
Distance_output(7) = {[]};
Distance_output(8) = {[]};
Distance_output(9) = {[]};
Distance_output(10) = {[]};
Distance_output(11) = {[]};
Distance_output(12) = {[]};
Distance_output(13) = {[]};
Distance_output(14) = {[]};

if sum(Index_norm)
   Distance_output(1) = {k_select};
   Distance_output(2) = {dist_Fro};
   Distance_output(3) = {dist_KL};
   Distance_output(4) = {dist_KL2};
   Distance_output(5) = {dist_Pearson};
   Distance_output(6) = {dist_Hellinger};
   Distance_output(7) = {dist_JS_rel};
   Distance_output(8) = {dist_JS_rel2};
   Distance_output(9) = {dist_JS};
   Distance_output(10) = {dist_AG_rel};
   Distance_output(11) = {dist_AG};
   Distance_output(12) = {dist_J};
   Distance_output(13) = {dist_Chi};
   Distance_output(14) = {dist_Tria};
end


%% FUNCTION Least_SparseTrace
%   Incoherent Sparse and Low-Rank Learning with Least Squares Loss.
%
%% OBJECTIVE
%   argmin_W { sum_i^t (0.5 * norm (Y{i} - X{i}' * W(:, i))^2)
%            + gamma * \|P\|_1 }
%   subject to: W = P + Q, \|Q\|_* <= tau
%   where \|Q\|_*      = sum( svd(Q, 0) )
%
%% INPUT
%   X: {n * d} * t - input matrix
%   Y: {n * 1} * t - output matrix
%   gamma: sparse component P L1,2-norm sprasity controlling parameter
%   tau: low rank component Q trace-norm regularization parameter
%
%% OUTPUT
%   W: model: d * t
%   funcVal: function value vector.
%
%% LICENSE
%   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/>.
%
%   Copyright (C) 2011 - 2012 Jiayu Zhou, Jianhui and Jieping Ye
%
%   You are suggested to first read the Manual.
%   For any problem, please contact with Jiayu Zhou via jiayu.zhou@asu.edu
%
%   Last modified on June 3, 2012.
%
%% RELATED PAPERS
%
%   [1] Chen, J., Liu, J. and Ye, J. Learning Incoherent Sparse and Low-Rank
%   Patterns from Multiple Tasks, KDD 2010
%
%% RELATED FUNCTIONS
%   init_opts

function [W funcVal Tp_val Tq_val] = Least_SparseTrace(X, Y, gamma, tau, opts )

if nargin <4
    error('\n Inputs: X, Y, rho1, and rho2 should be specified!\n');
end
X = multi_transpose(X);

if nargin <5
    opts = [];
end

% initialize options.
opts=init_opts(opts);

zero_threshold = 0;

d = size(X{1}, 1); m = length(Y);

% Initialize L and S
if opts.init==2
    P0 = zeros(d, m);
    Q0 = zeros(d, m);
elseif opts.init == 0
    P0 = randn(d, m);
    Q0 = P0;
else
    if isfield(opts,'P0')
        P0=opts.P0;
        if (nnz(size(P0)-[d, m]))
            error('\n Check the input .L0');
        end
    else
        P0=randn(d, m);
    end

    if isfield(opts,'Q0')
        Q0=opts.Q0;
        if (nnz(size(Q0)-[d, m]))
            error('\n Check the input .S0');
        end
    else
        Q0=P0;
    end

end

% Initialize
Tp_i = P0;
Tq_i = Q0;
Tp_i_min_1 = P0;
Tq_i_min_1 = Q0;

% initial parameters setting
%  ++++++++++++++++++++++++++++++++++++++++++

% Initialize L
Li = 1;

% Set an array to save the objective value
funcVal = [ ];

% Set auxiliary parameters

t_i_min_2 = 0;
t_i_min_1 = 1;
XY = cell(m, 1);
for jj = 1:m
    XY{jj} = X{jj} * Y{jj};
end

% main loop
% ++++++++++++++++++++++++++++++++++++++++++
for iter = 1 : opts.maxIter

    alpha_i = ( t_i_min_2 - 1 ) / t_i_min_1;

    Sp = ( 1 + alpha_i ) * Tp_i - alpha_i * Tp_i_min_1;
    Sq = ( 1 + alpha_i ) * Tq_i - alpha_i * Tq_i_min_1;

    derivative = zeros(d, m);
    if opts.pFlag
        parfor kk = 1:m
            Xi = X{kk};
            col = Sp(:, kk) + Sq(:, kk);
            derivative(:, kk) = 2 * Xi * (Xi' * col)  - 2 * XY{kk};
        end
    else
        for kk = 1:m
            Xi = X{kk};
            col = Sp(:, kk) + Sq(:, kk);
            derivative(:, kk) = 2 * Xi * (Xi' * col)  - 2 * XY{kk};
        end
    end

    lambda_old = 1;
    sign = 1;

    while (sign)

        beta = Li / 2;

        hat_Sp = Sp - derivative / Li;
        hat_Sq = Sq - derivative / Li;

        % Compute Tp
        Tp = Solve_OneNorm(hat_Sp, beta, gamma);

        % Compute Tq
        [U S V] = svd(hat_Sq, 'econ');
        s_bar = diag(S);
        s_bar = s_bar( s_bar > zero_threshold );
        U = U( :, 1:length(s_bar));
        V = V( :, 1:length(s_bar));

        [s_hat, lambda_new ]=eplb(s_bar, size(s_bar, 1), tau, lambda_old);
        Tq = U * diag(s_hat) * V';

        [sign qp_term]= line_search_cond_sparse_lowrank(Tp, Tq, Sp, Sq, X, Y, derivative, Li, opts);

        if (sign)
            Li = Li * 2;
            lambda_old = lambda_new;
        end

    end

    funcVal = cat(1, funcVal,  qp_term +  gamma * sum( abs(Tp(:)) ) );

    % test stop condition.
    switch(opts.tFlag)
        case 0
            if iter>=2
                if (abs( funcVal(end) - funcVal(end-1) ) <= opts.tol)
                    break;
                end
            end
        case 1
            if iter>=2
                if (abs( funcVal(end) - funcVal(end-1) ) <=...
                        opts.tol* funcVal(end-1))
                    break;
                end
            end
        case 2
            if ( funcVal(end)<= opts.tol)
                break;
            end
        case 3
            if iter>=opts.maxIter
                break;
            end
    end

    t_i_min_2 = t_i_min_1;
    t_i_min_1 = ( 1 + ( 1+4*t_i_min_1^2 )^0.5 ) / 2;

    Tp_i_min_1 = Tp_i;
    Tq_i_min_1 = Tq_i;

    Tp_i = Tp;
    Tq_i = Tq;

end % for loop

Tp_val = Tp;
Tq_val = Tq;
W = Tp_val + Tq_val;

end


function [g,a,info] = dtwfb2filterbank( dualwt, varargin)
%DTWFB2FILTERBANK DTWFB equivalent non-iterated filterbank
%   Usage: [g,a] = dtwfb2filterbank(dualwt)
%          [g,a,info] = dtwfb2filterbank(...)
%
%   Input parameters:
%      dualwt : Dual-tree wavelet filterbank specification.
%
%   Output parameters:
%      g      : Cell array of filters.
%      a      : Downsampling rate for each channel.
%      info   : Additional information.
%
%   `[g,a] = dtwfb2filterbank(dualwt)` constructs a set of filters *g* and
%   subsampling factors *a* of a non-iterated filterbank, which is
%   equivalent to the dual-tree wavelet filterbank defined by *dualwt*.
%   The returned parameters can be used directly in |filterbank| and other
%   routines. The format of *dualwt* is the same as in |dtwfb| and
%   |dtwfbreal|.
%
%   The function internally calls |dtwfbinit| and passes *dualwt* and all
%   additional parameters to it.
%
%   `[g,a,info] = dtwfb2filterbank(...)` additionally outputs a *info*
%   struct containing equivalent filterbanks of individual real-valued
%   trees as fields *info.g1* and *info.g2*.
%
%   Additional parameters:
%   ----------------------
%
%   `'real'`
%      By default, the function returns a filtebank equivalent to |dtwfb|.
%      The filters can be restricted to cover only the positive frequencies
%      and to be equivivalent to |dtwfbreal| by passing a `'real'` flag.
%
%   `'freq'`(default),`'nat'`
%     The filters are ordered to produce subbands in the same order as
%     |dtwfb| or |dtwfbreal| with the same flag.
%
%   Examples:
%   ---------
%
%   The following two examples create a multirate identity filterbank
%   using a duel-tree of depth 3:::
%
%      [g,a] = dtwfb2filterbank({'qshift3',3},'real');
%      filterbankfreqz(g,a,1024,'plot','linabs');
%
%   In the second example, the filterbank is identical to the full
%   wavelet tree:::
%
%      [g,a] = dtwfb2filterbank({'qshift3',3,'full'},'real');
%      filterbankfreqz(g,a,1024,'plot','linabs');
%
%   See also: dtwfbinit

complainif_notenoughargs(nargin,1,'DTWFB2FILTERBANK');

% Search for the 'real' flag
do_real = ~isempty(varargin(strcmp('real',varargin)));
if do_real
    %Remove the 'real' flag from varargin
    varargin(strcmp('real',varargin)) = [];
end

if ~isempty(varargin(strcmp('complex',varargin)))
    %Remove the 'complex' flag from varargin
    %It is not used elsewhere anyway
    varargin(strcmp('complex',varargin)) = [];
end

% Initialize the dual-tree
dtw = dtwfbinit({'strict',dualwt},varargin{:});

% Determine relation between the tree nodes
[wtPath, rangeLoc, rangeOut] = treeBFranges(dtw);
slice = ~cellfun(@isempty,rangeOut); % Limit to nodes with unconnected outputs
wtPath   = wtPath(slice);
rangeLoc = rangeLoc(slice);
rangeOut = rangeOut(slice);

% Multirate identity filters of the first tree
[g1,a] = nodesMultid(wtPath,rangeLoc,rangeOut,dtw);

% Multirate identity filters of the second tree
dtw.nodes = dtw.dualnodes;
g2 = nodesMultid(wtPath,rangeLoc,rangeOut,dtw);

if nargin>2
   % Return the filterbanks before doing the alignment
   info.g1 = cellfun(@(gEl) setfield(gEl,'h',gEl.h/2),g1,'UniformOutput',0);
   info.g2 = cellfun(@(gEl) setfield(gEl,'h',gEl.h/2),g2,'UniformOutput',0);
end

% Align filter offsets so they can be summed
for ii = 1:numel(g1)
   % Sanity checks
   assert(g1{ii}.offset<=0,sprintf('%s: Invalid wavelet filters.',upper(mfilename)));
   assert(g2{ii}.offset<=0,sprintf('%s: Invalid wavelet filters.',upper(mfilename)));

   % Insert zeros and update offsets
   offdiff = g1{ii}.offset-g2{ii}.offset;
   if offdiff>0
       g1{ii}.offset = g1{ii}.offset - offdiff;
       g1{ii}.h = [zeros(offdiff,1);g1{ii}.h(:)];
   elseif offdiff<0
       g2{ii}.offset = g2{ii}.offset + offdiff;
       g2{ii}.h = [zeros(-offdiff,1);g2{ii}.h(:)];
   end

   % Pad with zeros to a common length
   lendiff = numel(g1{ii}.h) - numel(g2{ii}.h);
   if lendiff~=0
       maxLen = max(numel(g1{ii}.h),numel(g2{ii}.h));
       g1{ii}.h = postpad(g1{ii}.h,maxLen);
       g2{ii}.h = postpad(g2{ii}.h,maxLen);
   end
end

% Filters covering the positive frequencies
g = cellfun(@(gEl,g2El) setfield(gEl,'h',(gEl.h+1i*g2El.h)/2),g1,g2,'UniformOutput',0);

% Mirror the filters when negative frequency filters are required too
if ~do_real
   gneg = cellfun(@(gEl,g2El) setfield(gEl,'h',(gEl.h-1i*g2El.h)/2),g1,g2,'UniformOutput',0);
   g = [g;gneg(end:-1:1)];
   a = [a;a(end:-1:1)];
end


function [expr morder] = sim_ex_Mullen_2011_Seizure(varargin)
% Simulation:  Epileptic Seizure
%
% Description:
%
%   13-variate VAR[6] system of stocastically-forced, damped coupled oscillators with time-varying (non-stationary) coupling.
%
%   This simulation creates a simulated "seizure" with time-varying coupling between clusters of sources which switches between 4 different stages.
%
%   The simulation is designed to be single-trial.
%
%   Recommended Settings:
%   Number of trials: 1
%   Sampling Rate:    100
%
%   The directed graph for this model can be viewed by executing the following command:
%
%   >>hlp_viewGraphicsResource('/sim/Mullen_2011_Seizure.jpg');
%
%
% Author Credits:
%
%   Tim Mullen, 2011
%
% References and Code:
%
%   Mullen, 2011. Unpublished
%
% ------------------------------------------------------------------------

% Cluster 1
f1   = .2;      % central frequency
tau1 = .2;      % damping time (larger-->highest variance)
% Cluster 2
f2   = .20;
tau2 = 3;
% Cluster 3
f3   = .10;
tau3 = 7;
% Cluster 4
f4   = .10;
tau4 = 6;

% start of seizure (1 minute)
S0 = 6000;
% set the approximate durations of each stage
S1_duration = 500;  % 5 sec at 100 Hz
S2_duration = 500;
S3_duration = 500;
S4_duration = 500;
% set seizure stage mid-points
S1_center = S0+S1_duration/2;                      % center of stage 1
S2_center = S1_center+S1_duration;                 % center of stage 2
S3_center = S2_center;                             % center of stage 3
S4_center = S3_center+S3_duration;                 % center of stage 4

Offset = 0;
SamplingRate = 1;

% specify the default system of equations
expr_def = { ...
          sprintf('x1(t) = {2*exp(-1/(%f+normpdfg(t,%f,%f,%f,100)))*cos(2*pi*%f/1)}*x1(t-1) + {-exp(-2/(%f+normpdfg(t,%f,%f,%f,100)))}*x1(t-2) + e1(t)',tau1,S1_duration/2,8,Offset+S1_center, f1, tau1,S1_duration,8,Offset+S1_center) ...                    % Ictal driver
          sprintf('x2(t) = %s + {normpdfg(t,%f,%f,%f,0.01)}*x3(t-2) + {normpdfg(t,%f,%f,%f,0.01)}*x4(t-2)  + {normpdfg(t,%f,%f,%f,1.3)}*x1(t-6)    + e2(t)',sim_dampedOscillator(f2,tau2,SamplingRate,2),  S1_duration/2,8,Offset+S1_center,   S1_duration/2,8,Offset+S1_center,  S1_duration/2,8,Offset+S1_center) ...                     % CLUSTER 2
          sprintf('x3(t) = %s +  {normpdfg(t,%f,%f,%f,0.7)}*x2(t-2) + {normpdfg(t,%f,%f,%f,0.1)}*x4(t-2)  + {normpdfg(t,%f,%f,%f,0.3)}*x5(t-3)    + e3(t)',sim_dampedOscillator(f2-1,tau2,SamplingRate,3),  S1_duration/2,8,Offset+S1_center,   S1_duration/2,8,Offset+S1_center,   (S2_duration+S3_duration)/2,10,Offset+S3_center) ...       % CLUSTER 2
          sprintf('x4(t) = %s +  {normpdfg(t,%f,%f,%f,0.7)}*x2(t-2) + {normpdfg(t,%f,%f,%f,0.1)}*x3(t-2)                                          + e4(t)',sim_dampedOscillator(f2+1,tau2,SamplingRate,4),  S1_duration/2,8,Offset+S1_center,  S1_duration/2,8,Offset+S1_center) ... % CLUSTER 2
          sprintf('x5(t) = %s +  {normpdfg(t,%f,%f,%f,0.001)}*x6(t-2) + {normpdfg(t,%f,%f,%f,0.5)}*x3(t-3)        + e5(t)'  ,sim_dampedOscillator(f3,tau3,SamplingRate,5), S3_duration/2,8,Offset+S3_center , S3_duration/2,8,Offset+S3_center) ...  % CLUSTER 3
          sprintf('x6(t) = %s +  {normpdfg(t,%f,%f,%f,0.001)}*x5(t-2)                                             + e6(t)'  ,sim_dampedOscillator(f3,tau3,SamplingRate,6), S3_duration/2,8,Offset+S3_center) ...                                  % CLUSTER 3
          sprintf('x7(t) = %s + {normpdfg(t,%f,%f,%f,1.3)}*x4(t-6) + {normpdfg(t,%f,%f,%f,0.01)}*x9(t-2) + {normpdfg(t,%f,%f,%f,0.01)}*x8(t-2) + {normpdfg(t,%f,%f,%f,0.01)}*x10(t-2)    + e7(t)' ,sim_dampedOscillator(f4,tau4,SamplingRate,7),     S4_duration/2,8,Offset+S4_center,     S4_duration/2,8,Offset+S4_center,     S4_duration/2,8,Offset+S4_center,     S4_duration/2,8,Offset+S4_center) ...  % CLUSTER 4
          sprintf('x8(t) = %s + {normpdfg(t,%f,%f,%f,0.6)}*x7(t-2)         + e8(t)' ,sim_dampedOscillator(f4,tau4,SamplingRate,8),     S4_duration/2,8,Offset+S4_center) ... % CLUSTER 4
          sprintf('x9(t) = %s + {normpdfg(t,%f,%f,%f,0.6)}*x7(t-2)         + e9(t)' ,sim_dampedOscillator(f4+1,tau4,SamplingRate,9),   S4_duration/2,8,Offset+S4_center) ... % CLUSTER 4
          sprintf('x10(t)= %s + {normpdfg(t,%f,%f,%f,0.6)}*x7(t-2)         + e10(t)',sim_dampedOscillator(f4-1,tau4,SamplingRate,10),  S4_duration/2,8,Offset+S4_center) ... % CLUSTER 4
          sprintf('x11(t)= 1.3*x11(t-1) + -0.8*x11(t-2)                             + e11(t)') ...    % Decoupled
          sprintf('x12(t)= 1.2*x12(t-1) + -0.4*x12(t-2)                             + e12(t)') ...    % Decoupled
          sprintf('x13(t)= 0.8*x13(t-1) + -0.4*x13(t-2) + -0.1*x13(t-4)             + e13(t)') ...    % Decoupled
          };

% set up argument definitions
arg_define(varargin, ...
    arg({'expr','DynamicalEquations'},expr_def,[],'System of equations'), ...
    arg({'morder','ModelOrder'},6,[1 Inf],'Model order. This is mandatory'), ...
    arg_subtoggle({'setDynamics','SetDynamics'},'off', ...
        {...
            arg_sub({'c1','Cluster1'},{},...
            { ...
                arg({'f0','Freq'},f1,[0 Inf],'Central frequency (Hz). This is in normalized Hz (i.e. f0=0.5=SamplingRate/2).'), ...
                arg({'tau','DampingTime'},tau1,[0 Inf],'Damping time') ...
            },'Cluster 1 Settings'), ...
            arg_sub({'c2','Cluster2'},{},...
            { ...
                arg({'f0','Freq'},f2,[0 Inf],'Central frequency (Hz). This is in normalized Hz (i.e. f0=0.5=SamplingRate/2).'), ...
                arg({'tau','DampingTime'},tau2,[0 Inf],'Damping time') ...
            },'Cluster 2 Settings'), ...
            arg_sub({'c3','Cluster3'},{},...
            { ...
                arg({'f0','Freq'},f3,[0 Inf],'Central frequency (Hz). This is in normalized Hz (i.e. f0=0.5=SamplingRate/2).'), ...
                arg({'tau','DampingTime'},tau3,[0 Inf],'Damping time') ...
            },'Cluster 3 Settings'), ...
            arg_sub({'c4','Cluster4'},{},...
            { ...
                arg({'f0','Freq'},f4,[0 Inf],'Central frequency (Hz). This is in normalized Hz (i.e. f0=0.5=SamplingRate/2).'), ...
                arg({'tau','DampingTime'},tau4,[0 Inf],'Damping time') ...
            },'Cluster 4 Settings'), ...
            arg_sub({'s1','Stage1'},{},...
            { ...
                arg({'duration','Duration'},S1_duration,[0 Inf],'Duration (samples)'), ...
                arg({'center','Center'},S1_center,[0 Inf],'Midpoint of stage (samples)'), ...
            },'Stage 1 Settings'), ...
            arg_sub({'s2','Stage2'},{},...
            { ...
                arg({'duration','Duration'},S2_duration,[0 Inf],'Duration (samples)'), ...
                arg({'center','Center'},S2_center,[0 Inf],'Midpoint of stage (samples)'), ...
            },'Stage 2 Settings'), ...
            arg_sub({'s3','Stage3'},{},...
            { ...
                arg({'duration','Duration'},S3_duration,[0 Inf],'Duration (samples)'), ...
                arg({'center','Center'},S3_center,[0 Inf],'Midpoint of stage (samples)'), ...
            },'Stage 3 Settings'), ...
            arg_sub({'s4','Stage4'},{},...
            { ...
                arg({'duration','Duration'},S4_duration,[0 Inf],'Duration (samples)'), ...
                arg({'center','Center'},S4_center,[0 Inf],'Midpoint of stage (samples)'), ...
            },'Stage 4 Settings'), ...
            arg({'szStart','SeizureStart'},S0,[0 Inf],'Start of seizure (samples)'), ...
        },'Override default system dynamics'));

if isempty(morder)
    error('SIFT:sim_examples:badParam','ModelOrder must be specified');
end

if g.setDynamics.arg_selection
    % re-evaluate expression with user-defined settings

    % set central frequency and damping times
    % Cluster 1
    f1   = g.setDynamics.c1.f0;      % central frequency
    tau1 = g.setDyamics.c1.tau;      % damping time (larger-->highest variance)
    % Cluster 2
    f2   = g.setDynamics.c2.f0;
    tau2 = g.setDyamics.c2.tau;
    % Cluster 3
    f3   = g.setDyamics.c3.f0;
    tau3 = g.setDyamics.c3.tau;
    % Cluster 4
    f4   = g.setDyamics.c4.f0;
    tau4 = g.setDyamics.c4.tau;

    % set start of seizure
    S0 = g.setDynamics.szStart;

    % set the approximate durations of each stage
    S1_duration = g.setDynamics.s1.duration;
    S2_duration = g.setDynamics.s2.duration;
    S3_duration = g.setDynamics.s3.duration;
    S4_duration = g.setDynamics.s4.duration;

    % set seizure stage mid-points
    if ~isempty(g.setDynamics.s1.center)
        S1_center = g.setDynamics.s1.center;
    else
        S1_center = S0+S1_duration/2;
    end
    if ~isempty(g.setDynamics.s2.center)
        S2_center = g.setDynamics.s2.center;
    else
        S2_center = S1_center+S1_duration;
    end
    if ~isempty(g.setDynamics.s3.center)
        S3_center = g.setDynamics.s3.center;
    else
         S3_center = S2_center;
    end
    if ~isempty(g.setDynamics.s4.center)
        S4_center = g.setDynamics.s4.center;
    else
        S4_center = S3_center+S3_duration;
    end

    % generate system of equations
    expr = { ...
          sprintf('x1(t) = {2*exp(-1/(%f+normpdfg(t,%f,%f,%f,100)))*cos(2*pi*%f/1)}*x1(t-1) + {-exp(-2/(%f+normpdfg(t,%f,%f,%f,100)))}*x1(t-2) + e1(t)',tau1,S1_duration/2,8,Offset+S1_center, f1, tau1,S1_duration,8,Offset+S1_center) ...                    % Ictal driver
          sprintf('x2(t) = %s + {normpdfg(t,%f,%f,%f,0.01)}*x3(t-2) + {normpdfg(t,%f,%f,%f,0.01)}*x4(t-2)  + {normpdfg(t,%f,%f,%f,1.3)}*x1(t-6)    + e2(t)',sim_dampedOscillator(f2,tau2,SamplingRate,2),  S1_duration/2,8,Offset+S1_center,   S1_duration/2,8,Offset+S1_center,  S1_duration/2,8,Offset+S1_center) ...                     % CLUSTER 2
          sprintf('x3(t) = %s +  {normpdfg(t,%f,%f,%f,0.7)}*x2(t-2) + {normpdfg(t,%f,%f,%f,0.1)}*x4(t-2)  + {normpdfg(t,%f,%f,%f,0.3)}*x5(t-3)    + e3(t)',sim_dampedOscillator(f2-1,tau2,SamplingRate,3),  S1_duration/2,8,Offset+S1_center,   S1_duration/2,8,Offset+S1_center,   (S2_duration+S3_duration)/2,10,Offset+S3_center) ...       % CLUSTER 2
          sprintf('x4(t) = %s +  {normpdfg(t,%f,%f,%f,0.7)}*x2(t-2) + {normpdfg(t,%f,%f,%f,0.1)}*x3(t-2)                                          + e4(t)',sim_dampedOscillator(f2+1,tau2,SamplingRate,4),  S1_duration/2,8,Offset+S1_center,  S1_duration/2,8,Offset+S1_center) ... % CLUSTER 2
          sprintf('x5(t) = %s +  {normpdfg(t,%f,%f,%f,0.001)}*x6(t-2) + {normpdfg(t,%f,%f,%f,0.5)}*x3(t-3)        + e5(t)'  ,sim_dampedOscillator(f3,tau3,SamplingRate,5), S3_duration/2,8,Offset+S3_center , S3_duration/2,8,Offset+S3_center) ...  % CLUSTER 3
          sprintf('x6(t) = %s +  {normpdfg(t,%f,%f,%f,0.001)}*x5(t-2)                                             + e6(t)'  ,sim_dampedOscillator(f3,tau3,SamplingRate,6), S3_duration/2,8,Offset+S3_center) ...                                  % CLUSTER 3
          sprintf('x7(t) = %s + {normpdfg(t,%f,%f,%f,1.3)}*x4(t-6) + {normpdfg(t,%f,%f,%f,0.01)}*x9(t-2) + {normpdfg(t,%f,%f,%f,0.01)}*x8(t-2) + {normpdfg(t,%f,%f,%f,0.01)}*x10(t-2)    + e7(t)' ,sim_dampedOscillator(f4,tau4,SamplingRate,7),     S4_duration/2,8,Offset+S4_center,     S4_duration/2,8,Offset+S4_center,     S4_duration/2,8,Offset+S4_center,     S4_duration/2,8,Offset+S4_center) ...  % CLUSTER 4
          sprintf('x8(t) = %s + {normpdfg(t,%f,%f,%f,0.6)}*x7(t-2)         + e8(t)' ,sim_dampedOscillator(f4,tau4,SamplingRate,8),     S4_duration/2,8,Offset+S4_center) ... % CLUSTER 4
          sprintf('x9(t) = %s + {normpdfg(t,%f,%f,%f,0.6)}*x7(t-2)         + e9(t)' ,sim_dampedOscillator(f4+1,tau4,SamplingRate,9),   S4_duration/2,8,Offset+S4_center) ... % CLUSTER 4
          sprintf('x10(t)= %s + {normpdfg(t,%f,%f,%f,0.6)}*x7(t-2)         + e10(t)',sim_dampedOscillator(f4-1,tau4,SamplingRate,10),  S4_duration/2,8,Offset+S4_center) ... % CLUSTER 4
          sprintf('x11(t)= 1.3*x11(t-1) + -0.8*x11(t-2)                             + e11(t)') ...    % Decoupled
          sprintf('x12(t)= 1.2*x12(t-1) + -0.4*x12(t-2)                             + e12(t)') ...    % Decoupled
          sprintf('x13(t)= 0.8*x13(t-1) + -0.4*x13(t-2) + -0.1*x13(t-4)             + e13(t)') ...    % Decoupled
          };
end


function [C,h,Ph,F,Fa,Fc,Eh,Ch,hE,hC,Q] = spm_reml_sc(YY,X,Q,N,hE,hC,V)
% ReML estimation of covariance components from y*y' - proper components
% FORMAT [C,h,Ph,F,Fa,Fc,Eh,Ch,hE,hC,Q] = spm_reml_sc(YY,X,Q,N,[hE,hC,V])
%
% YY  - (m x m) sample covariance matrix Y*Y'  {Y = (m x N) data matrix}
% X   - (m x p) design matrix
% Q   - {1 x q} covariance components
% N   - number of samples
%
% hE  - hyperprior expectation in log-space [default = -32]
% hC  - hyperprior covariance  in log-space [default = 256]
% V   - fixed covariance component
%
% C   - (m x m) estimated errors = h(1)*Q{1} + h(2)*Q{2} + ...
% h   - (q x 1) ReML hyperparameters h
% Ph  - (q x q) conditional precision of log(h)
%
% hE  - prior expectation of log scale parameters
% hC  - prior covariances of log scale parameters
% Eh  - posterior expectation of log scale parameters
% Ch  - posterior covariances of log scale parameters
%
% Q   - scaled covariance components
%
% F   - [-ve] free energy F = log evidence = p(Y|X,Q) = ReML objective
%
% Fa  - accuracy
% Fc  - complexity (F = Fa - Fc)
%
% Performs a Fisher-Scoring ascent on F to find MAP variance parameter
% estimates.  NB: uses weakly informative log-normal hyperpriors.
% See also spm_reml for an unconstrained version that allows for negative
% hyperparameters.
%
%__________________________________________________________________________
%
% SPM ReML routines:
%
%      spm_reml:    no positivity constraints on covariance parameters
%      spm_reml_sc: positivity constraints on covariance parameters
%      spm_sp_reml: for sparse patterns (c.f., ARD)
%
%__________________________________________________________________________
% Copyright (C) 2007-2017 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_reml_sc.m 7305 2018-05-07 13:35:06Z karl $

% assume a single sample if not specified
%--------------------------------------------------------------------------
try, N; catch, N  = 1;  end
try, V; catch, V  = 0;  end

% initialise h
%--------------------------------------------------------------------------
n     = length(Q{1});
m     = length(Q);
h     = zeros(m,1);
dFdh  = zeros(m,1);
dFdhh = zeros(m,m);
Inn   = speye(n,n);

[PQ{1:m}] = deal(zeros(n,n));

% ortho-normalise X
%--------------------------------------------------------------------------
if isempty(X)
    X = sparse(n,0);
    R = Inn;
else
    X = spm_svd(X,0);
    R = Inn - X*X';
end

% check fixed component
%--------------------------------------------------------------------------
if length(V) == 1
    V = V*Inn;
end

% initialise and specify hyperpriors
%==========================================================================

% scale Q and YY
%--------------------------------------------------------------------------
sY = spm_trace(R,YY)/(N*n);
YY = YY/sY;
V  = V/sY;
for i = 1:m
    sh(i,1) = spm_trace(R,Q{i})/n;
    Q{i}    = Q{i}/sh(i);
end

% hyperpriors
%--------------------------------------------------------------------------
try, hE = hE(:);        catch, hE = -32;   end
try, hP = spm_inv(hC);  catch, hP = 1/256; end

% check sise
%--------------------------------------------------------------------------
if length(hE) < m, hE = hE(1)*ones(m,1);   end
if length(hP) < m, hP = hP(1)*speye(m,m);  end

% intialise h: so that sum(exp(h)) = 1
%--------------------------------------------------------------------------
if any(diag(hP) > exp(16))
    h = hE;
end

% ReML (EM/VB)
%--------------------------------------------------------------------------
dF    = Inf;
as    = 1:m;
t     = 4;
for k = 1:32

    % compute current estimate of covariance
    %----------------------------------------------------------------------
    C     = V;
    for i = as
        C = C + Q{i}*exp(h(i));
    end
    iC    = spm_inv(C);

    % E-step: conditional covariance cov(B|y) {Cq}
    %======================================================================
    iCX    = iC*X;
    if ~isempty(X)
        Cq = inv(X'*iCX);
    else
        Cq = sparse(0);
    end

    % M-step: ReML estimate of hyperparameters
    %======================================================================

    % Gradient dF/dh (first derivatives)
    %----------------------------------------------------------------------
    P     = iC  - iCX*Cq*iCX';
    U     = Inn - P*YY/N;
    for i = as

        % dF/dh = -trace(dF/diC*iC*Q{i}*iC)
        %------------------------------------------------------------------
        PQ{i}   = P*Q{i};
        dFdh(i) = -spm_trace(PQ{i},U)*N/2;

    end

    % Expected curvature E{dF/dhh} (second derivatives)
    %----------------------------------------------------------------------
    for i = as
        for j = as

            % dF/dhh = -trace{P*Q{i}*P*Q{j}}
            %--------------------------------------------------------------
            dFdhh(i,j) = -spm_trace(PQ{i},PQ{j})*N/2;
            dFdhh(j,i) =  dFdhh(i,j);

        end
    end

    % modulate
    %----------------------------------------------------------------------
    dFdh  = dFdh.*exp(h);
    dFdhh = dFdhh.*(exp(h)*exp(h)');

    % add hyperpriors
    %----------------------------------------------------------------------
    e     = h     - hE;
    dFdh  = dFdh  - hP*e;
    dFdhh = dFdhh - hP;

    % Fisher scoring: update dh = -inv(ddF/dhh)*dF/dh
    %----------------------------------------------------------------------
    dh    = spm_dx(dFdhh(as,as),dFdh(as),{t});
    h(as) = h(as) + dh;

    % predicted change in F - increase regularisation if increasing
    %----------------------------------------------------------------------
    pF    = dFdh(as)'*dh;
    if pF > dF
        t = t - 1;
    else
        t = t + 1/8;
    end
    dF    = pF;

    % convergence
    %----------------------------------------------------------------------
    fprintf('%s %-23d: %10s%e [%+3.2f]\n','  ReML Iteration',k,'...',full(dF),t);
    if dF < 1e-2
        break
    else
        % eliminate redundant components (automatic selection)
        %------------------------------------------------------------------
        as  = find(h > hE);
        as  = as(:)';
    end
end

% log evidence = ln p(y|X,Q) = ReML objective = F = trace(R'*iC*R*YY)/2 ...
%--------------------------------------------------------------------------
Ph    = -dFdhh;
if nargout > 3

    % tr(hP*inv(Ph)) - nh (complexity KL cost of parameters = 0)
    %----------------------------------------------------------------------
    Ft = trace(hP/Ph) - length(Ph);

    % complexity - KL(Ph,hP)
    %----------------------------------------------------------------------
    Fc = Ft/2 + e'*hP*e/2 + spm_logdet(Ph/hP)/2;

    % Accuracy - ln p(Y|h)
    %----------------------------------------------------------------------
    Fa = Ft/2 - spm_trace(C*P,YY*P)/2 - N*n*log(2*pi)/2  - N*spm_logdet(C)/2;

    % Free-energy
    %----------------------------------------------------------------------
    F  = Fa - Fc - N*n*log(sY)/2;

end

% priors and posteriors of log parameters (with scaling)
%--------------------------------------------------------------------------
if nargout > 7

    hE = hE + log(sY) - log(sh);
    hC = spm_inv(hP);
    Eh = h  + log(sY) - log(sh);
    Ch = spm_inv(Ph);

end

% return exp(h) hyperpriors and rescale
%--------------------------------------------------------------------------
h  = sY*exp(h)./sh;
C  = sY*C;


function SE = functionPowerOptimization_prodSINR(signal,interference,Pmax,prelogFactor)
%Compute DL power allocation that solves the max product SINR problem,
%using the algorithm in Theorem 7.2.
%
%This function require additional software packages to be used, which
%need to be downloaded and installed separately. These packages are
%developed independently and are delivered with separate licenses.
%The implementation uses CVX (http://cvxr.com/cvx) and has been tested
%using CVX version 2.1. We recommend the use of the Mosek solver (we
%have tested using version 7.1.0.12).
%
%INPUT:
%signal       = K x L matrix where element (k,j) is a_jk in (7.2)
%interference = K x L x K x L matrix where (l,i,j,k) is b_lijk in (7.3)
%Pmax         = Maximum transmit power per BS
%prelogFactor = Prelog factor
%
%OUTPUT:
%SE = K x L matrix where element (k,j) is the downlink SE of UE k in cell j
%     using the max product power allocation solution
%
%
%This Matlab function was developed to generate simulation results to:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.1 (Last edited: 2018-06-08)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

%Extract number of UEs
K = size(signal,1);

%Extract number of cells
L = size(signal,2);

%% Solve geometric program in (7.8) using CVX
cvx_begin gp
cvx_quiet(true); % This suppresses screen output from the solver

variable rho(K,L);
variable c(K,L);

maximize prod(prod(c))

subject to

for j = 1:L

    for k = 1:K

        if signal(k,j)>0
            %SINR constraints of UE k in cell j
            c(k,j)*(sum(sum(rho.*interference(:,:,k,j))) + 1) <= (rho(k,j)*signal(k,j));

            rho(k,j) >= 0;

        else
            %This applies if UE k in cell j is inactive
            c(k,j) == 1;
            rho(k,j) >= 0;

        end

    end

    sum(rho(:,j)) <= Pmax;

end

cvx_end

%% Analyze the CVX output and prepare the output variables
if isempty(strfind(cvx_status,'Solved')) %The problem was not solved by CVX, for some reason, and we then consider equal power allocation
    rhoSolution = (Pmax/K)*ones(K,L);
else %The problem was solved by CVX
    rhoSolution = rho;
end

%Refine the solution obtained from CVX using the Matlab command fmincon.
%This is particularly important in case CVX fails to solve the problem
A = kron(eye(L),ones(1,K));
B = Pmax*ones(L,1);
options = optimoptions('fmincon','Algorithm','interior-point','MaxFunEvals',50000,'MaxIter',5000);
xend = fmincon(@(x) -functionComputesumSE_DL_poweralloc(x,signal,interference,prelogFactor),rhoSolution(:),A,B,[],[],zeros(K*L,1),[],[],options);
rhoBest = reshape(xend,[K L]);

%Compute the SEs using Theorem 4.6
SE = functionComputeSE_DL_poweralloc(rhoBest,signal,interference,prelogFactor);

function sumSE = functionComputesumSE_DL_poweralloc(rho,signal,interference,prelogFactor)

%Extract number of UEs
K = size(signal,1);

%Extract number of cells
L = size(signal,2);

%Reshape power variable since fmincon optimizes vectors
rho = reshape(rho,[K L]);

%Prepare to save results
SE = zeros(K,L);

for j = 1:L

    for k = 1:K

        if signal(k,j) > 0 %Check if the UE k in cell j is active

            %Compute the SE in Theorem 4.6 using the formulation in (7.1)
            SE(k,j) = prelogFactor*log2((rho(k,j)*signal(k,j)) / (sum(sum(rho.*interference(:,:,k,j))) + 1));

        else %If the UE is inactive

            SE(k,j) = 0;

        end

    end

end

%Compute the sum SE of all cells
sumSE = sum(SE(:));


function [angle, check] = WardSimpleRotAux(img1, img2, rect)
%
%
%       rot = WardSimpleRotAux(img1, img2, rect)
%
%       This function computes the Ward's MTB.
%
%       Input:
%           -img1: the target image
%           -img2: the image that needs to be aligned to img1
%
%       Output:
%           -rot: rotation angle (degree) for aligning img2 into img1.
%
%     Copyright (C) 2013-15 Francesco Banterle. A big thank to Greg J. Ward
%     for help during the implementation.
%
%     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/>.
%

[r, c, ~] = size(img1);

maxDivergence = 0.005;

%First block
r_img1  = img1(rect(1):rect(2),rect(3):rect(4),:);
r_img2  = img2(rect(1):rect(2),rect(3):rect(4),:);
r1_shift = WardGetExpShift(r_img1, r_img2);

%Mirror block
rect_mirror(1) = r - rect(2) + 1;
rect_mirror(2) = r - rect(1) + 1;
rect_mirror(3) = c - rect(4) + 1;
rect_mirror(4) = c - rect(3) + 1;

r_img1  = img1(rect_mirror(1):rect_mirror(2),rect_mirror(3):rect_mirror(4),:);
r_img2  = img2(rect_mirror(1):rect_mirror(2),rect_mirror(3):rect_mirror(4),:);
r2_shift = WardGetExpShift(r_img1, r_img2);

dx = rect_mirror(3) - rect(3);
dy = rect_mirror(1) - rect(1);

dxr = dx + 0.5 * (r2_shift(1) - r1_shift(1));
dyr = dy + 0.5 * (r2_shift(2) - r1_shift(2));

value = abs(sqrt((dxr * dxr + dyr * dyr) / (dx * dx + dy * dy)) - 1.0);

if(value <= maxDivergence)
    angle = atan2(dyr, dxr) - atan2(dy, dx);
    angle = (angle * 180.0) / pi;
    check = 1;
else
    angle = 0.0;
    check = 0;
end

end