% Eldar reconstruction

N = 3;                  % Nth order nonuniform sampling
TQ = 1;                 % Nyquist Period
L=20;                   % Resolution factor for fraction delay

T = [2*TQ 3*TQ 6*TQ];      % Decimation Periods
K = 0.5*lcm(2*T(1), 2*T(2)); K = 0.5*lcm(2*K, 2*T(3))/TQ;
capT = K*TQ; M = capT./T;
capM = lcm(M(1), M(2)); capM = lcm(capM, M(3));
excess = ceil((K-1)/capM);
maxf = K/(excess*capM+1);
TQ1 = maxf*TQ;
K1 = K/maxf;

ML = 400; % number of slices
w_c = 0.85;
NS = 100;  % Number of Sinusoids

LF = capM*2*K1+1;  %359,159,239          % min length of LF should be capM*2*K1
n = -(LF-1)/2:1:(LF-1)/2;
Hd = firpm(LF-1,[0 w_c],[0 w_c*pi],'differentiator');
delayV = (LF-1)/2;

std = [1e-6 1e-5 1e-4 1e-3 1e-2 1e-1];%5*1e-1];

serE = zeros(size(std));

MCruns = 25;
MCruns1 = 25;

for tt = 1:length(std)
    aa = 0;
    display(tt);
    for rrr = 1:MCruns1
        taus = [0 1.1+std(tt)*randn 2.2+std(tt)*randn]*TQ;
        if or(taus(2)==TQ,or(taus(2)==2*TQ,or(taus(3)==2*TQ,or(taus(2)==taus(3),taus(2)==taus(1)))))
            aa = aa+1;
            display(taus);
            continue;
        end

        tausI = sort([taus(1) taus(2) taus(3) T(1)+taus(1) T(2)+taus(2) 2*T(1)+taus(1)]);
        a = zeros(1,K);
        for p = 1:K
            a(p) = 1;
            for q = 1:K
                if q ~= p
                        a(p) = a(p)/sin(pi*(tausI(p)-tausI(q))/capT);
                end;
            end;
        end;

        for pp = 1:MCruns;
            Frq = rand(1,NS)*w_c/2;
            Amp = rand(1,NS)/(sqrt(NS)*2);
            Phi = rand(1,NS)*2*pi;
            inputN = zeros(1,ML*K1);
            input = zeros(1,ML*K);
            for k = 1:NS
              inputN = inputN + Amp(k)*sin(2*pi*Frq(k)*(0:ML*K1-1)*TQ1+Phi(k));
              input = input + Amp(k)*sin(2*pi*Frq(k)*(0:ML*K-1)*TQ+Phi(k));
            end;
        end

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

            y1 = upsample(x1,K1);

            LFE = M(p)*capM*2*K1+1;  %359,159,239          % min length of LF should be capM*2*K1
            nE = -(LFE-1)/2:1:(LFE-1)/2;
            h = sinc((nE/K1)-(taus(p)/T(p))).*kaiser_mine1(LFE,18,-K1*(taus(p)/T(p)));
            aaa = ones(1,M(p));
            bb = ones(M(p),LFE);
            for l = 1:M(p)
                for q = 1:N
                    if q ~= p
                        aaa(l) = aaa(l)*sin(pi*(taus(p)-taus(q)+(l-1)*T(p))/T(q));
                        bb(l,:) = bb(l,:).*sin(pi*((nE*TQ1/M(p))-taus(q)+(l-1)*T(p))/T(q));
                    end;
                end;
                bb(l,:) = bb(l,:)/aaa(l);
            end;

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

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            y1 = conv(y1,bbn);
            delay = (length(h)-1)/2;
            y1 = y1(1+delay:M(p):end-delay);
            y(p,:) = y1;
        end;
        y = real(sum(y,1));
        x = inputN;
        y = y(160:end-60);
        x = x(160:end-60);
        serE(tt) = serE(tt)+20*log10(norm(x,2)/norm(y-x,2));
    end
end

serE = serE/(MCruns*(MCruns1-aa));

plot(std,serE);
xlabel('Standard Deviation (\sigma)','fontsize',14,'fontweight','b');
ylabel('SNR in dB','fontsize',14,'fontweight','b');
grid on;box on;
set(gca,'fontsize',14,'fontweight','b')


% Copyright 2013 Oliver Johnson, Srikanth Patala
%
% This file is part of MisorientationMaps.
%
%     MisorientationMaps 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.
%
%     MisorientationMaps 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 MisorientationMaps.  If not, see <http://www.gnu.org/licenses/>.

function S=colormap222(v)

pts = reshape(double(v),[],3);

k1 = sqrt(3)*max(pts,[],2)./(sum(pts,2) + 1*(sum(pts,2) == 0));
pts = [k1.*pts(:,1) k1.*pts(:,2) k1.*pts(:,3)];

%%% Rotate Prism %%%%%%%%
g1 = rotvec2mat([1,1,1,pi/4]);g2 = rotvec2mat([-1,1,0,acos(1/sqrt(3))]);
g3 = rotvec2mat([0,0,1,-pi/3]);pts = pts*g1*g2*g3;pts(:,3)=pts(:,3)-1;

%%%%%% Prism --> Cone %%%%%%%%%%%
phi=atan2(pts(:,2),pts(:,1));maxphi = 2*pi; phi = mod(phi,maxphi);
rfin = hypot(pts(:,1),pts(:,2)).*sin(pi/6+mod(phi,2*pi/3))./(sin(pi/6));
pts = [sqrt(1/2).*rfin.*cos(phi) sqrt(1/2).*rfin.*sin(phi) pts(:,3)];

%%%%%% Cone --> Hemisphere %%%%%%
r = sqrt(pts(:,1).^2 + pts(:,2).^2 + pts(:,3).^2);
rad = sqrt(pts(:,1).^2 + pts(:,2).^2);

pts = [pts(:,1).*(rad - pts(:,3))./(r+1*(r==0)) ...
    pts(:,2).*(rad - pts(:,3))./(r+1*(r==0)) ...
    pts(:,3).*(rad - pts(:,3))./(r+1*(r==0))];

%%%%% Hemisphere --> Sphere

phi1 = atan2(pts(:,2),pts(:,1));phi1 = mod(phi1,2*pi);
ind1=find(phi1 < 2*pi/3 & phi1 > 0*pi/3);ind2=find(phi1 < 4*pi/3 & phi1 > 2*pi/3);
ind3=find(phi1 < 6*pi/3 & phi1 > 4*pi/3);ind = [ind1;ind2;ind3];

g4 = rotvec2mat([0,0,1,pi/3]);g5 = rotvec2mat([0,0,1,pi]);g6 = rotvec2mat([0,0,1,5*pi/3]);
pts(ind1,:) = pts(ind1,:)*g4;pts(ind2,:) = pts(ind2,:)*g5;pts(ind3,:) = pts(ind3,:)*g6;

phi2 = atan2(pts(ind,2),pts(ind,1));rad1 = hypot(pts(ind,2),pts(ind,1));
pts(ind,:) = [rad1.*cos(3*phi2/2) rad1.*sin(3*phi2/2) pts(ind,3)];

mult = 1 + 1;
phi4 = atan2(pts(ind,1),-pts(ind,3));rad4 = hypot(pts(ind,1),pts(ind,3));
pts(ind,:) = [rad4.*sin(mult*phi4) pts(ind,2) rad4.*-cos(mult*phi4)];

phi3 = atan2(pts(ind,2),pts(ind,1));rad3 = hypot(pts(ind,2),pts(ind,1));
pts(ind,:) = [rad3.*cos(2*phi3/3) rad3.*sin(2*phi3/3) pts(ind,3)];

pts(ind1,:) = pts(ind1,:)*g4';pts(ind2,:) = pts(ind2,:)*g5';pts(ind3,:) = pts(ind3,:)*g6';

g1 = rotvec2mat([0,0,1,pi/3]);
pts=pts*g1;

S = hsvsphere(pts);


