function [Out] = icaDim(Origdata,DEMDT,VN,Iterate,NDist)
% Matthew F. Glasser, Chad Donahue, Steve Smith, Christian Beckmann

%%%%%%%%%%%%%%%%%%%%
%Variables
Out.VNDIM=VN; %Variance Normalization Dimensionality initially set to 1
lnb = 0.5; %Lower noise bound for Golden Section Search
stabThresh = Iterate; %How many iterations meeting dimThresh criterion (1 for VNDIM=1 results, 2 for converged results)

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

%Remove Constant Timeseries and Detrend Data
if DEMDT==1
    data = Origdata(std(Origdata,[],2)>0,:);
    data_detrend = detrend(data')';
    data_trend = data - data_detrend;
elseif DEMDT==0
    data = Origdata(std(Origdata,[],2)>0,:);
    data_detrend = demean(data')';
    data_trend = single(zeros(size(data,1),size(data,2)));
elseif DEMDT==-1
    data = Origdata(std(Origdata,[],2)>0,:);
    data_detrend = data;
    data_trend = single(zeros(size(data,1),size(data,2)));
end

%Precompute PCA reconstruction
%octave doesn't have "rng", but it does accept "rand('twister', 0);" for matlab compatibility
rand('twister', 0);
[u,EigS,v]=nets_svds(data_detrend',0); %Eigenvalues of detrended data
Out.DOF=sum(diag(EigS)>(std(diag(EigS))*0.1)); %Compute degrees of freedom

c=1;
stabCount = 0;
while stabCount < stabThresh %Loop until dim output is stable

    c
    rand('twister', 0);
    %Variance normalization via PCA reconstruction: Isolate unstructured noise
    if VN~=0
      noise_unst = (u(:,Out.VNDIM(c):Out.DOF)*EigS(Out.VNDIM(c):Out.DOF,Out.VNDIM(c):Out.DOF)*v(:,Out.VNDIM(c):Out.DOF)')';
      Out.noise_unst_std = max(std(noise_unst,[],2),0.001);
      data_detrend_vn = data_detrend ./ repmat(Out.noise_unst_std,1,size(data_detrend,2));
    elseif VN==0
      data_detrend_vn = data_detrend;
      Out.noise_unst_std = single(ones(size(data_detrend,1),1));
    end
    if size(data_detrend_vn,1)<size(data_detrend_vn,2)
        if DEMDT~=-1
            d_pcaNorm=eig(cov(data_detrend_vn'));
        else
            d_pcaNorm=eig(data_detrend_vn*data_detrend_vn');
        end
    else
        if DEMDT~=-1
            d_pcaNorm=eig(cov(data_detrend_vn));
        else
            d_pcaNorm=eig(data_detrend_vn'*data_detrend_vn);
        end
    end
    Out.lambda_pcaNorm=flipud(d_pcaNorm);

    %Enable fitting multiple noise distributions
    DOF=Out.DOF;
    %DOF=sum(Out.lambda_pcaNorm>std(Out.lambda_pcaNorm)*0.1); %Recompute DOF because of demeaning in cov
    MaxX=length(data_detrend_vn);
    if DEMDT==-1
        MaxX=1000000;
    end
    lambda=Out.lambda_pcaNorm;
    S=0;
    clear EN
    for i=1:NDist
        [x(i),en] = FitWishart(lnb,S,DOF,MaxX,lambda(1:DOF));
        lambda=lambda(1:DOF)-en(1:DOF);
        en=[en;single(zeros(Out.DOF-length(en)+1,1))];
        EN(:,i)=en(1:Out.DOF);
        MaxX=x(i);
        DOF=min(find(lambda <= 0));
        lambda=[lambda(1:DOF);single(zeros(Out.DOF-DOF+1,1))];
        lambda=lambda(1:Out.DOF);
    end

    Out.EN=sum(EN,2);
    %Out.x(c)=x(1);
    Out.x(c)=round(x(end));
    %DOF=round(Out.DOF);

    %MaxS=5;
    %DOF=Out.DOF;
    %MaxX=round(Out.x(c));
    %MaxX=length(data_detrend_vn);
    %lambda=Out.lambda_pcaNorm;
    %[Out.x(c),Out.EN,Out.s(c)] = SmoothEst(lnb,MaxS,DOF,MaxX,lambda);

    %[Out.x(c),Out.EN] = FitWishart(lnb,Out.s(c),DOF,MaxX,lambda(1:DOF));

    %Divide eigenvalues by null for adjusted output values
    Out.lambdaAdj = abs(Out.lambda_pcaNorm(1:DOF)./(Out.EN(1:DOF)));
    %Out.lambdaAdj(DOF:end)=1;

    %Normalize adjusted eigenvalues to 1 for input to laplacian
    %Out.lambdaAdj_norm = Out.lambdaAdj ./ max(Out.lambdaAdj);
    Out.pcaDim_lambdaAdj = pca_dim(Out.lambdaAdj',Out.x(c));

    %Maximum of laplacian is estimate of optimal dimensionality
    [~, Out.calcDim]=max(Out.pcaDim_lambdaAdj.lap(1:DOF));

    if VN~=1
      stabCount = 2;
    end

    %Next loop will use calculated dimensionality
    c=c+1;

    Out.VNDIM(c) = Out.calcDim;
    %manual display of Out fields, to make octave less verbose
    disp('Out =');
    disp(['   VNDIM: ' mat2str(Out.VNDIM)]);
    disp(['   DOF: ' mat2str(Out.DOF)]);
    disp(['   noise_unst_std: array of size ' mat2str(size(Out.noise_unst_std))]);
    disp(['   lambda_pcaNorm: array of size ' mat2str(size(Out.lambda_pcaNorm))]);
    disp(['   EN: array of size ' mat2str(size(Out.EN))]);
    disp(['   x: ' mat2str(Out.x)]);
    disp(['   lambdaAdj: array of size ' mat2str(size(Out.lambdaAdj))]);
    disp(['   pcaDim_lambdaAdj: struct']);
    disp(['   calcDim: ' mat2str(Out.calcDim)]);

    %Store dims in array, check number of occurances to prevent dim loops
    stabCount = sum(Out.VNDIM==Out.calcDim);

end %End while loop for dim calcs
rand('twister', 0);
if DEMDT~=-1
    [u,EigS,v]=nets_svds(demean(data_detrend_vn)',0);
else
    [u,EigS,v]=nets_svds(data_detrend_vn',0);
end
u(isnan(u))=0; v(isnan(v))=0;
Out.EigSAdj=single(zeros(length(EigS),1));
Out.grot_one=diag(EigS(1:length(Out.EN),1:length(Out.EN)));
Out.grot_two=Out.grot_one.^2;
Out.grot_three=(Out.grot_two./max(Out.grot_two)).*max(Out.lambda_pcaNorm);
Out.grot_four=Out.grot_three-(Out.EN/median((Out.lambda_pcaNorm(1:Out.DOF)./max(Out.lambda_pcaNorm(1:Out.DOF)))./(diag(EigS(1:Out.DOF,1:Out.DOF).^2)./max(diag(EigS(1:Out.DOF,1:Out.DOF).^2)))));
Out.grot_five=(Out.grot_four./max(Out.lambda_pcaNorm)).*max(diag(EigS.^2));
firstOne = min(find(Out.grot_five <= 0));
Out.grot_six=Out.grot_five;
if ~length(firstOne)==0
    Out.grot_six(firstOne:end) = 0;
    Out.NewDOF=firstOne-1;
else
    Out.NewDOF=length(Out.grot_six);
end
%manual display of Out fields, to make octave less verbose
disp('Out =');
disp(['   VNDIM: ' mat2str(Out.VNDIM)]);
disp(['   DOF: ' mat2str(Out.DOF)]);
disp(['   noise_unst_std: array of size ' mat2str(size(Out.noise_unst_std))]);
disp(['   lambda_pcaNorm: array of size ' mat2str(size(Out.lambda_pcaNorm))]);
disp(['   EN: array of size ' mat2str(size(Out.EN))]);
disp(['   x: ' mat2str(Out.x)]);
disp(['   lambdaAdj: array of size ' mat2str(size(Out.lambdaAdj))]);
disp(['   pcaDim_lambdaAdj: struct']);
disp(['   calcDim: ' mat2str(Out.calcDim)]);
disp(['   EigSAdj: array of size ' mat2str(size(Out.EigSAdj))]);
disp(['   grot_one: array of size ' mat2str(size(Out.grot_one))]);
disp(['   grot_two: array of size ' mat2str(size(Out.grot_two))]);
disp(['   grot_three: array of size ' mat2str(size(Out.grot_three))]);
disp(['   grot_four: array of size ' mat2str(size(Out.grot_four))]);
disp(['   grot_five: array of size ' mat2str(size(Out.grot_five))]);
disp(['   grot_six: array of size ' mat2str(size(Out.grot_six))]);
disp(['   NewDOF: ' mat2str(Out.NewDOF)]);

Out.EigSAdj(1:length(Out.EN))=sqrt(Out.grot_six);

Out.data=single(zeros(size(Origdata,1),size(Origdata,2)));
if DEMDT~=-1
    Out.data(std(Origdata,[],2)>0,:)=data_trend + (((u*diag(Out.EigSAdj)*v')'+repmat(mean(data_detrend_vn),size(data_detrend_vn,1),1)) .* repmat(Out.noise_unst_std,1,size(data_detrend_vn,2)));
else
    Out.data(std(Origdata,[],2)>0,:)=data_trend + (((u*diag(Out.EigSAdj)*v')') .* repmat(Out.noise_unst_std,1,size(data_detrend_vn,2)));
end
temp=single(zeros(size(Origdata,1),1)); temp(std(Origdata,[],2)>0,:)=Out.noise_unst_std; Out.noise_unst_std=max(temp,0.001); clear temp;

end %End function

function [out] = lpdist(in)
    out=pdist(log(in));
end

function [x,EN] = FitWishart(lnb,S,DOF,MaxX,lambda) %FitWishart(lnb,step,DOF,MaxX,lambda)
    rand('twister', 0);
    EigDn1=round(DOF*lnb); %Isolate search to noise
    EigDn2=round(DOF-1); %Reqd for post MR+FIX deconcatinated tcs
    %EigDn2=round(DOF*0.75); %Reqd for post MR+FIX deconcatinated tcs

    a = DOF; %Lower bound for search range
    b = MaxX; %Upper bound for search range
    epsilon = 1; %Accuracy/stopping criterion
    iter = 500; %# iterations/secondary stopping criterion
    tau = double((sqrt(5)-1)/2); %Golden ratio (constant), 0.618...
    k = 1; %Iteration count

    %Initial section ranges to instantiate optimization
    x1 = a+(1-tau)*(b-a);
    x2 = a+tau*(b-a);
    %x1=a;
    %x2=b;

    %Calculate initial null spectra
    EN_x1 = iFeta([0:0.001:5],DOF,x1)'; %Call feta to calc null spectrum
    %EN_x1=flipud(eig(cov(Smooth(randn(round(x1),DOF),S))));
    EN_x1=EN_x1*median(lambda(EigDn1:EigDn2)./EN_x1(EigDn1:EigDn2)); %Remove offset between null & data
    f_x1 = lpdist([EN_x1(EigDn1:EigDn2)'; lambda(EigDn1:EigDn2)']); %Compute pairwise distance b/w null & data

    EN_x2 = iFeta([0:0.001:5],DOF,x2)';
    %EN_x2=flipud(eig(cov(Smooth(randn(round(x2),DOF),S))));
    EN_x2=EN_x2*median(lambda(EigDn1:EigDn2)./EN_x2(EigDn1:EigDn2));
    f_x2 = lpdist([EN_x2(EigDn1:EigDn2)'; lambda(EigDn1:EigDn2)']);

    while (abs(b-a)>epsilon) && (k<iter) %Loop until low error OR max iter met
        %k=k+1;
        rand('twister', 0);
        disp([num2str(a) ' ' num2str(x1) ' ' num2str(x2) ' ' num2str(b)]);
        %Check both terms for minimal pairwise distance and continue toward minimum
        if (f_x1<f_x2)
            b=x2;
            x2=x1;
            f_x2=f_x1;%don't recompute, this is the entire point of golden search
            EN_x2=EN_x1;
            x1=a+(1-tau)*(b-a);

            EN_x1 = iFeta([0:0.001:5],DOF,x1)';
            %EN_x1=flipud(eig(cov(Smooth(randn(round(x1),DOF),S))));
            EN_x1=EN_x1*median(lambda(EigDn1:EigDn2)./EN_x1(EigDn1:EigDn2));
            f_x1 = lpdist([EN_x1(EigDn1:EigDn2)'; lambda(EigDn1:EigDn2)']);

        else
            a=x1;
            x1=x2;
            f_x1=f_x2;
            EN_x1=EN_x2;
            x2=a+tau*(b-a);

            EN_x2 = iFeta([0:0.001:5],DOF,x2)';
            %EN_x2=flipud(eig(cov(Smooth(randn(round(x2),DOF),S))));
            EN_x2=EN_x2*median(lambda(EigDn1:EigDn2)./EN_x2(EigDn1:EigDn2));
            f_x2 = lpdist([EN_x2(EigDn1:EigDn2)'; lambda(EigDn1:EigDn2)']);
        end
        k = k+1;
    end

    %Compute null spectrum w/ optimal pairwise distance for output
    if (f_x1<f_x2)
        x = x1;
    else
        x = x2;
    end
    rand('twister', 0);
    %disp(randn(1,1));
    EN=flipud(eig(cov(Smooth(randn(round(x),DOF),S))));

    x=x/1+S;

    %Remove offset from null spectrum
    EN = EN*median(lambda(EigDn1:EigDn2) ./ EN(EigDn1:EigDn2));
end

function [x,EN,s] = SmoothEst(lnb,MaxS,DOF,MaxX,lambda)

    EigDn1=round(DOF*lnb); %Isolate search to noise
    EigDn2=round(DOF-1);

    M=randn(MaxX,DOF);

    a = 0; %Lower bound for search range
    b = MaxS; %Upper bound for search range
    epsilon = 0.01; %Accuracy/stopping criterion
    iter = 500; %# iterations/secondary stopping criterion
    tau = double((sqrt(5)-1)/2); %Golden ratio (constant), 0.618...
    k = 1; %Iteration count

    %Initial section ranges to instantiate optimization
    s1 = a+(1-tau)*(b-a);
    s2 = a+tau*(b-a);
    %x1=a;
    %x2=b;

    %Calculate initial null spectra
    %EN_x1 = iFeta([0:step:5],DOF,x1)'; %Call feta to calc null spectrum
    EN_s1=flipud(eig(cov(Smooth(M,s1/(2*sqrt(2*log(2)))))));
    EN_s1=EN_s1*median(lambda(EigDn1:EigDn2)./EN_s1(EigDn1:EigDn2)); %Remove offset between null & data
    f_s1 = lpdist([EN_s1(EigDn1:EigDn2)'; lambda(EigDn1:EigDn2)']); %Compute pairwise distance b/w null & data

    %EN_x2 = iFeta([0:step:5],DOF,x2)';
    EN_s2=flipud(eig(cov(Smooth(M,s2/(2*sqrt(2*log(2)))))));
    EN_s2=EN_s2*median(lambda(EigDn1:EigDn2)./EN_s2(EigDn1:EigDn2));
    f_s2 = lpdist([EN_s2(EigDn1:EigDn2)'; lambda(EigDn1:EigDn2)']);

    while (abs(b-a)>epsilon) && (k<iter) %Loop until low error OR max iter met
        %k=k+1;
        disp([num2str(a) ' ' num2str(s1) ' ' num2str(s2) ' ' num2str(b)]);
        %Check both terms for minimal pairwise distance and continue toward minimum
        if (f_s1<f_s2)
            b=s2;
            s2=s1;
            f_s2=f_s1;%don't recompute, this is the entire point of golden search
            EN_s2=EN_s1;
            s1=a+(1-tau)*(b-a);

            %EN_x1 = iFeta([0:step:5],DOF,x1)';
            EN_s1=flipud(eig(cov(Smooth(M,s1/(2*sqrt(2*log(2)))))));
            EN_s1=EN_s1*median(lambda(EigDn1:EigDn2)./EN_s1(EigDn1:EigDn2));
            f_s1 = lpdist([EN_s1(EigDn1:EigDn2)'; lambda(EigDn1:EigDn2)']);

        else
            a=s1;
            s1=s2;
            f_s1=f_s2;
            EN_s1=EN_s2;
            s2=a+tau*(b-a);

            %EN_x2 = iFeta([0:step:5],DOF,x2)';
            EN_s2=flipud(eig(cov(Smooth(M,s2))));
            EN_s2=EN_s2*median(lambda(EigDn1:EigDn2)./EN_s2(EigDn1:EigDn2));
            f_s2 = lpdist([EN_s2(EigDn1:EigDn2)'; lambda(EigDn1:EigDn2)']);
        end
        k = k+1;
    end

    %Compute null spectrum w/ optimal pairwise distance for output
    if (f_s1<f_s2)
        s = s1;
    else
        s = s2;
    end

    %EN = iFeta([0:step:5],DOF,x)';
    %EN=flipud(eig(cov(randn(round(x),DOF))));
    EN=flipud(eig(cov(Smooth(M,s/(2*sqrt(2*log(2)))))));
    x=MaxX/(1+s); %Number of resels
    %Remove offset from null spectrum
    EN = EN*median(lambda(EigDn1:EigDn2) ./ EN(EigDn1:EigDn2));
end

function [O] = Smooth(M,S)
if S==0
    O=M;
else
sigma = S/(2*sqrt(2*log(2)));
%sz = S*6;    % length of gaussFilter vector
%x = linspace(-sz / 2, sz / 2, sz);
%gaussFilter = exp(-x .^ 2 / (2 * sigma ^ 2));
%gaussFilter = gaussFilter / sum (gaussFilter); % normalize

width = round((6*sigma - 1)/2);
support = (-width:width);
gaussFilter = exp( -(support).^2 ./ (2*sigma^2) );
gaussFilter = gaussFilter/ sum(gaussFilter);

O=single(zeros(size(M,1),size(M,2)));

for i=1:size(M,2)
    op
    O(:,i) = conv (M(:,i), gaussFilter, 'same');
    %O(:,i) = filter (gaussFilter,1,M(:,i));
end
end
end


%CA driver
%HPP-gas

clear all
clf

nx=52; %must be divisible by 4
ny=100;

z=zeros(nx,ny);
o=ones(nx,ny);
sand = z ;
sandNew = z;
gnd = z ;
diag1 = z;
diag2 = z;
and12 = z;
or12 = z;
sums = z;
orsum = z;

gnd(1:nx,ny-3)=1 ; % right ground line
gnd(1:nx,3)=1 ; % left ground line
gnd(nx/4:nx/2-2,ny/2)=1; %the hole line
gnd(nx/2+2:nx,ny/2)=1; %the hole line
gnd(nx/4, 1:ny) = 1; %top line
gnd(3*nx/4, 1:ny) = 1 ;%bottom line

%fill the left side
r = rand(nx,ny);
sand(nx/4+1:3*nx/4-1, 4:ny/2-1) = r(nx/4+1:3*nx/4-1, 4:ny/2-1)<0.3;
%sand(nx/4+1:3*nx/4-1, ny*.75:ny-4) = r(nx/4+1:3*nx/4-1, ny*.75:ny-4)<0.75;
%sand(nx/2,ny/2) = 1;
%sand(nx/2+1,ny/2+1) = 1;

imh = image(cat(3,z,sand,gnd));
set(imh, 'erasemode', 'none')
axis equal
axis tight

for i=1:1000
    p=mod(i,2); %margolis neighborhood

    %upper left cell update
    xind = [1+p:2:nx-2+p];
    yind = [1+p:2:ny-2+p];

    %See if exactly one diagonal is ones
    %only (at most) one of the following can be true!
    diag1(xind,yind) = (sand(xind,yind)==1) & (sand(xind+1,yind+1)==1) & ...
        (sand(xind+1,yind)==0) & (sand(xind,yind+1)==0);

    diag2(xind,yind) = (sand(xind+1,yind)==1) & (sand(xind,yind+1)==1) & ...
        (sand(xind,yind)==0) & (sand(xind+1,yind+1)==0);

    %The diagonals both not occupied by two particles
    and12(xind,yind) = (diag1(xind,yind)==0) & (diag2(xind,yind)==0);

    %One diagonal is occupied by two particles
    or12(xind,yind)  = diag1(xind,yind) | diag2(xind,yind);

    %for every gas particle see if it near the boundary
    sums(xind,yind) = gnd(xind,yind) | gnd(xind+1,yind) | ...
                        gnd(xind,yind+1) | gnd(xind+1,yind+1) ;

    % cell layout:
    % x,y    x+1,y
    % x,y+1  x+1,y+1
    %If (no walls) and (diagonals are both not occupied)
    %then there is no collision, so move opposite cell to current cell
    %If (no walls) and (only one diagonal is occupied)
    %then there is a collision so move ccw cell to the current cell
    %If (a wall)
    %then don't change the cell (causes a reflection)
    sandNew(xind,yind) = ...
        (and12(xind,yind)  & ~sums(xind,yind) & sand(xind+1,yind+1)) + ...
        (or12(xind,yind) & ~sums(xind,yind) & sand(xind,yind+1)) + ...
        (sums(xind,yind) & sand(xind,yind));

    sandNew(xind+1,yind) = ...
        (and12(xind,yind)  & ~sums(xind,yind) & sand(xind,yind+1)) + ...
        (or12(xind,yind) & ~sums(xind,yind) & sand(xind,yind))+ ...
        (sums(xind,yind) & sand(xind+1,yind));

    sandNew(xind,yind+1) = ...
        (and12(xind,yind)  & ~sums(xind,yind) & sand(xind+1,yind)) + ...
        (or12(xind,yind) & ~sums(xind,yind) & sand(xind+1,yind+1))+ ...
        (sums(xind,yind) & sand(xind,yind+1));

     sandNew(xind+1,yind+1) = ...
        (and12(xind,yind)  & ~sums(xind,yind) & sand(xind,yind)) + ...
        (or12(xind,yind) & ~sums(xind,yind) & sand(xind+1,yind))+ ...
        (sums(xind,yind) & sand(xind+1,yind+1));

    sand = sandNew;

    set(imh, 'cdata', cat(3,z,sand,gnd) )
    drawnow
end


function [traj, infStates] = tapas_hgf_binary_pu_tbt(r, p, varargin)
% Calculates the trajectories of the agent's representations under the HGF
%
% This function can be called in two ways:
%
% (1) tapas_hgf_binary_pu_tbt(r, p)
%
%     where r is the structure generated by tapas_fitModel and p is the parameter vector in native space;
%
% (2) tapas_hgf_binary_pu_tbt(r, ptrans, 'trans')
%
%     where r is the structure generated by tapas_fitModel, ptrans is the parameter vector in
%     transformed space, and 'trans' is a flag indicating this.
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2016-2017 Rebecca Lawson, Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Transform paramaters back to their native space if needed
if ~isempty(varargin) && strcmp(varargin{1},'trans');
    p = tapas_hgf_binary_pu_tbt_transp(r, p);
end

% Number of levels
try
    l = r.c_prc.n_levels;
catch
    l = (length(p)+1)/5;

    if l ~= floor(l)
        error('tapas:hgf:UndetNumLevels', 'Cannot determine number of levels');
    end
end

% Unpack parameters
mu_0 = p(1:l);
sa_0 = p(l+1:2*l);
rho  = p(2*l+1:3*l);
ka   = p(3*l+1:4*l-1);
om   = p(4*l:5*l-2);
th   = exp(p(5*l-1));
eta0 = p(5*l);
eta1 = p(5*l+1);

% Add dummy "zeroth" trial
u = [0; r.u(:,1)];
al= [0; r.u(:,2)];

% Number of trials (including prior)
n = length(u);

% Assume that if u has more than one column, the last contains t
try
    if r.c_prc.irregular_intervals
        if size(u,2) > 1
            t = [0; r.u(:,end)];
        else
            error('tapas:hgf:InputSingleColumn', 'Input matrix must contain more than one column if irregular_intervals is set to true.');
        end
    else
        t = ones(n,1);
    end
catch
    if size(u,2) > 1
        t = [0; r.u(:,end)];
    else
        t = ones(n,1);
    end
end

% Initialize updated quantities

% Representations
mu = NaN(n,l);
pi = NaN(n,l);

% Other quantities
muhat = NaN(n,l);
pihat = NaN(n,l);
v     = NaN(n,l);
w     = NaN(n,l-1);
da    = NaN(n,l);

% Representation priors
% Note: first entries of the other quantities remain
% NaN because they are undefined and are thrown away
% at the end; their presence simply leads to consistent
% trial indices.
mu(1,1) = tapas_sgm(mu_0(1), 1);
pi(1,1) = Inf;
mu(1,2:end) = mu_0(2:end);
pi(1,2:end) = 1./sa_0(2:end);

% Pass through representation update loop
for k = 2:1:n
    if not(ismember(k-1, r.ign))

        %%%%%%%%%%%%%%%%%%%%%%
        % Effect of input u(k)
        %%%%%%%%%%%%%%%%%%%%%%

        % 2nd level prediction
        muhat(k,2) = mu(k-1,2) +t(k) *rho(2);

        % 1st level
        % ~~~~~~~~~
        % Prediction
        muhat(k,1) = tapas_sgm(ka(1) *muhat(k,2), 1);

        % Precision of prediction
        pihat(k,1) = 1/(muhat(k,1)*(1 -muhat(k,1)));

        % Mean update
        mu(k,1) = u(k);
        und1 = exp(-(u(k) -eta1)^2/(2*al(k)));
        und0 = exp(-(u(k) -eta0)^2/(2*al(k)));
        mu(k,1) = muhat(k,1) *und1 /(muhat(k,1) *und1 +(1 -muhat(k,1)) *und0);

        % Prediction error
        da(k,1) = mu(k,1) -muhat(k,1);

        % 2nd level
        % ~~~~~~~~~
        % Prediction: see above

        % Precision of prediction
        pihat(k,2) = 1/(1/pi(k-1,2) +exp(ka(2) *mu(k-1,3) +om(2)));

        % Updates
        pi(k,2) = pihat(k,2) +ka(1)^2/pihat(k,1);
        mu(k,2) = muhat(k,2) +ka(1)/pi(k,2) *da(k,1);

        % Implied posterior precision at first level
        sgmmu2 = tapas_sgm(ka(1) *mu(k,2), 1);
        pi(k,1) = pi(k,2)/(sgmmu2*(1-sgmmu2));

        % Volatility prediction error
        da(k,2) = (1/pi(k,2) +(mu(k,2) -muhat(k,2))^2) *pihat(k,2) -1;

        if l > 3
            % Pass through higher levels
            % ~~~~~~~~~~~~~~~~~~~~~~~~~~
            for j = 3:l-1
                % Prediction
                muhat(k,j) = mu(k-1,j) +t(k) *rho(j);

                % Precision of prediction
                pihat(k,j) = 1/(1/pi(k-1,j) +t(k) *exp(ka(j) *mu(k-1,j+1) +om(j)));

                % Weighting factor
                v(k,j-1) = t(k) *exp(ka(j-1) *mu(k-1,j) +om(j-1));
                w(k,j-1) = v(k,j-1) *pihat(k,j-1);

                % Updates
                pi(k,j) = pihat(k,j) +1/2 *ka(j-1)^2 *w(k,j-1) *(w(k,j-1) +(2 *w(k,j-1) -1) *da(k,j-1));

                if pi(k,j) <= 0
                    error('tapas:hgf:NegPostPrec', 'Negative posterior precision. Parameters are in a region where model assumptions are violated.');
                end

                mu(k,j) = muhat(k,j) +1/2 *1/pi(k,j) *ka(j-1) *w(k,j-1) *da(k,j-1);

                % Volatility prediction error
                da(k,j) = (1/pi(k,j) +(mu(k,j) -muhat(k,j))^2) *pihat(k,j) -1;
            end
        end

        % Last level
        % ~~~~~~~~~~
        % Prediction
        muhat(k,l) = mu(k-1,l) +t(k) *rho(l);

        % Precision of prediction
        pihat(k,l) = 1/(1/pi(k-1,l) +t(k) *th);

        % Weighting factor
        v(k,l)   = t(k) *th;
        v(k,l-1) = t(k) *exp(ka(l-1) *mu(k-1,l) +om(l-1));
        w(k,l-1) = v(k,l-1) *pihat(k,l-1);

        % Updates
        pi(k,l) = pihat(k,l) +1/2 *ka(l-1)^2 *w(k,l-1) *(w(k,l-1) +(2 *w(k,l-1) -1) *da(k,l-1));

        if pi(k,l) <= 0
            error('tapas:hgf:NegPostPrec', 'Negative posterior precision. Parameters are in a region where model assumptions are violated.');
        end

        mu(k,l) = muhat(k,l) +1/2 *1/pi(k,l) *ka(l-1) *w(k,l-1) *da(k,l-1);

        % Volatility prediction error
        da(k,l) = (1/pi(k,l) +(mu(k,l) -muhat(k,l))^2) *pihat(k,l) -1;
    else

        mu(k,:) = mu(k-1,:);
        pi(k,:) = pi(k-1,:);

        muhat(k,:) = muhat(k-1,:);
        pihat(k,:) = pihat(k-1,:);

        v(k,:)  = v(k-1,:);
        w(k,:)  = w(k-1,:);
        da(k,:) = da(k-1,:);

    end
end

% Implied learning rate at the first level
sgmmu2 = tapas_sgm(ka(1) *mu(:,2), 1);
lr1    = diff(sgmmu2)./da(2:n,1);
lr1(da(2:n,1)==0) = 0;

% Remove representation priors
mu(1,:)  = [];
pi(1,:)  = [];

% Check validity of trajectories
if any(isnan(mu(:))) || any(isnan(pi(:)))
    error('tapas:hgf:VarApproxInvalid', 'Variational approximation invalid. Parameters are in a region where model assumptions are violated.');
else
    % Check for implausible jumps in trajectories
    dmu = diff(mu(:,2:end));
    dpi = diff(pi(:,2:end));
    rmdmu = repmat(sqrt(mean(dmu.^2)),length(dmu),1);
    rmdpi = repmat(sqrt(mean(dpi.^2)),length(dpi),1);

    jumpTol = 16;
    if any(abs(dmu(:)) > jumpTol*rmdmu(:)) || any(abs(dpi(:)) > jumpTol*rmdpi(:))
        error('tapas:hgf:VarApproxInvalid', 'Variational approximation invalid. Parameters are in a region where model assumptions are violated.');
    end
end

% Remove other dummy initial values
muhat(1,:) = [];
pihat(1,:) = [];
v(1,:)     = [];
w(1,:)     = [];
da(1,:)    = [];

% Create result data structure
traj = struct;

traj.mu     = mu;
traj.sa     = 1./pi;

traj.muhat  = muhat;
traj.sahat  = 1./pihat;

traj.v      = v;
traj.w      = w;
traj.da     = da;

% Updates with respect to prediction
traj.ud = mu -muhat;

% Psi (precision weights on prediction errors)
psi        = NaN(n-1,l);
psi(:,2)   = 1./pi(:,2);
psi(:,3:l) = pihat(:,2:l-1)./pi(:,3:l);
traj.psi   = psi;

% Epsilons (precision-weighted prediction errors)
epsi        = NaN(n-1,l);
epsi(:,2:l) = psi(:,2:l) .*da(:,1:l-1);
traj.epsi   = epsi;

% Full learning rate (full weights on prediction errors)
wt        = NaN(n-1,l);
wt(:,1)   = lr1;
wt(:,2)   = psi(:,2);
wt(:,3:l) = 1/2 *(v(:,2:l-1) *diag(ka(2:l-1))) .*psi(:,3:l);
traj.wt   = wt;

% Create matrices for use by the observation model
infStates = NaN(n-1,l,4);
infStates(:,:,1) = traj.muhat;
infStates(:,:,2) = traj.sahat;
infStates(:,:,3) = traj.mu;
infStates(:,:,4) = traj.sa;

return;


function [rpav]=realized_preaveraged_variance(price,time,timeType,samplingType,samplingInterval,options)
% Estimated quadratic variation using Preaveraged Realized Variance
%
% USAGE:
%   [RPAV] = realized_preaveraged_variance(PRICE)
%   [RPAV] = realized_preaveraged_variance(PRICE,TIME,TIMETYPE,SAMPLINGTYPE,SAMPLINGINTERVAL)
%   [RPAV] = realized_preaveraged_variance(PRICE,TIME,TIMETYPE,SAMPLINGTYPE,SAMPLINGINTERVAL,OPTIONS)
%
% INPUTS:
%   PRICE            - m by 1 vector of high frequency prices
%   TIME             - [OPTIONAL] m by 1 vector of times where TIME(i) corresponds to PRICE(i).
%   TIMETYPE         - [OPTIONAL] String describing the way times are measured
%                       'wall'    24-hour clock of the form HHMMSS.mmm, e.g. 101543 or 153217
%                       'seconds' Time measured in seconds past midnight
%                       'unit'  Unit normalized date format, e.g. .1, .234, .9
%                         Unit normalized times are more general than the other types and can be
%                         applied to data from more than one calendar day
%   SAMPLINGTYPE     - [OPTIONAL] String describing the type of sampling to use when
%                        filtering PRICE
%                        'CalendarTime' - Sample in calendar time using observations separated by
%                          SAMPLINGINTERVAL seconds
%                        'CalendarUniform' - Sample in calendar time using SAMPLINGINTERVAL
%                          observations spread uniformly between TIME(1) and TIME(m)
%                        'BusinessTime' - Sample in business (tick) time using observation separated
%                          by SAMPLINGINTERVAL ticks
%                        'BusinessUniform' - Sample in business (tick) time using observations
%                          uniformly spaced in business time.
%                        'Fixed' - Sample at specific points in time. When using fixed,
%                          SAMPLINGINTERVAL must be a n by 1 vector of times with the same TIMETYPE
%                          as TIME (i.e. seconds if TIME is in seconds)
%   SAMPLINGINTERVAL  - [OPTIONAL] Scalar integer or n by 1 vector whose meaning depends on the
%                         selected SAMPLINGTYPE
%   OPTIONS           - [OPTIONAL] Preaveraged Realized Variance option structure initialized by calling
%                         realized_options('Preaveraging'). See help realized_options for a description of
%                         available options.
%
% OUTPUTS:
%   RK          - Preaveraged realized variance estimate
%
% COMMENTS:
%  Follows Christensen, Oomen and Podolski (2014) mostl closely, with the
%  noise variance estimator usef in Hautsch and Podolski (2013)
%
% EXAMPLES:
%
%  See also REALIZED_OPTIONS, REALIZED_KERNEL, REALIZED_NOISE_ESTIMATE, REALIZED_VARIANCE,
%  REALIZED_VARIANCE_OPTIMAL_SAMPLING, REALIZED_RANGE, REALIZED_QUANTILE_VARIANCE

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 1    Date: 2/27/2014

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
switch nargin
    case 1
        m = length(price);
        time = linspace(9.5*3600,16*3600,m)';
        timeType = 'seconds';
        samplingType = 'businesstime';
        samplingInterval = 1;
        options = realized_options('preaveraging');
    case 5
        options = realized_options('preaveraging');
    case 6
        % Nothing
    otherwise
        error('One, five or six inputs required.')
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Need method to estimate omega, and options.

% Inserted to protect against inputing integer times
time = double(time);

% 1. Filter the rpice
filteredPrice = realized_price_filter(price,time,timeType,samplingType,samplingInterval);
returns = diff(log(filteredPrice));
% 2. Compute rpreaveraged returns
theta = options.theta;
m = length(returns);
K = ceil(theta * sqrt(m));
g = @(x) min(x,1-x);
w = g((1:(K-1))/K);

preav_returns = nan(m-K+2,1);
for i=1:m-K+2
    preav_returns(i) = w*returns(i:i+K-2);
end

% 3. Compute constants
psi_1 = K*sum((g((1:K)/K) - g((0:(K-1))/K)).^2);
psi_2 = 1/K*sum((g((1:(K-1))/K)).^2);
% 4. Estimate noise
[noiseVariance, ~, ~, noiseEstimateOomen] = realized_noise_estimate(price, time, timeType, options);

omega = noiseEstimateOomen;
if omega<0
    omega = noiseVariance;
end
% 5. Compute Preaveraed Variance
const1 = m/(m-K+2);
const2 = 1/(K*psi_2);
bias = psi_1/(theta.^2 * psi_2) * omega^2;

rpav = const1 * const2 * sum(preav_returns.^2) - bias;


function [V, converged, i] = fdpf(Ybus, Sbus, V0, Bp, Bpp, ref, pv, pq, mpopt)
%FDPF  Solves the power flow using a fast decoupled method.
%   [V, CONVERGED, I] = FDPF(YBUS, SBUS, V0, BP, BPP, REF, PV, PQ, MPOPT)
%   solves for bus voltages given the full system admittance matrix (for
%   all buses), the complex bus power injection vector (for all buses),
%   the initial vector of complex bus voltages, the FDPF matrices B prime
%   and B double prime, and column vectors with the lists of bus indices
%   for the swing bus, PV buses, and PQ buses, respectively. The bus voltage
%   vector contains the set point for generator (including ref bus)
%   buses, and the reference angle of the swing bus, as well as an initial
%   guess for remaining magnitudes and angles. MPOPT is a MATPOWER options
%   vector which can be used to set the termination tolerance, maximum
%   number of iterations, and output options (see MPOPTION for details).
%   Uses default options if this parameter is not given. Returns the
%   final complex voltages, a flag which indicates whether it converged
%   or not, and the number of iterations performed.
%
%   See also RUNPF.

%   MATPOWER
%   Copyright (c) 1996-2016, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

%% default arguments
if nargin < 7
    mpopt = mpoption;
end

%% options
tol     = mpopt.pf.tol;
max_it  = mpopt.pf.fd.max_it;
lu_vec  = have_feature('lu_vec');

%% initialize
converged = 0;
i = 0;
V = V0;
Va = angle(V);
Vm = abs(V);

%% set up indexing for updating V
npv = length(pv);
npq = length(pq);

%% evaluate initial mismatch
mis = (V .* conj(Ybus * V) - Sbus(Vm)) ./ Vm;
P = real(mis([pv; pq]));
Q = imag(mis(pq));

%% check tolerance
normP = norm(P, inf);
normQ = norm(Q, inf);
if mpopt.verbose > 1
    fprintf('\niteration     max mismatch (p.u.)  ');
    fprintf('\ntype   #        P            Q     ');
    fprintf('\n---- ----  -----------  -----------');
    fprintf('\n  -  %3d   %10.3e   %10.3e', i, normP, normQ);
end
if normP < tol && normQ < tol
    converged = 1;
    if mpopt.verbose > 1
        fprintf('\nConverged!\n');
    end
end

%% reduce B matrices
Bp = Bp([pv; pq], [pv; pq]);
Bpp = Bpp(pq, pq);

%% factor B matrices
if lu_vec
    [Lp,  Up,  pp,  qp ] = lu(Bp,  'vector');
    [Lpp, Upp, ppp, qpp] = lu(Bpp, 'vector');
    [junk, iqp ] = sort(qp);
    [junk, iqpp] = sort(qpp);
    % [~, iqp ] = sort(qp);
    % [~, iqpp] = sort(qpp);
else
    [Lp, Up, Pp] = lu(Bp);
    [Lpp, Upp, Ppp] = lu(Bpp);
end

%% do P and Q iterations
while (~converged && i < max_it)
    %% update iteration counter
    i = i + 1;

    %%-----  do P iteration, update Va  -----
    if lu_vec
        dVa = -( Up \  (Lp \ P(pp)) );
        dVa = dVa(iqp);
    else
        dVa = -( Up \  (Lp \ (Pp * P)));
    end

    %% update voltage
    Va([pv; pq]) = Va([pv; pq]) + dVa;
    V = Vm .* exp(1j * Va);

    %% evalute mismatch
    mis = (V .* conj(Ybus * V) - Sbus(Vm)) ./ Vm;
    P = real(mis([pv; pq]));
    Q = imag(mis(pq));

    %% check tolerance
    normP = norm(P, inf);
    normQ = norm(Q, inf);
    if mpopt.verbose > 1
        fprintf('\n  P  %3d   %10.3e   %10.3e', i, normP, normQ);
    end
    if normP < tol && normQ < tol
        converged = 1;
        if mpopt.verbose
            fprintf('\nFast-decoupled power flow converged in %d P-iterations and %d Q-iterations.\n', i, i-1);
        end
        break;
    end

    %%-----  do Q iteration, update Vm  -----
    if lu_vec
        dVm = -( Upp \ (Lpp \ Q(ppp)) );
        dVm = dVm(iqpp);
    else
        dVm = -( Upp \ (Lpp \ (Ppp * Q)) );
    end

    %% update voltage
    Vm(pq) = Vm(pq) + dVm;
    V = Vm .* exp(1j * Va);

    %% evalute mismatch
    mis = (V .* conj(Ybus * V) - Sbus(Vm)) ./ Vm;
    P = real(mis([pv; pq]));
    Q = imag(mis(pq));

    %% check tolerance
    normP = norm(P, inf);
    normQ = norm(Q, inf);
    if mpopt.verbose > 1
        fprintf('\n  Q  %3d   %10.3e   %10.3e', i, normP, normQ);
    end
    if normP < tol && normQ < tol
        converged = 1;
        if mpopt.verbose
            fprintf('\nFast-decoupled power flow converged in %d P-iterations and %d Q-iterations.\n', i, i);
        end
        break;
    end
end

if mpopt.verbose
    if ~converged
        fprintf('\nFast-decoupled power flow did not converge in %d iterations.\n', i);
    end
end


% GP_COV_PP - Piecewise polynomial covariance function with compact support
% for Gaussian processes.
%
% [K, DK_LOGTHETA, DK_X2] = GP_COV_CS(X1, X2, LOGTHETA)

% Last modified 2010-10-06
% Copyright (c) Jaakko Luttinen (jaakko.luttinen@tkk.fi)

function covfunc = gp_cov_pp_init(x1,x2)

% Dimensions
d = rows(x2); % dimensionality of inputs
m = cols(x1); % number of other inputs
n = cols(x2); % number of inputs

q = 2; % PP parameter, fix to 2 for now..

% Distance matrix
D = sqrt(sq_dist(X1,X2));

covfunc = @set_hyperparameters;

  function [K,L] = set_hyperparameters(logtheta)

  % Threshold
  thres = exp(logtheta(1)); % length scale, threshold
  R = D./thres;
  I = sparse(R<1);
  NNZ = nnz(I); % number of nonzero elements

  j = floor(rows(x1)) + q + 1;
  switch q
    %%%%%%%%%%
   case 0
    error('not yet implemented for q=0')
    K(I) = (1-R(I)) .^ j;
    %%%%%%%%%%
   case 1
    error('not yet implemented for q=1')
    %%%%%%%%%%

   case 2
    a = j^2 + 4*j + 3;
    b = 3*j + 6;
    c = 3;
    z = 1/3;

    % Helper
    R2 = spalloc(m,n,NNZ);
    R2(I) = D2(I) ./ (thres^2);

    % Covariance matrix
    K = spalloc(m,n,NNZ);
    K(I) = z * (1-R(I)).^(j+2) .* (a*R2(I) + b*R(I) + c);

    dK_dR = spalloc(m,n,NNZ); % copy sparseness
    dK_dR(I) = -z * (j+2) * (1-R(I)).^(j+1) .* (a*R2(I) + b*R(I) + c) ...
        + z * (1-R(I)).^(j+2) .* (2*a*R(I) + b);

    % Gradient for hyperparameters
    if nargout >= 2
      % TODO 3D SPARSE MATRICES DO NOT WORK. USE CELL ARRAYS?
      dK_dlogtheta = spalloc(m,n,NNZ);
      dK_dlogtheta(I) = dK_dR(I) .* (-D(I) ./ thres);
      dK_dlogtheta = full(dK_dlogtheta); % blaaah.. :(
    end

    % Gradients for inputs x2
    if nargout >= 3
      if isempty(x2)
        error('Can''t calculate gradient: x2 not given');
      end
      % TODO 3D SPARSE MATRICES DO NOT WORK. USE CELL ARRAYS?
      % TODO: you have dK_dR and dD_dx but NOT dR_dD!!
      dK_dx2 = bsxfun(@times, reshape(full(dK_dR./thres),[1,m,n]), dD_dx2);
      % blaah the need for fullness.. :(
    end

    %%%%%%%%%%

   case 3
    error('not yet implemented for q=3')
    %%%%%%%%%%
   otherwise
    error('q not valid')
  end

  if ~issparse(K)
    error('K not sparse, it should, wtf?!');
  end

  if nargout >= 2
    LD = ldlchol(K);
    L = ldlsplit(LD);
  end
  end

end

x = [];  % data inputs
D = [];  % distance matrix for data
K = [];  % data covariance
LD = []; % LDL for data covariance
hyperparameters = [];

covstruct.set_data_inputs = @set_data_inputs;

  function set_data_inputs(inputs)
  % Evaluate Euclidean distances
  if ~isequal(x,inputs)
    x = inputs;
    D = sqrt(sq_dist(x));
    K = [];
    LD = [];
  end
  end

  function set_hyperparameters(params)
  if ~isequal(hyperparameters,params)
    hyperparameters = params;
    K = [];
    LD = [];
  end
  end

if nargin < 3
  x2 = [];
end
if nargin < 4
  distfun = @sqdistEuclidean;
end
if nargin < 5
  q = 2;
end

% 2-norm distances
if ~isempty(x2)
  if nargout <= 1
    D = feval(distfun, x1, x2, false);
  else
    [D, dD_dx2] = feval(distfun, x1, x2, false);
  end
  D2 = D.^2; % get this from the distfun?
else
  % Distances for variances
  D = spalloc(cols(x1),1,0); %zeros(cols(x1),1);
  D2 = D; %zeros(cols(x1),1);
end

% Dimensions
d = rows(x2); % dimensionality of inputs
m = cols(x1); % number of other inputs
n = cols(x2); % number of inputs

% Threshold
thres = exp(logtheta(1)); % length scale, threshold
R = D./thres;
I = sparse(R<1);
NNZ = nnz(I); % number of nonzero elements
%K = spones(I); % copy sparseness
% $$$ Dsp = spalloc(m,n,NNZ);
% $$$ Dsp(I) = D(I);
% $$$ D2sp = spalloc(m,n,NNZ);
% $$$ D2sp(I) = D2(I);

j = floor(rows(x1)) + q + 1;
switch q
  %%%%%%%%%%
 case 0
  error('not yet implemented for q=0')
  K(I) = (1-R(I)) .^ j;
  %%%%%%%%%%
 case 1
  error('not yet implemented for q=1')
  %%%%%%%%%%

 case 2
  a = j^2 + 4*j + 3;
  b = 3*j + 6;
  c = 3;
  z = 1/3;

  % Helper
  R2 = spalloc(m,n,NNZ);
  R2(I) = D2(I) ./ (thres^2);

  % Covariance matrix
  K = spalloc(m,n,NNZ);
  K(I) = z * (1-R(I)).^(j+2) .* (a*R2(I) + b*R(I) + c);

  dK_dR = spalloc(m,n,NNZ); % copy sparseness
  dK_dR(I) = -z * (j+2) * (1-R(I)).^(j+1) .* (a*R2(I) + b*R(I) + c) ...
      + z * (1-R(I)).^(j+2) .* (2*a*R(I) + b);

  % Gradient for hyperparameters
  if nargout >= 2
    % TODO 3D SPARSE MATRICES DO NOT WORK. USE CELL ARRAYS?
    dK_dlogtheta = spalloc(m,n,NNZ);
    dK_dlogtheta(I) = dK_dR(I) .* (-D(I) ./ thres);
    dK_dlogtheta = full(dK_dlogtheta); % blaaah.. :(
  end

  % Gradients for inputs x2
  if nargout >= 3
    if isempty(x2)
      error('Can''t calculate gradient: x2 not given');
    end
    % TODO 3D SPARSE MATRICES DO NOT WORK. USE CELL ARRAYS?
    % TODO: you have dK_dR and dD_dx but NOT dR_dD!!
    dK_dx2 = bsxfun(@times, reshape(full(dK_dR./thres),[1,m,n]), dD_dx2);
    % blaah the need for fullness.. :(
  end

  %%%%%%%%%%

 case 3
  error('not yet implemented for q=3')
  %%%%%%%%%%
 otherwise
  error('q not valid')
end

if ~issparse(K)
  error('K not sparse, it should, wtf?!');
end


function RCM = DEM_demo_Bayesian_Model_Reduction
% This demonstration code illustrates the application of post hoc model
% optimisation or Bayesian model reduction (BMR) in identifying gene and
% gene-gene interaction effects in behavioural or physiological variables.
% The basic idea is to replace conventional heuristics based on the
% assumption that the contribution of any gene is sampled from a sparse
% distribution (such that a small number contribute and a large number do
% not) with an explicit search over a model space that includes all sparse
% and non-sparse models.  This exhaustive search rests upon recent
% advances in model optimisation based upon variational Bayesian model
% inversion.  In short, it is possible to estimate the posterior
% distribution of model parameters under a reduced model, given the
% posterior and prior distributions under a full model.
% In this context, a reduced model corresponds to a model in which some
% parameters are removed (by shrinking their prior variance to zero).
% This means that it is only necessary to invert the full model and then
% perform automatic BMR (over all possible combinations of parameters)
% using a greedy search based upon the free energy approximation to log
% model evidence.  With sufficient signal to noise, this scheme can
% recover the small number of effects, even in under determined or
% ill-posed problems (where the number of potential effects can vastly
% exceed the number of samples).
%
% The illustration below uses 128 subjects who have been measured three
% times (say in three brain regions) and we want to model these
% measurements in terms of first and second order genetic contributions
% given 8 (binary) genetic variables and all (unique) pair wise
% interactions.  This means that there are 36 unknown parameters
% (excluding a constant and, say, age confounds over subjects).  In the
% scheme below, each measurement is inverted separately under a simple
% (polynomial) model with uninformative priors on the parameters and
% (precision) hyper-parameters describing beliefs about signal to noise.
% A fixed effects Bayesian model averaging (BMA) scheme is used in
% combination with BMR to identify the best model out of all possible
% combinations of first and second order effects.  With the signal to
% noise and number of samples used in this simulation, the recovery is
% generally perfect.  This scheme also illustrates inference over a
% partition of model space (or families of models).

%__________________________________________________________________________
% Copyright (C) 2012 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: DEM_demo_Bayesian_Model_Reduction.m 6306 2015-01-18 20:50:38Z karl $

rng('default')

% Genomic data (G)
%--------------------------------------------------------------------------
Ns = 256;                                   % unmber of subjects
Ng = 8;                                    % unmber of genes
Nr = 3;                                    % unmber of (fMRI) regions

% Genotype
%--------------------------------------------------------------------------
G     = rand(Ns,Ng) > 1/2;

% design matix (U) with second-order (gene-gene interaction) effects
%--------------------------------------------------------------------------
GG    = [];
for i = 1:Ns
    u     = [];
    for j = 1:Ng
        for k = (j + 1):Ng
          u(end + 1) = G(i,j)*G(i,k);
        end
    end
    GG = [GG; u];
end
U  = [G GG];
Nb = size(U,2);
R  = ones(1,Nr);
A  = zeros(Nb,1);

% first-order effects (g1 abd g4) and one interaction (g1 x g2)
%--------------------------------------------------------------------------
A([1 4 (Ng + 3)],:) = [1/2 1 1];             % effect sizes

% Simulate data with a SNR of about 4:1
%--------------------------------------------------------------------------
y  = U*A*R + randn(Ns,Nr);                 % SD(noise) = 1

% Bayesian model inversion
%==========================================================================

% Model specification
%--------------------------------------------------------------------------
M.IS   = @(P,M,U) U*P.A;
M.pE.A = zeros(Nb,1);
M.pC.A = zeros(Nb,1) + 128;
M.hE   = 0;
M.hC   = 1/128;

% confounds (at the between subject level)
%--------------------------------------------------------------------------
age   = rand(Ns,1);
Y.X0  = [age ones(Ns,1)];

for i = 1:Nr

    % model inversion for this region
    %----------------------------------------------------------------------
    Y.y       = y(:,i);
    [Ep,Cp]   = spm_nlsi_GN(M,U,Y);

    % save model for subsequent BMS
    %----------------------------------------------------------------------
    DCM{i}.M  = M;
    DCM{i}.Ep = Ep;
    DCM{i}.Cp = Cp;

end

% Bayesian model reduction: see below for family fun
%==========================================================================
RCM  = spm_dcm_post_hoc(DCM,@fun);

% show results
%--------------------------------------------------------------------------
spm_figure('Getwin','Model posterior (over families)'); clf

Qf         = zeros(1,8);
Qf(fun(A)) = 1;                           % true family
Pf         = zeros(1,8);
Pf(1:length(RCM.Pf)) = RCM.Pf;            % psoteror family probailities

subplot(2,1,1)
bar([Pf; Qf]')
xlabel('familiy')
title('Model posterior (over families)','FontSize',16)
axis square
legend({'esimated','true'})

subplot(2,1,2)
bar([RCM.Ep.A A])
title('Recovered and true effects','FontSize',16)
xlabel('parameter')
axis square

% Partition of mdoel space into famllies
%==========================================================================
function k = fun(A)

f1  = any(A(1:4));
f2  = any(A(5:8));
f3  = any(A(9:12));

if ~f1 && ~f2 && ~f3, k = 1; end
if ~f1 && ~f2 &&  f3, k = 2; end
if ~f1 &&  f2 && ~f3, k = 3; end
if ~f1 &&  f2 &&  f3, k = 4; end
if  f1 && ~f2 && ~f3, k = 5; end
if  f1 && ~f2 &&  f3, k = 6; end
if  f1 &&  f2 && ~f3, k = 7; end
if  f1 &&  f2 &&  f3, k = 8; end


function F = sign(F, pref)
%SIGN   Sign function of a CHEBFUN.
%   G = SIGN(F) returns a piecewise constant CHEBFUN G such that G(x) = 1 in the
%   interval where F(x) > 0, G(x) = -1 in the interval where F(x) < 0 and G(x) =
%   0 in the interval where F(x) = 0. Breakpoints in G are introduced at zeros
%   of F.
%
%   For the nonzero values of complex F, SIGN(F) = F./ABS(F)
%
% See also ABS, HEAVISIDE, ROOTS.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Deal with the empty case:
if ( isempty(F) )
    return
end

if ( nargin < 2 )
    pref = chebfunpref();
end

for k = 1:numel(F)
    F(k) = signColumn(F(k), pref);
end

end

function g = signColumn(f, pref)

% Add breaks at the appropriate roots of f:
g = addBreaksAtRoots(f, pref);

% Call SIGN on each of the FUNs: (result will be smooth)
for k = 1:numel(g.funs)
    g.funs{k} = sign(g.funs{k}, pref);
end

% Take the sign of the pointValues in the first row:
g.pointValues = sign(g.pointValues);

% Remove unnecessary breakpoints:
g = merge(g, pref);

end


function [x, info] = umfpack_btf (A, b, Control)
%UMFPACK_BTF factorize A using a block triangular form
%
% Example:
%   x = umfpack_btf (A, b, Control)
%
% solve Ax=b by first permuting the matrix A to block triangular form via dmperm
% and then using UMFPACK to factorize each diagonal block.  Adjacent 1-by-1
% blocks are merged into a single upper triangular block, and solved via
% MATLAB's \ operator.  The Control parameter is optional (Type umfpack_details
% and umfpack_report for details on its use).  A must be square.
%
% See also umfpack, umfpack2, umfpack_details, dmperm

% Copyright 1995-2007 by Timothy A. Davis.

if (nargin < 2)
    help umfpack_btf
    error ('Usage: x = umfpack_btf (A, b, Control)') ;
end

[m n] = size (A) ;
if (m ~= n)
    help umfpack_btf
    error ('umfpack_btf:  A must be square') ;
end
m1 = size (b,1) ;
if (m1 ~= n)
    help umfpack_btf
    error ('umfpack_btf:  b has the wrong dimensions') ;
end

if (nargin < 3)
    Control = umfpack2 ;
end

%-------------------------------------------------------------------------------
% find the block triangular form
%-------------------------------------------------------------------------------

% dmperm built-in may segfault in MATLAB 7.4 or earlier; fixed in MATLAB 7.5
% since dmperm now uses CSparse
[p,q,r] = dmperm (A) ;
nblocks = length (r) - 1 ;

info = [0 0 0] ;    % [nnz(L), nnz(U), nnz(F)], optional 2nd output

%-------------------------------------------------------------------------------
% solve the system
%-------------------------------------------------------------------------------

if (nblocks == 1 | sprank (A) < n)					    %#ok

    %---------------------------------------------------------------------------
    % matrix is irreducible or structurally singular
    %---------------------------------------------------------------------------

    [x info2] = umfpack2 (A, '\', b, Control) ;
    info = [info2(78) info2(79) 0] ;

else

    %---------------------------------------------------------------------------
    % A (p,q) is in block triangular form
    %---------------------------------------------------------------------------

    b = b (p,:) ;
    A = A (p,q) ;
    x = zeros (size (b)) ;

    %---------------------------------------------------------------------------
    % merge adjacent singletons into a single upper triangular block
    %---------------------------------------------------------------------------

    [r, nblocks, is_triangular] = merge_singletons (r) ;

    %---------------------------------------------------------------------------
    % solve the system: x (q) = A\b
    %---------------------------------------------------------------------------

    for k = nblocks:-1:1

	% get the kth block
        k1 = r (k) ;
        k2 = r (k+1) - 1 ;

	% solve the system
        [x2 info2] = solver (A (k1:k2, k1:k2), b (k1:k2,:), ...
	    is_triangular (k), Control) ;
	x (k1:k2,:) = x2 ;

        % off-diagonal block back substitution
        F2 = A (1:k1-1, k1:k2) ;
        b (1:k1-1,:) = b (1:k1-1,:) - F2 * x (k1:k2,:) ;

        info (1:2) = info (1:2) + info2 (1:2) ;
        info (3) = info (3) + nnz (F2) ;

    end

    x (q,:) = x ;

end

%-------------------------------------------------------------------------------
% merge_singletons
%-------------------------------------------------------------------------------

function [r, nblocks, is_triangular] = merge_singletons (r)
%
% Given r from [p,q,r] = dmperm (A), where A is square, return a modified r that
% reflects the merger of adjacent singletons into a single upper triangular
% block.  is_triangular (k) is 1 if the kth block is upper triangular.  nblocks
% is the number of new blocks.

nblocks = length (r) - 1 ;
bsize = r (2:nblocks+1) - r (1:nblocks) ;
t = [0 (bsize == 1)] ;
z = (t (1:nblocks) == 0 & t (2:nblocks+1) == 1) | t (2:nblocks+1) == 0 ;
y = [(find (z)) nblocks+1] ;
r = r (y) ;
nblocks = length (y) - 1 ;
is_triangular = y (2:nblocks+1) - y (1:nblocks) > 1 ;

%-------------------------------------------------------------------------------
% solve Ax=b, but check for small and/or triangular systems
%-------------------------------------------------------------------------------

function [x, info] = solver (A, b, is_triangular, Control)
if (is_triangular)
    % back substitution only
    x = A \ b ;
    info = [nnz(A) 0 0] ;
elseif (size (A,1) < 4)
    % a very small matrix, solve it as a dense linear system
    x = full (A) \ b ;
    n = size (A,1) ;
    info = [(n^2+n)/2 (n^2+n)/2 0] ;
else
    % solve it as a sparse linear system
    [x info] = umfpack_solve (A, '\', b, Control) ;
end


function [haus, meanAbsSurfDist] = calc_HausdorffMetric(structNum1,structNum2,planC)
%function [haus, meanAbsSurfDist] = calc_HausdorffMetric(structNum1,structNum2,planC)
%
%This function computes Hausdorff distance between structNum1 and structNum2
%
%APA, 11/20/2014

if ~exist('planC','var')
    global planC
end

[~, x1V, y1V, z1V, planC] = getStructSurface(structNum1,planC);
[~, x2V, y2V, z2V, planC] = getStructSurface(structNum2,planC);

haus = NaN;
meanAbsSurfDist = NaN;
if ~isempty(x1V) && ~isempty(x1V)
    [haus,meanAbsSurfDist] = hausdorff([x1V(:) y1V(:) z1V(:)],[x2V(:) y2V(:) z2V(:)]);
end


function [L,Lc,Lli] = toFramePlucker(C,Li)

% TOFRAMEPLUCKER  Transform plucker line to a given frame.
%   L = TOFRAMEPLUCKER(C,Li) expresses in frame C=[t;q] the line Li
%   originally expressed in global frame.
%
%   The formula for transformation is
%
%       L = [ Rt       [it]_x*Rt ]
%           [ zeros(3)        Rt ] * Li
%
%   where
%       Rt    = q2R(iq)
%      [it]_x = hat(it)
%       iq    = q2qc(q)
%       it    = t2it(t,q)
%       t     = C(1:3)
%       q     = C(4:7)
%
%   [L,Lc,Lli] = TOFRAMEPLUCKER(...) returns the Jacobians wrt C
%   and Li.
%
%   See also FROMFRAMEPLUCKER, TXP, HAT, CROSS, Q2R, Q2QC, T2IT.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

% extract input vectors ai and bi
ai = Li(1:3);
bi = Li(4:6);

t = C.t;
q = C.q;

if nargout == 1

    % get it and Rt of inverse transform
    it = t2it(t,q);
    Rt = C.Rt;

    % compute a and b
    b  = Rt*bi;
    a  = Rt*ai + hat(it)*b;

    % Build output
    L = [a;b];

else

    % get it and iq of inverse transform
    [it,ITt,ITq]     = t2it(t,q);

    % compute a and b
    [b,Bq,Bbi]       = Rtp(q,bi);
    [txb,TXBit,TXBb] = crossJ(it,b);
    [Ra,RAq,RAai]    = Rtp(q,ai);

    a = Ra + txb;

    % Jacobians from the chain rule
    At  = TXBit*ITt;
    Aq  = RAq + TXBit*ITq + TXBb*Bq;
    Aai = RAai;
    Abi = TXBb*Bbi;

    % Build outputs
    L   = [a;b];

    Lc  = [At Aq;zeros(3) Bq];
    Lli = [Aai Abi;zeros(3) Bbi];

end

return

%%

syms a b c d x y z real
syms L1 L2 L3 L4 L5 L6 real

q = [a;b;c;d];
t = [x;y;z];
C = [t;q];
Li = [L1;L2;L3;L4;L5;L6];

[L,Lc,Lli] = toFramePlucker(C,Li);

simplify(Lc  - jacobian(L,C))
simplify(Lli - jacobian(L,Li))

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function [SE_MR,SE_RZF,SE_MMMSE,SE_ZF,SE_SMMSE] = functionComputeSE_UL(Hhat,C,R,tau_c,tau_p,nbrOfRealizations,M,K,L,p)
%Compute UL SE for different receive combining schemes using Theorem 4.1.
%
%INPUT:
%Hhat              = M x nbrOfRealizations x K x L x L matrix with the MMSE
%                    channel estimates
%C                 = M x M x K x L x L matrix with estimation error
%                    correlation matrices when using MMSE estimation
%R                 = M x M x K x L x L matrix with spatial correlation
%                    matrices
%tau_c             = Length of coherence block
%tau_p             = Length of pilot sequences
%nbrOfRealizations = Number of channel realizations
%M                 = Number of antennas per BS
%K                 = Number of UEs per cell
%L                 = Number of BSs and cells
%p                 = Uplink transmit power per UE (same for everyone)
%
%OUTPUT:
%SE_MR    = K x L matrix where element (k,l) is the uplink SE of UE k in
%           cell l achieved with MR combining
%SE_RZF   = Same as SE_MR but with RZF combining
%SE_MMMSE = Same as SE_MR but with M-MMSE combining
%SE_ZF    = Same as SE_MR but with ZF combining
%SE_SMMSE = Same as SE_MR but with S-MMSE combining
%
%
%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.01 (Last edited: 2020-05-15)
%
%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.

%Store identity matrices of different sizes
eyeK = eye(K);
eyeM = eye(M);

%Compute the pre-log factor (normalized with number of channel realizations)
%assuming only uplink transmission
prelogFactor = (tau_c-tau_p)/(tau_c*nbrOfRealizations);

%Prepare to store simulation results
SE_MR = zeros(K,L);

if nargout > 1
    SE_RZF = zeros(K,L);
end

if nargout > 2
    SE_MMMSE = zeros(K,L);

    %Compute sum of all estimation error correlation matrices at every BS
    C_totM = reshape(p*sum(sum(C,3),4),[M M L]);

end

if nargout > 3
    SE_ZF = zeros(K,L);
end

if nargout > 4
    SE_SMMSE = zeros(K,L);

    %Compute sum of intra-cell estimation error correlation matrices at every BS
    CR_totS = zeros(M,M,L);

    for j = 1:L
        CR_totS(:,:,j) = p*(sum(C(:,:,:,j,j),3)+sum(sum(R(:,:,:,[1:j-1 j+1:end],j),3),4));
    end
end

%% Go through all channel realizations
for n = 1:nbrOfRealizations

    %Go through all cells
    for j = 1:L

        %Extract channel estimate realizations from all UEs to BS j
        Hhatallj = reshape(Hhat(:,n,:,:,j),[M K*L]);

        %Compute MR combining in (4.11)
        V_MR = Hhatallj(:,K*(j-1)+1:K*j);

        if nargout > 1 %Compute RZF combining in (4.9)
            V_RZF = (p*V_MR)/(p*(V_MR'*V_MR)+eyeK);
        end

        if nargout > 2 %Compute M-MMSE combining in (4.7)
            V_MMMSE = (p*(Hhatallj*Hhatallj')+C_totM(:,:,j)+eyeM)\(p*V_MR);
        end

        if nargout > 3 %Compute ZF combining in (4.10), with the small regularization term 1e-12 for numerical stability
            V_ZF = V_MR/(V_MR'*V_MR+1e-12*eyeK);
        end

        if nargout > 4 %Compute S-MMSE combining in (4.8)
            V_SMMSE = (p*(V_MR*V_MR')+CR_totS(:,:,j)+eyeM)\(p*V_MR);
        end

        %Go through all UEs in cell j
        for k = 1:K

            %%MR combining
            v = V_MR(:,k); %Extract combining vector

            %Compute numerator and denominator of instantaneous SINR in (4.3)
            numerator = p*abs(v'*Hhat(:,n,k,j,j))^2;
            denominator = p*sum(abs(v'*Hhatallj).^2) + v'*(C_totM(:,:,j)+eyeM)*v - numerator;

            %Compute instantaneous SE for one channel realization
            SE_MR(k,j) = SE_MR(k,j) + prelogFactor*real(log2(1+numerator/denominator));

            %%ZF combining
            if nargout > 3

                v = V_ZF(:,k); %Extract combining vector

                %Compute numerator and denominator of instantaneous SINR in (4.3)
                numerator = p*abs(v'*Hhat(:,n,k,j,j))^2;
                denominator = p*sum(abs(v'*Hhatallj).^2) + v'*(C_totM(:,:,j)+eyeM)*v - numerator;

                %Compute instantaneous SE for one channel realization
                SE_ZF(k,j) = SE_ZF(k,j) + prelogFactor*real(log2(1+numerator/denominator));

            end

            %%RZF combining
            if nargout > 1

                v = V_RZF(:,k); %Extract combining vector

                %Compute numerator and denominator of instantaneous SINR in (4.3)
                numerator = p*abs(v'*Hhat(:,n,k,j,j))^2;
                denominator = p*sum(abs(v'*Hhatallj).^2) + v'*(C_totM(:,:,j)+eyeM)*v - numerator;

                %Compute instantaneous SE for one channel realization
                SE_RZF(k,j) = SE_RZF(k,j) + prelogFactor*real(log2(1+numerator/denominator));

            end

            %%S-MMSE combining
            if nargout > 4

                v = V_SMMSE(:,k); %Extract combining vector

                %Compute numerator and denominator of instantaneous SINR in (4.3)
                numerator = p*abs(v'*Hhat(:,n,k,j,j))^2;
                denominator = p*sum(abs(v'*Hhatallj).^2) + v'*(C_totM(:,:,j)+eyeM)*v - numerator;

                %Compute instantaneous SE for one channel realization
                SE_SMMSE(k,j) = SE_SMMSE(k,j) + prelogFactor*real(log2(1+numerator/denominator));

            end

            %%M-MMSE combining
            if nargout > 2

                v = V_MMMSE(:,k); %Extract combining vector

                %Compute numerator and denominator of instantaneous SINR in (4.3)
                numerator = p*abs(v'*Hhat(:,n,k,j,j))^2;
                denominator = p*sum(abs(v'*Hhatallj).^2) + v'*(C_totM(:,:,j)+eyeM)*v - numerator;

                %Compute instantaneous SE for one channel realization
                SE_MMMSE(k,j) = SE_MMMSE(k,j) + prelogFactor*real(log2(1+numerator/denominator));

            end

        end

    end

end


% OP_GRADGRADU_GRADGRADV: assemble the matrix A = [a(i,j)], a(i,j) = (epsilon grad grad u_j, grad grad v_i).
%
%   mat = op_gradgradu_gradgradv (spu, spv, msh, epsilon);
%   [rows, cols, values] = op_gradgradu_gradgradv (spu, spv, msh, epsilon);
%
% INPUT:
%
%   spu:   structure representing the space of trial functions (see sp_scalar/sp_evaluate_col)
%   spv:   structure representing the space of test functions (see sp_scalar/sp_evaluate_col)
%   msh:   structure containing the domain partition and the quadrature rule (see msh_cartesian/msh_evaluate_col)
%   epsilon: diffusion coefficient
%
% OUTPUT:
%
%   mat:    assembled matrix
%   rows:   row indices of the nonzero entries
%   cols:   column indices of the nonzero entries
%   values: values of the nonzero entries
%
% Copyright (C) 2009, 2010 Carlo de Falco
% Copyright (C) 2011, 2017 Rafael Vazquez
% Copyright (C) 2013, Marco Pingaro
%
%    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 varargout = op_gradgradu_gradgradv (spu, spv, msh, coeff)

  der2u = reshape (spu.shape_function_hessians, spu.ncomp, [], msh.nqn, spu.nsh_max, msh.nel);
  der2v = reshape (spv.shape_function_hessians, spv.ncomp, [], msh.nqn, spv.nsh_max, msh.nel);

  ndir = size (der2u, 2);

  rows = zeros (msh.nel * spu.nsh_max * spv.nsh_max, 1);
  cols = zeros (msh.nel * spu.nsh_max * spv.nsh_max, 1);
  values = zeros (msh.nel * spu.nsh_max * spv.nsh_max, 1);

  jacdet_weights = msh.jacdet .* msh.quad_weights .* coeff;

  ncounter = 0;
  for iel = 1:msh.nel
    if (all (msh.jacdet(:, iel)))
      der2u_iel = reshape (der2u(:,:,:,:,iel), spu.ncomp*ndir, msh.nqn, 1, spu.nsh_max);
      der2v_iel = reshape (der2v(:,:,:,:,iel), spv.ncomp*ndir, msh.nqn, spv.nsh_max, 1);

      jacdet_iel = reshape (jacdet_weights(:,iel), [1,msh.nqn,1,1]);

      jacdet_der2u = bsxfun (@times, jacdet_iel, der2u_iel);
      tmp1 = sum (bsxfun (@times, jacdet_der2u, der2v_iel), 1);
      elementary_values = reshape (sum (tmp1, 2), spv.nsh_max, spu.nsh_max);

      [rows_loc, cols_loc] = ndgrid (spv.connectivity(:,iel), spu.connectivity(:,iel));
      indices = rows_loc & cols_loc;
      rows(ncounter+(1:spu.nsh(iel)*spv.nsh(iel))) = rows_loc(indices);
      cols(ncounter+(1:spu.nsh(iel)*spv.nsh(iel))) = cols_loc(indices);
      values(ncounter+(1:spu.nsh(iel)*spv.nsh(iel))) = elementary_values(indices);
      ncounter = ncounter + spu.nsh(iel)*spv.nsh(iel);

    else
      warning ('geopdes:jacdet_zero_at_quad_node', 'op_gradgradu_gradgradv: singular map in element number %d', iel)
    end
  end

  if (nargout == 1 || nargout == 0)
    varargout{1} = sparse (rows(1:ncounter), cols(1:ncounter), ...
                           values(1:ncounter), spv.ndof, spu.ndof);
  elseif (nargout == 3)
    varargout{1} = rows(1:ncounter);
    varargout{2} = cols(1:ncounter);
    varargout{3} = values(1:ncounter);
  else
    error ('op_gradgradu_gradgradv: wrong number of output arguments')
  end

end

%% COPY OF THE FIRST VERSION OF THE FUNCTION (MORE UNDERSTANDABLE)
% function mat = op_gradgradu_gradgradv (spu, spv, msh, coeff)
%   mat = spalloc (spv.ndof, spu.ndof, 1);
%
%   der2u = reshape (spu.shape_function_hessians, spu.ncomp, [], msh.nqn, spu.nsh_max, msh.nel);
%   der2v = reshape (spv.shape_function_hessians, spv.ncomp, [], msh.nqn, spv.nsh_max, msh.nel);
%
%   ndir = size (der2u, 2);
%
%   for iel = 1:msh.nel
%     if (all (msh.jacdet(:,iel)))
%       mat_loc = zeros (spv.nsh(iel), spu.nsh(iel));
%       for idof = 1:spv.nsh(iel)
%           ishh = reshape(der2v(:,:,:,idof,iel), spv.ncomp * ndir, []);
%           for jdof = 1:spu.nsh(iel)
%               jshh = reshape(der2u(:,:,:,jdof,iel), spu.ncomp * ndir, []);
%           % The cycle on the quadrature points is vectorized
%           %for inode = 1:msh.nqn
%               mat_loc(idof, jdof) = mat_loc(idof, jdof) + ...
%                   sum (msh.jacdet(:,iel) .* msh.quad_weights(:, iel) .* ...
%                   sum (ishh .* jshh, 1).' .* coeff(:,iel));
%           %end
%           end
%       end
%       mat(spv.connectivity(:, iel), spu.connectivity(:, iel)) = ...
%           mat(spv.connectivity(:, iel), spu.connectivity(:, iel)) + mat_loc;
%     else
%       warning ('geopdes:jacdet_zero_at_quad_node',...
%           'op_gradgradu_gradgradv: singular map in element number %d', iel)
%     end
%   end
% end


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

% get calibrated data
load 'MODEL_Hist';

% Discretization parameters
NTime = 12;                    % number of time steps
NSim = 10000;                    % number of simulations
NBatches = 1;                  % number of batches

NHist = 251;                     % number of historic scenarios

NModels = 10;                  % number of models

% Options
%K = 6800;                                % Strike
%C = 1;                                  % call (1) or put (0)
LF = 0; LC = 0.04; GF = 0; GC = 0.15;   % cliquet params
L = 80; U = 120;
T= 1;                                   % maturity of option

%option1 = 'Arithmetic Asian Call';
%option2 = 'Arithmetic Asian Put';
%exotic_price_c = @(x,y,r) Price_ArithmeticAsian(x,y,1,r,T);
%exotic_price_p = @(x,y,r) Price_ArithmeticAsian(x,y,0,r,T);

% exotic_price = @(x,r) Price_ArithmeticCliquet(x,LF,LC,GF,GC,r,T);
option1 = 'FixedStrike Lookback Call';
option2 = 'FixedStrike Lookback Put';
exotic_price_c = @(x,y,r) Price_LookBackFixedStrike(x,y,1,r,T);
exotic_price_p = @(x,y,r) Price_LookBackFixedStrike(x,y,0,r,T);

%option1 = 'FloatingStrike Lookback Call';
%option2 = 'FloatingStrike Lookback Put';
%exotic_price_c = @(x,y,r) Price_LookBackFloatingStrike(x,y,1,r,T);
%exotic_price_p = @(x,y,r) Price_LookBackFloatingStrike(x,y,0,r,T);

%exotic_price = @(x,r) Price_KnockOut(x,x,L,U,C,r,T);
vanilla_price_c = @(x,y,r) Price_CallPut(x,y,1,r,T);
vanilla_price_p = @(x,y,r) Price_CallPut(x,y,0,r,T);

% output
ep1c = zeros(NHist,NModels); %ep2c = ep1c; ep3c = ep1c;
ep1p = ep1c; %ep2p = ep1c; ep3p = ep1c;

vp_c = ep1c; vp_p = ep1c;
qp_c = ep1c; qp_p = ep1c;

vc = zeros(NHist,1); vp = vc;

% Black Scholes
for l = 1: NHist
    pathS = MC_B(Spot(l),r1y(l),0,T,BS_Par_Hist(l),NTime,NSim,NBatches);
    ep1c(l,1) = exotic_price_c(pathS,Spot(l),r1y(l));
    ep1p(l,1) = exotic_price_p(pathS,Spot(l),r1y(l));
    vp_c(l,1) = vanilla_price_c(pathS,Spot(l),r1y(l));
    vp_p(l,1) = vanilla_price_p(pathS,Spot(l),r1y(l));
    [vc(l), vp(l)] = blsprice(Spot(l), Spot(l), r1y(l), T, BS_Par_Hist(l),0);
    qp_c(l,1) = ep1c(l,1)/vp_c(l,1);
    qp_p(l,1) = ep1p(l,1)/vp_p(l,1);
end

% Merton
for l = 1: NHist
    pathS = MC_M(Spot(l),r1y(l),0,T,...
        MJ_Par_Hist(l,1),MJ_Par_Hist(l,2),MJ_Par_Hist(l,3),MJ_Par_Hist(l,4),...
        NTime,NSim,NBatches);
    ep1c(l,2) = exotic_price_c(pathS,Spot(l),r1y(l));
    ep1p(l,2) = exotic_price_p(pathS,Spot(l),r1y(l));
    vp_c(l,2) = vanilla_price_c(pathS,Spot(l),r1y(l));
    vp_p(l,2) = vanilla_price_p(pathS,Spot(l),r1y(l));
    qp_c(l,2) = ep1c(l,2)/vp_c(l,2);
    qp_p(l,2) = ep1p(l,2)/vp_p(l,2);
end

% Heston
for l = 1: NHist
    [pathS, pathV] = MC_QE(Spot(l),r1y(l),0,T,...
        HESTON_Par_Hist(l,1),HESTON_Par_Hist(l,2),HESTON_Par_Hist(l,3),...
        HESTON_Par_Hist(l,4),HESTON_Par_Hist(l,5),...
        NTime,NSim,NBatches);
    ep1c(l,3) = exotic_price_c(pathS,Spot(l),r1y(l));
    ep1p(l,3) = exotic_price_p(pathS,Spot(l),r1y(l));
    vp_c(l,3) = vanilla_price_c(pathS,Spot(l),r1y(l));
    vp_p(l,3) = vanilla_price_p(pathS,Spot(l),r1y(l));
    qp_c(l,3) = ep1c(l,3)/vp_c(l,3);
    qp_p(l,3) = ep1p(l,3)/vp_p(l,3);
end

% Bates
for l = 1: NHist
    [pathS, pathV] = MC_QE_j(Spot(l),r1y(l),0,T,...
        BATES_Par_Hist(l,1),BATES_Par_Hist(l,2),BATES_Par_Hist(l,4),BATES_Par_Hist(l,5),...
        BATES_Par_Hist(l,3),BATES_Par_Hist(l,7),BATES_Par_Hist(l,7),BATES_Par_Hist(l,8),...
        NTime,NSim,NBatches);
    ep1c(l,4) = exotic_price_c(pathS,Spot(l),r1y(l));
    ep1p(l,4) = exotic_price_p(pathS,Spot(l),r1y(l));
    vp_c(l,4) = vanilla_price_c(pathS,Spot(l),r1y(l));
    vp_p(l,4) = vanilla_price_p(pathS,Spot(l),r1y(l));
    qp_c(l,4) = ep1c(l,4)/vp_c(l,4);
    qp_p(l,4) = ep1p(l,4)/vp_p(l,4);
end

% % Variance Gamma CGM
% for l = 1:NHist
%     pathS  = MC_VG_CGM(Spot(l),r1y(l),0,T,...
%         VG_Par_Hist(l,1),VG_Par_Hist(l,2),VG_Par_Hist(l,3),...
%         NTime,NSim,NBatches);
%     ep1c(l,5) = exotic_price_c(pathS,Spot(l),r1y(l));
%     ep1p(l,5) = exotic_price_p(pathS,Spot(l),r1y(l));
%     vp_c(l,5) = vanilla_price_c(pathS,Spot(l),r1y(l));
%     vp_p(l,5) = vanilla_price_p(pathS,Spot(l),r1y(l));
%     qp_c(l,5) = ep1c(l,5)/vp_c(l,1);
%     qp_p(l,5) = ep1p(l,5)/vp_p(l,1);
% end
%
% % Normal Inverse Gaussian
% for l = 1:NHist
%     pathS  = MC_NIG(Spot(l),r1y(l),0,T,...
%         NIG_Par_Hist(l,1),NIG_Par_Hist(l,2),NIG_Par_Hist(l,3),...
%         NTime,NSim,NBatches);
%     ep1c(l,6) = exotic_price_c(pathS,Spot(l),r1y(l));
%     ep1p(l,6) = exotic_price_p(pathS,Spot(l),r1y(l));
%     vp_c(l,6) = vanilla_price_c(pathS,Spot(l),r1y(l));
%     vp_p(l,6) = vanilla_price_p(pathS,Spot(l),r1y(l));
%     qp_c(l,6) = ep1c(l,1)/vp_c(l,1);
%     qp_p(l,6) = ep1p(l,6)/vp_p(l,1);
% end
%
% % Variance Gamma - GOU
% for l = 1:NHist
%     pathS  = MC_VGGOU(Spot(l),r1y(l),0,T,...
%         VGGOU_Par_Hist(l,1), VGGOU_Par_Hist(l,2), VGGOU_Par_Hist(l,3),...
%         VGGOU_Par_Hist(l,6), VGGOU_Par_Hist(l,4), VGGOU_Par_Hist(l,5),...
%         NTime,NSim,NBatches);
%     ep1c(l,7) = exotic_price_c(pathS,Spot(l),r1y(l));
%     ep1p(l,7) = exotic_price_p(pathS,Spot(l),r1y(l));
%     vp_c(l,1) = vanilla_price_c(pathS,Spot(l),r1y(l));
%     vp_p(l,7) = vanilla_price_p(pathS,Spot(l),r1y(l));
%     qp_c(l,7) = ep1c(l,7)/vp_c(l,1);
%     qp_p(l,7) = ep1p(l,7)/vp_p(l,1);
% end
%
% % % Variance Gamma - CIR
% for l = 1: NHist
%     pathS =  MC_VGCIR(Spot(l),r1y(l),0,T,...
%         VGCIR_Par_Hist(l,1),VGCIR_Par_Hist(l,2),VGCIR_Par_Hist(l,3), ...
%         VGCIR_Par_Hist(l,4),VGCIR_Par_Hist(l,5),VGCIR_Par_Hist(l,6),...
%         NTime,NSim,NBatches);
%     ep1c(l,8) = exotic_price_c(pathS,Spot(l),r1y(l));
%     ep1p(l,8) = exotic_price_p(pathS,Spot(l),r1y(l));
%     vp_c(l,8) = vanilla_price_c(pathS,Spot(l),r1y(l));
%     vp_p(l,8) = vanilla_price_p(pathS,Spot(l),r1y(l));
%     qp_c(l,8) = ep1c(l,8)/vp_c(l,1);
%     qp_p(l,8) = ep1p(l,8)/vp_p(l,1);
% end
%
% % NIG - GOU
% for l = 1: NHist
%     pathS =  MC_NIGGOU(Spot(l),r1y(l),0,T,...
%         NIGGOU_Par_Hist(l,1),NIGGOU_Par_Hist(l,2),NIGGOU_Par_Hist(l,3), ...
%         NIGGOU_Par_Hist(l,4),NIGGOU_Par_Hist(l,5),NIGGOU_Par_Hist(l,6),NTime,NSim,NBatches);
%     ep1c(l,9) = exotic_price_c(pathS,Spot(l),r1y(l));
%     ep1p(l,9) = exotic_price_p(pathS,Spot(l),r1y(l));
%     vp_c(l,9) = vanilla_price_c(pathS,Spot(l),r1y(l));
%     vp_p(l,9) = vanilla_price_p(pathS,Spot(l),r1y(l));
%     qp_c(l,9) = ep1c(l,9)/vp_c(l,1);
%     qp_p(l,9) = ep1p(l,9)/vp_p(l,1);
% end
%
% % NIG - CIR
% for l = 1: NHist
%     pathS =  MC_NIGCIR(Spot(l),r1y(l),0,T,...
%         NIGCIR_Par_Hist(l,1),NIGCIR_Par_Hist(l,2),NIGCIR_Par_Hist(l,3), ...
%         NIGCIR_Par_Hist(l,5),NIGCIR_Par_Hist(l,6),NIGCIR_Par_Hist(l,4),NTime,NSim,NBatches);
%     ep1c(l,10) = exotic_price_c(pathS,Spot(l),r1y(l));
%     ep1p(l,10) = exotic_price_p(pathS,Spot(l),r1y(l));
%     vp_c(l,10) = vanilla_price_c(pathS,Spot(l),r1y(l));
%     vp_p(l,10) = vanilla_price_p(pathS,Spot(l),r1y(l));
%     qp_c(l,10) = ep1c(l,10)/vp_c(l,1);
%     qp_p(l,10) = ep1p(l,10)/vp_p(l,1);
% end
m1 = 'BS'; m2 = 'MJ'; m3 = 'Heston'; m4 = 'Bates';

plotit3(ep1c,option1,m1, m2, m3, m4);
plotit3(ep1p,option2,m1, m2, m3, m4);
plotit3(qp_c,strcat('Normalized ',option1),m1, m2, m3, m4);
plotit3(qp_p,strcat('Normalized ',option2),m1, m2, m3, m4);

modelriskc = repmat(max(ep1c,[],2),1,NModels)-ep1c;
modelriskp = -repmat(max(ep1p,[],2),1,NModels)+ep1p;


function A = y2a(y);

% ABCD = y2a(Y)
%
% Admittance to ABCD transformation
% only for 2x2xN matrices

% v1.1 - 03.02.2005 freq added

d = y(1,1,:).*y(2,2,:) - y(1,2,:).*y(2,1,:);

for i=1:size(y,3)
    while abs(y(2,1,i)) < 1e-8
        y(2,1,i) = y(2,1,i)*(1+rand*1e-8);
    end;
end;

A(1,1,:) = -y(2,2,:)./y(2,1,:);
A(1,2,:) = -1./y(2,1,:);
A(2,1,:) = -d./y(2,1,:);
A(2,2,:) = -y(1,1,:)./y(2,1,:);


function MedianCutAux(xMin, xMax, yMin, yMax, iter)
%
%
%        MedianCutAux(xMin, xMax, yMin, yMax, iter)
%
%
%     Copyright (C) 2011-14  Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

global L;
global imgWork;
global lights;

lx = xMax - xMin;
ly = yMax - yMin;

if((lx > 2) && (ly > 2) && (iter > 0))
    tot = sum(sum(L(yMin:yMax, xMin:xMax)));
    pivot = -1;

    if(lx > ly)
        %cut on the X-axis
        for i=xMin:(xMax - 1)
            c = sum(sum(L(yMin:yMax, xMin:i)));
            if(c >= (tot - c))
                pivot = i;
                break;
            end
        end

        if(pivot == -1)
            pivot = xMax-1;
        end

        MedianCutAux(xMin,    pivot, yMin, yMax, iter-1);
        MedianCutAux(pivot+1, xMax,  yMin, yMax, iter-1);
    else
        %cut on the Y-axis
        for i=yMin:(yMax - 1)
            c = sum(sum(L(yMin:i, xMin:xMax)));
            if(c >= (tot - c))
                pivot = i;
                break;
            end
        end

        if(pivot == -1)
            pivot = yMax-1;
        end

        MedianCutAux(xMin, xMax, yMin,    pivot, iter-1);
        MedianCutAux(xMin, xMax, pivot+1, yMax,  iter-1);
    end
else
    %Generation of the light source
    lights = [lights, CreateLight(xMin, xMax, yMin, yMax, L, imgWork)];
end

end


function [fAdj,numAdj]=sortTriangFacesAroundVertices(vertInFaces,fAdj,numAdj)
%%SORTTRIANGFACESAROUNDVERTICES Consider a closed polyhedron that is made
%       up of triangles whose vertices are sorted in counterclockwise order
%       when looking from the outside the polyhedron at the triangle, given
%       a list of which vertices are in which triangle, obtain a list of
%       which faces (triangles) are adjacent to each vertex with the faces
%       specified in counterclockwise order.
%
%INPUTS: vertInFaces A 3XnumFaces set of indices of vertices in all of the
%                    triangles in the polyhedron (all faces are triangles).
%                    The vertex locations do not need to be provided; just
%                    the indices of the triangles.
%       fAdj, numAdj If available, fAdj is a maxAdjFacesXnumVert matrix,
%                    where for the kth vertex, fAdj(1:numAdj(k),k) is the
%                    set of indices of the faces that are adjacent to that
%                    vertex. It effectively selects a column of
%                    vertInFaces. numAdj is a length numVertices vector. If
%                    these two inputs are omitted, they are reconstructed
%                    from vertInFaces.
%
%OUTPUTS: fAdj,numAdj These have the same definition as fAdj and numAdj on
%                     the input but the faces are sorted in
%                     counterclockwise order around each vertex.
%
%Having the ordering of faces around vertices recorded in a
%counterclockwise order is important in algorithms such as in [1] for
%finding 3D barycentric coordinates.
%
%In a completely closed polyhedron, one can start at the vertex of interest
%and then follow the triangles around in the correct order. Assume that all
%of the trianges are given with indices in counterclockwise order. One can
%rotates the indices of each triangles adjacent to the vertex such that the
%vertex of interest is in the first index. One can then trace the triangles
%around in the correct order by following the indices. The second vertex of
%the first triangle will also be the last index traced. To trace around,
%consider the example below:
%     4-------3
%    / \     / \
%   /   \   /   \
%  /     \ /     \
% 5-------1-------2
%  \      |      /
%   \     |     /
%    \    |    /
%     \   |   /
%      \  |  /
%       \ | /
%        \|/
%         6
%
%Starting with face 1-2-3, the second triangle must share vertex 3. Thus,
%one must search for a triangle whose second index is 3. That leads to
%1-3-4. Similarly, the third index of that triangle will be 4 and the
%second index of the next triangle must be 4, so tht leads to triangle
%1-4-5. Thus, one can continue tracing triangles aorund the vertex until
%the common vertex is number 2, in which case one knows that the final
%triangle has been found.
%
%REFERENCES:
%[1] M. S. Floater, "Generalized barycentric coordinates and applications,"
%    Acta Numerica, vol. 24, pp. 161-214, 1 May 2015.
%
%September 2022 David F.Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

numVert=max(vertInFaces(:));

if(nargin<2||isempty(fAdj))
    %Determine the maximum number of adjacent vertices.
    numAdj=zeros(numVert,1);
    for k=1:numVert
        idx=find(any(vertInFaces==k,1));
        numAdj(k)=length(idx);
    end

    %Store the adjacent vertices, but not in any particular order.
    maxAdj=max(numAdj);
    fAdj=zeros(maxAdj,numVert);
    for k=1:numVert
        idx=find(any(vertInFaces==k,1));
        fAdj(1:numAdj(k),k)=idx;
    end
end

for k=1:numVert
    numFaces=numAdj(k);
    %Rotate the vertices in each triangle until vertex k is first.
    for curFace=1:numFaces
        fIdx=fAdj(curFace,k);

        if(vertInFaces(2,fIdx)==k)
            vertInFaces(:,fIdx)=vertInFaces([2;3;1],fIdx);
        elseif(vertInFaces(3,fIdx)==k)
            vertInFaces(:,fIdx)=vertInFaces([3;1;2],fIdx);
        end%Otherwise, assume vertInFaces(1,fIdx)==k and no rotation is
           %needed.
    end
    newOrder=zeros(numFaces,1);
    newOrder(1)=1;%The first face remains unchanged.
    assignedFaces=false(numFaces,1);
    assignedFaces(1)=true;

    curAddedIdx=1;
    fIdx=fAdj(1,k);
    endNextIdx=vertInFaces(2,fIdx);
    curNextIdx=vertInFaces(3,fIdx);
    while(curNextIdx~=endNextIdx)
        for curFace=2:numFaces
            if(assignedFaces(curFace))
                continue;
            end
            fIdx=fAdj(curFace,k);

            if(vertInFaces(2,fIdx)==curNextIdx)
                %Found the next face.
                break;
            end
        end

        curAddedIdx=curAddedIdx+1;
        newOrder(curAddedIdx)=curFace;
        assignedFaces(curFace)=true;
        curNextIdx=vertInFaces(3,fIdx);
    end
    fAdj(1:numFaces,k)=fAdj(newOrder,k);
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.


%SerialLink.MANIPLTY Manipulability measure
%
% M = R.maniplty(Q, OPTIONS) is the manipulability index (scalar) for the
% robot at the joint configuration Q (1xN) where N is the number of robot
% joints.  It indicates dexterity, that is, how isotropic the robot's
% motion is with respect to the 6 degrees of Cartesian motion. The measure
% is high when the manipulator is capable of equal motion in all directions
% and low when the manipulator is close to a singularity.
%
% If Q is a matrix (MxN) then M (Mx1) is a vector of  manipulability
% indices for each joint configuration specified by a row of Q.
%
% [M,CI] = R.maniplty(Q, OPTIONS) as above, but for the case of the Asada
% measure returns the Cartesian inertia matrix CI.
%
% R.maniplty(Q) displays the translational and rotational manipulability.
%
% Two measures can be computed:
% - Yoshikawa's manipulability measure is based on the shape of the velocity
%   ellipsoid and depends only on kinematic parameters (default).
% - Asada's manipulability measure is based on the shape of the acceleration
%   ellipsoid which in turn is a function of the Cartesian inertia matrix and
%   the dynamic parameters.  The scalar measure computed here is the ratio of
%   the smallest/largest ellipsoid axis.  Ideally the ellipsoid would be
%   spherical, giving a ratio of 1, but in practice will be less than 1.
%
% Options::
% 'trans'       manipulability for transational motion only (default)
% 'rot'         manipulability for rotational motion only
% 'all'         manipulability for all motions
% 'dof',D       D is a vector (1x6) with non-zero elements if the
%               corresponding DOF is to be included for manipulability
% 'yoshikawa'   use Yoshikawa algorithm (default)
% 'asada'       use Asada algorithm
%
% Notes::
% - The 'all' option includes rotational and translational dexterity, but
%   this involves adding different units.  It can be more useful to look at the
%   translational and rotational manipulability separately.
% - Examples in the RVC book (1st edition) can be replicated by using the 'all' option
%
% References::
%
% - Analysis and control of robot manipulators with redundancy,
%   T. Yoshikawa,
%   Robotics Research: The First International Symposium (M. Brady and R. Paul, eds.),
%   pp. 735-747, The MIT press, 1984.
% - A geometrical representation of manipulator dynamics and its application to
%   arm design,
%   H. Asada,
%   Journal of Dynamic Systems, Measurement, and Control,
%   vol. 105, p. 131, 1983.
% - Robotics, Vision & Control, P. Corke, Springer 2011.
%
% See also SerialLink.inertia, SerialLink.jacob0.

% Copyright (C) 1993-2017, 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

%TODO
% return the ellipsoid?

function [w,mx] = maniplty(robot, q, varargin)

    opt.method = {'yoshikawa', 'asada'};
    opt.axes = {'all', 'trans', 'rot'};
    opt.dof = [];

    opt = tb_optparse(opt, varargin);

    if nargout == 0
        opt.axes = 'trans';
        mt = maniplty(robot, q, 'setopt', opt);
        opt.axes = 'rot';
        mr = maniplty(robot, q, 'setopt', opt);
        for i=1:numrows(mt)
        fprintf('Manipulability: translation %g, rotation %g\n', mt(i), mr(i));
        end
        return;
    end

    if isempty(opt.dof)
        switch opt.axes
            case 'trans'
                dof = [1 1 1 0 0 0];
            case 'rot'
                dof = [0 0 0 1 1 1];
            case 'all'
                dof = [1 1 1 1 1 1];
        end
    else
        dof = opt.dof;
    end

    opt.dof = logical(dof);

    if strcmp(opt.method, 'yoshikawa')
        w = zeros(numrows(q),1);
        for i=1:numrows(q)
            w(i) = yoshi(robot, q(i,:), opt);
        end
    elseif strcmp(opt.method, 'asada')
        w = zeros(numrows(q),1);
        if nargout > 1
            dof = sum(opt.dof);
            MX = zeros(dof,dof,numrows(q));
            for i=1:numrows(q)
                [ww,mm] = asada(robot, q(i,:), opt);
                w(i) = ww;
                MX(:,:,i) = mm;
            end
        else
            for i=1:numrows(q)
                w(i) = asada(robot, q(i,:), opt);
            end
        end
    end

    if nargout > 1
        mx = MX;
    end

function m = yoshi(robot, q, opt)
    J = robot.jacob0(q);

    J = J(opt.dof,:);
    m2 = det(J * J');
    m2 = max(0, m2);    % clip it to positive
    m = sqrt(m2);

function [m, mx] = asada(robot, q, opt)
    J = robot.jacob0(q);

    if rank(J) < 6
        warning('robot is in degenerate configuration')
        m = 0;
        return;
    end

    Ji = pinv(J);
    M = robot.inertia(q);
    Mx = Ji' * M * Ji;
    d = find(opt.dof);
    Mx = Mx(d,d);
    e = eig(Mx);
    m = min(e) / max(e);

    if nargout > 1
        mx = Mx;
    end


function [newTree, derTree] = splitTree(tree, maxOrder)
%SPLITTREE   Split syntax trees into derivative part and non-derivative part
%   Calling sequence:
%      [NEWTREE, DERTREE] = SPLITTREE(TREE, MAXORDER)
%   where the inputs are:
%      TREE:       The syntax tree to be split.
%      MAXORDER:   A vector that contains the the maximum differential order of
%                  each variable that appears in the problem.
%   and the outputs are
%      NEWTREE:    A syntax tree which describes the factor in which the highest
%                  order derivative appears. E.g. if we split the expression
%                  5*diff(u) + sin(u), NEWTREE is the syntax tree corresponding
%                  to 5*diff(u).
%      DERTREE:    A syntax tree which describes the remaining factors not
%                  included in NEWTREE. In the example above, DERTREE is the
%                  syntax tree corresponding to sin(u).

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Find what variables we have actually computed derivatives of.
diffVar = maxOrder > 0;

% If our input is not a syntax tree, we can be certain we don't need to split it
% (this can e.g. happen if TREE is a CHEBFUN or a scalar).
if ( ~isstruct(tree) )
    newTree = tree;
    derTree = [];
    return
end

% Find the diffOrders in the input tree.
treeDiffOrder = tree.diffOrder;

% If our tree doesn't have any derivatives of highest order that appear in the
% problem, we can return the subtree. But only need to check for those variables
% that we actually differentiate with respect to, e.g. if maxOrder = 0, we can
% return the subtree as well.
if ( all(treeDiffOrder(diffVar) < maxOrder(diffVar)) )
    newTree = tree;
    derTree = [];
    return
end

% Split the tree recursively, depending on how many inputs the operator takes.
switch tree.numArgs
    case 1
        % We're dealing with a unary operator.
        if ( strcmp(tree.method, 'uminus') )
            % For a unary minus, convert it to a "-1*" tree and split, which
            % will take care of negating the resulting trees:
            tempTree = struct('method', 'times', 'numArgs', 2, ...
                'left', -1, 'right', tree.center, ...
                'diffOrder', tree.diffOrder, 'height', tree.height, ...
                'ID', tree.ID, 'hasTerms', tree.hasTerms);
            [newTree, derTree] = treeVar.splitTree(tempTree, maxOrder);
        else
            % Otherwise, we can simply split the center node:
            [newTree, derTree] = treeVar.splitTree(tree.center, maxOrder);
        end
    case 2
        if ( any(strcmp(tree.method, {'diff', 'times', 'rdivide'})) )
            % We have already been through the expandTree() method, which
            % guarantees that there are no terms left in the tree which include
            % highest order derivatives along with other expressions in which
            % the unknown variable appears, e.g. 5*(diff(u) + u). Since we don't
            % reach this point in the code unless the input TREE contains the
            % maximum diffOrder of the problem, we can safely assume that the
            % input TREE is the derivative tree, and return it.
            newTree = [];
            derTree = tree;
        else
            % We're at + or -, split the left and right children trees
            % recursively.
            [newTreeLeft, derTreeLeft] = ...
                treeVar.splitTree(tree.left, maxOrder);
            [newTreeRight, derTreeRight] = ...
                treeVar.splitTree(tree.right, maxOrder);

            if ( isempty(newTreeLeft) )
                % We only had a derivative part on the left. Thus, the
                % non-derivative parts will only consist of the non-derivative
                % part of the right tree. However, if the method of our current
                % tree is MINUS(), we must add a UMINUS() in front of
                % NEWTREERIGHT before we can return it. That will be done in the
                % ONETREEFROMRIGHT() method. Simply put, we're converting a
                % binary minus between an empty tree on left and a non-empty
                % tree on the right to a UMINUS on the non-empty tree.
                newTree = oneTreeFromRight(newTreeRight, tree.method);
            elseif (isempty(newTreeRight) )
                % We only had a derivative part on the right, so the
                % non-derivative part only consists of the left part.
                newTree = newTreeLeft;
            elseif ( ~isstruct(newTreeLeft) && ~isstruct(newTreeRight) )
                % Both left and right trees were actually a CHEBFUN or scalar,
                % combine them in a single CHEBFUN/scalar and return as the
                % NEWTREE.
                newTree = eval([tree.method, '(newTreeLeft, newTreeRight)']);
            else
                % Had non derivative parts on both left and right, potentially a
                % combination of CHEBFUN/scalars and syntax trees. Need to
                % combine them.
                if ( ~isstruct(newTreeLeft) )
                    % Left tree was actually a CHEBFUN or scalar, so the new
                    % diffOrders and height only depend on the right tree.
                    newDiffOrder = newTreeRight.diffOrder;
                    newHeight = newTreeRight.height;
                elseif ( ~isstruct(newTreeRight) )
                    % Right tree was actually a CHEBFUN or scalar, so the new
                    % diffOrders and height only depend on the right tree.
                    newDiffOrder = newTreeLeft.diffOrder;
                    newHeight = newTreeLeft.height;
                else
                    % Both left and right tree were actually syntax trees, so
                    % find the maximum heights and diffOrders.
                    newDiffOrder = max(newTreeLeft.diffOrder, ...
                        newTreeRight.diffOrder);
                    newHeight = max(newTreeLeft.height, newTreeRight.height);
                end

                % Construct a new syntax tree from the new left and right child
                % trees (the non-derivative parts).
                newTree = struct('method', tree.method, ...
                    'numArgs', tree.numArgs, ...
                    'left', newTreeLeft, 'right', newTreeRight, ...
                    'diffOrder', newDiffOrder, ...
                    'height', newHeight);
            end

            if ( isempty(derTreeLeft) )
                % Left child tree only consisted of non-derivative part. Like
                % above, return the right tree, but add a UMINUS() on top of it
                % if needed.
                derTree = oneTreeFromRight(derTreeRight, tree.method);
            elseif (isempty(derTreeRight) )
                % Right child tree only consisted of non-derivative part.
                derTree = derTreeLeft;
            else
                % Combine the left and right derivative subtrees.
                derTree = struct('method', tree.method, ...
                    'numArgs', tree.numArgs, ...
                    'left', derTreeLeft, 'right', derTreeRight, ...
                    'diffOrder', max(derTreeLeft.diffOrder, ...
                    derTreeRight.diffOrder), ...
                    'height', max(derTreeLeft.height, derTreeRight.height));
            end

        end
end
end

function ot = oneTreeFromRight(tree, operator)
%ONETREEFROMRIGHT   Convert binary minus to uminus when left tree is empty.
%   Add a UMINUS in front of a syntax tree in the case where the left child tree
%   of MINUS() only consisted of the derivative part. Simply put, we're
%   converting a binary minus between an empty tree on left and a non-empty tree
%   on the right to a UMINUS on the non-empty tree.
if ( strcmp(operator, 'minus') )
    % Only need to worry if the original operator was a -.
    if ( isstruct(tree) )
        % If the input is a syntax tree, add a UMINUS on the top of it.
        ot = struct('method', 'uminus', 'numArgs', 1, 'center', tree, ...
            'diffOrder', tree.diffOrder, 'ID', tree.ID, ...
            'height', tree.height + 1);
    else
        % Input was a CHEBFUN or a scalar, can simply negate it.
        ot = -tree;
    end
else
    % Didn't have a MINUS(), so can simply return the input.
    ot = tree;
end
end


function [res] = tt_mvdot(a_tt,x_tt,y_tt)
%Fast computation of the bilinear form (Ax,y) in the TT-format
%   [RES]=TT_MVDOT(A_TT,X_TT,Y_TT) Computes scalar product(AX,Y), where
%   A_TT is a matrix, X_TT is a vector, Y_TT  is a vector
%
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------
d=size(a_tt,1);
%The c
core=a_tt{1}; matx=x_tt{1}; maty=y_tt{1};
n1=size(core,1); n2=size(core,2);
rc=size(core,3);
rx=size(matx,2); ry=size(maty,2);
core=maty'*reshape(core,[n1,n2*rc]); %core is ryxn2xrc
core=reshape(permute(reshape(core,[ry,n2,rc]),[3,1,2]),[rc*ry,n2]);
phi=permute(reshape(core*matx,[rc,ry,rx]),[1,3,2]); %core is rcxryxrx-> rcxrx*ry

for i=2:d-1
  core=a_tt{i}; core_x = x_tt{i}; core_y=y_tt{i};
  %Decipher sizes
 n1=size(core,1); n2=size(core,2); rc1=size(core,3); rc2=size(core,4);
 rx1=size(core_x,2); rx2=size(core_x,3); ry1=size(core_y,2); ry2=size(core_y,3);
 %Convolve phi & core over the first index
 %core is n1xn2xrc1xrc2 phi is rc1x rx1 x ry1
  core=reshape(permute(core,[3,1,2,4,5]),[rc1,n1*n2*rc2]);
  phi=reshape(phi,[rc1,rx1*ry1])'*core; %Is rx1 x ry1 x n1 x n2 x rc2
  %Convolve over n2 & rx1
  phi=permute(reshape(phi,[rx1,ry1,n1,n2,rc2]),[4,1,2,3,5]);
  %n2 x rx1 x ry1 x n1 x rc2;
  phi=reshape(core_x,[n2*rx1,rx2])'*reshape(phi,[n2*rx1,ry1*n1*rc2]);
  %phi is now rx2 x ry1 x n1 x rc2
  %Convolve over n1 & ry1
  phi=reshape(phi,[rx2,ry1*n1,rc2]); phi=permute(phi,[3,1,2]);
  %keyboard;
  phi=reshape(phi,[rc2*rx2,ry1*n1])*reshape(permute(core_y,[2,1,3]),[ry1*n1,ry2]);
  phi=reshape(phi,[rc2,rx2,ry2]);
end
  core=a_tt{d}; mat_x=x_tt{d}; mat_y=y_tt{d};
  n1=size(core,1); n2=size(core,2); rc=size(core,3);
  rx=size(mat_x,2); ry=size(mat_y,2);
  %Convolve phi & core over aux index
  %core is n1 x n2 x rc
  phi=reshape(core,[n1*n2,rc])*reshape(phi,[rc,rx*ry]);
  %phi is n1 x n2 x rx x ry
  %Convolve over n2 & rx
  phi=permute(reshape(phi,[n1,n2*rx,ry]),[1,3,2]);
  phi=reshape(phi,[n1*ry,n2*rx])*reshape(mat_x,[n2*rx,1]);
  %phi is a column of length
  res= phi'*reshape(mat_y,[n1*ry,1]);

return

end