% Test file for @chebfun/minandmax.m.

function pass = test_minandmax(pref)

if ( nargin == 0 )
    pref = chebfunpref();
end

% Check empty case.
[y, x] = minandmax(chebfun());
pass(1) = isempty(y) && isempty(x);

% Check operation without breakpoints.
f = chebfun(@(x) ((x - 0.2).^3 - (x - 0.2) + 1).*sec(x - 0.2), pref);
[y, x] = minandmax(f);
y_exact = [0.710869767377087 ; 1.884217141925336];
pass(2) = all(abs(y - y_exact) <= 10*vscale(f)*eps) && ...
    all(abs(feval(f, x) - y_exact) <= 10*vscale(f)*eps);

% Check operation with breakpoints.
f = chebfun(@(x) ((x - 0.2).^3 - (x - 0.2) + 1).*sec(x - 0.2), ...
    linspace(-1, 1, 10), pref);
[y, x] = minandmax(f);
y_exact = [0.710869767377087 ; 1.884217141925336];
pass(3) = all(abs(y - y_exact) <= 10*vscale(f)*eps) && ...
    all(abs(feval(f, x) - y_exact) <= 10*vscale(f)*eps);

% Check operation for complex-valued chebfuns.
f = chebfun({@(x) exp((1 + 1i)*x), @(x) 1-x/10}, [-1 0 1],  pref);
[y, x] = minandmax(f);
y_exact = [exp(-1 - 1i) ; 1];
err1 = abs(y - y_exact);
err2 = abs(feval(f, x) - y_exact);
pass(4) = all(err1 <= 10*vscale(f)*eps) && ...
    all(err2 <= 10*vscale(f)*eps);

% Check operation for pointValues.
f = chebfun({-1, 1, 2}, [-1, 0, 1, 2]);
[y, ignored] = minandmax(f);
pass(5) = all(y == [-1 ; 2]);

f.pointValues(1,1) = 10;
f.pointValues(3,1) = -10;
[y, x] = minandmax(f);
pass(6) = all(y == [-10 ; 10]) && all(x == [1 ; -1]);

% Check computation of local extrema.
f = chebfun(@(x) sin(x).^2 + sin(x.^2), [0, 4]);
[y, x] = minandmax(f, 'local');
y_exact = [        0
   1.923771282655145
  -0.342247088203205
   1.117294907913736
  -0.971179645473729
   1.343997479566445
   0.284846700239241];
x_exact = [        0
   1.323339426259694
   2.220599667639221
   2.781195946808315
   3.308480466603983
   3.776766383330969
   4.000000000000000];
pass(7) = numel(y == 7) && norm(y - y_exact, inf) < 10*vscale(f)*eps;

% Check operation for array-valued chebfuns.
f = chebfun(@(x) [sin(10*x) cos(10*x) exp(x)], [-1 -0.5 0.5 1]);
[y, x] = minandmax(f);
y_exact = [-1 -1 exp(-1) ; 1 1 exp(1)];
fx = feval(f, x(:));
fx = [fx(1:2,1) fx(3:4,2) fx(5:6,3)];
pass(8) = all(abs(y(:) - y_exact(:)) <= 10*vscale(f)*eps) && ...
    all(abs(fx(:) - y_exact(:)) <= 10*vscale(f)*eps);

f = chebfun(@(x) [exp((1 + 1i)*x) sec(1i*(x - 0.5))], [-1 0 1], ...
    pref);
[y, x] = minandmax(f);
y_exact = [exp(-1 - 1i) sec(-1.5i) ; exp(1 + 1i) 1];
fx = feval(f, x(:));
fx = [fx(1:2,1) fx(3:4,2)];
pass(9) = all(abs(y(:) - y_exact(:)) <= 10*vscale(f)*eps) && ...
    all(abs(fx(:) - y_exact(:)) <= 10*vscale(f)*eps);

op = @(x) sin(x).^2 + sin(x.^2);
f = chebfun(@(x) [op(x) op(x/2)], [0, 4]);
[y, x] = minandmax(f, 'local');
y_exact = [        0                  0
   1.923771282655145  1.923771282655145
  -0.342247088203205  0.070019315123878
   1.117294907913736  NaN
  -0.971179645473729  NaN
   1.343997479566445  NaN
   0.284846700239241  NaN];
x_exact = [        0                  0
   1.323339426259694  2.646678852519388
   2.220599667639221  4.000000000000000
   2.781195946808315  NaN
   3.308480466603983  NaN
   3.776766383330969  NaN
   4.000000000000000  NaN];
fx1 = feval(f, x_exact(:,1));
fx2 = feval(f, x_exact(1:3,2));
pass(10) = isequal(size(y), [7 2]) && ...
    all(isnan(y(4:end,2))) && all(isnan(x(4:end,2)));
pass(11) = norm(y(:,1) - y_exact(:,1), inf) < 10*vscale(f)*eps && ...
    norm(y(1:3,2) - y_exact(1:3,2), inf) < 10*vscale(f)*eps && ...
    norm(fx1(:,1) - y_exact(:,1), inf) < 10*vscale(f)*eps && ...
    norm(fx2(:,2) - y_exact(1:3,2), inf) < 10*vscale(f)*eps;

%% Test on singular function: piecewise smooth chebfun - splitting on.

dom = [-2 7];
pow = -0.5;
op = @(x) (x - dom(1)).^pow.*(sin(300*x).^2);
f = chebfun(op, dom, 'exps', [pow 0], 'splitting', 'on');
[y, x] = minandmax(f);
y_exact = [0 ; Inf];
fx = op(x);
pass(12) = ((max(abs(y - y_exact)) < 1e4*eps) && ...
          (max(abs(fx - y_exact)) < 1e4*eps));

%% Tests on function defined on unbounded domain:

% Doubly-infinite domain:

% Set the domain:
dom = [-Inf Inf];

op = @(x) (1-exp(-x.^2))./x;
f = chebfun(op, dom);
[vals, pos] = minandmax(f);
% These exact solutions are obtained using Mathematica:
vExact = [-0.6381726863389515 ; 0.6381726863389515];
pExact = [-1.120906422778534 ; 1.120906422778534];
errV = vals - vExact;
errP = pos - pExact;
pass(13) = ( norm(errV, inf) < 1e2*eps*vscale(f) ) && ...
    ( norm(errP, inf) < 1e2*eps*vscale(f) );

%% from #824
gam = chebfun('gamma(x)',[-4 4],'blowup','on','splitting','on');
mm = minandmax(1./gam);
pass(14) = norm(mm - [-1.125953228398760;4.079508980001102]) < 1e4*eps;

end


x = [ 1 2 3 4 5 6];
y = [ 2 6 8 7 8 5];
barh(x,y);
title('\bfExample of a Horizontal Bar Plot');
xlabel('\bf\ity');
ylabel('\bf\itx');
axis([0 10 0 7]);


function out=first_order_reshape(S,ref)

S=S{2};

for j=1:size(S.signal,2)
out(j)= S.signal{j} / S.signal{ref};
end


function varargout = plot(f, varargin)
%PLOT   Plot a BALLFUN on the ball
%   PLOT(F) creates a plot showing F on the ball.
%
%   PLOT(f, 'WedgeAz') plot a BALLFUN with a wedge in the azimuthal
%   (longitude) direction removed.
%
%   PLOT(f, 'WedgePol') plot a BALLFUN with a wedge in the polar
%   (latitude) direction removed.
%
% EXAMPLES:
%   f = cheb.galleryball;
%   plot(f)
%   plot(f, 'WedgeAz')
%   plot(f, 'WedgePol')
%
% See also BALLFUN/SURF, BALLFUN/SLICE.

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

% Check if the function is empty
if isempty(f)
    error('CHEBFUN:BALLFUN:plot:isempty','Function is empty.');
end

% Add a warning of the function is not real
if (f.isReal == 0)
    warning('CHEBFUN:BALLFUN:plot:isReal','Function is not real, plotting the real part.');
end

if ( nargin == 1 )
    h = plotBall(f);
elseif ( nargin == 2 ) && ( strcmpi(varargin{1},'wedgeaz') )
    h = plotWedgeAz(f);
elseif ( nargin == 2 ) && ( strcmpi(varargin{1},'wedgepol') )
    h = plotWedgePol(f);
else
    error('CHEBFUN:BALLFUN:plot:input Invalid input arguments')
end

if ( nargout > 0 )
    varargout = { h };
end
end

function h = plotBall(f)
% Plot a BALLFUN function on the ball

% Is the plot currently being held?
plotOnHold = ishold;
% Default plotting options
defaultOpts = {'facecolor', 'interp','edgecolor', 'none'};

% Define the size of F:
[m,n,p] = size(f);

% m >= 25 and n, p >= 28
m = 25*(m < 25) + m*(m >= 25);
n = 28*(n < 28) + n*(n>=28);
p = 28*(p < 28) + p*(p>=28);

% Impose m = 1 [6] and n, p = 0 [4] to avoid errors in the plot
m = m + mod(1-mod(m,6),6);
n = n + mod(4-mod(n,4),4);
p = p + mod(4-mod(p,4),4);

% Get the coeffs of the ballfun function F(r,lam,th), lam is the
% azimuthal variable in [-pi,pi] and theta the polar variable in [0,pi]
F = coeffs3(f, m, n, p);

% Convert to values
ff = real(ballfun.coeffs2vals(F));

% Permute lambda and theta
ff = permute(ff,[1 3 2]);

% Evaluation points
r   = chebpts( m );
lam  = [pi*trigpts( n ); pi];
th = [pi*trigpts( p );pi]-pi/2;

% Remove doubled-up data
r = r(floor(m/2)+1:end);
th = th(floor(p/2)+1:end);

% Reverse theta : 1st element of the array is theta = pi (South Pole), last element is
% th = 0 (not included) (North Pole)
ff  = ff(floor(m/2)+1:end,[1 end:-1:floor(p/2)+1],:);
ff(:,:,end+1) = ff(:,:,1);

% Define the meshgrid
[tt, rr, ll] = meshgrid(th, r, lam);

% Slices in the cylinder to plot
% Find the indice of r = 0.5
[~,idr]=min(abs(r-0.5));
rslice = rr(idr,1,1);
tslice = tt(1,[1,floor(p/4)+1],1);
lslice = ll(1,1,[1,floor(n/4)+1]);

hslicer = slice(tt,rr,ll,ff,tslice,rslice,lslice);

hold on
for j = 1:numel(hslicer)
    h = hslicer(j);
    [xs,ys,zs] = sph2cart(h.ZData,h.XData,h.YData);
    h = surf(xs,ys,zs,h.CData,defaultOpts{:});
end
delete(hslicer);

if ~plotOnHold
    hold off;
end

axis([-1 1 -1 1 -1 1])
daspect([1 1 1])

camlight('headlight');
lighting phong;
material dull;
end

function h = plotWedgeAz(f)
% Plot a BALLFUN with a wedge in the azimuthal direction removed

% Is the plot currently being held?
plotOnHold = ishold;
% Default plotting options
defaultOpts = {'facecolor', 'interp','edgecolor', 'none'};

% Azimuthal (longitude) values to include.  TODO: Make these optional inputs
az_intvl = [-pi/2 pi];

% Define the size of f:
[m,n,p] = size(f);

% m >= 25 and n, p >= 28
m = 25*(m < 25) + m*(m >= 25);
n = 28*(n < 28) + n*(n>=28);
p = 28*(p < 28) + p*(p>=28);

% Impose m = 1 [6] and n, p = 0 [4] to avoid errors in the plot
m = m + mod(1-mod(m,6),6);
n = n + mod(4-mod(n,4),4);
p = p + mod(4-mod(p,4),4);

% Construct the values of lambda and theta to plot on the outer sphere (r=1)
lam = linspace(az_intvl(1),az_intvl(2),n);
th = linspace(0,pi,p)';

% Evaluate the function on the outer sphere
ff = permute(fevalm(f,1,lam,th),[3 2 1]);

% Plot the result
h = surf(sin(th)*cos(lam),sin(th)*sin(lam),cos(th)*ones(1,n),ff,defaultOpts{:});
hold on

% Construct the values of r and theta to plot from the origin to the outer
% sphere (r=1).
r = chebpts(m); r = r(floor(m/2)+1:end); th = th';

% Evaluate the function on the wedge from the origin along the az_intvl(1).
ff = permute(fevalm(f,r,lam(1),th),[1 3 2]);
% Plot the result
surf(r*sin(th)*cos(lam(1)),r*sin(th)*sin(lam(1)),r*cos(th),ff,defaultOpts{:})

% Evaluate the function on the wedge from the origin along the az_intvl(2).
ff = permute(fevalm(f,r,lam(end),th),[1 3 2]);
% Plot the result
surf(r*sin(th)*cos(lam(end)),r*sin(th)*sin(lam(end)),r*cos(th),ff,defaultOpts{:})

if ~plotOnHold
    hold off;
end

camlight('headlight');
lighting phong;
material dull;

axis([-1 1 -1 1 -1 1])
daspect([1 1 1])
end

function h = plotWedgePol(f)
% Plot a BALLFUN with a wedge in the polar direction removed

% Is the plot currently being held?
plotOnHold = ishold;
% Default plotting options
defaultOpts = {'facecolor', 'interp','edgecolor', 'none'};

% Polar (latitude) values to include.  TODO: Make these optional inputs
pol_intvl = [pi/2 pi];
% Azimuthal (longitude) values to include.  TODO: Make these optional inputs
az_intvl = [0 pi];

% Define the size of f:
[m,n,p] = size(f);

