% :Usage:
% ::
%
%    [c, alld] = getVertexColors(xyz, v, actcolor, [basecolor], [mind], 'vert', [xyz2], [actcolor2], 'vert', [xyz3], [actcolor3])
%
% given a point list of XYZ mm coordinates (3 columns)
% and a list of vertices in an isosurface,
% returns FaceVertexCData color values for brain near points and brain not near points.
% c is vertex color specification, 3 columns indicating RGB values
%
% Inputs:
% xyz   a 3-vol list of vertices to color
% v     can be a matrix of vertices
%       or a handle to a patch object containing vertices
%       if it's a handle, this function sets the color to interp
%       and the FaceVertexCData to the color matrix c
% actcolor
%       [r g b] activation color
% basecolor
%       [r g b] baseline color - optional.
% mind  optional - min distance to color vertex
%       Vertices within mind of an xyz coordinate will be colored
% colorscale
%       optional.  followed by vector of values by which to multiply input
%       color
%       these are scaled to be between .3 and one.
%       if entered, this will make the colors vary by, for example, Z score
%       so Z-scores are an acceptable input.
%       cscale should be in the same coordinate order as xyz
%       for ADDITIONAL clusters, repeat the 'colorscale', Z argument pair in the function call
%
%       YOU CAN ALSO pass true RGB values for each xyz coordinate in: 'colorscale', rgblist,
%       IF cscale is a 3-vector, it specifies the ACTUAL colors, and is not scaled to .3 - 1
%
%
% following basecolor and mind:
% additional xyz coordinate lists, with syntax:
% 'vert', xyz2 [your xyz input], [r g b] color for xyz plot
%
% also, you can enter 'ovlcolor' followed by [r g b] for overlaps between xyz sets
%   colors will ONLY appear in the overlap color if they share actual coordinates in common,
%   not necessarily if surface vertices are within the specified distance from both sets of coords.
%
% to get a good brain surface, try this:
%figure
%p = patch('Faces', faces, 'Vertices', vertices, 'FaceColor', [.5 .5 .5], ...
% 'EdgeColor', 'none', 'SpecularStrength', .2, 'FaceAlpha', 1, 'SpecularExponent', 200);
%lighting gouraud;camlight right
%axis image; myLight = camlight(0, 0);set(myLight, 'Tag', 'myLight');
%set(gcf, 'WindowButtonUpFcn', 'lightFollowView');lightfollowview
%drawnow
%
% ..
% by Tor Wager  August 25, 2002
% ..

function [c, alld] = getVertexColors_old_backup(xyz, v, actcolor, varargin)

    mind = 3;
    basecolor = [.5 .5 .5];
    alld = [];
    xyza = xyz;
    cscale = [];
    allda = [];
    vv = [];

    % -----------------------------------------------------------------------
    % * set up input arguments
    % -----------------------------------------------------------------------
    doalph = 0; cscale = [];
    if ~isempty(varargin), basecolor = varargin{1}; end
    if length(varargin) > 1, mind = varargin{2}; end
    ind = 1;
    for i = 3:length(varargin)
        if strcmp(varargin{i}, 'vert')
            vv{ind} = varargin{i+1};

            % intersections
            xyzb{ind, 1} = intersect(xyz, vv{ind}, 'rows');
            for j = ind-1:-1:1
                xyzb{ind, j} = intersect(vv{j}, vv{ind}, 'rows');
                xyza = intersect(xyza, xyzb{ind, j}, 'rows');
            end

            cc{ind} = varargin{i+2};
            ind = ind+1;
        elseif strcmp(varargin{i}, 'ovlcolor')
            ocol = varargin{i+1};
        elseif strcmp(varargin{i}, 'alphaone')
            doalph = 1;
        elseif strcmp(varargin{i}, 'allcolor')
            acol = varargin{i+1};
        elseif strcmp(varargin{i}, 'colorscale')
            cscale{end+1} = varargin{i+1};
            if min(size(cscale{end})) == 1
                % scale colors (may be necessary) - only if single vector, not RGB values
                cscale{end} = cscale{end} ./ max(cscale{end});
            end
            if any(cscale{end} < 0), error('Some color scale values are less than zero.'), end

        end
    end

    if isempty(cscale)
        cscale{1} = ones(size(xyz, 1), 1);
        for i = 1:length(vv)    % additional vertices
            cscale{end+1} = ones(size(vv{i}, 1), 1);
        end
    end

    %if ~isempty(cscale) don't need this.
    %    if length(cscale) < length(vv)
    %        cscale{length(vv)} = [];
    %    end
    %end

    if ishandle(v)
        p = v;
        v = get(p, 'Vertices');
        c = get(p, 'FaceVertexCData');   % get existing colors from surface
    else
        % uh-oh, not a handle!
        warning('Figure handle missing: Figure was closed?')
        p = findobj('Type', 'patch');
        v = get(p(1), 'Vertices');
        c = get(p, 'FaceVertexCData');   % get existing colors from surface
    end

    bad = any(size(c) - size(v));
    if bad
        c = repmat(basecolor, size(v, 1), 1);
    end

    % -----------------------------------------------------------------------
    % * main xyz color change
    % -----------------------------------------------------------------------

    t1 = clock;
    fprintf('Main color vertices: ')

    c = change_colors(c, xyz, v, mind, cscale, actcolor, p);
    drawnow();

    % -----------------------------------------------------------------------
    % * additional optional vertices
    % -----------------------------------------------------------------------
    if exist('vv', 'var')
        for j = 1:length(vv)
            fprintf('\nAdditional vertices: ')

            % cscale:
            % pass in vector of ones length coords for solid color
            % or scalar vals for color mapping
            % pass in cell array

            % cc{j} is color, cscale{j+1} is scaling vals for coords
            c = change_colors(c, vv{j}, v, mind, cscale(j+1), cc{j}, p);
        end
        drawnow();
    end

    % -----------------------------------------------------------------------
    % * figure out which vertices should be colored with ocol (overlap color)
    % -----------------------------------------------------------------------

    if exist('ocol', 'var') && exist('xyzb', 'var') && ~isempty(cat(1, xyzb{:}))

        alld = zeros(size(v, 1), 1);  % keeps track of overlap vertices
        xyzb = cat(1, xyzb{:});

        t1 = clock;
        fprintf('\nOverlap vertices: ')

        cscaletmp = {ones(size(xyzb, 1), 1)};
        c = change_colors(c, xyzb, v, mind, cscaletmp, ocol, p);
        drawnow();

        fprintf('%3.0f.done in %3.0f s\n', i, etime(clock, t1))
    end

    % -----------------------------------------------------------------------
    % * figure out which vertices should be colored with acol (all color)
    % -----------------------------------------------------------------------
    if exist('acol', 'var') && ~isempty(xyza) && length(vv)>1

        xyzall = xyza;
        for i = 2:length(vv)
            xyzall = intersect(xyzall, vv{i}, 'rows');
        end

        t1 = clock;
        fprintf('\nAll overlap vertices: ')

        cscaletmp = {ones(size(xyzall, 1), 1)};
        c = change_colors(c, xyzall, v, mind, cscaletmp, acol, p);
        drawnow();

        fprintf('%3.0f.done in %3.0f s\n', i, etime(clock, t1))

    end

    % -----------------------------------------------------------------------
    % * final color change
    % -----------------------------------------------------------------------

    if exist('p', 'var')
        set(p, 'FaceColor', 'interp')
        set(p, 'FaceVertexCData', c)
        drawnow()
    end

    %lightFollowView
end

% -----------------------------------------------------------------------
% * SUB-FUNCTIONS
% -----------------------------------------------------------------------

function c = change_colors(c, coords, v, mind, cscale, actcolor, p)

    if isempty(coords), disp('Coords is empty. Nothing to plot.'), return, end

    % select vertices that are even close
    cmax = max(coords, [], 1);
    cmin = min(coords, [], 1);
    fprintf('%3.0f vertices.  selecting: ', size(v, 1));
    wh = any(v - repmat(cmax, size(v, 1), 1) > mind, 2);
    wh2 = any(repmat(cmin, size(v, 1), 1) - v > mind, 2);

    % list vertices to test and possibly change color
    whverts = (1:size(v, 1))';
    whverts(wh | wh2) = [];     % vertex indices in big list
    smallv = v;
    smallv(wh | wh2,:) = [];     % vertices--restricted list

    fprintf('%3.0f\n', size(whverts, 1));

    if isempty(smallv), return, end

    % select coords that are even close
    % ----------------------------------
    cmax = max(smallv, [], 1);
    cmin = min(smallv, [], 1);
    fprintf('%3.0f coords.  selecting: ', size(coords, 1));
    wh = any(coords - repmat(cmax, size(coords, 1), 1) > mind, 2);
    wh2 = any(repmat(cmin, size(coords, 1), 1) - coords > mind, 2);

    % if cscale is matrix, must select these values of cscale as well!
    if size(cscale{1}, 1) == size(coords, 1)
        cscale{1}(wh | wh2,:) = [];
    end

    coords(wh | wh2,:) = [];

    if isempty(coords), return, end

    nc = size(coords, 1);
    fprintf('%3.0f\n', nc);

    % break up coords into list and run
    % ----------------------------------

    % break up coords into list
    xyz2 = {}; indx = 1;
    for kk = 1:1000:nc
        setwh{indx} = (kk:min(nc, kk + 1000 - 1))';
        xyz2{indx} = coords(setwh{indx},:);

        indx = indx + 1;
    end

    fprintf('Running %3.0f sets of coordinates: 000', length(xyz2));

    indxval = 1;
    wh_coords_near_surface = false(size(coords, 1), 1);

    for setno = 1:length(xyz2)
        fprintf('\b\b\b%03d', setno);

        for i = 1:size(xyz2{setno}, 1)
            % find vertices that are within range of point i in set  setno
            vertex_indices = find_in_radius(xyz2, setno, i, smallv, mind, whverts);

            % two modes: if cscale{1} is a matrix, treats as rgb values, and put in
            % color stored in cscale.  if cscale{1} is a vector, treat it as a scaling value for actcolor
            % In either case, indxval should index location of coordinate in FULL
            % list (corresponding to full list in cscale)

            c = color_change_vertices(c, mind, indxval, cscale, actcolor, vertex_indices);

            if ~isempty(vertex_indices), wh_coords_near_surface(indxval) = 1; end

            indxval = indxval + 1;
        end

        if exist('p', 'var')
            set(p, 'FaceColor', 'interp')
            set(p, 'FaceVertexCData', c)
        end
    end
end

function z = dist_tmp(w, p)

    %
    % if isstr(w)
    %   switch (w)
    %     case 'deriv',
    %       z = '';
    %     otherwise
    %       error('Unrecognized code.')
    %   end
    %   return
    % end

    % CALCULATION
    if nargin == 1
        p = w;
        w = w';
    end

    [S, R] = size(w);
    [R2, Q] = size(p);
    if (R ~= R2), error('Inner matrix dimensions do not match.'), end

    z = zeros(S, Q);
    if (Q<S)
        p = p';
        copies = zeros(1, S);
        for q=1:Q
            z(:,q) = sum((w-p(q+copies,:)).^2, 2);
        end
    else
        w = w';
        copies = zeros(1, Q);
        for i=1:S
            z(i,:) = sum((w(:,i+copies)-p).^2, 1);
        end
    end
    z = sqrt(z);

end

