% Digital Video Stabilization and Rolling Shutter Correction using Gyroscopes
% Copyright (C) 2011 Alexandre Karpenko
%
% 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
% 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 W = imalign(T, I, p)
% align the template image T to the input image I

if size(T,3) == 3
    T = rgb2gray(T);
end
if size(I,3) == 3
    I = rgb2gray(I);
end
if max(T(:)) > 1
    T = double(T) / 255;
end
if max(I(:)) > 1
    I = double(I) / 255;
end

gradTx = filter2(fspecial('sobel')', T);
gradTy = filter2(fspecial('sobel') , T);
%{
figure(1); showim(reshape(gradTx, size(T)));
figure(2); showim(reshape(gradTy, size(T)));
pause;
%}

x = repmat((1:size(T,2)) , size(T,1), 1);
y = repmat((1:size(T,1))', 1, size(T,2));

gradT_dWdp = [gradTx(:) .* x(:) ...
              gradTy(:) .* x(:) ...
              gradTx(:) .* y(:) ...
              gradTy(:) .* y(:) ...
              gradTx(:)         ...
              gradTy(:)         ];

H = gradT_dWdp' * gradT_dWdp;

p = p(:);

for i=1:100
    W = [1+p(1)  p(3)  p(5); ...
          p(2)  1+p(4) p(6)];

    figure(1); clf;
    imshow(imtransform(T,  maketform('affine', W'), 'XData', [1 size(I,2)], 'YData', [1 size(I,1)]));
    hold on;
    h = imshow(I);
    set(h, 'AlphaData', 0.6);
    pause(0.01);

    WI = imtransform(I,  maketform('affine', W'), 'XData', [1 size(I,2)], 'YData', [1 size(I,1)], 'FillValues', NaN);
    errorIm = WI - T;
    errorIm(isnan(errorIm)) = 0;
    delp = H \ (gradT_dWdp' * errorIm(:));
    p = p + delp;
    display(p);
end


function [beta_gibbs,F_gibbs,L_gibbs,phi_gibbs,sigma_gibbs,lambda_t_gibbs,sigma_t_gibbs,sbar,favar,It,Bu]=...
    favar_stvol3gibbs(Xbart,Xt,yt,B0,phi0,alpha0,delta0,f0,upsilon0,betahat,sigmahat,gamma,G,I_o,omega,T,n,k,It,Bu,pick,pickf,favar,data_endo,lags)

%% preliminaries
% initialise variables
nfactorvar=favar.nfactorvar;
numpc=favar.numpc;
favarX=favar.X(:,favar.plotX_index);
onestep=favar.onestep;
Sigma=bear.nspd(favar.Sigma);
Ll=favar.L;
favar_X=favar.X;
% load priors
L0=favar.L0*eye(n);
a0=favar.a0;
b0=favar.b0;
% sigmahat=(1/T)*(EPS'*EPS);

% preallocation
Ll_gibbs=zeros(size(Ll(:),1),It-Bu);
R2_gibbs=zeros(size(favarX,2),It-Bu);

if onestep==0 %static factors in this case
    FY=data_endo;
    pbstring='two-step'; %string for the progress bar
    % elseif onestep==1
    %     pbstring='one-step'; %string for the progress bar
end

% preliminary elements for the algorithm
% compute the product G'*I_gamma*G (to speed up computations of deltabar)
GIG=G'*I_o*G;
% compute alphabar
alphabar=T+alpha0;

% initiate the Gibbs sampler
% initiate the counting of iterations
count=1;
pickcount=1;
% initiate the record matrices and cells
beta_gibbs=[];
F_gibbs=[];
L_gibbs=[];
phi_gibbs=[];
sigma_gibbs=[];
lambda_t_gibbs={};
sigma_t_gibbs={};

% step 1: determine initial values for the algorithm

% initial value for beta
beta=betahat;
B=reshape(beta,k,n);
% initial value for f_2,...,f_n
% obtain the triangular factorisation of sigmahat
[Fhat Lambdahat]=bear.triangf(sigmahat);
% obtain the initial value for F
F=Fhat;
% obtain the inverse of Fhat
[invFhat]=bear.invltod(Fhat,n);
% create the cell storing the different vectors of invF
Finv=cell(n,1);
% store the vectors
for ii=2:n
    Finv{ii,1}=invFhat(ii,1:ii-1);
end
% initial values for L
L=zeros(T,1);
% initial values for phi
phi=1;

% step 2: determine the sbar values and Lambda
sbar=diag(Lambdahat);
Lambda=sparse(diag(sbar));
% then determine sigma^(0)
sigma=F*Lambda*F';

% step 3: recover the series of initial values for lambda_1,...,lambda_T and sigma_1,...,sigma_T
lambda_t=repmat(diag(sbar),1,1,T);
sigma_t=repmat(sigmahat,1,1,T);

% create a progress bar
hbar = bear.parfor_progressbar(It,['Progress of the Gibbs sampler (',pbstring,').']);

%% run the Gibbs sampler
while count<=It

    % step 4: draw beta from its conditional posterior
    % first compute the summations required for omegabar and betabar
    summ1=zeros(k,k);
    summ2=zeros(k,n);
    % run the summation
    for jj=1:T
        prodt=Xt{jj,1}'*exp(-L(jj,1));
        summ1=summ1+prodt*Xt{jj,1};
        summ2=summ2+prodt*yt(:,:,jj)';
    end
    % then obtain the inverse of phi0
    invphi0=diag(1./diag(phi0));
    % obtain the inverse of phibar
    invphibar=summ1+invphi0;
    % recover phibar
    C=chol(bear.nspd(invphibar),'Lower')';
    invC=C\speye(k);
    phibar=invC*invC';
    % recover Bbar
    Bbar=phibar*(summ2+invphi0*B0);
    % draw B from its posterior
    B=bear.matrixndraw(Bbar,sigma,phibar,k,n);
    % finally recover beta by vectorising
    beta=B(:);

    % step 5: draw the series f_2,...,f_n from their conditional posteriors
    % recover first the residuals
    for jj=1:T
        epst(:,:,jj)=yt(:,:,jj)-Xbart{jj,1}*beta;
    end
    % then draw the vectors in turn
    for jj=2:n
        % first compute the summations required for upsilonbar and fbar
        summ1=zeros(jj-1,jj-1);
        summ2=zeros(jj-1,1);
        % run the summation
        for kk=1:T
            prodt=epst(1:jj-1,1,kk)*exp(-L(kk,1));
            summ1=summ1+prodt*epst(1:jj-1,1,kk)';
            summ2=summ2+prodt*epst(jj,1,kk)';
        end
        summ1=(1/sbar(jj,1))*summ1;
        summ2=(-1/sbar(jj,1))*summ2;
        % then obtain the inverse of upsilon0
        invupsilon0=diag(1./diag(upsilon0{jj,1}));
        % obtain upsilonbar
        invupsilonbar=summ1+invupsilon0;
        C=chol(bear.nspd(invupsilonbar));
        invC=C\speye(jj-1);
        upsilonbar=full(invC*invC');
        % recover fbar
        fbar=upsilonbar*(summ2+invupsilon0*f0{jj,1});
        % finally draw f_i^(-1)
        Finv{jj,1}=fbar+chol(bear.nspd(upsilonbar),'lower')*randn(jj-1,1);
    end
    % recover the inverse of F
    invF=eye(n);
    for jj=2:n
        invF(jj,1:jj-1)=Finv{jj,1};
    end
    % eventually recover F
    F=bear.invltod(invF,n);
    % update sigma
    sigma=F*Lambda*F';

    % step 6: draw phi from its conditional posterior
    % estimate deltabar
    deltabar=L'*GIG*L+delta0;
    % draw the value phi_i
    phi=bear.igrandn(alphabar/2,deltabar/2);

    % step 7: draw the series lambda_t from their conditional posteriors, t=1,...,T
    % consider periods in turn
    for kk=1:T
        % a candidate value will be drawn from N(lambdabar,phibar)
        % the definitions of lambdabar and phibar varies with the period, thus define them first
        % if the period is the first period
        if kk==1
            lambdabar=(gamma*L(2,1))/(1/omega+gamma^2);
            phibar=phi/(1/omega+gamma^2);
            % if the period is the final period
        elseif kk==T
            lambdabar=gamma*L(T-1,1);
            phibar=phi;
            % if the period is any period in-between
        else
            lambdabar=(gamma/(1+gamma^2))*(L(kk-1,1)+L(kk+1,1));
            phibar=phi/(1+gamma^2);
        end
        % now draw the candidate
        cand=lambdabar+phibar^0.5*randn;
        % compute the acceptance probability
        prob=bear.mhprob3(cand,L(kk,1),sbar,epst(:,1,kk),Finv,n);
        % draw a uniform random number
        draw=rand;
        % keep the candidate if the draw value is lower than the prob
        if draw<=prob
            L(kk,1)=cand;
            % if not, just keep the former value
        end
    end
    % then recover the series of matrices lambda_t and sigma_t
    for kk=1:T
        lambda_t(:,:,kk)=exp(L(kk,1))*diag(sbar);
        sigma_t(:,:,kk)=F*lambda_t(:,:,kk)*F';
    end

    %% Sample Sigma and Ll (static)
    [Sigma,Ll]=bear.favar_SigmaL(Sigma,Ll,nfactorvar,numpc,onestep,n,favar_X,FY,a0,b0,T,lags,L0);

    %% record phase
    % if the burn-in sample phase is not yet over
    if count<=Bu
        % simply add 1 to the iteration count
        count=count+1;
        % on the other hand, if the burn-in sample phase is over
    elseif count>Bu
        % adding one iteration to the count will depend on wether post-burn selection applies
        % if there is no post burn selection
        if pick==0
            % record the results
            beta_gibbs(:,count-Bu)=beta;
            F_gibbs(:,:,count-Bu)=F;
            L_gibbs(:,:,count-Bu)=L;
            phi_gibbs(count-Bu,1)=phi;
            sigma_gibbs(:,count-Bu)=sigma(:);
            % save the factors and loadings (keep the notation in the code consistent, although - except L - they don't change)
            Ll_gibbs(:,count-Bu)=Ll(:);

            % compute R2 (Coefficient of Determination) for plotX variables (keep the notation in the code consistent, although they don't change)
            R2=bear.favar_R2(favarX,FY);
            R2_gibbs(:,count-Bu)=R2(:);
            for jj=1:T
                lambda_t_gibbs{jj,1}(:,:,count-Bu)=lambda_t(:,:,jj);
                sigma_t_gibbs{jj,1}(:,:,count-Bu)=sigma_t(:,:,jj);
            end
            % then add one to the count
            count=count+1;
            % if there is post burn selection, only one draw over 'fpick' draws will be retained
        elseif pick==1
            % if the iteration does not correspond to fpick, don't record the results, don't increase the regular count, but do increase pickcount by 1, and do record the acceptance rate of the Metropolis-Hastings step
            if pickcount~=pickf
                pickcount=pickcount+1;
                % on the other hand, if the iteration does correspond to fpick
            elseif pickcount==pickf
                % do record the results
                beta_gibbs(:,count-Bu)=beta;
                F_gibbs(:,:,count-Bu)=F;
                L_gibbs(:,:,count-Bu)=L;
                phi_gibbs(count-Bu,1)=phi;
                sigma_gibbs(:,count-Bu)=sigma(:);
                % save the factors and loadings (keep the notation in the code consistent, although - except L - they don't change)
                Ll_gibbs(:,count-Bu)=Ll(:);

                % compute R2 (Coefficient of Determination) for plotX variables (keep the notation in the code consistent, although they don't change)
                R2=bear.favar_R2(favarX,FY);
                R2_gibbs(:,count-Bu)=R2(:);
                for jj=1:T
                    lambda_t_gibbs{jj,1}(:,:,count-Bu)=lambda_t(:,:,jj);
                    sigma_t_gibbs{jj,1}(:,:,count-Bu)=sigma_t(:,:,jj);
                end
                % then increase the regular count by 1 and re-initialise pickcount
                count=count+1;
                pickcount=1;
            end
        end
    end

    % update progress by one iteration
    hbar.iterate(1);

end

% in case we have thinning of the draws,
thin=abs(round(favar.thin)); % should be a positive integer
if thin~=1
    beta_gibbs=beta_gibbs(:,thin:thin:end);
    F_gibbs=F_gibbs(:,thin:thin:end);
    L_gibbs=L_gibbs(:,thin:thin:end);
    phi_gibbs=phi_gibbs(:,thin:thin:end);
    sigma_gibbs=sigma_gibbs(:,thin:thin:end);
    Ll_gibbs=Ll_gibbs(:,thin:thin:end);
    R2_gibbs=R2_gibbs(:,thin:thin:end);
    for jj=1:T
        lambda_t_gibbs=lambda_t_gibbs(:,:,thin:thin:end);
        sigma_t_gibbs=sigma_t_gibbs(:,:,thin:thin:end);
    end
    It=(1/thin)*It;
    Bu=(1/thin)*Bu;
end

% save in favar structure
favar.L_gibbs=Ll_gibbs;
favar.R2_gibbs=R2_gibbs;

close(hbar);   %close progress bar


function [stat,pval,statAll,pvalAll]=grangercause(y,constant,lags,het,uncorr,inference)
% Granger causality testing with a variance-covariance matrix estimated under a variety of
% assumptions on the covariance of the errors:
%   * Conditionally Homoskedastic and Uncorrelated
%   * Conditionally Homoskedastic but Correlated
%   * Heteroskedastic but Conditionally Uncorrelated
%   * Heteroskedastic and Correlated
%
% USAGE:
%   [STAT] = grangercause(Y,CONSTANT,LAGS)
%   [STAT,PVAL,STATALL,PVALALL] = grangercause(Y,CONSTANT,LAGS,HET,UNCORR,INFERENCE)
%
% INPUTS:
%   Y             - A T by K matrix of data
%   CONSTANT      - Scalar variable: 1 to include a constant, 0 to exclude
%   LAGS          - Non-negative integer vector representing the VAR orders to include in the model.
%   HET           - [OPTIONAL] A scalar integer indicating the type of covariance estimator
%                      0 - Homoskedastic
%                      1 - Heteroskedastic [DEFAULT]
%   UNCORR        - [OPTIONAL] A scalar integer indicating the assumed structure of the error
%                     covariance matrix
%                      0 - Correlated errors  [DEFAULT]
%                      1 - Uncorrelated errors
%   INFERENCE     - [OPTIONAL] Inference method
%                      1 - Likelihood ratio [DEFAULT]
%                      2 - LM test
%                      3 - Wald test
%
% OUTPUTS:
%   STAT          - K by K matrix of Granger causality statistics computed using the specified
%                     covariance estimator and inference method. STAT(i,j) corresponds to a test that
%                     y(i) is not caused by y(j)
%   PVAL          - K by K matrix of p-values corresponding to STAT
%   STATALL       - K by 1 vector of Granger causality statistics computed using the specified
%                     covariance estimator and inference method. STATALL(i) corresponds to a test that
%                     y(i) is not caused by any y(j), j neq i
%   PVALALL       - K by 1 vector of p-values corresponding to STATALL
%
% COMMENTS:
%   Granger causality tests based on a VAR including any lags.
%
%   y(:,t)' = CONST + P(1) * y(:,y-1) + P(2)*y(:,y-2) + ... + P(1)*y(:,t-K)'
%
%   where P(j) are K by K parameter matrices and CONST is a K by 1 parameter matrix (if CONSTANT==1)
%
% EXAMPLE:
%   Conduct GC testing in a VAR(1) with a constant
%        parameters = grangercause(y,1,1)
%   Conduct GC testing in a VAR(3) with no constant
%        parameters = grangercause(y,0,[1:3])
%   Conduct GC testing in a VAR that includes lags 1 and 3 with a constant
%        parameters = grangercause(y,1,[1 3])
%
% See also VECTORAR, VECTORARVCV

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 3.0    Date: 1/1/2007

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if nargin<3 || nargin>6
    error('3 to 6 inputs required')
end
if nargin==3
    het=1;
    uncorr=0;
    inference=1;
elseif nargin==4
    uncorr=0;
    inference=1;
elseif nargin==5
    inference=1;
end
% Check Y
if ndims(y)~=2
    error('Y must be T by K')
end
% Check constant
if ~isscalar(constant)
    error('CONSTANT must be either 0 or 1')
end
if ~ismember(constant,[0 1])
    error('CONSTANT must be either 0 or 1')
end
% Check lags
if ndims(lags)~=2
    error('LAGS must be a vector of positive integers containing lags to include')
end
if size(lags,1)>size(lags,2)
    lags = lags';
end
if ~all(lags>0)
    error('LAGS must be a vector of positive integers containing lags to include')
end
if ~all(floor(lags)==lags)
    error('LAGS must be a vector of positive integers containing lags to include')
end
if length(lags)~=length(unique(lags))
    error('LAGS must be a vector of unique elements')
end
lags=sort(lags);
% Check het
if ~isscalar(het)
    error('HET must be either 0 or 1')
end
if ~ismember(het,[0 1])
    error('HET must be either 0 or 1')
end
% Check uncorr
if ~isscalar(uncorr)
    error('UNCORR must be either 0 or 1')
end
if ~ismember(uncorr,[0 1])
    error('UNCORR must be either 0 or 1')
end
% Inference mathod
if ~isscalar(inference)
    error('INFERENCE must be either 0 or 1')
end
if ~ismember(inference,[1 2 3])
    error('INFERENCE must be either 0 or 1')
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Inference Methods
% 1. LR MLE : Relatively easy, need to iteratively depete the correct
% columns and reestimate.  Use a LR test
% 2. Wald: Just compute the VCV, then figure out how to select the correct
%       rows, and you are done
% 3. LM: Have to re-estimate under the null and use those errors to compute
%       the scores.  Test is based on scores.
% 4. LR Robust: Just like LM above but using errors estimated under the
%       alternative

% LR MLE
% First generate the unrestricted errors, then iterate across series and
% drop regressors

T=size(y,1);
K=size(y,2);
P=length(lags);
m=max(lags);
ylags=cell(K,1);
for k=1:K
    ytemp = [ones(m,1)*mean(y(:,k));
        y(:,k)];
    [nothing,ylags{k}]=newlagmatrix(ytemp,m,0);
end

X=zeros(T,K+constant);
index=1;
if constant
    X(:,1)=ones(T,1);
    index=index+1;
end
p=length(lags);
for i=1:p
    for k=1:K
        X(:,index)=ylags{k}(:,lags(i));
        index=index+1;
    end
end

% Each column contains the parameters for a single y
paramvec = X\y;
errors = y-X*paramvec;
s2=errors'*errors/T;
e=cell(K,K);
for i=1:K
    for j=1:K
        % i will be the LHS variable, j will be the lags
        tempX=X;
        allcols = 1:size(X,2);
        % Drop columns j, K+j, 2K+j,...P(K-1)*j (+constant if present)
        if constant
            drop = (j:K:size(X,2)-1)+constant;
        else
            drop = (j:K:size(X,2));
        end
        remain = setdiff(allcols,drop);
        beta = tempX(:,remain)\y(:,i);
        % These errors are estiamted under the null and can be used for LR and LM testing
        e{i,j} = y(:,i)-tempX(:,remain)*beta;
    end
end
% Construct the regressions needed to test the "all" hypothesis
eall = cell(K,1);
for i=1:K
    tempX = X;
    % Keep columns i, K+i, 2K+i, P(K-1)*i (+constant if present)
    if constant
        remain = [constant (i:K:(size(X,2)-1))+constant];
    else
        remain = i:K:size(X,2);
    end
    beta = tempX(:,remain)\y(:,i);
    eall{i} = y(:,i)-tempX(:,remain)*beta;
end

Np = p*K+constant;
stat = zeros(K);
statAll = zeros(K,1);
% Homoskedastic Likelihood Ratio
if inference==1 && het==0
    for i=1:K
        for j=1:K
            e2=errors;
            e2(:,i)=e{i,j};
            sR=e2'*e2/T;
            stat(i,j)=(T-P*K^2+P)*(log(det(sR))-log(det(s2)));
        end
    end
    for i=1:K
        e2=errors;
        e2(:,i) = eall{i};
        sR=e2'*e2/T;
        statAll(i)=(T-P*K^2+(K-1)*P)*(log(det(sR))-log(det(s2)));
    end
    % Heteroskedasticity robust LR;  key here is score covariance is computed
    % using errors estimated under the alternative
elseif inference==1 && het==1
    for i=1:K
        for j=1:K
            e2=errors;
            e2(:,i)=e{i,j};
            X2=repmat(X,1,K);
            e2=reshape(repmat(e2,Np,1),T,K*Np);
            s=X2.*e2;
            sbar=mean(s);
            % Estimate S using the errors, not e2
            S=vectorarscorecov(errors,X,het,uncorr,K,T,Np);
            stat(i,j)=(T-P*K^2+P)*sbar*S^(-1)*sbar';
        end
    end
    for i=1:K
        e2=errors;
        e2(:,i)=eall{i};
        X2=repmat(X,1,K);
        e2=reshape(repmat(e2,Np,1),T,K*Np);
        s=X2.*e2;
        sbar=mean(s);
        % Estimate S using the errors, not e2
        S=vectorarscorecov(errors,X,het,uncorr,K,T,Np);
        statAll(i)=(T-P*K^2+(K-1)*P)*sbar*S^(-1)*sbar';
    end
    % All LM tests since function will handle;  the key here is that the errors
    % are computed under the null
elseif inference==2
    for i=1:K
        for j=1:K
            e2=errors;
            e2(:,i)=e{i,j};
            X2=repmat(X,1,K);
            e2=reshape(repmat(e2,Np,1),T,K*Np);
            s=X2.*e2;
            sbar=mean(s);
            e2=errors;
            e2(:,i)=e{i,j};
            S=vectorarscorecov(e2,X,het,uncorr,K,T,Np);
            stat(i,j)=(T-P*K^2+P)*sbar*S^(-1)*sbar';
        end
    end
    for i=1:K
        e2=errors;
        e2(:,i)=eall{i};
        X2=repmat(X,1,K);
        e2=reshape(repmat(e2,Np,1),T,K*Np);
        s=X2.*e2;
        sbar=mean(s);
        e2=errors;
        e2(:,i)=eall{i};
        S=vectorarscorecov(e2,X,het,uncorr,K,T,Np);
        statAll(i)=(T-P*K^2+(K-1)*P)*sbar*S^(-1)*sbar';
    end
    % Wald tests
elseif inference==3
    XpXi = ((X'*X)/T)^(-1);
    Ainv = kron(eye(K),XpXi);
    B = vectorarscorecov(errors,X,het,uncorr,K,T,Np);
    V = Ainv*B*Ainv;
    %Vinv=V^(-1);
    % Now I have to cleverly select the parameters to set to 0 and then
    % compute the wald tests
    % Easier to iterate over the j's in the inside loop
    for i=1:K
        for j=1:K
            temp=zeros(size(paramvec))';
            % Select the columns of the parameters to test
            if constant
                pl = (j:K:size(X,2)-1)+constant;
            else
                pl = (j:K:size(X,2));
            end

            temp(i,pl)=1;
            temp=temp';
            temp=temp(:);
            pl = find(temp);
            p = paramvec;
            p = p(:);
            stat(i,j)=(T-P*K^2+P)*(p(pl)'*V(pl,pl)^(-1)*p(pl));
        end
    end
    for i=1:K
        temp=zeros(size(paramvec))';
        % Select the columns of the parameters to test
        if constant
            remain = [constant (i:K:size(X,2)-1)+constant];
        else
            remain = (i:K:size(X,2)-1);
        end
        pl = setdiff(1:size(X,2),remain);
        temp(i,pl)=1;
        temp=temp';
        temp=temp(:);
        pl = find(temp);
        p = paramvec;
        p = p(:);
        statAll(i)=(T-P*K^2+(K-1)*P)*(p(pl)'*V(pl,pl)^(-1)*p(pl));
    end
end
% All stats have the same dist
pval=1-chi2cdf(stat,length(lags));
pvalAll = 1-chi2cdf(statAll,(K-1)*length(lags));

function S=vectorarscorecov(errors,X,het,uncorr,K,T,Np)
s2=errors'*errors/T;
XpX=X'*X/T;
if ~het && uncorr
    S=kron(diag(diag(s2)),XpX);
elseif ~het && ~uncorr
    S=kron(s2,XpX);
elseif het && uncorr
    X2=repmat(X,1,K);
    e2=reshape(repmat(errors,Np,1),T,K*Np);
    s=X2.*e2;
    s=s-repmat(mean(s),T,1);
    S=zeros(Np*K);
    for i=1:K
        sel=(i-1)*Np+1:i*Np;
        temp = s(:,sel);
        S(sel,sel)=temp'*temp/T;
    end
elseif het && ~uncorr
    X2=repmat(X,1,K);
    e2=reshape(repmat(errors,Np,1),T,K*Np);
    s=X2.*e2;
    s=s-repmat(mean(s),T,1);
    S=s'*s/T;
end


%SerialLink.jacob_dot Derivative of Jacobian
%
% JDQ = R.jacob_dot(Q, QD) is the product (6x1) of the derivative of the
% Jacobian (in the world frame) and the joint rates.
%
% Notes::
% - This term appears in the formulation for operational space control XDD = J(Q)QDD + JDOT(Q)QD
% - Written as per the reference and not very efficient.
%
% References::
% - Fundamentals of Robotics Mechanical Systems (2nd ed)
%   J. Angleles, Springer 2003.
% - A unified approach for motion and force control of robot manipulators: The operational space formulation
%  O Khatib, IEEE Journal on Robotics and Automation, 1987.
%
% See also SerialLink.jacob0, diff2tr, tr2diff.

% 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

function Jdot = jacob_dot(robot, q, qd)

	n = robot.n;
    links = robot.links;

    % Using the notation of Angeles:
    %   [Q,a] ~ [R,t] the per link transformation
    %   P ~ R   the cumulative rotation t2r(Tj) in world frame
    %   e       the last column of P, the local frame z axis in world coordinates
    %   w       angular velocity in base frame
    %   ed      deriv of e
    %   r       is distance from final frame
    %   rd      deriv of r
    %   ud      ??

    for i=1:n
        T = links(i).A(q(i));
        Q{i} = t2r(T);
        a{i} = transl(T)';
    end

    P{1} = Q{1};
    e{1} = [0 0 1]';
    for i=2:n
        P{i} = P{i-1}*Q{i};
        e{i} = P{i}(:,3);
    end

    % step 1
    w{1} = qd(1)*e{1};
    for i=1:(n-1)
        w{i+1} = qd(i+1)*[0 0 1]' + Q{i}'*w{i};
    end

    % step 2
    ed{1} = [0 0 0]';
    for i=2:n
        ed{i} = cross(w{i}, e{i});
    end

    % step 3
    rd{n} = cross( w{n}, a{n});
    for i=(n-1):-1:1
        rd{i} = cross(w{i}, a{i}) + Q{i}*rd{i+1};
    end

    r{n} = a{n};
    for i=(n-1):-1:1
        r{i} = a{i} + Q{i}*r{i+1};
    end

    ud{1} = cross(e{1}, rd{1});
    for i=2:n
        ud{i} = cross(ed{i}, r{i}) + cross(e{i}, rd{i});
    end

    % step 4
    %  swap ud and ed
    v{n} = qd(n)*[ud{n}; ed{n}];
    for i=(n-1):-1:1
        Ui = blkdiag(Q{i}, Q{i});
        v{i} = qd(i)*[ud{i}; ed{i}] + Ui*v{i+1};
    end

    Jdot = v{1};


function [ a, b ] = p04_ab ( m )

%*****************************************************************************80
%
%% P04_AB returns bounds for problem 4.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Harald Niederreiter, Kevin McCurley,
%    Optimization of functions by quasi-random search methods,
%    Computing,
%    Volume 22, Number 2, 1979, pages 119-123.
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Output, real A(M,1), B(M,1), lower and upper bounds.
%
  a(1:m,1) = 0.0;
  b(1:m,1) = 1.0;

  return
end


function [rpm] = THz2rpm(THz)
% Convert frequency from terahertz to revolutions per minute.
% Chad A. Greene 2012
rpm = THz*60*1e+12;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Spherical Harmonic Modeling and Analysis Toolkit (SPHARM-MAT) is a 3D
% shape modeling and analysis toolkit.
% It is a software package developed at Shenlab in Center for Neuroimaging,
% Indiana University (SpharmMat@gmail.com, http://www.iupui.edu/~shenlab/)
% It is available to the scientific community as copyright freeware
% under the terms of the GNU General Public Licence.
%
% Copyright 2009, 2010, ShenLab, Center for Neuroimaging, Indiana University
%
% This file is part of SPHARM-MAT.
%
% SPHARM-MAT 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.
%
% SPHARM-MAT 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 SPHARM-MAT. If not, see <http://www.gnu.org/licenses/>.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function performPCA(confs, objs)

numSbj = length(objs);
groupID = confs.GroupID;

% set parameters
fvecs = [];	% fvecs is accumulator of coefficients from each species.
keep = numSbj-1;	% number of eigenvectors to keep

% do processing of each file
for i = 1:numSbj
	% read in file.
	file = objs{i};
	load(file);
	[path, name, ext] = fileparts(file);
	clear('faces', 'vertices', 'sph_verts');

	[nrows ncols] = size(fvec);
	temp = fvec.';
	temp = reshape(temp, (nrows*3), 1);
	fvecs = [fvecs temp];
end
clear('fvec', 'temp', 'name');

fvecs = fvecs.';
[eigenvecs, scores, eigenvals] = princomp(fvecs);
% trace_eig = trace(eigenvals);
perc_variance_explained = eigenvals/sum(eigenvals);

% drop cells that are to not be kept
eigenvecs = eigenvecs(:,1:keep);
%scores = scores(:,1:keep);
eigenvals = eigenvals(1:keep,1);
perc_variance_explained = perc_variance_explained(1:keep,:);
cum_percent_explained = cumsum(perc_variance_explained);

new_name = [confs.OutDirectory '/' confs.OutputName];
if exist(new_name,'file')
    prompt = {'Enter new filename:'};
    dlg_title = 'New File Name';
    num_lines = 1;
    def = {new_name};
    answer = inputdlg(prompt,dlg_title,num_lines,def);
    new_name = answer{1};
end

save(new_name, 'fvecs', 'eigenvecs', 'eigenvals', 'perc_variance_explained', ...
    'cum_percent_explained', 'scores','groupID');

return;


function [image1,image2,img3]=mosaic_map(img1,img2,d)

[m1,n1,p1] = size(img1);
m11 = ceil(m1/d);
n11 = ceil(n1/d);
for i=1:2:m11
    for j=2:2:n11
        img1((i-1)*d+1:i*d,(j-1)*d+1:j*d,:)=0;
    end
end
for i=2:2:m11
    for j=1:2:n11
        img1((i-1)*d+1:i*d,(j-1)*d+1:j*d,:)=0;
    end
end
image1=img1(1:m1,1:n1,:);

%%
[m2,n2,p2] = size(img2);
m22 = ceil(m2/d);
n22 = ceil(n2/d);
for i=1:2:m22
    for j=1:2:n22
       img2((i-1)*d+1:i*d,(j-1)*d+1:j*d,:)=0;
    end
end
for i=2:2:m22
    for j=2:2:n22
       img2((i-1)*d+1:i*d,(j-1)*d+1:j*d,:)=0;
    end
end
image2=img2(1:m2,1:n2,:);
%%
img3=image1+image2;

end


function test_ft_mvaranalysis

% MEM 2gb
% WALLTIME 00:10:00
% DEPENDENCY ft_mvaranalysis

fs = 500;
nchan = 32;
start_time = -1; %s
end_time = 2.5; %s
nsamples = (end_time - start_time) * fs + 1;

data = [];
data.time{1} = linspace(start_time, end_time, nsamples);
data.trial{1} = randn(nchan,nsamples);
data.label = cellstr(num2str((1:nchan).'));

cfg = [];
dataout = ft_mvaranalysis(cfg,data);


classdef madgwick < handle
    methods (Static = true)

        function q = imu(q, Gyroscope, Accelerometer, SamplePeriod, Beta)

            % Normalise accelerometer measurement
            acc = Accelerometer;
            if(norm(acc) == 0), return; end   % handle NaN
            acc = acc / norm(acc);  % normalise magnitude

            % Gradient decent algorithm corrective step
            F = (quatrotate(q, [0 0 1]) - acc)';
%             F = [2*(q(2)*q(4) - q(1)*q(3)) - Accelerometer(1)
%                 2*(q(1)*q(2) + q(3)*q(4)) - Accelerometer(2)
%                 2*(0.5 - q(2)^2 - q(3)^2) - Accelerometer(3)];

            J = [-2*q(3),	2*q(4),    -2*q(1),	2*q(2)
                2*q(2),     2*q(1),     2*q(4),	2*q(3)
                0,         -4*q(2),    -4*q(3),	0    ];
            step = (J'*F);
            step = step / norm(step);	% normalise step magnitude

            % Compute rate of change of quaternion
            qd = 0.5 * quatmultiply(q, [0 Gyroscope]) - Beta * step';

            % Integrate to yield quaternion
            q = q + qd * SamplePeriod;
            q = q / norm(q); % normalise quaternion
        end

        function q = ahrs(q, Gyroscope, Accelerometer, Magnetometer, SamplePeriod, Beta)

            % Normalise accelerometer measurement
            if(norm(Accelerometer) == 0), return; end	% handle NaN
            acc = Accelerometer / norm(Accelerometer);	% normalise magnitude

            % Normalise magnetometer measurement
            if(norm(Magnetometer) == 0), return; end	% handle NaN
            mag = Magnetometer / norm(Magnetometer);	% normalise magnitude

            % Reference direction of Earth's magnetic feild
            h = quatrotate(quatconj(q), mag);
            h = [0 h];
            b = [0 norm([h(2) h(3)]) 0 h(4)];

            % Gradient decent algorithm corrective step
             F= (quatrotate(q, [0 0 1]) - acc)';
             F = [F;  (quatrotate(q, [b(2) 0 b(4)]) - mag)'];

%             F = [2*(q(2)*q(4) - q(1)*q(3)) - Accelerometer(1)
%                 2*(q(1)*q(2) + q(3)*q(4)) - Accelerometer(2)
%                 2*(0.5 - q(2)^2 - q(3)^2) - Accelerometer(3)
%                 2*b(2)*(0.5 - q(3)^2 - q(4)^2) + 2*b(4)*(q(2)*q(4) - q(1)*q(3)) - Magnetometer(1)
%                 2*b(2)*(q(2)*q(3) - q(1)*q(4)) + 2*b(4)*(q(1)*q(2) + q(3)*q(4)) - Magnetometer(2)
%                 2*b(2)*(q(1)*q(3) + q(2)*q(4)) + 2*b(4)*(0.5 - q(2)^2 - q(3)^2) - Magnetometer(3)];
            J = [-2*q(3),                 	2*q(4),                    -2*q(1),                         2*q(2)
                2*q(2),                 	2*q(1),                    	2*q(4),                         2*q(3)
                0,                         -4*q(2),                    -4*q(3),                         0
                -2*b(4)*q(3),               2*b(4)*q(4),               -4*b(2)*q(3)-2*b(4)*q(1),       -4*b(2)*q(4)+2*b(4)*q(2)
                -2*b(2)*q(4)+2*b(4)*q(2),	2*b(2)*q(3)+2*b(4)*q(1),	2*b(2)*q(2)+2*b(4)*q(4),       -2*b(2)*q(1)+2*b(4)*q(3)
                2*b(2)*q(3),                2*b(2)*q(4)-4*b(4)*q(2),	2*b(2)*q(1)-4*b(4)*q(3),        2*b(2)*q(2)];
            step = (J'*F);
            step = step / norm(step);	% normalise step magnitude

            % Compute rate of change of quaternion
            qd = 0.5 * quatmultiply(q, [0 Gyroscope]) - Beta * step';

            % Integrate to yield quaternion
            q = q + qd * SamplePeriod;
            q = q / norm(q); % normalise quaternion
        end
    end
end


function Mh = hmxFusion(Mh)
%+========================================================================+
%|                                                                        |
%|         OPENHMX - LIBRARY FOR H-MATRIX COMPRESSION AND ALGEBRA         |
%|           openHmx is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : hmxFusion.m                                   |
%|    #    |   VERSION    : 0.40                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 14.03.2018                                    |
%| ( === ) |   SYNOPSIS   : Fusion and recompress for full, sparse and    |
%|  `---'  |                low-rank leaves                               |
%+========================================================================+

%%% H-Matrix
if (Mh.typ == 0)
    % Check and convert leaf data
    typ = zeros(1,4);
    for i = 1:4
        % Children data
        typ(i) = Mh.chd{i}.typ;
        dat    = Mh.chd{i}.dat;
        tol    = Mh.chd{i}.tol;
        dim    = size(Mh.chd{i});

        % Full or sparse matrix
        if (typ(i) == 2)
            % Non zeros terms
            n = nnz(dat);

            % Sparse
            if issparse(dat)
                % Compress
                if (n == 0)
                    A    = zeros(dim(1),0);
                    B    = zeros(0,dim(2));
                    flag = 1;

                % Full
                elseif (n > 1/4*prod(dim))
                    dat  = full(dat);
                    flag = 2;

                % Sparse
                else
                    flag = 0;
                end

            % Full
            else
                % Compress
                if (n == 0)
                    A    = zeros(dim(1),0);
                    B    = zeros(0,dim(2));
                    flag = 1;

                % Sparse
                elseif (n <= 1/4*prod(dim))
                    dat  = sparse(double(dat));
                    flag = 2;

                % Compress
                else
                    [A,B,flag] = hmxSVD(dat,tol);
                end
            end

            % Update
            if (flag == 1)
                Mh.chd{i}.dat = {A,B};
                Mh.chd{i}.typ = 1;
                typ(i)        = 1;

            elseif (flag == 2)
                Mh.chd{i}.dat = dat;
            end
        end
    end

    % Low-rank fusion (QRSVD)
    if (sum(typ==1) == 4)
        % Rank for each leaf
        rk = zeros(1,4);
        nk = 0;
        for i = 1:4
            rk(i) = size(Mh.chd{i}.dat{1},2);
            nk    = nk + sum(size(Mh.chd{i}))*rk(i);
        end

        % Low rank matrix
        A = zeros(size(Mh,1),sum(rk),class(Mh.pos{1}));
        B = zeros(sum(rk),size(Mh,2),class(Mh.pos{2}));
        j = 0;
        for i = 1:4
            A(Mh.row{i},j+(1:rk(i))) = Mh.chd{i}.dat{1};
            B(j+(1:rk(i)),Mh.col{i}) = Mh.chd{i}.dat{2};
            j = j + rk(i);
        end

        % Recompression
        [A,B] = hmxQRSVD(A,B,Mh.tol);

        % Update
        if (sum(size(Mh))*size(A,2) <= nk)
            Mh.typ = 1;
            Mh.row = cell(1,4);
            Mh.col = cell(1,4);
            Mh.chd = cell(1,4);
            Mh.dat = {A,B};
        end
    end

%%% Unavalaible case
else
    error('hmxFusion.m : unavailable case')
end
end


function varargout = acosh(varargin)

switch class(varargin{1})

    case 'sdpvar'
        varargout{1} = InstantiateElementWise(mfilename,varargin{:});

    case 'char'

        operator = CreateBasicOperator('positive','callback');
        operator.convexity = @convexity;
        operator.bounds = @bounds;
        operator.domain = [1 inf];

        varargout{1} = [];
        varargout{2} = operator;
        varargout{3} = varargin{3};

    otherwise
        error(['SDPVAR/' upper(mfilename) ' called with weird argument']);
end

function [L,U] = bounds(xL,xU)
if xU<=-1
    L = acosh(xU);
    U = acosh(xL);
elseif xL>=1
    L = acosh(xL);
    U = acosh(xU);
elseif xU <= 1
    L = 0;
    U = acosh(xL);
else
    L = 0;
    U = acosh(xU);
end

function vexity = convexity(xL,xU)
if xL >= 1
    vexity = 'concave';
elseif xU <= -1
    vexity = 'concave';
else
    vexity = 'none';
end


function val = objective(guess,IC)

%Runs a large number of simulations of the controlled simulation and then
%returns a combination of state and actuation error

duration = 3;
dt = 0.05;
n = round(duration/dt);

z = IC;

%Weighting between relative cost.
x_cost = 0.7^-2;
v_cost = 1.5^-2;
u_cost = 1.0^-2;

%Run simulation using RK4 integration in parallel
val = 0; %cost accumulator
for i=1:n
    [k1, u] = rhs(z,guess);
    k2 = rhs(z + 0.5*dt*k1,guess);
    k3 = rhs(z + 0.5*dt*k2,guess);
    k4 = rhs(z + k3,guess);

    z = z + (dt/6)*(k1 + 2*k2 + 2*k3 + k4);

    val = val + x_cost*sum(z(1,:).^2);
    val = val + v_cost*sum(z(2,:).^2);
    val = val + u_cost*sum(u.^2);
end

end

function [xdot, u] = rhs(z,k)

u = control(z,k);
xdot = dynamics(z,u);

end


function encmodel = standard_encoding(X,Y,T,options,binsize)
% Compute maps representing the "encoding" model for each
% time point (or window, see below) in the trial (i.e. a GML).
% The reported statistic is the explained variance from
% regressing Y on the data at this sensor/voxel.
%
% INPUT
% X: Brain data, (time by regions) or (time by trials by regions)
% Y: Stimulus, (time by q); q is no. of stimulus features OR
%              (no.trials by q), meaning that each trial has a single
%              stimulus value
% T: Length of series
% options: structure with the preprocessing options - see documentation in
%                       https://github.com/OHBA-analysis/HMM-MAR/wiki
% binsize: how many consecutive time points will be used for each estiamtion.
%           By default, this is 1, such that one encoding model
%               is estimated using 1 time point
%
% OUTPUT
% encmodel:  (time points by regions) maps of activation (explained variances)
%
% Author: Diego Vidaurre, OHBA, University of Oxford (2018)

options.Nfeatures = 0;
options.pca = 0;
options.embeddedlags = 0;
options.K = 1;
[X,Y,T] = preproc4hmm(X,Y,T,options);

if (binsize/2)==0
    warning(['binsize must be an odd number, setting to ' num2str(binsize+1)])
    binsize = binsize + 1;
end

N = length(T); ttrial = T(1);
p = size(X,2); q = size(Y,2);
X = reshape(X,[ttrial N p]);
Y = reshape(Y,[ttrial N q]);

if any(T~=ttrial), error('All trials must have the same length'); end
if nargin<5, binsize = 1; end

encmodel = NaN(ttrial,p);

for t = halfbin+1 : ttrial-halfbin
    r = t-halfbin:t+halfbin;
    for j = 1:p
        Xj = X(t,:,j);
        Xj = zscore(Xj(:));
        Yj = reshape(Y(r,:,:),binsize*N,q);
        beta = (Yj' * Yj) \ (Yj' * Xj);
        res = (Xj - Yj * beta).^2;
        res0 = Xj.^2;
        encmodel(t,j) = 1 - sum(res(:))/sum(res0(:));
    end
end
end


% Get vertex coordinates for the patch type 'T'
% [C, NW, NE, SW, SE] = HealpixGetPatchVertexCoordsT(n, i, j, INFO)
%
% Parameters
% n : grid resolution
% i : ring index
% j : intra-ring index
% INFO : intermediate information (output of HealpixSelectPatchClass())
% C : intra-patch coordinates
% NW : coordinates for north-east vertex
% NE : coordinates for north-west vertex
% SW, SE : coordinates for south vertex (always SW = SE since the patch is triangular shape)

function [C, NW, NE, SW, SE] = HealpixGetPatchVertexCoordsT(n, i, j, INFO)

int_j = fix(j);
if int_j == 0
    int_j = 4 * n;
end
east_int_j = mod(int_j + 1 - 1, 4 * INFO.int_i_n) + 1;

if INFO.is_south_pole == 0
    NW = [n, int_j];
    NE = [n, east_int_j];
    SW = [n + 1, int_j];
    SE = SW;

    decimal_i = INFO.decimal_i_n;
else
    SW = [3 * n, int_j];
    SE = [3 * n, east_int_j];
    NW = [3 * n - 1, int_j];
    NE = NW;

    decimal_i = 1 - INFO.decimal_i_n;
end

decimal_j = INFO.decimal_polar_intra_part;

if 1 > INFO.decimal_i_n
    C = [decimal_i, decimal_j / (1 - INFO.decimal_i_n)] - 0.5;
else
    C = [decimal_i, decimal_j] - 0.5;
end


close all; clear all;

%% Setting of the problem
global s
pde = fonedata; % f = 1;
pde.L = 1;
option.theta = 0.3;
option.estType = 'star';
option.maxIt = 20;
option.maxN = 2e5;
option.solver = 'mg';
option.yrefinement = 10; % ratio of h_y/h_x
option.gNquadorder = 5;
option.tol = 1e-6;
[node,elem] = squaremesh([-1,1,-1,1],0.5);
[node,elem] = delmesh(node,elem,'x>0 & y<0');
bdFlag = setboundary(node,elem,'Dirichlet');

%% s = 0.2
s = 0.2;
afemfracLap(node,elem,pde,bdFlag,option);

%% s = 0.4
s = 0.4;
afemfracLap(node,elem,pde,bdFlag,option);

%% s = 0.6
s = 0.6;
afemfracLap(node,elem,pde,bdFlag,option);

%% s = 0.8
s = 0.8;
afemfracLap(node,elem,pde,bdFlag,option);


function [out,Xt,str,ts] = rfnn_miso_grid(t,Xt,u,flag,itaVector,alphaVector, NumInVars,NumInTerms,x0,T)

% This program is an implementation of the on line RFNN (MISO) system.
% The structure of the network is determined by the user.
% The input space is partitioned using the grid-type method.
% All parameters of the network are estimated by Gradient Descent (GD)
% through error backpropagation.

    ninp = NumInVars;
    nout = 1;
   ninps = ninp+nout+1;  % number of inputs to sfunction [ x y LE ]
   NumRules = NumInTerms^NumInVars;  % Grid-Type Input Space Partitioning.
       ns = 4*NumInVars*NumInTerms + NumRules;
     nds = 3*NumInVars*NumInTerms + NumRules;
     % Learning Rates
     ita1 = itaVector(1); ita2 = itaVector(2);
     ita3 = itaVector(3); ita4 = itaVector(4);
     % Momentum Constants.
     alpha1 = alphaVector(1); alpha2 = alphaVector(2);
     alpha3 = alphaVector(3); alpha4 = alphaVector(4);
%  ----------------------- % initial informations --------------
if abs(flag)==0

    out = [0,ns+nds,1+ns+nds,ninps,0,1,1];    % states, outputs, inputs, ?, df, #ts
    str = [];                                 % API block consistency
     ts = T;                                  % sample time
    Xt = x0;
%  ----------------------- % state derivatives -----------------
elseif abs(flag) == 2

          x = u(1:ninp);
          e = u(ninp+1:ninp+nout);
   learning = u(ninp+nout+1);

if learning == 1

   % Unroll the states:
   off=1;
   off_end=NumInVars*NumInTerms;
   mean2=reshape(Xt(off:off_end),NumInVars,NumInTerms);

   off=off_end+1;
   off_end=off + NumInVars*NumInTerms-1;
   sigma2=reshape(Xt(off:off_end),NumInVars,NumInTerms);

   off=off_end+1;
   off_end=off+NumInVars*NumInTerms-1;
   Theta2=reshape(Xt(off:off_end),NumInVars,NumInTerms);

   off=off_end+1;
   off_end=off + NumRules-1;
   W = reshape(Xt(off:off_end),NumRules,1);

   off=off_end+1;
   off_end=off+NumInVars*NumInTerms-1;
   Out2 = reshape(Xt(off:off_end),NumInVars,NumInTerms);

   % Unroll the differential states:
   off=off_end+1;
   off_end=off + NumInVars*NumInTerms-1;
   dmean2=reshape(Xt(off:off_end),NumInVars,NumInTerms);

   off=off_end+1;
   off_end=off + NumInVars*NumInTerms-1;
   dsigma2=reshape(Xt(off:off_end),NumInVars,NumInTerms);

   off=off_end+1;
   off_end=off + NumInVars*NumInTerms-1;
   dTheta2=reshape(Xt(off:off_end),NumInVars,NumInTerms);

   off=off_end+1;
   off_end=off + NumRules-1;
   dW = reshape(Xt(off:off_end),NumRules,1);

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %                                                    FEEDFORWARD OPERATION                                                      %
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % LAYER 2 - INPUT TERM NODES
  Out2_pr = Out2;

 In2 = x*ones(1,NumInTerms) + Out2_pr.*Theta2;
 Out2 = exp(-((In2-mean2)./sigma2).^2);

% LAYER 3 - RULE (PRODUCT) NODES
precond = comb(Out2);
 Out3 = prod(precond,2);

%%%%%%%%%%%% END OF NETWORK FUNCTIONALITY SECTION %%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%			 					                 PARAMETER LEARNING SECTION	                       			                %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% BACKWARD PASS. Error Backpropagation
% LAYERS 4-3
delta3 = e*W;

% LAYER 2
Q = zeros(NumInVars,NumInTerms,NumRules);
for i=1:NumInVars
     for j=1:NumInTerms
          for k=1:NumRules
       	       if Out2(i,j)== precond(k,i) && Out2(i,j)~=0
      			  Q(i,j,k) = (Out3(k)/Out2(i,j))*delta3(k);
               end
          end
   	  end
 end

 ThetaE = sum(Q,3);

% LAYER 1 PARAMETER ADJUSTMENT BY GRADIENT DESCENT.
deltamean2  =  2*ThetaE.*Out2.*(In2-mean2)./((sigma2).^2);
deltasigma2 =  2*ThetaE.*Out2.*((In2-mean2).^2)./(sigma2.^3);
deltaTheta2 = -2*ThetaE.*Out2.*(In2-mean2).*Out2_pr./sigma2.^2;

dmean2 = ita2*deltamean2 + alpha2*dmean2;
 mean2  = mean2 + dmean2;

dsigma2 = ita3*deltasigma2 + alpha3*dsigma2;
  sigma2 = sigma2 + dsigma2;

dTheta2 = ita4*deltaTheta2 + alpha4*dTheta2;
 Theta2  = Theta2 + dTheta2;

% LAYER 4 PARAMETER ADJUSTMENT
  deltaW = e*Out3;
  dW = ita1*deltaW + alpha1*dW;
   W = W + dW;

%%%%%%%%%%%   END OF PARAMETER LEARNING PROCESS %%%%%%%%%
% State Vector Storage.
% Xt = [mean2 sigma2 Theta2 W Out2 dmean2 dsigma2 dTheta2 dW];

Xt = [ reshape(mean2,NumInVars*NumInTerms,1);
          reshape(sigma2,NumInVars*NumInTerms,1);
          reshape(Theta2,NumInVars*NumInTerms,1);
          W;
          reshape(Out2,NumInVars*NumInTerms,1);
          reshape(dmean2,NumInVars*NumInTerms,1);
          reshape(dsigma2,NumInVars*NumInTerms,1);
          reshape(dTheta2,NumInVars*NumInTerms,1);
          dW;];
end

out=Xt;

%  ----------------------- % outputs -------------------------
elseif flag == 3

  % Unpack the network's parameters first...
   off=1;
   off_end=NumInVars*NumInTerms;
   mean2=reshape(Xt(off:off_end),NumInVars,NumInTerms);

   off=off_end+1;
   off_end=off + NumInVars*NumInTerms-1;
   sigma2=reshape(Xt(off:off_end),NumInVars,NumInTerms);

   off=off_end+1;
   off_end=off+NumInVars*NumInTerms-1;
   Theta2 =reshape(Xt(off:off_end),NumInVars,NumInTerms);

   off=off_end+1;
   off_end=off + NumRules - 1;
   W = Xt(off:off_end);

   off=off_end+1;
   off_end=off+NumInVars*NumInTerms-1;
   Out2 = reshape(Xt(off:off_end),NumInVars,NumInTerms);

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %                                                       FEEDFORWARD OPERATION                                                       %
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % LAYER 1 - INPUT TERM NODES
  Out2_pr = Out2;

  x = u(1:ninp);
 In2 = x*ones(1,NumInTerms) + Out2_pr.*Theta2;
 Out2 = exp(-((In2-mean2)./sigma2).^2);

% LAYER 3 - RULE (PRODUCT) NODES
precond = comb(Out2);
 Out3 = prod(precond,2);

 % LAYER 4
  outact = W.'*Out3;

  % Block Outputs Vector Formation.
   out=[outact;Xt];

else
   out=[];
end


function tP = calcTriplePoints(gB)
%
% Input
%  gB - @grainBoundary

% list of phaseIds ordered as grainIds
grainId = gB.grainId;
grainPhaseId = zeros(max(grainId(:)),1);
grainPhaseId(grainId(grainId>0)) = gB.phaseId((grainId>0));

% compute triple points
[i,~,f] = find(gB.F);
I_VF = sparse(f,i,1,size(gB.V,1),size(gB.F,1));
I_VG = (I_VF * gB.I_FG)==2;
% triple points are those with exactly 3 neigbouring grains and 3
% boundary segments
itP = full(sum(I_VG,2)==3 & sum(I_VF,2)==3);
[tpGrainId,~] = find(I_VG(itP,:).');
tpGrainId = reshape(tpGrainId,3,[]).';
tpPhaseId = full(grainPhaseId(tpGrainId));

% compute ebsdId
% first step: compute faces at the triple point
% clean up incidence matrix
%I_FD(~any(I_FD,2),:) = [];
% incidence matrix between triple points and voronoi cells
%I_TD = I_VF(itP,:) * I_FD;
[tPBoundaryId,~] = find(I_VF(itP,:).');
tPBoundaryId = reshape(tPBoundaryId,3,[]).';

% get the three end vertices
iV = reshape(gB.F(tPBoundaryId,:),[],6).';
% TODO: the repmat can be removed in new versions of Matlab
iV = reshape(iV(iV ~= repmat(find(itP).',size(iV,1),1)).',3,[]).';

tP = triplePointList(find(itP),gB.V,...
  tpGrainId,tPBoundaryId,tpPhaseId,iV,gB.phaseMap,gB.CSList);

end


% This function produce short-time Fourier transform on the input signal
% according to the settings
% Author: Xiao Xiong
% Created: 2006
% Last modified: 24 Jun, 2006

function fft_x = sfft_multi(x,frame_size,frame_shift,FFT_length,window_type,do_DC_remove, useGPU, doDithering)

% x should be a TxN matrix, where T is the number of samples, and N is the
% number of channels.

if exist('do_DC_remove')==0 || length(do_DC_remove)==0
    do_DC_remove = 1;
end
if exist('useGPU')==0 || length(useGPU)==0
    useGPU = 0;
end

if exist('doDithering')==0 || length(doDithering)==0
   x = x + randn(size(x))/2^32;
end

% produce the hamming windowm
if exist('window_type')==0 || length(window_type)==0
    window = my_hamming(frame_size);
else
    switch window_type
        case 'hamming'
            window = my_hamming(frame_size);
        case 'hanning'
            window = hanning(frame_size);
        case 'rectangular'
            window = ones(frame_size,1);
    end
end

if do_DC_remove
    % DC offset removing
    x = DC_remove(gather(x),0.999);
    % pre-emphasis, boost the high frequency spectrum

    if 0
        % Method 2: Direct operation. In multichannel case, method 2 is
        % slower
        y = x(2:end,:) - 0.97*x(1:end-1,:);
        x = [x(1,:); y];
    else
        % Method 1: Call filter function. Too slow
        A    = [1 -0.97];
        x = filter(A,1,gather(x));
    end
end

if useGPU && ~IsInGPU(x)
    x = gpuArray(x);
end

if license('test', 'Signal_Toolbox')
    x_store = my_enframe(x, frame_size, frame_shift);
else
    for i=1:size(x,2)
        tmp = enframe(x(:,i), frame_size, frame_shift)';
        if i==1
            if useGPU
                x_store = gpuArray.zeros(size(tmp,1), size(tmp,2), size(x,2), class(gather(x)));
            else
                x_store = zeros(size(tmp,1), size(tmp,2), size(x,2), class(x));
            end
        end
        x_store(:,:,i) = tmp;
    end
    x_store = permute(x_store, [1 3 2]);
end

x_store = bsxfun(@times, x_store, window);

fft_x = fft(x_store,FFT_length);

N = FFT_length;
n = 0:N-1;
k = 0:N-1;
DFT_Trans = exp(-sqrt(-1)*2*pi*n(:)*k(:)'/N);
spec = DFT_Trans(1:N/2+1,1:frame_size) * squeeze(x_store(:,1,:));

end


classdef prtClassBinomial < prtClass
 %prtClassBinomial  Maximum a Posteriori classifier for binary data using
 %  IID binomial distributions for each feature (column) under each
 %  hypothesis (class)
 %
 %  Properties:
 %
 %       priorSuccesses = 1e-3;  %I think I did this right.
 %       priorTrials = 10000e-3;
 %

    properties (SetAccess=private)
        name = 'Binomial'
        nameAbbreviation = 'Binom'
        isNativeMary = true;
    end

    properties
        priorSuccesses = 1e-3;
        priorTrials = 10000e-3;
        pSuccessByClass = [];
    end

    methods
        % Constructor
        function self = prtClassBinomial(varargin)

            self.classTrain = 'prtDataInterfaceCategoricalTargets';
            self.classRun = 'prtDataSetBase';
            self.classRunRetained = false;

            self = prtUtilAssignStringValuePairs(self,varargin{:});
        end
    end

    methods (Access = protected, Hidden = true)

        function self = trainAction(self,ds)

            self.pSuccessByClass = [];
            for iClass = 1:ds.nClasses;
                classDs = ds.retainClassesByInd(iClass);
                pVec = (sum(classDs.X,1)+self.priorSuccesses)./(classDs.nObservations + self.priorTrials);
                pVec = full(pVec);
                self.pSuccessByClass(iClass,:) = pVec;
            end
        end

        function ds = runAction(self,ds)

            nClasses = size(self.pSuccessByClass,1);
            logLikelihoods = zeros(ds.nObservations, nClasses);
            for iY = 1:nClasses
                pVec = self.pSuccessByClass(iY,:);
                xOut = bsxfun(@times,ds.X,pVec) + bsxfun(@times,~ds.X,1-pVec);
                logLikelihoods(:,iY) = sum(log(xOut),2);
            end
            logLikelihoods = exp(bsxfun(@minus, logLikelihoods, prtUtilSumExp(logLikelihoods.').'));
            ds.X = logLikelihoods;
        end
    end
    methods
        function [sorted,pDist] = getSortedFeatures(self)
            % [sorted,pDist] = getSortedFeatures(self)
            %   Return the list of the features in sorted order.  pDist is
            %   the corresponding distance between the MLEs of the mean
            %   p(true)

            nClasses = size(self.pSuccessByClass,1);
            if nClasses ~= 2
                error('Only for binary problems');
            end
            pDist = self.pSuccessByClass(2,:)-self.pSuccessByClass(1,:);
            absDist = abs(pDist);
            [~,sorted] = sort(absDist,'descend');
            pDist = pDist(sorted);

        end
    end

end


function i4row_sort_a_test ( )

%*****************************************************************************80
%
%% I4ROW_SORT_A_TEST tests I4ROW_SORT_A;
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2009
%
%  Author:
%
%    John Burkardt
%
  m = 10;
  n = 4;
  b = 0;
  c = 10;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'I4ROW_SORT_A_TEST\n' );
  fprintf ( 1, '  For a rectangular integer matrix:\n' );
  fprintf ( 1, '  I4ROW_SORT_A sorts the rows;\n' );

  seed = 123456789;

  [ a, seed ] = i4mat_uniform_ab ( m, n, b, c, seed );

  i4mat_print ( m, n, a, '  The original matrix:' );

  a = i4row_sort_a ( m, n, a );

  i4mat_print ( m, n, a, '  The row-sorted matrix:' );

  return
end


function [energy] = getSpecOrbitEnergyFromSma(sma, gmu)
%getSpecOrbitEnergyFromSma Summary of this function goes here
%   Detailed explanation goes here

    energy = -gmu/(2*sma);
end


function [x,fval,exitflag,info,Opt] = opti_mintprog(f,A,b,Aeq,beq,lb,ub,int,sos,opts)
%OPTI_MINTPROG Solve a MILP using an OPTI MILP Solver (Matlab-Like Overload)
%
%   [x,fval,exitflag,info] = opti_mintprog(f,A,b,Aeq,beq,lb,ub,int) solves
%   the linear program min f'x where A,b are the inequality constraints,
%   Aeq,beq are the equality constraints, lb,ub are the decision variable
%   bounds and int is a string of integer variables ('C', 'I', 'B').
%
%   [x,fval,exitflag,info] = opti_minprog(f,...,int,sos) allows Special
%   Ordered Sets (SOS) to be specified via the sos structure. Fields must
%   include 'type' ('1' or '2'), 'index' (vector) and 'weight' (vector).
%   Multiple SOS can be supplied as cell arrays.
%
%   [x,fval,exitflag,info] = opti_mintprog(f,...,sos,opts) allows the user
%   to specify optiset options. This includes specifying a solver via the
%   'solver' field of optiset.
%
%   [x,...,info,Opt] = opti_mintprog(f,...) returns the internally built
%   OPTI object.

%   Copyright (C) 2011 Jonathan Currie (I2C2)

% Handle missing arguments
if nargin < 10, opts = optiset; end
if nargin < 9, sos = []; end
if nargin < 8, int = []; end
if nargin < 7, ub = []; end
if nargin < 6, lb = []; end
if nargin < 5, beq = []; end
if nargin < 4, Aeq = []; end
if nargin < 3, error('You must supply at least 3 arguments to opti_mintprog'); end

%Check SOS
if(~isempty(sos))
    if(~isfield(sos,'type')), error('The SOS structure must have the field type!'); end
    if(~isfield(sos,'index')), error('The SOS structure must have the field index!'); end
    if(~isfield(sos,'weight')), error('The SOS structure must have the field weight!'); end
end

%Build OPTI Object
Opt = opti('f',f,'ineq',A,b,'eq',Aeq,beq,'bounds',lb,ub,'int',int,'sos',sos,'options',opts);

%Solve
[x,fval,exitflag,info] = solve(Opt);


function L = spm_mesh_get_lm(M,T)
% Identification of local maxima on a textured surface mesh
% FORMAT L = spm_mesh_get_lm(M,T)
% M        - a [nx3] faces array or a patch structure or a [nxn] adjacency
%            matrix
% T        - a [nx1] texture vector
%
% L        - indices of vertices that are local maxima
%__________________________________________________________________________
% Copyright (C) 2010-2016 Wellcome Trust Centre for Neuroimaging

% Guillaume Flandin
% $Id: spm_mesh_get_lm.m 6867 2016-09-12 15:04:44Z guillaume $

%-Get adjacency matrix
%--------------------------------------------------------------------------
if ~isnumeric(M) || size(M,1) ~= size(M,2)
    A = spm_mesh_adjacency(M);
else
    A = M;
end

%-Get neighbours, restricted to vertices that actually have data (~= NaN)
%--------------------------------------------------------------------------
out      = isnan(T(:)');
if all(out), L = []; return; end             % empty domain
A(:,out) = 0;
A(out,:) = 0;
N        = spm_mesh_neighbours(A);
if isempty(N), L = find(~out); return; end   % only singletons
%S       = ~any(N(~out,:),2);                % some singletons

%-Identify local maxima
%--------------------------------------------------------------------------
T        = [T; -Inf];
N(N<1)   = numel(T);
L        = all(bsxfun(@gt,T(1:end-1),T(N)),2);
L        = find(L');


function [data,units] = compute_max_wing_angle(trx,n)

flies = trx.exp2flies{n};
nflies = numel(flies);
data = cell(1,nflies);
for i = 1:nflies,
  fly = flies(i);

  % signed maximum: left wing will be negative, right wing positive
  data{i} = max(-trx(fly).wing_anglel,trx(fly).wing_angler);

end
units = parseunits('rad');


function p=polelague(n)

% p=polegend(n)
% Almacena en las filas de la matriz p los coefs de los polinomios de Legendre

p(1,1)=1;
p(2,1:2)=[-1 1];
for k=2:n
   p(k+1,1:k+1)=((2*(k-2)*[0 p(k,1:k)]+3*[0 p(k,1:k)]-[p(k,1:k) 0]-(k-1).^2*[0 0 p(k-1,1:k-1)]));
end


function [l,L_k,L_hm,L_beta] = invPinHoleAPlucker(Sk,hm,beta)

% INVPINHOLEAPLUCKER Retro-projects anchored plucker line
%   INVPINHOLEAPLUCKER(K,L,BETA) retro-projects the anchored Plucker line
%   from the homogeneous 2D line HM and a pin hole camera K=[u0;v0;au;av]
%   at the origin. BETA specifies the unobservable direction of the line.
%   BETA is a 2-vector expressed in the plane base given by
%   PLANEVEC2PLANEBASE.
%
%   [l,L_k,L_hm,L_beta] = ... returns Jacobians wrt K, HM and BETA.
%
%   See also INVPINHOLEPLUCKER.

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

if nargout == 1
    % Plucker line
    pl = invPinHolePlucker(Sk,hm,beta) ;

    % anchored Plucker line L
    l = anchorPlucker(pl,[0;0;0]);

else
    % Plk lin in Sensor Frame
    [pl, PL_k, PL_hm, PL_beta] = invPinHolePlucker(Sk,hm,beta) ;

    % L in sensor frame
    [l, L_pl] = anchorPlucker(pl,[0;0;0]);

    % chain rule
    L_k    = L_pl * PL_k;
    L_hm   = L_pl * PL_hm;
    L_beta = L_pl * PL_beta;

end

% ========== 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 slide30

	a = [0 0 0;
		1 0 0;
		1 1 0;
		0 1 0;
		0 0 1;
		1 0 1;
		1 1 1;
		0 1 1];

	b = [1 2 6 5;
		2 3 7 6;
		3 4 8 7;
		4 1 5 8;
		1 2 3 4;
		5 6 7 8];

	p1 = patch('faces',b,...
		'vertices',a,...
		'facecolor',[.5 .5 .5],...
		'edgecolor',[1,1,1],...
		'facealpha',0.5);

	view(3)
	axis square

end


function [y,deriv] = compose_mv(outer,inner,x)
% COMPOSE_MV is an MV2DF (see MV2DF_API_DEFINITION.readme) which represents
% the combination of two functions. If 'outer' is an MV2DF for a function
% g() and 'inner' for a function f(), then this MV2DF represents g(f(x)).

%feature scopedaccelenablement off

if nargin==0
    test_this();
    return;
end

if isempty(x)
    y = @(w)compose_mv(outer,inner,w);
    return;
end

if isa(x,'function_handle')
    fh = compose_mv(outer,inner,[]);  % fh =@(x) outer(inner(x))
    y = compose_mv(fh,x,[]);          %  y =@(w) outer(inner(x(w)))
    return;
end

% if ~isa(outer,'function_handle')
%     outer = const_mv2df([],outer);
% end
% if ~isa(inner,'function_handle')
%     inner = const_mv2df([],inner);
% end

if nargout==1
    y = outer(inner(x));
    return;
end

[y1,deriv1] = inner(x);
[y,deriv2] = outer(y1);
deriv = @(g3) deriv_this(deriv1,deriv2,g3);

function [g,hess,linear] = deriv_this(deriv1,deriv2,g3)

if nargout==1
    g = deriv1(deriv2(g3));
    return;
end

[g2,hess2,lin2] = deriv2(g3);
[g,hess1,lin1] = deriv1(g2);

hess =@(d) hess_this(deriv1,hess1,hess2,lin1,lin2,d);

linear = lin1 && lin2;

function [h,Jv] = hess_this(deriv1,hess1,hess2,lin1,lin2,d)

if nargout==1
    if ~lin2
        [h1,Jv1] = hess1(d);
        h2 = hess2(Jv1);
        h2 = deriv1(h2);
    elseif ~lin1
        h1 = hess1(d);
    end
else
    [h1,Jv1] = hess1(d);
    [h2,Jv] = hess2(Jv1);
    if ~lin2
        h2 = deriv1(h2);
    end
end

if lin1 && lin2
    h=[];
elseif (~lin1) && (~lin2)
    h = h1+h2;
elseif lin1
    h = h2;
else % if lin2
    h = h1;
end

function test_this()

fprintf('-------------- Test 1 ------------------------\n');
fprintf('Composition g(f(w)): f() is non-linear and g() is non-linear:\n');
A = randn(4,5);
B = randn(5,4);

w = [A(:);B(:)];

f = @(w) gemm(w,4,5,4);
g1 = gemm(f,2,4,2);

test_MV2DF(g1,w);
fprintf('--------------------------------------\n\n');

fprintf('-------------- Test 2 ------------------------\n');
fprintf('Composition g(f(w)): f() is linear and g() is non-linear:\n');
A = randn(4,5);
B = randn(5,4);

w = [A(:);B(:)];

T = randn(40);
f = @(w) linTrans(w,@(x)T*x,@(y)T.'*y);
g2 = gemm(f,4,5,4);

test_MV2DF(g2,w);
fprintf('--------------------------------------\n\n');

fprintf('-------------- Test 3 ------------------------\n');
fprintf('Composition g(f(w)): f() is non-linear and g() is linear:\n');
A = randn(4,5);
B = randn(5,4);

w = [A(:);B(:)];
f = @(w) gemm(w,4,5,4);

T = randn(16);
g3 = linTrans(f,@(x)T*x,@(y)T.'*y);

test_MV2DF(g3,w);
fprintf('--------------------------------------\n\n');

fprintf('-------------- Test 4 ------------------------\n');
fprintf('Composition g(f(w)): f() is linear and g() is linear:\n');
w = randn(10,1);
T1 = randn(11,10);
f = @(w) linTrans(w,@(x)T1*x,@(y)T1.'*y);

T2 = randn(5,11);
g4 = linTrans(f,@(x)T2*x,@(y)T2.'*y);

test_MV2DF(g4,w);
fprintf('--------------------------------------\n\n');


classdef IMMOEA_F8 < PROBLEM
% <multi> <real> <large/none>
% Benchmark MOP for testing IM-MOEA

%------------------------------- Reference --------------------------------
% R. Cheng, Y. Jin, K. Narukawa, and B. Sendhoff, A multiobjective
% evolutionary algorithm using Gaussian process-based inverse modeling,
% IEEE Transactions on Evolutionary Computation, 2015, 19(6): 838-856.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 3;
            if isempty(obj.D); obj.D = 30; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,X)
            t = X(:,3:obj.D).^(1./(1+3*repmat(3:obj.D,size(X,1),1)/obj.D)) - repmat(X(:,1),1,obj.D-2);
            g = sum(t.^2,2);
            PopObj(:,1) = cos(pi/2*X(:,1)).*cos(pi/2*X(:,2)).*(1+g);
            PopObj(:,2) = cos(pi/2*X(:,1)).*sin(pi/2*X(:,2)).*(1+g);
            PopObj(:,3) = sin(pi/2*X(:,1)).*(1+g);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R = UniformPoint(N,3);
            R = R./repmat(sqrt(sum(R.^2,2)),1,3);
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            a = linspace(0,pi/2,10)';
            R = {sin(a)*cos(a'),sin(a)*sin(a'),cos(a)*ones(size(a'))};
        end
    end
end


function affineTransformationMatrix = get_affine_matrix(this)
% Transforms geometry parameters into affine 4x4 matrix (T*R*Z*S)
% with T = Translation, R = Rotation, Z = Zooming (Scaling with Resolution)
%      S = Shearing
% i.e. performing operations in the order from right to left, in particular
% rotation before translation
%
%   Y = MrImageGeometry()
%   Y.get_affine_matrix(inputs)
%
% This is a method of class MrImageGeometry.
%
% IN
%
% OUT
%
% EXAMPLE
%   get_affine_matrix
%
%   See also MrImageGeometry tapas_uniqc_spm_matrix

% Author:   Saskia Klein & Lars Kasper
% Created:  2014-07-15
% Copyright (C) 2014 Institute for Biomedical Engineering
%                    University of Zurich and ETH Zurich
%
% This file is part of the TAPAS UniQC 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/>.

P(1:3) = this.offcenter_mm;
P(4:6) = this.rotation_deg*pi/180;
P(7:9) = this.resolution_mm;
P(10:12) = this.shear;

affineTransformationMatrix = tapas_uniqc_spm_matrix(P);


function [n_best P_best V C]=fast_oopsi(F,V,P)
% this function solves the following optimization problem:
% (*) n_best = argmax_{n >= 0} P(n | F)
% which is a MAP estimate for the most likely spike train given the
% fluorescence signal.  given the model:
%
% <latex>
% \begin{align}
% C_t &= \gamma*C_{t-1} + n_t, \qquad & n_t & \sim \text{Poisson}(n_t; \lamda_t \Delta)
% F_t &= \alpha(C_t + \beta) + \sigma \varepsilon_t, &\varepsilon_t &\sim \mathcal{N}(0,1)
% \end{align}
% </latex>
%
% if F_t is a vector, then 'a' is a vector as well
% we approx the Poisson with an Exponential (which means we don't require integer numbers of spikes).
% we take an "interior-point" approach to impose the nonnegative contraint on (*).
% each step is solved in O(T)
% time by utilizing gaussian elimination on the tridiagonal hessian, as
% opposed to the O(T^3) time typically required for non-negative
% deconvolution.
%
% Input---- only F is REQUIRED.  the others are optional
% F:        fluorescence time series (can be a vector (1 x T) or a matrix (Np x T)
%
% V.        structure of algorithm Variables
%   Ncells: # of cells within ROI
%   T:      # of time steps
%   Npixels:# of pixels in ROI
%   dt:     time step size, ie, frame duration, ie, 1/(imaging rate)
%   n:      if true spike train is known, and we are plotting, plot it (only required is est_a==1)
%   h:      height of ROI (assumes square ROI) (# of pixels) (only required if est_a==1 and we are plotting)
%   w:      width of ROI (assumes square ROI) (# of pixels) (only required if est_a==1 and we are plotting)
%
%   THE FOLLOWING FIELDS CORRESPOND TO CHOICES THAT THE USER MAKE
%
%   fast_poiss:     1 if F_t ~ Poisson, 0 if F_t ~ Gaussian
%   fast_nonlin:    1 if F_t is a nonlinear f(C_t), and 0 if F_t is a linear f(C_t)
%   fast_plot:      1 to plot results after each pseudo-EM iteration, 0 otherwise
%   fast_thr:       1 if thresholding inferred spike train before estiamting {a,b}
%   fast_iter_max:  max # of iterations of pseudo-EM  (1 to use default initial parameters)
%   fast_ignore_post: 1 to keep iterating pseudo-EM even if posterior is not increasing, 0 otherwise
%
%   THE BELOW FIELDS INDICATE WHETHER ONE WANTS TO ESTIMATE EACH OF THE
%   PARAMETERS. IF ANY IS SET TO ZERO, THEN WE DO NOT TRY TO UPDATE THE
%   ORIGINAL ESTIMATE, GIVEN EITHER BY THE USER, OR THE INITIAL ESTIMATE
%   FROM THE CODE
%
%   est_sig:    1 to estimate sig
%   est_lam:    1 to estimate lam
%   est_gam:    1 to estimate gam
%   est_b:      1 to estimate b
%   est_a:      1 to estimate a
%
% P.        structure of neuron model Parameters
%
%   a:      spatial filter
%   b:      background fluorescence
%   sig:    standard deviation of observation noise
%   gam:    decayish, ie, tau=dt/(1-gam)
%   lam:    firing rate-ish, ie, expected # of spikes per frame
%
% Output---
% n_best:   inferred spike train
% P_best:   inferred parameter structure
% V:        structure of Variables for algorithm to run

%% initialize algorithm Variables
starttime   = cputime;
siz         = size(F);      if siz(2)==1, F=F'; siz=size(F); end
j=0;
% variables determined by the data
if nargin < 2,              V   = struct;       end
if ~isfield(V,'Ncells'),    V.Ncells = 1;       end     % # of cells in image
if ~isfield(V,'T'),         V.T = siz(2);       end     % # of time steps
if ~isfield(V,'Npixels'),   V.Npixels = siz(1); end     % # of pixels in ROI
if ~isfield(V,'dt'),                                    % frame duration
    fr = input('\nwhat was the frame rate for this movie (in Hz)?: ');
    V.dt = 1/fr;
end

% variables determined by the user
if ~isfield(V,'fast_poiss'),V.fast_poiss = 0;   end     % whether observations are Poisson
if ~isfield(V,'fast_nonlin'),   V.fast_nonlin   = 0; end
if V.fast_poiss && V.fast_nonlin,
    reply = input('\ncan be nonlinear observations and poisson, \ntype 1 for nonlin, 2 for poisson, anything else for neither: ');
    if reply==1,        V.fast_poiss = 0;   V.fast_nonlin = 1;
    elseif reply==2,    V.fast_poiss = 1;   V.fast_nonlin = 0;
    else                V.fast_poiss = 0;   V.fast_nonlin = 0;
    end
end
if ~isfield(V,'fast_iter_max'), V.fast_iter_max=1; end % max # of iterations before convergence

% things that matter if we are iterating to estimate parameters
if V.fast_iter_max>1;
    if V.fast_poiss || V.fast_nonlin,
        disp('\ncode does not currrently support estimating parameters for \npoisson or nonlinear observations');
        V.fast_iter_max=1;
    end

    if ~isfield(V,'fast_plot'), V.fast_plot = 0; end
    if V.fast_plot==1
        FigNum = 400;
        if V.Npixels>1, figure(FigNum), clf, end        % figure showing estimated spatial filter
        figure(FigNum+1), clf                           % figure showing estimated spike trains
        if isfield(V,'n'), siz=size(V.n); V.n(V.n==0)=NaN; if siz(1)<siz(2), V.n=V.n'; end; end
    end

    if ~isfield(V,'est_sig'),   V.est_sig   = 0; end    % whether to estimate sig
    if ~isfield(V,'est_lam'),   V.est_lam   = 0; end    % whether to estimate lam
    if ~isfield(V,'est_gam'),   V.est_gam   = 0; end    % whether to estimate gam
    if ~isfield(V,'est_a'),     V.est_a     = 0; end    % whether to estimate a
    if ~isfield(V,'est_b'),     V.est_b     = 1; end    % whether to estimate b
    if ~isfield(V,'fast_plot'), V.fast_plot = 1; end    % whether to plot results from each iteration
    if ~isfield(V,'fast_thr'),  V.fast_thr  = 0; end    % whether to threshold spike train before estimating 'a' and 'b'
    if ~isfield(V,'fast_ignore_post'), V.fast_ignore_post=0; end % whether to ignore the posterior, and just keep the last iteration
end

% normalize F if it is only a trace
if V.Npixels==1
    F=detrend(F);
    F=F-min(F)+eps;
end

%% set default model Parameters

if nargin < 3,          P       = struct;                       end
if ~isfield(P,'sig'),   P.sig   = mean(mad(F',1)*1.4826);       end
if ~isfield(P,'gam'),   P.gam   = (1-V.dt/1)*ones(V.Ncells,1);  end
if ~isfield(P,'lam'),   P.lam   = 10*ones(V.Ncells,1);          end
if ~isfield(P,'a'),     P.a     = median(F,2);                  end

if ~isfield(P,'b'),
    if V.Npixels==1, P.b = quantile(F,0.05);
    else P.b=median(F,2);
    end
end

%% define some stuff needed for est_MAP function

% for brevity and expediency
Z   = zeros(V.Ncells*V.T,1);                    % zero vector
M   = spdiags([repmat(-P.gam,V.T,1) repmat(Z,1,V.Ncells-1) (1+Z)], -V.Ncells:0,V.Ncells*V.T,V.Ncells*V.T);  % matrix transforming calcium into spikes, ie n=M*C
I   = speye(V.Ncells*V.T);                      % create out here cuz it must be reused
H1  = I;                                        % initialize memory for Hessian matrix
H2  = I;                                        % initialize memory for other part of Hessian matrix
d0  = 1:V.Ncells*V.T+1:(V.Ncells*V.T)^2;        % index of diagonal elements of TxT matrices
d1  = 1+V.Ncells:V.Ncells*V.T+1:(V.Ncells*V.T)*(V.Ncells*(V.T-1)); % index of off-diagonal elements of TxT matrices
posts = Z(1:V.fast_iter_max);                   % initialize likelihood
if numel(P.lam)==V.Ncells
    lam = V.dt*repmat(P.lam,V.T,1);             % for lik
elseif numel(P.lam)==V.Ncells*V.T
    lam = V.dt*P.lam;
else
    error('lam must either be length V.T or 1');
end

if V.fast_poiss==1
    H       = I;                                % initialize memory for Hessian matrix
    gamlnF  = gammaln(F+1);                     % for lik
    sumF    = sum(F,1)';                        % for grad & Hess
end

%% if not iterating to estimate parameters, only this is necessary
[n C posts(1)] = est_MAP(F,P);
n_best = n;
P_best = P;
V.fast_iter_tot = 1;
V.post = posts(1);
post_max = posts(1);

if V.fast_iter_max>1
    options = optimset('Display','off');        % don't show warnings for parameter estimation
    i       = 1;                                % iteration #
    i_best  = i;                                % iteration with highest likelihood
    conv    = 0;                                % whether algorithm has converged yet
else
    conv    = 1;
end

%%  if parameters are unknown, do pseudo-EM iterations
while conv == 0
    if V.fast_plot == 1, MakePlot(n,F,P,V); end % plot results from previous iteration
    i               = i+1;                      % update iteratation number
    V.fast_iter_tot = i;                        % record of total # of iterations
    P               = est_params(n,C,F,P,b);    % update parameters based on previous iteration
    [n C posts(i)]  = est_MAP(F,P);             % update inferred spike train based on new parameters

    if posts(i)>post_max || V.fast_ignore_post==1% if this is the best one, keep n and P
        n_best  = n;                            % keep n
        P_best  = P;                            % keep P
        i_best  = i;                            % keep track of which was best
        post_max= posts(i);                     % keep max posterior
    end

    % if lik doesn't change much (relatively), or returns to some previous state, stop iterating
    if  i>=V.fast_iter_max || (abs((posts(i)-posts(i-1))/posts(i))<1e-3 || any(posts(1:i-1)-posts(i))<1e-5)% abs((posts(i)-posts(i-1))/posts(i))<1e-5 || posts(i-1)-posts(i)>1e5;
        MakePlot(n,F,P,V);
        disp('convergence criteria met')
        V.post  = posts(1:i);
        conv    = 1;
    end
    sound(3*sin(linspace(0,90*pi,2000)))        % play sound to indicate iteration is over
end

V.fast_time = cputime-starttime;                % time to run code
V           = orderfields(V);                   % order fields alphabetically to they are easier to read
P_best      = orderfields(P_best);
% n_best      = n_best./repmat(max(n_best),V.T,1);

P_best.j=j;

%% fast filter function
    function [n C post] = est_MAP(F,P)

        % initialize n and C
        z = 1;                                  % weight on barrier function
        llam = reshape(1./lam',1,V.Ncells*V.T)';
        if V.fast_nonlin==1
            n = V.gauss_n;
        else
            n = 0.01+0*llam;                    % initialize spike train
        end
        C = 0*n;                                % initialize calcium
        for j=1:V.Ncells
            C(j:V.Ncells:end) = filter(1,[1, -P.gam(j)],n(j:V.Ncells:end)); %(1-P.gam(j))*P.b(j);
        end

        % precompute parameters required for evaluating and maximizing likelihood
        b           = repmat(P.b,1,V.T);       % for lik
        if V.fast_poiss==1
            suma    = sum(P.a);                 % for grad
        else
            M(d1)   = -repmat(P.gam,V.T-1,1);   % matrix transforming calcium into spikes, ie n=M*C
            ba      = P.a'*b; ba=ba(:);         % for grad
            aa      = repmat(diag(P.a'*P.a),V.T,1);% for grad
            aF      = P.a'*F; aF=aF(:);         % for grad
            e       = 1/(2*P.sig^2);            % scale of variance
            H1(d0)  = -2*e*aa;                   % for Hess
        end
        grad_lnprior  = M'*llam;                  % for grad

        % find C = argmin_{C_z} lik + prior + barrier_z
        while z>1e-13                           % this is an arbitrary threshold

            if V.fast_poiss==1
                Fexpect = P.a*(C+b')';          % expected poisson observation rate
                lik = -sum(sum(-Fexpect+ F.*log(Fexpect) - gamlnF)); % lik
            else
                if V.fast_nonlin==1
                    S = C./(C+P.k_d);
                else
                    S = C;
                end
                D = F-P.a*(reshape(S,V.Ncells,V.T))-b; % difference vector to be used in likelihood computation
                lik = e*D(:)'*D(:);             % lik
            end
            post = lik + llam'*n - z*sum(log(n));
            s    = 1;                           % step size
            d    = 1;                           % direction
            while norm(d)>5e-2 && s > 1e-3      % converge for this z (again, these thresholds are arbitrary)
                if V.fast_poiss==1
                    glik    = suma - sumF./(C+b');
                    H1(d0)  = sumF.*(C+b').^(-2); % lik contribution to Hessian
                elseif V.fast_nonlin==1
                    glik    = -2*P.a*P.k_d*D'.*(C+P.k_d).^-2;
                    H1diag  = (-P.a*P.k_d-2*(C+P.k_d).*D').*((C+P.k_d).^-4);
                    H1(d0)  = H1diag;
                else
                    glik    = -2*e*(aF-aa.*C-ba);  % gradient
                end
                g       = glik + grad_lnprior - z*M'*(n.^-1);
                H2(d0)  = n.^-2;                % log barrier part of the Hessian
                H       = H1 - z*(M'*H2*M);     % Hessian
                d   = H\g;                     % direction to step using newton-raphson
                hit = -n./(M*d);                % step within constraint boundaries
                hit=hit(hit>0);
                if any(hit<1)
                    s = min(1,0.99*min(hit));
                else
                    s = 1;
                end
                post1 = post+1;
                while post1>=post+1e-7          % make sure newton step doesn't increase objective
                    C1  = C+s*d;
                    n   = M*C1;
                    if V.fast_poiss==1
                        Fexpect = P.a*(C1+b')';
                        lik1    = -sum(sum(-Fexpect+ F.*log(Fexpect) - gamlnF));
                    else
                        if V.fast_nonlin==1
                            S1 = C1./(C1+P.k_d);
                        else
                            S1 = C1;
                        end
                        D = F-P.a*(reshape(S1,V.Ncells,V.T))-b; % difference vector to be used in likelihood computation
                        lik1 = e*D(:)'*D(:);             % lik
                    end
                    post1 = lik1 + llam'*n - z*sum(log(n));
                    s   = s/5;                  % if step increases objective function, decrease step size

                    if s<1e-20; disp('reducing s further did not increase likelihood'), break; end      % if decreasing step size just doesn't do it
                end
                C    = C1;                      % update C
                post = post1;                   % update post
            end
            z=z/10;                             % reduce z (sequence of z reductions is arbitrary)
        end

        % reshape things in the case of multiple neurons within the ROI
        n=reshape(n,V.Ncells,V.T)';
        C=reshape(C,V.Ncells,V.T)';
    end

%% Parameter Update
    function P = est_params(n,C,F,P,b)

        % generate regressor for spatial filter
        if V.est_a==1 || V.est_b==1
            if V.fast_thr==1
                CC=0*C;
                for j=1:V.Ncells
                    nsort   = sort(n(:,j));
                    nthr    = nsort(round(0.98*V.T));
                    nn      = Z(1:V.T);
                    nn(n(:,j)<=nthr)=0;
                    nn(n(:,j)>nthr)=1;
                    CC(:,j) = filter(1,[1 -P.gam(j)],nn) + (1-P.gam(j))*P.b(j);
                end
            else
                CC      = C;
            end

            if V.est_b==1
                A = [CC -1+Z(1:V.T)];
            else
                A=CC;
            end
            X = A\F';

            P.a = X(1:V.Ncells,:)';
            if V.est_b==1
                P.b = X(end,:)';
                b   = repmat(P.b,1,V.T);
            end

            D   = F-P.a*(reshape(C,V.Ncells,V.T)) - b;

            mse = D(:)'*D(:);
        end

        if V.est_a==0 && V.est_b==0 && (V.est_sig==1 || V.est_lam==1),
            D   = F-P.a*(reshape(C,V.Ncells,V.T)+b);
            mse = D(:)'*D(:);
        end

        % estimate other parameters
        if V.est_sig==1,
            P.sig = sqrt(mse)/V.T;
        end
        if V.est_lam==1,
            nnorm   = n./repmat(max(n),V.T,1);
            if numel(P.lam)==V.Ncells
                P.lam   = sum(nnorm)'/(V.T*V.dt);
                lam     = repmat(P.lam,V.T,1)*V.dt;
            else
                P.lam   = nnorm/(V.T*V.dt);
                lam     = P.lam*V.dt;
            end

        end
    end

%% MakePlot
    function MakePlot(n,F,P,V)
        if V.fast_plot == 1
            if V.Npixels>1                                     % plot spatial filter
                figure(FigNum), nrows=V.Ncells;
                for j=1:V.Ncells, subplot(1,nrows,j),
                    imagesc(reshape(P.a(:,j),V.w,V.h)),
                    title('a')
                end
            end

            figure(FigNum+1),  ncols=V.Ncells; nrows=3; END=V.T; h=zeros(V.Ncells,2);
            for j=1:V.Ncells                                  % plot inferred spike train
                h(j,1)=subplot(nrows,ncols,(j-1)*ncols+1); cla
                if V.Npixels>1, Ftemp=mean(F); else Ftemp=F; end
                plot(z1(Ftemp(2:END))+1), hold on,
                bar(z1(n_best(2:END,j)))
                title(['best iteration ' num2str(i_best)]),
                axis('tight')
                set(gca,'XTickLabel',[],'YTickLabel',[])

                h(j,2)=subplot(nrows,ncols,(j-1)*ncols+2); cla
                bar(z1(n(2:END,j)))
                if isfield(V,'n'), hold on,
                    for k=1:V.Ncells
                        stem(V.n(2:END,k)+k/10,'LineStyle','none','Marker','v','MarkerEdgeColor','k','MarkerFaceColor','k','MarkerSize',2)
                    end
                end
                set(gca,'XTickLabel',[],'YTickLabel',[])
                title(['current iteration ' num2str(i)]),
                axis('tight')
            end

            subplot(nrows,ncols,j*nrows),
            plot(1:i,posts(1:i))    % plot record of likelihoods
            title(['max lik ' num2str(post_max,4), ',   lik ' num2str(posts(i),4)])
            set(gca,'XTick',2:i,'XTickLabel',2:i)
            drawnow
        end
    end
end


classdef PartialDCT < spx.dict.Operator

properties(SetAccess=private)
    % Dimensions
    row_pics
    col_perm
    num_rows
    num_cols
end

methods

function self = PartialDCT(row_pics, col_perm)
    if nargin < 2
        error('Row selection and column permutation must be specified');
    end
    self.row_pics = row_pics;
    self.col_perm = col_perm;
    self.num_rows = numel(row_pics);
    self.num_cols = numel(col_perm);
    if self.num_rows > self.num_cols
        error('Number of rows cannot be greater than number of columns.');
    end
end

function [mm, nn]  = get_size(self)
    mm = self.num_rows;
    nn = self.num_cols;
end

function result = apply(self, vectors)
    if size(vectors, 1) ~= self.num_cols
        error('Dimensions mismatch');
    end
    result = dct(vectors(self.col_perm, :));
    result = result(self.row_pics, :);
end

function result = apply_ctranspose(self, vectors)
    if size(vectors, 1) ~= self.num_rows
        error('Dimensions mismatch');
    end
    num_vectors = size(vectors, 2);
    full_vectors = zeros(self.num_cols, num_vectors);
    full_vectors(self.row_pics, :) = vectors;
    result = zeros(self.num_cols, num_vectors);
    result(self.col_perm, :) = idct(full_vectors);
end

function result = double(self)
    % Converts the operator into a MATRIX
    result = self.apply(eye(self.num_cols));
end

function result = norm(self)
    error('Not implemented');
end

end

end


% DEMBRENDAN1 Use the GP-LVM to model the Frey face data with back constraints.

% FGPLVM

% Fix seeds
randn('seed', 1e5);
rand('seed', 1e5);

dataSetName = 'brendan';
experimentNo = 1;

% load data
[Y, lbls] = lvmLoadData(dataSetName);

% Set up model
options = fgplvmOptions('fitc');
options.optimiser = 'conjgrad';
options.back = 'kbr';
options.backOptions = kbrOptions(Y);
options.backOptions.kern = kernCreate(Y, 'rbf');
options.backOptions.kern.inverseWidth = 0.00001;

latentDim = 2;
d = size(Y, 2);

model = fgplvmCreate(latentDim, d, Y, options);

% Optimise the model.
iters = 1000;
display = 1;

model = fgplvmOptimise(model, display, iters);

% Save the results.
capName = dataSetName;;
capName(1) = upper(capName(1));
save(['dem' capName num2str(experimentNo) '.mat'], 'model');

if exist('printDiagram') & printDiagram
  fgplvmPrintPlot(model, lbls, capName, experimentNo);
end

% Load the results and display dynamically.
fgplvmResultsDynamic(dataSetName, experimentNo, 'image', [20 28], 1, 0, 1)

% compute the nearest neighbours errors in latent space.
errors = fgplvmNearestNeighbour(model, lbls);


function ind = mohsst5_points_to_grid_index(C)

%data = mohsst5_loaddata();

lon = C(1,:);
lat = C(2,:);

N_lon = 72;
N_lat = 36;

% Random locations to 5x5-grid
ind_lon = floor(lon/5)+(N_lon/2+1);
ind_lat = N_lat - (floor(lat/5)+(N_lat/2+1)) + 1;

ind = (ind_lon-1)*N_lat + ind_lat;


function [u] = spm_uc_RF(a,df,STAT,R,n)
% Corrected critical height threshold at a specified significance level
% FORMAT [u] = spm_uc_RF(a,df,STAT,R,n)
% a     - critical probability - {alpha}
% df    - [df{interest} df{residuals}]
% STAT  - Statistical field
%         'Z' - Gaussian field
%         'T' - T field
%         'X' - Chi-squared field
%         'F' - F field
% R     - RESEL Count {defining search volume}
% n     - number of conjoint SPMs
%
% u     - critical height {corrected}
%
%__________________________________________________________________________
%
% spm_uc returns the corrected critical threshold at a specified significance
% level (a). If n > 1 a conjunction probability over the n values of the
% statistic is returned.
%__________________________________________________________________________
% Copyright (C) 1999-2012 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_uc_RF.m 4634 2012-02-01 19:01:09Z guillaume $

%-Find approximate value
%--------------------------------------------------------------------------
u  = spm_u((a/max(R))^(1/n),df,STAT);
du = 1e-6;

%-Approximate estimate using E{m}
%--------------------------------------------------------------------------
d  = 1;
while abs(d) > 1e-6
    [P, P, p] = spm_P_RF(1,0,u,df,STAT,R,n);
    [P, P, q] = spm_P_RF(1,0,u + du,df,STAT,R,n);
    d         = (a - p)/((q - p)/du);
    u         = u + d;
    if isinf(u), u=+Inf; return; end
end

%-Refined estimate using 1 - exp(-E{m})
%--------------------------------------------------------------------------
d  = 1;
while abs(d) > 1e-6
    p         = spm_P_RF(1,0,u,df,STAT,R,n);
    q         = spm_P_RF(1,0,u + du,df,STAT,R,n);
    d         = (a - p)/((q - p)/du);
    u         = u + d;
    if isinf(u), u=+Inf; return; end
end


function [ FRF, FBB ] = OMP( Fopt, NRF, At )

FRF = [];
Fres = Fopt;
for k = 1:NRF
    PU = At' * Fres;
%     [aa,bb] = max(diag(PU * PU'));
    [aa,bb] = max(sum( abs(PU).^2, 2 ));
    FRF = [FRF , At(:,bb)];
    FBB = pinv(FRF) * Fopt; %use pseudoinverse to avoid the inverse of a possible singular matrix
    Fres = (Fopt - FRF * FBB) / norm(Fopt - FRF * FBB,'fro');
end

end


function anim = computeSMFromW( isProj, W, varargin )
% Recover rigid structure and motion from 2D calibrated measurements W
%
% Essential matrix decomposition
% Reference: HZ2, p259, Result 9.19, and p. 294
% isProj && nFrame==2 && isCalibrated
% fast
%
% Normalized 8-point algorithm
% Reference: HZ2, p279 and alg 11.1 p. 282
% isProj && nFrame==2 && ~isCalibrated
% fast
%
% Projective Factorization
% Reference: HZ2, p445, Algorithm 18.2
% Sturm and Triggs ECCV 96
% A Factorization Based Algorithm for Multi-Image Projective Structure and
% Motion
% isProj && nFrame>2 && method==0
%
% Projective Factorization
% Reference: Iterative Extensions of the Sturm/Triggs Algorithm:
% Convergence and Nonconvergence, from Oliensis, Hartley, PAMI 07
% isProj && nFrame>2 && method==Inf
% slower
%
% Gold Standard for Affine camera matrix
% Reference: HZ2, p351, Algorithm 14.1
% ~isProj && nFrame==2 && onlyErrorFlag
% fast
%
% Tomasi Kanade without the metric constraint
% Affine camera matrix, MLE estimation (Tomasi Kanade)
% Reference: HZ2, p437, Algorithm 18.1
% ~isProj && nFrame>2
% fast
%
% Tomasi Kanade with the metric constraint
% ~isProj && nFrame>2 && isCalibrated && method=0
% fast
%
% Tomasi Kanade with the metric constraint (more accurate computation)
% ~isProj && nFrame>2 && isCalibrated && method=inf
% slow
%
% Tomasi Kanade and autocalibration
% ~isProj && nFrame>2 && ~isCalibrated
% slow
%
% if doAffineUpgrade and/or doMetricUpgrade are true, affine and metric
% upgrades from Chandraker IJCV 2009 is applied
% slow
%
% If there are any missing entries:
% Low-Rank Matrix Fitting Based on Subspace Perturbation Analysis
% with Applications to Structure from Motion
% Hongjun Jia, Aleix M. Martinez, PAMI 08
%
% Returns the structure and camera parameters in an Animation object
%
% USAGE
%   anim = computeSMFromW( isProj, W, 'method',method )
%   anim = computeSMFromW( isProj, W,'method',Inf,'onlyErrorFlag',true )
%
% INPUTS
%  isProj     - flag indicating if the camera is projective
%  W          - [] [ 2 x nPoint x nFrame ] 2D projected features (NaN if
%               missing entry
%  varargin   - list of paramaters in quotes alternating with their values
%       - 'isCalibrated' [false] flag indicating if the cameras are
%                        calibrated (intrinsic parameters are identity)
%       - 'doAffineUpgrade' [false] flag indicating if the affine upgrade
%                        is computed
%       - 'doMetricUpgrade' [false] flag indicating if the metric upgrade
%                        is computed
%       - 'K',[] [3 x 1 ], [ 3 x nFrame ] calibration parameters
%                       (or [5 x 1 ], [ 5 x nFrame ] when projective)
%                       If given, they won't be optimized upon, you will
%                       need to run bundle adjustment after that
%       - 'KFull',[] [3 x 3 ] or [ 3 x 3 x nFrame ] calibration matrices
%       - 'method', [inf] method for performing SFM (see above for details)
%       - 'onlyErrorFlag', [false] flag indicating if only the error is
%         needed when nFrame==2 && method==Inf && ~isProj
%       - 'nItrSba', [100] number of bundle adjustment iterations
%       - 'tolAffine', [1e-5] tolerance for affine upgrade as explained in
%                          affineUpgrade
%       - 'tolMetric', [1e-5] tolerance for affine upgrade as explained in
%                          metricUprgade
%
% OUTPUTS 1
%  anim      - Animation object
%
% OUTPUTS 2
%  err       - error, acoording to Gold Standard for Affine camera matrix
%
% EXAMPLE
%
% See also
%
% Vincent's Structure From Motion Toolbox      Version 3.1.1
% Copyright (C) 2008-2011 Vincent Rabaud.  [vrabaud-at-cs.ucsd.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the GPL [see external/gpl.txt]

[ isCalibrated doAffineUpgrade doMetricUpgrade K KFull method ...
  onlyErrorFlag nItrSBA tolAffine tolMetric ] = getPrmDflt( varargin, ...
  { 'isCalibrated' false 'doAffineUpgrade' false 'doMetricUpgrade' false ...
  'K' [] 'KFull' [] 'method' inf 'onlyErrorFlag' false 'nItrSBA' 100 ...
  'tolAffine' 1e-5 'tolMetric' 1e-5}, 1);

% Remove points that do not have two views with no NaN's
good_point_mask = sum(~any(isnan(W),1),3) >=2;
W = W(:,good_point_mask,:);

nFrame = size(W,3); nPoint = size(W,2);

if doMetricUpgrade; doAffineUpgrade=true; end
if nFrame==2
  doAffineUpgrade=false;
  if ~isCalibrated; doMetricUpgrade=false; end
end

if size(W,1)==3; W = normalizePoint(W,3); end

% create an animation object that wil contain the output
anim=Animation(); anim.isProj=isProj; WOri=W;

% If calibrated, apply inv(K)
if ~isempty(K); anim.K=K; end
if ~isempty(KFull); anim.KFull=KFull; end
KFull=anim.KFull;
if ~isempty(KFull)
  isCalibrated=true;
  doMetricUpgrade=true;
  anim.KFull=[];
  % unapply the internal parameter matrix to the measurements
  if size(KFull,3)==1
    W = normalizePoint(multiTimes(inv(KFull),normalizePoint(W,-3),1.2),3);
  else
    invKFull = zeros(3,3,nFrame);
    for i=1:nFrame; invKFull(:,:,i) = inv(KFull(:,:,i)); end
    W = normalizePoint(multiTimes(inv(KFull),normalizePoint(W,-3),2),3);
  end
end

if isProj
  [P,S] = computeSMFromWProjective( W, method, isCalibrated );
else
  [P,S] = computeSMFromWAffine( W, method, onlyErrorFlag );
end
if onlyErrorFlag; anim=P; return; end

% fill the Animation object with the results
anim.S=S; anim.P=P; anim.W=W;

% perform an affine upgrade if requested
H=[];
if anim.isProj && ~isCalibrated
  % do bundle adjustment
  if nItrSBA > 0
    anim = bundleAdjustment( anim, 'nItr', nItrSBA );
  else
    anim = bundleAdjustment( anim, 'nItr', 20 );
  end

  % modify P so that P(:,:,1)==eye(3,4)
  anim=anim.setFirstPRtToId();

  % if we want to do an affine upgrade
  if doAffineUpgrade
    % only apply the quasi affine upgrade if we are under octave
    % (as Yalmip does not work there)
    if exist('OCTAVE_VERSION','builtin')==5
      warning('Only performing a quasi-affine upgrade under Octave');
      [ HEye, Hqa ] = affineUpgrade(anim);
      H=HEye*Hqa;
    else
      [ HEye, Hqa, pInf ] = affineUpgrade(anim, 'tol', tolAffine);
      % if the camera is calibrated or if we do not do a metric upgrade
      % stop here
      if ~doMetricUpgrade
        % apply H
        H=eye(4); H(4,1:3)=-pInf;
        H=HEye*H;
      end
    end
    if doMetricUpgrade
      % perform a metric upgrade if requested
      if exist('OCTAVE_VERSION','builtin')==5
        H=metricUpgrade(anim, 'isCalibrated', isCalibrated);
      else
        [ H, anim.KFull ]=metricUpgrade(anim, 'pInf', pInf, ...
          'isCalibrated', isCalibrated, 'tol', tolMetric);
      end
    end
  end
end
if anim.isProj && ~isCalibrated
  if doMetricUpgrade
    H=metricUpgrade(anim, 'isCalibrated', isCalibrated, 'method', method);
  end
end

% apply the homography H
if ~isempty(H)
  anim.P=multiTimes(anim.P,H,1);
  anim.S=normalizePoint(H\normalizePoint(anim.S,-4),4);
end

% recover rotations and translations
if doMetricUpgrade && (exist('OCTAVE_VERSION','builtin')~=5 || isCalibrated)
  P=anim.P;
  if ~isCalibrated && ~isempty(anim.KFull)
    if size(anim.KFull,3)==1; P=multiTimes(inv(anim.KFull),P,1.2);
    else P=multiDiv(anim.KFull,P,2);
    end
  end
  R=zeros(3,3,nFrame);
  if anim.isProj
    for i=1:nFrame
      P(:,:,i)=P(:,:,i)/nthroot(det(P(:,1:3,i)),3);
      R(:,:,i)=rotationMatrix(P(:,1:3,i));
    end
    anim.t=reshape(P(:,4,:),3,nFrame);
  else
    for i=1:nFrame
      R(:,:,i)=rotationMatrix(rotationMatrix(P(1:2,1:3,i)));
    end
    anim.t=reshape(P(1:2,4,:),2,nFrame); anim.t(3,:)=0;
  end
  anim.R=R;

  anim=anim.setFirstPRtToId();
end

% update the mask if any
mask=reshape(any(~isnan(W),1),nPoint,nFrame);
if any(mask==0); anim.mask=mask; end

% do a final bundle adjustment
if nItrSBA > 0; anim = bundleAdjustment( anim, 'nItr', nItrSBA ); end

W = anim.W; S = anim.S;
anim.W = zeros(2, length(good_point_mask), nFrame);
anim.S = zeros(3, length(good_point_mask), nFrame);
anim.W = W; anim.S = S;

% Re-assign the KFull from the input arguments if any
if ~isempty(KFull); anim.KFull=KFull; anim.W=WOri; end
end