% m >= 25 and n, p >= 28
m = 25*(m < 25) + m*(m >= 25);
n = 28*(n < 28) + n*(n>=28);
p = 28*(p < 28) + p*(p>=28);

% Impose m = 1 [6] and n, p = 0 [4] to avoid errors in the plot
m = m + mod(1-mod(m,6),6);
n = n + mod(4-mod(n,4),4);
p = p + mod(4-mod(p,4),4);

% Construct the values of lambda and theta to plot on the outer sphere (r=1)
% where the sphere is closed
lam = linspace(-pi,pi,n);
th = linspace(pol_intvl(1),pol_intvl(2),p)';

% Evaluate the function on the outer sphere
ff = permute(fevalm(f,1,lam,th),[3 2 1]);

% Plot the result
h = surf(sin(th)*cos(lam),sin(th)*sin(lam),cos(th)*ones(1,n),ff,defaultOpts{:});
hold on

% Construct the values of lambda and theta to plot on the outer sphere (r=1)
% where the sphere is open at the wedge
lam = linspace(az_intvl(1),az_intvl(2),n);
th = linspace(0,pol_intvl(1),p)';

% Evaluate the function on the outer sphere
ff = permute(fevalm(f,1,lam,th),[3 2 1]);

% Plot the result
surf(sin(th)*cos(lam),sin(th)*sin(lam),cos(th)*ones(1,n),ff,defaultOpts{:})

% Construct the values of r and theta to plot from the origin to the outer
% sphere (r=1).
r = chebpts(m); r = r(floor(m/2)+1:end); th = linspace(0,pol_intvl(1),p);

% Evaluate the function on the wedge from the origin along the az_intvl(1).
ff = permute(fevalm(f,r,lam(1),th),[1 3 2]);
% Plot the result
surf(r*sin(th)*cos(lam(1)),r*sin(th)*sin(lam(1)),r*cos(th),ff,defaultOpts{:})

% Construct the values of r and theta to plot from the origin to the outer
% sphere (r=1).
r = chebpts(m); r = r(floor(m/2)+1:end); th = linspace(0,pol_intvl(1),p);

% Evaluate the function on the wedge from the origin along the az_intvl(1).
ff = permute(fevalm(f,r,lam(end),th),[1 3 2]);
% Plot the result
surf(r*sin(th)*cos(lam(end)),r*sin(th)*sin(lam(end)),r*cos(th),ff,defaultOpts{:})

% Construct the values of r and lambda to plot from the origin to the outer
% sphere (r=1): slice along
r = chebpts(m); r = r(floor(m/2)+1:end); lam = linspace(-pi,pi,p);

% Evaluate the function on the wedge from the origin along the az_intvl(2).
ff = fevalm(f,r,lam,pol_intvl(1));
% Plot the result
surf(r*sin(pol_intvl(1))*cos(lam),r*sin(pol_intvl(1))*sin(lam),r*cos(pol_intvl(1))*ones(size(lam)),ff,defaultOpts{:})

if ~plotOnHold
    hold off;
end

camlight('headlight');
lighting phong;
material dull;

axis([-1 1 -1 1 -1 1])
daspect([1 1 1])
end


% Ryan Steindl based on Robotics Toolbox for MATLAB (v6 and v9)
%
% Copyright (C) 1993-2011, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com
function qd = dot(q, omega)
    E = q.s*eye(3,3) - skew(q.v);
    omega = omega(:);
    qd = Quaternion(-0.5*q.v*omega, 0.5*E*omega);
end


function varargout = cos(varargin)
%COS   Cosine of a DISKFUN.
%   COS(F) returns the cosine of F.
%
% See also DISKFUN/COSH and DISKFUN/SIN

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

[varargout{1:nargout}] = cos@separableApprox(varargin{:});

end


function ori = accumarray(subs,ori,varargin)
% accumarray for orientation
%
% Syntax
%   ori = accumarray(subs,ori)
%
% Input
%  subs -
%  ori - @orienrtation
%
% Output
%  ori - @orientation

% find a reference orientation for each class
ref = accumarray(subs,1:length(ori),[],@(x) x(1));
ori_ref = ori.subSet(ref(subs));

ori = project2FundamentalRegion(ori,ori_ref);

a = accumarray(subs,ori.a,varargin{:});
b = accumarray(subs,ori.b,varargin{:});
c = accumarray(subs,ori.c,varargin{:});
d = accumarray(subs,ori.d,varargin{:});

% normalize
s = sqrt(a.^2 + b.^2 + c.^2 + d.^2);
ori.a = a ./ s; ori.b = b ./ s; ori.c = c ./ s; ori.d = d ./ s;

ori.i = ori_ref.i(ref);


function [Nbegcol,Nlencol,Nrowndx,Nobjcnt,Nobjndx,cJacobian] = jacSparsity(interfacedata)

linear = setdiff(find(interfacedata.variabletype == 0),interfacedata.evalVariables);
oJacobian = zeros(length(linear),1);
variabletype = interfacedata.variabletype;
c = interfacedata.c;
F_struc = interfacedata.F_struc;
m = size(interfacedata.F_struc,1);

nonlinear = variabletype;
nonlinear(interfacedata.evalVariables) = 1;