function [vertex_indices, d] = find_in_radius(xyz2, setno, i, smallv, mind, whverts)
    % output: indices of vertices in BIG list, and distances

    % get vertices v within box -- fast method
    wh = find(all(abs(bsxfun(@minus, xyz2{setno}(i,:), smallv)) <= mind, 2));
    d = dist_tmp(smallv(wh,:), xyz2{setno}(i,:)');

    % convert back to big list
    vertex_indices = whverts(wh(d < mind));
end

function c = color_change_vertices(c, mind, i, cscale, actcolor, vertex_indices)

    % two modes: if cscale{1} is a matrix, treats as rgb values, and put in
    % color stored in cscale
    % if cscale{1} is a vector, treat it as a scaling value for actcolor
    % In either case, the ith index point should correspond to the
    % coordinate being worked on.

    n = length(vertex_indices);

    if n
        if min(size(cscale{1})) > 1  % if we have rgb values rather than scaling values
            % this occurs if heatmap = yes and colorscale = no, we pass in rgb values
            c(vertex_indices,:) = repmat(cscale{1}(i,:), n, 1);
        else
            c(vertex_indices,:) = repmat(actcolor.*cscale{1}(i,:), n, 1);
        end
    end

end


%Help file for INTLAB Version 5.1
%
% - corrections in long toolbox (thanks to Nobito Yamamoto and Nozomu Matsuda)
% - new function 'isspd': verify positive definiteness
% - revision of verifylss for sparse matrices (omit eigs etc.)
% - warning when defining complex by infsup(zinf,zsup) [overestimation, use midrad]
% - abss replaced by mag (thanks to Arnold for proposing better naming)
% - generation of extremely ill-conditioned matrices (randmat) improved
% - norm delivers interval result
% - rounding unchanged after call of any INTLAB routine
%


function dx = f_Wrapper(u,x,p,parIdx)
% dx = f(u,x,p)
    if coder.target('MATLAB')
        % Specify your own f(u,x,p) function for normal excution
    else
        % Specify your own f(u,x,p) function for code generation
        coder.cinclude('iiwa14.h');
        q = x(1:7,1);
        qd = x(8:end,1);
        qdd = zeros(7,1);
        tau = u(1:7,1);
        coder.ceval('qdd_cal',  coder.ref(q),...
                                coder.ref(qd),...
                                coder.ref(qdd),...
                                coder.ref(tau),...
                                parIdx);
        dx  = [qd;qdd];
    end

end


% dtiEpilepsy
%
% This script is applied to compare a single subject with epilepsy (an
% adult) with the template of comparable adults without epilepsy.  We are
% using the SIRL20 template for the moment.
%
% To run this script you will need the statistics toolbox on your path.
% This toolbox is part of our normal lab Matlab and it is part of the Unix
% side.  We don't have it yet for license 11723 version.  We should get it.
%

% Specify and load the subject data and the atlas data
%
%  Does the subject need to be spatially aligned with the template? Usually
%  this is done once for each dti scan.  If you haven't done it, you can
%  get it done here.
doSpatialNorm = true;

% Is there a warping needed for the B0 field?  Radiation necrosis needed
% this, but normally we don't.  See paragraph below.  The b0Norm is only
% invoked during the doSpatialNorm operation.
b0Norm = false;

% There are several possible templates.  They should be listed here.
templateName = 'SIRL20adult';

% Set up the files and directories.  We have had trouble reading across the
% network from time to time.  So on PCs, we copy the key files locally.  On
% Linux boxes, everything works in place.
if(ispc)
    dataDir = '\\white.stanford.edu\biac2-wandell2\data\Epilepsy';
    templateDir = '\\white.stanford.edu\biac2-wandell2\data\templates\adult';
else
    dataDir = '/biac2/wandell2/data/Epilepsy';
    templateDir = '/biac2/wandell2/data/templates/adult';
end
avgdir = fullfile(templateDir,[templateName 'warp3_averageDataset']);
% Full path to the file
%subjectDt6 = epiSubjectFile(dataDir, 1,'postictal');
subjectDt6 = epiSubjectFile(dataDir, 1,'control');

% This normalization operation should be a integrated script, we think.  It
% requires SPM2.  It requires comments.
if(doSpatialNorm)
    if(ispc)
        % To avoid a memory paging error in windows, we copy the templates to a
        % local directory.
        copyfile(fullfile(templateDir,[templateName '_brain.img']), fullfile(tempdir,[templateName '_brain.img']));
        copyfile(fullfile(templateDir,[templateName '_brain.hdr']), fullfile(tempdir,[templateName '_brain.hdr']));
        copyfile(fullfile(templateDir,[templateName '_EPI.img']), fullfile(tempdir,[templateName '_EPI.img']));
        copyfile(fullfile(templateDir,[templateName '_EPI.hdr']), fullfile(tempdir,[templateName '_EPI.hdr']));
        copyfile(fullfile(templateDir,[templateName 'warp3_averageDataset'],'tensorSummary.mat'), fullfile(tempdir,['tensorSummary.mat']));
        copyfile(fullfile(templateDir,[templateName 'warp3_averageDataset'],'average_dt6.mat'), fullfile(tempdir,['average_dt6.mat']));

        % On the PC, we put all the files in one directory, the temp
        % directory for this user.
        templateDir = tempdir;
        avgdir = templateDir;
    end
    dtiSpatialNormalize;
else
    % No normalization needed
    subjectDt6 = [subjectDt6 '_' templateName '.mat'];
end

% Needs further definition.
tensorSumFile = fullfile(avgdir,'tensorSummary.mat');

% VOXELWISE ANALYSIS
% We are not sure about the difference between the template, the
% average and the tensor summary.
% We think that the
if(exist(tensorSumFile,'file'))
    disp(['Loading control group tensor summary (' tensorSumFile ')']);
    % This loads various summary structures such as md, fa, logTensor,
    % meanB0 and notes.
    load(tensorSumFile);
else
    % Compute the tensor summary.
    dtiTensorSummary;
end

% Load the atlas information
template = load(fullfile(avgdir,'average_dt6'));
xformDtToAcpc = template.xformToAcPc;

% Directory containing this subject's data
subDir = fileparts(subjectDt6);

% Single subject DTI data, spatially normalized
disp(['Loading dt6 file (' subjectDt6 ')']);
ssDt_sn = load(subjectDt6);
%mask = mask&ssDt_sn.dt6(:,:,:,1)>0;

%figure; imagesc(makeMontage(template.b0)); axis image; colormap gray;
%figure; imagesc(makeMontage(ssDt_sn.b0)); axis image; colormap gray;

%------------------------
% At this point the atlas data and the subject are loaded.
% We begin statistical analysis.  We begin with FA and MD analyses in this
% section. Then we move on to the full tensor analyses in the following
% section.

% The mask is the valid data mask computed  within dtiTensorSummary.
ssDt_ind = dtiImgToInd(ssDt_sn.dt6, mask);
[eigVec, eigVal] = dtiEig(ssDt_ind);

% Compute a test comparing FA in this subject with the atlas. This is done
% by a t-test implemented in dtiTTest.
[ss_fa, ss_md]   = dtiComputeFA(eigVal);

% Create the T-test images and related data for FA
[Tfa, DISTR, df] = dtiTTest(fa.mean, fa.stdev, fa.n, ss_fa);
[TfaImg, tFDRfa, n_signif,index_signif, pvals] = dtiTTestImage(Tfa, DISTR, df, mask);

% showSlices = [15:62];
% figure; imagesc(makeMontage(TfaImg,showSlices)); axis image; colormap cool; colorbar;
% set(gcf,'Name','FA test'); title(sprintf('tthresh, no FDR (p<10^-^4) = %0.1f',tThresh));

% Save out images that can be viewed in dtiFiberUI
dtiWriteNiftiWrapper(TfaImg, xformDtToAcpc, fullfile(subDir,['fa_' DISTR '-test_' num2str(df(1)) 'df.nii.gz']));
dtiWriteNiftiWrapper(-1*TfaImg, xformDtToAcpc, fullfile(subDir,['negfa_' DISTR '-test_' num2str(df(1)) 'df.nii.gz']));
dtiWriteNiftiWrapper(dtiIndToImg(fa.stdev,mask), xformDtToAcpc, fullfile(subDir,'fa_variance.nii.gz'));

% Repeat the process for mean diffusivity
[Tmd, DISTR, df] = dtiTTest(md.mean, md.stdev, md.n, ss_md);
[TmdImg, tFDRmd, n_signif,index_signif, pvals] = dtiTTestImage(Tmd, DISTR, df, mask);

% showSlices = [15:62];
% figure; imagesc(makeMontage(TmdImg,showSlices)); axis image; colormap cool; colorbar;
% set(gcf,'Name','MD test'); title(sprintf('tthresh, no FDR (p<10^-^4) = %0.1f',tThresh));

% Save out images that can be viewed in dtiFiberUI.  Until we fix the
% dtiFiberUI, we can't really select out the negative values.  So we write
% the data out both positive and negative for the moment.  We need to
% amend dtiFiberUI so that it
dtiWriteNiftiWrapper(TmdImg, xformDtToAcpc, fullfile(subDir,['md_' DISTR '-test_' num2str(df(1)) 'df.nii.gz']));
dtiWriteNiftiWrapper(-1*TmdImg, xformDtToAcpc, fullfile(subDir,['negmd_' DISTR '-test_' num2str(df(1)) 'df.nii.gz']));
dtiWriteNiftiWrapper(dtiIndToImg(md.stdev,mask), xformDtToAcpc, fullfile(subDir,'md_variance.nii.gz'));

% To view the locations of the significantly different voxels, use
% dtiFiberUI.  Load the file, say, fa_t-test_19df.nii.gz as a NIFTI image
% overlay into the program.  You can adjust the cutoff in the user
% interface.  You can use, say, the fdr threshold (tFDRxx) as the p < 0.05
% cutoff.

% clear TmdImg TfaImg Tmd Tfa

%--------------------------------------
% The next two analyses require log-space tensors.

% Log-transform the single subject data
eigVal(eigVal<0) = 0;
eigVal = log(eigVal);
ssDt_ind = dtiEigComp(eigVec, eigVal);

% Test for VECTOR differences
[T, DISTR, df] = dtiLogTensorTest('vec', logTensor.mean, logTensor.stdev, logTensor.n, ssDt_ind);

Timg = dtiIndToImg(T, mask);
fThresh = finv(1-10^-4, df(1), df(2));
fMax = finv(1-10^-12, df(1), df(2));
Timg(Timg>fMax) = fMax;
fMax = max(Timg(:));
Simg = dtiIndToImg(logTensor.stdev,mask);

% figure; imagesc(makeMontage(Timg,showSlices)); axis image; colormap hot; colorbar;
% set(gcf,'Name','Vec test'); title(sprintf('fthresh (p<10^-^4) = %0.1f',fThresh));
%
% figure; imagesc(makeMontage(Simg,showSlices)); axis image; colormap hot; colorbar;
% set(gcf,'Name','Vec test variance');

%
dtiWriteNiftiWrapper(Timg, xformDtToAcpc, fullfile(subDir,['vec_' DISTR '-test_' num2str(df(1)) ',' num2str(df(2)) 'df.nii.gz']));
dtiWriteNiftiWrapper(Simg, xformDtToAcpc, fullfile(subDir,'vec_variance.nii.gz'));
disp(['dtiFiberUI threshold: ' num2str(fThresh/fMax)]);

% Test for VALUE differences
[T, DISTR, df] = dtiLogTensorTest('val', logTensor.mean, logTensor.stdev, logTensor.n, ssDt_ind);

Timg = dtiIndToImg(T, mask);
fThresh = finv(1-10^-4, df(1), df(2));
fMax = finv(1-10^-12, df(1), df(2));
Timg(Timg>fMax) = fMax;
fMax = max(Timg(:));
Simg = dtiIndToImg(logTensor.stdev,mask);

% figure; imagesc(makeMontage(Timg,showSlices)); axis image; colormap hot; colorbar;
% set(gcf,'Name','Values test'); title(sprintf('fthresh (p<10^-^4) = %0.1f',fThresh));
%
% figure; imagesc(makeMontage(Simg,showSlices)); axis image; colormap hot; colorbar;
% set(gcf,'Name','Values test variance');

%
dtiWriteNiftiWrapper(Timg, xformDtToAcpc, fullfile(subDir,['val_' DISTR '-test_' num2str(df(1)) ',' num2str(df(2)) 'df.nii.gz']));
dtiWriteNiftiWrapper(Simg, xformDtToAcpc, fullfile(subDir,'val_variance.nii.gz'));
disp(['dtiFiberUI threshold: ' num2str(fThresh/fMax)]);

% figure; imagesc(makeMontage(sqrt(Timg),[20:55])); axis image; colormap hot; colorbar;
% set(gcf,'Name','Vec Standardized Distance');

% Possible to test for 'full' also.  Need more comments in dtiLogTensorTest

% FDR analysis for eigenvector differences
%
% Sqrt(F) is the standardized distance between the groups.  Ask about this.
%
fdrVal = 0.05; fdrType = 'general';
T(isnan(T)) = 0;
pvals = 1-fcdf(T, df(1), df(2));
[n_signif,index_signif] = fdr(pvals,fdrVal,fdrType,'mean');
disp(n_signif);max(pvals(index_signif))

% Convert back to an fThreshold
pThreshFDR = max(pvals(index_signif));
fThreshFDR = finv(1-pThreshFDR, df(1), df(2));
disp(sprintf('f-threshold for FDR (%s method) of %0.3f: %0.2f (%0.3f).\n',...
    fdrType,fdrVal,fThreshFDR,fThreshFDR/fMax));

%---- Make locally viewable versions of the images ----
%
logPimg = dtiIndToImg(-log10(pvals), mask);
cmap = autumn(256); maxLogP = 10; minLogP = -log10(pThreshFDR);

anatRgb = repmat(mrAnatHistogramClip(double(ssDt_sn.anat.img),0.4,0.98),[1,1,1,3]);
tmp = mrAnatResliceSpm(logPimg, inv(ssDt_sn.xformToAcPc), [], ssDt_sn.anat.mmPerVox, [1 1 1 0 0 0]);
tmp(tmp>maxLogP) = maxLogP;
tmp = (tmp-minLogP)./(maxLogP-minLogP);
overlayMask = (tmp>=0);
tmp(~overlayMask) = 0;
overlayMask = repmat(overlayMask,[1 1 1 3]);
overlayRgb = reshape(cmap(round(tmp*255+1),:),[size(tmp) 3]);
anatRgb(overlayMask) = overlayRgb(overlayMask);

% reorient so that the eyes point up
anatRgb = flipdim(permute(anatRgb,[2 1 3 4]),1);
%sl = [2:2:40];
sl = [-36:2:60];
for(ii=1:length(sl)) slLabel{ii} = sprintf('Z = %d',sl(ii)); end
slImg = inv(ssDt_sn.anat.xformToAcPc)*[zeros(length(sl),2) sl' ones(length(sl),1)]';
slImg = round(slImg(3,:));
anatOverlay = makeMontage3(anatRgb, slImg, ssDt_sn.anat.mmPerVox(1), 0, slLabel);
mrUtilPrintFigure(fullfile(subDir,'ss_t1_vecSPM'));
legendLabels = explode(',',sprintf('%0.1f,',[minLogP:1:maxLogP]));
legendLabels{end} = ['>=' num2str(maxLogP)];
mrUtilMakeColorbar(cmap, legendLabels, '-log10(p)', fullfile(subDir,'vecSPM_legend'));

templateBrain = template.anat.img;
templateBrain(template.anat.brainMask<0.25) = 0;
templateBrain(template.anat.brainMask<0.5) = templateBrain(template.anat.brainMask<0.5)*.5;
avgRgb = repmat(templateBrain,[1,1,1,3]);
avgRgb(overlayMask) = overlayRgb(overlayMask);

% reorient so that the eyes point up
avgRgb = flipdim(permute(avgRgb,[2 1 3 4]),1);
avgOverlay = makeMontage3(avgRgb, slImg, ssDt_sn.anat.mmPerVox(1), 0, slLabel);

slImg = inv(ssDt_sn.xformToAcPc)*[zeros(length(sl),2) sl' ones(length(sl),1)]';
slImg = round(slImg(3,:));
overlayRgb = flipdim(permute(overlayRgb,[2 1 3 4]),1);
overlay = makeMontage3(overlayRgb, slImg, [], 2);

% figure; imagesc(makeMontage(logPimg,showSlices)); axis image; colormap hot; colorbar;
threshMask = zeros(size(pvals));
threshMask(index_signif) = 1;
threshMask = dtiIndToImg(threshMask, mask);
img = logPimg; img(threshMask<1) = 0;
%  figure; imagesc(makeMontage(img,showSlices)); axis image; colormap hot; colorbar;

%%%END HERE%%%

%--------------------------------------------------------------------
% FDR Analysis

% Quantile transformation
Tchisq = chi2inv(cdf(DISTR, T, df(1), df(2)), df(1));
DISTR = 'chi2';
% Histograms
H = fdrHist(Tchisq,0.2,1);

% Empirical null
Tmax = prctile(Tchisq,90);
w = (H.x < Tmax);
[params, paramsCov, H0hat] = fdrEmpNull(H, w, DISTR, {}, df);
[p0, s0, df0] = deal(params(1), params(2), params(3));
paramsConf = [[log(p0), s0, df0] + 1.95*sqrt(diag(paramsCov))';
    [log(p0), s0, df0] - 1.95*sqrt(diag(paramsCov))'];

% p0 adjustment for theoretical null
[params, paramsCov, H0] = fdrEmpNull(H, w, DISTR, {'df','s'}, df);
p0H0 = params(1);

% FDR curves
[fdrH0, t] = fdrCurveHist('FDR', H0, 1);
[fdrH0hat, t] = fdrCurveHist('FDR', H0hat, 1);

% Threshold
thr = fdrThresh(1, fdrH0(:,1), t, level);
thr = fdrThresh(1, fdrH0hat(:,1), t, level);

%-----------------------------------------------------------------------
% FDR Plots

% Histogram of test stats
figure, set(gcf, 'name', 'Histograms'), hold on
h = bar(H.x, H.hist, 1, 'w');
h0 = plot(H0.x, H0.yhat, 'b');
h1 = plot(H0hat.x, H0hat.yhat, 'r');
hold off, legend([h0 h1], 'theo null','emp null',1)
xlabel('T'); ylabel('voxel count');
a=axis; axis([0 prctile(T,99) a(3:4)]);

% FDR curves
figure,	set(gcf, 'name', 'FDR'), hold on
plot(t, fdrH0(:,1), 'b')
plot(t, fdrH0hat(:,1), 'r')
legend('theo null','emp null')
plot(t, fdrH0(:,2), 'b:', t, fdrH0(:,3), 'b:')
plot(t, fdrH0hat(:,2), 'r:', t, fdrH0hat(:,3), 'r:')
hold off, axis([0 prctile(Tchisq,99.99) 0 1]);
xlabel('threshold'); ylabel('FDR');

pthresh = 1-chi2cdf(t,df(1));
figure;plot(log10(pthresh), fdrH0(:,1), 'b')
xlabel('log10(p-value)'); ylabel('FDR');

% Test for eigenvalue differences
[T, M, S, DISTR, df] = dtiLogTensorTest(1, [2:size(allDt6_ind,3)], allDt6_ind, 'val');
Timg = dtiIndToImg(T, mask);
fThresh = finv(1-10^-4, df(1), df(2));
fMax = finv(1-10^-12, df(1), df(2));
Timg(Timg>fMax) = fMax;
fMax = max(Timg(:));
figure; imagesc(makeMontage(Timg,[20:55])); axis image; colormap hot; colorbar;
set(gcf,'Name','Val test'); title(sprintf('fthresh (p<10^-^4) = %0.1f',fThresh));
dtiWriteNiftiWrapper(Timg, xformDtToAcpc, fullfile(subDir,['val_' DISTR '-test_' num2str(df(1)) ',' num2str(df(2)) 'df.nii.gz']));
dtiWriteNiftiWrapper(dtiIndToImg(S,mask), xformDtToAcpc, fullfile(subDir,'val_variance.nii.gz'));


function [params] = sv_calcMcNodes(params,nNodeStart, nNodeEnd)
% function [params] = sv_calcMcNodes(params,nNodeStart, nNodeEnd);
% ----------------------------------------------------------------
% Calculation of magnitude of completeness specifying the nodes to be calculated for distributing on different CPUs
%
% Input parameters:
%   params.mCatalog           Earthquake catalog
%   params.mPolygon           Polygon (defined by ex_selectgrid)
%   params.vX                 X-vector (defined by ex_selectgrid)
%   params.vY                 Y-vector (defined by ex_selectgrid)
%   params.vUsedNodes         Used nodes vX * vY defining the mPolygon (defined by ex_selectgrid)
%   params.bRandom            Perform random simulation (=1) or real calculation (=0)
%   params.nCalculation       Number of random simulations
%   params.bMap               Calculate a map (=1) or a cross-section (=0)
%   params.bNumber            Use constant number (=1) or constant radius (=0)
%   params.nNumberEvents      Number of earthquakes if bNumber == 1
%   params.fMaxRadius         Maximum Radius using a constant number of events; works only with bNumber == 1
%   params.fRadius            Radius of gridnode if bNumber == 0
%   params.nMinimumNumber     Minimum number of earthquakes per node for determining a b-value
%   params.fMinMag            Lower limit of magnitude range for testing
%   params.fMaxMag            Upper limit of magnitude range for testing
%   params.bTimePeriod        Calculate seismicity difference for 2 periods (0) until start and end of catalog or
%                             a specific time period before and after fSplitTime (1)
%   params.fTimePeriod        Length of time periods
%   params.bTstart            Check for starting time of temporal mapping
%   params.fTstart            Starting time for temporal mapping
%   params.bBstnum            Check for boostrap sampling
%   params.fBstnum            Number of bootstrap samples
%   params.fBinning           Bin size for magnitude binning
%   params.sComment           Comment on calculation

% Output parameters:
%   Same as input parameters including
%   params.mValueGrid         Matrix of calculated values
%   params.vcsGridNames       Names of parameters calculated
%   Check sv_NodeCalcMc.m for a list of variables!!
%
% J. Woessner; woessner@seismo.ifg.ethz.ch
% last update: 27.03.03

global bDebug;
if bDebug
    report_this_filefun(mfilename('fullpath'));
end

% Initialize
vResults = [];
params.sComment = [];
if isempty(params.fBinning)
    params.fBinning = 0.1;
end

% Determine time period of catalog
params.fTminCat = min(params.mCatalog(:,3));
params.fTmaxCat = max(params.mCatalog(:,3));
% Adjust to decimal years
fTimePeriod =params.fTimePeriod/365;

% Init result matrix
mValueGrid_ = [];

% Temporary saving the original catalog
mCatalog = params.mCatalog;

% Check for bootstrapping or not
% ------------------------------
% Case of calculations with bootstrapping
if (params.bBstnum == 1)
    % Loop over time
    fTstart = params.fTstart;
    while fTstart < params.fTmaxCat
        mValueGrid_ = [];
        params.mCatalog = mCatalog;
        % Create Indices to catalog and select quakes in time period
        vSel = (fTstart <= params.mCatalog(:,3) & params.mCatalog(:,3) < fTstart+fTimePeriod);
        params.mCatalog = params.mCatalog(vSel,:);
        [params.caNodeIndices] = ex_CreateIndexCatalog(params.mCatalog, params.mPolygon, params.bMap, params.nGriddingMode, ...
         params.nNumberEvents, params.fRadius, params.fSizeRectHorizontal, params.fSizeRectDepth);
        % Loop over all grid nodes
        hWaitbar1 = waitbar(0,'Calculating nodes...');
        set(hWaitbar1,'Numbertitle','off','Name','Node percentage');
        for nNode_ = nNodeStart:nNodeEnd
            % Create node catalog
            mNodeCatalog_ = params.mCatalog(params.caNodeIndices{nNode_}, :);
            % Check for constant number of events calculations
            if (params.nGriddingMode == 0)
                [mNodeCatalog_] = ex_CheckMaxRadius(mNodeCatalog_, params.mPolygon, nNode_, params.caNodeIndices, params.fMaxRadius, params.nNumberEvents, params.bMap);
            end
            [nX,nY] = size(mNodeCatalog_);
            if (nX < params.nMinimumNumber)
                mValueGrid_= [mValueGrid_; NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN nX NaN NaN NaN NaN];
            else
                [rCalcNodeResult_] = sv_NodeCalcMc(params,mNodeCatalog_);
                % Store the results
                mValueGrid_= [mValueGrid_; rCalcNodeResult_.fMc_max rCalcNodeResult_.fMc_90 rCalcNodeResult_.fMc_95 rCalcNodeResult_.fMc_com...
                        rCalcNodeResult_.fMc_EMR rCalcNodeResult_.fMc_shi rCalcNodeResult_.fMc_Bst...
                        rCalcNodeResult_.fStd_Mc rCalcNodeResult_.fBvalue_Bst rCalcNodeResult_.fStd_B...
                        rCalcNodeResult_.fAvalue_Bst rCalcNodeResult_.fStd_A nX...
                        rCalcNodeResult_.bH rCalcNodeResult_.fPval rCalcNodeResult_.bH_Bst rCalcNodeResult_.fPval_Bst];
            end; % End of if on nNode_
            if rem(nNode_,floor(length(params.mPolygon(:,1))/10)) == 0
                waitbar(nNode_/length(params.mPolygon(:,1)))
                %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
                % Temporary saving
                params.vcsGridNames = cellstr(char('Mc max. curvature' , 'Mc 90% goodness of fit' , 'Mc 95% goodness of fit',...
                    'Mc best combination', 'Mc EMR-method', 'Mc(Shi-b-uncertainty)', 'Mc(Bst-mean)', 'Mc(Bst-2nd-moment)',...
                    'Mc(Bst-b)', 'Mc(b_2nd-moment)','Mc(Bst-a)', 'Mc(a_2nd-moment)','Number of events',...
                    'H(KST)','P(KST)','H(KST_Bst)','P(KST_Bst)'));
                params.mValueGrid = mValueGrid_;
                % Add parameter to params.sComment
                if  params.nGriddingMode == 0;   % Constant number
                    params.sComment = ['Starttime ' num2str(fTstart) ' Spacing ' num2str(params.fSpacingHorizontal) ' deg.,'...
                            ' Time period ' num2str(params.fTimePeriod) ' d, Constant number: ' num2str(params.nNumberEvents) ', MaxRadius: '...
                            num2str(params.fMaxRadius) ' km'];
                    vResults = params;
                    save(['result_Time' num2str(fTstart) '_Constnum_' num2str(params.nNumberEvents) '_MaxRad_' num2str(params.fMaxRadius)...
                            '_Nmin_' num2str(params.nMinimumNumber) '_Node' num2str(nNode_) '.mat'], 'vResults');
                elseif params.nGriddingMode == 1;   % Constant radius
                    params.sComment = ['Starttime ' num2str(fTstart) ' Spacing ' num2str(params.fSpacingHorizontal) ' deg.,'...
                            ' Time period ' num2str(params.fTimePeriod) ' d, Radius: ' num2str(params.fRadius) ' km, Nmin: ' num2str(params.nMinimumNumber)];
                    vResults = params;
                    save(['result_Time' num2str(fTstart) '_Rad_' num2str(params.fRadius) '_Nmin_' num2str(params.nMinimumNumber)  '_Node' num2str(nNode_) '.mat'], 'vResults');
                else  % Rectangle mode
                    params.sComment = ['Starttime ' num2str(fTstart) ' Spacing ' num2str(params.fSpacingHorizontal) ' deg.,'...
                            ' Time period ' num2str(params.fTimePeriod) ' d, Rect. X: ' num2str(params.fSizeRectHorizontal) ' km, Rect. Y: ' num2str(params.fSizeRectDepth)...
                            ' km, Nmin: ' num2str(params.nMinimumNumber)];
                    vResults = params;
                    save(['result_Time' num2str(fTstart) '_RectX_' num2str(params.fSizeRectHorizontal) '_RectY_' num2str(params.fSizeRectDepth)...
                            '_Nmin_' num2str(params.nMinimumNumber)  '_Node' num2str(nNode_) '.mat'], 'vResults');
                end; % END of params.nGriddingmode
                vResults =[];
            end; % End updating waitbar
        end; % for nNode
        close(hWaitbar1);
        % Parameter description
        params.vcsGridNames = cellstr(char('Mc max. curvature' , 'Mc 90% goodness of fit' , 'Mc 95% goodness of fit',...
            'Mc best combination', 'Mc EMR-method', 'Mc(Shi-b-uncertainty)', 'Mc(Bst-mean)', 'Mc(Bst-2nd-moment)',...
            'Mc(Bst-b)', 'Mc(b_2nd-moment)','Mc(Bst-a)', 'Mc(a_2nd-moment)','Number of events',...
                    'H(KST)','P(KST)','H(KST_Bst)','P(KST_Bst)'));
        params.mValueGrid = mValueGrid_;
        % Add parameter to params.sComment
        if  params.nGriddingMode == 0;   % Constant number
            params.sComment = ['Starttime ' num2str(fTstart) ' Spacing ' num2str(params.fSpacingHorizontal) ' deg.,'...
                    ' Time period ' num2str(params.fTimePeriod) ' d, Constant number: ' num2str(params.nNumberEvents) ', MaxRadius: '...
                    num2str(params.fMaxRadius) ' km'];
            vResults = params;
            save(['result_Time' num2str(fTstart) '_Constnum_' num2str(params.nNumberEvents) '_MaxRad_' num2str(params.fMaxRadius)...
                    '_Nmin_' num2str(params.nMinimumNumber) '.mat'], 'vResults');
        elseif params.nGriddingMode == 1;   % Constant radius
            params.sComment = ['Starttime ' num2str(fTstart) ' Spacing ' num2str(params.fSpacingHorizontal) ' deg.,'...
                    ' Time period ' num2str(params.fTimePeriod) ' d, Radius: ' num2str(params.fRadius) ' km, Nmin: ' num2str(params.nMinimumNumber)];
            vResults = params;
            save(['result_Time' num2str(fTstart) '_Rad_' num2str(params.fRadius) '_Nmin_' num2str(params.nMinimumNumber) '.mat'], 'vResults');
        else  % Rectangle mode
            params.sComment = ['Starttime ' num2str(fTstart) ' Spacing ' num2str(params.fSpacingHorizontal) ' deg.,'...
                    ' Time period ' num2str(params.fTimePeriod) ' d, Rect. X: ' num2str(params.fSizeRectHorizontal) ' km, Rect. Y: ' num2str(params.fSizeRectDepth)...
                    ' km, Nmin: ' num2str(params.nMinimumNumber)];
            vResults = params;
            save(['result_Time' num2str(fTstart) '_RectX_' num2str(params.fSizeRectHorizontal) '_RectY_' num2str(params.fSizeRectDepth)...
                    '_Nmin_' num2str(params.nMinimumNumber) '.mat'], 'vResults');
        end
        vResults =[];
        fTstart = fTstart+fTimePeriod;
    end; % End of while fTstart

    % Case of no bootstrapping
else
    % Loop over time
    fTstart = params.fTstart;
    while fTstart < params.fTmaxCat
        mValueGrid_ = [];
        params.mCatalog = mCatalog;
        % Create Indices to catalog and select quakes in time period
        vSel = (fTstart <= params.mCatalog(:,3) & params.mCatalog(:,3) < fTstart+fTimePeriod);
        params.mCatalog = params.mCatalog(vSel,:);
        [params.caNodeIndices] = ex_CreateIndexCatalog(params.mCatalog, params.mPolygon, params.bMap, params.nGriddingMode, ...
         params.nNumberEvents, params.fRadius, params.fSizeRectHorizontal, params.fSizeRectDepth);
        % Loop over all grid nodes
        hWaitbar1 = waitbar(0,'Calculating nodes...');
        set(hWaitbar1,'Numbertitle','off','Name','Node percentage');
        for nNode_ = nNodeStart:nNodeEnd
            % Create node catalog
            mNodeCatalog_ = params.mCatalog(params.caNodeIndices{nNode_}, :);
            % Check for constant number of events calculations
            if (params.nGriddingMode == 0)
                [mNodeCatalog_] = ex_CheckMaxRadius(mNodeCatalog_, params.mPolygon, nNode_, params.caNodeIndices, params.fMaxRadius, params.nNumberEvents, params.bMap);
            end
            [nX,nY] = size(mNodeCatalog_);
            if (nX < params.nMinimumNumber)
                mValueGrid_= [mValueGrid_; NaN NaN NaN NaN NaN NaN nX];
            else
                [rCalcNodeResult_] = sv_NodeCalcMc(params,mNodeCatalog_);
                mValueGrid_= [mValueGrid_; rCalcNodeResult_.fMc_max rCalcNodeResult_.fMc_90 rCalcNodeResult_.fMc_95 rCalcNodeResult_.fMc_com...
                        rCalcNodeResult_.fMc_EMR rCalcNodeResult_.fMc_shi nX];
            end; % End of if on nX
            if rem(nNode_,500) == 0
                waitbar(nNode_/length(params.mPolygon(:,1)))
                %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
                % Temporary saving
                params.vcsGridNames = cellstr(char('Mc max. curvature' , 'Mc 90% goodness of fit' , 'Mc 95% goodness of fit',...
                    'Mc best combination', 'Mc EMR-method', 'Mc(Shi-b-uncertainty)','Number of events'));
                params.mValueGrid = mValueGrid_;
                % Add parameter to params.sComment
                if  params.nGriddingMode == 0;   % Constant number
                    params.sComment = ['Starttime ' num2str(fTstart) ' Spacing ' num2str(params.fSpacingHorizontal) ' deg.,'...
                            ' Time period ' num2str(params.fTimePeriod) ' d, Constant number: ' num2str(params.nNumberEvents) ', MaxRadius: '...
                            num2str(params.fMaxRadius) ' km'];
                    vResults = params;
                    save(['result_Time' num2str(fTstart) '_Constnum_' num2str(params.nNumberEvents) '_MaxRad_' num2str(params.fMaxRadius)...
                            '_Nmin_' num2str(params.nMinimumNumber) '_Node' num2str(nNode_) '.mat'], 'vResults');
                elseif params.nGriddingMode == 1;   % Constant radius
                    params.sComment = ['Starttime ' num2str(fTstart) ' Spacing ' num2str(params.fSpacingHorizontal) ' deg.,'...
                            ' Time period ' num2str(params.fTimePeriod) ' d, Radius: ' num2str(params.fRadius) ' km, Nmin: ' num2str(params.nMinimumNumber)];
                    vResults = params;
                    save(['result_Time' num2str(fTstart) '_Rad_' num2str(params.fRadius) '_Nmin_' num2str(params.nMinimumNumber)  '_Node' num2str(nNode_) '.mat'], 'vResults');
                else  % Rectangle mode
                    params.sComment = ['Starttime ' num2str(fTstart) ' Spacing ' num2str(params.fSpacingHorizontal) ' deg.,'...
                            ' Time period ' num2str(params.fTimePeriod) ' d, Rect. X: ' num2str(params.fSizeRectHorizontal) ' km, Rect. Y: ' num2str(params.fSizeRectDepth)...
                            ' km, Nmin: ' num2str(params.nMinimumNumber)];
                    vResults = params;
                    save(['result_Time' num2str(fTstart) '_RectX_' num2str(params.fSizeRectHorizontal) '_RectY_' num2str(params.fSizeRectDepth)...
                            '_Nmin_' num2str(params.nMinimumNumber)  '_Node' num2str(nNode_) '.mat'], 'vResults');
                end; % END of params.nGriddingmode
                vResults =[];
            end; % End updating waitbar
        end; % for nNode
        close(hWaitbar1);
        % Parameter description
        params.vcsGridNames = cellstr(char('Mc max. curvature' , 'Mc 90% goodness of fit' , 'Mc 95% goodness of fit',...
            'Mc best combination', 'Mc EMR-method', 'Mc(Shi-b-uncertainty)'));
        params.mValueGrid = mValueGrid_;
        % Add parameter to params.sComment
        if  params.nGriddingMode == 0;   % Constant number
            params.sComment = ['Starttime ' num2str(fTstart) ' Spacing ' num2str(params.fSpacingHorizontal) ' deg.,'...
                    ' Time period ' num2str(params.fTimePeriod) ' d, Constant number: ' num2str(params.nNumberEvents) ', MaxRadius: '...
                    num2str(params.fMaxRadius) ' km'];
            vResults = params;
            save(['result_Time' num2str(fTstart) '_Constnum_' num2str(params.nNumberEvents) '_MaxRad_' num2str(params.fMaxRadius)...
                    '_Nmin_' num2str(params.nMinimumNumber) '.mat'], 'vResults');
        elseif params.nGriddingMode == 1;   % Constant radius
            params.sComment = ['Starttime ' num2str(fTstart) ' Spacing ' num2str(params.fSpacingHorizontal) ' deg.,'...
                    ' Time period ' num2str(params.fTimePeriod) ' d, Radius: ' num2str(params.fRadius) ' km, Nmin: ' num2str(params.nMinimumNumber)];
            vResults = params;
            save(['result_Time' num2str(fTstart) '_Rad_' num2str(params.fRadius) '_Nmin_' num2str(params.nMinimumNumber) '.mat'], 'vResults');
        else  % Rectangle mode
            params.sComment = ['Starttime ' num2str(fTstart) ' Spacing ' num2str(params.fSpacingHorizontal) ' deg.,'...
                    ' Time period ' num2str(params.fTimePeriod) ' d, Rect. X: ' num2str(params.fSizeRectHorizontal) ' km, Rect. Y: ' num2str(params.fSizeRectDepth)...
                    ' km, Nmin: ' num2str(params.nMinimumNumber)];
            vResults = params;
            save(['result_Time' num2str(fTstart) '_RectX_' num2str(params.fSizeRectHorizontal) '_RectY_' num2str(params.fSizeRectDepth)...
                    '_Nmin_' num2str(params.nMinimumNumber) '.mat'], 'vResults');
        end
        vResults =[];
        fTstart = fTstart+fTimePeriod;
    end; % End of while fTstart
end; % END of if params.bBst


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  qdd = call_direct_dynamics_fast(input)
%  Auxiliar function for the simulink model SIMULATE_ROBOT_AND_CONTROLLER.
%  Rearranges the inputs coming from the simulink model and calls the
%  function accel.
%  As a result the instantaneous acceleration at each joint is returned.
%
%  See also ACCEL.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Lesser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function qdd = call_direct_dynamics_2dofplanar(input)

global robot

torque = input(1:2);   % Input torque at each joint
q   = input(3:4);	   % Joint positions
qd  = input(5:6);	   % Joint speeds
fe=[0 0 0 0 0 0]'; %external forces applied
torque

% Compute acceleration
qdd = forwarddynamics_2dofplanar(robot, q, qd, torque, 9.81, fe)


function [diff_dose] = dicomrt_dosediff(doseone,dosetwo,method,dose_xmesh,dose_ymesh,dose_zmesh,VOI,voi2use)
% dicomrt_dosediff(doseone,dosetwo,method,dose_xmesh,dose_ymesh,dose_zmesh,VOI,voi2use)
%
% Calculate dose difference between two 3D matrices
%
% doseone and dosetwo can be rtplan and/or monte carlo 3D dose distributions.
% method is an OPTIONAL rameter which specify the way doses are normalised
%
% 1. method=a          matrices are normalised to the value a expressed in Gy
% 2. method=[x, y, z]  matrices are independently normalised to the dose value at point (x,y,z) (in cm)
% 3. method=dmean      matrices are independently normalised to the mean dose value in VOI voi2use (key insensitive)
% 4. method=0          matrices are not normalised (default)
%
% dose_xmesh,dose_ymesh,dose_zmesh are OPTIONAL x-y-z coordinates of the center of the matrix voxels
% VOI is a cell array which contain the patients VOIs (OPTIONAL to use with option 3)
% voi2use is a vector pointing to the number of VOI (OPTIONAL to use with option 3)
%
% Examples:
%
% C=dicomrt_dosediff(A,B,0,dose_xmesh,dose_ymesh,dose_zmesh,VOI,0)
% Store in C the dose difference between B and A. C=(B-A).
%
% C=dicomrt_dosediff(A,B,60,method,dose_xmesh,dose_ymesh,dose_zmesh,VOI,voi2use)
% Store in C the dose difference between Bnorm and Anorm. C=(Bnorm-Anorm), where
% Bnorm and Anorm are normalised to 60Gy (100%).
%
% C=dicomrt_dosediff(A,B,[10.5 -18 7],method,dose_xmesh,dose_ymesh,dose_zmesh)
% Store in C the dose difference between Bnorm and Anorm. C=(Bnorm-Anorm), where
% Bnorm and Anorm are normalised to the respective dose values at dnorm=(10.5 -18 7).
%
% C=dicomrt_dosediff(A,B,'Dmean',method,dose_xmesh,dose_ymesh,dose_zmesh,VOI,3)
% Store in C the dose difference between Bnorm and Anorm. C=(Bnorm-Anorm), where
% Bnorm and Anorm are normalised to the respective mean dose values in VOI number 3
%
% See also: dicomrt_doseratio, dicomrt_loadmcdose
%
% Copyright (C) 2002 Emiliano Spezi (emiliano.spezi@physics.org)

% Check number of argument and set-up some parameters and variables
error(nargchk(2,8,nargin))

% Define parameter
voilookup=1;
filter=0;
dosenorm=0;

% Check case and set-up some parameters and variables
[doseone_dose_temp,type_doseone_dose,labeld1]=dicomrt_checkinput(doseone,1);
[dosetwo_dose_temp,type_dosetwo_dose,labeld1]=dicomrt_checkinput(dosetwo,1);
doseone_dose=dicomrt_varfilter(doseone_dose_temp);
dosetwo_dose=dicomrt_varfilter(dosetwo_dose_temp);

if exist('VOI')==1 & exist('voi2use')==1
    if strcmpi(type_doseone_dose,'mc')==1
        doseone_dose_temp=dicomrt_mask(VOI,doseone_dose_temp,dose_xmesh,dose_ymesh,dose_zmesh,voilookup,filter,'y');
    end
    if strcmpi(type_dosetwo_dose,'mc')==1
        dosetwo_dose_temp=dicomrt_mask(VOI,dosetwo_dose_temp,dose_xmesh,dose_ymesh,dose_zmesh,voilookup,filter,'y');
    end
end

% Create label for dose diff
%if type_doseone_dose=='mc' & type_dosetwo_dose=='rtplan'
%    dose_diff_label='rtplan -mc'
%elseif type_doseone_dose=='mc' & type_dosetwo_dose=='mc'
%    dose_diff_label='mc2 - mc1'
%elseif type_doseone_dose=='rtplan' & type_dosetwo_dose=='mc'
%    dose_diff_label='mc - rtplan'
%else
%    dose_diff_label='rtplan2 - rtplan1'
%end

if exist('method')==1
    if isnumeric(method)==1 & length(method)==1 & method~=0
        % normalize to a specific dose level passed through "method"
        doseone_dose=doseone_dose./method.*100;
        dosetwo_dose=dosetwo_dose./method.*100;
        diff_dose=dosetwo_dose-doseone_dose;
    elseif isnumeric(method)==1 & length(method)==1 & method==0
        % no normalisation is carried out
        diff_dose=dosetwo_dose-doseone_dose;
    elseif isnumeric(method)==1 & length(method)==3 & ...
            (exist('dose_xmesh')~=1 | exist('dose_ymesh')~=1 | exist('dose_zmesh')~=1)
        error('dicomrt_dosediff: This normalisation method requires mesh to be provided. Exit now!');
    elseif isnumeric(method)==1 & length(method)==3 & ...
            (exist('dose_xmesh')==1 | exist('dose_ymesh')==1 | exist('dose_zmesh')==1)
        % normalize to a specific point in 3D passed through "method"
        locx=dicomrt_findpointVECT(dose_xmesh,method(1));
        locy=dicomrt_findpointVECT(dose_ymesh,method(2));
        locz=dicomrt_findpointVECT(dose_zmesh,method(3));
        doseone_dose=doseone_dose./doseone_dose(locy,locx,locz).*100;
        dosetwo_dose=dosetwo_dose./dosetwo_dose(locy,locx,locz).*100;
        diff_dose=dosetwo_dose-doseone_dose;
    elseif ischar(method)==1 & strcmpi(method,'dmean')==1 & ...
            (exist('VOI')~=1 | exist('voi2use')~=1)
        error('dicomrt_dosediff: This normalisation method requires VOI and voi2use to be provided. Exit now!');
    elseif ischar(method)==1 & strcmpi(method,'dmean')==1 & ...
            (exist('VOI')==1 | exist('voi2use')==1)
        % normalize to dmean
        dmean_one=dicomrt_MDcal(doseone,0,dose_xmesh,dose_ymesh,dose_zmesh,VOI,voi2use);
        dmean_two=dicomrt_MDcal(dosetwo,0,dose_xmesh,dose_ymesh,dose_zmesh,VOI,voi2use);
        doseone_dose=doseone_dose./dmean_one.*100;
        dosetwo_dose=dosetwo_dose./dmean_two.*100;
        diff_dose=dosetwo_dose-doseone_dose;
    end
else
    % no normalisation is carried out
    diff_dose=dosetwo_dose-doseone_dose;
end

% Restore original variable format
[diff_dose]=dicomrt_restorevarformat(dosetwo_dose_temp,diff_dose);

% Label Plan and update time of creation
if iscell(diff_dose)==1
    diff_dose{1,1}{1}.RTPlanLabel=[diff_dose{1,1}{1}.RTPlanLabel,'-DDIFF'];
    diff_dose{1,1}{1}.RTPlanDate=date;
    time=fix(clock);
    creationtime=[num2str(time(4)),':',num2str(time(5))];
    diff_dose{1,1}{1}.RTPlanTime=creationtime;
end


tdir = '/biac2/wandell2/data/templates/';

% load the zeroth-iteration template (built from a simple average
% of all the ac-pc aligned brains
%im = loadAnalyze(fullfile(tdir,'SIRL55-0.img'));

baseDir = '/biac2/wandell2/data/reading_longitude/dti/*0*';
[files,subCodes] = findSubjects(baseDir, '_dt6', {'es041113','tk040817'});
N = length(files);

% Load all the talscales
for(ii=[1:N])
  fprintf('Loading scale factors from %s (%d of %d)...\n',subCodes{ii}, ii, N);
  d = load(files{ii},'anat');
  sf(ii) = d.anat.talScale;
end
% To get the mean extent, we average the Talairach scale factors
% and then divide Talairach's actual dimensions (in mm) by these
% scales.
tal = mrAnatGetTalairachDists;
meanExtent.sac = abs(tal.sac)/mean([sf(:).sac]);
meanExtent.iac = abs(tal.iac)/mean([sf(:).iac]);
meanExtent.lac = abs(tal.lac)/mean([sf(:).lac]);
meanExtent.rac = abs(tal.rac)/mean([sf(:).rac]);
meanExtent.aac = abs(tal.aac)/mean([sf(:).aac]);
meanExtent.acpc = abs(tal.acpc)/mean([sf(:).acpc]);
meanExtent.ppc = abs(tal.ppc)/mean([sf(:).ppc]);
for(ii=[1:N])
  extent(ii).sac = abs(tal.sac)/sf(ii).sac;
  extent(ii).iac = abs(tal.iac)/sf(ii).iac;
  extent(ii).lac = abs(tal.lac)/sf(ii).lac;
  extent(ii).rac = abs(tal.rac)/sf(ii).rac;
  extent(ii).aac = abs(tal.aac)/sf(ii).aac;
  extent(ii).acpc = abs(tal.acpc)/sf(ii).acpc;
  extent(ii).ppc = abs(tal.ppc)/sf(ii).ppc;
  newScale(ii).sac = meanExtent.sac/extent(ii).sac;
  newScale(ii).iac = meanExtent.iac/extent(ii).iac;
  newScale(ii).lac = meanExtent.lac/extent(ii).lac;
  newScale(ii).rac = meanExtent.rac/extent(ii).rac;
  newScale(ii).aac = meanExtent.aac/extent(ii).aac;
  newScale(ii).acpc = meanExtent.acpc/extent(ii).acpc;
  newScale(ii).ppc = meanExtent.ppc/extent(ii).ppc;
  newScale(ii).pcReference = -meanExtent.acpc;
end

% Reslice using the talscale measurements.
d = load(files{1},'anat');
ac = round(mrAnatXformCoords(inv(d.anat.xformToAcPc),[0 0 0]));
imSkull = d.anat.img;
im = imSkull; im(~d.anat.brainMask) = 0;
sz = size(im);
%imSkull = mrAnatHistogramClip(double(imSkull), 0.5, 0.99);
proportionNonBrain = sum(~d.anat.brainMask(:))./prod(size(d.anat.brainMask));
im = mrAnatHistogramClip(double(im), proportionNonBrain, 0.99);
bb = [-d.anat.mmPerVox.*(ac-1); d.anat.mmPerVox.*(sz-ac)];
ts = newScale(1); ts.talScaleDir = 'tal2acpc';
% warp T1 to tal space
ts.outMat = inv(d.anat.xformToAcPc);
[im,newXform] = mrAnatResliceSpm(im,ts,bb,[1 1 1],[7 7 7 0 0 0],0);
im(isnan(im)) = 0;
im(im<0) = 0; im(im>1) = 1;
ax = zeros(sz(2), sz(1), N+1);
cr = zeros(sz(3), sz(1), N+1);
sg = zeros(sz(3), sz(2), N+1);
meanIm = im;
ax(:,:,1) = flipud(permute(squeeze(im(:,:,ac(3))),[2,1]));
cr(:,:,1) = flipud(permute(squeeze(im(:,ac(2),:)),[2,1]));
sg(:,:,1) = flipud(permute(squeeze(im(ac(1),:,:)),[2,1]));
for(ii=[2:N])
  fprintf('Processing %s (%d of %d)...\n',subCodes{ii}, ii, N);
  d = load(files{ii},'anat');
  % All coords should be the same.
  ac = round(mrAnatXformCoords(inv(d.anat.xformToAcPc),[0 0 0]));
  im = d.anat.img; im(~d.anat.brainMask) = 0;
  im = mrAnatHistogramClip(double(im), 0.4, 0.99);
  ts = newScale(ii); ts.talScaleDir = 'tal2acpc';
  ts.outMat = inv(d.anat.xformToAcPc);
  [im,newXform] = mrAnatResliceSpm(im,ts,bb,[1 1 1],[7 7 7 0 0 0],0);
  im(isnan(im)) = 0;
  im(im<0) = 0; im(im>1) = 1;
  meanIm = meanIm + im;
  ax(:,:,ii) = flipud(permute(squeeze(im(:,:,ac(3))),[2,1]));
  cr(:,:,ii) = flipud(permute(squeeze(im(:,ac(2),:)),[2,1]));
  sg(:,:,ii) = flipud(permute(squeeze(im(ac(1),:,:)),[2,1]));
end
meanIm = meanIm./N;
figure; image(makeMontage(uint8(meanIm*255)));
colormap(gray(256)); axis equal tight off;
ax(:,:,N+1) = mean(ax(:,:,[1:N]), 3);
cr(:,:,N+1) = mean(cr(:,:,[1:N]), 3);
sg(:,:,N+1) = mean(sg(:,:,[1:N]), 3);
ax = uint8(ax.*255+0.5);
cr = uint8(cr.*255+0.5);
sg = uint8(sg.*255+0.5);
figure;image(makeMontage(ax));colormap(gray(256));axis equal tight off;
figure;image(makeMontage(cr));colormap(gray(256));axis equal tight off;
figure;image(makeMontage(sg));colormap(gray(256));axis equal tight off;

tdir = '/snarp/u1/data/templates/';
imwrite(makeMontage(ax), gray(256), fullfile(tdir, 'SIRL55scaledToMean_ax.png'));
imwrite(makeMontage(cr), gray(256), fullfile(tdir, 'SIRL55scaledToMean_cr.png'));
imwrite(makeMontage(sg), gray(256), fullfile(tdir, 'SIRL55scaledToMean_sg.png'));

% Now build an atlas from the mean image
V.dat = int16(meanIm.*(2^15-1)+0.5);
notes = ['SIRL55ms: average of 55 mean-Tal-scaled brains. Created at ' datestr(now,31)];
mmPerVox = d.anat.mmPerVox;
origin = ac;
hdr = saveAnalyze(V.dat, fullfile(tdir,'SIRL55ms'), mmPerVox, notes, origin);
save(fullfile(tdir,'SIRL55ms_details.mat'), 'meanIm','notes','extent','meanExtent','origin','mmPerVox');


function [coord_norm] = warp_fsaverage_sym(cfg, elec)

% WARP_FSAVERAGE_SYM maps left or right hemisphere electrodes onto
% FreeSurfer's fsaverage_sym's left hemisphere. To perform this mapping,
% you first need to have processed the subject's MRI with FreeSurfer's
% recon-all functionality and additionaly have registered the subject's resulting
% surfaces to freesurfer fsaverage_sym template using surfreg as described
% in section 1.2 of https://surfer.nmr.mgh.harvard.edu/fswiki/Xhemi
%
% The configuration must contain the following options
%   cfg.headshape      = string, filename containing subject headshape
%                      (e.g. <path to freesurfer/surf/lh.pial>)
%   cfg.fshome         = string, path to freesurfer
%
% See also FT_ELECTRODEREALIGN, WARP_FSAVERAGE

% Copyright (C) 2019, Arjen Stolk
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

subj_pial = ft_read_headshape(cfg.headshape);
[PATHSTR, NAME] = fileparts(cfg.headshape); % lh or rh
if strcmp(NAME, 'lh')
  subj_reg = ft_read_headshape([PATHSTR filesep 'lh.fsaverage_sym.sphere.reg']);
elseif strcmp(NAME, 'rh')
  subj_reg = ft_read_headshape([PATHSTR(1:strfind(PATHSTR, [filesep 'surf'])-1) filesep 'xhemi' filesep 'surf' filesep 'lh.fsaverage_sym.sphere.reg']);
end
if ~isfolder([cfg.fshome filesep 'subjects' filesep 'fsaverage_sym']) || ~isfolder([PATHSTR(1:strfind(PATHSTR, [filesep 'surf'])-1) filesep 'xhemi'])
  ft_error(['fsaverage_sym and/or xhemi folders cannot be found'])
end
fsavg_pial = ft_read_headshape([cfg.fshome filesep 'subjects' filesep 'fsaverage_sym' filesep 'surf' filesep 'lh.pial']);
fsavg_reg = ft_read_headshape([cfg.fshome filesep 'subjects' filesep 'fsaverage_sym' filesep 'surf' filesep 'lh.sphere.reg']); % always map onto the left hemi

for e = 1:numel(elec.label)
  % subject space (3D surface): electrode pos -> vertex index
  dist = sqrt(sum(((subj_pial.pos - repmat(elec.elecpos(e,:), size(subj_pial.pos,1), 1)).^2),2));
  [dum, minidx] = min(dist);

  % intersubject space (3D sphere): vertex index -> vertex pos -> template vertex index
  dist2 = sqrt(sum(((fsavg_reg.pos - repmat(subj_reg.pos(minidx,:), size(fsavg_reg.pos,1), 1)).^2),2));
  [dum, minidx2] = min(dist2);
  clear minidx

  % template space (3D surface): template vertex index -> template electrode pos
  coord_norm(e,:) = fsavg_pial.pos(minidx2,:);
  clear minidx2
end
clear subj_pial subj_reg fsavg_pial fsavg_reg


function [DCM,BMR,BMA] = spm_dcm_bmr_all(DCM,field,OPT)
% Bayesian model reduction of all permutations of model parameters
% FORMAT [RCM,BMR,BMA] = spm_dcm_bmr_all(DCM,field,OPT)
%
% DCM      - A single estimated DCM (or PEB) structure:
%
%  DCM.M.pE  - prior expectation
%  DCM.M.pC  - prior covariance
%  DCM.Ep    - posterior expectation
%  DCM.Cp    - posterior covariances
%  DCM.beta  - prior expectation of reduced parameters (default: 0)
%  DCM.gamma - prior variance    of reduced parameters (default: 0)
%              NB: beta = 'pE' uses full priors
%
% field      - parameter fields in DCM{i}.Ep to optimise [default: {'A','B'}]
%             'All' will invoke all fields (i.e. random effects)
%             If Ep is not a structure, all parameters will be considered
%
% OPT        - Bayesian model selection or averaging: 'BMS' or 'BMA'
%              [default: 'BMA']
%
% Returns:
%
% DCM - Bayesian Model Average (BMA) over models in the final iteration of
%       the search:
%
%       DCM.Ep    - (BMA) posterior expectation
%       DCM.Cp    - (BMA) posterior covariance
%       DCM.Pp    - Model posterior over parameters (with and without)
%
% BMR -  (Nsub) summary structure reporting the model space from the last
%        iteration of the search:
%
%        BMR.name - character/cell array of parameter names
%        BMR.F    - free energies (relative to full model)
%        BMR.P    - and posterior (model) probabilities
%        BMR.K    - [models x parameters] model space (1 = off, 0 = on)
%
% BMA - Baysian model average (over reduced models; see spm_dcm_bma)
%
%--------------------------------------------------------------------------
% This routine searches over reduced (nested) models of a full model (DCM)
% using Bayesian model reduction and performs Bayesian Model Averaging.
% 'Reduced' means some free parameters (parameters with a non-
% zero prior covariance) are switched off by fixing their prior variance
% to zero.
%
% If there are fewer than nmax = 8 free parameters, all permutations of
% switching off parameters will be tested. Otherwise, this routine
% implements the following greedy search procedure. The nmax parameters
% are identified which, when switched off individually, produce the least
% reduction (greatest increase) in model evidence. All permutations of
% switching off these parameters are then evaluated and the best
% permutation is retained. This procedure is repeated until all nmax
% parameters are retained or there are no more parameters to consider.
% Finally, BMA is performed on the models from the last iteration.
%
% NB: The full model should be estimated prior to running this function.
%
% See also: spm_dcm_post_hoc - this routine is essentially a simplified
% version of spm_dcm_post_hoc
%__________________________________________________________________________
% Copyright (C) 2010-2014 Wellcome Trust Centre for Neuroimaging

% Karl Friston, Peter Zeidman
% $Id: spm_dcm_bmr_all.m 7717 2019-11-27 11:10:36Z peter $

%-specification of null prior covariance
%--------------------------------------------------------------------------
if isfield(DCM,'beta'),  beta  = DCM.beta;  else, beta  = 0; end
if isfield(DCM,'gamma'), gamma = DCM.gamma; else, gamma = 0; end

%-Check fields of parameter stucture (and options)
%--------------------------------------------------------------------------
if nargin < 3
    OPT   = 'BMA';
end
if nargin < 2 || isempty(field)
    field = {'A','B'};
end
if ischar(field)
    field = {field};
end

%-deal with filenames structure
%--------------------------------------------------------------------------
if ischar(DCM)
    DCM = load(DCM,'DCM');
    DCM = DCM.DCM;
end

% Get prior covariances
%--------------------------------------------------------------------------
if isstruct(DCM.M.pC), DCM.M.pC = diag(spm_vec(DCM.M.pC)); end
if spm_length(DCM.M.pE) ~= size(DCM.M.pC,1)
    DCM.M.pC = diag(spm_vec(DCM.M.pC));
end

% Get priors and posteriors
%--------------------------------------------------------------------------
qE  = DCM.Ep;
qC  = DCM.Cp;
pE  = DCM.M.pE;
pC  = DCM.M.pC;

% Remove (a priori) null space
%--------------------------------------------------------------------------
U   = spm_svd(pC);
qE  = U'*spm_vec(qE);
pE  = U'*spm_vec(pE);
qC  = U'*qC*U;
pC  = U'*pC*U;

%-Greedy search (GS) - eliminating parameters in a top down fashion
%==========================================================================

% Accumulated reduction vector (C)
%--------------------------------------------------------------------------
q   = diag(DCM.M.pC);
if sum(q < 1024)
    C   = double(q > mean(q(q < 1024))/1024);
else
    C   = double(q > 0);
end
GS  = 1;
while GS

    %-Find free coupling parameters
    %----------------------------------------------------------------------
    if isstruct(DCM.Ep)
        k = spm_fieldindices(DCM.Ep,field{:});
    else
        k = 1:spm_length(DCM.Ep);
    end
    k = k(find(C(k))); %#ok<FNDSB>

    % If there are too many parameters find those with the least evidence
    %----------------------------------------------------------------------
    nparam = length(k);
    nmax   = fix(max(nparam/4,8));
    if nparam > nmax

        % Model search over new prior without the i-th parameter
        %------------------------------------------------------------------
        Z     = zeros(1,nparam);
        for i = 1:nparam

            % Identify parameters to retain r and to remove s
            %--------------------------------------------------------------
            r   = C; r(k(i)) = 0; s = 1 - r;

            % Create reduced prior covariance matrix
            %--------------------------------------------------------------
            R   = U'*diag(r + s*gamma)*U;
            rC  = R*pC*R;

            % Create reduced prior means
            %--------------------------------------------------------------
            if isnumeric(beta)
                S  = U'*diag(r)*U;
                rE = S*pE + U'*s*beta;
            else
                rE = pE;
            end

            Z(i) = spm_log_evidence(qE,qC,pE,pC,rE,rC);
        end

        % Find parameters with the least evidence
        %------------------------------------------------------------------
        [z,i] = sort(-Z);
        k     = k(i(1:nmax));

        % Flag a greedy search
        %------------------------------------------------------------------
        GS = 1;

    elseif isempty(k)
        fprintf('\nThere are no free parameters in this model.\n')
        return
    else
        GS = 0;
    end

    % compare models
    %======================================================================
    for j = 1:2

        if j == 1
            % compare models with and without nmax parameters first
            %--------------------------------------------------------------
            K = repmat(logical([1;0]),1,numel(k));
        else
            % compare all combinations
            %--------------------------------------------------------------
            k = k(1:min(8,end));
            K = spm_perm_mtx(numel(k));
        end

        % Model search over new prior (covariance)
        %------------------------------------------------------------------
        nK    = size(K,1);
        G     = zeros(1,nK);
        for i = 1:nK

            % Identify parameters to retain (r) and to remove (s)
            %--------------------------------------------------------------
            r    = C; r(k(K(i,:))) = 0; s = 1 - r;

            % Create reduced prior covariance matrix
            %--------------------------------------------------------------
            R    = U'*diag(r + s*gamma)*U;
            rC   = R*pC*R;

            % Create reduced prior means
            %--------------------------------------------------------------
            if isnumeric(beta)
                S  = U'*diag(r)*U;
                rE = S*pE + U'*s*beta;
            else
                rE = pE;
            end

            G(i) = spm_log_evidence(qE,qC,pE,pC,rE,rC);
        end

        % if sufficient complexity reduction then omit combinations
        %------------------------------------------------------------------
        if G(1) - G(end) > nmax && nparam > nmax
            break;
        else
            nmax = 8;
        end
    end

    % posterior probability
    %----------------------------------------------------------------------
    p            = spm_softmax(G(:));

    %-Get selected model and prune redundant parameters
    %======================================================================
    [z,i]        = max(p);
    C(k(K(i,:))) = 0;

    % Continue greedy search if any parameters have been eliminated
    %----------------------------------------------------------------------
    nelim  = full(sum(K(i,:)));
    GS     = GS & nelim;

    % Show results
    % ---------------------------------------------------------------------
    fprintf('%i out of %i free parameters removed \n',nelim,nparam)

    if nmax <= 8
        spm_figure('Getwin','BMR - all'); clf
        subplot(3,2,1)
        if numel(G) > 32, plot(G,'k'), else, bar(G,'c'), end
        title('log-posterior','FontSize',16)
        xlabel('model','FontSize',12)
        ylabel('log-probability','FontSize',12)
        axis square

        subplot(3,2,2)
        if numel(G) > 32, plot(p,'k'), else, bar(p,'r'), end
        title('model posterior','FontSize',16)
        xlabel('model','FontSize',12)
        ylabel('probability','FontSize',12)
        axis square
        drawnow
    end

end

%-Inference over families (one family per coupling parameter)
%==========================================================================
for i = 1:length(k)
    Pk(1,i) = mean(p(~K(:,i)));
    Pk(2,i) = mean(p( K(:,i)));
end
Pk    = Pk(1,:)./sum(Pk);
Pp    = C;
Pp(k) = Pk;

%-Bayesian model selection or average
%==========================================================================
qE    = DCM.Ep;
qC    = DCM.Cp;
pE    = DCM.M.pE;
pC    = DCM.M.pC;
pE    = spm_vec(pE);

switch OPT

    case('BMA')
        % Bayesian model averaging
        %------------------------------------------------------------------
        Gmax     = max(G);

    case('BMS')
        % Bayesian model selection (place winning G outside Occam's window
        %------------------------------------------------------------------
        [Gmax,i] = max(G);
        G(i)     = G(i) + 16;
        Gmax     = Gmax + 16;

    otherwise
end

BMA   = {};
for i = 1:length(K)

    % if this mdel is in Occam's window, inlcude in BMA
    %----------------------------------------------------------------------
    if G(i) > (Gmax - 8)

        % reduced model
        %------------------------------------------------------------------
        r            = C;
        r(k(K(i,:))) = 0;
        s            = 1 - r;
        R            = diag(r + s*gamma);
        rC           = R*pC*R;
        S            = diag(r);
        if isnumeric(beta)
            rE       = S*spm_vec(pE) + s*beta;
        else
            rE       = pE;
        end

        % BMR
        %------------------------------------------------------------------
        [F,Ep,Cp]    = spm_log_evidence_reduce(qE,qC,pE,pC,rE,rC);
        BMA{end + 1} = struct('Ep',Ep,'Cp',Cp,'F',F);
    end
end

switch OPT
    case('BMA')

        % Bayesian model averaging
        %------------------------------------------------------------------
        BMA   = spm_dcm_bma(BMA);
        Ep    = BMA.Ep;
        Cp    = BMA.Cp;

    case('BMS')

        % Bayesian model selection
        %------------------------------------------------------------------
        BMA   = BMA{1};
        Ep    = BMA.Ep;
        Cp    = BMA.Cp;

    otherwise
end

if isstruct(Cp) || (spm_length(Cp) == spm_length(Ep))
    Cp = diag(spm_vec(Cp));
end

% Show full and reduced conditional estimates (for Bayesian average)
%--------------------------------------------------------------------------
spm_figure('Getwin','BMR - all');

if isstruct(DCM.Ep)
    i  = spm_find_pC(pC,DCM.Ep,field);
else
    i  = 1:spm_length(DCM.Ep);
end
qE     = spm_vec(qE);
Ep     = spm_vec(Ep);

j = i(ismember(i,1:length(spm_vec(Ep))));

% BMR summary and plotting
%--------------------------------------------------------------------------
try
    Pnames     = spm_fieldindices(DCM.Ep,k);
catch
    try
        Np     = numel(DCM.Pnames);
        Pnames = DCM.Pnames(rem(k - 1,Np) + 1);
    catch
        Pnames = 'all parameters';
    end
end
BMR.name = Pnames;
BMR.F    = G;
BMR.P    = p;
BMR.K    = K;
BMR.k    = k;

subplot(3,2,3), spm_plot_ci(qE(i),qC(i,i))
title('MAP (full)','FontSize',16)
axis square, a = axis;

subplot(3,2,4), spm_plot_ci(Ep(j),abs(Cp(j,j)))
title('MAP (reduced)','FontSize',16), axis square, axis(a)

subplot(3,2,5), imagesc(1 - K')
xlabel('model'), ylabel('parameter'), title('model space','FontSize',16)
set(gca,'YTickLabel',BMR.name);
axis tight, axis square

subplot(3,2,6)
Np = length(i);
bar(1:Np,diag(Pp(i)))
xlabel('parameter'), title(' posterior','FontSize',16)
axis square, drawnow, axis([0 (Np + 1) 0 1])

%-Save Bayesian parameter average (Ep,Cp) and family-wise inference (Pp)
%==========================================================================
if isstruct(DCM.Ep)
    if Np < 32
        legend(spm_fieldindices(DCM.Ep,i))
    end
    Pp    = spm_unvec(Pp,DCM.Ep);
    Ep    = spm_unvec(Ep,DCM.Ep);
end

DCM.Pp    = Pp;        % Model posterior over parameters (with and without)
DCM.Ep    = Ep;        % Bayesian model averages
DCM.Cp    = Cp;        % Bayesian model variance

% Clear free energy if supplied (which is no longer meaningful)
if isfield(DCM,'F')
    DCM = rmfield(DCM,'F');
end


function [adfreq, n, ts, fn, d] = nex_cont(filename, varname)
% nex_cont(filename, varname): Read continuous variable from a .nex file
%
% [adfreq, n, ts, fn, d] = nex_cont(filename, varname)
%
% INPUT:
%   filename - if empty string, will use File Open dialog
%   varname - variable name
%
%           continuous (a/d) data come in fragments. Each fragment has a timestamp
%           and a number of a/d data points. The timestamp corresponds to
%           the time of recording of the first a/d value in this fragment.
%           All the data values stored in the vector d.
% OUTPUT:
%   n - total number of data points
%   ts - array of fragment timestamps (one timestamp for fragment, in seconds)
%   fn - number of data points in each fragment
%   d - array of a/d values (in millivolts)

% original from Plexon, download from http://www.plexoninc.com (8/4/02)
% modifications by Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

n = 0;
adfreq = 0;
ts = 0;
fn = 0;
d = 0;

if(nargin ~= 2)
   disp('2 input arguments are required')
   return
end

if(ischar(filename) == 0)
   disp('input arguments should be character arrays')
   return
end

if(ischar(varname) == 0)
   disp('input arguments should be character arrays')
   return
end

if(length(filename) == 0)
   [fname, pathname] = uigetfile('*.nex', 'Select a Nex file');
    filename = strcat(pathname, fname);
end

fid = fopen(filename, 'r', 'ieee-le');
if(fid == 0)
   return
end

disp(strcat('file = ', filename));
magic = fread(fid, 1, 'int32');
version = fread(fid, 1, 'int32');
comment = fread(fid, 256, 'char');
freq = fread(fid, 1, 'double');
tbeg = fread(fid, 1, 'int32');
tend = fread(fid, 1, 'int32');
nvar = fread(fid, 1, 'int32');
fseek(fid, 260, 'cof');
name = zeros(1, 64);
found = 0;
for i=1:nvar
    type = fread(fid, 1, 'int32');
    var_version = fread(fid, 1, 'int32');
    name = fread(fid, [1 64], 'char');
    offset = fread(fid, 1, 'int32');
    nf = fread(fid, 1, 'int32');
    dummy = fread(fid, 32, 'char');
    adfreq = fread(fid, 1, 'double');
    adtomv = fread(fid, 1, 'double');
    n = fread(fid, 1, 'int32');
    name = char(name);
    name = deblank(name);
    k = strcmp(name, deblank(varname));
    if(k == 1)
        if type ~= 5
            disp(sprintf('%s is not a continuous variable', deblank(varname)));
            return;
        end
        found = 1;
        fseek(fid, offset, 'bof');
        ts = fread(fid, [1 nf], 'int32');
        fn = fread(fid, [1 nf], 'int32');
        d = fread(fid, [1 n], 'int16');
        break
    end
    dummy = fread(fid, 76, 'char');
end

fclose(fid);

if found == 0
    disp('did not find variable in the file');
else
    ts = ts/freq;
    d = d*adtomv;
    fn(nf+1) = n;
    fn = diff(fn);
    disp(strcat('number of data points = ', num2str(n)));
end


function  [ output_image, meta ]= read_gff( filename, varargin )
%READ_GFF Sandia GSAT Image File Format
%   complexdata = read_gff( filename, rowrange, colrange, subsample, decimationfun )
%
%   INPUTS:
%      FILENAME:      Input file to read.
%      ROWRANGE:      1x2 array [first row, last row], row #1 is top.
%         Default is entire image range.  Can also use an empty array ([])
%         to specify entire row range.
%      COLRANGE:      1x2 array [first column, last column], column #1 is
%         left side.   Default is entire image range.  Can also use an
%         empty array ([]) to specify entire column range.
%      SUBSAMPLE:     1x2 array [subsample rate in first dimension,
%         subsample ratein second dimension].  Default is [1 1] (no
%         subsampling).
%      DECIMATIONFUN: Decimation function, used if SUBSAMPLE is defined to
%         be greater than 1.  Options include 'none' (default)', 'max',
%         'mean', or any other function defined in Matlab.
%
%   OUTPUTS:
%      COMPLEXDATA:   Array of complex data values of data type single
%         complex.
%      META:          Structure containing metadata
%
%   Author: Wade Schwartzkopf (NGA/IDT)
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

% Read metadata
native_meta = read_gff_meta(filename);
meta=meta2sicd_gff(native_meta);
meta.native.gff=native_meta;
if native_meta.RowMajor
   datasize = [native_meta.RgCnt native_meta.AzCnt];
else
   datasize = [native_meta.AzCnt native_meta.RgCnt];
end
switch native_meta.ImageType
    case 0
        datatype='uchar';
    case 1
        if native_meta.BytesPerPixel == 4 % 2 bytes phase, 2 bytes magnitude
            datatype='uint16';
        else % 8 bytes total, 4 bytes phase, 2 bytes magnitude
            datatype='uint32';
        end
    case 2
        datatype='float32';
end
if native_meta.Endian
    endian='b';
else
    endian='l';
end

% Parse input parameters; reverse indices since image 180 degrees rotated
% from shadows down orientations
newvarargin=varargin;
if nargin>1
    rowrange=datasize(1)-varargin{1}(end:-1:1)+1;
    if native_meta.RowMajor
        newvarargin{2}=rowrange;
        newvarargin{1}=[];
    else
        newvarargin{1}=rowrange;
    end
end
if nargin>2
    colrange=datasize(2)-varargin{2}(end:-1:1)+1;
    if native_meta.RowMajor
        newvarargin{1}=colrange;
    else
        newvarargin{2}=colrange;
    end
end
if nargin>3
    if native_meta.RowMajor
        newvarargin{3}=varargin{3}(end:-1:1);
    end
end

% Read complex data
fid = fopen(filename,'r',endian);
output_image = read_complex(fid,datasize,native_meta.Length,...
    datatype,logical(native_meta.ImageType),newvarargin{:});
fclose(fid);

if native_meta.ImageType==1 % Int types are phase-magnitude, rather than real-imag, which read_complex expects
    output_image=double(imag(output_image)).*exp(1i * double(real(output_image)) * 2*pi/(2^16));
end
if native_meta.RowMajor
    output_image=output_image.';
end
output_image=rot90(output_image,2); % GFF stored shadows up; switch to shadows down

end

% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////


function diffS = comparePyradWithIBSI1OrigImgWithInterp
% Compare pyradiomics features against IBSI benchmark for config C.
%--------------------------------------------------------------------------
% AI 7/1/2020

%% Calc. features for configuration 'C' using Pyradiomics
fpath = fullfile(fileparts(fileparts(getCERRPath)),...
    'Unit_Testing/data_for_cerr_tests/IBSI1_CT_phantom/IBSILungCancerCTImage.mat.bz2');
planC = loadPlanC(fpath,tempdir);
planC = updatePlanFields(planC);
planC = quality_assure_planC(fpath,planC);
strName='GTV-1';

configPath_avg = fullfile(fileparts(fileparts(getCERRPath)),...
    'Unit_Testing/settings_for_comparisons/pyRadConfigC_avg.yaml');
pyFeat1S = calcRadiomicsFeatUsingPyradiomics(planC,strName,configPath_avg);
configPath_merge = fullfile(fileparts(fileparts(getCERRPath)),...
    'Unit_Testing/settings_for_comparisons/pyRadConfigC_merge.yaml');
pyFeat2S = calcRadiomicsFeatUsingPyradiomics(planC,strName,configPath_merge);

%% Compare with IBSI

% Get IBSI bechmark
ibsiConfigCResult = fullfile(fileparts(fileparts(getCERRPath)),...
    'Unit_Testing/data_for_cerr_tests/IBSI1_CT_phantom/IBSI_results_configC.mat');
temp = load(ibsiConfigCResult);
IBSIfeatS = temp.IBSIfeatS;

% Shape features
pyShapeS = getPyradFeatDict(pyFeat1S,{'original_shape'});
pyShapeS = mapPyradFieldnames(pyShapeS,'original','shape');
shapeFeatC = fieldnames(IBSIfeatS.shapeS);
for n = 1:length(shapeFeatC)
    ibsiVal = IBSIfeatS.shapeS.(shapeFeatC{n});
    if isfield(pyShapeS,shapeFeatC{n})
        pyRadVal = pyShapeS.(shapeFeatC{n});
        pctDiff = (pyRadVal-ibsiVal)*100/ibsiVal;
        diffS.Shape.(shapeFeatC{n}) = pctDiff;
    end
end

% First order features
pyFirstOrdS = getPyradFeatDict(pyFeat1S,{'original_firstorder'});
pyFirstOrdS = mapPyradFieldnames(pyFirstOrdS,'original','firstorder');
%Convert kurtosis to excess kurtosis
pyFirstOrdS.kurtosis = pyFirstOrdS.kurtosis -3;
firstOrdFeatC = fieldnames(IBSIfeatS.Original.firstOrderS);
for n = 1:length(firstOrdFeatC)
    ibsiVal = IBSIfeatS.Original.firstOrderS.(firstOrdFeatC{n});
    if isfield(pyFirstOrdS,firstOrdFeatC{n})
        pyRadVal = pyFirstOrdS.(firstOrdFeatC{n});
        pctDiff = (pyRadVal-ibsiVal)*100/ibsiVal;
        diffS.FirstOrder.(firstOrdFeatC{n}) = pctDiff;
    end
end

% GLCM
%Avg
pyGlcmS = getPyradFeatDict(pyFeat1S,{'original_glcm'});
pyGlcmS = mapPyradFieldnames(pyGlcmS,'original','glcm');
glcmFeatC = fieldnames(IBSIfeatS.Original.glcmFeatS.AvgS);
for n = 1:length(glcmFeatC)
    ibsiVal = IBSIfeatS.Original.glcmFeatS.AvgS.(glcmFeatC{n});
    if isfield(pyGlcmS,glcmFeatC{n})
        pyRadVal = pyGlcmS.(glcmFeatC{n});
        pctDiff = (pyRadVal-ibsiVal)*100/ibsiVal;
        diffS.GLCM.Avg.(glcmFeatC{n}) = pctDiff;
    end
end
%Merge
pyGlcm2S = getPyradFeatDict(pyFeat2S,{'original_glcm'});
pyGlcm2S = mapPyradFieldnames(pyGlcm2S,'original','glcm');
glcmFeat2C = fieldnames(IBSIfeatS.Original.glcmFeatS.CombS);
for n = 1:length(glcmFeat2C)
    ibsiVal = IBSIfeatS.Original.glcmFeatS.CombS.(glcmFeat2C{n});
    if isfield(pyGlcm2S,glcmFeat2C{n})
        pyRadVal = pyGlcm2S.(glcmFeat2C{n});
        pctDiff = (pyRadVal-ibsiVal)*100/ibsiVal;
        diffS.GLCM.Merge.(glcmFeat2C{n}) = pctDiff;
    end
end

% GLRLM
%Avg
pyGlrlmS = getPyradFeatDict(pyFeat1S,{'original_glrlm'});
pyGlrlmS = mapPyradFieldnames(pyGlrlmS,'original','glrlm');
glrlmFeatC = fieldnames(IBSIfeatS.Original.rlmFeatS.AvgS);
for n = 1:length(glrlmFeatC)
    ibsiVal = IBSIfeatS.Original.rlmFeatS.AvgS.(glrlmFeatC{n});
    if isfield(pyGlrlmS,glrlmFeatC{n})
        pyRadVal = pyGlrlmS.(glrlmFeatC{n});
        pctDiff = (pyRadVal-ibsiVal)*100/ibsiVal;
        diffS.GLRLM.Avg.(glrlmFeatC{n}) = pctDiff;
    end
end
%Merge
pyGlrlm2S = getPyradFeatDict(pyFeat2S,{'original_glrlm'});
pyGlrlm2S = mapPyradFieldnames(pyGlrlm2S,'original','glrlm');
glrlm2FeatC = fieldnames(IBSIfeatS.Original.rlmFeatS.CombS);
for n = 1:length(glrlm2FeatC)
    ibsiVal = IBSIfeatS.Original.rlmFeatS.CombS.(glrlm2FeatC{n});
    if isfield(pyGlrlm2S,glrlm2FeatC{n})
        pyRadVal = pyGlrlm2S.(glrlm2FeatC{n});
        pctDiff = (pyRadVal-ibsiVal)*100/ibsiVal;
        diffS.GLRLM.Merge.(glrlm2FeatC{n}) = pctDiff;
    end
end

% NGTDM
pyNgtdmS = getPyradFeatDict(pyFeat1S,{'original_ngtdm'});
pyNgtdmS = mapPyradFieldnames(pyNgtdmS,'original','ngtdm');
ngtdmFeatC = fieldnames(IBSIfeatS.Original.ngtdmFeatS);
for n = 1:length(ngtdmFeatC)
    ibsiVal = IBSIfeatS.Original.ngtdmFeatS.(ngtdmFeatC{n});
    if isfield(pyNgtdmS,ngtdmFeatC{n})
        pyRadVal = pyNgtdmS.(ngtdmFeatC{n});
        pctDiff = (pyRadVal-ibsiVal)*100/ibsiVal;
        diffS.NGTDM.(ngtdmFeatC{n}) = pctDiff;
    end
end

% SZM features
pyGlszmS = getPyradFeatDict(pyFeat1S,{'original_glszm'});
pyGlszmS = mapPyradFieldnames(pyGlszmS,'original','glszm');
glszmFeatC = fieldnames(IBSIfeatS.Original.szmFeatS);
for n = 1:length(glszmFeatC)
    ibsiVal = IBSIfeatS.Original.szmFeatS.(glszmFeatC{n});
    if isfield(pyGlszmS,glszmFeatC{n})
        pyRadVal = pyGlszmS.(glszmFeatC{n});
        pctDiff = (pyRadVal-ibsiVal)*100/ibsiVal;
        diffS.GLSZM.(glszmFeatC{n}) = pctDiff;
    end
end

end


%     NeuroSLAM System Copyright (C) 2018-2019
%     NeuroSLAM: A Brain inspired SLAM System for 3D Environments
%
%     Fangwen Yu (www.yufangwen.com), Jianga Shang, Youjian Hu, Michael Milford(www.michaelmilford.com)
%
%     The NeuroSLAM V1.0 (MATLAB) was developed based on the OpenRatSLAM (David et al. 2013).
%     The RatSLAM V0.3 (MATLAB) developed by David Ball, Michael Milford and Gordon Wyeth in 2008.
%
%     Reference:
%     Ball, David, Scott Heath, Janet Wiles, Gordon Wyeth, Peter Corke, and Michael Milford.
%     "OpenRatSLAM: an open source brain-based SLAM system." Autonomous Robots 34, no. 3 (2013): 149-176.
%
%     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/>.

% Feb 6, 2019
groundTruthFile = 'C:\NeuroSLAM_Datasets\02_NeuroSLAM_Groudtruth\01_SynPerData_GT.txt';
expMapFile = 'C:\NeuroSLAM_Datasets\03_NeuroSLAM_Experiments_Results\SynPerData\01_exp_map_ml.txt';
% plot_3d_multilayer_experience_map(groundTruthFile, expMapFile, xExpMapScaling, yExpMapScaling, zExpMapScaling, xExpMapTrans, yExpMapTrans, zExpMapTrans, xGtScaling, yGtScaling, zGtScaling)
plot_3d_multilayer_experience_map(groundTruthFile, expMapFile, 0.1, 0.1, 0.1, 1.2, 1.3, 0, 21.5,23, 23);


function data=createdatamatpt(data,E,win)
% Helper function to create an event triggered matrix from a single
% channel of spike times.
% Usage:  data=createdatamatpt(data,E,win)
% Inputs:
% data   (input spike times as a structural array or as a column vector) - required
% E      (events to use as triggers) - required
% win    (window around triggers to use data matrix -[winl winr]) - required
%          e.g [1 1] uses a window starting 1 sec before E and
%              ending 1 sec after E if E and data are in secs.
% Note that E, win and data must have consistent units
% Outputs:
% data      (event triggered data as a structural array - times are stored
% relative to the E-winl
%
if nargin < 3; error('Need all input arguments'); end;
if isstruct(data);
   fnames=fieldnames(data);
   eval(['dtmp=data.' fnames{1} ';'])
else
   dtmp=data(:);
end;
NE=length(E);
winl=win(1);
winr=win(2);
data2(1:NE)=struct('times',[]);
for n=1:NE,
    indx=find(dtmp > E(n)-winl & dtmp<= E(n)+winr);
    if ~isempty(indx)
       data2(n).times=dtmp(indx)-E(n)+winl;
    else
       data2(n).times=[];
    end
end
data=data2;


%% --------------------------
% MemNet_M6R6 for image denoising (gaussian noise)
% edit by yingtai 12/08/2017
% -------------------------------
function test_MemNet_M6R6_GD()
setenv('LC_ALL','C')
addpath /data2/taiying/MSU_Code/119-caffe-matlab/matlab; % change to your caffe path
setenv('GLOG_minloglevel','2')
addpath('../');
addpath('../evaluation_func/');
addpath('../evaluation_func/matlabPyrTools-master/');

%% parameters
gpu_id = 5;
Noise_level = 30;
data_set_id = 1;
thresh_hei = 120; % threshold patch size for inference, since too big image may cost too much memory
thresh_wid = 120;
rf = 16;

pathfolder = ['../../data/GaussianDenoising/'];
if data_set_id == 1
    % S14
    setTestCur = 'S14';
    path = [pathfolder setTestCur '/'];
    d = dir([path '*.bmp']);
    filenum = 14;
end
if data_set_id == 2
    % BSD200
    setTestCur = 'BSD200';
    path = [pathfolder setTestCur '/'];
    d = dir([path '*.jpg']);
    filenum = 200;
end

savepath = ['./results/'];
folderResultCur = fullfile(savepath, [setTestCur,'_Noise',num2str(Noise_level)]);
%%% folder to store results
if ~exist(folderResultCur,'file')
    mkdir(folderResultCur);
end

mean_noised = [];
mean_memnet = [];
% caffe.set_mode_cpu(); % for CPU
caffe.set_mode_gpu(); % for GPU
caffe.set_device(gpu_id);

weights = ['../../model/MemNet_M6R6_80C64_GD.caffemodel'];
model_path = './MemNet_M6R6_80C64_deploy';

noise_set =[];
memnet_set = [];
im_b_set = cell(filenum,1);
im_h_set = cell(filenum,1);
im_gnd_set = cell(filenum,1);

for iii = 1:1:length(d)
    disp(['id: ' num2str(iii)]);
    imageName = d(iii).name;
    imageName = imageName(1:end-4);
    im  = imread([path d(iii).name]);
    randn('seed',54);

    %% test: resize
    if data_set_id == 3
        im = imresize(im,1/2);
    end

    %% rgb -> gray
    im_gray = im;
    if size(im,3)>1
        im_gray = rgb2gray(im);
    end
    im_gnd = im2double(im_gray);

    [hei,wid,channels] = size(im_gnd);
    % add noise
    im_b = single(im_gnd + Noise_level/255*randn(size(im_gray)));

    %% adaptively spilt
    % decide patch numbers
    hei_patch = ceil(hei/(thresh_hei+rf));
    wid_patch = ceil(wid/(thresh_wid+rf));
    hei_stride = ceil(hei/hei_patch);
    wid_stride = ceil(wid/wid_patch);
    use_start_x = 0;
    use_start_y = 0;
    use_end_x = 0;
    use_end_y = 0;

    ext_start_x = 0;
    ext_end_x = 0;
    ext_start_y = 0;
    ext_end_y = 0;

    posext_start_x = 0;
    posext_start_y = 0;
    posext_end_x = 0;
    posext_end_y = 0;

    % extract each patch for inference
    im_h = [];
    for x = 1 : hei_stride : hei
        for y = 1 : wid_stride : wid
            % decide the length of hei and wid for each patch
            use_start_x = x;
            use_start_y = y;
            if x - rf > 1 % add border
                ext_start_x = x-rf;
                posext_start_x = rf+1;
            else
                ext_start_x = x;
                posext_start_x = 1;
            end
            if y-rf > 1
                ext_start_y = y-rf;
                posext_start_y = rf+1;
            else
                ext_start_y = y;
                posext_start_y = 1;
            end

            use_end_x = use_start_x+hei_stride-1;
            use_end_y = use_start_y+wid_stride-1;

            if use_start_x+hei_stride+rf-1 <= hei
                hei_length = hei_stride+rf;
                ext_end_x = use_start_x+hei_length-1;
                posext_end_x = hei_length-rf+posext_start_x-1;

            else
                hei_length = hei-ext_start_x+1;
                ext_end_x = ext_start_x+hei_length-1;
                posext_end_x = hei_length;
                use_end_x = ext_start_x+hei_length-1;
            end
            if use_start_y+wid_stride+rf-1 <= wid
                wid_length = wid_stride+rf;
                ext_end_y = use_start_y+wid_length-1;
                posext_end_y = wid_length-rf+posext_start_y-1;

            else
                wid_length = wid-ext_start_y+1;
                ext_end_y = ext_start_y+wid_length-1;
                posext_end_y = wid_length;
                use_end_y = ext_start_y+wid_length-1;
            end

            subim_input = im_b(ext_start_x : ext_end_x, ext_start_y : ext_end_y);  % input
            data = permute(subim_input,[2, 1, 3]);
            model = [model_path '.prototxt'];
            subim_output = do_cnn(model,weights,data);
            subim_output = subim_output';
            subim_output = subim_output(posext_start_x:posext_end_x,posext_start_y:posext_end_y);
            % fill im_h with sub_output
            im_h(use_start_x:use_end_x,use_start_y:use_end_y) = subim_output;
        end
    end

    im_h1 = single(im_h) * 255;
    im_gnd1 = single(im_gnd) * 255;
    im_b1 = single(im_b) * 255;

    im_b_set{iii} = im_b1;
    im_h_set{iii} = im_h1;
    im_gnd_set{iii} = im_gnd1;

    %% compute PSNR and SSIM and IFC
    noised(1) = compute_psnr(im_gnd1,im_b1);
    memnet(1) = compute_psnr(im_gnd1,im_h1);
    noised(2) = ssim_index(im_gnd1,im_b1);
    memnet(2) = ssim_index(im_gnd1,im_h1);

    noise_set = [noise_set; noised];
    memnet_set = [memnet_set; memnet];
    %% save images
    imwrite(uint8(im_h1),fullfile(folderResultCur,[imageName,'_Noise',num2str(Noise_level),'.png']));
end
mean_noised = [mean_noised; [mean(noise_set(:,1)) mean(noise_set(:,2))]];
mean_memnet = [mean_memnet; [mean(memnet_set(:,1)) mean(memnet_set(:,2))]];

%%% save PSNR and SSIM metrics
PSNR_set = memnet_set(:,1);
SSIM_set = memnet_set(:,2);
save(fullfile(folderResultCur,['PSNR_',setTestCur,'_Noise',num2str(Noise_level),'.mat']),['PSNR_set'])
save(fullfile(folderResultCur,['SSIM_',setTestCur,'_Noise',num2str(Noise_level),'.mat']),['SSIM_set'])

disp(['noise = ' num2str(mean_noised(1,:)) '---- MemNet = ' num2str(mean_memnet(1,:))]);

end


warning('off','all');

tic;
image = double(imread('forest.jpg'))/255;

image = imresize(image, 0.1);

result = dehaze(image, 0.95, 15);
toc;

figure, imshow(image)
figure, imshow(result)

warning('on','all');


% Wrapper for vl_nntranslate2D block
% inputs{1} :   X     : 1 x 2 x n x b
% inputs{2} :   T     : 1 x 2 x 1 x b
% outputs{1}:   y     : 1 x 2 x n x b

classdef translate2D < dagnn.Layer
    methods
        function outputs = forward(~, inputs, ~)

            useGPU = isa(inputs{1}, 'gpuArray');
            if useGPU
                outputs{1} = gpuArray ( vl_nntranslate2D(gather(inputs{1}), gather(inputs{2})) );
            else
                outputs{1} = vl_nntranslate2D(inputs{1}, inputs{2});
            end

        end

        function [derInputs, derParams] = backward(~, inputs, ~, derOutputs)

            useGPU = isa(inputs{1}, 'gpuArray');
            if useGPU
                [y,dsdy] = vl_nntranslate2D(gather(inputs{1}),gather(inputs{2}), gather(derOutputs{1}));
                derInputs = {gpuArray(y),gpuArray(dsdy)};
            else
                [y,dsdy] = vl_nntranslate2D(inputs{1},inputs{2}, derOutputs{1});
                derInputs = {y,dsdy};
            end

            derParams = {};
        end

        function outputSizes = getOutputSizes(~, inputSizes)
            outputSizes = inputSizes{1};
        end

        function obj = translate2D(varargin)
            obj.load(varargin);
        end
    end
end


function [rec,prec,ap] = wsddnVOCevaldet(VOCopts,cls,res,draw)

% load test set
tic;
VOCopts.annocachepath=[VOCopts.localdir '%s_anno_cache.mat'];
cp=sprintf(VOCopts.annocachepath,VOCopts.testset);
if exist(cp,'file')
  fprintf('%s: pr: loading ground truth\n',cls);
  load(cp,'gtids','recs');
else
  [gtids,t]=textread(sprintf(VOCopts.imgsetpath,VOCopts.testset),'%s %d');
  for i=1:length(gtids)
    % display progress
    if toc>1
      fprintf('%s: pr: load: %d/%d\n',cls,i,length(gtids));
      drawnow;
      tic;
    end

    % read annotation
    recs(i)=PASreadrecord(sprintf(VOCopts.annopath,gtids{i}));
  end
  save(cp,'gtids','recs');
end

fprintf('%s: pr: evaluating detections\n',cls);

% hash image ids
hash=wsddnVOChash_init(gtids);

% extract ground truth objects

npos=0;
gt(length(gtids))=struct('BB',[],'diff',[],'det',[]);
for i=1:length(gtids)
  % extract objects of class
  clsinds=strmatch(cls,{recs(i).objects(:).class},'exact');
  gt(i).BB=cat(1,recs(i).objects(clsinds).bbox)';
  gt(i).diff=[recs(i).objects(clsinds).difficult];
  gt(i).det=false(length(clsinds),1);
  npos=npos+sum(~gt(i).diff);
end

% load results
ids        = res.ids;
confidence = res.confidence;
BB         = res.bbox';

% sort detections by decreasing confidence
[sc,si]=sort(-confidence);
ids=ids(si);
BB=BB(:,si);

% assign detections to ground truth objects
nd=length(confidence);
tp=zeros(nd,1);
fp=zeros(nd,1);
tic;
for d=1:nd
  % display progress
  if toc>1
    fprintf('%s: pr: compute: %d/%d\n',cls,d,nd);
    drawnow;
    tic;
  end

  % find ground truth image
  i=wsddnVOChash_lookup(hash,ids{d});
  if isempty(i)
    error('unrecognized image "%s"',ids{d});
  elseif length(i)>1
    error('multiple image "%s"',ids{d});
  end

  % assign detection to ground truth object if any
  bb=BB(:,d);
  ovmax=-inf;
  for j=1:size(gt(i).BB,2)
    bbgt=gt(i).BB(:,j);
    bi=[max(bb(1),bbgt(1)) ; max(bb(2),bbgt(2)) ; min(bb(3),bbgt(3)) ; min(bb(4),bbgt(4))];
    iw=bi(3)-bi(1)+1;
    ih=bi(4)-bi(2)+1;
    if iw>0 & ih>0
      % compute overlap as area of intersection / area of union
      ua=(bb(3)-bb(1)+1)*(bb(4)-bb(2)+1)+...
        (bbgt(3)-bbgt(1)+1)*(bbgt(4)-bbgt(2)+1)-...
        iw*ih;
      ov=iw*ih/ua;
      if ov>ovmax
        ovmax=ov;
        jmax=j;
      end
    end
  end
  % assign detection as true positive/don't care/false positive
  if ovmax>=VOCopts.minoverlap
    if ~gt(i).diff(jmax)
      if ~gt(i).det(jmax)
        tp(d)=1;            % true positive
        gt(i).det(jmax)=true;
      else
        fp(d)=1;            % false positive (multiple detection)
      end
    end
  else
    fp(d)=1;                    % false positive
  end
end

% compute precision/recall
fp=cumsum(fp);
tp=cumsum(tp);
rec=tp/npos;
prec=tp./(fp+tp);

ap=wsddnVOCap(rec,prec);

if draw
  % plot precision/recall
  plot(rec,prec,'-');
  grid;
  xlabel 'recall'
  ylabel 'precision'
  title(sprintf('class: %s, subset: %s, AP = %.3f',cls,VOCopts.testset,ap));
end