% LORENZ system
% Train ensemble of models (e.g. for varying training data length, noise
% level, etc.)
% System identification: SINDYc

clear all, close all, clc

SystemModel = 'LORENZ';
DERIV_NOISE = 0;
TRACK_MODEL_BEST = 0;
%% Set Paths
figpath = '../FIGURES/EX_LORENZ_Dependencies/SINDYc/'; mkdir(figpath)
datapath = '../DATA/EX_LORENZ_Dependencies/SINDYc/'; mkdir(datapath)

% Overwrite if necessary
if exist('DERIV_NOISE')
    if DERIV_NOISE == 1
        figpath = '../FIGURES/EX_LORENZ_Dependencies/SINDYc/TVRegDiff/'; mkdir(figpath)
        datapath = '../DATA/EX_LORENZ_Dependencies/SINDYc/TVRegDiff/'; mkdir(datapath)
    end
end
addpath('../utils');

%% Parameters
ModelName = 'SINDYc';
polyorder = 2;
usesine = 0;
lambda0 = 0.1;     % lambda is our sparsification knob.
dep_trainlength = 1;
dep_noise = 0;

MOD_VAL = 10;

%% Select case 1 or 2
% 1) Dependency on training length
Ntrain_vec = [5:15,20:5:95,100:100:1000];%,1250,1500,2000,3000];%,1500:500:3000];
eta_vec = 0;%0.05;
Nr = 1;%50;

% 2) Noise dependency
% Ntrain_vec = 3000;
% eta_vec = [0.01 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5];
% Nr = 50;

N_LENGTHS = length(Ntrain_vec);
N_ETA = length(eta_vec);

%% Parameters
SAVE_MODEL = 0;
SHOW_RESULTS = 0;
SHOW_STATS = 1;
ONLY_TRAINING_LENGTH = 1; % if 0 : 3000 time unit, otherwise 1000

if ONLY_TRAINING_LENGTH == 1
    PostName = ['_TrainLength'];
else
    PostName = [];
end
%% Generate Data %{'sine2', 'chirp','prbs', 'sphs'}
InputSignalType = 'sphs'; %prbs; chirp; noise; sine2; sphs; mixed
getTrainingData

DataTrain.x = x;
DataTrain.t = t;
DataTrain.tspan = tspan;
DataTrain.u = u;

xstd = std(xv(:,3));

close all
rng(0,'twister')