for i = find(c(:)')%1:length(c)
%    if c(i)
        if nonlinear(i)%variabletype(i) | ismember(i,interfacedata.evalVariables)
            if ismember(i,interfacedata.evalVariables)
                j = find(i == interfacedata.evalVariables);
                variables = interfacedata.evalMap{j}.variableIndex;
            else
                variables = find(interfacedata.monomtable(i,:));
            end
            oJacobian(find(ismember(linear,variables))) = 1;
        end
%    end
end
if m > 0
    cJacobian = zeros(m,length(linear));
    for i = 1:size(F_struc,2)-1
        %if nonlinear(i)%variabletype(i) | ismembc(i,interfacedata.evalVariables)
            f = F_struc(:,i+1);
            for j = find(f(:)')%1:size(F_struc,1)
                %   if f(j)%F_struc(j,i+1)
                %                if variabletype(i) | ismembc(i,interfacedata.evalVariables)
                if ismembc(i,interfacedata.evalVariables)
                    k = find(i == interfacedata.evalVariables);
                    variables = interfacedata.evalMap{k}.variableIndex;
                elseif nonlinear(i)
                    variables = find(interfacedata.monomtable(i,:));
                else
                    variables = i;;
                end
                cJacobian(j,find(ismembc(linear,variables))) = 1;
            end
       % else

       % end
    end
    [Nbegcol,Nlencol,Nrowndx] = lindosparse(cJacobian);
else
    cJacobian = [];
    Nbegcol = [];
    Nrowndx = [];
    Nlencol = [];
    Nbegcol = [Nbegcol sum(Nlencol)];
end
oJacobian = double(oJacobian | any(interfacedata.Q(linear,linear),2));

Nobjndx = find(oJacobian) - 1;
Nobjcnt = length(Nobjndx);
if  isempty(Nobjndx)
    Nobjndx = [];
end


clc
clear
close all

%% Execute the configuration file to read parameters for data paths
addpath('config');
addpath(genpath('functions'));
configFile;

%% Starting parallel pooling (requires Parallel Processing Toolbox)
% This section takes a while to load for the first time
% To shutdown, run: delete(gcp('nocreate'));
if (isempty(gcp) && data_params.use_multithreads)
    parpool();
end

%% Read directories containing images
img_files1 = dir(strcat(data_params.path1,'*.png'));
img_files2 = dir(strcat(data_params.path2,'*.png'));
num_of_images = length(img_files1);

%% Read camera parameters
[P1, P2] = createCamProjectionMatrices(cam_params);

%% Read ground truth file if flag is true
if data_params.show_gt_flag
  ground_truth = load(data_params.gt_file);
  gt_x_max = max(ground_truth(:, end - 8));
  gt_x_min = min(ground_truth(:, end - 8));
  gt_z_max = max(ground_truth(:, end));
  gt_z_min = min(ground_truth(:, end));
end

%% Initialize variables for odometry
pos = [0;0;0];
Rpos = eye(3);

%% Start Algorithm
start = 0;
for t = 1 : num_of_images
    %% Read images for time instant t
    I2_l = imread([img_files1(t+1).folder, '/', img_files1(t).name]);
    I2_r = imread([img_files2(t+1).folder, '/', img_files2(t).name]);
    fprintf('Frame: %i\n', t);

    %% Bootstraping for initialization
    if (start == 0)
        vo_previous.pts1_l = computeFeatures(I2_l, vo_params.feature);
        vo_previous.pts1_r = computeFeatures(I2_r, vo_params.feature);
        start = 1;
        I1_l = I2_l;
        I1_r = I2_r;
        fprintf('\n---------------------------------\n');
        continue;
    end

    %% Implement SOFT for time instant t+1
    [R, tr, vo_previous] = visualSOFT(t, I1_l, I2_l, I1_r, I2_r, P1, P2, vo_params, vo_previous);

    %% Estimated pose relative to global frame at t = 0
    pos = pos + Rpos * tr';
    Rpos = R * Rpos;

    %% Prepare frames for next iteration
    I1_l = I2_l;
    I1_r = I2_r;

    %% Plot the odometry transformed data
    subplot(2, 2, [2, 4]);

    % Read ground truth pose if flag is true
    if data_params.show_gt_flag
      axis([gt_x_min gt_x_max gt_z_min gt_z_max])
      T = reshape(ground_truth(t, :), 4, 3)';
      pos_gt = T(:, 4);
      scatter(pos_gt(1), pos_gt(3), 'r', 'filled');
      hold on;
    end
    scatter( - pos(1), pos(3), 'b', 'filled');
    title(sprintf('Odometry plot at frame %d', t))
    xlabel('x-axis (in meters)');
    ylabel('z-axis (in meters)');

    if data_params.show_gt_flag
        legend('Ground Truth Pose', 'Estimated Pose')
    else
        legend('Estimated Pose')
    end

    %% Pause to visualize the plot
    pause(0.0001);
    fprintf('\n---------------------------------\n');
end


function value = r8mat_max ( m, n, a )

%*****************************************************************************80
%
%% R8MAT_MAX returns the maximum entry of an R8MAT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the number of rows in A.
%
%    Input, integer N, the number of columns in A.
%
%    Input, real A(M,N), the M by N matrix.
%
%    Output, real VALUE, the maximum entry of A.
%
  value = max ( max ( a(1:m,1:n) ) );

  return
end


function [year,month,day,hour,minute,second]=UTC2Cal(Jul1,Jul2,giveDayFrac)
%%UTC2Cal Convert universal coordinated times (UTC) given as a
%         two-part pseudo-Julian dates to a date in terms of the Gregorian
%         calendar in years, months, days, hours, minutes and seconds or
%         in terms of the Gregorian calendar in years, months, days and
%         the fraction of a day.
%
%INPUTS: Jul1, Jul2 Vectors or matrices of the two parts of Julian dates
%                   given in UTC. The units of the date are days. The full
%                   date is the sum of both terms. The date is broken into
%                   two parts to provide more bits of precision. It does
%                   not matter how the date is split.
%       giveDayFrac An optional boolean variable specifying whether the
%                   output should be as years months, days and a fraction
%                   of a day. If this parameter is omitted, the default is
%                   false. That means that the output will be given as
%                   years, months, days, hours, minutes, and seconds.
%
%OUTPUTS: Regardless of the value of giveDayFrac, the first three outputs
%         are the same. They are:
%           year A vector or matrix of years in the Gregorian calendar
%                under UTC time, one for for each Julian date.
%          month A vector or matrix of months in the Gregorian calendar
%                under UTC time, one for each Julian date. 1<=month<=12
%            day A vector or matrix of days in the Gregorian calendar
%                under UTC time, one for each Julian date. Days count from
%                1.
%          If giveDayFrac is omitted or is false, then three additional
%          outputs are
%           hour  A vector or matrix of hours in the Gregorian calendar
%                 under UTC time, one for each Julian date. 0<=hour<=23.
%          minute A vector or matrix of minutes in the Gregorian calendar
%                 under UTC time, one for each Julian date.
%                 0<=minute<=59.
%          second A vector or matrix of seconds in the Gregorian calendar
%                 under UTC time, one for each Julian date. This includes
%                 the possibility of a leap second on the day in question.
%          On the other hand, if giveDayFrac is true, then the one
%          additional output is
%          dayFrac A vector or matrix of values >=0 and <1 indicating the
%                  fraction of the day elapsed, one for each Julian date.
%
%The UTC date is only pseudo-Julian, because there is not a fixed number
%of seconds in a Julian day. The convention used in the IAU standard is
%that the Julian day matches the UTC day regardless of whether the UTC day
%is 86399, 86400 or 86401 SI seconds (depending on the presence of leap
%seconds).
%
%UTC began at 1960 January 1.0 (JD 2436934.5) and this function should not
%be called with an earlier date.
%
%The algorithm can be compiled for use in Matlab  using the
%CompileCLibraries function.
%
%The algorithm is run in Matlab using the command format
%[year,month,day,hour,minute,second]=UTC2Cal(Jul1,Jul2);
%or using
%[year,month,day,dayFrac]=UTC2Cal(Jul1,Jul2,true);
%
%December 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

error('This function is only implemented as a mexed C or C++ function. Please run CompileCLibraries.m to compile the function for use.')

end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function circmo()
    %   This subroutine "circle"  selects the Ni closest earthquakes
    %   around a interactively selected point.  Resets ZG.newcat and ZG.newt2
    %   Operates on "primeCatalog".
    %
    % axis: hmo
    % plots to: plos as xk
    % inCatalog: a
    % outCatalog: newt2, newcat
    % mouse controlled
    % closest events
    %
    % turned into function by Celso G Reyes 2017

    ZG=ZmapGlobal.Data; % used by get_zmap_globals

    %  Input Ni:
    %
    report_this_filefun();
    ZG=ZmapGlobal.Data;

    delete(findobj('Tag','plos1'))

    axes(hmo)

    % interactively get the circle of interest
    shape=ShapeCircle();

    [ZG.newt2, max_km] = selectCircle(newa, shape.toStruct());

    disp(['Radius of selected Circle: ' num2str(max_km)  ' km' ])

    set(gca,'NextPlot','add')
    plot(ZG.newt2.Longitude,ZG.newt2.Latitude,'xk','Tag','plos1');

    ZG.newcat = ZG.newt2;                   % resets ZG.newcat and ZG.newt2

    % plot cumulative number
    ctp=CumTimePlot(ZG.newt2);
    ctp.plot();

end


function scn_spm_choose_hpfilter(spm_results_dir, varargin)
% Plots and choice of optimal high-pass filter from an SPM first-level
% model directory (with statistics and contrasts estimated.)
%
% :Usage:
% ::
%
%     scn_spm_choose_hpfilter(spm_results_dir, ['events_only'],['from_multireg',nr_regs)
%
% SPM5 compatible and SPM8.
%
% Called by: scn_spm_design_check.m
% For all regressors or events only: see scn_spm_get_events_of_interest.m
%
% ..
%    Tor Wager
%    August 2010
%    Lukas Van Oudenhove
%    March 2022: added 'from_multireg' option which was already implemented
%    in scn_spm_get_events_of_interest.m
% ..

if nargin < 1, spm_results_dir = pwd; end

spmfilename = fullfile(spm_results_dir, 'SPM.mat');
if ~exist(spmfilename, 'file')
    error('SPM.mat does not exist in %s\n', spm_results_dir);
end
load(spmfilename);

EVENTS_ONLY = false;
FROM_MULTIREG = false;
SORTBYVIF = false;

i=1;
while i<=numel(varargin)
    if ischar(varargin{i})
        switch varargin{i}
            case 'events_only'
                EVENTS_ONLY = true;
            case 'from_multireg' % added lukasvo76
                FROM_MULTIREG = true;
                i=i+1;
                nr_regs = varargin{i};
            otherwise
                error(['Unrecognized argument: ' varargin{i}])
        end
    else
        error(['Unrecognized argument: ' varargin{i}])
    end
    i=i+1;
end

% Gets events of interest: All regressors, or events only if 'events_only'
% is input as keyword, or first nr_regs if 'from_multireg' is additionally
% specified
if EVENTS_ONLY
    if FROM_MULTIREG % added lukasvo76
        wh_cols = scn_spm_get_events_of_interest(SPM, 'events_only','from_multireg',nr_regs);
    else
        wh_cols = scn_spm_get_events_of_interest(SPM, 'events_only');
    end
else
    wh_cols = scn_spm_get_events_of_interest(SPM);
end

% Prepare design columns of interest
X_interest = SPM.xX.X(:, wh_cols);
I = SPM.xX.X(:, SPM.xX.iB);
X_interest = X_interest - I * pinv(I) * X_interest;

k = size(X_interest, 2);

%% Start by plotting the regressors of interest

create_figure('regressors', 2, 2);
set(gcf,'WindowState','maximized'); % lukasvo76 edited to improve figure quality
delete(subplot(2, 2, 1:2));
axh = axes('Position', [.05 .55 .9 .4]);
set(axh, 'FontSize', 16);

nregs = length(wh_cols);

if k < 30 % edited lukasvo76
    plot_matrix_cols(X_interest, 'horiz');
else
    imagesc(X_interest); colormap gray; colorbar
end

%% Plot the regressors after HP filtering in red

spm_hplength = SPM.xX.K(1).HParam;

X_filtered = filter_X(X_interest, cat(1, SPM.xX.K(:).X0));

hold on;
if k < 30 % edited lukasvo76
    han = plot_matrix_cols(X_filtered, 'horiz');
    set(han, 'Color', 'r');
else
    imagesc(X_filtered); colormap gray; colorbar
end

title(['Task regressors: Black = before filtering, Red = after current ' num2str(spm_hplength) ' s HP filter']);
drawnow

%% Calculate the cumulative power at different frequencies
TR = SPM.xY.RT;

[cumpower, myfreq, design_var_loss, five_percent_HPlength] = ...
    cumulative_power(X_interest, TR);

currenthp = mean(cat(1, SPM.xX.K.HParam));
best_hplen = five_percent_HPlength;

% Plot it
subplot(2, 2, 3);

for i = 1:nregs
    plot(myfreq(:, i), cumpower(:, i), 'k');
end
xlabel('Frequency (Hz)') ; ylabel('Cumpow (regs)');
axis tight
title(sprintf('<5%% regressor var. lost at HPlen = %3.0f sec', five_percent_HPlength))

try
    plot_vertical_line(currenthp, 'r');
    plot_vertical_line(1./best_hplen, 'b');
catch
    disp('Bug! check me')
    keyboard
end

drawnow

%% If Contrasts are entered, then plot those too
X_contrasts = get_contrast_vectors(SPM);
% can handle t- or F-contrasts

subplot(2, 2, 4);
if isempty(X_contrasts)
    title('NO Contrasts Entered!');

else
    [cumpower, myfreq, design_var_loss, five_percent_HPlength_contrasts] = ...
        cumulative_power(X_contrasts, TR);

    best_hplen = five_percent_HPlength_contrasts;

    % Plot it
    for i = 1:size(X_contrasts, 2)
        plot(myfreq(:, i), cumpower(:, i), 'k');
    end
    xlabel('Frequency (Hz)') ; ylabel('Cumpow (contrasts)');
    axis tight
    title(sprintf('<5%% con-variance lost at HPlen = %3.0f sec', five_percent_HPlength_contrasts))

    try
        plot_vertical_line(currenthp, 'r');
        plot_vertical_line(1./best_hplen, 'b');
    catch
        disp('Bug! check me')
        keyboard
    end

end

drawnow

%% Now re-filter the regressors with the best length

[Rmtx, dummy, K] = use_spm_filter(TR, size(X_interest, 1), 'none', 'specify', best_hplen);

X_filtered = filter_X(X_interest, K);

axes(axh); hold on;

if k < 20
    han = plot_matrix_cols(X_filtered, 'horiz');
    set(han, 'Color', 'b');
else
    imagesc(X_filtered); colormap gray; colorbar
end

text(1, nregs + .7, sprintf('Blue = filtering with <=5%% variance loss at %3.0f sec', best_hplen), 'FontSize', 16);
drawnow

end

function X_filtered = filter_X(X_interest, K)

%px = pinv(K);           % pinv of the filtering matrix
Rmtx = eye(size(K, 1)) - K * pinv(K);  % Residual-inducing matrix, y' = Rmtx*y

for i = 1:size(X_interest, 2)

    y = X_interest(:, i);             % select regressor to filter
    %y = y - K * px * y;     % residuals after filtering
    y = Rmtx * y;

    X_filtered(:, i) = scale(y);

end

end

function [cumpower, myfreq, design_var_loss, five_percent_HPlength] = cumulative_power(X_interest, TR)

for i = 1:size(X_interest, 2)

    [myfft, freq] = fft_calc(X_interest(:, i), TR);

    cumpower(:, i) = cumsum(myfft);
    myfreq(:, i) = freq;

    wh = find( cumpower(:, i) < .05 );
    if isempty(wh)
        wh = 2; % first one after intercept
        design_var_loss(i) = cumpower(wh, i);
        fprintf('No cutoff with < .5%% power for reg/contrast %3.0f! Min loss is %3.2f%%\n', i, 100*design_var_loss(i))
    end

    wh = wh(end);  % highest index value with < 5% power loss

    design_var_loss(i) = cumpower(wh, i);
    five_percent_loss_freq(i) = myfreq(wh, i);

end

% HP filter length that preserves 5% design variance loss
% or less in all regressors
five_percent_HPlength = 1 ./ min(five_percent_loss_freq);

end

function X_contrasts = get_contrast_vectors(SPM)
% can handle t- or F-contrasts
X_contrasts = [];
if ~isfield(SPM, 'xCon') || length(SPM.xCon) == 0
    return
end

% Remove intercepts
X = SPM.xX.X;
I = SPM.xX.X(:, SPM.xX.iB);
X = X - I * pinv(I) * X;

for mycon = 1:length(SPM.xCon)

    d = X * SPM.xCon(mycon).c;
    X_contrasts = [X_contrasts d];

end

end


function pstruct = tapas_hgf_namep(pvec)
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2012-2013 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

pstruct = struct;

l = length(pvec)/5;

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

pstruct.mu_0      = pvec(1:l);
pstruct.sa_0      = pvec(l+1:2*l);
pstruct.rho       = pvec(2*l+1:3*l);
pstruct.ka        = pvec(3*l+1:4*l-1);
pstruct.om        = pvec(4*l:5*l-1);
pstruct.pi_u      = pvec(5*l);

return;


function Y = cosh(X)
    % Symbolic hyperbolic cosine.

    % Convert inputs to SymExpression
    % X = SymExpression(X);

    % construct the operation string
    sstr = ['Cosh[' X.s ']'];

    % create a new object with the evaluated string
    Y = SymExpression(sstr);
end


function [pvec, pstruct] = tapas_hgf_categorical_transp(r, ptrans)
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2013 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Number of states whose contingencies have to be learned
no = r.c_prc.n_outcomes;

pvec    = NaN(1,length(ptrans));
pstruct = struct;

pvec(1:no)       = ptrans(1:no);                           % mu2_0
pstruct.mu2_0    = pvec(1:no);
pvec(no+1:2*no)  = exp(ptrans(no+1:2*no));                 % sa2_0
pstruct.sa2_0    = pvec(no+1:2*no);
pvec(2*no+1)     = ptrans(2*no+1);                         % mu3_0
pstruct.mu3_0    = pvec(2*no+1);
pvec(2*no+2)     = exp(ptrans(2*no+2));                    % sa3_0
pstruct.sa3_0    = pvec(2*no+2);
pvec(2*no+3)     = tapas_sgm(ptrans(2*no+3),r.c_prc.kaub); % ka
pstruct.ka       = pvec(2*no+3);
pvec(2*no+4)     = ptrans(2*no+4);                         % om
pstruct.om       = pvec(2*no+4);
pvec(2*no+5)     = tapas_sgm(ptrans(2*no+5),r.c_prc.thub); % th
pstruct.th       = pvec(2*no+5);

return;


function fiff_write_dig_file(filename,lpa,nas,rpa,hpi,eeg,eegref,extra)
%
% function fiff_write_dig_file(filename,lpa,nas,rpa,hpi,eeg,eegref,extra)
%
% Create a fif file containing the Polhemus data
%
% filename      Output file name
%
% The following need to be specified in the Neuromag MEG
% coordinate system in meters
%
% lpa           Left auricular point
% nas           Nasion
% rpa           Right auricular point
% hpi           HPI coil locations               (optional)
% eeg           EEG electrode locations          (optional)
% eegref        EEG reference electrode location (optional)
% extra         Additional head surface points   (optional)
%

%
%   Author : Matti Hamalainen, MGH Martinos Center
%   License : BSD 3-clause
%
%   Revision 1.1  2008/06/04 16:23:24  msh
%   Added fiff_write_dig_file.m
%
%

%
me   = 'MNE:mne_create_dig_file';
%
global FIFF;
if isempty(FIFF)
   FIFF = fiff_define_constants();
end
%
%   Start writing...
%
fid  = fiff_start_file(filename);
%
% Make the transform to bring LPA and RPA to the x axis and nasion
% to the y axis
%
d1 = nas - lpa;
d2 = rpa - lpa;

alpha = d1*d2'/(d2*d2');
r0 = (1-alpha)*lpa + alpha*rpa;     % Origin is where nasion projects on the LPA - RPA line
ex = d2/sqrt(d2*d2');               % x axis through the auricular points, positive from left to right
ey = (nas-r0);                      % y axis points from the origin to the nasion
ey = ey/sqrt(ey*ey');               % Normalize
ez = cross(ex,ey);                  % Use cross product to get a right-handed coordinate system

T = inv([ ex 0 ; ey 0 ; ez 0 ; r0 1 ]);
% The above gives the coordinate transform from the Neuromag coordinates to the
% Polhemus data coordinates; inverse is needed to go the other way
T = T(:,1:3);   % We just need the first three columns because the result is not an augmented vector

%
% Ready to start
%
fiff_start_block(fid,FIFF.FIFFB_ISOTRAK);

fiff_write_int(fid,FIFF.FIFF_MNE_COORD_FRAME,FIFF.FIFFV_COORD_HEAD);

d.kind  = FIFF.FIFFV_POINT_CARDINAL;
d.ident = FIFF.FIFFV_POINT_LPA;
d.r     = [lpa 1]*T;
fiff_write_dig_point(fid,d);

d.kind  = FIFF.FIFFV_POINT_CARDINAL;
d.ident = FIFF.FIFFV_POINT_NASION;
d.r     = [nas 1]*T;
fiff_write_dig_point(fid,d);

d.kind  = FIFF.FIFFV_POINT_CARDINAL;
d.ident = FIFF.FIFFV_POINT_RPA;
d.r     = [rpa 1]*T;
fiff_write_dig_point(fid,d);

fprintf(1,'Wrote the fiducial locations\n');

if exist('hpi','var')
   for k = 1:size(hpi,1)
      d.kind  = FIFF.FIFFV_POINT_HPI;
      d.ident = k;
      d.r     = [hpi(k,:) 1]*T;
      fiff_write_dig_point(fid,d);
   end
   if (size(hpi,1) > 0)
      fprintf(1,'Wrote %d HPI coil locations\n',size(hpi,1));
   end
end

if exist('eeg','var')
   if exist('eegref','var') && ~isempty(eegref)
      d.kind  = FIFF.FIFFV_POINT_EEG;
      d.ident = 0;
      d.r     = [eegref 1]*T;
      fiff_write_dig_point(fid,d);
      fprintf(1,'Wrote EEG reference electrode location\n');
   end
   for k = 1:size(eeg,1)
      d.kind  = FIFF.FIFFV_POINT_EEG;
      d.ident = k;
      d.r     = [eeg(k,:) 1]*T;
      fiff_write_dig_point(fid,d);
   end
   if (size(eeg,1) > 0)
      fprintf(1,'Wrote %d EEG electrode locations\n',size(eeg,1));
   end
end

if exist('extra','var')
   for k = 1:size(extra,1)
      d.kind  = FIFF.FIFFV_POINT_EXTRA;
      d.ident = k;
      d.r     = [extra(k,:) 1]*T;
      fiff_write_dig_point(fid,d);
   end
   if (size(extra,1) > 0)
      fprintf(1,'Wrote %d extra points\n',size(extra,1));
   end
end

fiff_end_block(fid,FIFF.FIFFB_ISOTRAK);

fiff_end_file(fid);


%% DEMO_febio_0035_blob_shear_contact_hex8
% Below is a demonstration for:
%
% * Building geometry for a hemi-spherical blob with hexahedral elements
% which is being sheared by a rigid wall.
% This demo consists off:
% * Defining the boundary conditions
% * Coding the febio structure
% * Running the model
% * Importing and visualizing the displacement results

%% Keywords
%
% * febio_spec version 3.0
% * febio, FEBio
% * indentation
% * contact, sliding, friction
% * rigid body constraints
% * hexahedral elements, hex8, hex20
% * quadrilaterial elements, quad4, quad8
% * shell elements
% * sphere
% * static, solid
% * hyperelastic, Ogden
% * hyperelastic, Fung orthotropic
% * displacement logfile

%%

clear; close all; clc;

%% Plot settings
fontSize=15;
faceAlpha1=0.8;
faceAlpha2=0.3;
markerSize=40;
markerSize2=15;
lineWidth=3;

%% Control parameters

% Path names
defaultFolder = fileparts(fileparts(mfilename('fullpath')));
savePath=fullfile(defaultFolder,'data','temp');

% Defining file names
febioFebFileNamePart='tempModel';
febioFebFileName=fullfile(savePath,[febioFebFileNamePart,'.feb']); %FEB file name
febioLogFileName=fullfile(savePath,[febioFebFileNamePart,'.txt']); %FEBio log file name
febioLogFileName_disp=[febioFebFileNamePart,'_disp_out.txt']; %Log file name for exporting displacement

% Hemi-sphere parameters
hemiSphereRadius=6;
numElementsMantel=4;
smoothEdge=1;
solidElementType='hex8';%'hex20';
membraneThickness=0.1;

% Ground plate parameters
plateRadius=1.3*hemiSphereRadius;

% Probe parameters
probeWidth=3*hemiSphereRadius;
filletProbe=hemiSphereRadius/2; %Fillet radius

% Define probe displacement
probeDisplacement=hemiSphereRadius*2;
probeOverlapFactor=0.3;
probeLength=hemiSphereRadius*2;

% Material parameter set
l=1e-3;
lt=1000*l;
m=1e-3;
mt=1000*m;

mu1=mt;
mu2=mt;
mu3=m;
lambda11=lt;
lambda22=lt;
lambda12=lt;
lambda33=l;
lambda23=l;
lambda13=l;
[E1,E2,E3,G12,G23,G31,v12,v23,v31]=lameInvertHookeOrthotropic(mu1,mu2,mu3,lambda11,lambda22,lambda33,lambda12,lambda23,lambda13);

materialPropertiesFung.E1=E1;
materialPropertiesFung.E2=E2;
materialPropertiesFung.E3=E3;
materialPropertiesFung.G12=G12;
materialPropertiesFung.G23=G23;
materialPropertiesFung.G31=G31;
materialPropertiesFung.v12=v12;
materialPropertiesFung.v23=v23;
materialPropertiesFung.v31=v31;
materialPropertiesFung.c=10;
materialPropertiesFung.k=1000*mean([G12 G23 G31]);

%Ogden parameters
materialPropertiesOgden.c1=1e-3; %Shear-modulus-like parameter
materialPropertiesOgden.m1=2; %Material parameter setting degree of non-linearity
materialPropertiesOgden.k=1000*materialPropertiesOgden.c1; %Bulk modulus

% FEA control settings
numTimeSteps=25;
max_refs=25; %Max reforms
max_ups=0; %Set to zero to use full-Newton iterations
opt_iter=15; %Optimum number of iterations
max_retries=25; %Maximum number of retires
symmetric_stiffness=0;
min_residual=1e-20;
step_size=1/numTimeSteps;
dtmin=(1/numTimeSteps)/100; %Minimum time step size
dtmax=(1/numTimeSteps); %Maximum time step size

%Contact parameters
contactPenalty=0.01;
laugon=0;
minaug=1;
maxaug=10;
fric_coeff=0;
max_traction=0;

%% Creating model geometry and mesh
%

%Control settings
optionStruct.sphereRadius=hemiSphereRadius;
optionStruct.coreRadius=optionStruct.sphereRadius/2;
optionStruct.numElementsMantel=numElementsMantel;
optionStruct.numElementsCore=optionStruct.numElementsMantel*2;
optionStruct.outputStructType=2;
optionStruct.makeHollow=0;
optionStruct.cParSmooth.n=25;

% %Creating sphere
[meshStruct]=hexMeshHemiSphere(optionStruct);

% Access model element and patch data
Fb_blob=meshStruct.facesBoundary;
Cb_blob=meshStruct.boundaryMarker;
V_blob=meshStruct.nodes;
E_blob=meshStruct.elements;

F_blob=element2patch(E_blob);

%%

pointSpacingBlob=max(patchEdgeLengths(Fb_blob,V_blob));

%Smoothen edges
if smoothEdge==1
    %Get rigid region
    ind=1:1:size(V_blob,1); %Indices for all nodes
    indRigid1=find(ismember(ind,Fb_blob(Cb_blob==2,:)) & ~ismember(ind,Fb_blob(Cb_blob==1,:))); %Indices for new bottom surface nodes
    indRigid2=find(ismember(ind,Fb_blob(Cb_blob==1,:)) & ~ismember(ind,Fb_blob(Cb_blob==2,:))); %Indices for new bottom surface nodes
    indRigid=[indRigid1(:); indRigid2(:);];

    %Smoothing
    cPar.Method='HC';
    cPar.n=250;
    cPar.RigidConstraints=indRigid;
    [Vb_blob]=patchSmooth(F_blob,V_blob,[],cPar);
    indSmooth=unique(F_blob(:));
    V_blob(indSmooth,:)=Vb_blob(indSmooth,:);
    %Fix color data with new bottom surface
    Cb_blob=ones(size(Cb_blob));
    Cb_blob(all(ismember(Fb_blob,indRigid1),2))=2;

    meshStruct.nodes=V_blob;
end

if strcmp(solidElementType,'hex20')
    [E_blob,V_blob,~,~,Fb_blob]=hex8_hex20(E_blob,V_blob,{},Fb_blob);
    [Fb_blob_plot]=element2patch(Fb_blob,[],'quad8');

    meshStruct.elements=E_blob;
    meshStruct.nodes=V_blob;
    meshStruct.Fb=Fb_blob_plot;
    shellElementType='quad8';
else
    Fb_blob_plot=Fb_blob;
    shellElementType='quad4';
end

%%
% Visualize blob mesh

hFig=cFigure;
subplot(1,2,1); hold on;
hp=gpatch(Fb_blob_plot,V_blob,Cb_blob,'k',1);
hp.Marker='.';
hp.MarkerSize=markerSize2;

% patchNormPlot(Fb_blob,V_blob);
% plotV(V_blob(indRigid,:),'g.','MarkerSize',25);
axisGeom(gca,fontSize);
colormap(gjet); icolorbar;
camlight headlight;

hs=subplot(1,2,2); hold on;
title('Cut view of solid mesh','FontSize',fontSize);
optionStruct.hFig=[hFig hs];
gpatch(Fb_blob_plot,V_blob,'kw','none',0.25);
meshView(meshStruct,optionStruct);
axisGeom(gca,fontSize);
drawnow;

%% Creating rigid body shear surface

pointSpacingProbe=pointSpacingBlob/2;

%Sketching side profile
d=hemiSphereRadius*cos(asin(1-probeOverlapFactor));
x=[-probeLength-hemiSphereRadius -d -d];
y=[0 0 0];
z=[hemiSphereRadius*(1-probeOverlapFactor) hemiSphereRadius*(1-probeOverlapFactor) hemiSphereRadius*1.5];
V_probe_curve_sketch=[x(:) y(:) z(:)];

%Fillet sketch
np=100; %Number of points used to construct each fillet edge
[V_probe_curve]=filletCurve(V_probe_curve_sketch,filletProbe,np,0);
numPointsProbeCurve=ceil(max(pathLength(V_probe_curve))/pointSpacingProbe);
[V_probe_curve] = evenlySampleCurve(V_probe_curve,numPointsProbeCurve,'pchip',0);

% Extruding curve
% controlParametersExtrude.pointSpacing=pointSpacingProbe;
controlParametersExtrude.depth=hemiSphereRadius*2.5;
controlParametersExtrude.numSteps=ceil(controlParametersExtrude.depth/pointSpacingProbe);
controlParametersExtrude.numSteps=controlParametersExtrude.numSteps+iseven(controlParametersExtrude.numSteps); %Force uneven
controlParametersExtrude.patchType='quad';
controlParametersExtrude.dir=0;
controlParametersExtrude.n=[0 1 0];
controlParametersExtrude.closeLoopOpt=0;

[F_probe,V_probe]=polyExtrude(V_probe_curve,controlParametersExtrude);
F_probe=fliplr(F_probe); %Invert face orientation so normals point to blob

if strcmp(solidElementType,'hex20')
    [F_probe,V_probe]=quad4_quad8(F_probe,V_probe);
    [F_probe_plot]=element2patch(F_probe,[],'quad8');
else
    F_probe_plot=F_probe;
end

center_of_mass_probe=mean(V_probe,1);

%%
% Visualizing probe mesh

cFigure; hold on;
title('The probe surface mesh','fontSize',fontSize);
gpatch(Fb_blob_plot,V_blob,'kw','none',0.5);
hl(1)=plotV(V_probe_curve_sketch,'k.-.','lineWidth',3,'MarkerSize',25);
hl(2)=plotV(V_probe_curve,'r-','lineWidth',3,'MarkerSize',25);
hl(3)=gpatch(F_probe_plot,V_probe,'gw','k',1);
% hl(3).Marker='.';
% hl(3).MarkerSize=markerSize2;
legend(hl,{'Sketched probe curve','Rounded probe curve','Probe surface mesh'}); clear hl;
axisGeom(gca,fontSize);
camlight headlight;
drawnow;

%% Join model node sets

V=[V_blob; V_probe];
F_probe=F_probe+size(V_blob,1);
F_probe_plot=F_probe_plot+size(V_blob,1);

%%
% Visualizing model

cFigure; hold on;
gtitle('Model components',fontSize);
hl(1)=gpatch(Fb_blob_plot,V,'rw','k',0.8);
hl(2)=gpatch(F_probe_plot,V,'gw','k',0.8);
legend(hl,{'Blob','Probe'}); clear hl;
axisGeom(gca,fontSize);
camlight headlight;
drawnow;

%% Get local material axes

[N3,Vn]=patchNormal(Fb_blob_plot,V);
[N1,N2]=vectorOrthogonalPair(N3); %Get orthogonal vector pair

%%
% Visualizing axes

cFigure; hold on;
gtitle('Local material axes',fontSize);
hl(1)=gpatch(Fb_blob_plot,V,'w','k',1);
hl(2)=quiverVec(Vn,N1,pointSpacingBlob,'y');
hl(3)=quiverVec(Vn,N2,pointSpacingBlob,'g');
hl(4)=quiverVec(Vn,N3,pointSpacingBlob,'b');
legend(hl,{'Blob','1st axis','2nd axis','3rd axis'}); clear hl;
axisGeom(gca,fontSize);
camlight headlight;
drawnow;

%% Visualize contact surfaces

cFigure;
title('Probe blob contact pair','fontsize',fontSize);
hl(1)=gpatch(F_probe_plot,V,'rw','k',1);
patchNormPlot(F_probe_plot,V);
hl(2)=gpatch(Fb_blob_plot,V,'gw','k',1);
patchNormPlot(Fb_blob_plot,V);
legend(hl,{'Secondary','Primary'}); clear hl;
axisGeom(gca,fontSize);
camlight headlight;

drawnow;

%% Setup boundary conditions
%

bcSupportList=unique(Fb_blob(Cb_blob==2,:));

%%
% Visualize boundary condition sets

cFigure; hold on;
title('Boundary conditions ','fontsize',fontSize);
gpatch(F_probe_plot,V,'kw','none',0.2);
gpatch(Fb_blob_plot,V,'kw','none',0.2);
hl(1)=plotV(V(bcSupportList,:),'k.','MarkerSize',markerSize);

legend(hl,{'Supported nodes'}); clear hl;
axisGeom(gca,fontSize);
camlight headlight;

drawnow;

%% Defining the FEBio input structure
% See also |febioStructTemplate| and |febioStruct2xml| and the FEBio user
% manual.

%Get a template with default settings
[febio_spec]=febioStructTemplate;

%febio_spec version
febio_spec.ATTR.version='3.0';

%Module section
febio_spec.Module.ATTR.type='solid';

%Control section
febio_spec.Control.analysis='STATIC';
febio_spec.Control.time_steps=numTimeSteps;
febio_spec.Control.step_size=1/numTimeSteps;
febio_spec.Control.solver.max_refs=max_refs;
febio_spec.Control.solver.max_ups=max_ups;
febio_spec.Control.solver.symmetric_stiffness=0;
febio_spec.Control.time_stepper.dtmin=dtmin;
febio_spec.Control.time_stepper.dtmax=dtmax;
febio_spec.Control.time_stepper.max_retries=max_retries;
febio_spec.Control.time_stepper.opt_iter=opt_iter;

%Material section
materialName1='Material1';
febio_spec.Material.material{1}.ATTR.name=materialName1;
febio_spec.Material.material{1}.ATTR.type='Ogden';
febio_spec.Material.material{1}.ATTR.id=1;
febio_spec.Material.material{1}.c1=materialPropertiesOgden.c1;
febio_spec.Material.material{1}.m1=materialPropertiesOgden.m1;
febio_spec.Material.material{1}.c2=materialPropertiesOgden.c1;
febio_spec.Material.material{1}.m2=-materialPropertiesOgden.m1;
febio_spec.Material.material{1}.k=materialPropertiesOgden.k;

materialName2='Material2';
febio_spec.Material.material{2}.ATTR.name=materialName2;
febio_spec.Material.material{2}.ATTR.type='Fung orthotropic';
febio_spec.Material.material{2}.ATTR.id=2;
febio_spec.Material.material{2}.E1=materialPropertiesFung.E1;
febio_spec.Material.material{2}.E2=materialPropertiesFung.E2;
febio_spec.Material.material{2}.E3=materialPropertiesFung.E3;
febio_spec.Material.material{2}.G12=materialPropertiesFung.G12;
febio_spec.Material.material{2}.G23=materialPropertiesFung.G23;
febio_spec.Material.material{2}.G31=materialPropertiesFung.G31;
febio_spec.Material.material{2}.v12=materialPropertiesFung.v12;
febio_spec.Material.material{2}.v23=materialPropertiesFung.v23;
febio_spec.Material.material{2}.v31=materialPropertiesFung.v31;
febio_spec.Material.material{2}.c=materialPropertiesFung.c;
febio_spec.Material.material{2}.k=materialPropertiesFung.k;

materialName3='Material3';
febio_spec.Material.material{3}.ATTR.name=materialName3;
febio_spec.Material.material{3}.ATTR.type='rigid body';
febio_spec.Material.material{3}.ATTR.id=3;
febio_spec.Material.material{3}.density=1;
febio_spec.Material.material{3}.center_of_mass=center_of_mass_probe;

%Mesh section
% -> Nodes
febio_spec.Mesh.Nodes{1}.ATTR.name='nodeSet_all'; %The node set name
febio_spec.Mesh.Nodes{1}.node.ATTR.id=(1:size(V,1))'; %The node id's
febio_spec.Mesh.Nodes{1}.node.VAL=V; %The nodel coordinates

% -> Elements
partName1='Part1';
febio_spec.Mesh.Elements{1}.ATTR.name=partName1; %Name of this part
febio_spec.Mesh.Elements{1}.ATTR.type=solidElementType; %Element type
febio_spec.Mesh.Elements{1}.elem.ATTR.id=(1:1:size(E_blob,1))'; %Element id's
febio_spec.Mesh.Elements{1}.elem.VAL=E_blob; %The element matrix

partName2='Part2';
febio_spec.Mesh.Elements{2}.ATTR.name=partName2; %Name of this part
febio_spec.Mesh.Elements{2}.ATTR.type=shellElementType; %Element type
febio_spec.Mesh.Elements{2}.elem.ATTR.id=size(E_blob,1)+(1:1:size(Fb_blob,1))'; %Element id's
febio_spec.Mesh.Elements{2}.elem.VAL=Fb_blob;

partName3='Part3';
febio_spec.Mesh.Elements{3}.ATTR.name=partName3; %Name of this part
febio_spec.Mesh.Elements{3}.ATTR.type=shellElementType; %Element type
febio_spec.Mesh.Elements{3}.elem.ATTR.id=size(E_blob,1)+size(Fb_blob,1)+(1:1:size(F_probe,1))'; %Element id's
febio_spec.Mesh.Elements{3}.elem.VAL=F_probe;

% -> NodeSets
nodeSetName1='bcSupportList';
febio_spec.Mesh.NodeSet{1}.ATTR.name=nodeSetName1;
febio_spec.Mesh.NodeSet{1}.node.ATTR.id=bcSupportList(:);

%MeshDomains section
febio_spec.MeshDomains.SolidDomain.ATTR.name=partName1;
febio_spec.MeshDomains.SolidDomain.ATTR.mat=materialName1;

febio_spec.MeshDomains.ShellDomain{1}.ATTR.name=partName2;
febio_spec.MeshDomains.ShellDomain{1}.ATTR.mat=materialName2;

febio_spec.MeshDomains.ShellDomain{2}.ATTR.name=partName3;
febio_spec.MeshDomains.ShellDomain{2}.ATTR.mat=materialName3;

% % -> Surfaces
surfaceName1='contactSurface1';
febio_spec.Mesh.Surface{1}.ATTR.name=surfaceName1;
febio_spec.Mesh.Surface{1}.(shellElementType).ATTR.id=(1:1:size(F_probe,1))';
febio_spec.Mesh.Surface{1}.(shellElementType).VAL=F_probe;

surfaceName2='contactSurface2';
febio_spec.Mesh.Surface{2}.ATTR.name=surfaceName2;
febio_spec.Mesh.Surface{2}.(shellElementType).ATTR.id=(1:1:size(Fb_blob(Cb_blob==1,:),1))';
febio_spec.Mesh.Surface{2}.(shellElementType).VAL=Fb_blob(Cb_blob==1,:);

% -> Surface pairs
contactPairName1='Contact1';
febio_spec.Mesh.SurfacePair{1}.ATTR.name=contactPairName1;
febio_spec.Mesh.SurfacePair{1}.primary=surfaceName2;
febio_spec.Mesh.SurfacePair{1}.secondary=surfaceName1;

%MeshData section
% -> ElementData
febio_spec.MeshData.ElementData{1}.ATTR.var='shell thickness';
febio_spec.MeshData.ElementData{1}.ATTR.elem_set=partName2;
febio_spec.MeshData.ElementData{1}.elem.ATTR.lid=(1:1:size(Fb_blob,1))';
febio_spec.MeshData.ElementData{1}.elem.VAL=membraneThickness.*ones(size(Fb_blob));

%MeshData section
% -> ElementData
febio_spec.MeshData.ElementData{2}.ATTR.elem_set=partName2;
febio_spec.MeshData.ElementData{2}.ATTR.var='mat_axis';

for q=1:1:size(N1,1)
    febio_spec.MeshData.ElementData{2}.elem{q}.ATTR.lid=q;
    febio_spec.MeshData.ElementData{2}.elem{q}.a=N1(q,:);
    febio_spec.MeshData.ElementData{2}.elem{q}.d=N2(q,:);
end

%Boundary condition section
% -> Fix boundary conditions
febio_spec.Boundary.bc{1}.ATTR.type='fix';
febio_spec.Boundary.bc{1}.ATTR.node_set=nodeSetName1;
febio_spec.Boundary.bc{1}.dofs='x,y,z';

%Rigid section
% -> Prescribed rigid body boundary conditions
febio_spec.Rigid.rigid_constraint{1}.ATTR.name='RigidFix_1';
febio_spec.Rigid.rigid_constraint{1}.ATTR.type='fix';
febio_spec.Rigid.rigid_constraint{1}.rb=3;
febio_spec.Rigid.rigid_constraint{1}.dofs='Ry,Rz,Ru,Rv,Rw';

febio_spec.Rigid.rigid_constraint{2}.ATTR.name='RigidPrescribe';
febio_spec.Rigid.rigid_constraint{2}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{2}.rb=3;
febio_spec.Rigid.rigid_constraint{2}.dof='Rx';
febio_spec.Rigid.rigid_constraint{2}.value.ATTR.lc=1;
febio_spec.Rigid.rigid_constraint{2}.value.VAL=probeDisplacement;
febio_spec.Rigid.rigid_constraint{2}.relative=0;

%Contact section
febio_spec.Contact.contact{1}.ATTR.surface_pair=contactPairName1;
febio_spec.Contact.contact{1}.ATTR.type='sliding-elastic';
febio_spec.Contact.contact{1}.two_pass=0;
febio_spec.Contact.contact{1}.laugon=laugon;
febio_spec.Contact.contact{1}.tolerance=0.2;
febio_spec.Contact.contact{1}.gaptol=0;
febio_spec.Contact.contact{1}.minaug=minaug;
febio_spec.Contact.contact{1}.maxaug=maxaug;
febio_spec.Contact.contact{1}.search_tol=0.01;
febio_spec.Contact.contact{1}.search_radius=0.1*sqrt(sum((max(V,[],1)-min(V,[],1)).^2,2));
febio_spec.Contact.contact{1}.symmetric_stiffness=0;
febio_spec.Contact.contact{1}.auto_penalty=1;
febio_spec.Contact.contact{1}.penalty=contactPenalty;
febio_spec.Contact.contact{1}.fric_coeff=fric_coeff;

%LoadData section
% -> load_controller
febio_spec.LoadData.load_controller{1}.ATTR.id=1;
febio_spec.LoadData.load_controller{1}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{1}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{1}.points.point.VAL=[0 0; 1 1];

%Output section
% -> log file
febio_spec.Output.logfile.ATTR.file=febioLogFileName;
febio_spec.Output.logfile.node_data{1}.ATTR.file=febioLogFileName_disp;
febio_spec.Output.logfile.node_data{1}.ATTR.data='ux;uy;uz';
febio_spec.Output.logfile.node_data{1}.ATTR.delim=',';

%% Quick viewing of the FEBio input file structure
% The |febView| function can be used to view the xml structure in a MATLAB
% figure window.

%%
% |febView(febio_spec); %Viewing the febio file|

%% Exporting the FEBio input file
% Exporting the febio_spec structure to an FEBio input file is done using
% the |febioStruct2xml| function.

febioStruct2xml(febio_spec,febioFebFileName); %Exporting to file and domNode

%% Running the FEBio analysis
% To run the analysis defined by the created FEBio input file the
% |runMonitorFEBio| function is used. The input for this function is a
% structure defining job settings e.g. the FEBio input file name. The
% optional output runFlag informs the user if the analysis was run
% succesfully.

febioAnalysis.run_filename=febioFebFileName; %The input file name
febioAnalysis.run_logname=febioLogFileName; %The name for the log file
febioAnalysis.disp_on=1; %Display information on the command window
febioAnalysis.runMode='external';%'internal';

[runFlag]=runMonitorFEBio(febioAnalysis);%START FEBio NOW!!!!!!!!

%% Import FEBio results

if 1%runFlag==1 %i.e. a succesful run

    %%
    % Importing nodal displacements from a log file
    dataStruct=importFEBio_logfile(fullfile(savePath,febioLogFileName_disp),1,1);

    %Access data
    N_disp_mat=dataStruct.data; %Displacement
    timeVec=dataStruct.time; %Time

    %Create deformed coordinate set
    V_DEF=N_disp_mat+repmat(V,[1 1 size(N_disp_mat,3)]);

    %%
    % Plotting the simulated results using |anim8| to visualize and animate
    % deformations

    DN_magnitude=sqrt(sum(N_disp_mat(:,:,end).^2,2)); %Current displacement magnitude

    % Create basic view and store graphics handle to initiate animation
    hf=cFigure; hold on;
    gtitle([febioFebFileNamePart,': Press play to animate']);
    hp1=gpatch(Fb_blob_plot,V_DEF(:,:,end),DN_magnitude,'k',1); %Add graphics object to animate
    hp1.FaceColor='interp';
    hp2=gpatch(F_probe_plot,V_DEF(:,:,end),'kw','none',0.5); %Add graphics object to animate
%     gpatch(Fb_all,V,0.5*ones(1,3),'none',0.25); %A static graphics object

    axisGeom(gca,fontSize);
    colormap(gjet(250)); colorbar;
    caxis([0 max(DN_magnitude(Fb_blob_plot(:)))]); caxis manual;
    axis(axisLim(V_DEF)); %Set axis limits statically
    camlight headlight;
    view(15,8);
    drawnow;
    % Set up animation features
    animStruct.Time=timeVec; %The time vector
    for qt=1:1:size(N_disp_mat,3) %Loop over time increments
        DN_magnitude=sqrt(sum(N_disp_mat(:,:,qt).^2,2)); %Current displacement magnitude

        %Set entries in animation structure
        animStruct.Handles{qt}=[hp1 hp1 hp2]; %Handles of objects to animate
        animStruct.Props{qt}={'Vertices','CData','Vertices'}; %Properties of objects to animate
        animStruct.Set{qt}={V_DEF(:,:,qt),DN_magnitude,V_DEF(:,:,qt)}; %Property values for to set in order to animate
    end
    anim8(hf,animStruct); %Initiate animation feature
    drawnow;

end

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>

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


function [F,L,KL] = spm_vb_Fn(Y,block)
% Compute voxel-wise contributions to model evidence
% FORMAT [F,L,KL] = spm_vb_Fn(Y,block)
%
% Y          - [T x N] time series
% block      - data structure (see spm_vb_glmar)
%
% F          - [N x 1] vector where nth entry is unique contribution to
%              model evidence from voxel n
% L          - [N x 1] Average Likelihood
% KL.w       - [N x 1] KL w - unique contribution
% KL.a       - [N x 1] KL a - unique contribution
% KL.lam     - [N x 1] KL Lambda
% KL.alpha   - Scalar
% KL.beta    - Scalar
%__________________________________________________________________________
% Copyright (C) 2005-2014 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_vb_Fn.m 6079 2014-06-30 18:25:37Z spm $

T  = block.T;
p  = block.p;
k  = block.k;
N  = block.N;
X  = block.X;

C2 = block.C2;

Bk = kron(diag(block.mean_alpha),block.Dw);
B  = block.Hw*Bk*block.Hw';

if p>0
    if ~strcmp(block.priors.A,'Discrete')
        Jk = kron(diag(block.mean_beta),block.Da);
        J  = block.Ha*Jk*block.Ha';
    end
end

%-Get KL-alpha
%--------------------------------------------------------------------------
KL.alpha = 0;
for j=1:k
    KL.alpha = KL.alpha + spm_kl_gamma(block.b_alpha(j),block.c_alpha(j),block.b_alpha_prior(j),block.c_alpha_prior(j));
end

% Get KL-beta
%--------------------------------------------------------------------------
KL.beta = 0;
if p > 0
    if strcmp(block.priors.A,'Discrete')
        for j=1:p
            for s=1:block.priors.S
                KL.beta = KL.beta + spm_kl_gamma(block.b_beta(j,s),block.c_beta(j,s),block.b_beta_prior(j,s),block.c_beta_prior(j,s));
            end
        end
    else
        for j = 1:p
            KL.beta = KL.beta + spm_kl_gamma(block.b_beta(j),block.c_beta(j),block.b_beta_prior(j),block.c_beta_prior(j));
        end
    end
end

% Get average Likelihood, KL-Lambda and terms for KL-w, KL-a
%--------------------------------------------------------------------------
for n=1:N
    subblock_n = [(n-1)*k+1:n*k];

    if p>0
        Gn = spm_vb_get_Gn(Y,block,n);
    else
        wc = block.w_cov{n};
        en = (Y(:,n)-X*block.w_mean(subblock_n,1));
        Gn = trace(wc*block.XTX)+en'*en;
    end

    L(n)   = -0.5*block.mean_lambda(n)*Gn;
    L(n)   = L(n) + 0.5*(T-p)*(psi(block.c_lambda(n)) + log(block.b_lambda(n)));
    L(n)   = L(n)-0.5*block.C2/N;

    KL.lam(n) = spm_kl_gamma(block.b_lambda(n),block.c_lambda(n),block.b_lambda_prior(n),block.c_lambda_prior(n));

    KL_w1  = -0.5*sum(log(block.mean_alpha))-0.5*log(det(block.w_cov{n}));
    KL_w1  = KL_w1-0.5*k*block.log_det_Dw/N;
    KL_w2  = 0.5*trace(B(subblock_n,subblock_n)*block.w_cov{n});

    subblock_ni = [1:N*k];
    subblock_ni(subblock_n) = [];
    Bnn         = B(subblock_n,subblock_n);
    Bni         = B(subblock_n,subblock_ni);
    Bin         = B(subblock_ni,subblock_n);

    w_mean  = block.w_mean(subblock_n,1);
    KL_w3   = 0.5*w_mean'*Bnn*w_mean+0.5*w_mean'*Bni*block.w_mean(subblock_ni,1);
    KL_w3   = KL_w3-0.5*k;
    KL.w(n) = KL_w1+KL_w2+KL_w3;

    if p>0
        asubblock_n = [(n-1)*p+1:n*p];
        a_mean      = block.a_mean(asubblock_n,1);
        if strcmp(block.priors.A,'Discrete')
            ibeta_n = diag(block.priors.gamma(n,:)*(1./block.mean_beta'));
            a_n     = block.priors.gamma(n,:)*block.as';
            KL.a(n) = spm_kl_normal(a_mean,block.a_cov{n},a_n,ibeta_n);
%             a_mean
%             sqrt(block.a_cov{n})
%             a_n
%             sqrt(ibeta_n)
        else
            asubblock_ni = [1:N*p];
            asubblock_ni(asubblock_n) = [];
            Jnn = J(asubblock_n,asubblock_n);
            Jni = J(asubblock_n,asubblock_ni);
            KL_a1 = -0.5*sum(log(block.mean_beta)) - 0.5*log(det(block.a_cov{n}));
            KL_a1 = KL_a1-0.5*p*block.log_det_Da/N;
            KL_a2 = 0.5*trace(Jnn*block.a_cov{n});
            KL_a3 = 0.5*a_mean'*Jnn*a_mean+0.5*a_mean'*Jni*block.a_mean(asubblock_ni,1);
            KL_a3 = KL_a3-0.5*p;
            KL.a(n) = KL_a1 + KL_a2 + KL_a3;
        end
    else
        KL.a(n) = 0;
    end
end

F = L -KL.w  -KL.lam -KL.a -KL.alpha/N -KL.beta/N;


tic;
neuron.options.nk = 1; %round(T/(60*neuron.Fs)); % number of knots for spline basis, the interval between knots is 180 seconds
nr_patch = str2double(get(edit_nr, 'string'));
nc_patch = str2double(get(edit_nc, 'string'));

patch_par = round([nr_patch, nc_patch]); %1;  % divide the optical field into 3 X 3 patches and do initialization patch by patch
K = str2double(get(edit_K, 'string'));  % maximum number of neurons to search within each patch. you can use [] to search the number automatically
neuron.options.bd = 0; % boundaries to be removed due to motion correction
[center, Cn, pnr] = neuron.initComponents_endoscope(Y, K, patch_par, debug_on, save_avi); %#ok<ASGLU>
figure;
imagesc(Cn);
hold on; plot(center(:, 2), center(:, 1), 'or');
colormap; axis off tight equal;

[~, srt] = sort(max(neuron.C, [], 2)./get_noise_fft(neuron.C), 'descend');
neuron.orderROIs(srt);
neuron_init = neuron.copy();


% Locate place fields in a 1D firing map.
%
% Locates the place fields and calculates the field sizes and spacing
% between fields.
%
%  USAGE
%   [fieldsCurve, fields, spacing] = analyses.placefield1D(map, <options>)
%   map           Firing rate map either structure obtained using analyses.map
%                 or a matrix that represents firing map. Note that if you want to filter place fields based on number
%                 of spikes, then you should use map structure.
%   <options>     optional list of property-value pairs (see table below)
%
%   ======================================================================================
%    Properties     Values
%   --------------------------------------------------------------------------------------
%    'thresholdType' Type of the firing rate threshold. '%' specifies that 'threshold'
%                    value is in range [0; 1] and rates lower than 'threshold * curveAmplitude'
%                    are not considered as place fields. curveAmplitude is the range of a firing
%                    curve, i.e. max(curve) - min(curve).
%                    'rate' specifies that 'threshold' value is an absolute rate. Values lower than
%                    'threshold' are not considered as place fields. Default it '%'.
%
%    'threshold'    Threshold for a firing rate. How the value is treated depends on options
%                   'thresholdType'. Default = 0.2 and 'thresholdType' = '%'.
%
%    'minRows'      Minimum number of row bins in a place field. Fields with
%                   fewer bins are not considered as place fields. Remember to
%                   adjust this value when you change the bin width. Default = 3.
%                   If minRows == 0, then minimum number of bins is not checked.
%
%    'binWidth'     Bin length in centimetres. Used to calculate size of fields in cm.
%                   Default is 1.
%
%    'minSpikes'    Minimum number of spikes in a place field. Default is 0, which means
%                   that minimum number of spikes is not checked. To use this value 'map'
%                   must be a result of function analyses.map.
%
%    'minDistance'  Minimum number of bins between adjacent fields. Distance is calculated between
%                   end of one field and start of another field. If distance is smaller or
%                   equals to 'minDistance', then two fields are merged together. Note that more
%                   than two fields can be merged if they are all adjacent. Default is 0, which
%                   means that fields should have a common border.
%
%    'debug'        'on' plot turning curve, place fields and their centres of mass. 'off' do not
%                   plot anything. Default is 'off'.
%
%    'pos'          Position samples. Used to calculate posInd. If not provided,
%                   then posInd will be an empty matrix.
%   ======================================================================================
%
%   fieldsCurve     Vector of the same size as underline firing rate curve. The elements of fieldsCurve
%                   are integer values greater or equal to 0. 0 elements correspond to background (not
%                   a place field). Values of 1 constitute first field. Values of 2 constitute second
%                   field; and so on.
%   fields          Structure with information about each field. Structure fields are:
%       col         vector of columns that constitute this field.
%       row         vector of rows (artificial). Same size as col, all ones;
%       peak        field peak;
%       x, y        field centre of mass point. See NOTES for details about centre of mass calculation;
%       meanRate    mean firing rate;
%       width       field width in cm;
%       size        size of the field. Do not rely on it! It's from old code and could be misleading.
%                   Calculated as width * <mean firing rate of a field>.
%       posInd      Indices of position samples that correspond to this field. In case position sample
%                   matrix is of size Nx5 ([t x y x1 y1]), posInd corresponds to the left most position
%                   columns ([x y]). If pos argument is not provided, then posInd will be an empty matrix.
%   spacing         Matrix of spacing between fields. Entry (i, j) equals to distance between fields i and j.
%
%  NOTE
%   Since centre of mass for a curve is somewhat undefined, it is calculated for plate bounded by a firing curve
%   and a function y = 0 for all x. See http://tutorial.math.lamar.edu/Classes/CalcII/CenterOfMass.aspx
%
function [fieldsCurve, fields, spacing] = placefield1D(mapS, varargin)
    if nargin < 1 || mod(length(varargin), 2) ~= 0
        error('BNT:numArgs', 'Incorrect number of parameters (type ''help analyses.placefield1D'' for details).');
    end

    % Default values
    threshold = 0.1;
    minRows = 2;
    binWidth = 1; % [cm];
    minSpikes = 0;
    minFieldDistance = 0; % [bins]
    debug = false;
    isThrshPercentage = true;
    pos = [];

    % Parse parameter list
    i = 1;
    while i < length(varargin)
        if ~ischar(varargin{i}),
            error(['Parameter ' num2str(i+2) ' is not a property (type ''help analyses.placefield1D'' for details).']);
        end

        switch(lower(varargin{i})),
            case 'threshold'
                threshold = varargin{i+1};
                i = i + 2;

            case 'minrows'
                minRows = varargin{i+1};
                if ~helpers.isdscalar(minRows, '>=0')
                    error('Incorrect value for property ''minRows'' (type ''help analyses.placefield1D'' for details).');
                end
                i = i + 2;

            case 'binwidth'
                binWidth = varargin{i+1};
                if ~helpers.isdscalar(binWidth, '>0')
                    error('Incorrect value for property ''binWidth'' (type ''help analyses.placefield1D'' for details).');
                end
                i = i + 2;

            case 'minspikes'
                minSpikes = varargin{i+1};
                if ~helpers.isdscalar(minSpikes, '>=0')
                    error('Incorrect value for property ''minSpikes'' (type ''help analyses.placefield1D'' for details).');
                end
                if minSpikes > 0 && ~isstruct(mapS)
                    error('You should only use minSpikes with a structure-like firing curve (type ''help analyses.placefield1D'' for details).');
                end
                i = i + 2;

            case 'mindistance'
                minFieldDistance = varargin{i+1};
                if ~helpers.isdscalar(minFieldDistance, '>=0')
                    error('Incorrect value for property ''minDistance'' (type ''help analyses.placefield1D'' for details).');
                end
                i = i + 2;

            case 'debug'
                debug = strcmpi(varargin{i+1}, 'on');
                i = i + 2;

            case 'thresholdtype'
                isThrshPercentage = strcmpi(varargin{i+1}, '%');
                if ~helpers.isstring(varargin{i+1}, '%', 'rate'),
                    error('Incorrect value for property ''thresholdType'' (type ''help analyses.placefield1D'' for details).');
                end
                i = i + 2;

            case 'pos'
                pos = varargin{i+1};
                if ~ismatrix(pos) || size(pos, 2) < 2
                    error('Incorrect value for property ''pos'' (type ''help analyses.placefield1D'' for details).');
                end
                i = i + 2;

            otherwise,
                error(['Unknown property ''' num2str(varargin{i}) ''' (type ''help analyses.placefield1D'' for details).']);
        end
    end
    if isThrshPercentage
        if ~helpers.isdscalar(threshold, '>=0', '<=1')
            error('Incorrect value for property ''threshold'' (type ''help analyses.placefield1D'' for details).');
        end
    else
        if ~helpers.isdscalar(threshold, '>=0')
            error('Incorrect value for property ''threshold'' (type ''help analyses.placefield1D'' for details).');
        end
    end

    if isstruct(mapS)
%         mapAxis = mapS.x;
        if size(mapS.z, 1) > 1 && size(mapS.z, 2) > 1
            error('Incorrect value for property ''map''. Firing curve is 2D, but should be a vector (type ''help analyses.placefield1D'' for details).');
        end

        curve = mapS.z;
    else
        if size(mapS, 1) > 1 && size(mapS, 2) > 1
            error('Incorrect value for property ''map''. It should be a vector (type ''help analyses.placefield1D'' for details).');
        end
%         mapAxis = 1:length(mapS);

        curve = mapS;
    end
    curveLen = length(curve);

    fieldsCurve = zeros(1, curveLen);
    fields = struct('row', {}, 'col', {}, ...
        'size', {}, 'peak', {}, ...
        'x', {}, 'y', {}, ...
        'meanRate', {}, 'width', {}, ...
        'posInd', {} ...
        );
    spacing = [];

    if curveLen == 0
        return
    end

    % extend curve, so that findpeaks function finds all peaks
    map = zeros(1, curveLen + 2);
    map(2:2+curveLen-1) = curve;

    [~, locs] = findpeaks(map);
    dMap = diff(map);
    dMap(end+1) = dMap(end);

    if isThrshPercentage
        threshold = min(curve) + ((max(curve) - min(curve)) * threshold);
    end

    startField = zeros(1, 100);
    stopField = zeros(1, 100);
    fieldWidth = zeros(1, 100);
    fieldSize = zeros(1, 100);
    curField = 1;

    for i = 1:length(locs)
        if map(locs(i)) < threshold
            continue;
        end
        curStopField = locs(i);
        while curStopField < length(dMap) && dMap(curStopField) < 0 && map(curStopField) >= threshold
            curStopField = curStopField + 1;
        end
        % substract one because of the loop
        curStopField = curStopField -1;

        curStartField = locs(i) - 1;
        while curStartField > 0 && dMap(curStartField) > 0 && map(curStartField) >= threshold
            curStartField = curStartField - 1;
        end
        if curStartField == 0
            curStartField = 1;
        end
        curStopField = curStopField - 1; % substract one cause of map -> curve translation

        if curStopField > length(curve)
            curStopField = length(curve);
        end

        startField(curField) = curStartField;
        stopField(curField) = curStopField;

        fieldWidth(curField) = (curStopField - curStartField + 1) * binWidth;
        fieldSize(curField) = fieldWidth(curField) * nanmean(curve(curStartField:curStopField));
        curField = curField + 1;
    end

    startField(curField:end) = [];
    stopField(curField:end) = [];
    fieldWidth(curField:end) = [];
    fieldSize(curField:end) = [];

    nFields = length(fieldWidth);
    removeField = false(1, nFields);

    fieldsCurve = zeros(1, curveLen);
    if nFields == 0
        fields = struct();
        fields(1) = [];
        spacing = [];
        return;
    end

    % merge neighbouring fields
    curField = 1;
    for i = 1:nFields
        if i + 1 > nFields
            break;
        end

        fieldsDist = abs(startField(i+1) - stopField(i));
        if fieldsDist <= minFieldDistance
            stopField(curField) = stopField(i+1);
            removeField(i+1) = true;
        else
            curField = i + 1;
        end
    end
    startField(removeField) = [];
    stopField(removeField) = [];
    fieldWidth(removeField) = [];
    fieldSize(removeField) = [];

    if ~isempty(find(removeField, 1))
        % adjust fields width and size after merge
        nFields = length(startField);
        for i = 1:nFields
            fieldWidth(i) = (stopField(i) - startField(i) + 1) * binWidth;
            fieldSize(i) = fieldWidth(i) * nanmean(curve(startField(i):stopField(i)));
        end
    end

%     fields(nFields) = struct('row', {}, 'col', {}, ...
%         'size', {}, 'peak', {}, ...
%         'x', {}, 'y', {}, ...
%         'meanRate', {}, 'width', {}, ...
%         'posInd', {} ...
%         );
%     fields(nFields) = struct('row', [], 'peak', 0, 'x', -1, 'y', -1, 'meanRate', -1, 'posInd', []);

    curField = 1;
    removeField = false(1, nFields);
    for i = 1:nFields
        fieldData = curve(startField(i):stopField(i));
        peak = max(fieldData);

        % check field peak rate
        if peak < threshold
            removeField(i) = true;
            continue;
        end

        % Number of spikes in the field
        if minSpikes > 0
            numSpikes = sum(mapS.Nspikes(startField(i):stopField(i)));
            if numSpikes < minSpikes
                % Marks field as having to few spikes
                removeField(i) = true;
                continue;
            end
        end

        % check number of bins
        if length(startField(i):stopField(i)) <= minRows
            removeField(i) = true;
            continue;
        end

        fields(curField).col = startField(i):stopField(i);
        fields(curField).row = ones(1, length(fields(curField).col));
        fields(curField).peak = peak;
        fields(curField).meanRate = nanmean(fieldData);
        fields(curField).width = fieldWidth(i);
        fields(curField).size = fieldSize(i);
        fieldsCurve(startField(i):stopField(i)) = curField;

        if ~isempty(pos)
            if isstruct(mapS)
                xSpace = mapS.x;
            else
                nBins = length(curve);
                limitsX = [nanmin(pos(:, bntConstants.PosX)) nanmax(pos(:, bntConstants.PosX))];
                xSpace = linspace(limitsX(1), limitsX(2), nBins);
            end
            xMin = xSpace(startField(i));
            xMax = xSpace(stopField(i));
            posIndX = pos(:, bntConstants.PosX) >= xMin & pos(:, bntConstants.PosX) <= xMax;
            fields(curField).posInd = find(posIndX);
        end

        curField = curField + 1;
    end

    fields(curField:end) = [];
    nFields = length(fields);

    if debug && nFields > 0
        figure, plot(curve);
        v = axis();
        ymin = v(3);
        ymax = v(4);
        color = [1 0 0];
        fieldSize = zeros(1, nFields);
        for i = 1:nFields
            startField = fields(i).col(1);
            stopField = fields(i).col(end);

            p = patch([startField startField stopField stopField], [ymax ymin ymin ymax], color);
            set(p, 'FaceAlpha', 0.5);
            color(1) = color(1) - 0.1;
            color(2) = color(2) + 0.2;
%             color(3) = color(3) + 0.2;
            if color(1) < 0
                color(1) = 0;
            end
            if color(2) > 1
                color(2) = 1;
            end

            fieldSize(i) = fields(i).size;
        end
        xlabel('Bins'), ylabel('Rate');
        titleStr = sprintf('Field(s) with size(s) %s', sprintf('%f, ', fieldSize));
        titleStr(end-1:end) = [];
        title(titleStr);
    end

    % Calculate the spacing using centre of mass (COM). Since centre of mass for a curve is somewhat
    % undefined I'll calculate COM for palte bounded by curve and a function y = 0 for all x.
    % http://tutorial.math.lamar.edu/Classes/CalcII/CenterOfMass.aspx
    distMat = zeros(nFields, 2);

    % store points for debug purposes
    pointsX = zeros(1, nFields);
    pointsY = zeros(1, nFields);

    for i = 1:nFields
        x = fields(i).col(:);
        y = curve(x);
        y = y - min(y); % make it from 0. This gives better estimation

        y1 = x .* y;
        y2 = (y.^2)/2;
        A = trapz(x, y);

        Mx = trapz(x, y1)/A;
        My = min(curve(x)) + trapz(x, y2)/A;

        pointsX(i) = Mx;
        pointsY(i) = My;

        fields(i).x = Mx;
        fields(i).y = My;

        distMat(i, :) = [Mx My];

%             % Raymond's code
%             posMass = sum(mapAxis(x).*curve(x));
%             mass = sum(curve(x));
%             com(i) = posMass/mass;
    end

    if debug
        hold on;
        plot(pointsX, pointsY, 'o');
    end

    if nFields <= 1
        spacing = [];
    else
        D = pdist(distMat);
        spacing = squareform(D);
    end
end


function i4block_print_test ( )

%*****************************************************************************80
%
%% I4BLOCK_PRINT_TEST tests I4BLOCK_PRINT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 June 2012
%
%  Author:
%
%    John Burkardt
%
  l = 4;
  m = 3;
  n = 2;
  xvec = [ ...
        1,  2,  3,   4,  1, ...
        4,  9, 16,   1,  8, ...
       27, 64,  2,   4,  6, ...
        8,  2,  8,  18, 32, ...
        2, 16, 54, 128 ];

  x = reshape ( xvec, l, m, n );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'I4BLOCK_PRINT_TEST\n' );
  fprintf ( 1, '  I4BLOCK_PRINT prints an I4BLOCK.\n' );

  i4block_print ( l, m, n, x, '  The 3D array:' )

  return
end


clear;
%% load file (courtesy of C. Lacefield and R. Bruno, Columbia University)

addpath(genpath('../../ca_source_extraction'));
nam = 'dendritic_demo.tif';
if ~exist(nam,'file')  % download file if it doesn't exist in the directory
    url = 'https://www.dropbox.com/s/wykkq4lwoo85ml5/dendritic_demo.tif.zip?dl=1';
    filename = 'dendritic_demo.zip';
    outfilename = websave(filename,url);
    unzip(filename);
end

Y = bigread2(nam);
Y = Y - min(Y(:));
if ~isa(Y,'double');    Y = double(Y);  end         % convert to double

[d1,d2,T] = size(Y);                                % dimensions of dataset
d = d1*d2;                                          % total number of pixels

%% view data

nY = quantile(Y(:),0.01);
mY = quantile(Y(:),0.995);

figure;
for t = 1:4:T
    imagesc(Y(:,:,t),[nY,mY]);
    title(sprintf('Frame %i out of %i',t,T),'fontweight','bold','fontsize',14);
    drawnow;
    pause(0.01);
end

%% Set parameters

K = 50;                                           % number of components to be found
tau = [];                                         % std of gaussian kernel (size of neuron - not needed for dendritic data)
p = 0;                                            % No AR dynamics for dendritic data
merge_thr = 0.8;                                  % merging threshold

options = CNMFSetParms(...
    'd1',d1,'d2',d2,...                           % dimensions of datasets
    'init_method','HALS',...                      % initialize algorithm with plain NMF
    'max_iter_hals_in',50,...                     % maximum number of iterations
    'search_method','dilate',...                  % search locations when updating spatial components
    'temporal_iter',2,...                         % number of block-coordinate descent steps
    'merge_thr',0.8,...                           % merging threshold
    'conn_comp',false,...                         % do not limit to largest connected component for each found component
    'maxthr',0.05...                              % for every component set pixels that are below max_thr*max_value to 0
    );
%% Data pre-processing

[P,Y] = preprocess_data(Y,p);

%% fast initialization of spatial components using greedyROI and HALS

[Ain,Cin,bin,fin,center] = initialize_components(Y,K,tau,options,P);  % initialize

Yr = reshape(Y,d,T);
plot_dend_components_GUI(Yr,Ain,Cin,bin,fin,options);  % view the components

%% update spatial components

[A,b,Cin] = update_spatial_components(Yr,Cin,fin,[Ain,bin],P,options);

%% update temporal components

[C,f,P,S] = update_temporal_components(Yr,A,b,Cin,fin,P,options);

%% merge found components
[Am,Cm,K_m,merged_ROIs,P,Sm] = merge_components(Yr,A,b,C,f,P,S,options);

display_merging = 1; % flag for displaying merging example
if and(display_merging, ~isempty(merged_ROIs))
    i = 1; %randi(length(merged_ROIs));
    ln = length(merged_ROIs{i});
    figure;
        set(gcf,'Position',[300,300,(ln+2)*300,300]);
        for j = 1:ln
            subplot(1,ln+2,j); imagesc(reshape(A(:,merged_ROIs{i}(j)),d1,d2));
                title(sprintf('Component %i',j),'fontsize',16,'fontweight','bold'); axis equal; axis tight;
        end
        subplot(1,ln+2,ln+1); imagesc(reshape(Am(:,K_m-length(merged_ROIs)+i),d1,d2));
                title('Merged Component','fontsize',16,'fontweight','bold');axis equal; axis tight;
        subplot(1,ln+2,ln+2);
            plot(1:T,(diag(max(C(merged_ROIs{i},:),[],2))\C(merged_ROIs{i},:))');
            hold all; plot(1:T,Cm(K_m-length(merged_ROIs)+i,:)/max(Cm(K_m-length(merged_ROIs)+i,:)),'--k')
            title('Temporal Components','fontsize',16,'fontweight','bold')
        drawnow;
else
    fprintf('No components were merged. \n')
end

%% order and extract DF/F

[A_or,C_or,S_or,P] = order_ROIs(A,C,S,P); % order components
K_m = size(C_or,1);
[C_df,~] = extract_DF_F(Yr,[A_or,b],[C_or;f],K_m+1); % extract DF/F values (optional)

%% display components

plot_dend_components_GUI(Yr,A_or,C_or,b,f,options)

%% make movie

make_dendritic_video(A_or,C_or,b,f,Yr,d1,d2)


function RtOut = inverseRt(RtIn)

RtOut = [RtIn(1:3,1:3)', - RtIn(1:3,1:3)'* RtIn(1:3,4)];


%VAR Datafile overload
%
%   [V,U] = VAR(A,W)
%
% A - Datafile
% W - Vector of weights, one per object
%
% V - Vector of feature variances
% U - Mean vector
%
% Computes variance V and mean U in a single run for speed.
% Objects are assumed to have the same number of features.
% Take care that the feature size of A has been correctly set.
% The routine is useful in case the data is too large to be
% converted to a dataset first.


% op_alignAverages.m
% Jamie Near, McGill University 2014.
%
% USAGE:
% [out,fs,phs]=op_alignAverages(in,tmax,med,ref);
%
% DESCRIPTION:
% Perform spectral registration in the time domain to correct frequency and
% phase drifts.  As described in Near et al.  Frequency and phase drift
% correction of magnetic resonance spectroscopy data by spectral
% registration in the time domain. Magn Reson Med 2015; 73(1):44-50.
%
% June 15th 2017:  Made the tmax and med arguments optional.  If tmax is
% not specified, the value is determined automatically by finding the time
% at which the SNR of the FID drops permanently below 5.  This idea
% was suggested by Mark Mikkelsen.  Thanks Mark!!
%
% INPUTS:
% in        = Input data structure.
% tmax      = Maximum time (s) in time domain to use for alignment.
%             (Optional.  Default is the time at which SNR drops below 5)
% med       = Align averages to the median of the averages? ('y','n', 'a', or
%             'r').  (Optional.  Default = 'n').  If you select 'n', all
%             averages will be aligned to a single average.  The average
%             chosen as the reference average will be the one with the
%             lowest 'unlikeness' metric (see 'op_rmbadaverages.m').  If
%             select 'y', all averages will be alinged to the median of
%             the averages.  If you select 'a', all averages will be
%             aligned to the average of the averages.  If you select 'r',
%             all averages will be aligned to an externally provided
%             reference spectrum.
% ref       = An externally provided reference spectrum that you would like
%             to align everything to (Required only if med = 'r').
%
% OUTPUTS:
% out       = Output following alignment of averages.
% fs        = Vector of frequency shifts (in Hz) used for alignment.
% phs       = Vector of phase shifts (in degrees) used for alignment.

function [out,fs,phs]=op_alignAverages(in,tmax,med,initPars)

if ~in.flags.addedrcvrs
    error('ERROR:  I think it only makes sense to do this after you have combined the channels using op_addrcvrs.  ABORTING!!');
end

if in.dims.averages==0
    %DO NOTHING
    disp('WARNING:  No averages found.  Returning input without modification!');
    out=in;
    fs=0;
    phs=0;

else

    parsFit=[0,0];

    if nargin<3
        med='n'
        if nargin<2
            %if tmax is not specified, find the time at which the SNR
            %drops below 5
            disp('tmax not supplied.  Calculating tmax....');
            sig=abs(in.fids);
            noise=std(real(in.fids(ceil(0.75*end):end,:,:)),[]);
            noise=mean(mean(mean(noise,2),3),4);
            snr=sig/noise;

            for n=1:(numel(snr)/size(snr,1))
                N=find(snr(:,n)>5);
                tmax_est(n)=in.t(N(end));
            end
            tmax=median(tmax_est);
            disp(['tmax = ' num2str(tmax*1000) 'ms.']);
        end
    end

    if (strcmp(med,'r') || strcmp(med,'R'))
        if nargin<4
            error('ERROR:  If using the ''r'' option for input variable ''med'', then a 4th input argument must be provided');
        end
    else
        if nargin<4
            ref=struct();
        end
    end

    if in.dims.subSpecs==0
        B=1;
    else
        B=in.sz(in.dims.subSpecs);
    end

    fs=zeros(in.sz(in.dims.averages),B);
    phs=zeros(in.sz(in.dims.averages),B);
    fids=zeros(in.sz(in.dims.t),1,B);
    for m=1:B
        if med=='y' || med=='Y'
            disp('Aligning all averages to the median of the averages.');
            base=op_median(in);
            base=[real(base.fids( in.t>=0 & in.t<tmax ,m));imag(base.fids( in.t>=0 & in.t<tmax ,m))];
            ind_min=0;
        elseif med=='a' || med=='a'
            disp('Aligning all averages to the average of the averages.');
            base=op_averaging(in);
            base=[real(base.fids( in.t>=0 & in.t<tmax ,m));imag(base.fids( in.t>=0 & in.t<tmax ,m))];
            ind_min=0;
        elseif med=='n' || med=='N'
            %First find the average that is most similar to the total average:
            inavg=op_median(in);
            for k=1:in.sz(in.dims.averages)
                for l=1:B
                    metric(k,l)=sum((real(in.fids(in.t>=0 & in.t<=tmax,k,l))-(real(inavg.fids(inavg.t>=0 & inavg.t<=tmax,l)))).^2);
                end
            end
            [temp,ind_min]=min(metric(:,m));

            %Now set the base function using the index of the most similar
            %average:
            disp(['Aligning all averages to average number ' num2str(ind_min) '.']);
            base=[real(in.fids(in.t>=0 & in.t<tmax,ind_min,m));imag(in.fids(in.t>=0 & in.t<tmax,ind_min,m))];
            fids(:,ind_min,m)=in.fids(:,ind_min,m);
        elseif med=='r' || med=='R'
            disp('Aligning all averages to an externally provided reference spectrum.');
            base=ref;
            base=[real(base.fids( in.t>=0 & in.t<tmax ,m));imag(base.fids( in.t>=0 & in.t<tmax ,m))];
            ind_min=0;
        end
        for n=1:in.sz(in.dims.averages)
            if n~=ind_min
                parsGuess=parsFit;
                %disp(['fitting subspec number ' num2str(m) ' and average number ' num2str(n)]);
                parsFit=nlinfit(in.fids(in.t>=0 & in.t<tmax,n,m),base,@op_freqPhaseShiftComplexNest,parsGuess);
                fids(:,n,m)=op_freqPhaseShiftNest(parsFit,in.fids(:,n,m));
                fs(n,m)=parsFit(1);
                phs(n,m)=parsFit(2);
                %plot(in.ppm,fftshift(ifft(fids(:,1,m))),in.ppm,fftshift(ifft(fids(:,n,m))));
            end
        end
    end

    %re-calculate Specs using fft
    specs=fftshift(ifft(fids,[],in.dims.t),in.dims.t);

    %FILLING IN DATA STRUCTURE
    out=in;
    out.fids=fids;
    out.specs=specs;

    %FILLING IN THE FLAGS
    out.flags=in.flags;
    out.flags.writtentostruct=1;
    out.flags.freqcorrected=1;

end

    function y=op_freqPhaseShiftComplexNest(pars,input)
        f=pars(1);     %Frequency Shift [Hz]
        p=pars(2);     %Phase Shift [deg]

        dwelltime=in.dwelltime;
        t=0:dwelltime:(length(input)-1)*dwelltime;
        fid=input(:);

        shifted=addphase(fid.*exp(1i*t'*f*2*pi),p);

        y=[real(shifted);imag(shifted)];
        %y=real(fid.*exp(-1i*t'*f*2*pi));

    end

    function y=op_freqPhaseShiftNest(pars,input)
        f=pars(1);     %Frequency Shift [Hz]
        p=pars(2);     %Phase Shift [deg]

        dwelltime=in.dwelltime;
        t=0:dwelltime:(length(input)-1)*dwelltime;
        fid=input(:);

        y=addphase(fid.*exp(1i*t'*f*2*pi),p);
        %y=real(fid.*exp(-1i*t'*f*2*pi));

    end

end


function [posterior, out] = VBA_multisession_factor(posterior,out)
% [posterior, out] = VBA_multisession_factor(posterior,out)
% take a multisession model, extract within session parameters and
% trajectories (in posterior.perSession), and clean up the out structure
% by undoing the multisession wrapping performed by VBA_multisession_expand.

%% skip if no multisession
if ~(isfield(out.options,'multisession') && isfield(out.options.multisession,'expanded') && out.options.multisession.expanded)
    return;
end

%% extract session_wise posteriors
Ts = [0 cumsum(out.options.multisession.split)];

multisession = out.options.inF{2};
out.options.inF = out.options.inF{1:end-1};
out.options.inG = out.options.inG{1:end-1};

out.options.f_fname = multisession.f_fname;
out.options.g_fname = multisession.g_fname;

for i=1:numel(out.options.multisession.split)

    idx_X0 = multisession.indices.X0(:,i);
    idx_theta = multisession.indices.theta(:,i);
    idx_phi = multisession.indices.phi(:,i);

    posteriors(i).muX0 = posterior.muX0(idx_X0);
    posteriors(i).SigmaX0 = posterior.SigmaX0(idx_X0,idx_X0);

    posteriors(i).muTheta = posterior.muTheta(idx_theta);
    posteriors(i).SigmaTheta = posterior.SigmaTheta(idx_theta,idx_theta);

    posteriors(i).muPhi = posterior.muPhi(idx_phi);
    posteriors(i).SigmaPhi = posterior.SigmaPhi(idx_phi,idx_phi);

    if isfield(posterior,'a_sigma')
        posteriors(i).a_sigma =  posterior.a_sigma ;
        posteriors(i).b_sigma =  posterior.b_sigma ;
    end
    if isfield(posterior,'a_alpha')
        posteriors(i).a_alpha =  posterior.a_alpha ;
        posteriors(i).b_alpha =  posterior.b_alpha ;
    end

    idx_t = (Ts(i)+1):Ts(i+1);

    posteriors(i).iQy = posterior.iQy(idx_t,:);

    try
    posteriors(i).iQx =  posterior.iQx(idx_t);%cellfun(@(Q) Q(idx_X0,idx_X0),posterior.iQx(idx_t),'UniformOutput',false);
    posteriors(i).muX = posterior.muX(idx_X0,idx_t);
    posteriors(i).SigmaX.current =  cellfun(@(Q) Q(idx_X0,idx_X0),posterior.SigmaX.current(idx_t,:),'UniformOutput',false);
    posteriors(i).SigmaX.inter =  cellfun(@(Q) Q(idx_X0,idx_X0),posterior.SigmaX.inter(:,idx_t),'UniformOutput',false);
    end
end

posterior.perSession = posteriors;

out.options.multisession.expanded = false ;

end


% Library for Quadratic Programming.
%
% Copyright (C) 2006-2008 Vojtech Franc, xfrancv@cmp.felk.cvut.cz
% Center for Machine Perception, CTU FEL Prague
%
%
% libqp_gsmo         Generalized Sequential Minimal Optimizer (SMO) algorithm.
% libqp_splx         Solves Quadratic Programming task with simplex constraints.
%
% libqp_compile      Compiles MEX interface for LIBQP solvers.
%
% libqp_gsmo_test    Test script for LIBQP_GSMO solver.
% libqp_splx_test    Test script for LIBQP_SPLX solver.
%


function gf=gradloglikGaP(x, varargin)
% f=loglikGaP(x, varargin)
% complete log likellihood of the GaP model
% Vxt = varargin{1};      %data
% sigpsf = varargin{2};  %std deviation of the PSF gaussian approx
% alpha = varargin{3}; %parameters of the Gamma prior on the blinking
% beta = varargin{4}; %parameters of the Gamma prior on the blinking
% peval = varargin{5}; %parameters
% x(1:end-2*peval.ncomp) is Hkt

Vxt = varargin{1};      %data
sigpsf = varargin{2};  %std deviation of the PSF gaussian approx
alpha = varargin{3}; %parameters of the Gamma prior on the blinking
beta = varargin{4}; %parameters of the Gamma prior on the blinking
peval = varargin{5}; %parameters

[Hkt, cx, cy, Wxk] = loglikGaPreadparam(x,varargin);

[Wxkbg,Hktbg]=addbg(Wxk, Hkt, peval.bg);
Hkt=max(Hkt,eps); % adjust small values to avoid undeflow
P=Wxkbg*Hktbg; %current approximation

%linear grasdient shifted by cx
xxvc = lineargrad([peval.nx, peval.ny, peval.ncomp], cx, 'xx');
yyvc = lineargrad([peval.nx, peval.ny, peval.ncomp], cy, 'yy');

% dW/dcx:
Wxtcx=1/sigpsf^2*xxvc.*Wxk;
% dW/dcy:
Wxtcy=1/sigpsf^2*yyvc.*Wxk;

% d(log(L))/dHkt:
% gfHkt=(alpha-1)*1./Hkt - 1/beta*Hkt + Wxk'*(Vxt./P)-1;
gfHkt= Wxk'*(Vxt./P)-1; %without background (->not Wxkgb) and d(log(L)/dcx)
% d(log(L))/dcx:
gfcx=diag(Wxtcx'*(Vxt./P-ones(peval.nx*peval.ny, peval.nt))*Hkt');
% d(log(L))/dcy:
gfcy=-diag(Wxtcy'*(Vxt./P-ones(peval.nx*peval.ny, peval.nt))*Hkt');

% gf = [reshape(gfHkt,1,peval.nt*peval.ncomp), gfcx', gfcy'];
gf = [reshape(gfHkt,1,peval.nt*peval.ncomp)];
% gf = [gfcx', gfcy'];
gf=-gf; %conjugate gradient is minimizing!
end
function Wnorm=normalizePSF(W)
sw=size(W);
Wr=reshape(W, sw(1)*sw(2),sw(3));
q=squeeze(sum(Wr,1));
Wrnorm=Wr./repmat(q,sw(1)*sw(2),1);
Wnorm=reshape(Wrnorm,sw(1), sw(2), sw(3));
end
function xxvc = lineargrad(sizevec, cx, dir)
switch dir
    case 'xx'
%         xxp=double(xx(sizevec, 'true')); %linear function - pixels
        xxp=double(xx(sizevec, 'corner')); %linear function - pixels
    case 'yy'
%         xxp=double(yy(sizevec, 'true')); %linear function - pixels
        xxp=double(yy(sizevec, 'corner')); %linear function - pixels
    otherwise
        error('Wrong dir')
end

xxv=reshape(xxp,sizevec(1)*sizevec(2),sizevec(3)); %linear function - vector
xxvc=xxv-repmat(cx,sizevec(1)*sizevec(2),1);
% xxp=double(xx([peval.nx, peval.ny, peval.ncomp], 'true')); %linear function - pixels
%yyp=double(yy([peval.nx, peval.ny, peval.ncomp], 'true'));
% xxv=reshape(xxp,peval.nx*peval.ny,peval.ncomp); %linear function - vector
%yyv=reshape(yyp,peval.nx*peval.ny,peval.ncomp);
% xxvc=xxv-repmat(cx,peval.ncomp,1);
%yyvc=yyv-repmat(cy,peval.ncomp,1);
end