function b = generative_model(A,D,m,modeltype,modelvar,params,epsilon)
%GENERATIVE_MODEL   run generative model code
%
%   B = GENERATIVE_MODEL(A,D,m,modeltype,modelvar,params)
%
%   Generates synthetic networks using the models described in the study by
%   Betzel et al (2016) in Neuroimage.
%
%   Inputs:
%           A,          binary network of seed connections
%           D,          Euclidean distance/fiber length matrix
%           m,          number of connections that should be present in
%                       final synthetic network
%           modeltype,  specifies the generative rule (see below)
%           modelvar,   specifies whether the generative rules are based on
%                       power-law or exponential relationship
%                       ({'powerlaw'}|{'exponential})
%           params,     either a vector (in the case of the geometric
%                       model) or a matrix (for all other models) of
%                       parameters at which the model should be evaluated.
%           epsilon,    the baseline probability of forming a particular
%                       connection (should be a very small number
%                       {default = 1e-5}).
%
%   Output:
%           B,          m x number of networks matrix of connections
%
%
%   Full list of model types:
%   (each model type realizes a different generative rule)
%
%       1.  'sptl'          spatial model
%       2.  'neighbors'     number of common neighbors
%       3.  'matching'      matching index
%       4.  'clu-avg'       average clustering coeff.
%       5.  'clu-min'       minimum clustering coeff.
%       6.  'clu-max'       maximum clustering coeff.
%       7.  'clu-diff'      difference in clustering coeff.
%       8.  'clu-prod'      product of clustering coeff.
%       9.  'deg-avg'       average degree
%       10. 'deg-min'       minimum degree
%       11. 'deg-max'       maximum degree
%       12. 'deg-diff'      difference in degree
%       13. 'deg-prod'      product of degree
%
%
%   Example usage:
%
%       load demo_generative_models_data
%
%       % get number of bi-directional connections
%       m = nnz(A)/2;
%
%       % get cardinality of network
%       n = length(A);
%
%       % set model type
%       modeltype = 'neighbors';
%
%       % set whether the model is based on powerlaw or exponentials
%       modelvar = [{'powerlaw'},{'powerlaw'}];
%
%       % choose some model parameters
%       params = [-2,0.2; -5,1.2; -1,1.5];
%       nparams = size(params,1);
%
%       % generate synthetic networks
%       B = generative_model(Aseed,D,m,modeltype,modelvar,params);
%
%       % store them in adjacency matrix format
%       Asynth = zeros(n,n,nparams);
%       for i = 1:nparams;
%           a = zeros(n); a(B(:,i)) = 1; a = a + a';
%           Asynth(:,:,i) = a;
%       end
%
%   Reference: Betzel et al (2016) Neuroimage 124:1054-64.
%
%   Richard Betzel, Indiana University/University of Pennsylvania, 2015

if ~exist('epsilon','var')
    epsilon = 1e-5;
end

n = length(D);
nparams = size(params,1);
b = zeros(m,nparams);

switch modeltype

    case 'clu-avg'
        clu = clustering_coef_bu(A);
        Kseed = bsxfun(@plus,clu(:,ones(1,n)),clu')/2;
        for iparam = 1:nparams
            eta = params(iparam,1);
            gam = params(iparam,2);
            b(:,iparam) = fcn_clu_avg(A,Kseed,D,m,eta,gam,modelvar,epsilon);
        end

    case 'clu-diff'
        clu = clustering_coef_bu(A);
        Kseed = abs(bsxfun(@minus,clu(:,ones(1,n)),clu'));
        for iparam = 1:nparams
            eta = params(iparam,1);
            gam = params(iparam,2);
            b(:,iparam) = fcn_clu_diff(A,Kseed,D,m,eta,gam,modelvar,epsilon);
        end

    case 'clu-max'
        clu = clustering_coef_bu(A);
        Kseed = bsxfun(@max,clu(:,ones(1,n)),clu');
        for iparam = 1:nparams
            eta = params(iparam,1);
            gam = params(iparam,2);
            b(:,iparam) = fcn_clu_max(A,Kseed,D,m,eta,gam,modelvar,epsilon);
        end

    case 'clu-min'
        clu = clustering_coef_bu(A);
        Kseed = bsxfun(@min,clu(:,ones(1,n)),clu');
        for iparam = 1:nparams
            eta = params(iparam,1);
            gam = params(iparam,2);
            b(:,iparam) = fcn_clu_min(A,Kseed,D,m,eta,gam,modelvar,epsilon);
        end

    case 'clu-prod'
        clu = clustering_coef_bu(A);
        Kseed = clu*clu';
        for iparam = 1:nparams
            eta = params(iparam,1);
            gam = params(iparam,2);
            b(:,iparam) = fcn_clu_prod(A,Kseed,D,m,eta,gam,modelvar,epsilon);
        end

    case 'deg-avg'
        kseed = sum(A,2);
        Kseed = bsxfun(@plus,kseed(:,ones(1,n)),kseed')/2;
        for iparam = 1:nparams
            eta = params(iparam,1);
            gam = params(iparam,2);
            b(:,iparam) = fcn_deg_avg(A,Kseed,D,m,eta,gam,modelvar,epsilon);
        end

    case 'deg-diff'
        kseed = sum(A,2);
        Kseed = abs(bsxfun(@minus,kseed(:,ones(1,n)),kseed'));
        for iparam = 1:nparams
            eta = params(iparam,1);
            gam = params(iparam,2);
            b(:,iparam) = fcn_deg_diff(A,Kseed,D,m,eta,gam,modelvar,epsilon);
        end

    case 'deg-max'
        kseed = sum(A,2);
        Kseed = bsxfun(@max,kseed(:,ones(1,n)),kseed');
        for iparam = 1:nparams
            eta = params(iparam,1);
            gam = params(iparam,2);
            b(:,iparam) = fcn_deg_max(A,Kseed,D,m,eta,gam,modelvar,epsilon);
        end

    case 'deg-min'
        kseed = sum(A,2);
        Kseed = bsxfun(@min,kseed(:,ones(1,n)),kseed');
        for iparam = 1:nparams
            eta = params(iparam,1);
            gam = params(iparam,2);
            b(:,iparam) = fcn_deg_min(A,Kseed,D,m,eta,gam,modelvar,epsilon);
        end

    case 'deg-prod'
        kseed = sum(A,2);
        Kseed = (kseed*kseed').*~eye(n);
        for iparam = 1:nparams
            eta = params(iparam,1);
            gam = params(iparam,2);
            b(:,iparam) = fcn_deg_prod(A,Kseed,D,m,eta,gam,modelvar,epsilon);
        end

    case 'neighbors'
        Kseed = (A*A).*~eye(n);
        for iparam = 1:nparams
            eta = params(iparam,1);
            gam = params(iparam,2);
            b(:,iparam) = fcn_nghbrs(A,Kseed,D,m,eta,gam,modelvar,epsilon);
        end

    case 'matching'
        Kseed = matching_ind(A);
        Kseed = Kseed + Kseed';
        for iparam = 1:nparams
            eta = params(iparam,1);
            gam = params(iparam,2);
            b(:,iparam) = fcn_matching(A,Kseed,D,m,eta,gam,modelvar,epsilon);
        end

    case 'sptl'
        for iparam = 1:nparams
            eta = params(iparam,1);
            b(:,iparam) = fcn_sptl(A,D,m,eta,modelvar{1});
        end

end

function b = fcn_clu_avg(A,K,D,m,eta,gam,modelvar,epsilon)
K = K + epsilon;
n = length(D);
mseed = nnz(A)/2;
A = A > 0;
mv1 = modelvar{1};
mv2 = modelvar{2};
switch mv1
    case 'powerlaw'
        Fd = D.^eta;
    case 'exponential'
        Fd = exp(eta*D);
end
switch mv2
    case 'powerlaw'
        Fk = K.^gam;
    case 'exponential'
        Fk = exp(gam*K);
end

c = clustering_coef_bu(A);
k = sum(A,2);

Ff = Fd.*Fk.*~A;
[u,v] = find(triu(ones(n),1));
indx = (v - 1)*n + u;
P = Ff(indx);

for i = (mseed + 1):m
    C = [0; cumsum(P)];
    r = sum(rand*C(end) >= C);
    uu = u(r);
    vv = v(r);
    A(uu,vv) = 1;
    A(vv,uu) = 1;
    k([uu,vv]) = k([uu,vv]) + 1;
    bu = A(uu,:);
    su = A(bu,bu);
    bv = A(vv,:);
    sv = A(bv,bv);
    bth = bu & bv;
    c(bth) = c(bth) + 2./(k(bth).^2 - k(bth));
    c(uu) = nnz(su)/(k(uu)*(k(uu) - 1));
    c(vv) = nnz(sv)/(k(vv)*(k(vv) - 1));
    c(k <= 1) = 0;
    bth([uu,vv]) = true;
    K(:,bth) = bsxfun(@plus,c(:,ones(1,sum(bth))),c(bth,:)')/2 + epsilon;
    K(bth,:) = bsxfun(@plus,c(:,ones(1,sum(bth))),c(bth,:)')'/2 + epsilon;

    switch mv2
        case 'powerlaw'
            Ff(bth,:) = Fd(bth,:).*((K(bth,:)).^gam);
            Ff(:,bth) = Fd(:,bth).*((K(:,bth)).^gam);
        case 'exponential'
            Ff(bth,:) = Fd(bth,:).*exp((K(bth,:))*gam);
            Ff(:,bth) = Fd(:,bth).*exp((K(:,bth))*gam);
    end
    Ff = Ff.*~A;
    P = Ff(indx);
end
b = find(triu(A,1));

function b = fcn_clu_diff(A,K,D,m,eta,gam,modelvar,epsilon)
K = K + epsilon;
n = length(D);
mseed = nnz(A)/2;
A = A > 0;
mv1 = modelvar{1};
mv2 = modelvar{2};
switch mv1
    case 'powerlaw'
        Fd = D.^eta;
    case 'exponential'
        Fd = exp(eta*D);
end
switch mv2
    case 'powerlaw'
        Fk = K.^gam;
    case 'exponential'
        Fk = exp(gam*K);
end

c = clustering_coef_bu(A);
k = sum(A,2);

Ff = Fd.*Fk.*~A;
[u,v] = find(triu(ones(n),1));
indx = (v - 1)*n + u;
P = Ff(indx);

for i = (mseed + 1):m
    C = [0; cumsum(P)];
    r = sum(rand*C(end) >= C);
    uu = u(r);
    vv = v(r);
    A(uu,vv) = 1;
    A(vv,uu) = 1;
    k([uu,vv]) = k([uu,vv]) + 1;
    bu = A(uu,:);
    su = A(bu,bu);
    bv = A(vv,:);
    sv = A(bv,bv);
    bth = bu & bv;
    c(bth) = c(bth) + 2./(k(bth).^2 - k(bth));
    c(uu) = nnz(su)/(k(uu)*(k(uu) - 1));
    c(vv) = nnz(sv)/(k(vv)*(k(vv) - 1));
    c(k <= 1) = 0;
    bth([uu,vv]) = true;
    K(:,bth) = abs(bsxfun(@minus,c(:,ones(1,sum(bth))),c(bth,:)')) + epsilon;
    K(bth,:) = abs(bsxfun(@minus,c(:,ones(1,sum(bth))),c(bth,:)'))' + epsilon;

    switch mv2
        case 'powerlaw'
            Ff(bth,:) = Fd(bth,:).*((K(bth,:)).^gam);
            Ff(:,bth) = Fd(:,bth).*((K(:,bth)).^gam);
        case 'exponential'
            Ff(bth,:) = Fd(bth,:).*exp((K(bth,:))*gam);
            Ff(:,bth) = Fd(:,bth).*exp((K(:,bth))*gam);
    end
    Ff = Ff.*~A;
    P = Ff(indx);
end
b = find(triu(A,1));

function b = fcn_clu_max(A,K,D,m,eta,gam,modelvar,epsilon)
K = K + epsilon;
n = length(D);
mseed = nnz(A)/2;
A = A > 0;
mv1 = modelvar{1};
mv2 = modelvar{2};
switch mv1
    case 'powerlaw'
        Fd = D.^eta;
    case 'exponential'
        Fd = exp(eta*D);
end
switch mv2
    case 'powerlaw'
        Fk = K.^gam;
    case 'exponential'
        Fk = exp(gam*K);
end

c = clustering_coef_bu(A);
k = sum(A,2);

Ff = Fd.*Fk.*~A;
[u,v] = find(triu(ones(n),1));
indx = (v - 1)*n + u;
P = Ff(indx);

for i = (mseed + 1):m
    C = [0; cumsum(P)];
    r = sum(rand*C(end) >= C);
    uu = u(r);
    vv = v(r);
    A(uu,vv) = 1;
    A(vv,uu) = 1;
    k([uu,vv]) = k([uu,vv]) + 1;
    bu = A(uu,:);
    su = A(bu,bu);
    bv = A(vv,:);
    sv = A(bv,bv);
    bth = bu & bv;
    c(bth) = c(bth) + 2./(k(bth).^2 - k(bth));
    c(uu) = nnz(su)/(k(uu)*(k(uu) - 1));
    c(vv) = nnz(sv)/(k(vv)*(k(vv) - 1));
    c(k <= 1) = 0;
    bth([uu,vv]) = true;
    K(:,bth) = bsxfun(@max,c(:,ones(1,sum(bth))),c(bth,:)') + epsilon;
    K(bth,:) = bsxfun(@max,c(:,ones(1,sum(bth))),c(bth,:)')' + epsilon;

    switch mv2
        case 'powerlaw'
            Ff(bth,:) = Fd(bth,:).*((K(bth,:)).^gam);
            Ff(:,bth) = Fd(:,bth).*((K(:,bth)).^gam);
        case 'exponential'
            Ff(bth,:) = Fd(bth,:).*exp((K(bth,:))*gam);
            Ff(:,bth) = Fd(:,bth).*exp((K(:,bth))*gam);
    end
    Ff = Ff.*~A;
    P = Ff(indx);
end
b = find(triu(A,1));

function b = fcn_clu_min(A,K,D,m,eta,gam,modelvar,epsilon)
K = K + epsilon;
n = length(D);
mseed = nnz(A)/2;
A = A > 0;
mv1 = modelvar{1};
mv2 = modelvar{2};
switch mv1
    case 'powerlaw'
        Fd = D.^eta;
    case 'exponential'
        Fd = exp(eta*D);
end
switch mv2
    case 'powerlaw'
        Fk = K.^gam;
    case 'exponential'
        Fk = exp(gam*K);
end

c = clustering_coef_bu(A);
k = sum(A,2);

Ff = Fd.*Fk.*~A;
[u,v] = find(triu(ones(n),1));
indx = (v - 1)*n + u;
P = Ff(indx);

for i = (mseed + 1):m
    C = [0; cumsum(P)];
    r = sum(rand*C(end) >= C);
    uu = u(r);
    vv = v(r);
    A(uu,vv) = 1;
    A(vv,uu) = 1;
    k([uu,vv]) = k([uu,vv]) + 1;
    bu = A(uu,:);
    su = A(bu,bu);
    bv = A(vv,:);
    sv = A(bv,bv);
    bth = bu & bv;
    c(bth) = c(bth) + 2./(k(bth).^2 - k(bth));
    c(uu) = nnz(su)/(k(uu)*(k(uu) - 1));
    c(vv) = nnz(sv)/(k(vv)*(k(vv) - 1));
    c(k <= 1) = 0;
    bth([uu,vv]) = true;
    K(:,bth) = bsxfun(@min,c(:,ones(1,sum(bth))),c(bth,:)') + epsilon;
    K(bth,:) = bsxfun(@min,c(:,ones(1,sum(bth))),c(bth,:)')' + epsilon;

    switch mv2
        case 'powerlaw'
            Ff(bth,:) = Fd(bth,:).*((K(bth,:)).^gam);
            Ff(:,bth) = Fd(:,bth).*((K(:,bth)).^gam);
        case 'exponential'
            Ff(bth,:) = Fd(bth,:).*exp((K(bth,:))*gam);
            Ff(:,bth) = Fd(:,bth).*exp((K(:,bth))*gam);
    end
    Ff = Ff.*~A;
    P = Ff(indx);
end
b = find(triu(A,1));

function b = fcn_clu_prod(A,K,D,m,eta,gam,modelvar,epsilon)
K = K + epsilon;
n = length(D);
mseed = nnz(A)/2;
A = A > 0;
mv1 = modelvar{1};
mv2 = modelvar{2};
switch mv1
    case 'powerlaw'
        Fd = D.^eta;
    case 'exponential'
        Fd = exp(eta*D);
end
switch mv2
    case 'powerlaw'
        Fk = K.^gam;
    case 'exponential'
        Fk = exp(gam*K);
end

c = clustering_coef_bu(A);
k = sum(A,2);

Ff = Fd.*Fk.*~A;
[u,v] = find(triu(ones(n),1));
indx = (v - 1)*n + u;
P = Ff(indx);

for i = (mseed + 1):m
    C = [0; cumsum(P)];
    r = sum(rand*C(end) >= C);
    uu = u(r);
    vv = v(r);
    A(uu,vv) = 1;
    A(vv,uu) = 1;
    k([uu,vv]) = k([uu,vv]) + 1;
    bu = A(uu,:);
    su = A(bu,bu);
    bv = A(vv,:);
    sv = A(bv,bv);
    bth = bu & bv;
    c(bth) = c(bth) + 2./(k(bth).^2 - k(bth));
    c(uu) = nnz(su)/(k(uu)*(k(uu) - 1));
    c(vv) = nnz(sv)/(k(vv)*(k(vv) - 1));
    c(k <= 1) = 0;
    bth([uu,vv]) = true;
    K(bth,:) = (c(bth,:)*c') + epsilon;
    K(:,bth) = (c*c(bth,:)') + epsilon;

    switch mv2
        case 'powerlaw'
            Ff(bth,:) = Fd(bth,:).*((K(bth,:)).^gam);
            Ff(:,bth) = Fd(:,bth).*((K(:,bth)).^gam);
        case 'exponential'
            Ff(bth,:) = Fd(bth,:).*exp((K(bth,:))*gam);
            Ff(:,bth) = Fd(:,bth).*exp((K(:,bth))*gam);
    end
    Ff = Ff.*~A;
    P = Ff(indx);
end
b = find(triu(A,1));

function b = fcn_deg_avg(A,K,D,m,eta,gam,modelvar,epsilon)
n = length(D);
mseed = nnz(A)/2;
k = sum(A,2);
[u,v] = find(triu(ones(n),1));
indx = (v - 1)*n + u;
D = D(indx);
mv1 = modelvar{1};
mv2 = modelvar{2};
switch mv1
    case 'powerlaw'
        Fd = D.^eta;
    case 'exponential'
        Fd = exp(eta*D);
end
K = K + epsilon;
switch mv2
    case 'powerlaw'
        Fk = K.^gam;
    case 'exponential'
        Fk = exp(gam*K);
end
P = Fd.*Fk(indx).*~A(indx);
b = zeros(m,1);
b(1:mseed) = find(A(indx));
for i = (mseed + 1):m
    C = [0; cumsum(P)];
    r = sum(rand*C(end) >= C);
    w = [u(r),v(r)];
    k(w) = k(w) + 1;
    switch mv2
        case 'powerlaw'
            Fk(:,w) = [((k + k(w(1)))/2) + epsilon, ((k + k(w(2)))/2) + epsilon].^gam;
            Fk(w,:) = ([((k + k(w(1)))/2) + epsilon, ((k + k(w(2)))/2) + epsilon].^gam)';
        case 'exponential'
            Fk(:,w) = exp([((k + k(w(1)))/2) + epsilon, ((k + k(w(2)))/2) + epsilon]*gam);
            Fk(w,:) = exp([((k + k(w(1)))/2) + epsilon, ((k + k(w(2)))/2) + epsilon]*gam)';
    end
    P = Fd.*Fk(indx);
    b(i) = r;
    P(b(1:i)) = 0;
end
b = indx(b);

function b = fcn_deg_diff(A,K,D,m,eta,gam,modelvar,epsilon)
n = length(D);
mseed = nnz(A)/2;
k = sum(A,2);
[u,v] = find(triu(ones(n),1));
indx = (v - 1)*n + u;
D = D(indx);
mv1 = modelvar{1};
mv2 = modelvar{2};
switch mv1
    case 'powerlaw'
        Fd = D.^eta;
    case 'exponential'
        Fd = exp(eta*D);
end
K = K + epsilon;
switch mv2
    case 'powerlaw'
        Fk = K.^gam;
    case 'exponential'
        Fk = exp(gam*K);
end
P = Fd.*Fk(indx).*~A(indx);
b = zeros(m,1);
b(1:mseed) = find(A(indx));
for i = (mseed + 1):m
    C = [0; cumsum(P)];
    r = sum(rand*C(end) >= C);

    w = [u(r),v(r)];
    k(w) = k(w) + 1;
    switch mv2
        case 'powerlaw'
            Fk(:,w) = (abs([k - k(w(1)), k - k(w(2))]) + epsilon).^gam;
            Fk(w,:) = ((abs([k - k(w(1)), k - k(w(2))]) + epsilon).^gam)';
        case 'exponential'
            Fk(:,w) = exp((abs([k - k(w(1)), k - k(w(2))]) + epsilon)*gam);
            Fk(w,:) = exp((abs([k - k(w(1)), k - k(w(2))]) + epsilon)*gam)';
    end
    P = Fd.*Fk(indx);
    b(i) = r;
    P(b(1:i)) = 0;
end
b = indx(b);

function b = fcn_deg_min(A,K,D,m,eta,gam,modelvar,epsilon)
n = length(D);
mseed = nnz(A)/2;
k = sum(A,2);
[u,v] = find(triu(ones(n),1));
indx = (v - 1)*n + u;
D = D(indx);
mv1 = modelvar{1};
mv2 = modelvar{2};
switch mv1
    case 'powerlaw'
        Fd = D.^eta;
    case 'exponential'
        Fd = exp(eta*D);
end
K = K + epsilon;
switch mv2
    case 'powerlaw'
        Fk = K.^gam;
    case 'exponential'
        Fk = exp(gam*K);
end
P = Fd.*Fk(indx).*~A(indx);
b = zeros(m,1);
b(1:mseed) = find(A(indx));
for i = (mseed + 1):m
    C = [0; cumsum(P)];
    r = sum(rand*C(end) >= C);
    w = [u(r),v(r)];
    k(w) = k(w) + 1;
    switch mv2
        case 'powerlaw'
            Fk(:,w) = [min(k,k(w(1))) + epsilon, min(k,k(w(2))) + epsilon].^gam;
            Fk(w,:) = ([min(k,k(w(1))) + epsilon, min(k,k(w(2))) + epsilon].^gam)';
        case 'exponential'
            Fk(:,w) = exp([min(k,k(w(1))) + epsilon, min(k,k(w(2))) + epsilon]*gam);
            Fk(w,:) = exp([min(k,k(w(1))) + epsilon, min(k,k(w(2))) + epsilon]*gam)';
    end
    P = Fd.*Fk(indx);
    b(i) = r;
    P(b(1:i)) = 0;
end
b = indx(b);

function b = fcn_deg_max(A,K,D,m,eta,gam,modelvar,epsilon)
n = length(D);
mseed = nnz(A)/2;
k = sum(A,2);
[u,v] = find(triu(ones(n),1));
indx = (v - 1)*n + u;
D = D(indx);
mv1 = modelvar{1};
mv2 = modelvar{2};
switch mv1
    case 'powerlaw'
        Fd = D.^eta;
    case 'exponential'
        Fd = exp(eta*D);
end
K = K + epsilon;
switch mv2
    case 'powerlaw'
        Fk = K.^gam;
    case 'exponential'
        Fk = exp(gam*K);
end
P = Fd.*Fk(indx).*~A(indx);
b = zeros(m,1);
b(1:mseed) = find(A(indx));
for i = (mseed + 1):m
    C = [0; cumsum(P)];
    r = sum(rand*C(end) >= C);
    w = [u(r),v(r)];
    k(w) = k(w) + 1;
    switch mv2
        case 'powerlaw'
            Fk(:,w) = [max(k,k(w(1))) + epsilon, max(k,k(w(2))) + epsilon].^gam;
            Fk(w,:) = ([max(k,k(w(1))) + epsilon, max(k,k(w(2))) + epsilon].^gam)';
        case 'exponential'
            Fk(:,w) = exp([max(k,k(w(1))) + epsilon, max(k,k(w(2))) + epsilon]*gam);
            Fk(w,:) = exp([max(k,k(w(1))) + epsilon, max(k,k(w(2))) + epsilon]*gam)';
    end
    P = Fd.*Fk(indx);
    b(i) = r;
    P(b(1:i)) = 0;
end
b = indx(b);

function b = fcn_deg_prod(A,K,D,m,eta,gam,modelvar,epsilon)
n = length(D);
mseed = nnz(A)/2;
k = sum(A,2);
[u,v] = find(triu(ones(n),1));
indx = (v - 1)*n + u;
D = D(indx);
mv1 = modelvar{1};
mv2 = modelvar{2};
switch mv1
    case 'powerlaw'
        Fd = D.^eta;
    case 'exponential'
        Fd = exp(eta*D);
end
K = K + epsilon;
switch mv2
    case 'powerlaw'
        Fk = K.^gam;
    case 'exponential'
        Fk = exp(gam*K);
end
P = Fd.*Fk(indx).*~A(indx);
b = zeros(m,1);
b(1:mseed) = find(A(indx));
for i = (mseed + 1):m
    C = [0; cumsum(P)];
    r = sum(rand*C(end) >= C);
    w = [u(r),v(r)];
    k(w) = k(w) + 1;
    switch mv2
        case 'powerlaw'
            Fk(:,w) = ([k*k(w(1)) + epsilon, k*k(w(2)) + epsilon].^gam);
            Fk(w,:) = (([k*k(w(1)) + epsilon, k*k(w(2)) + epsilon].^gam)');
        case 'exponential'
            Fk(:,w) = exp([k*k(w(1)) + epsilon, k*k(w(2)) + epsilon]*gam);
            Fk(w,:) = exp([k*k(w(1)) + epsilon, k*k(w(2)) + epsilon]*gam)';
    end
    P = Fd.*Fk(indx);
    b(i) = r;
    P(b(1:i)) = 0;
end
b = indx(b);

function b = fcn_nghbrs(A,K,D,m,eta,gam,modelvar,epsilon)
K = K + epsilon;
n = length(D);
mseed = nnz(A)/2;
A = A > 0;
mv1 = modelvar{1};
mv2 = modelvar{2};
switch mv1
    case 'powerlaw'
        Fd = D.^eta;
    case 'exponential'
        Fd = exp(eta*D);
end
switch mv2
    case 'powerlaw'
%         gam = abs(gam);
        Fk = K.^gam;
    case 'exponential'
        Fk = exp(gam*K);
end
Ff = Fd.*Fk.*~A;
[u,v] = find(triu(ones(n),1));
indx = (v - 1)*n + u;
P = Ff(indx);
for i = (mseed + 1):m
    C = [0; cumsum(P)];
    r = sum(rand*C(end) >= C);
    uu = u(r);
    vv = v(r);
    x = A(uu,:);
    y = A(:,vv);
    A(uu,vv) = 1;
    A(vv,uu) = 1;
    K(uu,y) = K(uu,y) + 1;
    K(y,uu) = K(y,uu) + 1;
    K(vv,x) = K(vv,x) + 1;
    K(x,vv) = K(x,vv) + 1;
    switch mv2
        case 'powerlaw'
            Ff(uu,y) = Fd(uu,y).*(K(uu,y).^gam);
            Ff(y,uu) = Ff(uu,y)';
            Ff(vv,x) = Fd(vv,x).*(K(vv,x).^gam);
            Ff(x,vv) = Ff(vv,x)';
        case 'exponential'
            Ff(uu,y) = Fd(uu,y).*exp(K(uu,y)*gam);
            Ff(y,uu) = Ff(uu,y)';
            Ff(vv,x) = Fd(vv,x).*exp(K(vv,x)*gam);
            Ff(x,vv) = Ff(vv,x)';
    end
    Ff(A) = 0;
    P = Ff(indx);
end
b = find(triu(A,1));

function b = fcn_matching(A,K,D,m,eta,gam,modelvar,epsilon)
K = K + epsilon;
n = length(D);
mseed = nnz(A)/2;
mv1 = modelvar{1};
mv2 = modelvar{2};
switch mv1
    case 'powerlaw'
        Fd = D.^eta;
    case 'exponential'
        Fd = exp(eta*D);
end
switch mv2
    case 'powerlaw'
        Fk = K.^gam;
    case 'exponential'
        Fk = exp(gam*K);
end
Ff = Fd.*Fk.*~A;
[u,v] = find(triu(ones(n),1));
indx = (v - 1)*n + u;
P = Ff(indx);
for ii = (mseed + 1):m

    C = [0; cumsum(P)];
    r = sum(rand*C(end) >= C);
    uu = u(r);
    vv = v(r);

    A(uu,vv) = 1;
    A(vv,uu) = 1;

    updateuu = find(A*A(:,uu));
    updateuu(updateuu == uu) = [];
    updateuu(updateuu == vv) = [];

    updatevv = find(A*A(:,vv));
    updatevv(updatevv == uu) = [];
    updatevv(updatevv == vv) = [];

    c1 = [A(:,uu)', A(uu,:)];
    for i = 1:length(updateuu)
        j = updateuu(i);
        c2 = [A(:,j)' A(j,:)];
        use = ~(~c1&~c2);
        use(uu) = 0;  use(uu+n) = 0;
        use(j) = 0;  use(j+n) = 0;
        ncon = sum(c1(use))+sum(c2(use));
        if (ncon==0)
            K(uu,j) = epsilon;
            K(j,uu) = epsilon;
        else
            K(uu,j) = (2*(sum(c1(use)&c2(use))/ncon)) + epsilon;
            K(j,uu) = K(uu,j);
        end

    end

    c1 = [A(:,vv)', A(vv,:)];
    for i = 1:length(updatevv)
        j = updatevv(i);
        c2 = [A(:,j)' A(j,:)];
        use = ~(~c1&~c2);
        use(vv) = 0;  use(vv+n) = 0;
        use(j) = 0;  use(j+n) = 0;
        ncon = sum(c1(use))+sum(c2(use));
        if (ncon==0)
            K(vv,j) = epsilon;
            K(j,vv) = epsilon;
        else
            K(vv,j) = (2*(sum(c1(use)&c2(use))/ncon)) + epsilon;
            K(j,vv) = K(vv,j);
        end
    end
    switch mv2
        case 'powerlaw'
            Fk = K.^gam;
        case 'exponential'
            Fk = exp(gam*K);
    end
    Ff = Fd.*Fk.*~A;
    P = Ff(indx);
end
b = find(triu(A,1));

function b = fcn_sptl(A,D,m,eta,modelvar)
n = length(D);
mseed = nnz(A)/2;
switch modelvar
    case 'powerlaw'
        Fd = D.^eta;
    case 'exponential'
        Fd = exp(eta*D);
end
[u,v] = find(triu(ones(n),1));
indx = (v - 1)*n + u;
P = Fd(indx).*~A(indx);
b = zeros(m,1);
b(1:mseed) = find(A(indx));
for i = (mseed + 1):m
    C = [0; cumsum(P)];
    r = sum(rand*C(end) >= C);
    b(i) = r;
    P = Fd(indx);
    P(b(1:i)) = 0;
end
b = indx(b);


%% Epipolar Geometry
%
% In this sample:
%
% * We will learn about the basics of multiview geometry
% * We will see what is epipole, epipolar lines, epipolar constraint etc.
%
% Sources:
%
% * <https://docs.opencv.org/3.2.0/da/de9/tutorial_py_epipolar_geometry.html>
%

%% Theory
%
% When we take an image using pin-hole camera, we loose an important
% information, i.e depth of the image, or how far is each point in the image
% from the camera because it is a 3D-to-2D conversion. So it is an important
% question whether we can find the depth information using these cameras. And
% the answer is to use more than one camera. Our eyes works in similar way
% where we use two cameras (two eyes) which is called stereo vision. So let's
% see what OpenCV provides in this field.
%
% (_Learning OpenCV_ by Gary Bradsky has a lot of information in this field.)
%
% Before going to depth images, let's first understand some basic concepts in
% multiview geometry. In this section we will deal with epipolar geometry. See
% the image below which shows a basic setup with two cameras taking the image
% of same scene.
%
% <<https://docs.opencv.org/3.2.0/epipolar.jpg>>
%
% If we are using only the left camera, we can't find the 3D point
% corresponding to the point $x$ in image because every point on the line
% $OX$ projects to the same point on the image plane. But consider the right
% image also. Now different points on the line $OX$ projects to different
% points ($x'$) in right plane. So with these two images, we can triangulate
% the correct 3D point. This is the whole idea.
%
% The projection of the different points on $OX$ form a line on right plane
% (line $l'$). We call it *epiline* corresponding to the point $x$. It means,
% to find the point $x$ on the right image, search along this epiline. It
% should be somewhere on this line. (Think of it this way, to find the
% matching point in other image, you need not search the whole image, just
% search along the epiline. So it provides better performance and accuracy).
% This is called *Epipolar Constraint*. Similarly all points will have its
% corresponding epilines in the other image. The plane $XOO'$ is called
% *Epipolar Plane*.
%
% $O$ and $O'$ are the camera centers. From the setup given above, you can see
% that projection of right camera $O'$ is seen on the left image at the point,
% $e$. It is called the *epipole*. Epipole is the point of intersection of
% line through camera centers and the image planes. Similarly $e'$ is the
% epipole of the left camera. In some cases, you won't be able to locate the
% epipole in the image, they may be outside the image (which means, one camera
% doesn't see the other).
%
% All the epilines pass through its epipole. So to find the location of
% epipole, we can find many epilines and find their intersection point.
%
% So in this session, we focus on finding epipolar lines and epipoles. But to
% find them, we need two more ingredients, *Fundamental Matrix (F)* and
% *Essential Matrix (E)*. Essential Matrix contains the information about
% translation and rotation, which describe the location of the second camera
% relative to the first in global coordinates. See the image below
% (Image courtesy: _Learning OpenCV_ by Gary Bradsky):
%
% <<https://docs.opencv.org/3.2.0/essential_matrix.jpg>>
%
% But we prefer measurements to be done in pixel coordinates, right?
% Fundamental Matrix contains the same information as Essential Matrix in
% addition to the information about the intrinsics of both cameras so that we
% can relate the two cameras in pixel coordinates. (If we are using rectified
% images and normalize the point by dividing by the focal lengths, $F=E$). In
% simple words, Fundamental Matrix F, maps a point in one image to a line
% (epiline) in the other image. This is calculated from matching points from
% both the images. A minimum of 8 such points are required to find the
% fundamental matrix (while using 8-point algorithm). More points are
% preferred and use RANSAC to get a more robust result.
%

%% Code
%
% First we need to find as many possible matches between two images to find
% the fundamental matrix. For this, we use SIFT descriptors with FLANN based
% matcher and ratio test.
%
% Next we find the Fundamental Matrix from the list of best matches from both
% the images.
%
% Then we find the epilines. Epilines corresponding to the points in first
% image are drawn on second image. So mentioning of correct images are
% important here. We get an array of lines. So we define a new function to
% draw these lines on the images.
%
% Below is the result we get:
%
% <<https://docs.opencv.org/3.2.0/epiresult.jpg>>
%
% You can see in the left image that all epilines are converging at a point
% outside the image at right side. That meeting point is the epipole.
%
% For better results, images with good resolution and many non-planar points
% should be used.
%
% Notes:
%
% * One important topic is the forward movement of camera. Then epipoles will
%   be seen at the same locations in both with epilines emerging from a fixed
%   point. See
%   <http://answers.opencv.org/question/17912/location-of-epipole/ this>.
% * Fundamental Matrix estimation is sensitive to quality of matches, outliers
%   etc. It becomes worse when all selected matches lie on the same plane. See
%   <http://answers.opencv.org/question/18125/epilines-not-correct/ this>.
%

function epipolar_geometry_demo()
    %%
    % a pair of stereo images (grayscale)
    img1 = cv.imread(fullfile(mexopencv.root(),'test','books_left.jpg'), ...
        'Grayscale',true);  % query image
    img2 = cv.imread(fullfile(mexopencv.root(),'test','books_right.jpg'), ...
        'Grayscale',true); % train image

    %%
    % detect keypoints and calculate descriptors using SIFT
    obj = cv.SIFT('ConstrastThreshold',0.03);
    [kp1,desc1] = obj.detectAndCompute(img1);
    [kp2,desc2] = obj.detectAndCompute(img2);

    %%
    % match descriptors using FLANN
    matcher = cv.DescriptorMatcher('FlannBasedMatcher', ...
        'Index',{'KDTree', 'Trees',5}, 'Search',{'Checks',50});
    m = matcher.knnMatch(desc1, desc2, 2);

    %%
    % keep only "good" matches (ratio test as per Lowe's paper)
    m = cat(1, m{:});    % N-by-2 array of structs
    idx_good = ([m(:,1).distance] < 0.8*[m(:,2).distance]);
    m = m(idx_good,1);

    %%
    % extract keypoints from filtered matches
    pts1 = cat(1, kp1([m.queryIdx]+1).pt);
    pts2 = cat(1, kp2([m.trainIdx]+1).pt);
    if true
        pts1 = int32(pts1);
        pts2 = int32(pts2);
    end

    %%
    % find Fundamental matrix
    [F,mask] = cv.findFundamentalMat(pts1, pts2, 'Method','LMedS');
    mask = logical(mask);

    %%
    % select only inlier points
    pts1 = pts1(mask,:);
    pts2 = pts2(mask,:);

    %%
    % random colors to draw matching points and lines
    clrs = randi([0 255], [size(pts1,1) 3], 'uint8');
    clrs(:,4) = 0;

    %%
    % find epilines corresponding to points in right image (second image)
    % and draw its lines on left image
    lines1 = cv.computeCorrespondEpilines(pts2, F, 'WhichImage',2);
    [img11,~] = drawlines(img1, img2, lines1, pts1, pts2, clrs);

    %%
    % find epilines corresponding to points in left image (first image)
    % and draw its lines on right image
    lines2 = cv.computeCorrespondEpilines(pts1, F, 'WhichImage',1);
    [img22,~] = drawlines(img2, img1, lines2, pts2, pts1, clrs);

    %%
    % show result
    if ~mexopencv.isOctave() && mexopencv.require('images')
        imshowpair(img11, img22, 'montage')
        title('Left/Right')
    else
        figure('Position',[200 200 1200 400])
        subplot(121), imshow(img11), title('Left')
        subplot(122), imshow(img22), title('Right')
    end
end

function [img1,img2] = drawlines(img1, img2, lines, pts1, pts2, clrs)
    %DRAWLINES  Draw epilines and points on images
    %
    %     [img1,img2] = drawlines(img1, img2, lines, pts1, pts2, clrs)
    %
    % ## Input
    % * __img1__ first image
    % * __img2__ second image
    % * __lines__ epilines corresponding to `pts2` in `img2`
    % * __pts1__ points in `img1`
    % * __pts2__ points in `img2`
    % * __clrs__ color of each line and matching points
    %
    % ## Output
    % * __img1__ image with drawn points and epilines for the points in `img2`
    % * __img2__ image with drawn points
    %
    % Epilines corresponding to the points in 1st image is drawn on 2nd image.
    %
    % See also: estimateFundamentalMatrix, epipolarLine, lineToBorderPoints
    %

    % convert to RGB
    img1 = cv.cvtColor(img1, 'GRAY2RGB');
    img2 = cv.cvtColor(img2, 'GRAY2RGB');

    % epilines
    w = size(img1,2);
    N = size(lines,1);
    p1 = int32([zeros(N,1), -lines(:,3)./lines(:,2)]);
    p2 = int32([ones(N,1)*w, -(lines(:,3)+lines(:,1)*w)./lines(:,2)]);
    img1 = cv.line(img1, p1, p2, 'Colors',clrs, 'LineType','AA');

    % matching points
    img1 = cv.circle(img1, pts1, 5, 'Colors',clrs, 'Thickness','Filled');
    img2 = cv.circle(img2, pts2, 5, 'Colors',clrs, 'Thickness','Filled');
end


% Ryan Steindl based on Robotics Toolbox for MATLAB (v6 and v9)
%
% Copyright (C) 1993-2011, 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
function q = interp(Q1, Q2, r)
%Quaternion.interp Interpolate rotations expressed by quaternion objects
%
% QI = Q1.interp(Q2, R) is a unit-quaternion that interpolates between Q1 for R=0
% to Q2 for R=1. This is a spherical linear interpolation (slerp) that can be
% interpretted as interpolation along a great circle arc on a sphere.
%
% If R is a vector QI is a vector of quaternions, each element
% corresponding to sequential elements of R.
%
% Notes:
% - the value of r is clipped to the interval 0 to 1
%
% See also ctraj, Quaternion.scale.

    q1 = double(Q1);
    q2 = double(Q2);

    theta = acos(q1*q2');
    count = 1;

    % clip values of r
    r(r<0) = 0;
    r(r>1) = 1;

    if length(r) == 1
        if theta == 0
            q = Q1;
        else
            q = Quaternion( (sin((1-r)*theta) * q1 + sin(r*theta) * q2) / sin(theta) );
        end
    else
        for R=r(:)'
            if theta == 0
                qq = Q1;
            else
                qq = Quaternion( (sin((1-R)*theta) * q1 + sin(R*theta) * q2) / sin(theta) );
            end
            q(count) = qq;
            count = count + 1;
        end
    end
end


function tapas_hgf_jget_plotTraj(r)
% Plots the estimated trajectories for the HGF perceptual model for
% the JGET project
% Usage example:  est = tapas_fitModel(responses, inputs); tapas_hgf_plotTraj(est);
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2013 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/>.

% Optional plotting of standard deviations (true or false)
plotsd = true;

% Set up display
scrsz = get(0,'screenSize');
outerpos = [0.2*scrsz(3),0.2*scrsz(4),0.8*scrsz(3),0.8*scrsz(4)];
figure(...
    'OuterPosition', outerpos,...
    'Name', 'HGF trajectories');

% Time axis
if size(r.u,2) > 1 && ~isempty(find(strcmp(fieldnames(r.c_prc),'irregular_intervals'))) && r.c_prc.irregular_intervals
    t = r.u(:,end)';
else
    t = ones(1,size(r.u,1));
end

ts = cumsum(t);
ts = [0, ts];

% Do we know the generative parameters?
if size(r.u,2) > 2
    genpar = true;
    mean   = r.u(:,2);
    sd     = r.u(:,3);
else
    genpar = false;
end

% Number of levels
try
    l = r.c_prc.n_levels;
catch
    l = length(r.p_prc.p)/8;
end

% Upper levels
for j = 1:l-1

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Left subplot (x)                       %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    subplot(l+1,2,2*j-1);

    if plotsd == true
        upperprior = r.p_prc.mux_0(l-j+1) +1.96*sqrt(r.p_prc.sax_0(l-j+1));
        lowerprior = r.p_prc.mux_0(l-j+1) -1.96*sqrt(r.p_prc.sax_0(l-j+1));
        upper = [upperprior; r.traj.mux(:,l-j+1)+1.96*sqrt(r.traj.sax(:,l-j+1))];
        lower = [lowerprior; r.traj.mux(:,l-j+1)-1.96*sqrt(r.traj.sax(:,l-j+1))];

        plot(0, upperprior, 'ob', 'LineWidth', 1);
        hold all;
        plot(0, lowerprior, 'ob', 'LineWidth', 1);
        fill([ts, fliplr(ts)], [(upper)', fliplr((lower)')], ...
             'b', 'EdgeAlpha', 0, 'FaceAlpha', 0.15);
    end
    plot(ts, [r.p_prc.mux_0(l-j+1); r.traj.mux(:,l-j+1)], 'b', 'LineWidth', 1.5);
    hold all;
    plot(0, r.p_prc.mux_0(l-j+1), 'ob', 'LineWidth', 1.5); % prior
    xlim([0 ts(end)]);
    title(['Posterior expectation of x_' num2str(l-j+1)], 'FontWeight', 'bold');
    ylabel(['\mu x_', num2str(l-j+1)]);

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Right subplot (alpha)                  %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    subplot(l+1,2,2*j);

    if plotsd == true
        upperprior = r.p_prc.mua_0(l-j+1) +1.96*sqrt(r.p_prc.saa_0(l-j+1));
        lowerprior = r.p_prc.mua_0(l-j+1) -1.96*sqrt(r.p_prc.saa_0(l-j+1));
        upper = [upperprior; r.traj.mua(:,l-j+1)+1.96*sqrt(r.traj.saa(:,l-j+1))];
        lower = [lowerprior; r.traj.mua(:,l-j+1)-1.96*sqrt(r.traj.saa(:,l-j+1))];

        plot(0, upperprior, 'ob', 'LineWidth', 1);
        hold all;
        plot(0, lowerprior, 'ob', 'LineWidth', 1);
        fill([ts, fliplr(ts)], [(upper)', fliplr((lower)')], ...
             'b', 'EdgeAlpha', 0, 'FaceAlpha', 0.15);
    end
    plot(ts, [r.p_prc.mua_0(l-j+1); r.traj.mua(:,l-j+1)], 'b', 'LineWidth', 1.5);
    hold all;
    plot(0, r.p_prc.mua_0(l-j+1), 'ob', 'LineWidth', 1.5); % prior
    xlim([0 ts(end)]);
    title(['Posterior expectation of \alpha_' num2str(l-j+1)], 'FontWeight', 'bold');
    ylabel(['\mu \alpha_', num2str(l-j+1)]);
end

% Input level
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Left subplot (x)                       %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
subplot(l+1,2,2*l-1);

if plotsd == true
    upperprior = r.p_prc.mux_0(1) +1.96*sqrt(r.p_prc.sax_0(1));
    lowerprior = r.p_prc.mux_0(1) -1.96*sqrt(r.p_prc.sax_0(1));
    upper = [upperprior; r.traj.mux(:,1)+1.96*sqrt(r.traj.sax(:,1))];
    lower = [lowerprior; r.traj.mux(:,1)-1.96*sqrt(r.traj.sax(:,1))];

    plot(0, upperprior, 'or', 'LineWidth', 1);
    hold all;
    plot(0, lowerprior, 'or', 'LineWidth', 1);
    fill([ts, fliplr(ts)], [(upper)', fliplr((lower)')], ...
         'r', 'EdgeAlpha', 0, 'FaceAlpha', 0.15);
end
plot(ts, [r.p_prc.mux_0(1); r.traj.mux(:,1)], 'r', 'LineWidth', 1.5);
hold all;
plot(0, r.p_prc.mux_0(1), 'or', 'LineWidth', 1.5); % prior
plot(ts(2:end), r.u(:,1), '.', 'Color', [0 0.6 0]); % inputs
if genpar
    plot(ts(2:end), mean, '-', 'Color', 'k', 'LineWidth', 1); % mean of input distribution
    plot(ts(2:end), mean +1.96.*sd, '--', 'Color', 'k', 'LineWidth', 1); % 95% interval of input distribution
    plot(ts(2:end), mean -1.96.*sd, '--', 'Color', 'k', 'LineWidth', 1); % 95% interval of input distribution
end
xlim([0 ts(end)]);
title(['Input u (green) and posterior expectation of x_1 (red) for \kappa_x=', ...
       num2str(r.p_prc.kax), ', \omega_x=', num2str(r.p_prc.omx)], 'FontWeight', 'bold');
ylabel('u, \mu x_1');
hold off;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Right subplot (alpha)                  %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
subplot(l+1,2,2*l);

if plotsd == true
    upperprior = r.p_prc.mua_0(1) +1.96*sqrt(r.p_prc.saa_0(1));
    lowerprior = r.p_prc.mua_0(1) -1.96*sqrt(r.p_prc.saa_0(1));
    upper = [upperprior; r.traj.mua(:,1)+1.96*sqrt(r.traj.saa(:,1))];
    lower = [lowerprior; r.traj.mua(:,1)-1.96*sqrt(r.traj.saa(:,1))];

    transupperprior = sqrt(exp(r.p_prc.kau *upperprior +r.p_prc.omu));
    translowerprior = sqrt(exp(r.p_prc.kau *lowerprior +r.p_prc.omu));
    transupper = sqrt(exp(r.p_prc.kau *upper +r.p_prc.omu));
    translower = sqrt(exp(r.p_prc.kau *lower +r.p_prc.omu));

    plot(0, transupperprior, 'or', 'LineWidth', 1);
    hold all;
    plot(0, translowerprior, 'or', 'LineWidth', 1);
    fill([ts, fliplr(ts)], [(transupper)', fliplr((translower)')], ...
         'r', 'EdgeAlpha', 0, 'FaceAlpha', 0.15);
end
transmuaprior = sqrt(exp(r.p_prc.kau *r.p_prc.mua_0(1) +r.p_prc.omu));
plot(ts, [transmuaprior; sqrt(exp(r.p_prc.kau *r.traj.mua(:,1) +r.p_prc.omu))], 'r', 'LineWidth', 1.5);
hold all;
plot(0, transmuaprior, 'or', 'LineWidth', 1.5); % prior
if genpar
    plot(ts(2:end), sd, '--', 'Color', 'k', 'LineWidth', 1);
end
xlim([0 ts(end)]);
title(['Belief on noise (red) for \kappa_\alpha=', ...
       num2str(r.p_prc.kaa), ', \omega_\alpha=', num2str(r.p_prc.oma)], 'FontWeight', 'bold');
ylabel('\mu \alpha_1');
hold off;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Decision model                         %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
subplot(l+1,2,2*l+1);

if plotsd == true
    upper = r.traj.muxhat(:,1)+1.96*sqrt(r.p_obs.ze +r.traj.saxhat(:,1));
    lower = r.traj.muxhat(:,1)-1.96*sqrt(r.p_obs.ze +r.traj.saxhat(:,1));

    fill([ts(2:end), fliplr(ts(2:end))], [(upper)', fliplr((lower)')], ...
         'r', 'EdgeAlpha', 0, 'FaceAlpha', 0.15);
    hold all;
end
plot(ts(2:end), r.traj.muxhat(:,1), 'Color', [153/256 17/256 153/256], 'LineWidth', 1.5);
hold all;
plot(ts(2:end), r.y(:,1), '.', 'Color', [1 0.65 0], 'MarkerSize', 15); % responses
if genpar
    plot(ts(2:end), mean, '-', 'Color', 'k', 'LineWidth', 1); % mean of input distribution
    plot(ts(2:end), mean +1.96.*sd, '--', 'Color', 'k', 'LineWidth', 1); % 95% interval of input distribution
    plot(ts(2:end), mean -1.96.*sd, '--', 'Color', 'k', 'LineWidth', 1); % 95% interval of input distribution
end
xlim([1 ts(end)]);
title('Decision model: prediction of decision (purple) and decision (orange)', 'FontWeight', 'bold');
ylabel('y, \^{\mu} x_1');
xlabel({'Trial number', ' '}); % A hack to get the relative subplot sizes right
hold off;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Learning rate                         %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if genpar
subplot(l+1,2,2*l+2);
[AX, H1, H2 ] = plotyy(ts(2:end), mean, ts(2:end), r.traj.lrx(:,1));
hold all;
ylim(AX(1), [min(mean -1.96.*sd-3), max(mean +1.96.*sd+3)]);
plot(AX(1), ts(2:end), mean +1.96.*sd, '--', 'Color', 'k', 'LineWidth', 1.1); % 95% interval of input distribution
plot(AX(1), ts(2:end), mean -1.96.*sd, '--', 'Color', 'k', 'LineWidth', 1.1); % 95% interval of input distribution
set(H1, 'Color', 'k', 'LineWidth', 1.1);
set(H2, 'Color', [178/256, 34/256, 34/256], 'LineWidth', 1.5);
set(AX(1), 'YColor', 'k');
set(AX(2), 'YColor', 'k');
xlim(AX(1), [1 ts(end)]);
xlim(AX(2), [1 ts(end)]);
title('Learning rate (bordeaux) and input sampling distribution (black)', 'FontWeight', 'bold');
ylabel(AX(1), 'Input');
ylabel(AX(2), '\sigma_x');
xlabel('Trial number');
hold off;
end


% MP_SP_DRCHLT_L2_PROJ_UDOTN: assign the normal degrees of freedom trough an L2 projection for a multipatch geometry.
%  To be used with the 'RT' and 'NDL' spaces. The imposed condition reads   u \cdot n = h \cdot n
%
%   [vel, normal_dofs] = mp_sp_drchlt_l2_proj_udotn (space, msh, gnum, ornt, bnd_sides, bnd_func)
%
% INPUTS:
%
%    space:     space object (see sp_vector_div_transform)
%    msh:       mesh object (see msh_cartesian)
%    gnum:      global numbering of the degrees of freedom (see mp_interface_hdiv)
%    ornt:      global orientation of the degrees of freedom (see mp_interface_hdiv)
%  boundaries: array of structures containing the information for the boundaries (see mp_geo_load)
%    bnd_sides: boundary sides on which the Dirichlet condition is imposed
%    bnd_func:  the condition to be imposed (h in the equation)
%
% OUTPUT:
%
%     vel:         assigned value to the normal degrees of freedom
%     normal_dofs: global numbering of the normal basis functions
%
% Copyright (C) 2014, 2015 Rafael Vazquez
%
%    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 [u, normal_dofs] = mp_sp_drchlt_l2_proj_udotn (space, msh, gnum, ornt, boundaries, refs, bnd_func)

  if (isa (space, 'sp_multipatch'))
    warning ('For spaces of the class SP_MULTIPATCH, using the function SP_DRCHLT_PROJ_L2_UDOTN inside the class')
    [u, dofs] = sp_drchlt_proj_l2_udotn (sp, msh, refs, bnd_func);
    return
  end

  ndof = max ([gnum{:}]);
  M = spalloc (ndof, ndof, 3*ndof);
  rhs = zeros (ndof, 1);

  normal_dofs = [];
  for iref = refs
    for bnd_side = 1:boundaries(iref).nsides
      iptc = boundaries(iref).patches(bnd_side);
      iside = boundaries(iref).faces(bnd_side);

      if (strcmpi (space{iptc}.transform, 'div-preserving'))
        msh_side = msh_eval_boundary_side (msh{iptc}, iside);
        sp_side = sp_eval_boundary_side (space{iptc}, msh_side);
      else
        error ('The function only works with div-conforming spaces')
      end

      normal_dofs = union (normal_dofs, gnum{iptc}(sp_side.dofs));
      for idim = 1:msh{iptc}.rdim
        x{idim} = reshape (msh_side.geo_map(idim,:,:), msh_side.nqn, msh_side.nel);
      end
      g = bnd_func (x{:}, iref);

      M_loc = op_u_v (sp_side, sp_side, msh_side, ones(size(x{1})));
      rhs_loc = op_fdotn_v (sp_side, msh_side, g);

      global_dofs = gnum{iptc}(sp_side.dofs);
      M(global_dofs, global_dofs) = M(global_dofs, global_dofs) + M_loc;
      rhs(global_dofs) = rhs(global_dofs) + rhs_loc;
    end
  end

  u = M(normal_dofs, normal_dofs) \ rhs(normal_dofs);

end


function [image_information_metric, rniirs, diagnostic] = RGIQE(filename, varargin)
% RGIQE Radar generalized image quality equation
%    [image_information_metric, rniirs] = RGIQE(bandwidth, nesz_db, graze_degrees)
%    [image_information_metric, rniirs] = RGIQE(filename, 'PropertyName', PropertyValue, ...)
%
% An implementation of a generalized image quality equation to provide a
% quantitative image quality metric and an RNIIRS prediction.
%
% RNIIRS (Radar National Imagery Interpretation Rating Scale) is an image
% quality scale from 1 to 9.  This "quality" refers to local image fidelity
% and a users ability to perform certain detection and recognition tasks
% with an image.  The "quality" that RNIIRS attempts to capture does NOT
% refer to image size, geopositioning accuracy, radiometric accuracy, or
% any other host of characterists that may improve an image's utility.
% Although RNIIRS is subjective, generally assessed by a trained analyst,
% an image quality equation (IQE) may be used to predict RNIIRS for a given
% SAR collect.
%
% The General Image Quality Equation (GIQE) provided here uses generic,
% fundamental, sensor-indepenedent parameters of radar collection to
% measure local image fidelity by quantifying the information density in
% the ground in terms of bits\m^2, using Shannon's channel capacity
% theorem.  Unlike RNIIRS, this value is quantitative and not subjective or
% based on analysts' assessments.  We can however compute an RNIIRS
% prediction based off of it by fitting the information metric to analysts
% ratings.  Both the information theory metric and the RNIIRS prediction
% are computed in this function.
%
%    [image_information_metric, rniirs] = RGIQE(bandwidth, nesz_db, graze_degrees)
% INPUTS:
%
%    bandwidth               Transmitted (and processed) bandwidth in Hz.
%    nesz_db                 Noise equivalent sigma-0 in dB.  Should
%                               capture all noise, including multiplicative
%                               sources.
%    graze_degrees           Grazing angle in degrees.
%
%
%    [image_information_metric, rniirs] = RGIQE(filename, 'PropertyName', PropertyValue, ...)
% INPUTS:
%
%    filename                Must either be a complex image in a format
%                               recognized by the MATLAB SAR Toolbox or an
%                               XML file with SICD-formatted XML.  If empty
%                               or not passed, the user will be prompted to
%                               select a file with a dialog box.
%
%    Property name           Description
%    multi_noise             Multiplicative noise.  This value is equal to:
%
%                               MNR * sigma_0_background
%
%                               where MNR is the multiplicative noise ratio
%                               and sigma_0_background is the assumed power
%                               level of the background and ambiguous
%                               areas.
%                               MNR can further be broken down into:
%
%                               MNR = ISLR + QNR + AMBR
%
%                               where ISLR is the integrated sidelobe
%                               ratio, QNR is the quantization noise ratio,
%                               and AMBR is the ambiguity ratio.
%                               (Default = 0)
%    interactive             "AUTO", "NOISE", "SIGNAL", "BOTH", or "NONE".
%                               Enable interactive measurement of noise and
%                               signal levels.  "AUTO" only runs
%                               interactive measurment if metadata from
%                               source file does not provide it. (Default
%                               is "AUTO").
%    frames                  If a file contains more than one complex
%                               dataset, this value specifies for which of
%                               those datasets to compute metrics. (Default
%                               is all datasets in the file.)
%    signal_sigma_override   Allows caller to override signal level used
%                               for computation (and INTERACTIVE input
%                               parameter).  In general, this is not
%                               recommended, as RNIIRS estimation function
%                               was trained using the default values. If
%                               the dataset provided is radiometrically
%                               calibrated, this value will be treated as
%                               calibrated signal level in the ground plane
%                               (sigma-0).  If the dataset is not
%                               calibrated, this value will be assume to be
%                               average pixel power. (Default is 1 square
%                               meter of radar return per square meter on
%                               the ground for co-pol, 0.25 for cross-pol.)
%
% Note that the selection of signal level and noise level are fundamentally
% different in this model.  Noise is measureable, quantifiable, and likely
% fairy accuractely predictable (at least for additive noise).  There is a
% right answer for what the noise level is for any given dataset. On the
% other hand, signal level is somewhat arbitrarily chosen, selected to be
% the power level of the scene content that one may be most interested in
% examining.  It is for this reason that this function allows for a signal
% level override, but not a direct noise level override (although noise can
% be adjusted with the MULTI_NOISE input parameter.)
%
% Author: Wade Schwartzkopf, NGA/Research

% Of the lines in this file, >95% is file and input parameter parsing and
% documention, and <5% actually computing the image quality metrics...

%% Bandwidth formulation
% Assumes azimuth bandwidth to match range in ground plane
% NESZ provided should capture all noise, including multiplicative sources
if nargin == 3 && isnumeric(filename) && isnumeric(varargin{1}) && isnumeric(varargin{2})
    bandwidth = filename;  % Hz
    nesz = varargin{1};  % in dB
    graze = varargin{2};  % in degrees
    bandwidth = bandwidth*2/SPEED_OF_LIGHT;  % Convert units from Hz to cycles/m
    bandwidth = bandwidth*cosd(graze);  % Convert from slant plane to ground plane
    bandwidth = bandwidth.^2;  % Convert from 1D bandwidth to 2D bandwidth area (assumes azimuth bandwidth same as range)
    image_information_metric = bandwidth.*log2(1+1/(10^(nesz/10)));
    rniirs = estimate_rniirs(image_information_metric);
    diagnostic = struct();  % No diagnostics for this version
    return;
end

%% SICD/SLC version.
%% Open input file
if ((nargin<1)||isempty(filename)) % If no filename was give, use dialog box to ask for one
    % Recall last interactively selected path used
    if ispref('matlab_sar_toolbox','last_used_directory')
        pathname = getpref('matlab_sar_toolbox','last_used_directory');
        if ~ischar(pathname)||~exist(pathname,'dir')
            pathname = pwd;
        end
    else
        pathname = pwd;
    end
    [filename,pathname]=uigetfile(sar_file_extensions(),...
        'Open SAR Data File',pathname);
    setpref('matlab_sar_toolbox','last_used_directory',pathname);
else % Path was already passed in with filenames
    pathname='';
end
if(filename)
    fullfilename=[pathname filename];
else % filename=0.  Cancel was pressed, instead of a file being chosen.
    return;
end
if ~isempty(pathname)
    setpref('matlab_sar_toolbox','last_used_directory',pathname); %store path
end
try
    ro = open_reader(fullfilename);
    % Check for multiple images
    if ~iscell(ro)
        ro = {ro};
    end
    meta = cell(numel(ro),1);
    for i = 1:numel(ro)
        meta{i} = ro{i}.get_meta();
    end
catch
    try
        fid = fopen(fullfilename);
        meta = {sicdxml2struct(xmlread( java.io.StringBufferInputStream( char(fread(fid)'))))};
        fclose(fid);
    catch
        error('RGIQE:InvalidFile','Invalid File.');
    end
end

%% Parse other input parameters
p = inputParser;
p.addParameter('multi_noise', 0, @(x) isnumeric(x) && isscalar(x));
p.addParameter('interactive', 'AUTO', @(x) ismember(upper(x), ...
    {'AUTO', 'NOISE', 'SIGNAL', 'BOTH', 'NONE'}));
p.addParameter('frames', 1:numel(meta), @(x) all(ismember(x, 1:numel(meta))));
p.addParameter('signal_sigma_override', [], @(x) isnumeric(x) && isscalar(x));
parse(p,varargin{:})

%% Iterate through datasets provided
meta = meta(p.Results.frames);
image_information_metric = zeros(numel(meta),1);
rniirs = zeros(numel(meta),1);
for i = 1:numel(meta)
    meta{i} = derived_sicd_fields(meta{i});  % Assure all radiometric fields are derived if not there

    %% Get noise level
    if isfield(meta{i}, 'Radiometric') && ...
            isfield(meta{i}.Radiometric,'NoiseLevel') && ...
            isfield(meta{i}.Radiometric.NoiseLevel,'NoiseLevelType') && ...
            strcmpi(meta{i}.Radiometric.NoiseLevel.NoiseLevelType,'ABSOLUTE') && ...
            isfield(meta{i}.Radiometric.NoiseLevel,'NoisePoly') && ...
            ~ismember(upper(p.Results.interactive), {'NOISE', 'BOTH'})
        % SICD metadata can only tell us additive noise.  Estimation of
        % multiplicative noise must come from elsewhere.
        noise_sigma = 10^(meta{i}.Radiometric.NoiseLevel.NoisePoly(1)/10) + p.Results.multi_noise;
    elseif ismember(upper(p.Results.interactive), {'AUTO', 'NOISE', 'BOTH'})
        % Measured noise contains both additive and multiplicative noise
        uiwait(msgbox('Please select a noise region for measurement.','Noise measurement'));
        aoi = mitm_viewer(fullfilename,'mode','aoi','closeAfterSelect',true,'initialFrame',i);
        if isempty(aoi), return; end  % Cancel was selected
        cdata = double(ro{i}.read_chip(...
            [aoi(1) aoi(1)+aoi(3)-1],[aoi(2) aoi(2)+aoi(4)-1]));
        noise_sigma = mean(abs(cdata(:)).^2);
    else
        noise_sigma = NaN;
    end
    if isfield(meta{i}, 'Radiometric') && ...
            isfield(meta{i}.Radiometric, 'SigmaZeroSFPoly')
        % Scale to be equal to noise equivalent sigma zero
        noise_sigma = meta{i}.Radiometric.SigmaZeroSFPoly(1) * noise_sigma;
    end

    %% Get signal level
    % One issue with using the Shannon-Hartley channel capacity formulation
    % is that it requires a signal level.  Certainly the amount of
    % information a SAR image conveys is related to its signal level, so
    % this is how we hope it would behave.  A target with brighter returns
    % is always easier to interpret than a target that is in the noise.
    % However, it presents a problem to image quality prediction, since the
    % signal level can't be known prior to measurement.  This constraint is
    % implicitly true for traditional RNIIRS assessments as well, which
    % assumes specific types of targets.
    % One approach for setting the signal level used in the channel
    % capacity equation would be to assume a fixed radiometric signal level
    % that is representative of the sorts of scene content we are generally
    % interested in and use that same signal level across all images for
    % which we compute this metric.  This allows for an even
    % apples-to-apples comparison across images of varying content and is
    % consistent with the traditional RNIIRS viewpoint, which assumes
    % specific types of tasks and scene content.
    % Another approach would be to use the signal level in the actual scene
    % by measuring the average pixel power across a full image or AOI. This
    % approach may be more accurate at measuring the true information in
    % any given image area.  However, it may be less consistent with the
    % traditional RNIIRS viewpoint, and this approach can only be used
    % after the collection has been made, not prior to it for planning
    % collection and predicting image quality.
    % We also allow the caller of the function to pass their own signal
    % level. Passing a non-default signal level is not recommended for
    % RNIIRS estimation, since the RNIIRS function was trained on the
    % default values.  However, we provide this flexibility in case it
    % might be educational to see how it affects the information theory
    % metric.
    if ~ismember('signal_sigma_override', p.UsingDefaults)
        signal_sigma = p.Results.signal_sigma_override;
    elseif isfield(meta{i}, 'Radiometric') && ...
            isfield(meta{i}.Radiometric, 'SigmaZeroSFPoly') && ...
            ~ismember(upper(p.Results.interactive), {'SIGNAL', 'BOTH'})
        % Default value: 1 square meter of return per square meter on
        % the ground (roughly similar to a large vehicle over its area).
        % Ulaby plots show cross-pol generally 5-10dB lower than
        % co-pol.  We pick 1/4 as simple round number.
        if isfield(meta{i},'ImageFormation') && ...
                isfield(meta{i}.ImageFormation,'TxRcvPolarizationProc') && ...
                numel(unique(split(meta{i}.ImageFormation.TxRcvPolarizationProc,':')))>1
            signal_sigma = 0.25;
        else
            signal_sigma = 1;
        end
    elseif ismember(upper(p.Results.interactive), {'AUTO', 'SIGNAL', 'BOTH'})
        ButtonName = questdlg(['Do you want to use average signal '...
            'power across entire image or select an AOI?'], ...
            'Signal level estimation', ...
            'Image', 'AOI', 'Image');
        if isempty(ButtonName), return; end %window was closed CANCEL
        if strcmpi(ButtonName,'AOI')
            aoi = mitm_viewer(fullfilename,'mode','aoi','closeAfterSelect',true,'initialFrame',i);
            if isempty(aoi), return; end  % Cancel was selected
            cdata = double(ro{i}.read_chip(...
                [aoi(1) aoi(1)+aoi(3)-1],[aoi(2) aoi(2)+aoi(4)-1]));
            signal_sigma = mean(abs(cdata(:)).^2) - noise_sigma;
        else
            samplesize=[1000 1000]; % Exract 1000x1000 array of samples to estimate mean
            subsample=ceil(double([meta{i}.ImageData.NumCols meta{i}.ImageData.NumRows])./samplesize);
            data = abs(single(ro{i}.read_chip([1 meta{i}.ImageData.NumCols],...
                    [1 meta{i}.ImageData.NumRows],subsample)));
            signal_sigma = mean(abs(data(:)).^2) - noise_sigma;
        end
        if isfield(meta{i}, 'Radiometric') && ...
                isfield(meta{i}.Radiometric, 'SigmaZeroSFPoly')
            % Scale to be equal to noise equivalent sigma zero
            signal_sigma = meta{i}.Radiometric.SigmaZeroSFPoly * signal_sigma;
        end
        warning('RGIQE:SignalLevelMeasured',['More uncertainty with ' ...
            'RNIIRS estimates exists when signal level is sampled, ' ...
            'rather than when using a radiometric constant.']);
    else
        signal_sigma = NaN;
    end

    %% Save some diagnostic statistics
    if isfield(meta{i}, 'Radiometric') && ...
            isfield(meta{i}.Radiometric, 'SigmaZeroSFPoly')
        diagnostic.nesz(i) = 10*log10(noise_sigma);
    end
    diagnostic.snr(i) = signal_sigma/noise_sigma;
    diagnostic.sp_resolution(i) = sqrt(meta{i}.Grid.Row.ImpRespWid * ...
        meta{i}.Grid.Col.ImpRespWid);
    diagnostic.ellipicity(i) = max(meta{i}.Grid.Row.ImpRespWid, ...
        meta{i}.Grid.Col.ImpRespWid)/min(meta{i}.Grid.Row.ImpRespWid, ...
        meta{i}.Grid.Col.ImpRespWid);
    if numel(meta)>1
        diagnostic.meta{i} = meta{i};
    else
        diagnostic.meta = meta{i};
    end

    %% Compute bandwidth area
    % The cosine of the slope angle is proper scale factor to project the
    % bandwidth area into the ground plane.
    diagnostic.bandwidth_area(i) = ...
        meta{i}.Grid.Col.ImpRespBW * meta{i}.Grid.Row.ImpRespBW * ...
        cosd(meta{i}.SCPCOA.SlopeAng);
    % Computing area from vertices is much more work, but it may help
    % visualize.
    % Project slant plane spatial frequency bounds to ground plane
    % coords_slant_x = meta{i}.Grid.Col.KCtr + ...
    %     meta{i}.Grid.Col.ImpRespBW * [-1 1 1 -1 -1]/2;
    % coords_slant_y = meta{i}.Grid.Row.KCtr + ...
    %     meta{i}.Grid.Row.ImpRespBW * [1 1 -1 -1 1]/2;
    % ruvect = [meta{i}.Grid.Row.UVectECF.X meta{i}.Grid.Row.UVectECF.Y meta{i}.Grid.Row.UVectECF.Z];
    % cuvect = [meta{i}.Grid.Col.UVectECF.X meta{i}.Grid.Col.UVectECF.Y meta{i}.Grid.Col.UVectECF.Z];
    % coords_slant_3d = ruvect'*coords_slant_y + cuvect'*coords_slant_x;
    % Compute basis vectors for ground plane
    % gpn = wgs_84_norm([meta{i}.GeoData.SCP.ECF.X meta{i}.GeoData.SCP.ECF.Y meta{i}.GeoData.SCP.ECF.Z])';
    % gruvect = project_ground(ruvect, gpn); % Project range vector to ground plane
    % gruvect = gruvect/norm(gruvect);
    % gcuvect = cross(gruvect,gpn);
    % gcuvect = gcuvect/norm(gcuvect);
    % Project spatial frequencies to ground plane
    % coords_ground_3d = coords_slant_3d;
    % coords_ground_x = zeros(size(coords_slant_x));
    % coords_ground_y = zeros(size(coords_slant_y));
    % for j=1:numel(coords_slant_x)
    %     % Planes in ECF
    %     coords_ground_3d(:,j) = project_ground(coords_slant_3d(:,j)',gpn)';
    %     % Convert to 2D basis vectors in ground plane
    %     coords_ground_x(j) = dot(coords_ground_3d(:,j),gcuvect);
    %     coords_ground_y(j) = dot(coords_ground_3d(:,j),gruvect);
    % end
    % This is a way to compute the area of a any polygon from its vertices.
    % diagnostic.bandwidth_area = sum((coords_ground_x(1:(end-1)).*coords_ground_y(2:end)) - ...
    %     (coords_ground_y(1:(end-1)).*coords_ground_x(2:end)))/2;
    % Visualize spatial frequency projections in slant and ground planes
    % figure;
    % title('Spatial frequency bounds 3D');
    % g_plane = gruvect'*[100 100 -100 -100 100] + gcuvect'*[-100 100 100 -100 -100];
    % plot3(coords_slant_3d(1,:),coords_slant_3d(2,:),coords_slant_3d(3,:),...
    %     coords_ground_3d(1,:),coords_ground_3d(2,:),coords_ground_3d(3,:),...
    %     g_plane(1,:),g_plane(2,:),g_plane(3,:));
    % legend('Slant plane bounds', 'Ground plane bounds', 'Ground plane')
    % figure;
    % plot(coords_slant_x, coords_slant_y, coords_ground_x, coords_ground_y);
    % title('Spatial frequency bounds');
    % legend('Slant plane bounds', 'Ground plane bounds')

    %% After 300+ line of setup code and comments, we finally compute quality metric...
    % Shannon-Hartley theorem of channel capacity
    % C = B * log2(1 + (S/N))
    % We propose a 2-dimension information rate based on a 2-dimensional
    % bandwidth.  This 2D bandwidth is an area in a space where each axis
    % is cycles/m, as opposed to 1D distance in Hz (cycles per second).
    % Therefore the units of this metric are bits/m^2.
    image_information_metric(i) = diagnostic.bandwidth_area(i) * log2(1 + diagnostic.snr(i));

    %% Map bits/m^2 into RNIIRS
    if ismember('signal_sigma_override', p.UsingDefaults)
        rniirs(i) = estimate_rniirs(image_information_metric(i));
    else  % If assumed signal is not default, this RNIIRS mapping is not valid
        rniirs(i) = NaN;
    end
end
try
    for i = 1:numel(meta)
        ro{i}.close();
    end
end

end

function rniirs = estimate_rniirs(image_information_metric)
    % The coefficients here were derived by fitting hundreds of SAR
    % datasets with varying parameters to analysts ratings.  This
    % author prefers the information metric but also provides this as
    % well for comparison to this legacy scale.
    % coeffs = [0.3960 3.7555];  % Probably more digits of precision than we really have
    coeffs = [0.4357 3.4761];  % Coefficients were updated by NGA NIQU (Feb 1, 2022) to more accruately represent RNIIRS tables
    rniirs = polyval(coeffs,log2(image_information_metric));
    % For completeness, we handle the very low RNIIRS case where the
    % logarithmic equation would have been negative.  Negative is not
    % allowed in the RNIIRS scale. We compute where a line tangent to
    % the bits/m^2-to-RNIIRS function intersects with the origin.  We
    % will use this tangent line as the mapping for very low RNIIRS,
    % since it approaches zero as information approaches zero and
    % remains non-negative. This way the entire mapping function
    % between bits/m^2 and RNIIRS is continuous and has a continuous
    % derivative.  The function will be logarithmic with respect to the
    % information metric above this point (presumably nearly all data
    % will fit in here) and linear with respect to the metric below
    % this point (extremely low RNIIRS).  Note that since NGA has no
    % precedent or analysts ratings for RNIIRS<1, we are really free to
    % define this however we want.
    linlog_transition = exp(1 - coeffs(2)*log(2)/coeffs(1));
    % Derivative of linear portion (as well as intersection with log portion)
    d_rniirs = coeffs(1)/(log(2)*linlog_transition);
    % This is what the two lines would look line at very low RNIIRS:
    % x=linspace(0,.02,1000);
    % figure; plot(x, polyval(coeffs,log2(x)), x, x*d_rniirs);
    % That was a lot of explanation for a case that will likely rarely
    % be used, wasn't it?
    if image_information_metric<linlog_transition
        rniirs = d_rniirs * image_information_metric;
    end
end

function out = project_ground(point, gpn)
    % Projection of a point along a given direction to a plane is
    % just the intersection of the line defined by that point (l0)
    % and direction (l) and the plane defined by a point in the
    % plane (p0) and the normal (p):
    % l0 - ((l0 - p0).p/(l.p))*l
    % where . represents the dot product.
    % In this specific case, the origin is in the plane, and the projection
    % direction is the same as the plane normal, so this equation is much
    % simplified:
    out = point - dot(point,gpn)*gpn;
end


function    g = lfmGradientUpsilon(gamma,sigma2,gradThetaGamma,t1, t2, mode)

% LFMGRADIENTUPSILON Gradient of the function \upsilon(z) with respect to
% one of the hyperparameters of the kernel: m_k, C_k, D_k, m_r, C_r or D_r.
% FORMAT
% DESC Computes the gradient of the function \upsilon(z) with respect to
% one of the parameters of the system (mass, spring or damper).
% ARG gamma : Gamma value of the system.
% ARG sigma2 : length scale of latent process.
% ARG gradThetaGamma : Vector with the gradient of gamma1 and gamma2 with
% respect to the desired parameter.
% ARG Tt1 : first time input (number of time points 1 x number of time points 2).
% ARG Tt2 : second time input (number of time points 1 x number of time points 2).
% ARG mode : indicates in which way the variables t1 and t2 must be
% transposed
% RETURN g : Gradient of the kernel with respect to the desired
% parameter.
%
% COPYRIGHT : David Luengo, Mauricio Alvarez 2008
%
% SEEALSO : lfmKernGradient, lfmXlfmKernGradient, lfmGradientH

% KERN

%Parameters of the function

sigma = sqrt(sigma2);

% Initialization of vectors and matrices

switch mode
    case 1
        % In this mode the Tt1 is actually the Tt1 and Tt2 is actually Tt2
        % Z1 is a matrix with order given by Tt1 and z2 is a vector with
        % order given by Tt2
        Tt11 = repmat(t1, 1, size(t2, 1));
        Tt22 = repmat(t2', size(t1, 1), 1);
        Z1 = (Tt11-Tt22)/sigma - sigma*gamma/2;
        z2 = t2/sigma + sigma*gamma/2;
        Z2 = repmat(z2.', size(t1, 1), 1);
        [preFactorZ1, gradPreFactorZ1] = quadrantWofzGrad(Z1, sigma,1);
        [preFactorz2, gradPreFactorz2] = quadrantWofzGrad(z2, sigma,1);
        Tt1minusTt2 = Tt11 - Tt22;
        preFactor1 = exp(sigma2*(gamma^2)/4 - gamma*(Tt1minusTt2)...
            + log(sigma2*gamma - 2*(Tt1minusTt2)) + log(gradThetaGamma));
        preFactor2 = exp(-(Tt1minusTt2).*(Tt1minusTt2)/sigma2 ...
            + log(gradThetaGamma) + log(gradPreFactorZ1));
        preFactor3 = exp(repmat((-t2.*t2/sigma2).',size(t1, 1), 1) ...
            - gamma*Tt11 + log(gradThetaGamma)...
            + log(Tt11.*repmat(preFactorz2.',size(t1, 1), 1) ...
            + repmat((gradPreFactorz2).',size(t1, 1), 1)));
    case 2

        % In this mode the Tt1 is actually the Tt2 and Tt2 is actually Tt1
        % Z1 is a matrix with the order given by Tt2 and z2 is a vector
        % with the order given by Tt1

        Tt11 = repmat(t2', size(t1, 1), 1);
        Tt22 = repmat(t1, 1, size(t2, 1));
        Z1 = (Tt11-Tt22)/sigma - sigma*gamma/2;
        z2 = t1/sigma + sigma*gamma/2;
        Z2 = repmat(z2,  1, size(t2, 1));
        [preFactorZ1, gradPreFactorZ1] = quadrantWofzGrad(Z1, sigma,1);
        [preFactorz2, gradPreFactorz2] = quadrantWofzGrad(z2, sigma,1);
        Tt1minusTt2 = Tt11 - Tt22;
        preFactor1 = exp(sigma2*(gamma^2)/4 - gamma*(Tt1minusTt2)...
            + log(sigma2*gamma - 2*(Tt1minusTt2)) + log(gradThetaGamma));
        preFactor2 = exp(-(Tt1minusTt2).*(Tt1minusTt2)/sigma2 ...
            + log(gradThetaGamma) + log(gradPreFactorZ1));
        preFactor3 = exp( repmat(-t1.*t1/sigma2,1, size(t2, 1))...
            - gamma*Tt11 + log(gradThetaGamma) ...
            + log(Tt11.*repmat(preFactorz2,1, size(t2, 1)) + repmat(gradPreFactorz2,1, size(t2, 1))));

    case {3,4}

        % In this mode Tt1 is actually Tt1 and Tt2 is zero.
        % Z1 is a vector and Z2 is a scalar
        z1 = t1/sigma - sigma*gamma/2;
        z2 = sigma*gamma/2;
        Z1 = repmat(z1, 1, size(t2,1));
        Z2 = repmat(z2, size(t1, 1), size(t2,1));
        [preFactorz1, gradPreFactorz1] = quadrantWofzGrad(z1, sigma,1);
        [preFactorz2, gradPreFactorz2] = quadrantWofzGrad(z2, sigma,1);
        preFactor1 = repmat(exp(sigma2*(gamma^2)/4 - gamma*(t1) ...
            + log(sigma2*gamma - 2*t1) + log(gradThetaGamma)), 1, size(t2, 1));
        preFactor2 = repmat(exp(-t1.*t1/sigma2 + log(gradThetaGamma)...
            + log(gradPreFactorz1)), 1, size(t2, 1));
        preFactor3 = repmat(exp(- gamma*t1 + log(gradThetaGamma) ...
            + log(t1*preFactorz2 + gradPreFactorz2)),1, size(t2, 1));

end

g = zeros(size(preFactor1));
% Evaluation of Upsilon when real(Z1)>=0 and real(Z2)>=0
ind = (real(Z1)>=0) & (real(Z2)>=0);
if any(any(ind))
    g(ind) = preFactor1(ind) - preFactor2(ind) + preFactor3(ind);
end;
% Evaluation of Upsilon when real(Z1)<0 and real(Z2)>=0
ind = (real(Z1)<0) & (real(Z2)>=0);
if any(any(ind))
    g(ind) = preFactor2(ind) + preFactor3(ind);
end
% Evaluation of Upsilon when real(Z1)>=0 and real(Z2)<0
ind = (real(Z1)>=0) & (real(Z2)<0);
if any(any(ind))
    g(ind) = - preFactor2(ind) - preFactor3(ind);
end;
% Evaluation of Upsilon when real(Z1)<0 and real(Z2)<0
ind = (real(Z1)<0) & (real(Z2)<0);
if any(any(ind))
    g(ind) = -preFactor1(ind) + preFactor2(ind) - preFactor3(ind);
end;

if mode==4
    g = g.';
end


% Intrinsic and Extrinsic Camera Parameters
%
% This script file can be directly excecuted under Matlab to recover the camera intrinsic and extrinsic parameters.
% IMPORTANT: This file contains neither the structure of the calibration objects nor the image coordinates of the calibration points.
%            All those complementary variables are saved in the complete matlab data file Calib_Results.mat.
% For more information regarding the calibration model visit http://www.vision.caltech.edu/bouguetj/calib_doc/

%-- Focal length:
fc = [ 1588.853342543796771 ; 1589.141065148706048 ];

%-- Principal point:
cc = [ 336.852104684709388 ; 166.220483494858200 ];

%-- Skew coefficient:
alpha_c = 0.000000000000000;

%-- Distortion coefficients:
kc = [ -0.183448717338282 ; -0.034396151458065 ; -0.005473481906443 ; 0.004392952805618 ; 0.000000000000000 ];

%-- Focal length uncertainty:
fc_error = [ 31.801580904116800 ; 29.314132245923947 ];

%-- Principal point uncertainty:
cc_error = [ 49.273756028476519 ; 39.985395373378047 ];

%-- Skew coefficient uncertainty:
alpha_c_error = 0.000000000000000;

%-- Distortion coefficients uncertainty:
kc_error = [ 0.066741056034172 ; 0.873747062658826 ; 0.006527073109278 ; 0.004504614774849 ; 0.000000000000000 ];

%-- Image size:
nx = 640;
ny = 480;

%-- Various other variables (may be ignored if you do not use the Matlab Calibration Toolbox):
%-- Those variables are used to control which intrinsic parameters should be optimized

n_ima = 15;						% Number of calibration images
est_fc = [ 1 ; 1 ];					% Estimation indicator of the two focal variables
est_aspect_ratio = 1;				% Estimation indicator of the aspect ratio fc(2)/fc(1)
center_optim = 1;					% Estimation indicator of the principal point
est_alpha = 0;						% Estimation indicator of the skew coefficient
est_dist = [ 1 ; 1 ; 1 ; 1 ; 0 ];	% Estimation indicator of the distortion coefficients

%-- Extrinsic parameters:
%-- The rotation (omc_kk) and the translation (Tc_kk) vectors for every calibration image and their uncertainties

%-- Image #1:
omc_1 = [ -1.557681e+00 ; -2.205804e+00 ; 1.330361e+00 ];
Tc_1  = [ 1.734034e+02 ; 5.794926e+01 ; 2.044993e+03 ];
omc_error_1 = [ 2.933522e-02 ; 2.081152e-02 ; 3.616192e-02 ];
Tc_error_1  = [ 6.355186e+01 ; 5.156988e+01 ; 3.664085e+01 ];

%-- Image #2:
omc_2 = [ -1.702089e+00 ; -2.256995e+00 ; 1.267502e+00 ];
Tc_2  = [ -6.722390e+01 ; 5.941507e+01 ; 2.057353e+03 ];
omc_error_2 = [ 3.035591e-02 ; 2.055681e-02 ; 3.795895e-02 ];
Tc_error_2  = [ 6.380940e+01 ; 5.186705e+01 ; 3.924807e+01 ];

%-- Image #3:
omc_3 = [ -1.701930e+00 ; -2.253495e+00 ; 1.271626e+00 ];
Tc_3  = [ -2.657159e+02 ; 5.535306e+01 ; 2.111954e+03 ];
omc_error_3 = [ 3.119852e-02 ; 2.203487e-02 ; 3.792260e-02 ];
Tc_error_3  = [ 6.555459e+01 ; 5.345885e+01 ; 4.297524e+01 ];

%-- Image #4:
omc_4 = [ 1.893838e+00 ; 1.951405e+00 ; -6.883375e-01 ];
Tc_4  = [ 1.355395e+02 ; 6.304476e+01 ; 2.007853e+03 ];
omc_error_4 = [ 1.462647e-02 ; 2.105632e-02 ; 3.955605e-02 ];
Tc_error_4  = [ 6.234890e+01 ; 5.068514e+01 ; 3.911636e+01 ];

%-- Image #5:
omc_5 = [ 1.908480e+00 ; 1.850529e+00 ; -5.507656e-01 ];
Tc_5  = [ -9.864709e+01 ; 6.448043e+01 ; 2.021515e+03 ];
omc_error_5 = [ 1.405146e-02 ; 2.113590e-02 ; 3.862142e-02 ];
Tc_error_5  = [ 6.272370e+01 ; 5.106967e+01 ; 4.131143e+01 ];

%-- Image #6:
omc_6 = [ 1.889738e+00 ; 1.980019e+00 ; -7.390048e-01 ];
Tc_6  = [ -3.575459e+02 ; 6.164224e+01 ; 2.073652e+03 ];
omc_error_6 = [ 1.076545e-02 ; 2.434070e-02 ; 4.112093e-02 ];
Tc_error_6  = [ 6.427989e+01 ; 5.275973e+01 ; 4.590561e+01 ];

%-- Image #7:
omc_7 = [ 1.904119e+00 ; 1.613811e+00 ; -2.469117e-01 ];
Tc_7  = [ 1.050648e+02 ; 6.953312e+01 ; 1.959883e+03 ];
omc_error_7 = [ 1.877046e-02 ; 1.939786e-02 ; 3.660657e-02 ];
Tc_error_7  = [ 6.086765e+01 ; 4.946251e+01 ; 3.876840e+01 ];

%-- Image #8:
omc_8 = [ 1.874544e+00 ; 1.387578e+00 ; 5.963331e-03 ];
Tc_8  = [ -1.316064e+02 ; 7.296929e+01 ; 1.962026e+03 ];
omc_error_8 = [ 1.991265e-02 ; 1.966408e-02 ; 3.477026e-02 ];
Tc_error_8  = [ 6.097585e+01 ; 4.958890e+01 ; 3.966239e+01 ];

%-- Image #9:
omc_9 = [ 1.867279e+00 ; 1.338834e+00 ; 5.192820e-02 ];
Tc_9  = [ -3.848199e+02 ; 7.244019e+01 ; 2.003159e+03 ];
omc_error_9 = [ 1.905893e-02 ; 2.014607e-02 ; 3.409265e-02 ];
Tc_error_9  = [ 6.267821e+01 ; 5.108076e+01 ; 4.288011e+01 ];

%-- Image #10:
omc_10 = [ -1.715796e+00 ; -2.041805e+00 ; 9.303852e-01 ];
Tc_10  = [ 1.012077e+02 ; 7.501598e+01 ; 1.710616e+03 ];
omc_error_10 = [ 2.385211e-02 ; 2.111071e-02 ; 3.698204e-02 ];
Tc_error_10  = [ 5.316514e+01 ; 4.311025e+01 ; 3.065417e+01 ];

%-- Image #11:
omc_11 = [ -1.805616e+00 ; -2.089567e+00 ; 8.651741e-01 ];
Tc_11  = [ -6.268099e+01 ; 8.236545e+01 ; 1.672806e+03 ];
omc_error_11 = [ 2.455832e-02 ; 2.181052e-02 ; 3.822892e-02 ];
Tc_error_11  = [ 5.190998e+01 ; 4.218558e+01 ; 3.143920e+01 ];

%-- Image #12:
omc_12 = [ -1.897248e+00 ; -2.132223e+00 ; 7.958098e-01 ];
Tc_12  = [ -2.617251e+02 ; 8.772964e+01 ; 1.655251e+03 ];
omc_error_12 = [ 2.597201e-02 ; 2.298575e-02 ; 3.987007e-02 ];
Tc_error_12  = [ 5.154881e+01 ; 4.202592e+01 ; 3.365145e+01 ];

%-- Image #13:
omc_13 = [ 2.114980e+00 ; 2.117562e+00 ; -3.547934e-01 ];
Tc_13  = [ 3.651036e+01 ; 1.009631e+02 ; 1.516712e+03 ];
omc_error_13 = [ 1.733561e-02 ; 1.778940e-02 ; 4.120690e-02 ];
Tc_error_13  = [ 4.712996e+01 ; 3.839118e+01 ; 3.001941e+01 ];

%-- Image #14:
omc_14 = [ 2.128866e+00 ; 2.219968e+00 ; -5.388805e-01 ];
Tc_14  = [ -6.343915e+01 ; 1.021354e+02 ; 1.517822e+03 ];
omc_error_14 = [ 1.478417e-02 ; 2.098690e-02 ; 4.310996e-02 ];
Tc_error_14  = [ 4.710837e+01 ; 3.841050e+01 ; 2.980588e+01 ];

%-- Image #15:
omc_15 = [ 2.121751e+00 ; 2.168024e+00 ; -4.694731e-01 ];
Tc_15  = [ -2.722483e+02 ; 1.012912e+02 ; 1.548764e+03 ];
omc_error_15 = [ 1.690688e-02 ; 2.488503e-02 ; 4.454520e-02 ];
Tc_error_15  = [ 4.814071e+01 ; 3.949586e+01 ; 3.320000e+01 ];


function [flips,scorepath,covmats_unflipped] = findflip(data,T,options)
% Finds an optimal permutation of the channels, where goodness is measured
% as the mean lagged partial  cross-correlation across pair of channels and lags.
% In other words, it finds a permutation where the signs of the lagged
% partial correlations agree as much as possible across subjects.
%
% INPUTS
% data          observations, either a struct with X (time series)
%                             or just a matrix containing the time series
%               Alternatively, it can be an (unflipped) array of
%                   autocorrelation matrices (ndim x ndim x no.lags x no. trials),
%                   as computed for example by getCovMats()
% T             length of series. If data is supplied as the
%               autocorrelation matrices, then this is not required and
%               can be specified as []
% options:
%  maxlag        max lag to consider
%  nbatch        no. of channels to evaluate at each iteration (0 for all)
%  noruns        how many random initialisations will be carried out
%  standardise   if 1, standardise the data
%  partial       if 1, base on partial correlation instead of correlation
%  maxcyc        for each initialization, maximum number of cycles of the greedy algorithm
%  verbose       do we get loud?
%
% data_ref and T_ref refer to having a previous data set to
% with which we want to be consistent (see above). These are assumed to be
% already sign-disambiguated

% OUTPUT
% flips         (length(T) X No. channels) binary matrix saying which channels must be
%               flipped for each time series
% scorepath     cell with the score of the winning solutions
% covmats_unflipped  the disambiguated covariance matrices
%
% Author: Diego Vidaurre, University of Oxford.

if nargin < 3, options = struct; end

options = checkoptions_flip(options);
covmats_unflipped = Get_Global_Variables_For_BitFlip_Evaluation(data,T,options);
N = size(covmats_unflipped,4);

ndim = size(covmats_unflipped,1);
score = -Inf;
scorepath = cell(options.noruns,1);

if isinf(options.maxcyc) && (options.nbatch > 0)
    error('If maxcyc is Inf, options.nbatch must be 0')
end

% repetitions of the search
for r = 1:options.noruns

    flipsr = Init_Solution(N,ndim,options,r);
    score_r = EvaluateFlips(flipsr,covmats_unflipped);
    scorepath{r} = score_r;
    if options.verbose
        fprintf('Run %d, Init, score %f \n',r,score_r)
    end

    for cyc = 1:options.maxcyc

        if options.nbatch > 0
            channels = randperm(ndim,options.nbatch);
        else
            channels = randperm(ndim);
        end

        ScoreMatrix = zeros(N,ndim);
        for d = channels
            for j = 1:N
                ScoreMatrix(j,d) = EvaluateFlips(flipsr,covmats_unflipped,j,d);
            end
        end

        [score_r,I] = max(ScoreMatrix(:));
        [j,d] = ind2sub([N ndim],I);

        ds = score_r - max(scorepath{r});
        if ds > options.threshold
            flipsr(j,d) = ~flipsr(j,d);
            if options.verbose
                fprintf('Run %d, Cycle %d, score +%f, flipped (%d,%d) \n',r,cyc,ds,j,d)
            end
            scorepath{r} = [scorepath{r} score_r];
        elseif options.nbatch == 0
            break
        else
            fprintf('Run %d, Cycle %d, score +0 \n',r,cyc)
        end

    end

    if options.verbose
        fprintf('Run %d, Finish, score %f \n',r,scorepath{r}(end))
    end

    if scorepath{r}(end) > score
        score = scorepath{r}(end);
        flips = flipsr;
    end

end

if options.verbose
    fprintf('Final Score=%f\n',score)
end

% Among the equivalent flippings, we keep the one w/ the lowest no. of flips
for j = 1:N
    if mean(flips(j,:))>0.5
        flips(j,:) = 1 - flips(j,:);
    end
end

end

function options = checkoptions_flip(options)
if ~isfield(options,'maxlag'), options.maxlag = 10; end
if ~isfield(options,'noruns'), options.noruns = 1; end
if ~isfield(options,'probinitflip'), options.probinitflip = 0.25; end
if ~isfield(options,'standardise'), options.standardise = 1; end
if ~isfield(options,'partial'), options.partial = 0; end
if ~isfield(options,'verbose'), options.verbose = 1; end
if ~isfield(options,'nbatch'), options.nbatch = 0; end
if ~isfield(options,'maxcyc'), options.maxcyc = 10000; end
if ~isfield(options,'threshold'), options.threshold = 0.00001; end
end

function flipsr = Init_Solution(N,ndim,options,r)
% Prepare the necessary data structures
if ~isfield(options,'Flips')
    if r==1 % first run starts at no flips
        flipsr = zeros(N,ndim);
    else
        flipsr = binornd(1,options.probinitflip,N,ndim); % random init
    end
else
    flipsr = options.Flips;
end
end

function covmats_unflipped = Get_Global_Variables_For_BitFlip_Evaluation(data,T,options)
% Prepare the necessary data structures
if length(size(data))==4 % it is an array of autocorrelation matrices already
    covmats_unflipped = data; clear data
else
    covmats_unflipped = getAllCovMats(data,T,options);
end
end

function s = EvaluateFlips(flipsr,covmats_unflipped,j,d)
% evaluate for a change in subject j and channel d
if nargin>3, flipsr(j,d) = ~flipsr(j,d);  end
signmats = getSignMat(flipsr);
covmats = applySign(covmats_unflipped,signmats);
s = getscore(covmats);
end

function score = getscore(M)
% get the score from the (flipped) matrices of autocorrelation contained in M
% M is a matrix (ndim x ndim x lags x subjects) with autocovariances matrix for all subjects
N = size(M,4); ndim = size(M,1); L = size(M,3);
M = reshape(M,[(ndim^2)*L N]);
C = corr(M);
score = mean(C(triu(true(N),1)));
end

function CovMats = applySign(CovMats,SignMats)
% apply matrices of sign flipping to the autocorrelation matrices
N = size(SignMats,3); nlags = size(CovMats,3);
for j = 1:N
    CovMats(:,:,:,j) = CovMats(:,:,:,j) .* repmat(SignMats(:,:,j),[1 1 nlags]);
end
end

function SignMats = getSignMat(Flips)
% Construct matrices of sign flipping for the autocorrelation matrices
[N,ndim] = size(Flips);
SignMats = zeros(ndim,ndim,N);
for j = 1:N
    flips = ones(1,ndim);
    flips(Flips(j,:)==1) = -1;
    SignMats(:,:,j) = flips' * flips;
end
end


function [amu] = mg2amu(mg)
% Convert mass from milligrams to atomic mass units.
% Chad Greene 2012
amu = mg*602213665167500000000;