errBest = inf*ones(N_LENGTHS,N_ETA);
% BestModels(1:N_LENGTHS,1:N_ETA) = struct('name', [], 'polyorder', [], 'usesine', [], 'Xi', [], ...
%     'dt', [], 'N', [], 'Ttraining', [], 'Err', [], 'ErrM', []);
BestModelsList = zeros(N_LENGTHS,N_ETA);
Lambda = zeros(N_LENGTHS,N_ETA);
Nvar = 3;
%% SINDYc
if ONLY_TRAINING_LENGTH == 0
    for iN = 1:N_LENGTHS
        for iNoise = 1:N_ETA
            disp(['Running for noise case ', num2str(iNoise), ' of ', num2str(N_ETA)])

            Results = struct('err', zeros(Nr,1), 'errM', zeros(Nr,1), 'xA', zeros(length(tv),Nvar), 'xB', zeros(length(tv),Nvar,Nr),'Ttraining',zeros(Nr,1));

            for iR = 1:Nr

                % Setup data
                x = DataTrain.x(1:Ntrain_vec(iN),:);
                u = DataTrain.u(1:Ntrain_vec(iN));
                t = DataTrain.t(1:Ntrain_vec(iN));
                tspan = DataTrain.tspan(1:Ntrain_vec(iN));

                % Add noise
                eps = eta_vec(iNoise)*xstd;
                x = x + eps*randn(size(x));

                % Train model
                lambda = lambda0;
                tic
                trainSINDYc
                telapsed = toc

                % Prediction over training phase
                p.ahat = Xi(:,1:3);
                p.polyorder = polyorder;
                p.usesine = usesine;
                p.dt = dt;
                [N,Ns] = size(DataTrain.x);
                xSINDYc = zeros(Ns,N); xSINDYc(:,1) = x0';
                for ct=1:N-1
                    xSINDYc(:,ct+1) = rk4u(@sparseGalerkinControl_Discrete,xSINDYc(:,ct),DataTrain.u(ct),dt,1,[],p);
                end
                xSINDYc = xSINDYc';

                % Show validation
                SHOW_PREDICTION_FOR_TRAINING_PHASE

                %% Prediction
                % Reference
                xA      = [xv];
                tA      = tv;

                % Model
                [N,Ns] = size(xA);
                xB = zeros(Ns,N); xB(:,1) = DataTrain.x(end,:)';
                for ct=1:N
                    xB(:,ct+1) = rk4u(@sparseGalerkinControl_Discrete,xB(:,ct),uv(ct),dt,1,[],p);
                end
                xB = xB(:,1:N+1)';
                tB = [tv(1)-dt;tv];

                % Show training and prediction
                SHOW_PREDICTION_FOR_VALIDATION_PHASE

                %% Error
                err = mean(sum((xB(2:end,:)-xA).^2,2));
                errM = mean(sum((xB(2:250+1,:)-xA(1:250,:)).^2,2));

                %% Save Data
                Model.name = 'SINDYc';
                Model.polyorder = polyorder;
                Model.usesine = usesine;
                Model.Xi = Xi;
                Model.dt = dt;
                Model.N  = Ntrain_vec(iN);
                Model.Ttraining = telapsed;
                Model.Err = err;
                Model.ErrM = errM;

                if SAVE_MODEL == 1
                    if mod(iR,MOD_VAL) == 0 || iR == 1

                        save(fullfile(datapath,['EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'_N',sprintf('%04g',Ntrain_vec(iN)),'_Eta',sprintf('%03g',100*eta_vec(iNoise)),'_iR',num2str(iR),'.mat']),'Model')
                    end
                end

                Results.err(iR) = err;
                Results.errM(iR) = errM;
                Results.xA = xA;
                Results.xB(:,1:3,iR) = xB(2:end,:);
                Results.Ttraining(iR) = telapsed;

                %% Track best model
                % errBest = 10^12*ones(N_LENGTHS,N_ETA)
                if TRACK_MODEL_BEST == 1
                    if Results.err(iR)<errBest(iN,iNoise) || iR == 1
                        errBest(iN,iNoise) = Results.err(iR);

                        BestModel = Model;
                        save(fullfile(datapath,['EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'_N',sprintf('%04g',Ntrain_vec(iN)),'_Eta',sprintf('%03g',100*eta_vec(iNoise)),'_BEST_MODEL.mat']),'Model')

                        %BestModels(iN,iNoise) = Model;
                        BestModelsList(iN,iNoise) = iR;
                        Lambda(iN,iNoise) = lambda;

                        clear ph
                        f1 = figure('visible','off');box on, hold on,
                        ccolors = get(gca,'colororder');
                        plot([tB(1),tB(1)],[-25 65],':','Color',[0.4,0.4,0.4],'LineWidth',1.5)
                        plot([t(end),t(end)],[-25 65],':','Color',[0.4,0.4,0.4],'LineWidth',1.5)
                        ylim([-25 65])
                        text(5,55,'Training', 'FontSize',12)
                        %                     text(10+tA(1),230,'Validation', 'FontSize',12)
                        text(3+tA(1),55,'Validation', 'FontSize',12)

                        if eps~=0
                            ph(4) = plot(tspan,x(:,1),'-','Color',0.7*ones(1,3),'LineWidth',1); %ccolors(1,:)+[0.15 0.3 0.25]
                            plot(tspan,x(:,2),'-','Color',0.7*ones(1,3),'LineWidth',1); %ccolors(2,:)+[0.15 0.3 0.25]
                            plot(tspan,x(:,3),'-','Color',0.7*ones(1,3),'LineWidth',1);
                            ph(1) = plot([DataTrain.t;tA],[DataTrain.x(:,1);xA(:,1)],'-','Color',ccolors(1,:),'LineWidth',0.5);
                            ph(2) = plot([DataTrain.t;tA],[DataTrain.x(:,2);xA(:,2)],'-','Color',ccolors(2,:),'LineWidth',0.5);
                            ph(3) = plot([DataTrain.t;tA],[DataTrain.x(:,3);xA(:,3)],'-','Color',ccolors(3,:),'LineWidth',0.5);
                        else
                            ph(1) = plot([DataTrain.t;tA],[DataTrain.x(:,1);xA(:,1)],'-','Color',ccolors(1,:),'LineWidth',1);
                            ph(2) = plot([DataTrain.t;tA],[DataTrain.x(:,2);xA(:,2)],'-','Color',ccolors(2,:),'LineWidth',1);
                            ph(3) = plot(t,x(:,1),'--','Color',[0 1 0],'LineWidth',1); % Training data
                            plot(t,x(:,2),'--','Color',[0 1 0],'LineWidth',1);
                        end

                        ph(5) = plot(tB,xB(:,1),'-.','Color',ccolors(1,:)-[0 0.2 0.2],'LineWidth',2);
                        ph(6) = plot(tB,xB(:,2),'-.','Color',ccolors(2,:)-[0.1 0.2 0.09],'LineWidth',2);
                        ph(7) = plot(tB,xB(:,3),'-.','Color',ccolors(3,:)-[0.1 0.2 0.09],'LineWidth',2);
                        grid off
                        xlim([0 tv(end)])
                        xlabel('Time')
                        ylabel('xi')
                        set(gca,'LineWidth',1, 'FontSize',14)
                        set(gcf,'Position',[100 100 300 200])
                        set(gcf,'PaperPositionMode','auto')
                        print('-depsc2', '-loose','-cmyk', [figpath,'EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'_Validation_BEST_MODEL_N',sprintf('%04g',Ntrain_vec(iN)),'_Eta',sprintf('%03g',100*eta_vec(iNoise)),'_noleg.eps']);

                        if eps~=0
                            lh = legend(ph([1,4,5]),'Truth','Training',ModelName,'Location','NorthWest');
                            %                         lh.Position = [lh.Position(1)+0.13,lh.Position(2)-0.2,lh.Position(3:4)];
                            lh.Position = [lh.Position(1)+0.02,lh.Position(2)-0.06,lh.Position(3:4)];
                        else
                            lh = legend(ph([1,4,5]),'Truth','Training',ModelName,'Location','NorthWest');
                            lh.Position = [lh.Position(1)+0.13,lh.Position(2)-0.2,lh.Position(3:4)];
                        end
                        print('-depsc2', '-loose','-cmyk', [figpath,'EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'_Validation_BEST_MODEL_N',sprintf('%04g',Ntrain_vec(iN)),'_Eta',sprintf('%03g',100*eta_vec(iNoise)),'.eps']);
                        close(f1);
                    end
                end
                lambda = lambda0;
            end
            if SHOW_STATS == 1
                %%
                xBmin = min(Results.xB(:,1:3,:),[],3);
                xBmax = max(Results.xB(:,1:3,:),[],3);
                clear ph
                figure('visible','off'),box on, hold on,
                ccolors = get(gca,'colororder');
                plot([tB(1),tB(1)],[-25 65],':','Color',[0.4,0.4,0.4],'LineWidth',1.5)
                plot([t(end),t(end)],[-25 65],':','Color',[0.4,0.4,0.4],'LineWidth',1.5)
                ylim([-25 65])
                t1 = text(5,55,'Training', 'FontSize',12);
                t2 = text(5+tA(1),55,'Validation', 'FontSize',12);

                X=[tB(2:end)',fliplr(tB(2:end)')];                %#create continuous x value array for plotting
                Y=[xBmin(:,1)',flipud(xBmax(:,1))'];              %#create y values for out and then back
                fillh1 = fill(X,Y,ccolors(1,:)-[0 0.2 0.2]);                  %#plot filled area
                fillh1.EdgeColor = ccolors(1,:)-[0 0.2 0.2]; fillh1.FaceAlpha = 0.5;

                X=[tB(2:end)',fliplr(tB(2:end)')];                %#create continuous x value array for plotting
                Y=[xBmin(:,2)',flipud(xBmax(:,2))'];              %#create y values for out and then back
                fillh = fill(X,Y,ccolors(2,:)-[0.1 0.2 0.09]);                  %#plot filled area
                fillh.EdgeColor = ccolors(2,:)-[0.1 0.2 0.09]; fillh.FaceAlpha = 0.5;

                X=[tB(2:end)',fliplr(tB(2:end)')];                %#create continuous x value array for plotting
                Y=[xBmin(:,3)',flipud(xBmax(:,3))'];              %#create y values for out and then back
                fillh = fill(X,Y,ccolors(3,:)-[0.1 0.2 0.09]);                  %#plot filled area
                fillh.EdgeColor = ccolors(3,:)-[0.1 0.2 0.09]; fillh.FaceAlpha = 0.5;

                if eps~=0
                    ph(4) = plot(tspan,x(:,1),'-','Color',0.7*ones(1,3),'LineWidth',1); %ccolors(1,:)+[0.15 0.3 0.25]
                    plot(tspan,x(:,2),'-','Color',0.7*ones(1,3),'LineWidth',1); %ccolors(2,:)+[0.15 0.3 0.25]
                    plot(tspan,x(:,3),'-','Color',0.7*ones(1,3),'LineWidth',1);
                    ph(1) = plot([DataTrain.t;tA],[DataTrain.x(:,1);xA(:,1)],'-','Color',ccolors(1,:),'LineWidth',1);
                    ph(2) = plot([DataTrain.t;tA],[DataTrain.x(:,2);xA(:,2)],'-','Color',ccolors(2,:),'LineWidth',1);
                    ph(3) = plot([DataTrain.t;tA],[DataTrain.x(:,3);xA(:,3)],'-','Color',ccolors(3,:),'LineWidth',1);
                else
                    ph(1) = plot([DataTrain.t;tA],[DataTrain.x(:,1);xA(:,1)],'-','Color',ccolors(1,:),'LineWidth',1);
                    ph(2) = plot([DataTrain.t;tA],[DataTrain.x(:,2);xA(:,2)],'-','Color',ccolors(2,:),'LineWidth',1);
                    ph(3) = plot([DataTrain.t;tA],[DataTrain.x(:,3);xA(:,3)],'-','Color',ccolors(3,:),'LineWidth',1);
                    ph(4) = plot(t,x(:,1),'--','Color',[0 1 0],'LineWidth',1); % Training data
                    plot(t,x(:,2),'--','Color',[0 1 0],'LineWidth',1);
                    plot(t,x(:,3),'--','Color',[0 1 0],'LineWidth',1);
                end

                grid off
                xlim([0 tv(end)])
                xlabel('Time')
                ylabel('xi')
                set(gca,'LineWidth',1, 'FontSize',14)
                set(gcf,'Position',[100 100 300 200])
                set(gcf,'PaperPositionMode','auto');

                if eps~=0
                    lh = legend([ph([1,4]),fillh1],'Truth','Training',ModelName,'Location','NorthWest');
                    lh.Position = [lh.Position(1)+0.02,lh.Position(2)-0.06,lh.Position(3:4)];
                else
                    lh = legend(ph([1,4]),'Truth','Training',ModelName,'Location','NorthWest');
                    lh.Position = [lh.Position(1)+0.02,lh.Position(2)-0.06,lh.Position(3:4)];
                end

                print('-depsc2', '-painters','-loose','-cmyk', [figpath,'EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'_Validation_N',sprintf('%04g',Ntrain_vec(iN)),'_Eta',sprintf('%03g',100*eta_vec(iNoise)),'_STATS.eps']);

                delete(lh), delete(t1), delete(t2)
                print('-depsc2', '-painters','-loose','-cmyk', [figpath,'EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'_Validation_N',sprintf('%04g',Ntrain_vec(iN)),'_Eta',sprintf('%03g',100*eta_vec(iNoise)),'_STATS_noleg.eps']);
            end
            %%
            save(fullfile(datapath,['EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'_N',sprintf('%04g',Ntrain_vec(iN)),'_Eta',sprintf('%03g',100*eta_vec(iNoise)),'_STATS.mat']),'Results')
        end
    end
    % % save(fullfile(datapath,['EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'_BEST_MODELS.mat']),'BestModels')
    save(fullfile(datapath,['EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'_BEST_MODELS_LIST',PostName,'.mat']),'BestModelsList')
    save(fullfile(datapath,['EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'_BEST_MODELS_Lambda',PostName,'.mat']),'Lambda')
end

%%
if ONLY_TRAINING_LENGTH == 1
    length_vec = 5:2:Ntrain-1;
    Results = struct('err', zeros(length(length_vec),2),'PredHor_Ball', zeros(length(length_vec),2), 'PredLength', zeros(length(length_vec),2),'RelErr', zeros(length(length_vec),2),'RelErrMax', zeros(length(length_vec),2), 'Ttraining', zeros(length(length_vec),1));
    Results.Ntrain_vec = length_vec;
    for iL = 1:length(length_vec)
        iN = length_vec(iL);
        if mod(iN,50)
            disp(['Running for length ', num2str(iN), 'of ', num2str(Ntrain-1)])
        end
        % Setup data
        x = DataTrain.x(1:iN,:);
        u = DataTrain.u(1:iN);
        t = DataTrain.t(1:iN);

        % Train model
        lambda = lambda0;
        tic
        trainSINDYc
        telapsed = toc

        % Prediction over training phase
        p.ahat = Xi(:,1:3);
        p.polyorder = polyorder;
        p.usesine = usesine;
        p.dt = dt;
        [N,Ns] = size(DataTrain.x);
        xSINDYc = zeros(Ns,N); xSINDYc(:,1) = x0';
        for ct=1:N-1
            xSINDYc(:,ct+1) = rk4u(@sparseGalerkinControl_Discrete,xSINDYc(:,ct),DataTrain.u(ct),dt,1,[],p);
        end
        xSINDYc = xSINDYc';

        %% Error over training phase
        Results.err(iL,1) = mean(sum((xSINDYc(1:end,:)-DataTrain.x).^2,2));
        Results.RelErr(iL,1) = mean( sum( abs( (DataTrain.x - xSINDYc)./DataTrain.x ),2) );
        Results.RelErrMax(iL,1) = mean( max( abs( (DataTrain.x - xSINDYc)./DataTrain.x ),[],2) );

        tmp = abs( (DataTrain.x - xSINDYc)./DataTrain.x );
        TF = tmp>0.1;
        idx = [];
        for iVar = 1:Nvar
            idx = [idx ; find(TF(:,iVar)==1,1,'first')];
        end
        Results.PredLength(iL,1) = max(idx);

        tmp2 = sqrt(sum(abs(DataTrain.x - xSINDYc).^2,2));
        idx = find(tmp2>3,1,'first');
        Results.PredHor_Ball(iL,1) = idx-1;
        %% Prediction
        % Reference
        xA      = [xv];
        tA      = tv;

        % Model
        [N,Ns] = size(xA);
        xB = zeros(Ns,N); xB(:,1) = DataTrain.x(end,:)';
        for ct=1:N
            xB(:,ct+1) = rk4u(@sparseGalerkinControl_Discrete,xB(:,ct),uv(ct),dt,1,[],p);
        end
        xB = xB(:,1:N+1)';
        tB = [tv(1)-dt;tv];

        %% Error over prediction phase
        xB = xB(2:end,:);
        Results.err(iL,2) = mean(sum((xB-xA).^2,2));
        Results.RelErr(iL,2) = mean( sum( abs( (xA - xB)./xA ),2) );
        Results.RelErrMax(iL,2) = mean( max( abs( (xA - xB)./xA ),[],2) );
        Results.Ttraining(iL) = telapsed;

        tmp = abs( (xA - xB)./xA );
        TF = tmp>0.1;
        idx = [];
        for iVar = 1:Nvar
            idx = [idx ; find(TF(:,iVar)==1,1,'first')];
        end
        Results.PredLength(iL,2) = max(idx);

        tmp2 = sqrt(sum(abs(xA - xB).^2,2));
        idx = find(tmp2>3,1,'first');
        Results.PredHor_Ball(iL,2) = idx-1;
    end

    Results.DataTrain = DataTrain;
    Results.DataValid.x = xv;
    Results.DataValid.u = uv;
    Results.DataValid.t = tv;
    Results.DataValid.tspan = tspanv;
    save(fullfile(datapath,['EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'_Eta',sprintf('%03g',100*eta_vec),'_Nevol','_STATS.mat']),'Results')
end


function Y = expvarMeanOut(model, X);

% EXPVARMEANOUT Output of an EXPVARMEAN model.
% FORMAT
% DESC gives the output of the mean of a variational approximation
% to a given kernel. This is for use with the exponentiated kernel
% when making a variational approximation to an exponentiated
% Gaussian process.
% ARG model : the model for which the output is required.
% ARG X : the input data for which the output is required.
% RETURN Y : the output.
%
%
% SEEALSO :  modelOut, expvarMeanCreate
%
% COPYRIGHT : Neil D. Lawrence, 2006

% SHEFFIELDML

Y = exp(0.5*kernDiagCompute(model.kern.argument, X));


function D = calc_dist_matrix(M, samples)

if nargin==1 || isempty(samples)
    samples = 1:M.n;
end

march = fastmarchmex('init', int32(M.TRIV-1), double(M.VERT(:,1)), double(M.VERT(:,2)), double(M.VERT(:,3)));

D = zeros(length(samples));

for i=1:length(samples)
%     fprintf('(%d/%d)\n', i, length(samples));
    source = inf(M.n,1);
    source(samples(i)) = 0;
    d = fastmarchmex('march', march, double(source));
    D(:,i) = d(samples);
end

fastmarchmex('deinit', march);

D = 0.5*(D+D');

end


function [net1] = vl_simplenn_mergebnorm(net)

%% merge bnorm parameters into adjacent Conv layer

for i = 1:numel(net.layers)
    if strcmp(net.layers{i}.type, 'conv')
        net.layers{i}.weightDecay(2) = 1;
    end
end

for i = 1:numel(net.layers)
    if strcmp(net.layers{i}.type, 'bnorm')
        ws = net.layers{i}.weights{1};
        bs = net.layers{i}.weights{2};
        mu_sigmas = net.layers{i}.weights{3};
        for j = 1:numel(ws)
            net.layers{i-1}.weights{1}(:,:,:,j) =single(double(net.layers{i-1}.weights{1}(:,:,:,j))*double(ws(j))/(double(mu_sigmas(j,2))));
            net.layers{i-1}.weights{2}(j) =single(double(bs(j)) - double(ws(j))*double(mu_sigmas(j,1))/(double(mu_sigmas(j,2))));
        end
        net.layers{i-1}.learningRate(2) = 1;
    end
end

net1 = net;
net1.layers = {};
net1 = rmfield(net1,'meta');
for i = 1:numel(net.layers)
    if ~strcmp(net.layers{i}.type, 'bnorm')
        net1.layers{end+1} = net.layers{i};
    end
end

net1.layers = net1.layers(1:end-1);

end


function [cnn_feature1, cnn_feature2] = FeatureMapNormalization(cnn_feature)

r = size(cnn_feature,1);
c = size(cnn_feature,2);
f = size(cnn_feature,3);
t = size(cnn_feature,4);
cnn_feature1 = permute(cnn_feature,[1,2,4,3]);
cnn_feature1 = reshape(cnn_feature1,r*c*t,[]);
cnn_feature1 = bsxfun(@rdivide,cnn_feature1,max(cnn_feature1,[],1)+eps);
cnn_feature1 = reshape(cnn_feature1,r,c,t,f);
cnn_feature1 = permute(cnn_feature1,[1,2,4,3]);

cnn_feature2 = bsxfun(@rdivide,cnn_feature,max(cnn_feature,[],3)+eps);

end


%% rangeframe
%
%  James Houghton
%  James.P.Houghton@gmail.com
%  December 8 2009
%
% A range frame lets you include information in what would otherwise
% be dumb ink on your plot, such as mean, median, quartiles, or standard deviation.
%
% This code based upon:
%
%  Edward Tufte: Visual Display of Quantitative Information
%  Second Edition
%  Graphics Press
%  pg 132
%
%% ToDo:
%
% # Clean up tick/number positioning
% # Find a better way to name data pulled out of the get return array
%
%% Function Definition
% Inputs:
%
% * *main_axis* - handle to the axis we intend to modify
% * *x_frame*   - a five element vector showing horizontal range frame limits, described below
% * *y_frame*   - a five element vector showing vertical range frame limits, described below
%
% Outputs:
%
% * *main_axis* - handle to the modified axis
%
function [main_axis] = rangeframe(main_axis, x_frame, y_frame)
%% get current axis properties
dims = get(main_axis, 'Position');
    left   = dims(1);           % break these out for clarity
    bottom = dims(2);           % (I wish I knew a better way...)
    width  = dims(3);
    height = dims(4);
x_axis_lims = get(main_axis, 'XLim');
    x_axis_min = x_axis_lims(1);
    x_axis_max = x_axis_lims(2);
y_axis_lims = get(main_axis, 'YLim');
    y_axis_min = y_axis_lims(1);
    y_axis_max = y_axis_lims(2);

%% draw range frame

% set tick length for middle value
l_tick = .01;

% map x_frame array to normalized figure coordinates
x_frame_mapped = (x_frame-x_axis_min)/(x_axis_max-x_axis_min) * width + left;

% map y_frame array to normalized figure coordinates
y_frame_mapped = (y_frame-y_axis_min)/(y_axis_max-y_axis_min) * height + bottom;

% draw horizontal axis range frame
annotation('line', [x_frame_mapped(1),x_frame_mapped(2)], [bottom, bottom]);
annotation('line', [x_frame_mapped(3),x_frame_mapped(3)], [bottom-l_tick, bottom+l_tick], 'Linewidth', 2);
annotation('line', [x_frame_mapped(4),x_frame_mapped(5)], [bottom, bottom]);

% draw vertical axis range frame
annotation('line', [left, left], [y_frame_mapped(1),y_frame_mapped(2)]);
annotation('line', [left-l_tick, left+l_tick], [y_frame_mapped(3),y_frame_mapped(3)], 'Linewidth', 2);
annotation('line', [left, left], [y_frame_mapped(4),y_frame_mapped(5)]);

%% Modify the existing axes to improve the visibility of the range frame

% modify the normal axis
set(main_axis, ...
    'color', 'none', ...    % make sure the background is clear (white would also work)
    'xgrid', 'off', ...     % make sure there is no background grid
    'ygrid','off', ...
    'box', 'off', ...       % get rid of upper and right hand axis frame
    'TickDir', 'out', ...   % set the tick marks to point towards numbers to distringuish from center tick
    'TickLength', [l_tick/2,0]);    % make ths ticks short for the same reason

% cover the axis with a mask to hide the lines
mask_axis = copyobj(main_axis, gcf);
set(mask_axis, ...
    'xcolor', 'w', ...      % maxe the axis lines white
    'ycolor','w', ...
    'Xtick',[], ...         % dont print any ticks or numbers
    'Ytick',[]);


function M = unroll_dbn_topology(intra, inter, T, intra1)
% UNROLL_DBN_TOPOLOGY Make the block diagonal adjacency matrix for a DBN consisting of T slices
% M = unroll_dbn_topology(intra, inter, T, intra1)
%
% intra is the connectivity within a slice, inter between two slices.
% M will have intra along the diagonal, and inter one above the diagonal.
% intra1 is an optional argumnet, in case the intra is different for the first slice.

if nargin < 4, intra1 = intra; end

ss = length(intra); % slice size
M = sparse(ss*T, ss*T);

b = 1:ss;
M(b,b) = intra1;
M(b,b+ss) = inter;

for t=2:T-1
  b = (1:ss) + (t-1)*ss;
  M(b,b) = intra;
  M(b,b+ss) = inter;
end

t = T;
b = (1:ss) + (t-1)*ss;
M(b,b) = intra;


function jed = transition_to_jed_english ( )

%*****************************************************************************80
%
%% TRANSITION_TO_JED_ENGLISH returns the English calendar transition as a JED.
%
%  Discussion:
%
%    In the English calendar, the last moment of the Julian calendar was
%      11:59 pm, 2 September 1752 Julian/English,
%      11:59 pm, 13 September 1752 Gregorian/CE.
%    The first minute of the Gregorian calendar ended at
%      12:01 am, 3 September 1752 Julian,
%      12:01 am, 15 September 1752 Gregorian/CE/English.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 December 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real JED, the Julian Ephemeris Date of the date.
%
  jed = 2361221.5;

  return
end


function f = non_domination_sort_mod(x, M, V)

%% function f = non_domination_sort_mod(x, M, V)
% This function sort the current popultion based on non-domination. All the
% individuals in the first front are given a rank of 1, the second front
% individuals are assigned rank 2 and so on. After assigning the rank the
% crowding in each front is calculated.

%  Copyright (c) 2009, Aravind Seshadri
%  All rights reserved.

% [N, m] = size(x);
% clear m;
N = size(x,1); % modified by zzb
x_temp = zeros(N,M + V + 1); % modified by zzb
x_temp(:,1:M + V) = x(:,1:M + V); % modified by zzb
% Initialize the front number to 1.
front = 1;
% There is nothing to this assignment, used only to manipulate easily in
% MATLAB.
% F(front).f = [];
% individual = [];
F = struct('f',cell(1,N)); % modified by zzb
individual = struct('n',cell(1,N),'p',cell(1,N)); % modified by zzb

%% Non-Dominated sort.
% The initialized population is sorted based on non-domination. The fast
% sort algorithm [1] is described as below for each

% ?for each individual p in main population P do the following
%   ?Initialize Sp = []. This set would contain all the individuals that is
%     being dominated by p.
%   ?Initialize np = 0. This would be the number of individuals that domi-
%     nate p.
%   ?for each individual q in P
%       * if p dominated q then
%           ?add q to the set Sp i.e. Sp = Sp ? {q}
%       * else if q dominates p then
%           ?increment the domination counter for p i.e. np = np + 1
%   ?if np = 0 i.e. no individuals dominate p then p belongs to the first
%     front; Set rank of individual p to one i.e prank = 1. Update the first
%     front set by adding p to front one i.e F1 = F1 ? {p}
% ?This is carried out for all the individuals in main population P.
% ?Initialize the front counter to one. i = 1
% ?following is carried out while the ith front is nonempty i.e. Fi != []
%   ?Q = []. The set for storing the individuals for (i + 1)th front.
%   ?for each individual p in front Fi
%       * for each individual q in Sp (Sp is the set of individuals
%         dominated by p)
%           ?nq = nq-1, decrement the domination count for individual q.
%           ?if nq = 0 then none of the individuals in the subsequent
%             fronts would dominate q. Hence set qrank = i + 1. Update
%             the set Q with individual q i.e. Q = Q ? q.
%   ?Increment the front counter by one.
%   ?Now the set Q is the next front and hence Fi = Q.
%
% This algorithm is better than the original NSGA ([2]) since it utilize
% the informatoion about the set that an individual dominate (Sp) and
% number of individuals that dominate the individual (np).

for i = 1 : N
    % Number of individuals that dominate this individual
    individual(i).n = 0;
    % Individuals which this individual dominate
    individual(i).p = [];
    for j = 1 : N
        dom_less = 0;
        dom_equal = 0;
        dom_more = 0;
        for k = 1 : M
            if (x_temp(i,V + k) < x_temp(j,V + k))
                dom_less = dom_less + 1;
            elseif (x_temp(i,V + k) == x_temp(j,V + k))
                dom_equal = dom_equal + 1;
            else
                dom_more = dom_more + 1;
            end
        end
        if dom_less == 0 && dom_equal ~= M
            individual(i).n = individual(i).n + 1;
        elseif dom_more == 0 && dom_equal ~= M
            individual(i).p = [individual(i).p j];
        end
    end
    if individual(i).n == 0
        x_temp(i,M + V + 1) = 1;
        F(front).f = [F(front).f i];
    end
end
% Find the subsequent fronts
while ~isempty(F(front).f)
   Q.temp = []; % modified by zzb
   for i = 1 : length(F(front).f)
       if ~isempty(individual(F(front).f(i)).p)
            for j = 1 : length(individual(F(front).f(i)).p)
            	individual(individual(F(front).f(i)).p(j)).n = ...
                	individual(individual(F(front).f(i)).p(j)).n - 1;
                if individual(individual(F(front).f(i)).p(j)).n == 0
                    x_temp(individual(F(front).f(i)).p(j),M + V + 1) = front + 1;
                    Q.temp = [Q.temp individual(F(front).f(i)).p(j)]; % modified by zzb
                end
            end
       end
   end
   front =  front + 1;
   F(front).f = Q.temp; % modified by zzb
end

%% Crowding distance
%The crowing distance is calculated as below
% ?For each front Fi, n is the number of individuals.
%   ?initialize the distance to be zero for all the individuals i.e. Fi(dj ) = 0,
%     where j corresponds to the jth individual in front Fi.
%   ?for each objective function m
%       * Sort the individuals in front Fi based on objective m i.e. I =
%         sort(Fi,m).
%       * Assign infinite distance to boundary values for each individual
%         in Fi i.e. I(d1) = ? and I(dn) = ?
%       * for k = 2 to (n-1)
%           ?I(dk) = I(dk) + (I(k + 1).m ? I(k ? 1).m)/fmax(m) - fmin(m)
%           ?I(k).m is the value of the mth objective function of the kth
%             individual in I

% Find the crowding distance for each individual in each front
[~,index_of_fronts] = sort(x_temp(:,M + V + 1));
sorted_based_on_front = x_temp; % Initialize by x % modified by zzb
for i = 1 : length(index_of_fronts)
    sorted_based_on_front(i,:) = x_temp(index_of_fronts(i),:);
end
current_index = 0;

% for front = 1 : (length(F) - 1) % The last front of F is []
for front_count = 1 : (front - 1) % modified by zzb
    % objective = [];
    % distance = 0;
    % y = [];
    y = zeros(length(F(front_count).f),2*M + V + 1); % modified by zzb
    previous_index = current_index + 1;
    for i = 1 : length(F(front_count).f) % modified by zzb
        %y(i,:) = sorted_based_on_front(current_index + i,:);
        y(i,1:M + V + 1) = sorted_based_on_front(current_index + i,:); % modified by zzb
    end
    current_index = current_index + i;
    % Sort each individual based on the objective
    % sorted_based_on_objective = []; % modified by zzb
    for i = 1 : M
        [~, index_of_objectives] = sort(y(:,V + i)); % modified by zzb
        % sorted_based_on_objective = [];
        sorted_based_on_objective = y;  % Initialize by y  % modified by zzb
        for j = 1 : length(index_of_objectives)
            sorted_based_on_objective(j,:) = y(index_of_objectives(j),:);
        end
        f_max = sorted_based_on_objective(length(index_of_objectives), V + i);
        f_min = sorted_based_on_objective(1, V + i);
        y(index_of_objectives(length(index_of_objectives)),M + V + 1 + i) = Inf;
        y(index_of_objectives(1),M + V + 1 + i) = Inf;
         for j = 2 : length(index_of_objectives) - 1
            next_obj = sorted_based_on_objective(j + 1,V + i);
            previous_obj = sorted_based_on_objective(j - 1,V + i);
            if (f_max - f_min == 0)
                y(index_of_objectives(j),M + V + 1 + i) = Inf;
            else
                y(index_of_objectives(j),M + V + 1 + i) = ...
                     (next_obj - previous_obj)/(f_max - f_min);
            end
         end
    end
    % distance = [];
    % distance(:,1) = zeros(length(F(front_count).f),1); % modified by zzb
    distance = zeros(length(F(front_count).f),1); % modified by zzb
    for i = 1 : M
       % distance(:,1) = distance(:,1) + y(:,M + V + 1 + i);% modified by zzb
       distance = distance + y(:,M + V + 1 + i);% modified by zzb
    end
    y(:,M + V + 2) = distance;
    % y = y(:,1 : M + V + 2);
    % z(previous_index:current_index,:) = y;
    z(previous_index:current_index,:) = y(:,1 : M + V + 2);
end
% f = z();
f = z;
%% References
% [1] *Kalyanmoy Deb, Amrit Pratap, Sameer Agarwal, and T. Meyarivan*, |A Fast
% Elitist Multiobjective Genetic Algorithm: NSGA-II|, IEEE Transactions on
% Evolutionary Computation 6 (2002), no. 2, 182 ~ 197.
%
% [2] *N. Srinivas and Kalyanmoy Deb*, |Multiobjective Optimization Using
% Nondominated Sorting in Genetic Algorithms|, Evolutionary Computation 2
% (1994), no. 3, 221 ~ 248.


function [Fm,Vm]=cap_patchcylinder(F1,V1,F2,V2,nr,nz)

% function [Fm,Vm]=cap_patchcylinder(F1,V1,F2,V2,nr,nz)
% ------------------------------------------------------------------------
% This function assumes the inputs F1, V1 and F2, V2 define the faces and
% vertices of two concentrid cylinders and closes to top and bottom faces
% by connecting the cylinders together. The input nr defines the number of
% radial points to add for the connection. The input nz defines the number
% of steps used in the z-direction for the cylinders.
%
% Kevin Mattheus Moerman
% gibbon.toolbox@gmail.com
%
% Change log:
% 2014/09/25
% 2019/03/29 Updated help documentation
%
%------------------------------------------------------------------------

%%

Fm=[F1; (F2+size(V1,1))];
Vm=[V1; V2];

%Coordinates created from nr*nz matrix

%Top edge indices
It=ones(nr-1,1); Jt=(1:1:nr-1)';
INDt=sub2ind([nz,nr-1],It,Jt);
% Vt1=V1(INDt,:); Vt2=V2(INDt,:); Vt=[Vt1; Vt2];

%Bottom edge indices
Ib=nz.*ones(nr-1,1); Jb=(1:1:nr-1)';
INDb=sub2ind([nz,nr-1],Ib,Jb);
% Vb1=V1(INDb,:); Vb2=V2(INDb,:); Vb=[Vb1; Vb2];

Fp=(ones(nr-1,1)*[1 2 2 1])+(((1:1:nr-1*ones)'-1)*ones(1,4)); Fp(Fp==nr)=1; %Quad order

Fq=[INDt(Fp(:,1:2)) INDt(Fp(:,3:4))+size(V1,1)];
Ftt=[Fq(:,[1 3 2]); Fq(:,[1 4 3])]; %Tri order

Fq=[INDb(Fp(:,1:2)) INDb(Fp(:,3:4))+size(V1,1)];
Ftb=[Fq(:,[1 3 2]); Fq(:,[1 4 3])]; %Tri order

Fm=[Fm; Ftt; Ftb];

end

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


function [ hyps ] = generateHypsB( lines, vp, tol, omap, gc )
%GENERATEHYPS generate hypothesis, screen out inconsistent ones according
%to OM, GC, and merged map
%   lines: line segment
%   vp: vanishing point, in z,y,x order
%   tol: tolerance when assigning lines to vp
%   omap & gc: evidence
%   hyps: all hyps with >0.5 consistency with OM will be returned;

%% >> ASSIGN LINE SEGMENTS WITH TYPE

%% get line segments align with vanishing point
lines1 = getVanishingLine(lines, vp(1,:), tol);
lines2 = getVanishingLine(lines, vp(2,:), tol);
lines3 = getVanishingLine(lines, vp(3,:), tol);

%% extend line representation with line type
% line: [1x3 n] [planeID] [1x2 uv1] [u/d f/b r/l] (1/-1, 0 not sure, 2 not available)
lines1_attr = zeros(size(lines1,1),9);
lines2_attr = zeros(size(lines2,1),9);
lines3_attr = zeros(size(lines3,1),9);
lines1_attr(:,1:6) = lines1(:,1:6);
lines2_attr(:,1:6) = lines2(:,1:6);
lines3_attr(:,1:6) = lines3(:,1:6);

%% horizontal line, line connecting vp2 and vp3, u=1, d=-1
horizontal = cross(vp(2,:),vp(3,:));
horizontal = horizontal./norm(horizontal);
if horizontal(:,3)<0
    horizontal = -horizontal;
end
% line belong to vp2 and vp3 should have this attribute
% lines1 not available
lines1_attr(:,7) = 2;
% lines2
for i = 1:size(lines2_attr,1)
    l = lines2(i,:);
    u = l(5:6)*2*pi-pi;
    v = computeUVN( l(1:3), u, l(4));
    xyz = uv2xyzN([u' v'], l(4));
    b1 = dot(xyz(1,:), horizontal)>0;
    b2 = dot(xyz(2,:), horizontal)>0;
    if b1&&b2
        lines2_attr(i,7) = 1;
    elseif ~b1&&~b2
        lines2_attr(i,7) = -1;
    else
        lines2_attr(i,7) = 0;
    end
end
% lines3
for i = 1:size(lines3_attr,1)
    l = lines3(i,:);
    u = l(5:6)*2*pi-pi;
    v = computeUVN( l(1:3), u, l(4));
    xyz = uv2xyzN([u' v'], l(4));
    b1 = dot(xyz(1,:), horizontal)>0;
    b2 = dot(xyz(2,:), horizontal)>0;
    if b1&&b2
        lines3_attr(i,7) = 1;
    elseif ~b1&&~b2
        lines3_attr(i,7) = -1;
    else
        lines3_attr(i,7) = 0;
    end
end

%% front and back, line connecting vp1 and vp3, f=1, b=-1
frontNback = cross(vp(1,:),vp(3,:));
frontNback = frontNback./norm(frontNback);
if frontNback(:,2)<0
    frontNback = -frontNback;
end
% line belong to vp1 and vp3 should have this attribute
% lines2 not available
lines2_attr(:,8) = 2;
% lines1
for i = 1:size(lines1_attr,1)
    l = lines1(i,:);
    u = l(5:6)*2*pi-pi;
    v = computeUVN( l(1:3), u, l(4));
    xyz = uv2xyzN( [u' v'], l(4));
    b1 = dot(xyz(1,:), frontNback)>0;
    b2 = dot(xyz(2,:), frontNback)>0;
    if b1&&b2
        lines1_attr(i,8) = 1;
    elseif ~b1&&~b2
        lines1_attr(i,8) = -1;
    else
        lines1_attr(i,8) = 0;
    end
end
% lines3
for i = 1:size(lines3_attr,1)
    l = lines3(i,:);
    u = l(5:6)*2*pi-pi;
    v = computeUVN( l(1:3), u, l(4));
    xyz = uv2xyzN( [u' v'], l(4));
    b1 = dot(xyz(1,:), frontNback)>0;
    b2 = dot(xyz(2,:), frontNback)>0;
    if b1&&b2
        lines3_attr(i,8) = 1;
    elseif ~b1&&~b2
        lines3_attr(i,8) = -1;
    else
        lines3_attr(i,8) = 0;
    end
end

%% left and right, line connecting vp1 and vp2, r=1, l=-1
leftNright = cross(vp(1,:),vp(2,:));
leftNright = leftNright./norm(leftNright);
if leftNright(:,1)<0
    leftNright = -leftNright;
end
% line belong to vp1 and vp2 should have this attribute
% lines3 not available
lines3_attr(:,9) = 2;
% lines1
for i = 1:size(lines1_attr,1)
    l = lines1(i,:);
    u = l(5:6)*2*pi-pi;
    v = computeUVN( l(1:3), u, l(4));
    xyz = uv2xyzN( [u' v'], l(4));
    b1 = dot(xyz(1,:), leftNright)>0;
    b2 = dot(xyz(2,:), leftNright)>0;
    if b1&&b2
        lines1_attr(i,9) = 1;
    elseif ~b1&&~b2
        lines1_attr(i,9) = -1;
    else
        lines1_attr(i,9) = 0;
    end
end
% lines2
for i = 1:size(lines2_attr,1)
    l = lines2(i,:);
    u = l(5:6)*2*pi-pi;
    v = computeUVN( l(1:3), u, l(4));
    xyz = uv2xyzN( [u' v'], l(4));
    b1 = dot(xyz(1,:), leftNright)>0;
    b2 = dot(xyz(2,:), leftNright)>0;
    if b1&&b2
        lines2_attr(i,9) = 1;
    elseif ~b1&&~b2
        lines2_attr(i,9) = -1;
    else
        lines2_attr(i,9) = 0;
    end
end

%% save line segments according to type
typ = [1 1 2; 1 2 -1; 1 -1 2; 1 2 1; ...
       -1 1 2; -1 2 -1; -1 -1 2; -1 2 1; ...
       2 1 -1; 2 1 1; 2 -1 1; 2 -1 -1];
allLines = [lines1_attr; lines2_attr; lines3_attr];
typLine = repmat(struct('lines', [], 'lsLength', [], 'lspool', [], ...
                        'poolLength', []), [size(typ,1) 1]);
for i = 1:size(typ,1)
    valid = (allLines(:,7)==typ(i,1)) & (allLines(:,8)==typ(i,2)) & (allLines(:,9)==typ(i,3));
    typLine(i).lines = allLines(valid,:);
    lsLength = typLine(i).lines(:,6)-typLine(i).lines(:,5);
    lsLength(lsLength<0) = lsLength(lsLength<0) + 1;
    sumLength = sum(lsLength);
    lspool = zeros( ceil(sumLength*100), 1);
    accumuLen = 0;
    poolLength = length(lspool);
    for j = 1:length(lsLength)
        startid = round(accumuLen/sumLength*poolLength) + 1;
        accumuLen = accumuLen + lsLength(j);
        endid = round(accumuLen/sumLength*poolLength);
        lspool(startid:endid) = j;
    end

    typLine(i).lsLength = lsLength;
    typLine(i).lspool = lspool;
    typLine(i).poolLength = poolLength;
end

%% >> BUILD UP RECIPE FROM LINE TYPE TO BOX
% +1: max coords; -1: min coords;
% uf [ 0 +1 +1]  df [ 0 +1 -1]  lf [-1 +1  0];
% ul [-1  0 +1]  dl [-1  0 -1]  rf [+1 +1  0];
% ub [ 0 -1 +1]  db [ 0 -1 -1]  rb [+1 -1  0];
% ur [+1  0 +1]  dr [+1  0 -1]  lb [-1 -1  0];

%% generate recipe: from line type to box by check rank of matrix
allFunc = [ 0 0 0 1 0 -1; ...
            1 0 0 0 0 -1; ...
            0 0 1 0 0 -1; ...
            0 1 0 0 0 -1; ...
            0 0 0 1 -1 0; ...
            1 0 0 0 -1 0; ...
            0 0 1 0 -1 0; ...
            0 1 0 0 -1 0; ...
            1 0 0 -1 0 0; ...
            0 1 0 -1 0 0; ...
            0 1 -1 0 0 0; ...
            1 0 -1 0 0 0];
% combos = combntns(1:size(allFunc,1), 5);
combos = nchoosek(1:size(allFunc,1), 5);
degen = false(size(combos,1),1);
for i = 1:size(combos,1)
    equset = allFunc(combos(i,:), :);
    if rank(equset)<5
        degen(i) = true;
    end
end
cube_recipe = combos(~degen,:);

%% define geometry of cuboid: correlation among corner, edge, and face
% 8 corners, each is the intersection of 3 edges from 3 vps respectively
edgeToPoint = [ 9  2  1; ...
               12  2  3; ...
               11  4  3; ...
               10  4  1; ...
                9  6  5; ...
               12  6  7; ...
               11  8  7; ...
               10  8  5];
% 8 corners, each is assigned with type [u(+1)/d(-1) f(+1)/b(-1) r(+1)/l(-1)]
pointDirect = [ 1  1 -1; ...
                1 -1 -1; ...
                1 -1  1; ...
                1  1  1; ...
               -1  1 -1; ...
               -1 -1 -1; ...
               -1 -1  1; ...
               -1  1  1];
% 12 edges, each connects two corners
PointToEdge = [ 1  4; ...
                2  1; ...
                2  3; ...
                3  4; ...
                5  8; ...
                6  5; ...
                6  7; ...
                7  8; ...
                5  1; ...
                8  4; ...
                7  3; ...
                6  2];
% 6 faces, each face has 4 corners
% ranked as u d f b r l; start from left bottom, clockwise
PointToSurface = [1 2 3 4; ...
                  6 5 8 7; ...
                  5 1 4 8; ...
                  7 3 2 6; ...
                  8 4 3 7; ...
                  6 2 1 5];
SurfaceNm = [1;1;2;2;3;3];

%% >> START TO GENERATE CUBOID HYPOTHESIS, VALIDATE WITH OMAP
maxSample = 200000;
validHyps = repmat(struct('extLine', [], 'hCorner', [], 'srcLine', [], ...
                          'omapScr', 0, 'vpLength', [], 'recipe', [], 'gcScr', -1, 'mgScr', -1, 'opScr', -1 ), maxSample, 1);

%% sample line segments, long line segments are more likely to be selected.
rcpNum = size(cube_recipe,1);
for samid = 1:maxSample
    if rem(samid,1000)==0
%         fprintf('%d/%d hypothesis validated, score: %f/%f\n', samid, maxSample, maxScore, numPoint);
        fprintf('%d/%d hypothesis sampled\r', samid, maxSample);
    end
    rcpid = cube_recipe(randsample(rcpNum, 1),:);

    if any([typLine(rcpid).poolLength]==0)
        validHyps(samid).srcLine = [];
        validHyps(samid).recipe = rcpid;
        continue;
    end

    lineID1 = typLine(rcpid(1)).lspool(randsample(typLine(rcpid(1)).poolLength, 1));
    lineID2 = typLine(rcpid(2)).lspool(randsample(typLine(rcpid(2)).poolLength, 1));
    lineID3 = typLine(rcpid(3)).lspool(randsample(typLine(rcpid(3)).poolLength, 1));
    lineID4 = typLine(rcpid(4)).lspool(randsample(typLine(rcpid(4)).poolLength, 1));
    lineID5 = typLine(rcpid(5)).lspool(randsample(typLine(rcpid(5)).poolLength, 1));

    hlines = zeros(12,9);
    hlines(rcpid(1),:) = typLine(rcpid(1)).lines(lineID1,:);
    hlines(rcpid(2),:) = typLine(rcpid(2)).lines(lineID2,:);
    hlines(rcpid(3),:) = typLine(rcpid(3)).lines(lineID3,:);
    hlines(rcpid(4),:) = typLine(rcpid(4)).lines(lineID4,:);
    hlines(rcpid(5),:) = typLine(rcpid(5)).lines(lineID5,:);

    validHyps(samid).srcLine = hlines;
    validHyps(samid).recipe = rcpid;
end
fprintf('\n');

%% finish up unknown edges, compute corner, check if ls locates in line
for samid = 1:maxSample
    if rem(samid,1000)==0
%         fprintf('%d/%d hypothesis validated, score: %f/%f\n', samid, maxSample, maxScore, numPoint);
        fprintf('%d/%d hypothesis validated\r', samid, maxSample);
    end
    if isempty(validHyps(samid).srcLine)
        validHyps(samid).omapScr = -1;
        continue;
    end
    hlines = validHyps(samid).srcLine;
    hCorner = zeros(8,3);
    lineExist = false(12,1);
    lineExist(validHyps(samid).recipe) = true;
    rcpid = validHyps(samid).recipe;
    % >> compute all the other edges, corner
    bDegenerate = false;
    while any(~lineExist)
        hits = lineExist(edgeToPoint);
        actLine = sum(hits,2)==2;
        i = find(actLine,1)';
        srcLineID = edgeToPoint(i,hits(i,:));
        dstLineID = edgeToPoint(i,~hits(i,:));
        dstVpID = find(~hits(i,:));
        dstCor = cross( hlines(srcLineID(1),1:3), hlines(srcLineID(2),1:3), 2);
        if norm(dstCor)<0.1
            bDegenerate = true;
            break;
        end

        hCorner(i,:) = dstCor./norm(dstCor);
        % check if intersection locates at right position
        v = dot([dstCor;dstCor;dstCor],[horizontal;frontNback;leftNright],2);
        if sign(v(1))~=pointDirect(i,1) ...
         ||sign(v(2))~=pointDirect(i,2) ...
         ||sign(v(3))~=pointDirect(i,3)
            hCorner(i,:) = -hCorner(i,:);
        end

        dstNM = cross(hCorner(i,:), vp(dstVpID,:), 2);
        hlines(dstLineID,1:3) = dstNM./norm(dstNM);
        lineExist(dstLineID) = true;
    end

    if bDegenerate
        validHyps(samid).omapScr = -1;
        continue;
    end

    I = find(actLine); i = I(end);
    srcLineID = edgeToPoint(i,hits(i,:));
    dstCor = cross( hlines(srcLineID(1),1:3), hlines(srcLineID(2),1:3), 2);
    hCorner(i,:) = dstCor./norm(dstCor);
    % check if intersection locates at right position
    v = dot([dstCor;dstCor;dstCor],[horizontal;frontNback;leftNright],2);
    if sign(v(1))~=pointDirect(i,1) ...
     ||sign(v(2))~=pointDirect(i,2) ...
     ||sign(v(3))~=pointDirect(i,3)
        hCorner(i,:) = -hCorner(i,:);
    end

    % >> compute extended line representation
    rcpidR = setdiff(1:12, rcpid);
    areaXY = abs(sum(hlines(rcpidR,1:3).*repmat([0 0 1], [length(rcpidR) 1]),2));
    areaYZ = abs(sum(hlines(rcpidR,1:3).*repmat([1 0 0], [length(rcpidR) 1]),2));
    areaZX = abs(sum(hlines(rcpidR,1:3).*repmat([0 1 0], [length(rcpidR) 1]),2));
    [~, planeIDs] = max([areaXY areaYZ areaZX], [], 2); % 1:XY 2:YZ 3:ZX
    hlines(rcpidR,4) = planeIDs;

    extLine = hlines(:,1:6);
    for i = 1:12
        endPoint = hCorner(PointToEdge(i,:),:);
        uv = xyz2uvN(endPoint,extLine(i,4));
        umin = (min(uv(:,1))+pi)/2/pi;    umax = (max(uv(:,1))+pi)/2/pi;
        if umax-umin>0.5
            extLine(i,5) = umax;
            extLine(i,6) = umin;
        else
            extLine(i,5) = umin;
            extLine(i,6) = umax;
        end
    end

    % check if line segments locates in line
    validated = true;
    for i = rcpid
        lineRange = extLine(i,5:6);
        lsegRange = hlines(i,5:6);
        b = insideRange(lsegRange, lineRange);
        if ~b(1) || ~b(2)
            validated = false;
            break;
        end
    end
    if ~validated
        validHyps(samid).omapScr = -1;
        continue;
    end
    validHyps(samid).srcLine = hlines(rcpid,1:6);
    validHyps(samid).extLine = extLine;
    validHyps(samid).hCorner = hCorner;
end
fprintf('\n');

%% compute the checking direction on omap
[candiSetXYZ, ~] = icosahedron2sphere(6);
[ohei, owid, ~] = size(omap);
candiSetUV = uv2coords(xyz2uvN(candiSetXYZ), owid, ohei);
candiInd = sub2ind([ohei owid], candiSetUV(:,2), candiSetUV(:,1));
% convert gc to omap
gndGC = zeros(ohei, owid, 3);
gndGC(:,:,1) = max(gc(:,:,5),gc(:,:,6));
gndGC(:,:,2) = max(gc(:,:,1),gc(:,:,3));
gndGC(:,:,3) = max(gc(:,:,2),gc(:,:,4));
normGndGC = sum(gndGC(candiInd))+sum(gndGC(candiInd+1*ohei*owid))+sum(gndGC(candiInd+2*ohei*owid));
% omap
gndOmap = omap;
gndOmap(gndOmap>0) = 1;
normGndOmap = sum(gndOmap, 3);
gndOmap = gndOmap./(repmat(normGndOmap+0.0001, [1 1 3]));
normGndOmap = sum(gndOmap(candiInd))+sum(gndOmap(candiInd+1*ohei*owid))+sum(gndOmap(candiInd+2*ohei*owid));
numPoint = size(candiSetXYZ,1);

% merge
gndMerge = zeros(ohei, owid, 3);
gndMerge(1:ohei/2,:,:) = gndOmap(1:ohei/2,:,:);
gndMerge(ohei/2+1:ohei, :, :) = gndGC(ohei/2+1:ohei, :, :);
% normMegMap = sum(gndOmap, 3);
% gndOmap = gndOmap./(repmat(normMegMap+0.0001, [1 1 3]));
normMegMap = sum(gndMerge(candiInd))+sum(gndMerge(candiInd+1*ohei*owid))+sum(gndMerge(candiInd+2*ohei*owid));

% optimal merge
gndOptim = zeros(ohei, owid, 3);
gndOptim(1:685,:,:) = gndOmap(1:685,:,:);
gndOptim(686:ohei, :, :) = gndGC(686:ohei, :, :);
% normMegMap = sum(gndOmap, 3);
% gndOmap = gndOmap./(repmat(normMegMap+0.0001, [1 1 3]));
normOptMap = sum(gndOptim(candiInd))+sum(gndOptim(candiInd+1*ohei*owid))+sum(gndOptim(candiInd+2*ohei*owid));

%% check consistency of orientation on uniformly sampled point on sphere
for samid = 1:maxSample
    hypVp = false(numPoint,3);
    if validHyps(samid).omapScr>-0.5
        hCorner = validHyps(samid).hCorner;
        for i = 1:6
%             suf1 = cross(hCorner(PointToSurface(i,2),:), hCorner(PointToSurface(i,1),:), 2);
%             suf2 = cross(hCorner(PointToSurface(i,3),:), hCorner(PointToSurface(i,2),:), 2);
%             suf3 = cross(hCorner(PointToSurface(i,4),:), hCorner(PointToSurface(i,3),:), 2);
%             suf4 = cross(hCorner(PointToSurface(i,1),:), hCorner(PointToSurface(i,4),:), 2);
%             vad1 = dot(candiSetXYZ, repmat(suf1, [numPoint 1]), 2);
%             vad2 = dot(candiSetXYZ, repmat(suf2, [numPoint 1]), 2);
%             vad3 = dot(candiSetXYZ, repmat(suf3, [numPoint 1]), 2);
%             vad4 = dot(candiSetXYZ, repmat(suf4, [numPoint 1]), 2);
%             vadIN = vad1<0 & vad2<0 & vad3<0 & vad4<0;
            vadIN = insideCone( hCorner(PointToSurface(i,[4 3 2 1]),:), candiSetXYZ, 0);
            hypVp( vadIN, SurfaceNm(i)) = true;
        end
%         response = selVp & hypVp;
%         validHyps(samid).omapScr = sum( sum(response,2)>0 )/size(response,1);
        response = sum(gndOmap(candiInd(hypVp(:,1))+0*ohei*owid)) ...
                 + sum(gndOmap(candiInd(hypVp(:,2))+1*ohei*owid)) ...
                 + sum(gndOmap(candiInd(hypVp(:,3))+2*ohei*owid));
        validHyps(samid).omapScr = response/normGndOmap;
        response = sum(gndGC(candiInd(hypVp(:,1))+0*ohei*owid)) ...
                 + sum(gndGC(candiInd(hypVp(:,2))+1*ohei*owid)) ...
                 + sum(gndGC(candiInd(hypVp(:,3))+2*ohei*owid));
        validHyps(samid).gcScr = response/normGndGC;
        response = sum(gndMerge(candiInd(hypVp(:,1))+0*ohei*owid)) ...
                 + sum(gndMerge(candiInd(hypVp(:,2))+1*ohei*owid)) ...
                 + sum(gndMerge(candiInd(hypVp(:,3))+2*ohei*owid));
        validHyps(samid).mgScr = response/normMegMap;
        response = sum(gndOptim(candiInd(hypVp(:,1))+0*ohei*owid)) ...
                 + sum(gndOptim(candiInd(hypVp(:,2))+1*ohei*owid)) ...
                 + sum(gndOptim(candiInd(hypVp(:,3))+2*ohei*owid));
        validHyps(samid).opScr = response/normOptMap;
    end
end

%% save only validate hypothesis, with high score
omapScr = [validHyps.omapScr];
% [B, IX] = sort(omapScr, 'descend');
% selIX = IX(B>-0.5);
% hyps = validHyps(selIX);

hyps = validHyps(omapScr>-0.5);
end

function b = insideRange(pt, range)
range = range + [-0.02 +0.02];
if range(2)>range(1)
    b = pt>=range(1) & pt<=range(2);
else
    b1 = pt>=range(1) & pt<=1;
    b2 = pt>=0 & pt<=range(2);
    b = b1 | b2;
end

end


function varargout = efficency(X,P)
% function [con_eff,hrf_eff] = efficency(X,PARAMS)
% Tor Wager, 9/30/02
%
% a useful summary function for a design, incorporating tests of contrasts and of HRF shape estimation
%
% PARAMS structure contains inputs
% Optional fields:
%   contrasts   required for testing contrast efficiency; see calcEfficiency.m
%   Vi          intrinsic autocorrelation matrix; empty assumes identity matrix (independence)
%   S           smoothing matrix; empty for no smoothing
%   HRF         canonical hemodynamic response function; empty assumes SPM's canonical HRF
%               in this function, FIR estimation length fixed at 12 s.
%   nonlint     saturation threshold (ceiling) for predictors; simple nonlinearity model
%
% Required fields for condition function (vector) input:
%   ISI         sampling resolution of design vector, as time between elements in s
%   TR          sampling resolution of final design matrix (volumes acquired), in s
%
% Functions Called: (from OptimizeDesign GA toolbox)
% designvector2model.m
% tor_make_deconv_mtx2.m
% calcEfficiency.m

hrfsec = 30;

if ~isfield(P,'contrasts'), P.contrasts = []; end

if ~isfield(P,'S'), P.S = []; end

if any(size(X) == 1)    % then it's a condition function

    if ~isfield(P,'ISI'), error('ISI is required field in PARAMS input'), end
    if ~isfield(P,'TR'), error('TR is required field in PARAMS input'), end
    if ~isfield(P,'Vi'), P.Vi = eye(size(des,1)); end

    if ~isfield(P,'HRF'), P.HRF = spm_hrf(.1); end
    if ~isfield(P,'nonlint'), P.nonlint = []; end

    numsamps = ceil(length(X)*P.ISI/P.TR);
    X = designvector2model(X,P.ISI,P.HRF,P.TR,numsamps,P.nonlint,P.S);

    delta = [];
    for i = 1:max(X)
        delta(:,i) = (X == i);
    end
else                    % then it's a design matrix
end

svi = P.S * P.Vi;

if ~isempty(P.contrasts)
        % -------------------------------------------------------------------------------------------------
		% * efficiency
		% -------------------------------------------------------------------------------------------------

        	xtxitx = pinv(X);                                       		% inv(X'S'SX)*(SX)'; pseudoinv of (S*X)

            contrastweights = ones(1,size(xtxitx,1)-1);
			[dummy,varargout{1}] = calcEfficiency(contrastweights,P.contrasts,pinv(X),svi);

else

        [eff, eff_vector] = calcEfficiency([], [], pinv(X), svi);
        varargout{1} = eff_vector;
end

if isfield(P,'ISI') & isfield(P,'TR') & nargout > 1 & exist('delta', 'var')

        % -------------------------------------------------------------------------------------------------
		% * HRF shape estimation efficiency
		% -------------------------------------------------------------------------------------------------

			[X2] = tor_make_deconv_mtx2(delta,round(hrfsec / P.TR),P.TR / P.ISI);
            if ~isempty(P.S), X2 = P.S * X2;,end

        	xtxitx = pinv(X2);                                       		% inv(X'S'SX)*(SX)'; pseudoinv of (S*X)
			[dummy,varargout{2}] = calcEfficiency([],[],xtxitx,svi);

elseif nargout > 1
            disp(['Missing ISI or TR field, or already-constructed model entered: HRF efficiency not calculated.'])
            varargout{2} = [];
end

return


function tests = test_ft_specest_mtmfft

% MEM 1gb
% WALLTIME 00:10:00
% DEPENDENCY ft_specest_mtmfft

if nargout
  % assume that this is called by RUNTESTS
  tests = functiontests(localfunctions);
else
  % assume that this is called from the command line
  fn = localfunctions;
  for i=1:numel(fn)
    feval(fn{i});
  end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function testOptions(testCase)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

fsample = 1000;
nchan   = 8;
nsample = 1000;

dat     = randn(nchan, nsample) + 1 + linspace(0,1,nsample);
time    = (0:(nsample-1))/fsample;
freqoi  = 1:500;
verbose = false;

result = {};
result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', freqoi, 'taper', 'bartlett'      , 'verbose', verbose);
result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', freqoi, 'taper', 'barthannwin'   , 'verbose', verbose);
result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', freqoi, 'taper', 'blackman'      , 'verbose', verbose);
result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', freqoi, 'taper', 'blackmanharris', 'verbose', verbose);
result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', freqoi, 'taper', 'bohmanwin'     , 'verbose', verbose);
result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', freqoi, 'taper', 'chebwin'       , 'verbose', verbose);
result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', freqoi, 'taper', 'flattopwin'    , 'verbose', verbose);
result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', freqoi, 'taper', 'gausswin'      , 'verbose', verbose);
result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', freqoi, 'taper', 'hann'          , 'verbose', verbose); % hann and hanning are nearly the same in the MATLAB implementation, and exactly the same in external/signal
result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', freqoi, 'taper', 'kaiser'        , 'verbose', verbose);
result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', freqoi, 'taper', 'nuttallwin'    , 'verbose', verbose);
result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', freqoi, 'taper', 'parzenwin'     , 'verbose', verbose);
result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', freqoi, 'taper', 'rectwin'       , 'verbose', verbose);
result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', freqoi, 'taper', 'taylorwin'     , 'verbose', verbose);
result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', freqoi, 'taper', 'tukeywin'      , 'verbose', verbose);
result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', freqoi, 'taper', 'triang'        , 'verbose', verbose);

result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', 11:20, 'taper', 'hanning', 'verbose', verbose);
result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', 21:30, 'taper', 'hanning', 'verbose', verbose);
result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', 31:40, 'taper', 'hanning', 'verbose', verbose);

result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', freqoi, 'tapsmofrq', 1, 'taper', 'dpss', 'verbose', verbose); % Warning: using only one taper for specified smoothing
result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', freqoi, 'tapsmofrq', 3, 'taper', 'dpss', 'verbose', verbose);
result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', freqoi, 'tapsmofrq', 5, 'taper', 'dpss', 'verbose', verbose);

result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', freqoi, 'taper', 'hanning', 'verbose', verbose, 'pad', 1.1, 'padtype', 'zero');
result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', freqoi, 'taper', 'hanning', 'verbose', verbose, 'pad', 1.2, 'padtype', 'zero');
result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', freqoi, 'taper', 'hanning', 'verbose', verbose, 'pad', 1.3, 'padtype', 'zero');

% try out the various padding types, don't remove the polynomial fit
% this will result in warning: output frequencies are different from input frequencies
result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', freqoi, 'taper', 'hanning', 'verbose', verbose, 'polyorder', -1, 'pad', 1/0.9, 'padtype', 'zero');
result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', freqoi, 'taper', 'hanning', 'verbose', verbose, 'polyorder', -1, 'pad', 1/0.9, 'padtype', 'mean');
result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', freqoi, 'taper', 'hanning', 'verbose', verbose, 'polyorder', -1, 'pad', 1/0.9, 'padtype', 'localmean');
result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', freqoi, 'taper', 'hanning', 'verbose', verbose, 'polyorder', -1, 'pad', 1/0.9, 'padtype', 'edge');
result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', freqoi, 'taper', 'hanning', 'verbose', verbose, 'polyorder', -1, 'pad', 1/0.9, 'padtype', 'mirror');

% the default polyorder 0 was already computed above, so use different frequencies
result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', 2:2:100, 'taper', 'hanning', 'verbose', verbose, 'polyorder', 0);
result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', 2:2:100, 'taper', 'hanning', 'verbose', verbose, 'polyorder', 1);
result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', 2:2:100, 'taper', 'hanning', 'verbose', verbose, 'polyorder', 2);
result{end+1} = ft_specest_mtmfft(dat, time, 'freqoi', 2:2:100, 'taper', 'hanning', 'verbose', verbose, 'polyorder', 3);

% all iterations were done with (slightly) different options, hence the results should not be equal
for i=1:numel(result)
  for j=(i+1):numel(result)
    assert(~isequaln(result{i}, result{j}), 'the results %d and %d should not be equal', i, j);
  end
end


% lsinit;
% find first two points and establish correct scale
%

if size(alist,2)==0,
  % evaluate at absolutely smallest point
  alp=0;
  if amin>0, alp=amin; end;
  if amax<0, alp=amax; end;
  % new function value
  falp=feval(func,data,x+alp*p);
  alist=alp;flist=falp;
elseif size(alist,2)==1,
  % evaluate at absolutely smallest point
  alp=0;
  if amin>0, alp=amin; end;
  if amax<0, alp=amax; end;
  if alist~=alp,
    % new function value
    falp=feval(func,data,x+alp*p);
    alist=[alist,alp];flist=[flist,falp];
  end;
end;
aamin=min(alist);aamax=max(alist);
if amin>aamin | amax<aamax,
  alist,amin,amax
  error('non-admissible step in alist');
end;

% establish correct scale
if aamax-aamin<=scale,
  alp1=max(amin,min(-scale,amax));
  alp2=max(amin,min(+scale,amax));
  alp=inf;
  if aamin-alp1>=alp2-aamax, alp=alp1; end;
  if alp2-aamax>=aamin-alp1, alp=alp2; end;
  if alp<aamin | alp>aamax,
    % new function value
    falp=feval(func,data,x+alp*p);
    alist=[alist,alp];flist=[flist,falp];
  end;
end;

if size(alist,2)==1,
  scale,aamin,aamax,alp1,alp2
  error('lsinit bug: no second point found');
end;

lssort;


function origin = dtiMrMeshOrigin(handles)
% Computes the (x,y,z) origin of the image plane data
%
%   origin = dtiMrMeshOrigin(handles)
%
% NOTE: This routine was extracted from dtiMrMesh3AxisImage so we could
% build independent mrMesh outputs.  I think it may be computing the origin
% of the quarter images from dtiSplit... routine.
%
% Authors: Wandell, Dougherty
%
% Stanford VISTA Team

curPosition = dtiGet(handles,'curpos');

[xImX,xImY,xImZ] = dtiMrMeshImageCoords(handles,1,curPosition(1));
[yImX,yImY,yImZ] = dtiMrMeshImageCoords(handles,2,curPosition(2));
[zImX,zImY,zImZ] = dtiMrMeshImageCoords(handles,3,curPosition(3));

% For x,y,z you place the current position into the relevant location and
% then for the other two you find the value that is zero (which is the
% mid-point of the coordinates and substract off half of the number
% coordinates. I don't understand why (BW).
origin.x = -[-curPosition(1), ...
        find(xImY(:,1)==0) - length(xImY(:,1))/2, ...
        find(xImZ(1,:)==0) - length(xImZ(1,:))/2];

origin.y = -[find(yImX(:,1)==0) - length(yImX(:,1))/2, ...
        -curPosition(2), ...
        find(yImZ(1,:)==0) - length(yImZ(1,:))/2];

origin.z = -[find(zImX(1,:)==0) - length(zImX(1,:))/2, ...
        find(zImY(:,1)==0) - length(zImY(:,1))/2, ...
        -curPosition(3)];

return;

%------------------------------------
function [x,y,z] = dtiMrMeshImageCoords(handles,sliceThisDim,sliceNum)
%
%   [x,y,z] = dtiMrMeshImageCoords(handles,sliceThisDim,sliceNum);
%
% Produces an array of grid points that can be transformed in dtiGetSlice
% to image coords.  They are used for interpolating values in dtGetSlice.
%
% This routine should be extracted and then called from dtGetSlice  instead
% of the code that is there.
%
% Stanford VISTA Team

imDims = dtiGet(1, 'defaultBoundingBox');

nvals = max(imDims) - min(imDims) + 1;

% Computes a single integer for the desired slice, and two vectors showing
% the support of the other two.  So, if you are in the x-slice, you get
% that number of the x-slide and two vectors showing the y and z
% coordinates.
if(sliceThisDim == 1), x = sliceNum;
else x = linspace(imDims(1,1),imDims(2,1),nvals(1));
end;

if(sliceThisDim == 2), y = sliceNum;
else y = linspace(imDims(1,2),imDims(2,2),nvals(2));
end;

if(sliceThisDim == 3), z = sliceNum;
else z = linspace(imDims(1,3),imDims(2,3),nvals(3));
end;

% Convert the single number and the two linear dimensions into 3 3D
% matrices that define the x,y,z coordinates at each anatomical point.
% The singleton dimension is all one value, and the other two 3D
% matrices combine
[x,y,z] = meshgrid(x,y,z);

% Squeeze out the singleton dimension.
x = squeeze(x);
y = squeeze(y);
z = squeeze(z);

return


function [ug] = amu2ug(amu)
% Convert mass from atomic mass units to micrograms.
% Chad Greene 2012
ug = amu*1.6605402e-18;


function [varargout] = solver_RPCA_Lagrangian(Y,lambda_L,lambda_S,varargin)
% [L,S,errHist] = solver_RPCA_Lagrangian(Y,lambda_L,lambda_S, A_cell, opts)
% Solves the problem
%   minimize_{L,S} .5|| L + S - Y ||_F^2 + lambda_L ||L||_* + lambda_S ||S||_1
%
%   or if A_cell is provided, where A_cell = {A, At}
%   (A is a function handle, At is a function handle to the transpose of A)
%   then
%
%   minimize_{L,S} .5|| A(L + S) - Y ||_F^2 + lambda_L ||L||_* + lambda_S ||S||_1
%   (here, Y usually represents A(Y) )
%
%   errHist(:,1) is a record of the residual
%   errHist(:,2) is a record of the full objective (.f*resid^2 + lambda_L,
%      etc.)
%   errHist(:,3) is the output of opts.errFcn if provided
%
% opts is a structure with options:
%   opts.sum, opts.max  (as described above)
%   opts.L0         initial guess for L (default is 0)
%   opts.S0         initial guess for S (default is 0)
%   opts.tol        sets stopping tolerance
%   opts.maxIts     sets maximum number of iterations
%   opts.printEvery will print information this many iterations
%   opts.displayTime will print out timing information (default is true for large problems)
%   opts.errFcn     a function of (L,S) that records information
%   opts.trueObj    if provided, this will be subtracted from errHist(2,:)
%   opts.Lip        Lipschitz constant, i.e., 2*spectralNorm(A)^2
%                       by default, assume 2 (e.g., good if A = P_Omega)
%   opts.FISTA      whether to use FISTA or not. By default, false
%     opts.restart  how often to restart FISTA; set to -Inf to make it automatic
%   opts.BB         whether to use the Barzilai-Borwein spectral steplength
%     opts.BB_type  which BB stepsize to take. Default is 1, the larger step
%     opts.BB_split whether to calculate stepslengths for S and L independently.
%       Default is false, which is recommended.
%   opts.quasiNewton  uses quasi-Newton-like Gauss-Seidel scheme. By
%                       default, true
%     opts.quasiNewton_stepsize     stepsize length. Default is .8*(2/Lip)
%     opts.quasinewton_SLS          whether to take S-L-S sequence (default is true)
%                                   otherwise, takes a L-S Gauss-Seidel sequence
%   opts.SVDstyle   controls what type of SVD is performed.
%       1 = full svd using matlab's "svd". Best for small problems
%       2 = partial svd using matlab's "svds". Not recommended.
%       3 = partial svd using PROPACK, if installed. Better than option 2, worse than 4
%       4 = partial svd using randomized linear algebra, following
%           the Halko/Tropp/Martinnson "Structure in Randomness" paper
%       in option 4, there are additional options:
%       opts.SVDwarmstart   whether to "warm-start" the algorithm
%       opts.SVDnPower  number of power iterations (default is 2 unless warm start)
%       opts.SVDoffset  oversampling, e.g., "rho" in Tropp's paper. Default is 5
%
%   opts.L1L2      instead of using l1 penalty, e.g., norm(S(:),1), we can
%       also use block norm penalties, such as (if opts.L1L2 = 'rows')
%       the sum of the l2-norm of rows (i.e., l1-norm of rows),
%       or if opts.L1L2='cols', the sum of the l2-norms of colimns.
%       By default, or if opts.L1L2 = [] or false, then uses usual l1 norm.
%       [Feature added April 17 2015]
%
% Stephen Becker, March 6 2014
% See also solver_RPCA_constrained.m

[varargout{1:nargout}] = solver_RPCA_constrained(Y,lambda_S/lambda_L, -lambda_L, varargin{:});


%MAT_INTO_HDR  The old versions of SPM (any version before SPM5) store
%	an affine matrix of the SPM Reoriented image into a matlab file
%	(.mat extension). The file name of this SPM matlab file is the
%	same as the SPM Reoriented image file (.img/.hdr extension).
%
%	This program will convert the ANALYZE 7.5 SPM Reoriented image
%	file into NIfTI format, and integrate the affine matrix in the
%	SPM matlab file into its header file (.hdr extension).
%
%	WARNING: Before you run this program, please save the header
%	file (.hdr extension) into another file name or into another
%	folder location, because all header files (.hdr extension)
%	will be overwritten after they are converted into NIfTI
%	format.
%
%  Usage: mat_into_hdr(filename);
%
%  filename:	file name(s) with .hdr or .mat file extension, like:
%		'*.hdr', or '*.mat', or a single .hdr or .mat file.
%	e.g.	mat_into_hdr('T1.hdr')
%		mat_into_hdr('*.mat')
%

%  - Jimmy Shen (jimmy@rotman-baycrest.on.ca)
%
%-------------------------------------------------------------------------
function mat_into_hdr(files)

   pn = fileparts(files);
   file_lst = dir(files);
   file_lst = {file_lst.name};
   file1 = file_lst{1};
   [p n e]= fileparts(file1);

   for i=1:length(file_lst)
      [p n e]= fileparts(file_lst{i});
      disp(['working on file ', num2str(i) ,' of ', num2str(length(file_lst)), ': ', n,e]);
      process=1;

      if isequal(e,'.hdr')
         mat=fullfile(pn, [n,'.mat']);
         hdr=fullfile(pn, file_lst{i});

         if ~exist(mat,'file')
            warning(['Cannot find file "',mat  , '". File "', n, e, '" will not be processed.']);
            process=0;
         end
      elseif isequal(e,'.mat')
         hdr=fullfile(pn, [n,'.hdr']);
         mat=fullfile(pn, file_lst{i});

         if ~exist(hdr,'file')
            warning(['Can not find file "',hdr  , '". File "', n, e, '" will not be processed.']);
            process=0;
         end
      else
         warning(['Input file must have .mat or .hdr extension. File "', n, e, '" will not be processed.']);
         process=0;
      end

      if process
         load(mat);
         R=M(1:3,1:3);
         T=M(1:3,4);
         T=R*ones(3,1)+T;
         M(1:3,4)=T;

         [h filetype fileprefix machine]=load_nii_hdr(hdr);
         h.hist.qform_code=0;
         h.hist.sform_code=1;
         h.hist.srow_x=M(1,:);
         h.hist.srow_y=M(2,:);
         h.hist.srow_z=M(3,:);
         h.hist.magic='ni1';

         fid = fopen(hdr,'w',machine);
         save_nii_hdr(h,fid);
         fclose(fid);
      end
   end

   return;				% mat_into_hdr


function realimag(c)
%REALIMAG     Display real and imaginary part of interval Taylor separately
%
%   realimag(c)
%

% written  05/21/09     S.M. Rump
%

  loose = strcmp(get(0,'FormatSpacing'),'loose');

  name = inputname(1);
  if isempty(name)                    % happens for display(taylorinit(random))
    name = 'ans';
  end

  if isreal(c.t)
    display(c,name)
  else
    if loose, disp(' '); end
    display(real(c),['real(' name ')'])
    if loose, disp(' '); end

    if loose, disp(' '); end
    display(imag(c),['imag(' name ')'])
    if loose, disp(' '); end
  end


function [ x, y ] = dswap ( n, x, incx, y, incy )

%*****************************************************************************80
%
%% DSWAP interchanges two vectors.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 June 2005
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979.
%
%    Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
%    Basic Linear Algebra Subprograms for Fortran Usage,
%    Algorithm 539,
%    ACM Transactions on Mathematical Software,
%    Volume 5, Number 3, September 1979, pages 308-323.
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vectors.
%
%    Input, real X(*), one of the vectors to swap.
%
%    Input, integer INCX, the increment between successive entries of X.
%
%    Input, real Y(*), one of the vectors to swap.
%
%    Input, integer INCY, the increment between successive elements of Y.
%
%    Output, real X(*), the swapped vector.
%
%    Output, real Y(*), the swapped vector.
%
  if ( n <= 0 )

  elseif ( incx == 1 & incy == 1 )

    m = mod ( n, 3 );

    for i = 1 : m
      temp = x(i);
      x(i) = y(i);
      y(i) = temp;
    end

    for i = m+1 : 3 : n

      temp = x(i);
      x(i) = y(i);
      y(i) = temp;

      temp = x(i+1);
      x(i+1) = y(i+1);
      y(i+1) = temp;

      temp = x(i+2);
      x(i+2) = y(i+2);
      y(i+2) = temp;

    end

  else

    if ( 0 <= incx )
      ix = 1;
    else
      ix = ( - n + 1 ) * incx + 1;
    end

    if ( 0 <= incy )
      iy = 1;
    else
      iy = ( - n + 1 ) * incy + 1;
    end

    for i = 1 : n
      temp = x(ix);
      x(ix) = y(iy);
      y(iy) = temp;
      ix = ix + incx;
      iy = iy + incy;
    end

  end

  return
end


function airy_hi_values_test ( )

%*****************************************************************************80
%
%% AIRY_HI_VALUES_TEST demonstrates the use of AIRY_HI_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 February 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'AIRY_HI_VALUES_TEST:\n' );
  fprintf ( 1, '  AIRY_HI_VALUES stores values of \n' );
  fprintf ( 1, '  the Airy Hi function.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      X           FX\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, fx ] = airy_hi_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %12f  %24.16f\n', x, fx );

  end

  return
end


function rules(nx,ny,nz,alpha)

ssz=get(0,'ScreenSize');

hf=figure;
x0=ssz(3)/2;
y0=ssz(4)/2;
kfsz=0.9;
fpos=[x0-kfsz*ssz(3)/2 y0-kfsz*ssz(4)/2 kfsz*ssz(3) kfsz*ssz(4)];
set(hf,'MenuBar','none','Color',[1 1 1],'name','rules','NumberTitle','off','units','pixels','position',fpos);

ha = axes('parent',hf);
set(ha,'Visible','off');

set(ha,'UserData',[nx,ny,nz,alpha]);

% menu:
 hm=uimenu(hf,'Label','Save Image / Print');
    uimenu(hm,'Label','Save Image As...','Callback',['save_im(' num2str(hf) ')']);
    uimenu(hm,'Label','Page Setup...','Callback',['pgs(' num2str(hf) ')'],'Separator','on');
    uimenu(hm,'Label','Print Preview...','Callback',['pp(' num2str(hf) ')']);
    uimenu(hm,'Label','Print...','Callback',['pd('  num2str(hf) ')']);

euler_rule(ha);

h1 = uicontrol('Style', 'pushbutton', 'String', 'Euler to axis angle',...
    'Position', [0.25*fpos(3) 0.03*fpos(4) 0.2*fpos(4) 0.04*fpos(4)], 'Callback', ['euler_rule(' num2str(ha,'%20.20e') ')']);

h2 = uicontrol('Style', 'pushbutton', 'String', 'axis angle to Euler',...
    'Position', [0.45*fpos(3) 0.03*fpos(4) 0.2*fpos(4) 0.04*fpos(4)], 'Callback', ['axan_rule(' num2str(ha,'%20.20e') ')']);

h3 = uicontrol('Style', 'pushbutton', 'String', 'quatenion form',...
    'Position', [0.65*fpos(3) 0.03*fpos(4) 0.2*fpos(4) 0.04*fpos(4)], 'Callback', ['quat_rule(' num2str(ha,'%20.20e') ')']);


function out = degree
% x * degree =  x in standard arc measure

out = pi/180;


 function xs = eml_inc_em(x, Gb, yi, ci, ri, varargin)
%|function xs = eml_inc_em(x, Gb, yi, ci, ri, varargin)
%| E-ML-INC-EM algorithm for image reconstruction from Poisson emission data
%| (incremental EM algorithm)
%| model: Y_i ~ Poisson(c_i [G x]_i + r_i)
%| in
%|	x	[np,1]		initial estimate
%|	Gb	[nd,np]		Gblock object (see eml_osem_test.m)
%|	yi,ci,ri [nb,na]	see em_fbp.m (for model too)
%|
%| options
%|	'niter'	(#)		# of iterations
%|	'isave'	[]		list of iterations to archive
%|				(default: [] 'last)
%|	chat	(0 | 1)		verbosity
%|	pixmax	(value)		upper constraint for pixel values
%|	hds	(value)		'1' or '3' hidden data space
%|	os	(#)		how many "warmup" osem iterations
%|
%| out
%|	xs [np nsave]		estimates each (saved) iteration
%|
%| Copyright 2004-3-20, Jeff Fessler, University of Michigan

if nargin < 3, ir_usage, end

% options
arg.chat = 0;
arg.pixmax = inf;
arg.hds = 1;
arg.niter = 1;
arg.isave = 'last';
arg.os = 0;
arg.userfun = [];

arg = vararg_pair(arg, varargin);
arg.isave = iter_saver(arg.isave, arg.niter);

if ~isvar('ci') || isempty(ci)
	ci = ones(size(yi));
end
if ~isvar('ri') || isempty(ri)
	ri = zeros(size(yi));
end

Gb = block_ob(Gb, 'ensure'); % make it a block object (if not already)
nblock = block_ob(Gb, 'n');
starts = subset_start(nblock);

eml_check(yi, ci, ri, 'os', nblock);
[nb na] = size(yi);

ticker(mfilename, 1, arg.niter)

% precompute hidden data space factor
gam = eml_hds(Gb, ci, ri, arg.hds);
if arg.chat, printf('hds = %g', gam), end

% system (block) sensitivities
np = length(x);
asum = zeros(np, 1);
precon = zeros(np, nblock); % classic OSEM preconditioner
for iset=1:nblock
	ticker([mfilename ' : precon'], iset, nblock)
	istart = starts(iset);
	ia = istart:nblock:na;
	asum_m = Gb{istart}' * col(ci(:,ia));
	asum = asum + asum_m;
	asum_m(asum_m == 0) = Inf; % avoid divide by 0
	precon(:, iset) = 1 ./ asum_m;
end, clear asum_m
% asum = Gb' * ci(:);

x = max(x,0);
x = min(x,arg.pixmax);
xs = zeros(numel(x), length(arg.isave));
if any(arg.isave == 0)
	xs(:,find(arg.isave == 0)) = x;
end

%
% precompute sufficient statistics,
% possibly using osem warmup iterations
%
ff = zeros(np,nblock);
if arg.os == 0
	for iblock=1:nblock
		ff(:,iblock) = compute_fm(x, Gb, yi, ci, ri, gam, iblock);
	end

else
	for iter = 1:arg.os
		for iset = 1:nblock
			ticker([mfilename ': osem'], [iter iset], [arg.os nblock])
			iblock = starts(iset);
			ia = iblock:nblock:na;

			tmp = compute_fm(x, Gb, yi, ci, ri, gam, iblock);

			pre = precon(:,min(iset,ncol(precon)));	% 1 or iset
			x = max(pre .* tmp - gam, 0);

			if iter == arg.os
				ff(:,iblock) = tmp;
			end
		end

		if any(arg.isave == iter)
			xs(:, arg.isave == iter) = x;
		end
	end
end
fsum = sum(ff, 2);

%
% loop over incremental EM iterations
%

for iter = (2+arg.os):arg.niter

	%
	% loop over subsets
	%
	for iset=1:nblock
		ticker(mfilename, [iter iset], [arg.niter nblock])
		iblock = starts(iset);

		x = fsum ./ asum - gam;
		x = max(x,0);
		x = min(x,arg.pixmax);

		fsum = fsum - ff(:,iblock);
		ff(:,iblock) = compute_fm(x, Gb, yi, ci, ri, gam, iblock);
		fsum = fsum + ff(:,iblock);
	end

	if arg.chat, printf('Range %g %g', min(x), max(x)), end
	if any(arg.isave == iter)
		xs(:, arg.isave == iter) = x;
	end
end

%
% compute_fm()
% compute sufficient statistics
%
function fm = compute_fm(x, Gb, yi, ci, ri, gam, iblock)
eterm = eml_eterm(x, Gb, yi, ci, ri, iblock);
fm = (x + gam) .* eterm;


%EIMM_SMOOTH  EKF based fixed-interval IMM smoother using two IMM-EKF filters.
%
% Syntax:
%   [X_S,P_S,X_IS,P_IS,MU_S] = EIMM_SMOOTH(MM,PP,MM_i,PP_i,MU,p_ij,mu_0j,ind,dims,A,a,a_param,Q,R,H,h,h_param,Y)
%
% In:
%   MM    - Means of forward-time IMM-filter on each time step
%   PP    - Covariances of forward-time IMM-filter on each time step
%   MM_i  - Model-conditional means of forward-time IMM-filter on each time step
%   PP_i  - Model-conditional covariances of forward-time IMM-filter on each time step
%   MU    - Model probabilities of forward-time IMM-filter on each time step
%   p_ij  - Model transition probability matrix
%   ind   - Indices of state components for each model as a cell array
%   dims  - Total number of different state components in the combined system
%   A     - Dynamic model matrices for each linear model and Jacobians of each
%           non-linear model's measurement model function as a cell array
%   a     - Cell array containing function handles for dynamic functions
%           for each model having non-linear dynamics
%   a_param - Parameters of a as a cell array.
%   Q     - Process noise matrices for each model as a cell array.
%   R     - Measurement noise matrices for each model as a cell array.
%   H     - Measurement matrices for each linear model and Jacobians of each
%           non-linear model's measurement model function as a cell array
%   h     - Cell array containing function handles for measurement functions
%           for each model having non-linear measurements
%   h_param - Parameters of h as a cell array.
%   Y     - Measurement sequence
%
% Out:
%   X_S  - Smoothed state means for each time step
%   P_S  - Smoothed state covariances for each time step
%   X_IS - Model-conditioned smoothed state means for each time step
%   P_IS - Model-conditioned smoothed state covariances for each time step
%   MU_S - Smoothed model probabilities for each time step
%
% Description:
%   EKF based two-filter fixed-interval IMM smoother.
%
% See also:
%   EIMM_UPDATE, EIMM_PREDICT

% History:
%   09.01.2008 JH The first official version.
%
% Copyright (C) 2007,2008 Jouni Hartikainen
%
% $Id: imm_update.m 111 2007-11-01 12:09:23Z jmjharti $
%
% This software is distributed under the GNU General Public
% Licence (version 2 or later); please refer to the file
% Licence.txt, included with the software, for details.

function [x_sk,P_sk,x_sik,P_sik,mu_sk] = eimm_smooth(MM,PP,MM_i,PP_i,MU,p_ij,mu_0j,ind,dims,A,a,a_param,Q,R,H,h,h_param,Y)
    % Default values for mean and covariance
    MM_def = zeros(dims,1);
    PP_def = diag(ones(dims,1));

    % Number of models
    m = length(A);

    % Number of measurements
    n = size(Y,2);

    % The prior model probabilities for each step
    p_jk = zeros(m,n);
    p_jk(:,1) = mu_0j;
    for i1 = 2:n
        for i2 = 1:m
            p_jk(i2,i1) = sum(p_ij(:,i2).*p_jk(:,i1-1));
        end
    end

    % Backward-time transition probabilities
    p_ijb = cell(1,n);
    for k = 1:n
        for i1 = 1:m
            % Normalizing constant
            b_i = sum(p_ij(:,i1).*p_jk(:,k));
            for j = 1:m
                p_ijb{k}(i1,j) = 1/b_i.*p_ij(j,i1).*p_jk(j,k);
            end
        end
    end

    % Space for overall smoothed estimates
    x_sk = zeros(dims,n);
    P_sk = zeros(dims,dims,n);
    mu_sk = zeros(m,n);

    % Values of smoothed estimates at the last time step.
    x_sk(:,end)   = MM(:,end);
    P_sk(:,:,end) = PP(:,:,end);
    mu_sk(:,end)  = MU(:,end);

    % Space for model-conditioned smoothed estimates
    x_sik = cell(m,n);
    P_sik = cell(m,n);

    % Values for last time step
    x_sik(:,end) = MM_i(:,end);
    P_sik(:,end) = PP_i(:,end);

    % Backward-time estimated model probabilities
    mu_bp = MU(:,end);

    % Space for model-conditioned backward-time updated means and covariances
    x_bki = cell(1,m);
    P_bki = cell(1,m);

    % Initialize with default values
    for i1 = 1:m
       x_bki{i1} = MM_def;
       x_bki{i1}(ind{i1}) = MM_i{i1,end};
       P_bki{i1} = PP_def;
       P_bki{i1}(ind{i1},ind{i1}) = PP_i{i1,end};
    end

    % Space for model-conditioned backward-time predicted means and covariances
    x_kp = cell(1,m);
    P_kp = cell(1,m);

    % Initialize with default values
    for i1 = 1:m
       x_kp{i1} = MM_def;
       P_kp{i1} = PP_def;
    end

    for k = n-1:-1:1
        % Space for normalizing constants and conditional model probabilities
        a_j = zeros(1,m);
        mu_bijp = zeros(m,m);

        for i2 = 1:m
            % Normalizing constant
            a_j(i2) = sum(p_ijb{k}(:,i2).*mu_bp(:));
             % Conditional model probability
            mu_bijp(:,i2) = 1/a_j(i2).*p_ijb{k}(:,i2).*mu_bp(:);

            % Retrieve the transition matrix or the Jacobian of the dynamic model
            if isnumeric(A{i2})
                A2 = A{i2};
            elseif isstr(A{i2}) | strcmp(class(A{i2}),'function_handle')
                A2 = feval(A{i2},x_bki{i2}(ind{i2}),a_param{i2});
            else
                A2 = A{i2}(x_bki{i2}(ind{i2}),a_param{i2});
            end

            % Backward-time EKF prediction step
            [x_kp{i2}(ind{i2}), P_kp{i2}(ind{i2},ind{i2})] = ekf_predict1(x_bki{i2}(ind{i2}),...
                                                              P_bki{i2}(ind{i2},ind{i2}),...
                                                              inv(A2),Q{i2},a{i2},[],a_param{i2});

        end

        % Space for mixed predicted mean and covariance
        x_kp0 = cell(1,m);
        P_kp0 = cell(1,m);

        % Space for measurement likelihoods
        lhood_j = zeros(1,m);

        for i2 = 1:m
            % Initialize with default values
            x_kp0{i2} = MM_def;
            P_kp0{i2} = PP_def;
            P_kp0{i2}(ind{i2},ind{i2}) = zeros(length(ind{i2}),length(ind{i2}));

            % Mix the mean
            for i1 = 1:m
                x_kp0{i2}(ind{i2}) = x_kp0{i2}(ind{i2}) + mu_bijp(i1,i2)*x_kp{i1}(ind{i2});
            end

            % Mix the covariance
            for i1 = 1:m
                P_kp0{i2}(ind{i2},ind{i2}) = P_kp0{i2}(ind{i2},ind{i2}) + mu_bijp(i1,i2)*(P_kp{i1}(ind{i2},ind{i2})+(x_kp{i1}(ind{i2})-x_kp0{i2}(ind{i2}))*(x_kp{i1}(ind{i2})-x_kp0{i2}(ind{i2}))');
            end

            % Backward-time EKF update
            %
            % If the measurement model is linear don't pass h and h_param to ekf_update1
            if isempty(h) | isempty(h{i2})
                [x_bki{i2}(ind{i2}), P_bki{i2}(ind{i2},ind{i2}),K,MUP,S,lhood_j(i2)] = ekf_update1(x_kp0{i2}(ind{i2}),P_kp0{i2}(ind{i2},ind{i2}),Y(:,k),H{i2},R{i2},[],[],[]);
            else
                [x_bki{i2}(ind{i2}), P_bki{i2}(ind{i2},ind{i2}),K,MUP,S,lhood_j(i2)] = ekf_update1(x_kp0{i2}(ind{i2}),P_kp0{i2}(ind{i2},ind{i2}),Y(:,k),H{i2},R{i2},h{i2},[],h_param{i2});
            end
        end

        % Normalizing constant
        a_s = sum(lhood_j.*a_j);
        % Updated model probabilities
        mu_bp = 1/a_s.*a_j.*lhood_j;

        % Space for conditional measurement likelihoods
        lhood_ji = zeros(m,m);
        for i1 = 1:m
            for i2 = 1:m
                d_ijk = MM_def;
                D_ijk = PP_def;
                d_ijk = d_ijk + x_kp{i1};
                d_ijk(ind{i2}) = d_ijk(ind{i2}) - MM_i{i2,k};
                PP2 = zeros(dims,dims);
                PP2(ind{i2},ind{i2}) = PP_i{i2,k};
                D_ijk = P_kp{i1} + PP2;

                % Calculate the (approximate) conditional measurement likelihoods
                lhood_ji(i2,i1) = gauss_pdf(d_ijk,0,D_ijk);
            end
        end

        d_j = zeros(m,1);
        for i2 = 1:m
           d_j(i2) = sum(p_ij(i2,:).*lhood_ji(i2,:));
        end
        d = sum(d_j.*MU(:,k));

        mu_ijsp = zeros(m,m);
        for i1 = 1:m
            for i2 = 1:m
                mu_ijsp(i1,i2) = 1./d_j(i2)*p_ij(i2,i1)*lhood_ji(i2,i1);
            end
        end

        mu_sk(:,k) = 1/d.*d_j.*MU(:,k);

        % Space for two-step conditional smoothing distributions p(x_k^j|m_{k+1}^i,y_{1:N}),
        % which are a products of two Gaussians
        x_jis = cell(m,m);
        P_jis = cell(m,m);
        for i2 = 1:m
            for i1 = 1:m
                MM1 = MM_def;
                MM1(ind{i2}) = MM_i{i2,k};

                PP1 = PP_def;
                PP1(ind{i2},ind{i2}) = PP_i{i2,k};

                iPP1 = inv(PP1);
                iPP2 = inv(P_kp{i1});

                % Covariance of the Gaussian product
                P_jis{i2,i1} = inv(iPP1+iPP2);
                % Mean of the Gaussian product
                x_jis{i2,i1} = P_jis{i2,i1}*(iPP1*MM1 + iPP2*x_kp{i1});
            end
        end

        % Mix the two-step conditional distributions to yield model-conditioned
        % smoothing distributions.
        for i2 = 1:m
            % Initialize with default values
            x_sik{i2,k} = MM_def;
            P_sik{i2,k} = PP_def;
            P_sik{i2,k}(ind{i2},ind{i2}) = zeros(length(ind{i2}),length(ind{i2}));

            % Mixed mean
            for i1 = 1:m
                x_sik{i2,k} = x_sik{i2,k} + mu_ijsp(i1,i2)*x_jis{i2,i1};
            end

            % Mixed covariance
            for i1 = 1:m
                P_sik{i2,k} = P_sik{i2,k} + mu_ijsp(i1,i2)*(P_jis{i2,i1} + (x_jis{i2,i1}-x_sik{i2,k})*(x_jis{i2,i1}-x_sik{i2,k})');
            end
        end

        % Mix the overall smoothed mean
        for i1 = 1:m
            x_sk(:,k) = x_sk(:,k) + mu_sk(i1,k)*x_sik{i1,k};
        end

        % Mix the overall smoothed covariance
        for i1 = 1:m
            P_sk(:,:,k) = P_sk(:,:,k) + mu_sk(i1,k)*(P_sik{i1,k} + (x_sik{i1,k}-x_sk(:,k))*(x_sik{i1,k}-x_sk(:,k))');
        end

    end


function DEM_demo_DCM_LAP
% Demo applying the Laplace scheme to DCM with hidden states
%__________________________________________________________________________
% This routine demonstrates Generalized filtering for a DCM (Dynamic Causal
% Model) of fMRI responses using simulated data. This is an endogenous
% DCM in that there are no exogenous inputs. The demonstration specifies
% and inverts a full connectivity model and then illustrates post-hoc model
% optimization to recover (discover) the true architecture. It concludes
% with an automatic model optimization in terms of the prior variances over
% coupling parameters.
%__________________________________________________________________________
% Copyright (C) 2010 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: DEM_demo_DCM_LAP.m 6483 2015-06-21 21:14:34Z karl $

% Specify a DCM to generate synthetic data
%==========================================================================
rng('default')

% DEM Structure: create inputs
% -------------------------------------------------------------------------
T  = 256;
TR = 3.22;
n  = 4;
U  = spm_conv(randn(n,T),0,2)/4;

% set inversion parameters
% -------------------------------------------------------------------------
M(1).E.s  = 1/2;         % smoothness of random fluctuations
M(1).E.d  = 2;           % embedding dimension
M(1).E.n  = 4;           % embedding dimension
M(1).E.nE = 32;          % maximum number of DEM iterations

% priors
% -------------------------------------------------------------------------
A  = ones(n,n);
B  = zeros(n,n,0);
C  = zeros(n,n);
D  = zeros(n,n,0);

[pE,pC] = spm_dcm_fmri_priors(A,B,C,D);

% adjust M.f (GF works in time bins not seconds) and initialize M.P
% -------------------------------------------------------------------------
M(1).f  = inline(['spm_fx_fmri(x,v,P)*' num2str(TR)],'x','v','P');
M(1).g  = 'spm_gx_fmri';
M(1).x  = sparse(n,5);
M(1).pE = pE;
M(1).pC = pC;

M(2).v  = sparse(n,1);

% simulate endogenous dynamics
%==========================================================================

% true parameters (stochastic estimates)
% -------------------------------------------------------------------------
pP   = pE;
a    =  0.3;
b    = -0.3;
c    =  0.0;
pP.A = [c  a  0  0  0;
        a  c  b  0  0;
        0  b  c  a  0;
        0  0  a  c  b;
        0  0  0  a  c];

pP.A = pP.A(1:n,1:n);
pP.C = eye(n,n);
SIM  = spm_DEM_generate(M,U,{pP},{6,16},{16});

% Show simulated response
%--------------------------------------------------------------------------
spm_figure('Getwin','Figure 1');
spm_DEM_qU(SIM.pU)

% Specify generative model for inversion (DCM)
% =========================================================================

% set inversion parameters
% -------------------------------------------------------------------------
DCM.M       = M;
DCM.M(2).v  = 0;

% allow (only) neuronal [x, s, f, q, v] hidden states to fluctuate
% -------------------------------------------------------------------------
W           = ones(n,1)*exp([12 16 16 16 16]);
DCM.M(1).xP = exp(6);
DCM.M(1).V  = exp(6);        % prior log precision (noise)
DCM.M(1).W  = diag(W);       % fixed precision (hidden-state)
DCM.M(2).V  = exp(16);       % fixed precision (hidden-cause)

% Add data
% -------------------------------------------------------------------------
DCM.Y       = SIM.Y;

% Full connectivity inversion
% =========================================================================
F  = ones(n,n);
B  = zeros(n,n,0);
C  = zeros(n,1);
D  = zeros(n,n,0);

[pE pC]     = spm_dcm_fmri_priors(F,B,C,D);
DCM.M(1).pE = pE;
DCM.M(1).pC = pC;
FULL        = spm_LAP(DCM);

% Search model space with Savage-Dickey density ratio
% =========================================================================
[A K Nk]  = spm_dcm_sparse_priors(n);

% find true model
% -------------------------------------------------------------------------
for i = 1:length(A);
    if ~any(spm_vec(~~(pP.A + eye(n,n)) - A{i}))
        tA = i; break
    end
end

% find candidate models based on full-connectivity
% -------------------------------------------------------------------------
pE    = FULL.M(1).pE.A;
qE    = FULL.qP.P{1}.A;
qC    = FULL.qP.C(1:n*n,1:n*n);
pC    = FULL.M(1).pC(1:n*n,1:n*n);
for i = 1:length(A)
    k       = find(~A{i});
    rE      = pE;
    rC      = pC;
    rE(k)   = 0;
    rC(k,k) = 0;
    P(i,1)  = spm_log_evidence(qE,qC,pE,pC,rE,rC);
end

% posterior density under best model
% -------------------------------------------------------------------------
[p,i]     = max(P);
k         = find(~A{i});
rE        = pE;
rC        = pC;
rE(k)     = 0;
rC(k,k)   = 0;
[F,sE,sC] = spm_log_evidence_reduce(qE,qC,pE,pC,rE,rC);

% log-posterior (model)
% -------------------------------------------------------------------------
PP    = exp(P - max(P));
PP    = PP/sum(PP);

% Graphics (density on parameter and model space)
% -------------------------------------------------------------------------
spm_figure('Getwin','Figure 2'); clf

subplot(2,2,1)
spm_plot_ci(spm_vec(sE),sC),     hold on
bar(spm_vec(pP.A),1/2), hold off
title('true and MAP connections','FontSize',16)
axis square

subplot(2,2,2)
bar(P)
title('log-posterior','FontSize',16)
xlabel('model','FontSize',12)
ylabel('log-probability','FontSize',12)
axis square

subplot(2,2,3)
plot(Nk,    P,    '.k','MarkerSize',16), hold on
plot(Nk(tA),P(tA),'.r','MarkerSize',32), hold off
title('log-evidence','FontSize',16)
xlabel('graph size','FontSize',12)
ylabel('log-probability','FontSize',12)
axis square

subplot(2,2,4)
bar(PP)
title('posterior','FontSize',16)
xlabel('model','FontSize',12)
ylabel('probability','FontSize',12)
axis square

% Compare true and AMS adjacency
% -------------------------------------------------------------------------
spm_figure('Getwin','Figure 3'); clf

% Automatic model selection (optimizing prior variances on parameters)
% =========================================================================
[rE,rC]   = spm_dcm_optimise(qE,qC,pE,pC);
rA        = spm_unvec(diag(rC),pE);

subplot(2,2,1)
imagesc(full(A{tA} - diag(diag(A{tA}))))
title('true adjacency','FontSize',16)
xlabel('source','FontSize',12)
ylabel('target','FontSize',12)
axis square

subplot(2,2,2)
imagesc(full(rA))
title('optmised priors','FontSize',16)
xlabel('source','FontSize',12)
ylabel('target','FontSize',12)
axis square