function parameters = cohenimcra_estimation(ns_ps,parameters);
%         parameters = struct('n',2,'len',len_val,'noise_cap',ns_ps,'noise_tild',ns_ps,'gamma',ones(len_val,1),'Sf',Sf_val,...
%             'Smin',Sf_val,'S',Sf_val,'S_tild',Sf_val,'GH1',ones(len_val,1),'Smin_tild',Sf_val,'Smin_sw',Sf_val,'Smin_sw_tild',Sf_val,...
%             'stored_min',max(ns_ps)*ones(len_val,U_val),'stored_min_tild',max(ns_ps)*ones(len_val,U_val)','u1',1,'u2',1,'j',2,...
%             'alpha_d',0.85,'alpha_s',0.9,'U',8,'V',15,'Bmin',1.66,'gamma0',4.6,'gamma1',3,'psi0',1.67,'alpha',0.92,'beta',1.47,...
%             'b',b_val,'Sf_tild',Sf_tild_val);

n = parameters.n;
len = parameters.len;
gamma0 = parameters.gamma0;
psi0 = parameters.psi0;
alpha = parameters.alpha;
beta = parameters.beta;
b = parameters.b;
U = parameters.U;
V = parameters.V;
Bmin = parameters.Bmin;
gamma = parameters.gamma;
gamma1 = parameters.gamma1;
GH1 = parameters.GH1;
Sf = parameters.Sf;
Smin = parameters.Smin;
S = parameters.S;
S_tild = parameters.S_tild;
Sf_tild = parameters.Sf_tild;
Smin_tild = parameters.Smin_tild;
Smin_sw = parameters.Smin_sw;
Smin_sw_tild = parameters.Smin_sw_tild;
stored_min = parameters.stored_min;
stored_min_tild = parameters.stored_min_tild;
u1 = parameters.u1;
u2 = parameters.u2;
j = parameters.j;
alpha_d = parameters.alpha_d;
alpha_s = parameters.alpha_s;

noise_cap = parameters.noise_ps;
noise_tild = parameters.noise_tild;

gamma_old=gamma;
gamma=ns_ps./noise_cap;                                     %Eq 3 to compute a posteriori SNR
eps_cap=alpha*(GH1.^2).*gamma_old+(1-alpha)*max(gamma-1,0); %Eq 32 to compute a priori SNR
v=gamma.*eps_cap./(1+eps_cap);
exp_int=expint(v); %ei(v);
GH1=eps_cap.*exp(.5*exp_int)./(1+eps_cap);                    %Eq 33 to compute the value of GH1
Sf(1)=ns_ps(1);Sf(end)=ns_ps(end);
for f=2:len-1
    Sf(f)=sum(b.*[ns_ps(f-1); ns_ps(f); ns_ps(f+1)]);
end

S=alpha_s*S+(1-alpha_s)*Sf;                                 %Eq 14 and 15 for computing S(k,l)
Smin=min(Smin,S);Smin_sw=min(Smin_sw,S);
gamma_min=ns_ps./(Bmin*Smin);
psi=S./(Bmin*Smin);                                         %Eq 18 to compute gamma_min  and Psi
I=zeros(len,1);
index=find(gamma_min<gamma0 & psi<psi0);
I(index)=1;                                                 %Eq 21 to compute I(k,l)
for f=2:len-1
    if (I(f-1)+I(f)+I(f+1))==0
        Sf_tild(f)=S_tild(f);
    else
        Sf_tild(f)=sum(b.*[I(f-1); I(f); I(f+1)].*[ns_ps(f-1); ns_ps(f); ns_ps(f+1)])/sum(b.*[I(f-1); I(f); I(f+1)]);
    end                                                                   %Eq 26 for updating Sf_tild
end
if I(1)==0
    Sf_tild(1)=S_tild(1);
    Sf_tild(end)=S_tild(end);
else
    Sf_tild(1)=ns_ps(1);
    Sf_tild(end)=ns_ps(end);
end

S_tild=alpha_s*S_tild+(1-alpha_s)*Sf_tild;                              %Eq 27 for updating S_tild
Smin_tild=min(Smin_tild,S_tild);Smin_sw_tild=min(Smin_sw_tild,S_tild);
gamma_min_tild=ns_ps./(Bmin*Smin_tild);
psi_tild=S./(Bmin*Smin_tild);
q=zeros(len,1);                                                  %Eq 29 to find a priori speech absence probability
index=find(gamma_min_tild<=1 & psi_tild<psi0);
index1=setdiff([1:len],index);
if (~isempty(index))
    q(index)=1;
end
index=find(gamma_min_tild>1 & gamma_min_tild<gamma1 & psi_tild<psi0);
if (~isempty(index))
    q(index)=(gamma1-gamma_min_tild(index))/(gamma1-1);
end
%        p=1./(1+((q./(1-q)).*(1+eps_cap).*exp(-v)));
p=zeros(len,1);
if (~isempty(index1))
    temp1 = q(index1)./(1-q(index1));
    temp2 = 1 + eps_cap(index1);
    temp3 = exp(-v(index1));
    p(index1) = (1 + temp1.*temp2.*temp3).^-1;
end
%             p(index1)=1./(1+((q(index1)./(1-q(index1))).*(1+eps_cap(index1)).*exp(-v(index1))));    %Eq 7 to find conditional speech presence probability
alpha_d_tild=alpha_d+(1-alpha_d)*p;                                 %Eq 11 to update the time and frequency dependent smoothing factor
noise_tild=alpha_d_tild.*noise_tild+(1-alpha_d_tild).*ns_ps;        %Eq 10 to update noise estimate
noise_cap=beta*noise_tild;                                          %Eq 12 to update noise estimate for correction factor
j=j+1;
if j==V

    stored_min(:,u1)=Smin_sw;
    u1=u1+1;if u1==U+1; u1=1;end
    Smin=min(stored_min,[],2);
    Smin_sw=S;
    stored_min_tild(:,u2)=Smin_sw_tild;
    u2=u2+1;if u2==U+1; u2=1;end
    Smin_tild=min(stored_min_tild,[],2);
    Smin_sw_tild=S_tild;
    j=0;
end
ix=find(p>1 | q>1 | p<0 | q<0);
if (~isempty(ix))
    keyboard;
end
noise_ps = noise_cap;

parameters.n = n+1;
parameters.gamma = gamma;
parameters.GH1 = GH1;
parameters.Sf = Sf;
parameters.Smin = Smin;
parameters.S = S;
parameters.S_tild = S_tild;
parameters.Smin_tild = Smin_tild;
parameters.Smin_sw = Smin_sw;
parameters.Smin_sw_tild = Smin_sw_tild;
parameters.stored_min = stored_min;
parameters.stored_min_tild = stored_min_tild;
parameters.u1 = u1;
parameters.u2 = u2;
parameters.j = j;
parameters.noise_tild=noise_tild;
parameters.noise_ps = noise_ps;


function [M,Vr,Vc] = meanShiftIm( X,sigSpt,sigRng,softFlag,maxIter,minDel )
% Applies the meanShift algorithm to a joint spatial/range image.
%
% See "Mean Shift Analysis and Applications" by Comaniciu & Meer for info.
%
% Assumes X is an MxNxP array, where an X(i,j,:) represents the range data
% at locations (i,j).  This function runs meanShift on each of the MxN data
% points.  It takes advantage of the lattice structure of an image for
% efficiency (it only needs to calculate full distance between two points
% if they are near each other spatially).
%
% In the original formulation of the algorithm, after normalization of the
% data, the search window around each point x has radius 1 (ie
% corresponding to 1 std of the data). That is the search window only
% encloses 2*s+1 pixels, and of those, all which fall within 1 unit from x
% are used to calcluate the new mean.  If softFlag==0 the original
% formulation is used.  If softFlag==1, instead of using a fixed radius,
% each point p is used in the calulation of the mean with points close to x
% given significantly more weight.  Specifically, each point p is given
% weight exp(-dist(x,p)).  So instead of having a fixed cutoff at r, the
% cutoff is 'soft' (same idea as in softmax), and occurs at approximately
% r.  The implementation remains efficient by actually using a hard cutoff
% at points further then 2r spatially from x.
%
% The resulting matrix M is of size MxNx(P+2).  M(i,j,1) represents the
% convergent row location of X(i,j,:) - (which had initial row location i)
% and M(i,j,2) represents the final column location.  M(i,j,p+2) represents
% the convergent value for X(i,j,p).  The optionaly outputs Vr and Vc are
% 2D arrays where Vr(i,j)=M(i,j,1)-i and Vc(i,j)=M(i,j,2)-j.  That is they
% represent the spatial offset between the original location of a point and
% its convergent location.  Display using quiver(Vc,Vr,0).
%
% USAGE
%  [M,Vr,Vc] = meanShiftIm( X,sigSpt,sigRng,[softFlag],[maxIter],[minDel] )
%
% INPUTS
%  X        - MxNxP data array, P may be 1
%  sigSpt   - integer specifying spatial standard deviation
%  sigRng   - value specifying the standard deviation of the range data
%  softFlag - [0]- see above
%  maxIter  - [100] maximum number of iterations per data point
%  minDel   - [.001] minimum amount of spatial change defining convergence
%
% OUTPUTS
%  M        - array of convergent locations [see above]
%  Vr       - spatial motion in row direction
%  Vc       - spatial motion in col direction
%
% EXAMPLE
%  I=double(imread('cameraman.tif'))/255;
%  [M,Vr,Vc] = meanShiftIm( I,5,.2 );
%  figure(1); im(I); figure(2); im( M(:,:,3) );
%  % color image:
%  I=double(imread('hestain.png'))/255;
%  [M,Vr,Vc] = meanShiftIm( I,5,.2 );
%  figure(1); im(I); figure(2); im( M(:,:,3:end) );
%
% See also MEANSHIFT, MEANSHIFTIMEXPLORE
%
% Piotr's Computer Vision Matlab Toolbox      Version 2.0
% Copyright 2014 Piotr Dollar.  [pdollar-at-gmail.com]
% Licensed under the Simplified BSD License [see external/bsd.txt]

[sigSpt,er] = checkNumArgs( sigSpt, 1, 0, 1 ); error(er);
if( nargin<4 || isempty(softFlag)); softFlag = 0; end
if( nargin<5 || isempty(maxIter) ); maxIter = 100; end
if( nargin<6 || isempty(minDel)); minDel = .001; end

[mrows, ncols, p] = size(X); p = p+2;
[gridRs, gridCs] = ndgrid( 1:mrows, 1:ncols );
data = cat( 3, cat( 3, gridRs/sigSpt, gridCs/sigSpt), X/sigRng );

%%% MAIN LOOP
M = data;
ticId = ticStatus('meanShiftIm');  %t0 = clock;  tlast = t0;
if( softFlag ); radius = sigSpt*2; else radius = sigSpt; end
for i=1:mrows; for j=1:ncols; %#ok<ALIGN>
    Mij = data(i,j,:); Mij = Mij(:)';
    itercount = 0; diff = 1;
    while( itercount < maxIter && diff>minDel )

      % get data which is possibly relevant (within spatial range)
      r = round( Mij(1)*sigSpt );  c = round( Mij(2)*sigSpt );
      boundsr = max(1,r-radius):min(mrows,r+radius);
      boundsc = max(1,c-radius):min(ncols,c+radius);
      dataWin = data( boundsr, boundsc, : );
      dataWinF = reshape( dataWin, [], p );

      % get next mean
      MijOld = Mij;
      n = size( dataWinF, 1);
      D = sum( (dataWinF - ones(n,1)*Mij).^2, 2 );
      if( softFlag )
        S = exp( -D ); sumS = sum(S); Srep = S(:,ones(1,p));
        Mij = sum( dataWinF .* Srep, 1 ) / sumS;
      else
        dataWinF = dataWinF( D < 1, : );
        Mij = sum( dataWinF, 1 ) / size( dataWinF,1 );
      end

      % check if Mij changed [only on basis of x,y location]
      diff = sum( (MijOld(1:2)-Mij(1:2)).^2 );
      itercount = itercount+1;

    end
    M(i,j,:) = Mij(:);
    fracdone = ((i-1)*ncols+j) / (mrows*ncols);
    tocStatus( ticId, fracdone );
  end
end
M = cat(3, M(:,:,1:2)*sigSpt, M(:,:,3:end)*sigRng );

%%% Output spatial difference
if( nargout>1 )
  Vr = M(:,:,1)-gridRs;  Vc = M(:,:,2)-gridCs;
end


function tapas_ehgf_plotTraj(r)
% Plots the estimated or generated trajectories for the HGF perceptual model
% Usage example:  est = tapas_fitModel(responses, inputs); tapas_hgf_plotTraj(est);
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2012-2020 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

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

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

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

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

% Number of levels
l = length(r.p_prc.p)/5;

% Upper levels
for j = 1:l-1

    % Subplots
    subplot(l,1,j);

    if plotsd == true
        upperprior = r.p_prc.mu_0(l-j+1) +sqrt(r.p_prc.sa_0(l-j+1));
        lowerprior = r.p_prc.mu_0(l-j+1) -sqrt(r.p_prc.sa_0(l-j+1));
        upper = [upperprior; r.traj.mu(:,l-j+1)+sqrt(r.traj.sa(:,l-j+1))];
        lower = [lowerprior; r.traj.mu(:,l-j+1)-sqrt(r.traj.sa(:,l-j+1))];

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

% Input level
subplot(l,1,l);

if plotsd == true
    upperprior = r.p_prc.mu_0(1) +sqrt(r.p_prc.sa_0(1));
    lowerprior = r.p_prc.mu_0(1) -sqrt(r.p_prc.sa_0(1));
    upper = [upperprior; r.traj.mu(:,1)+sqrt(r.traj.sa(:,1))];
    lower = [lowerprior; r.traj.mu(:,1)-sqrt(r.traj.sa(:,1))];

    plot(0, upperprior, 'or', 'LineWidth', 1);
    hold all;
    plot(0, lowerprior, 'or', 'LineWidth', 1);
    fill([ts, fliplr(ts)], [(upper)', fliplr((lower)')], ...
         'r', 'EdgeAlpha', 0, 'FaceAlpha', 0.15);
end
plot(ts, [r.p_prc.mu_0(1); r.traj.mu(:,1)], 'r', 'LineWidth', 2);
hold all;
plot(0, r.p_prc.mu_0(1), 'or', 'LineWidth', 2); % prior
plot(ts(2:end), r.u(:,1), '.', 'Color', [0 0.6 0]); % inputs
if ~isempty(find(strcmp(fieldnames(r),'y'))) && ~isempty(r.y)
    plot(ts(2:end), r.y(:,1), '.', 'Color', [1 0.7 0]); % responses
    title(['Response y (orange), input u (green), and posterior expectation of x_1 ', ...
           '(red) for \rho=', num2str(r.p_prc.rho), ', \kappa=', ...
           num2str(r.p_prc.ka), ', \omega=', num2str(r.p_prc.om), ...
           ', \pi_u=', num2str(r.p_prc.pi_u)], ...
          'FontWeight', 'bold');
    ylabel('y, u, \mu_1');
else
    title(['Input u (green) and posterior expectation of x_1 ', ...
           '(red) for \rho=', num2str(r.p_prc.rho), ', \kappa=', ...
           num2str(r.p_prc.ka), ', \omega=', num2str(r.p_prc.om), ...
           ', \pi_u=', num2str(r.p_prc.pi_u)], ...
          'FontWeight', 'bold');
    ylabel('u, \mu_1');
end
xlim([0 ts(end)]);
xlabel({'Trial number', ' '}); % A hack to get the relative subplot sizes right
hold off;


function outpoints = mni2tal(inpoints)
% Converts coordinates from MNI brain to best guess
% for equivalent Talairach coordinates
% FORMAT outpoints = mni2tal(inpoints)
% Where inpoints is N by 3 or 3 by N matrix of coordinates
%  (N being the number of points)
% outpoints is the coordinate matrix with Talairach points
% Matthew Brett 10/8/99

% ensure that SPM is available, needed for spm_matrix
hasspm = ft_hastoolbox('spm8up', 3) || ft_hastoolbox('spm2', 1);

dimdim = find(size(inpoints) == 3);
if isempty(dimdim)
  ft_error('input must be a N by 3 or 3 by N matrix')
end
if dimdim == 2
  inpoints = inpoints';
end

% Transformation matrices, different zooms above/below AC
upT   = spm_matrix([0 0 0 0.05 0 0 0.99 0.97 0.92]);
downT = spm_matrix([0 0 0 0.05 0 0 0.99 0.97 0.84]);

tmp = inpoints(3,:)<0;  % 1 if below AC
inpoints = [inpoints; ones(1, size(inpoints, 2))];
inpoints(:, tmp) = downT * inpoints(:, tmp);
inpoints(:, ~tmp) = upT * inpoints(:, ~tmp);
outpoints = inpoints(1:3, :);
if dimdim == 2
  outpoints = outpoints';
end


% system process model in local s0 frame with dcm formulation, Cs0e is the
% transformation from the sensor local s0 frame to e-frame
% written by Huai
% rqs02e, r s0 in e, q s0 2e, rvqs0, rs in s0, vs in s0, qs0 2s
% acc, m/s^2, acc by imu in s frame, gyro, rad/s, angular rate by imu
% w.r.t i frame coordinated in s frame, dt, time interval for covariance update
% modelNo, imu model for bias and scale factor errors
function [STM Qd]=sys_local_dcm_v001(rqs02e, rvqs0, acc, gyro, dt, imutype, modelNo)
% the covariance corresponds to states,
% rs in s0, v s in s0, q s02s, gravity in s0, ba, bg, sa, sg, qs2c, Ts in c
% for modelNo=3 and imutype=5, MEMS
% acc bias drift, random walk
% gyro bias drift, random walk
% acc scale factor, random walk
% gyro scale factor, random walk
% for modelNo=1 the acc and gyro turn on bias are removed

%system disturbance coefs
navStates=12;
N=zeros(navStates,6);
N(7:9,4:6)=eye(3); %attitude
Cs02s=quat2dcm_v000(rvqs0(7:10));
N(4:6,1:3)=Cs02s'; %velocity

%system matrix
A=zeros(navStates);
A(1:3, 4:6)=eye(3); % rs in s0
%Velocity
WIE_E=7292115e-11;
wie2s0=quatrot_v000(rqs02e(4:7),[0;0;WIE_E], 1);
% A(4:6,1:3)=-skew(wie2s0)^2; % for low grade IMU, w_ie^e is buried
% $\delta\bar{g}^{s0}(\mathbf{x}_s^{s0})$ is noisy, but buried by low grade
% IMU noise
% A(4:6,4:6)=-2*skew(wie2s0);
A(4:6,7:9)=-Cs02s'*skew(acc);
% A(4:6,10:12)=eye(3); % for low grade IMU, w_ie^e is buried

%Attitude
A(7:9,7:9)=skew(-gyro);
Anav=A;
Nnav=N;
% X(k+1) = ffun[X(k),U(k),V(k)]
% X(k+1) = Ak*X(k)+Gk*Vk

%%%%Imu error model parameters
[Aimu_d, Qimu_d, Cimu, Rimu]=imu_err_model_v001(acc, gyro, dt, imutype, modelNo);
%%%%Combine and discretize nav and imu models
% this discretization can also be accomplished by Loan's matrix exponential
% method, see sys_metric_phipsi_v000.m

Anav_d=eye(navStates)+dt*Anav;  %Use 1st order taylor series to discretize Anav
Qnav=Nnav*Rimu*Nnav';
Qnav_d=dt/2*(Anav_d*Qnav+Qnav*Anav_d');      %Use trapezoidal rule to discretize Rimu

STM=zeros(navStates+size(Aimu_d,1));
STM(1:navStates,1:navStates)=Anav_d;
STM(1:navStates,1+navStates:end)=Nnav*Cimu*dt;
STM(1+navStates:end,1+navStates:end)=Aimu_d;

Qd=zeros(navStates+size(Aimu_d,1));
Qd(1:navStates,1:navStates)=Qnav_d;
Qd(1+navStates:end,1+navStates:end)=Qimu_d;
Qd(1:navStates,1+navStates:end)=Nnav*Cimu*Qimu_d*dt/2;
Qd(1+navStates:end,1:navStates)=Qd(1:navStates,1+navStates:end)';
end


function c3 = sandia_sgmgg_coef_inc2 ( m, n1, s1, c1, s2 )

%*****************************************************************************80
%
%% SANDIA_SGMGG_COEF_INC2 computes tentative coefficient changes.
%
%  Discussion:
%
%    An active set S1 of N1 sparse grid indices is given, each of
%    size M.
%
%    The coefficient C1 of each sparse grid index is also given.
%
%    A candidate sparse grid index S2 is provided.
%
%    This function determines the N+1 coefficients that would be
%    appropriate if the candidate S2 was added to the active set
%    as the (N+1)-st item.
%
%  Example:
%
%    Input:
%
%      +1 * {0,2}
%      -1 * {0,1}  +1 * {1,1}
%                  -1 * {1,0}  +1 * {2,0}
%
%    Add {3,0}
%
%    Output:
%
%      +1 * {0,2}
%      -1 * {0,1}  +1 * {1,1}
%                  -1 * {1,0}   0 * {2,0}  +1 * {3,0}
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 August 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the dimension of the vector.
%
%    Input, integer N1, the number of points in the active set.
%
%    Input, integer S1(M,N1), the indices for the active set.
%
%    Input, integer C1(N1), the coefficients for the active set.
%
%    Input, integer S2(M), the indices for the candidate.
%
%    Output, integer C3(N1+1), the coefficients for the active set
%    plus the candidate.
%

%
%  During the calculation, we may try to update coefficients of inactive
%  index sets.  By the end of the calculation, all these inactive index
%  sets should have accumulated total coefficients of 0 again.  As a check,
%  we temporarily set aside space for these objects, and check, at the end,
%  that the coefficients are zero.
%
  n4 = 0;
  c4 = zeros(n1,1);
  s4 = zeros(m,n1);
%
%  Copy C1.
%
  c3(1:n1) = c1(1:n1);
  c3(n1+1) = 1;
%
%  Consider the effect of the new item S2 on each of the current
%  items in the active set S1.
%
  for j = 1 : n1
%
%  Determine S, the element-wise minimum of the J-th item in S1 versus S2.
%
    k = j;

    for i = 1 : m

      if ( s2(i) < s1(i,j) )
        s(i) = s2(i);
        k = - 1;
      else
        s(i) = s1(i,j);
      end

    end
%
%  If S = S1(*,J), K is J.
%
    if ( k ~= -1 )

      c3(k) = c3(k) - c1(j);
%
%  If S is equal to an element of the active set, we set K to that index.
%
    else

      for j2 = 1 : n1

        k = j2;

        for i2 = 1 : m

          if ( s1(i2,j2) ~= s(i2) )
            k = - 1;
            break
          end

        end

        if ( k ~= - 1 )
          c3(k) = c3(k) - c1(j);
          break
        end

      end

    end
%
%  If S is equal to an element of the inactive set, set K to that index.
%
    if ( k == -1 )

      for j2 = 1 : n4

        k = j2;

        for i2 = 1 : m

          if ( s4(i2,j2) ~= s(i2) )
            k = - 1;
            break
          end

        end

        if ( k ~= - 1 )
          c4(k) = c4(k) - c1(j);
          break
        end

      end

    end
%
%  S is not equal to S1(*,J), or any element of S1, or any element of S4.
%  Add S to the set of elements S4.
%
    if ( k == -1 )
      n4 = n4 + 1;
      k = n4;
      c4(k) = 0;
      s4(1:m,k) = s(1:m);
      c4(k) = c4(k) - c1(j);
    end

  end
%
%  At the end, the C4(1:N4) should all be zero.
%
  if ( any ( c4(1:n4) ~= 0 ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'SANDIA_SGMGG_COEF_INC2 - Fatal error!\n' );
    fprintf ( 1, '  Some inactive indices were assigned a nonzero coefficient.\n' );
    i4mat_transpose_print ( m, n4, s4, '  S4:' );
    i4vec_print ( n4, c4, '  C4:' );
    error ( 'SANDIA_SGMGG_COEF_INC2 - Fatal error!' );
  end

  return
end


function net = train(net, tutor, varargin)

% TRAIN
%
% Train a max-win multi-class support vector classifier network using the
% specified tutor to train each component two-class network.
%
%    load data/iris x y;
%
%    C      = 100;
%    kernel = rbf(0.5);
%
%    net = train(maxwin, smosvctutor, x, y, C, kernel);

%
% File        : @maxwin/train.m
%
% Date        : Wednesday 13th September 2000
%
% Author      : Dr Gavin C. Cawley
%
% Description : Gateway function used to train a max-win multi-class support
%               vector classifier network using a given tutor.  Part of an
%               object-oriented implementation of Vapnik's Support Vector
%               Machine, as described in [1].
%
% References  : [1] V.N. Vapnik,
%                   "The Nature of Statistical Learning Theory",
%                   Springer-Verlag, New York, ISBN 0-387-94559-8,
%                   1995.
%
% History     : 13/09/2000 - v1.00
%
% Copyright   : (c) Dr Gavin C. Cawley, September 2000
%
%    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 2 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, write to the Free Software
%    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
%

y = varargin{2};

if size(y, 2) == 1

   for i=1:max(y)

      varargin{2} = 2*(y == i) - 1;

      net.net(i) = train(tutor, varargin{:});

   end

else

   for i=1:size(y, 2)

      varargin{2} = y(:,i);

      net.net(i) = train(tutor, varargin{:});

   end

end

% bye bye...


function mesh = genIntfMesh3D(mesh,intf)

%% USAGE: enrich mesh information for interface problem.
% INPUTS:
% mesh --- nt-by-3 matrix contains p and t information
% intf --- interface function in analytical form
%               g(x,y,z) = 0
%
% OUTPUTS:
% pLoc --- mesh node indices.  possible values:
%               pLoc(i) =-1 if g(xi,yi,zi) < 0
%               pLoc(i) = 0 if g(xi,yi,zi) = 0 (on interface)
%               pLoc(i) = 1 if g(xi,yi,zi) > 0
% tLoc --- mesh element indices.  possible values:
%               tLoc(i) = 1 if the element Ti is in region 1 (g(X) < 0)
%               tLoc(i) = 2 if the element Ti is in region 2 (g(X) > 0)
%               tLoc(i) = -ki if Ti is the ki-th interface element
% intType --- interface element type.  possible values:
%             0 for noninterface element,
%             1 for Type 1: one vertex on one side, three vertices on the other
%             2 for Type 2: two vertice on each side of interface)
% eLoc --- mesh edge indices.  possible values:
%               eLoc(i) = 1 if the element Ti is in region 1 (g(X) < 0)
%               eLoc(i) = 2 if the element Ti is in region 2 (g(X) > 0)
%               eLoc(i) = -ki if Ti is the ki-th interface element
% eIntP --- neI-by-3 vector, stores the coordinate of interface
%                    intersection points on each edge.
%
% Last updated by Xu Zhang on 07/09/20

%% 1. generate pLoc
%% basic assumption:
p = mesh.p; t = mesh.t; e = mesh.e; f = mesh.f;
nt = length(t); ne = length(e); nf = length(f);
pLoc = sign(feval(intf,p(:,1),p(:,2),p(:,3))); pLoc(pLoc == 0) = 1;

%% 2. generate tLoc and intType
id1 = sign(feval(intf,p(t(:,1),1),p(t(:,1),2),p(t(:,1),3)));
id2 = sign(feval(intf,p(t(:,2),1),p(t(:,2),2),p(t(:,2),3)));
id3 = sign(feval(intf,p(t(:,3),1),p(t(:,3),2),p(t(:,3),3)));
id4 = sign(feval(intf,p(t(:,4),1),p(t(:,4),2),p(t(:,4),3)));
id = id1+id2+id3+id4;
idm = id1.*id2.*id3.*id4;
tLoc = zeros(nt,1); intType = zeros(nt,1);
r1 = find(id == -4); tLoc(r1) = 1; intType(r1) = 0;
r2 = find(id == 4);  tLoc(r2) = 2; intType(r2) = 0;
r6 = (id == -1); tLoc(r6) = 1; intType(r6) = 0; % one pt is - and others are zero
r7 = (id == 1); tLoc(r7) = 2;  intType(r7) = 0; % one pt is + and others are zero
r8 = (id == -2 & idm == 0); tLoc(r8) = 1; intType(r8) = 0; % two pts are - and others are zero
r9 = (id == 2 & idm == 0);  tLoc(r9) = 2; intType(r8) = 0; % two pts are + and others are zero
r10 = (id == -3 & idm == 0); tLoc(r10) = 1; intType(r10) = 0; % three pts are - and the other s zero
r11 = (id == 3 & idm == 0);  tLoc(r11) = 2; intType(r11) = 0; % three pts are + and the other s zero
r3 = (id == -2 & idm ~= 0); intType(r3) = 1;
r4 = (id == 2 & idm ~= 0);  intType(r4) = 1;
r5 = (id == 0); intType(r5) = 2;
nt1 = length(r1) + sum(r6) + sum(r8) + sum(r10);
nt2 = length(r2) + sum(r7) + sum(r9) + sum(r11);
ntI = nt-nt1-nt2;
tLoc(tLoc == 0) = -(1:ntI);

%% 3. generate eLoc
eLoc = zeros(ne,1);
ide1 = sign(feval(intf,p(e(:,1),1),p(e(:,1),2),p(e(:,1),3))); ide1(ide1 == 0) = 1;
ide2 = sign(feval(intf,p(e(:,2),1),p(e(:,2),2),p(e(:,2),3))); ide2(ide2 == 0) = 1;
ide = ide1+ide2;
re1 = find(ide == -2); eLoc(re1) = 1;
re2 = find(ide == 2);  eLoc(re2) = 2;
ne1 = length(re1); ne2 = length(re2); neI = ne-ne1-ne2;
eLoc(eLoc == 0) = -(1:neI);

%% 4. generate eIntP
p1 = p(e(eLoc<0,1),:);
p2 = p(e(eLoc<0,2),:);
eIntP = IntersectPoint3D(p1, p2, intf);

%% 5. generate fLoc
fLoc = zeros(nf,1);
fID = sum(pLoc(mesh.f),2);
rf1 = find(fID == -3); fLoc(rf1) = 1;
rf2 = find(fID == 3);  fLoc(rf2) = 2;
nf1 = length(rf1); nf2 = length(rf2); nfI = nf-nf1-nf2;
fLoc(fLoc == 0) = -(1:nfI);

%% Form Mesh
mesh.pLoc = pLoc;
mesh.eLoc = eLoc;
mesh.tLoc = tLoc;
mesh.fLoc = fLoc;
mesh.eIntP = eIntP;
mesh.intType = intType;


function [gr, diams, vols] = imGranulo(img, granuloType, strelShape, strelSizes, varargin)
% Compute granulometry curve of a given image.
%
%   GR = imGranulo(IMG, GRTYPE, STRELSHAPE, SIZES)
%   Computes the granulometry curve for the input image IMG, using the
%   operation GRTYPE, the shape of structuring element given by STRELSHAPE,
%   and the list of structuring element sizes given by SIZES.
%   IMG should be a 2D image (binary or grayscale)
%   GRTYPE can be one of {'opening', 'closing', 'erosion', 'dilation'}.
%   STRELSHAPE can be one of {'square', 'octagon', 'diamond', 'disk',
%     'lineh', 'linev'}.
%   SIZES are given as radius. Diameters of strels are obtained as 2*R+1.
%   The result GR is a 1-by-N array with as many columns as the number of
%     elements provided in SIZES array.
%
%   [GR, DIAMS] = imGranulo(...)
%   Also returns the diameters of the structuring elements for each step of
%   the analysis. DIAMS is 1-by-N array with the same number of columns as
%   the GR array.
%
%   [GR, DIAMS, VOLS] = imGranulo(...)
%   Also returns the array of volume curves, corresponding to the sum of
%   gray levels at each step of the granulometry.
%
%   ... = imGranulo(..., 'verbose', TF)
%   If TF is true, displays the iteration steps on the command window.
%
%
%   Example
%     % Compute granulometric curve by opening on coins image (with two
%     % distinct sizes), using disk structuring element
%     img = imread('coins.png');
%     Rmax = 50;
%     gr = imGranulo(img, 'opening', 'disk', 1:Rmax);
%     % display as a function of strel diameter
%     figure; plot(2*(1:Rmax)+1, gr); xlim([0 2*Rmax]);
%     xlabel('Strel diameter (pixel)'); ylabel('Percentage of Variations');
%     title('Granulometry on "coins" image');
%
%   See also
%     imGranulometry, granuloMeanSize, imGranuloByRegion
%

% ------
% Author: David Legland
% e-mail: david.legland@inrae.fr
% Created: 2014-05-05,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2014 INRAE - Cepia Software Platform.

%% Parse input arguments

% check ROI
roi = true(size(img));
if ~isempty(varargin)
    var1 = varargin{1};
    if islogical(var1) && all(size(var1) == size(img))
        roi = var1;
        varargin(1) = [];
    end
end

% check additional input arguments
verbose = false;
while length(varargin) > 1
    switch lower(varargin{1})
        case 'verbose', verbose = varargin{2};
        otherwise
            error(['Can not understand option: ' varargin{1}]);
    end
    varargin(1:2) = [];
end

% create structuring element factory, as a function handle that returns a
% strel object (or an array) from the radius of the strel.
if isa(strelShape, 'functionHandle')
    strelMaker = strelShape;
else
    % create factory from strel name
    switch lower(strelShape)
        case 'square'
            strelMaker = @(r) strel('square', r);
        case {'octagon', 'diamond'}
            strelMaker = @(r) strel(lower(strelShape), r);
        case 'disk'
            % do not use simplification, as it is not suitable for
            % granulometries
            strelMaker = @(r) strel('disk', r, 0);
        case 'lineh'
            strelMaker = @(r) ones(1, 2*r+1);
        case 'linev'
            strelMaker = @(r) ones(2*r+1, 1);
        otherwise
            error(['matImage:' mfilename], ...
                ['Could not process strel type: ' strelShape]);
    end
end

% create funtion handle for morphological operation
if isa(granuloType, 'functionHandle')
    morphoOp = granuloType;
else
    switch lower(granuloType)
        case 'opening'
            morphoOp = @imopen;
        case 'closing'
            morphoOp = @imclose;
        case 'dilation'
            morphoOp = @imdilate;
        case 'erosion'
            morphoOp = @imerode;

        otherwise
            error(['matImage:' mfilename], ...
                ['Could not process granulometry type: ' granuloType]);
    end
end

%% Initialize

% number of structuring element sizes
nSizes = length(strelSizes);

% compute the associated diameter list (for output)
diams = 2 * strelSizes + 1;

% allocate memory for volume curves (sum of gray levels)
vols = zeros(1, nSizes + 1);

% initialize reference volume
vol0 = sum(img(roi));
vols(1) = vol0;

%% Iteration over structuring element sizes

% iterate
for i = 1:nSizes
    % size of strel for current iteration
    radius = strelSizes(i);

    if verbose
        fprintf('iter %2d/%d, radius = %f\n', i, nSizes, radius);
    end

    % create current strel
    se = strelMaker(radius);

    % compute morphological operation
    img2 = morphoOp(img, se);

    % sum of gray levels within each region
    vols(i+1) = sum(img2(roi));
end

% compute granulometry curve
vols2 = (vols - vol0) / (vols(end) - vol0);
gr = 100 * diff(vols2);


%----------------------------------------------------------------
% Depth Map Super-Resolution by Deep Multi-Scale Guidance, ECCV16
%                     written by T.-W. HUI
%---------------------------------------------------------------
clear all;
close all;
clc;

%% Settings
dataDir = '/path/to/training_data/';
saveDir = '/path/to/h5/';

chunksz = 128;

scale = 2;
size_dataY = 40;
size_dataD = 20;
size_label = 40;
border_dataY = 1;
border_label = 1;
stride = 22;
data_aug = true;
index_val = [1 20 28 58 64 66 69 73 75 79];

%% Initialization
dataY = zeros(size_dataY, size_dataY, 1, 1);
dataD = zeros(size_dataD, size_dataD, 1, 1);
label = zeros(size_label, size_label, 1, 1);

size_subIm = size_dataD*scale;

pad_dataY = (size_subIm - size_dataY)/2;
pad_label = (size_subIm - size_label)/2;
assert(pad_dataY >= 0, 'pad_dataY cannot be an negative number!');
assert(pad_label >= 0, 'pad_label cannot be an negative number!');

count = 0;

%% Generate training  data
load([dataDir '/RGB']);
load([dataDir '/Df']);

index_train = setdiff(1:numel(Df), index_val);
fprintf('%d images is used for training.\n', numel(index_train));

RGB = RGB(index_train);
Df  = Df(index_train);

h = ones(3,3)/9;

for k = 1 : size(Df,1)

    fprintf('Processing %d of %d, id: %d, ', k, size(Df,1), index_train(k));

    im_Dh = modcrop(Df{k}, scale);
    im_M  = im_Dh > 0;
    im_Dh = normalizeIm(im_Dh, [0 1]);
    im_Dl = imresize(im_Dh, 1/scale, 'bicubic');
    im_Dl_LF = imfilter(im_Dl, h, 'symmetric');
    im_label = im_Dh - imresize(im_Dl_LF, scale, 'bicubic');
    [H, W] = size(im_label);

    I = RGB{k};
    I = rgb2ycbcr(I);
    Y = double(I(:,:,1));
    im_Y = modcrop(Y, scale);
    im_Y = normalize_cleanIm(im_Y);
    im_Y_LF = imfilter(im_Y, h, 'symmetric');
    im_inputY = normalize_cleanIm(im_Y - im_Y_LF);

    im_grad_Dh = imgradient(im_Dh, 'sobel');

    stride_ = stride;

    if index_train(k) >= 75
        stride_ = floor(2.0*stride_);
    elseif index_train(k) >= 65
        stride_ = floor(1.5*stride_);
    end

    for x = 1 : stride_ : H-size_subIm+1
        for y = 1 :stride_ : W-size_subIm+1

            subIm_M = im_M(x : x+size_subIm-1, y : y+size_subIm-1);

            if sum(sum(subIm_M == 0)) == 0

                subim_dataY = im_inputY(x+pad_dataY : x+pad_dataY+size_dataY-1, y+pad_dataY : y+pad_dataY+size_dataY-1);

                subim_dataD = imresize(im_Dh(x : x+size_subIm-1, y : y+size_subIm-1), 1/scale, 'bicubic');
                subim_dataD = subim_dataD - imfilter(subim_dataD, h, 'symmetric');

                subim_label = im_label(x+pad_label : x+pad_label+size_label-1, y+pad_label : y+pad_label+size_label-1);

                count = count + 1;
                dataY(:, :, 1, count) = subim_dataY;
                dataD(:, :, 1, count) = subim_dataD;
                label(:, :, 1, count) = subim_label;

            end

        end
    end

end

fprintf('---------------------------------------\n')

fprintf('Total: %d patches.\n', count);
num_patches = count;

% Data augmntation
if data_aug
    num_patches = num_patches*2;
    dataY_tmp = dataY; clear dataY;
    dataD_tmp = dataD; clear dataD;
    label_tmp = label; clear label;
    dataY = cat(4, dataY_tmp, rot90(dataY_tmp,1));
    dataD = cat(4, dataD_tmp, rot90(dataD_tmp,1));
    label = cat(4, label_tmp, rot90(label_tmp,1));
end

fprintf('Number of patches used: %d\n', num_patches);

% Remove left-most column and bottom row of dataY and label
dataY = dataY(1:end-border_dataY,1:end-border_dataY,:,:);
label = label(1:end-border_label,1:end-border_label,:,:);

% Re-arrange order
order = randperm(num_patches);
dataY = dataY(:,:,:,order);
dataD = dataD(:,:,:,order);
label = label(:,:,:,order);

%% writing to HDF5
created_flag = false;
totalct = 0;

for batchno = 1 : floor(num_patches/chunksz)
    last_read = (batchno - 1)*chunksz;
    batchdata = dataY(:, :, :, last_read+1 : last_read+chunksz);

    startloc = struct('dat', [1,1,1,totalct+1], 'lab', [1,1,1,totalct+1]);
    curr_dat_sz = store2hdf5_v2(savepathY, '-Y', batchdata, [], [], ~created_flag, startloc, chunksz);
    created_flag = true;
    totalct = curr_dat_sz(end);
end

h5disp(savepathY);

created_flag = false;
totalct = 0;

for batchno = 1 : floor(num_patches/chunksz)
    last_read = (batchno - 1)*chunksz;
    batchdata = dataD(:, :, :, last_read+1 : last_read+chunksz);
    batchlabs = label(:, :, :, last_read+1 : last_read+chunksz);

    startloc = struct('dat', [1,1,1,totalct+1], 'lab', [1,1,1,totalct+1]);
    curr_dat_sz = store2hdf5_v2(savepathD, '-D', batchdata, [], batchlabs, ~created_flag, startloc, chunksz);
    created_flag = true;
    totalct = curr_dat_sz(end);
end

h5disp(savepathD);


% OP_U_OTIMES_N_V_OTIMES_N: assemble the matrix A = [A(i,j)], A(i,j) = (epsilon (u \otimes n)_j, (v \otimes n)_i ).
%
%   A = op_u_otimes_n_v_otimes_n (spu, spv, msh, coeff);
%
% INPUT:
%
%   spu:   structure representing the space of trial functions (see sp_vector/sp_eval_boundary_side)
%   spv:   structure representing the space of test functions (see sp_vector/sp_eval_boundary_side)
%   msh:   structure containing the domain partition and the quadrature rule for the boundary,
%           since it must contain the normal vector (see msh_cartesian/msh_eval_boundary_side)
%   coeff: vector-valued function f, evaluated at the quadrature points
%
% OUTPUT:
%
%   A: assembled matrix
%
% Copyright (C) 2015, Rafael, 2017 Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.

function varargout = op_u_otimes_n_v_otimes_n (spu, spv, msh, mshv, coeff)

  shpu = reshape (spu.shape_functions, spu.ncomp, msh.nqn, spu.nsh_max, msh.nel);
  shpv = reshape (spv.shape_functions, spv.ncomp, msh.nqn, spv.nsh_max, msh.nel);

  rows = zeros (msh.nel * spu.nsh_max * spv.nsh_max, 1);
  cols = zeros (msh.nel * spu.nsh_max * spv.nsh_max, 1);
  values = zeros (msh.nel * spu.nsh_max * spv.nsh_max, 1);

  jacdet_weights = msh.jacdet .* msh.quad_weights .* coeff;

  ncounter = 0;
  for iel = 1:msh.nel
    if (all (msh.jacdet(:, iel)))
      shpu_iel = reshape (shpu(:, :, :, iel), spu.ncomp, msh.nqn, spu.nsh_max);
      shpv_iel = reshape (shpv(:, :, :, iel), spv.ncomp, msh.nqn, spv.nsh_max);

      u_otimes_n_iel = zeros (spu.ncomp, spu.ncomp, msh.nqn, spu.nsh_max);
      v_otimes_n_iel = zeros (spv.ncomp, spv.ncomp, msh.nqn, spv.nsh_max);
% I need the normal from both sides
      normalu_iel = msh.normal (:, :, iel);
      normalv_iel = mshv.normal (:, :, iel);
      for ii = 1:spu.ncomp
        for jj = 1:spu.ncomp
          u_otimes_n_iel(ii,jj,:,:) = bsxfun (@times, shpu_iel(ii,:,:), normalu_iel(jj,:));
          v_otimes_n_iel(ii,jj,:,:) = bsxfun (@times, shpv_iel(ii,:,:), normalv_iel(jj,:));
        end
      end
% Should I permute it, before reshaping?
      u_otimes_n_iel = reshape (u_otimes_n_iel, spu.ncomp*spu.ncomp, msh.nqn, 1, spu.nsh_max);
      v_otimes_n_iel = reshape (v_otimes_n_iel, spv.ncomp*spv.ncomp, msh.nqn, spv.nsh_max, 1);

      jacdet_iel = reshape (jacdet_weights(:,iel), [1, msh.nqn, 1, 1]);
      v_oxn_times_jw = bsxfun (@times, jacdet_iel, v_otimes_n_iel);
      tmp1 = sum (bsxfun (@times, v_oxn_times_jw, u_otimes_n_iel), 1);
      elementary_values = reshape (sum (tmp1, 2), spv.nsh_max, spu.nsh_max);

      [rows_loc, cols_loc] = ndgrid (spv.connectivity(:,iel), spu.connectivity(:,iel));
      indices = rows_loc & cols_loc;
      rows(ncounter+(1:spu.nsh(iel)*spv.nsh(iel))) = rows_loc(indices);
      cols(ncounter+(1:spu.nsh(iel)*spv.nsh(iel))) = cols_loc(indices);
      values(ncounter+(1:spu.nsh(iel)*spv.nsh(iel))) = elementary_values(indices);
      ncounter = ncounter + spu.nsh(iel)*spv.nsh(iel);

    else
      warning ('geopdes:jacdet_zero_at_quad_node', 'op_u_otimes_n_v_otimes_n: singular map in element number %d', iel)
    end
  end

  if (nargout == 1 || nargout == 0)
    varargout{1} = sparse (rows(1:ncounter), cols(1:ncounter), ...
                           values(1:ncounter), spv.ndof, spu.ndof);
  elseif (nargout == 3)
    varargout{1} = rows(1:ncounter);
    varargout{2} = cols(1:ncounter);
    varargout{3} = values(1:ncounter);
  else
    error ('op_u_otimes_n_v_otimes_n: wrong number of output arguments')
  end

end


function [gammaCube,gammaPassRateCell] = matRad_gammaIndex(cube1,cube2,resolution,criteria,slice,n,localglobal,cst)
% gamma index calculation
% according to http://www.ncbi.nlm.nih.gov/pubmed/9608475
%
% call
%   [gammaCube,gammaPassRateCell] = matRad_gammaIndex(cube1,cube2,resolution,criteria,cst)
%   [gammaCube,gammaPassRateCell] = matRad_gammaIndex(cube1,cube2,resolution,criteria,slice,cst)
%   [gammaCube,gammaPassRateCell] = matRad_gammaIndex(cube1,cube2,resolution,criteria,n,cst)
%   [gammaCube,gammaPassRateCell] = matRad_gammaIndex(cube1,cube2,resolution,criteria,localglobal,cst)
%   [gammaCube,gammaPassRateCell] = matRad_gammaIndex(cube1,cube2,resolution,criteria,slice,n,cst)
%               ...
%   [gammaCube,gammaPassRateCell] = matRad_gammaIndex(cube1,cube2,resolution,criteria,slice,n,localglobal,cst)
%
% input
%   cube1:         dose cube as an M x N x O array
%   cube2:         dose cube as an M x N x O array
%   resolution:    resolution of the cubes [mm/voxel]
%   criteria:      [1x2] vector specifying the distance to agreement
%                  criterion; first element is percentage difference,
%                  second element is distance [mm]
%   slice:         (optional) slice in cube1/2 that will be visualized
%   n:             (optional) number of interpolations. there will be 2^n-1
%                  interpolation points. The maximum suggested value is 3.
%   localglobal:   (optional) parameter to choose between 'global' and 'local'
%                  normalization
%   cst:           list of interessing volumes inside the patient
%
% output
%
%   gammaCube:          result of gamma index calculation
%   gammaPassRateCell:  rate of voxels passing the specified gamma criterion
%                  evaluated for every structure listed in 'cst'.
%                  note that only voxels exceeding the dose threshold are
%                  considered.
%
% References
%   [1]  http://www.ncbi.nlm.nih.gov/pubmed/9608475
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copyright 2015 the matRad development team.
%
% This file is part of the matRad project. It is subject to the license
% terms in the LICENSE file found in the top-level directory of this
% distribution and at https://github.com/e0404/matRad/LICENSES.txt. No part
% of the matRad project, including this file, may be copied, modified,
% propagated, or distributed except according to the terms contained in the
% LICENSE file.
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[env, ~] = matRad_getEnvironment();

% set parameters for gamma index calculation
if exist('criteria','var')
    relDoseThreshold = criteria(1); % in [%]
    dist2AgreeMm     = criteria(2); % in [mm]
else
    dist2AgreeMm     = 3; % in [mm]
    relDoseThreshold = 3; % in [%]
end

% set parameters for gamma index calculation
if ~exist('n','var')
    n = 0;
end

% set global or local gamma evaluation
if ~exist('localglobal','var')
    localglobal = 'global';
end

% check if cubes consistent
if ~isequal(size(cube1),size(cube2))
   error('dose cubes must be the same size\n');
end

% define search nighborhood and resolution
if round(n) ~= n
    error('n must be an integer value');
else
    resolution = resolution./(2.^n);
end

neighborhood = 1.5* dist2AgreeMm; % [mm]
searchX = ceil(neighborhood./resolution(1));
searchY = ceil(neighborhood./resolution(2));
searchZ = ceil(neighborhood./resolution(3));

% init cube
% cut all the zeros surrounding the interesting part
cut1 = any(any(cube1,2),3);
cut2 = any(any(cube1,1),3);
cut3 = any(any(cube1,1),2);

% avoids that "zero-slides" are deleted between two interest regions
k = find(cut1); cut1(k(1):k(end)) = 1;
k = find(cut2); cut2(k(1):k(end)) = 1;
k = find(cut2); cut2(k(1):k(end)) = 1;

% copy cubes
cubex1c = cube1;
cubex2c = cube2;

% cut cubes
cubex1c( ~cut1, :, :) = []; % rows
cubex1c( :, ~cut2, :) = []; % columns
cubex1c( :, :, ~cut3) = []; % slices
cubex2c( ~cut1, :, :) = []; % rows
cubex2c( :, ~cut2, :) = []; % columns
cubex2c( :, :, ~cut3) = []; % slices

% pad cubes
cubex2 = zeros([size(cubex2c,1)+2*searchX size(cubex2c,2)+2*searchY size(cubex2c,3)+2*searchZ]);
cubex2((1+searchX):(end-searchX), ...
       (1+searchY):(end-searchY), ...
       (1+searchZ):(end-searchZ)) = cubex2c;

% interpolate if necessary
if n > 0
    switch env
        case 'MATLAB'
            cubex2 = interp3(cubex2,n,'cubic');
        case 'OCTAVE'
            cubex2 = interp3(cubex2,n,'linear');
    end
end

% set up temporary cubes required for calculation
gammaCubeSq = inf*ones(size(cubex1c));

% adjust dose threshold
if strcmp(localglobal,'local')
    doseThreshold = cubex1c .* relDoseThreshold/100;
elseif strcmp(localglobal,'global')
    doseThreshold = relDoseThreshold/100 * max(cube1(:));
end

% search for min
for i = -searchX:searchX
    for j = -searchY:searchY
        for k = -searchZ:searchZ

            delta_sq = ((i*resolution(1))^2 + ...
                        (j*resolution(2))^2 + ...
                        (k*resolution(3))^2) / dist2AgreeMm^2;

            tmpCube = cubex1c - cubex2((1+((2^n)*searchX)+i) : 2^n : (end-((2^n)*searchX)+i), ...
                                       (1+((2^n)*searchY)+j) : 2^n : (end-((2^n)*searchY)+j), ...
                                       (1+((2^n)*searchZ)+k) : 2^n : (end-((2^n)*searchZ)+k));

            tmpCube = tmpCube.^2 ./ doseThreshold.^2 + delta_sq;

            gammaCubeSq = min(gammaCubeSq,tmpCube);

        end
    end

%     display '.';

end

% evaluate gamma cube and set to zero all the voxel that contain an
% infinite value
gammaCubeSqx                 = zeros(size(cube1));
gammaCubeSqx(cut1,cut2,cut3) = gammaCubeSq;
gammaCube                    = sqrt(gammaCubeSqx);

% set values where we did not compute gamma keeping inf in the cube to zero
gammaCube(cube1<=0 & cube2<=0) = 0;

% compute gamma pass rate
doseIx          = cube1 > relDoseThreshold/100*max(cube1(:)) | cube2 > relDoseThreshold/100*max(cube2(:));
numOfPassGamma  = sum(gammaCube(doseIx) < 1);
gammaPassRate   = 100 * numOfPassGamma / sum(doseIx(:));

% compute stats for all segmented sturtures
if exist('cst','var')
    gammaPassRateCell = cell(1,2);
    gammaPassRateCell{1,1} = 'Whole CT';
    gammaPassRateCell{1,2} = gammaPassRate;

    for i = 1:size(cst, 1)
        volume = cst{i,4}{1,1}; % indices of voxels of the interesting volume
        doseIxVol = false(size(doseIx));
        doseIxVol(volume) = doseIx(volume);
        numOfPassGammaVol  = sum(gammaCube(doseIxVol) < 1);
        gammaPassRateVol   = 100 * numOfPassGammaVol / sum(doseIxVol(:));
        if isnan(gammaPassRateVol) % remove organs not receiving any dose (resulting in NaN)
            continue;
        end
        gammaPassRateCell{end+1, 1} = cst{i,2};
        gammaPassRateCell{end, 2} = gammaPassRateVol;

    end
end

% visualize if applicable
if exist('slice','var') && ~isempty(slice)
    figure
    set(gcf,'Color',[1 1 1]);
    imagesc(gammaCube(:,:,slice),[0 2])
    myColormap = matRad_getColormap('gammaIndex');

    colormap(gca,myColormap);
    colorbar

    title({[num2str(gammaPassRate,5) '% of points > ' num2str(relDoseThreshold) ...
            '% pass gamma criterion (' num2str(relDoseThreshold) '% / ' ...
            num2str(dist2AgreeMm) 'mm)']; ['with ' num2str(2^n-1) ' interpolation points']});
end


function [hyp, flogtheta, i,simy,simse2] = trainGPoe(hyp, inf, mean, cov, lik, input, target, lag, simf, Nsamples)
% Function for the optimisation (training) of GP model hyperparameters
%
%% Syntax
% [hyp, flogtheta, i] = trainGPoe(hyp, inf, mean, cov, lik, input, target,
%                                 simf, lag, Nsamples);
%
%% Description
% Function for the optimisation (training) of GP model hyperparameters
% based on the training data via maximum marginal likelihood.
% Uses routines gp and minimize.
% Based on the work of C.E.Rasmussen.
%
% Input:
% * hyp      ... the structure of initial hyperparameters
% * inf      ... the function specifying the inference method
% * cov      ... the prior covariance function (see below)
% * mean     ... the prior mean function
% * lik      ... the likelihood function
% * input    ... the input part of the training data,  NxD matrix
% * target   ... the output part of the training data (ie. target), Nx1 vector
% * simf     ... the function handle of simulation function (e.g. @simulGPmc)
% * lag      ... the order of the model (number of used lagged outputs)
% * Nsamples ... the number of samples for MCMC simulation (optional)
% * minf     ... the function handle of the minimization method to be used
%                (optional, default=@minimize_and_return). The minimization
%				  method should return the following values:
%				  [X, fX, i,fout] = minimize(X, f, length, P1, P2, P3, ... ),
%				  where fout is the additional argument (along fitness and gradient)
%				  of the evaulation function f
%
% Output:
% * hyp       ... optimized hyperparameters
% * flogtheta ... the minus log likelihood for the different runs (init. to 0)
% * i         ... the number of iterations needed for the last optimization
%
% Examples:
% demo_example_gp_training.m
%
% See Also:
% gp, minimize, covFunctions, trainlgmp
%

if(nargin < 11)
  minf = @minimize;
end
if (nargin < 10)
  Nsamples = 100; % default value of Nsamples for MC simulation
end
if (nargin < 9)
  simf = @simulGPnaive; %choose the default simulation routine
end

if (nargin < 8)
  warning(['The number of dynamical system model order was not provided. ' ...
  'Guessing from the input data...']);
  lag=size(input,2)/2;
  assert(mod(lag,1)==0);
  fprintf('Guess: lag=%d\n',lag);
end

if (nargin < 7)
  error('Too few parameters are given.');
end

MIN_DIFF = 0.002;
flogtheta = [0,-1/eps];

% we reconstruct the input signal from the regressor matrix as
%  the two following commented lines show:
%%N = length(input,1) +    lag-1
%%[u(1) ... u(N-lag)]';	[u(N-lag+1) ...  u(N)]

u=[input(:,lag+1)      ;	input(end,lag+2:end);NaN];

simulated_y=[input(1:lag,1);target];

while (abs(flogtheta(end) - flogtheta(end-1))>MIN_DIFF)
	disp(' ');
	disp(strcat(['delta flogtheta: ', num2str(abs(flogtheta(end) - flogtheta(end-1)))]));
	disp(' ')
	[hyp, flogthetatmp,i] = feval(minf, hyp, @simLL, -100,...
				inf, mean, cov, lik, u, simulated_y, target, lag, simf);

	%in the last iteration of minimisation algorithm the simulation is evaluated again for obtaining
	% the simulated response 'simulated_y'
	disp('saving simulated response.')
    [~,~,simulated_y] = simLL(hyp,inf, mean, cov, lik, u, simulated_y, target, lag, simf);

    if isempty(flogthetatmp) % no improvement: at minimum
        disp('oops')
        break
    end
	flogtheta = [flogtheta flogthetatmp(end)];
end

function [nlZ,dnlZ,simulated_y]=simLL(hyp, inf, mean, cov, lik, u, simulated_y, target,lag,simf)
	% simLL simulates the response and forms the regressor vectors from the response. Despite the response signal in regressor vectors is the simulated one, the regressands are always the original measurements.
	%
	% log-likelihood is calculated from the gp model with input data from simulation (in regressor matrix) and measurements (as regressands).
	%
	% simulated_y is a NxS or 2NxS matrix where N is the signal length and S is the number of past iterations. The values from N+1 to 2N represent the predicted variances.
	%
	N=size(target,1)+lag;
	if size(simulated_y,1)==N
		simulated_y=[simulated_y;simulated_y*NaN];
	end

	simulated_input0 = construct(lag, u, simulated_y(1:N,end));

	[y_mu, se2] = feval(simf,hyp, inf, mean, cov, lik, simulated_input0, ...
	target, simulated_input0, lag);

	%y_mu_full combines measured initial values and simulated response:
	simulated_y(1:N,end+1)=[simulated_y(1:lag,end);y_mu(:)];
	simulated_y(N+1:2*N,end)=[zeros(lag,1);se2(:)];

	simulated_input1 = construct(lag, u, simulated_y(1:N,end));
	[nlZ,dnlZ]=gp(hyp,inf,mean,cov,lik,simulated_input1,target);
end

simy=simulated_y(1:end/2,:);
simse2=simulated_y(end/2:end,:);
end


function sensor_data = kspaceSecondOrder(kgrid, medium, source, sensor, varargin)
%KSPACESECONDORDER  Fast time-domain simulation of wave propagation for homogeneous media.
%
% DESCRIPTION:
%       kspaceSecondOrder simulates the time-domain propagation of linear
%       compressional waves through a one, two, or three dimensional
%       homogeneous acoustic medium given four input structures: kgrid,
%       medium, source, and sensor. The computation is based on an exact
%       second-order k-space model for media with power law absorption. At
%       each time-step (defined by kgrid.t_array), the pressure at the
%       positions defined by sensor.mask are recorded and stored. If
%       kgrid.t_array is set to 'auto', this array is automatically
%       generated using makeTime. To prevent wave wrapping, the
%       computational domain can be automatically expanded by a factor of
%       two by setting the optional input 'ExpandGrid' to true.
%
%       An initial pressure distribution can be specified by assigning a
%       matrix (the same size as the computational grid) of arbitrary
%       numeric values to source.p0. An initial pressure gradient can
%       similarly be specified using source.dp0dt. The pressure is returned
%       as an array of time series at the sensor locations defined by
%       sensor.mask. This is specified as a binary matrix (i.e., a matrix
%       of 1's and 0's the same size as the computational grid)
%       representing the grid points within the computational grid that
%       will collect the data. The sensor_data is returned using MATLAB's
%       standard column-wise linear matrix index ordering with the recorded
%       data indexed as sensor_data(sensor_position, time). The final
%       pressure field over the complete computational grid can also be
%       obtained by setting sensor.record to {'p', 'p_final'}. In this
%       case, the output sensor_data is returned as a structure with the
%       outputs appended as the structure fields sensor_data.p and
%       sensor_data.p_final.
%
%       Compared to the first-order simulation functions
%       kspaceFirstOrder1D, kspaceFirstOrder2D, and kspaceFirstOrder3D,
%       kspaceSecondOrder is restricted to homogeneous media and has less
%       functionality. However, it is also more computationally efficient
%       and allows an initial pressure gradient to be specified.
%
% USAGE:
%       sensor_data = kspaceSecondOrder(kgrid, medium, source, sensor)
%       sensor_data = kspaceSecondOrder(kgrid, medium, source, sensor, ...)
%
% INPUTS:
%       kgrid               - k-Wave grid structure returned by makeGrid
%                             containing Cartesian and k-space grid fields
%       kgrid.t_array       - evenly spaced array of time values [s] (set
%                             to 'auto' by makeGrid)
%
%       medium.sound_speed  - homogeneous sound speed within the acoustic
%                             medium [m/s]
%       medium.alpha_power  - power law absorption exponent
%       medium.alpha_coeff  - power law absorption coefficient
%                             [dB/(MHz^y cm)]
%       medium.alpha_mode   - optional input to force either the absorption
%                             or dispersion terms in the equation of state
%                             to be excluded; valid inputs are
%                             'no_absorption' or 'no_dispersion'
%
%       source.p0           - initial pressure within the acoustic medium
%       source.dp0dt        - initial pressure gradient within the acoustic
%                             medium
%
%       sensor.mask         - binary grid specifying where the pressure is
%                             recorded at each time-step
%       sensor.record       - cell array of the acoustic parameters to
%                             record in the form sensor.record = {'p'};
%                             valid inputs are:
%                               'p'       (acoustic pressure)
%                               'p_final' (final pressure field)
%
% OPTIONAL INPUTS:
%       Optional 'string', value pairs that may be used to modify the
%       default computational settings.
%
%       'ExpandGrid'    - Boolean controlling whether the grid size is
%                         expanded on two sides to delay the time before
%                         wave wrapping occurs (default = false).
%       'MeshPlot'      - Boolean controlling whether mesh is used in place
%                         of imagesc to plot the pressure field (default =
%                         false).
%       'PlotFrames'    - Boolean controlling whether the pressure field
%                         for each time step is plotted in a new window
%                         (default = false).
%       'PlotFreq'      - The number of iterations which must pass before
%                         the simulation plot is updated (default = 10).
%       'PlotScale'     - [min, max] values used to control the scaling for
%                         imagesc (visualisation) (default = [-1, 1].
%       'PlotSim'       - Boolean controlling whether the simulation
%                         iterations are progressively plotted (default =
%                         true).
%       'Smooth'        - Boolean controlling whether source.p0 is smoothed
%                         using smooth before computation (default = true).
%
% OUTPUTS:
% If sensor.record is not defined by the user:
%       sensor_data         - time varying pressure recorded at the sensor
%                             positions given by sensor.mask
%
% If sensor.record is defined by the user:
%       sensor_data.p       - time varying pressure recorded at the sensor
%                             positions given by sensor.mask (returned if
%                             'p' is set)
%       sensor_data.p_final - final pressure field over the complete domain
%                             (returned if 'p_final' is set)
%
% ABOUT:
%       author      - Bradley Treeby and Ben Cox
%       date        - 21st August 2008
%       last update - 25th August 2014
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox
%
% See also kspaceFirstOrder1D, kspaceFirstOrder2D, kspaceFirstOrder3D,
% makeGrid, makeTime, smooth

% This file is part of k-Wave. k-Wave 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.
%
% k-Wave is distributed in the hope that it will be useful, but WITHOUT ANY
% WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for
% more details.
%
% You should have received a copy of the GNU Lesser General Public License
% along with k-Wave. If not, see <http://www.gnu.org/licenses/>.

% start the timer
tic;

% =========================================================================
% DEFINE LITERALS
% =========================================================================

% minimum number of input variables
NUM_REQ_INPUT_VARIABLES = 4;

% optional input defaults
EXPAND_GRID_DEF = false;
MESH_PLOT_DEF = false;
PLOT_FRAMES_DEF = false;
PLOT_FREQ_DEF = 10;
PLOT_SCALE_DEF = [-1 1];
PLOT_SIM_DEF = true;
SMOOTH_P0_DEF = true;

% create colormap for visualisations
COLOR_MAP = getColorMap;

% =========================================================================
% CHECK INPUT STRUCTURES
% =========================================================================

% calculate t_array using makeTime if it is not given
if strcmp(kgrid.t_array, 'auto')
    kgrid.t_array = makeTime(kgrid, medium.sound_speed);
end

% check medium fields
checkFieldNames(medium, {'sound_speed', 'density', 'alpha_coeff', 'alpha_power', 'alpha_mode'});
enforceFields(medium, {'sound_speed'});
if isfield(medium, 'density')
    disp('WARNING: medium.density is not a valid input parameter');
end

% check source fields
checkFieldNames(source, {'p0', 'dp0dt'});

% check the absorption mode input is valid
if isfield(medium, 'alpha_mode')
    if ~ischar(medium.alpha_mode) || (~strcmp(medium.alpha_mode, 'no_absorption') && ~strcmp(medium.alpha_mode, 'no_dispersion'))
        error('medium.alpha_mode must be set to ''no_absorption'' or ''no_dispersion''');
    end
end

% extract which source fields have been given
source_case = 0;
if isfield(source, 'p0')
    source_case = source_case + 1;
end
if isfield(source, 'dp0dt')
    source_case = source_case + 2;
end

% check sensor fields
checkFieldNames(sensor, {'mask', 'record'});
enforceFields(sensor, {'mask'});

% check the sensor mask is binary
if sum(sensor.mask(:)) ~= numel(sensor.mask) - sum(sensor.mask(:) == 0)
    error('sensor.mask must be a binary grid (numeric values must be 0 or 1)');
end

% set default record options
record.p = true;
record.p_final = false;

% check for sensor.record and set usage flags - if no flags are given, the
% time history of the acoustic pressure is recorded by default
if isfield(sensor, 'record')

    % list of allowable record flags
    record.flags = {'p', 'p_final'};

    % check the contents of the cell array are valid inputs
    for record_index = 1:length(sensor.record)
        if ~ismember(sensor.record(record_index), record.flags);
            error(['''' sensor.record{record_index} ''' is not a valid input for sensor.record']);
        end
    end

    % set the usage flags depending on the cell array
    if ismember('p', sensor.record)
        record.p = true;
    else
        % set record.p to false if a user input for sensor.record is given
        % and 'p' is not set
        record.p = false;
    end
    if ismember('p_final', sensor.record)
        record.p_final = true;
    end

end

% =========================================================================
% CHECK OPTIONAL INPUTS
% =========================================================================

% shorten commonly used field names
t_array = kgrid.t_array;
c = medium.sound_speed;
if isfield(medium, 'alpha_coeff')
    a0 = medium.alpha_coeff;
    y = medium.alpha_power;
else
    a0 = 0;
end

% assign default input parameters
expand_grid = EXPAND_GRID_DEF;
mesh_plot = MESH_PLOT_DEF;
plot_frames = PLOT_FRAMES_DEF;
plot_freq = PLOT_FREQ_DEF;
plot_scale = PLOT_SCALE_DEF;
plot_sim = PLOT_SIM_DEF;
smooth_p0 = SMOOTH_P0_DEF;

% replace defaults with user defined values if provided and check inputs
if nargin < NUM_REQ_INPUT_VARIABLES
    error('Not enough input parameters');
elseif rem(nargin - NUM_REQ_INPUT_VARIABLES, 2) %**
    error('Optional input parameters must be given as param, value pairs');
elseif ~isempty(varargin)
    for input_index = 1:2:length(varargin)
        switch varargin{input_index}
            case 'ExpandGrid'
                expand_grid = varargin{input_index + 1};
                if ~islogical(expand_grid)
                    error('Optional input ExpandGrid must be Boolean');
                end
            case 'MeshPlot'
                mesh_plot = varargin{input_index + 1};
                if ~islogical(mesh_plot)
                    error('Optional input MeshPlot must be Boolean');
                end
            case 'PlotFrames'
                plot_frames = varargin{input_index + 1};
                if ~islogical(plot_frames)
                    error('Optional input PlotFrames must be Boolean');
                end
            case 'PlotFreq'
                plot_freq = varargin{input_index + 1};
                if ~(numel(plot_freq) == 1 && isnumeric(plot_freq))
                    error('Optional input PlotFreq must be a single numerical value');
                end
            case 'PlotScale'
                plot_scale = varargin{input_index + 1};
                if ~(numel(plot_scale) == 2 && isnumeric(plot_scale))
                    error('Optional input PlotScale must be a 2 element numerical array');
                end
            case 'PlotSim'
                plot_sim = varargin{input_index + 1};
                if ~islogical(plot_sim)
                    error('Optional input PlotSim must be Boolean');
                end
            case 'Smooth'
                smooth_p0 = varargin{input_index + 1};
                if ~islogical(smooth_p0)
                    error('Optional input Smooth must be Boolean');
                end
            otherwise
                error('Unknown optional input');
        end
    end
end

% cleanup unused variables
clear *_DEF NUM_REQ_INPUT_VARIABLES time;

% update command line status
disp('Running k-space simulation...');

% =========================================================================
% UPDATE COMMAND LINE STATUS
% =========================================================================

% update command line status
disp(['  time steps: ' num2str(length(t_array))]);
switch numDim(kgrid.k)
    case 1
        disp(['  input grid size: ' num2str(kgrid.Nx) ' grid points (' scaleSI(kgrid.x_size) 'm)']);
    case 2
        [x_sc, scale, prefix] = scaleSI(min(kgrid.x_size, kgrid.y_size)); %#ok<*ASGLU>
        disp(['  input grid size: ' num2str(kgrid.Nx) ' by ' num2str(kgrid.Ny) ' grid points (' num2str(kgrid.x_size*scale) ' by ' num2str(kgrid.y_size*scale) prefix 'm)']);
    case 3
        [x_sc, scale, prefix] = scaleSI(min([kgrid.x_size, kgrid.y_size, kgrid.z_size])); %#ok<ASGLU>
        disp(['  input grid size: ' num2str(kgrid.Nx) ' by ' num2str(kgrid.Ny) ' by ' num2str(kgrid.Nz) ' grid points (' num2str(kgrid.x_size*scale) ' by ' num2str(kgrid.y_size*scale) ' by ' num2str(kgrid.z_size*scale) prefix 'm)']);
end
disp(['  maximum supported frequency: ' scaleSI( kgrid.k_max * min(c(:)) / (2*pi) ) 'Hz']);

% =========================================================================
% CALCULATE FFT OF SOURCE FUNCTIONS AND EXPAND GRID
% =========================================================================

% smooth p0 distribution if required restoring the maximum magnitude
if smooth_p0
    if source_case ~= 2
        disp('  smoothing source.p0 distribution...');
        source.p0 = smooth(kgrid, source.p0, true);
    end
    if source_case > 1
        disp('  smoothing source.dp0dt distribution...');
        source.dp0dt = smooth(kgrid, source.dp0dt, true);
    end
end

if ~expand_grid
    % extract wavenumber matrix from kgrid
    k = kgrid.k;

    % extract the location of the sensor points
    sensor_index = (sensor.mask == 1);

    % compute FFT of source functions
    if source_case ~= 2
        p0_k = fftn(source.p0);
    end
    if source_case > 1
        dp0dt_k = fftn(source.dp0dt);
    end
else
    switch numDim(kgrid.k)
        case 1

            % create a larger kgrid to prevent wave wrapping
            kgrid_APE = makeGrid(kgrid.Nx*2, kgrid.dx);

            % extract wavenumber matrix from kgrid
            k = kgrid_APE.k;

            % expand the sensor mask
            sensor_mask = [sensor.mask, zeros(size(sensor.mask))];

            % expand the source functions
            if source_case ~= 2
                p0 = zeros(size(k));
                p0(1:kgrid.Nx) = source.p0;
            end
            if source_case > 1
                dp0dt = zeros(size(k));
                dp0dt(1:kgrid.Nx) = source.dp0dt;
            end

        case 2

            % create a larger kgrid to prevent wave wrapping
            kgrid_APE = makeGrid(kgrid.Nx*2, kgrid.dx, kgrid.Ny*2, kgrid.dy);

            % extract wavenumber matrix from kgrid
            k = kgrid_APE.k;

            % expand the sensor mask
            sensor_mask = zeros(size(k));
            sensor_mask(1:kgrid.Nx, 1:kgrid.Ny) = sensor.mask;

            % expand the source functions
            if source_case ~= 2
                p0 = zeros(size(k));
                p0(1:kgrid.Nx, 1:kgrid.Ny) = source.p0;
            end
            if source_case > 1
                dp0dt = zeros(size(k));
                dp0dt(1:kgrid.Nx, 1:kgrid.Ny) = source.dp0dt;
            end

        case 3

            % create a larger kgrid to prevent wave wrapping
            kgrid_APE = makeGrid(kgrid.Nx*2, kgrid.dx, kgrid.Ny*2, kgrid.dy, kgrid.Nz*2, kgrid.dz);

            % extract wavenumber matrix from kgrid
            k = kgrid_APE.k;

            % extract the location of the sensor points within the enlarged grid
            sensor_mask = zeros(size(k));
            sensor_mask(1:kgrid.Nx, 1:kgrid.Ny, 1:kgrid.Nz) = sensor.mask;

            % expand the source functions
            if source_case ~= 2
                p0 = zeros(size(k));
                p0(1:kgrid.Nx, 1:kgrid.Ny, 1:kgrid.Nz) = source.p0;
            end
            if source_case > 1
                dp0dt = zeros(size(k));
                dp0dt(1:kgrid.Nx, 1:kgrid.Ny, 1:kgrid.Nz) = source.dp0dt;
            end

    end

    % extract the location of the sensor points within the enlarged grid
    sensor_index = (sensor_mask == 1);

    % compute FFT of source functions
    if source_case ~= 2
        p0_k = fftn(p0);
    end
    if source_case > 1
        dp0dt_k = fftn(dp0dt);
    end

    % delete unused variables
    clear kgrid_APE sensor_mask p0 dp0dt source;
end

% =========================================================================
% DEFINE TIME PROPAGATORS
% =========================================================================

% shift the wavenumbers
k = ifftshift(k);

% find the index of the zero wave numbers
if source_case > 1
    k_0_index = find(k == 0);
end

% define time propagation function
if a0 ~= 0

    % convert attenuation to nepers
    a0 = db2neper(a0, y);

    % define coefficients
    if y == 2 || (isfield(medium, 'alpha_mode') && strcmp('no_dispersion', medium.alpha_mode))

        % compute Upsilon
        Upsilon = sqrt(1 - a0^2*c^(2*y)*k.^(2*y-2));

        % reset first value to avoid possible INF when k = 0
        Upsilon(1) = 1;

        % precalculate coefficients to speed up computation
        ckU = c*k.*Upsilon;
        coeff_1 = a0*c^(y + 1)*k.^(y);
        coeff_2 = 0;

    elseif (isfield(medium, 'alpha_mode') && strcmp('no_absorption', medium.alpha_mode))

        % compute Upsilon
        Upsilon = sqrt(1 - 2*a0*c^y*k.^(y-1)*tan(pi*y/2));

        % reset first value to avoid possible INF when k = 0
        Upsilon(1) = 1;

        % precalculate coefficients to speed up computation
        ckU = c*k.*Upsilon;
        coeff_1 = 0;
        coeff_2 = a0*c^y*k.^(y-1) ./ Upsilon;

        % reset first value to avoid possible INF when k = 0
        coeff_2(1) = 0;

    else

        % compute Upsilon
        Upsilon = sqrt(1 - a0^2*c^(2*y)*k.^(2*y-2) - 2*a0*c^y*k.^(y-1)*tan(pi*y/2));

        % reset first value to avoid possible INF when k = 0
        Upsilon(1) = 1;

        % precalculate coefficients to speed up computation
        ckU = c*k.*Upsilon;
        coeff_1 = a0*c^(y + 1)*k.^(y);
        coeff_2 = a0*c^y*k.^(y-1) ./ Upsilon;

        % reset first value to avoid possible INF when k = 0
        coeff_2(1) = 0;

    end

else
    % precalculate coefficients to speed up computation
    ck = c*k;
end

% =========================================================================
% PREPARE VISUALISATIONS AND STORAGE VARIABLES
% =========================================================================

% preallocate storage variables
if record.p
    sensor_data.p = zeros(sum(sensor.mask(:)), length(t_array));
end

% pre-compute suitable axes scaling factor
if plot_sim
    switch numDim(kgrid.k)
        case 1
            [x_sc, scale, prefix] = scaleSI(max(kgrid.x_vec));
        case 2
            [x_sc, scale, prefix] = scaleSI(max([kgrid.x_vec; kgrid.y_vec]));
        case 3
            [x_sc, scale, prefix] = scaleSI(max([kgrid.x_vec; kgrid.y_vec; kgrid.z_vec]));
    end
end

% initialise the figures used for animation if 'PlotSim' is set to 'true'
if plot_sim
    % create empty figure
    img = figure;

    % create waitbar and shift position so it doesn't overlap the figure window
    pbar = waitbar(0, 'Computing Pressure Field', 'Visible', 'off');
    posn_pbar = get(pbar, 'OuterPosition');
    posn_img = get(img, 'OuterPosition');
    posn_pbar(2) = max(min(posn_pbar(2) - posn_pbar(4), posn_img(2) - posn_pbar(4) - 10), 0);
    set(pbar, 'OuterPosition', posn_pbar, 'Visible', 'on');
end

% update command line status
disp(['  precomputation completed in ' scaleTime(toc)]);
disp('  starting time loop...');

% =========================================================================
% LOOP THROUGH TIME STEPS
% =========================================================================

% loop through each value of t
for t_index = 1:length(t_array)

    % extract the time point
    t = t_array(t_index);

    % compute pressure field
    switch (source_case + 3*(a0 ~= 0))
        case 1
            TP_p0 = cos(ck*t);
            p = ifftn(  p0_k.*TP_p0  );
        case 2
            TP_dp0dt = sin(ck*t) ./ (ck);
            TP_dp0dt(k_0_index) = t; %#ok<*FNDSB>
            p = ifftn(  dp0dt_k.*TP_dp0dt  );
        case 3
            TP_p0 = cos(ck*t);
            TP_dp0dt = sin(ck*t) ./ (ck);
            TP_dp0dt(k_0_index) = t;
            p = ifftn(  p0_k.*TP_p0 + dp0dt_k.*TP_dp0dt  );
        case 4
            TP_p0 = exp(-coeff_1*t) .* ( cos(ckU*t) - coeff_2 .* sin(ckU*t) );
            p = ifftn(  p0_k.*TP_p0  );
        case 5
            TP_dp0dt = sin(ckU*t) .* exp(-coeff_1*t) ./ ckU;
            TP_dp0dt(k_0_index) = t.* exp(-coeff_1(k_0_index)*t);
            p = ifftn(  dp0dt_k.*TP_dp0dt  );
        case 6
            TP_p0 = exp(-coeff_1*t) .* ( cos(ckU*t) - coeff_2 .* sin(ckU*t) );
            TP_dp0dt = sin(ckU*t) .* exp(-coeff_1*t) ./ ckU;
            TP_dp0dt(k_0_index) = t.* exp(-coeff_1(k_0_index)*t);
            p = ifftn(  p0_k.*TP_p0 + dp0dt_k.*TP_dp0dt  );
    end

    % extract required data
    if record.p
        sensor_data.p(:, t_index) = real(p(sensor_index));
    end

    % plot data if required
    if plot_frames || ( plot_sim && rem(t_index, plot_freq) == 0 )

        % update progress bar
        waitbar(t_index/length(t_array), pbar);
        drawnow;

        % update plot
        if plot_frames
            figure;
        end

        p = real(p);

        switch numDim(kgrid.k)
            case 1
                % extract the required pressure field from the enlarged grid
                if expand_grid
                    p = p(1:kgrid.Nx);
                end
                plot(kgrid.x*scale, p);
                xlabel(['x-position [' prefix 'm]']);
                set(gca, 'YLim', plot_scale);

            case 2
                % extract the required pressure field from the enlarged grid
                if expand_grid
                    p = p(1:kgrid.Nx, 1:kgrid.Ny);
                end

                if mesh_plot
                    mesh(kgrid.x_vec*scale, kgrid.y_vec*scale, p, 'EdgeColor', 'Black');
                    axis image;
                    set(gca, 'ZLim', plot_scale);
                    axis off;
                else
                    % add sensor mask onto plot
                    p(sensor.mask == 1) = plot_scale(2);

                    imagesc(kgrid.y_vec*scale, kgrid.x_vec*scale, p, plot_scale);
                    colormap(COLOR_MAP);
                    axis image;
                    ylabel(['x-position [' prefix 'm]']);
                    xlabel(['y-position [' prefix 'm]']);
                end

            case 3
                % extract the required pressure field from the enlarged grid
                if expand_grid
                    p = p(1:kgrid.Nx, 1:kgrid.Ny, 1:kgrid.Nz);
                end
                planeplot(kgrid, p, '', plot_scale, scale, prefix, COLOR_MAP);
        end
    end
end

% =========================================================================
% CLEAN UP
% =========================================================================

% assign output data
if ~isfield(sensor, 'record')
    sensor_data = sensor_data.p;
elseif record.p_final
    switch numDim(kgrid.k)
        case 1
            sensor_data.p_final = p(1:kgrid.Nx);
        case 2
            sensor_data.p_final = p(1:kgrid.Nx, 1:kgrid.Ny);
        case 3
            sensor_data.p_final = p(1:kgrid.Nx, 1:kgrid.Ny, 1:kgrid.Nz);
    end
end

% clean up used figures
if plot_sim
    close(img);
    close(pbar);
end

% update command line status
disp(['  computation completed in ' scaleTime(toc)]);

function planeplot(kgrid, data, data_title, plot_scale, scale, prefix, color_map)
% Subfunction to produce a plot of a three-dimensional matrix through the
% three central planes

subplot(2, 2, 1), imagesc(kgrid.y_vec*scale, kgrid.x_vec*scale, squeeze(data(:, :, kgrid.Nz/2)), plot_scale);
title([data_title 'x-y plane']);
axis image;
subplot(2, 2, 2), imagesc(kgrid.z_vec*scale, kgrid.x_vec*scale, squeeze(data(:, kgrid.Ny/2, :)), plot_scale);
title('x-z plane');
axis image;
xlabel(['(All axes in ' prefix 'm)']);
subplot(2, 2, 3), imagesc(kgrid.z_vec*scale, kgrid.y_vec*scale, squeeze(data(kgrid.Nx/2, :, :)), plot_scale);
title('y-z plane');
axis image;
colormap(color_map);
drawnow;


function [x,options]=simps(fun,x,ix,options,vlb,vub,varargin)
%SIMPS (Strategy Simplex) finds the constrained minimum.
%
%   Syntax:
%   [X1,OPTIONS]=SIMPS(FUN,X0,IX,OPTIONS,VLB,VUB,P1,P2...)
%   where IX,OPTIONS,VLB,VUB,P1,P2... are optional.
%
%   Compare to Matlab's functions:
%   [X1,OPTIONS]=FMINS(FUN,X0,OPTIONS,[],P1,P2...)
%   [X1,OPTIONS]=CONSTR(FUN,X0,OPTIONS,VLB,VUB,[],P1,P2...) - OPTIM
%   toolbox
%
%   'FUN' - function to be minimized (usually an M-file: FUN.M).
%   It must be of the form: [F,G]=FUN(X,P1,P2...), where F returns the
%   value, and G returns the vector of constraints.
%   Constraints are satisfied at the point X if the respective
%   components of G are non-positive. For the non-constrained
%   minimization, use G=[].
%   The point X can be a [real] vector, matrix, or several dimensional
%   array. Both F and G must be well defined for all VLB<=X<=VUB (see
%   below).
%   P1,P2... are optional problem-dependent parameters, bypassed by
%   the minimizer SIMPS.
%
%   X0 - initial guess,
%   X1 - point of minimum as obtained;
%   both of the same type and dimension as X in the function 'FUN'.
%
%   IX - integer vector, subset of 1:PROD(SIZE(X0)) (which is also its
%   default value). The minimization is performed only against X0(IX),
%   while the other components of X0 are supposed to be fixed.
%   Let us denote LX=LENGTH(IX).
%
%   OPTIONS - see FOPTIONS. Only the following components are used:
%   Input:
%   OPTIONS(1)  - Whether to display intermediate results (1),
%                 or not (0, default).
%   OPTIONS(2)  - Relative X-termination tolerance (default: 1e-4).
%   OPTIONS(3)  - Relative F-termination tolerance (default: 1e-4).
%   OPTIONS(14) - Maximum number of function evaluations per each
%                 internal LX-dim. and 2-dim. simplex call
%                 (default: 100*LX).
%   Output:
%   OPTIONS(8)  - Function value at the point X1 (minimum).
%   OPTIONS(10) - Total number of function evaluations.
%
%   VLB,VUB - lower and upper bounds for X.
%   Both must be vectors of length PROD(SIZE(X0)) - total number of
%   components of X0.
%   Supply -INF, resp. +INF, (default) for unrestricted coordinates.
%
%   Comments:
%
%   - SIMPS is an iterative method, where each iteration consists of
%   one LX (full dim.) Nelder-Mead simplex call (direct search),
%   followed by LX+1 two dim. Nelder-Mead simplex calls (working on 2
%   dim. subspaces). These LX+1 two-dim. simplex calls provide initial
%   vertices V for the next full dim. simplex call (next iteration).
%
%   - The number of outer iterations is limited to LX or until the
%   longest dimension of simplex V (see above) falls below the
%   X-tolerance (OPTIONS(2)).
%   The internal LX and 2 dim. simplex calls are limited by proposed
%   F-tolerance or max. number of function evaluations (OPTIONS(2) and
%   OPTIONS(14)).
%
%   - The inner LX and 2 dim. Nelder-Mead minimizers are both realized
%   through the calls to the internal AMOEBA function, based
%   essentially on the code of the Matlab's FMINS non-linear simplex
%   implementation.
%
%   - The constraints are basically implemented by penalizing the
%   function (see the internal WRAPPER function): unsatisfied
%   constraints (-sum(G(find(G<0))), multiplied by factor 1e6, are
%   being added to the function value F.
%
%   - The index vector IX provides a useful and convenient method for
%   numerical experiments with complicated unknown minima, allowing to
%   user to perform searches limited to smaller dimensional subspaces.
%   This extra feature does not require run-time editing on the side
%   of target function 'FUN', as would be the case if certain
%   variables were fixed by use of optional parameters P1,P2...
%
%   - In contrast to the Matlab's FMINS, CONSTR and other minimizers,
%   both X and F termination tolerances are considered relative, not
%   absolute.
%   Nevertheless, X, F and G should be ideally of the same order of
%   magnitude (1), at least locally around the point of minimum.
%
%   - There is of course no guarantee that the output X1 represents
%   the point of global minimum as required.
%   However, the method (strategy) provides an advantage over the
%   plain nonlinear simplex, and it has been proved to be specially
%   useful for target functions with plenty of narrow local minima -
%   standard traps for analitically based minimizers.
%   The method is not limited to continuos functions and does not
%   require derivatives.
%
%   Authors:
%   Zeljko Bajzer (bajzer@mayo.edu) and Ivo Penzar (penzar@mayo.edu)
%   Mayo Clinic and Foundation, Rochester, Minnesota, USA
%   June, 1998.

if nargin<2, error('simps requires two input arguments'); end
if nargin<3, ix=1:prod(size(x)); end
ix=unique(ix); lx=length(ix);
if nargin<4, options=[]; end
if nargin<5, vlb=-Inf+zeros(size(1,lx)); end
if nargin<6, vub=Inf+zeros(size(1,lx)); end

options=foptions(options);
prnt=options(1);
if ~options(14)
 options(14)=50*lx;
end
tolx=options(2);
tolf=options(3);
maxfcalls=options(14);

varargs={vlb,vub,fun,varargin{:}};

xx=x(ix)';
if all(abs(xx)<eps)
 xx=xx+eps;
end
d=0.5;
% Guess goes to the centre:
%v=repmat((1-d/(lx+1))*xx,1,lx+1)+[diag(d*xx) zeros(lx,1)];
% Guess goes to a corner:
v=repmat(xx,1,lx+1)+[diag(d*xx) zeros(lx,1)];
fcalls=0; dv=Inf;
if prnt
 fprintf('%7s %10s %12s\n','f-COUNT','FUNCTION','x-TOL');
end
i=0;
while (i<lx)&(dv>=tolx)
 [y,fmin,tmp4]=amoeba(v,tolf,maxfcalls,...
                      x,ix,varargs{:});
 y=y'; x(ix)=y;
 fcalls=fcalls+tmp4;
 if prnt
  fprintf('%7d %10.5f\n',fcalls,fmin);
 end
 if lx==1
  break
 end
 if lx==2
  v(:,1)=y;
 else
  for j=1:lx-1
   j1=[j j+1];
   [y(j1),fmin,tmp4]=amoeba([xx(j1) y(j1) [xx(j1(1)) y(j1(2))]'],...
                            tolf,maxfcalls,...
                            x,ix(j1),varargs{:});
   x(ix)=y; v(:,j)=y;
   fcalls=fcalls+tmp4;
  end
 end
 j1=[1 lx];
 [y(j1),fmin,tmp4]=amoeba([xx(j1) y(j1) [xx(j1(1)) y(j1(2))]'],...
                          tolf,maxfcalls,...
                          x,ix(j1),varargs{:});
 x(ix)=y; v(:,lx)=y;
 fcalls=fcalls+tmp4;
 if lx>3
  j1=[2 lx-1];
 end
 [y(j1),fmin,tmp4]=amoeba([xx(j1) y(j1) [y(j1(1)) xx(j1(2))]'],...
                          tolf,maxfcalls,...
                          x,ix(j1),varargs{:});
 x(ix)=y; v(:,lx+1)=y;
 fcalls=fcalls+tmp4;
 dv=max(tol(min(v,[],2),max(v,[],2)));
 if prnt
  fprintf('%7d %10.5f %12.6f\n',fcalls,fmin,dv);
 end
 i=i+1;
end
if prnt
 fprintf('\n');
end

options(8)=fmin;
options(10)=fcalls;

return

function [x,fmin,fcalls]=amoeba(v,tolf,maxfcalls,varargin)
% Internal Nelder-Mead minimizer, based essentially on the code
% of the Matlab's FMINS non-linear simplex implementation.
%
% - Input and output parameters are changed
%   (e.g., initial simplex v is provided from outside);
% - There is no x-tolerance termination checking;
% - There is no display of intermediate results.

rho=1; chi=2; psi=0.5; sigma=0.5;

n=length(v(:,1));
onesn=ones(1,n);
two2np1=2:n+1;
one2n=1:n;
zerosn=zeros(1,n);
x=zerosn;

fv=zeros(1,n+1);
for j=1:n+1
 x(:)=v(:,j);
 fv(j)=wrapper(x,varargin{:});
end
[fv,jj]=sort(fv);
v=v(:,jj);
fmin=fv(1); fcalls=n+1;

while (fcalls<maxfcalls)&...
      (tol(fmin,fv(n+1))>tolf)
 shrink=0;
 xbar=sum(v(:,one2n),2)/n;
 xr=(1+rho)*xbar-rho*v(:,n+1);
 x(:)=xr;
 fxr=wrapper(x,varargin{:});
 fcalls=fcalls+1;

 if fxr<fv(1)
  xe=(1+rho*chi)*xbar-rho*chi*v(:,n+1);
  x(:)=xe;
  fxe=wrapper(x,varargin{:});
  fcalls=fcalls+1;
  if fxe<fxr
   v(:,n+1)=xe;
   fv(n+1)=fxe;
  else
   v(:,n+1)=xr;
   fv(n+1)=fxr;
  end

 elseif fxr<fv(n)
  v(:,n+1)=xr;
  fv(n+1)=fxr;

 else
  if fxr<fv(n+1)
   xc=(1+psi*rho)*xbar-psi*rho*v(:,n+1);
   x(:)=xc;
   fxc=wrapper(x,varargin{:});
   fcalls=fcalls+1;
   if fxc<=fxr
    v(:,n+1)=xc;
    fv(n+1)=fxc;
   else
    shrink=1;
   end
  else
   xcc=(1-psi)*xbar+psi*v(:,n+1);
   x(:)=xcc;
   fxcc=wrapper(x,varargin{:});
   fcalls=fcalls+1;
   if fxcc<fv(n+1)
    v(:,n+1)=xcc;
    fv(n+1)=fxcc;
   else
    shrink=1;
   end
  end
  if shrink
   for j=two2np1
    v(:,j)=v(:,1)+sigma*(v(:,j)-v(:,1));
    x(:)=v(:,j);
    fxcc=wrapper(x,varargin{:});
   end
   fcalls=fcalls+n;
  end
 end

 [fv,jj]=sort(fv);
 v=v(:,jj);
 fmin=fv(1);
end
x(:)=v(:,1);

return

function r=tol(a,b)
% Internal helper function, calculates the relative distances
% between points a and b, componentwise.

c=(abs(b)+abs(a))/2;
j=find(c<eps); c(j)=ones(size(j));
r=abs(b-a)./c;

return

function fmin=wrapper(x,x0,ix,vlb,vub,funfcn,varargin)
% Internal wrapper function, provides a proper interface for
% AMOEBA to the user's target function.

x1=zeros(1,prod(size(x0)));
x1(:)=x0;
x1(ix)=x;
x0(:)=max(min(x1,vub),vlb);

funfcn=fcnchk(funfcn,1+length(varargin));
[fmin,fconstr]=feval(funfcn,x0,varargin{:});

fconstr=[vlb-x1,x1-vub,fconstr];
fmin=fmin+1e6*sum(fconstr(find(fconstr>0)));

return


%
% [f, Covf, pseudoX, logtheta, Kpp, Kxp, Lp] =
%     gplearn(y, V, X, pseudoX, kX, logtheta, ...)
%
% V is inverse covariance (i.e. precision) matrix for observation noise -
% sparse format is recommended.
%
% In practice, inv(V) should be diagonal. However, inv(V) can be more
% complex sparse matrix as long as it has zeros where kX is non-zero. That
% is, observation noise can be correlated if the corresponding latent
% function values are independent a priori. This more general property is
% used in PCA model, where inv(V) and kX are block-diagonal (although rows
% and columns may be in mixed order!) but the blocks "overlap" only on the
% diagonal, that is, they both have non-zero elements at the same time only
% on the diagonal.
%
% y = inv(V)*c, that is, c = V*y
% N( inv(V)*c | 0, inv(V) + Kxp inv(Kpp) Kpx )

% $$$ function [f, Covf, pseudoX, logtheta, Kpp, Kxp, Lp] = gplearn(y, V, ...
% $$$                                                   X, pseudoX, kX, logtheta, ...
% $$$                                                   varargin)
function [f, Covf, Xp, logtheta, Kpp, Kxp, Lp] = gplearn(logtheta, covfunc, ...
                                                  X, y, V, Xp, varargin)

% function [] = gp_learn(covfunc, theta, y, V)

warning('Deprecated. Use gp_learn or gp_learn_pseudo');

if ~iscell(covfunc)
  covfunc = {covfunc};
end

opts = struct( ...
    'maxsearch', 10, ...
    'vy', [], ...
    'cholv', [], ...
    'checkgrad', false, ...
    'updatepseudo', true, ...
    'priorlogtheta', []);

[ opts, errmsg, wrnmsg ] = argschk( opts, varargin{:} );
if ~isempty(errmsg), error( errmsg ), end
if ~isempty(wrnmsg), warning( wrnmsg ), end

m = cols(Xp); % number of pseudoinputs
d = rows(X); % dimensionality of the input space

if isempty(opts.cholv)
  LV = chol(V, 'lower');
%  [LV,V] = safechol(V, 1e-16, 'lower');
else
  LV = opts.cholv;
end

if isempty(opts.vy)
  c = V*y;
else
  c = opts.vy;
end

if opts.maxsearch ~= 0
  if opts.updatepseudo && ~isempty(Xp)
    % Learn pseudo-inputs and hyperparameters
    x0 = [Xp(:); logtheta(:)];
    func = @(x) bound2cost(x, c, V, LV, X, covfunc, m, d, true);
    fiter = inf;
    while ~any(~isinf(fiter))
      [x, fiter] = minimize(x0, func, opts.maxsearch);
      x0 = x0 - 1; % adhoc trying to help ill conditioned matrices..
    end
    Xp = reshape(x(1:(m*d)), [d,m]);
    logtheta = x((m*d+1):end);
  else
    % Learn only hyperparameters
    x0 = logtheta(:);
    func = @(x) bound2cost(x, c, V, LV, X, covfunc, [], d, false, Xp);
    fiter = inf;
    while ~any(~isinf(fiter))
      [logtheta, fiter] = minimize(x0, func, opts.maxsearch);
      x0 = x0 - 1; % adhoc trying to help ill conditioned matrices..
    end
  end
  if opts.checkgrad
    mycheckgrad(func, x0, 1e-8)
  end
end

% THE FOLLOWING WOULD COME AS A SIDE PRODUCT FROM THE OPTIMIZATION BUT HOW
% TO GET THEM??

if isempty(Xp)
  % Full case (no pseudo-inputs)

  n = cols(X);
  In = diag(sparse(ones(n,1)));

  K = feval(covfunc{:}, logtheta, X, X);
% $$$   norm_K = normest(K);
% $$$   K = K + 1e-14*norm_K*In; % SET PROPERLY
%  K = K + 1e-6*In;

  % DEBUG: REGULARIZE
  K = regularize(K);
  Lp = chol(K, 'lower');
%  [Lp,K] = safechol(K, 1e-10, 'lower');
  %K = Lp*Lp';
  Kpp = K;
  Kxp = [];

  % We assume that V is DIAGONAL
  V = sparse(V);
  L = chol(inv(V) + K, 'lower');
%  L = safechol(inv(V) + K, 1e-16, 'lower');

  % Posterior mean and covariance
  if false
    % TODO: NOTE, THAT DIAGONAL COVARIANCE APPROXIMATION IS A BIT
    % ADHOC!!!!! CAUSES LOGLIKELIHOOD TO BE INCORRECT!!!
    f = K * c - K * solve_triu(L', solve_tril(L, K*c));
    Covf = spalloc(n,n,n);
    for i=1:n
      r = solve_tril(L,K(:,i));
      Covf(i,i) = K(i,i) - r'*r;
    end
  else
    invLK = full(solve_tril(L,K));
    Covf = K - invLK' * invLK;
    f = Covf * c;
  end

else
  % Pseudo input case

  Im = diag(sparse(ones(m,1)));

  % GP-prior covariances
  Kpp = feval(covfunc{:}, logtheta, Xp, Xp);
% $$$   norm_Kpp = normest(Kpp);
% $$$   Kpp = Kpp + 1e-14*norm_Kpp*Im; % SET PROPERLY
%  Kpp = Kpp + 1e-3*Im; % SET PROPERLY
  Kxp = feval(covfunc{:}, logtheta, X, Xp);

  % DEBUG: REGULARIZE
  %Kpp = regularize(Kpp);

  % Helpful variables
  reg = 1;
  while 1
    try
      Kpp = regularize(Kpp, reg);
      Lp = chol(Kpp, 'lower');
      break
    catch
      warning('Ill conditioned covariance matrix in learning phase!!');
      reg = reg * 10;
    end
  end
  %  [Lp, Kpp] = safechol(Kpp, 1e-10, 'lower');
  %Kpp = Lp*Lp';

  %szLp = size(Lp)
  %szKxp = size(Kxp)
  %szLV = size(LV)
  R = solve_tril(Lp, Kxp' * LV);
  S = Im + R*R';
  LS = chol(S, 'lower');
%  LS = safechol(S, 1e-16, 'lower');
  z = LV \ c;
  beta = solve_tril(LS, R*z);
  invLsLp = solve_tril(LS, Lp');

  % Posterior mean and covariance
  f = invLsLp' * beta;
  Covf = invLsLp' * invLsLp;

end

% $$$ if issparse(Lp)
% $$$   density_in_gplearn = nnz(Lp) / prod(size(Lp));
% $$$ end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [f, df] = bound2cost(x, c, V, LV, inputs, kX, m, d, updatepseudo, ...
                              pseudoinputs)

if updatepseudo
  pseudoinputs = reshape(x(1:(m*d)), [d,m]);
  logtheta = x((m*d+1):end);
  [bound, dlogtheta, dpseudoinputs] = pseudobound(c, V, LV, inputs, ...
                                                  pseudoinputs, logtheta, kX);
else
  logtheta = x(:);
  [bound, dlogtheta] = pseudobound(c, V, LV, inputs, pseudoinputs, logtheta, ...
                                   kX);
end

f = -bound;

if updatepseudo
  df = -[dpseudoinputs(:); dlogtheta(:)];
else
  df = -dlogtheta(:);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%
function [bound, dlogtheta, dpseudoinputs] = ...
    pseudobound(c, V, LV, inputs, pseudoinputs, logtheta, covfunc)

if ~isempty(pseudoinputs)
  %% PSEUDO INPUT CASE
  %disp('Pseudo case in gplearn > pseudobound');

  d = rows(pseudoinputs); % dimensionality of pseudo-inputs
  m = cols(pseudoinputs); % number of pseudo-inputs

  % GP-prior (co)variances and their gradients
  switch nargout
   case 1
    Kpp = feval(covfunc{:}, logtheta, pseudoinputs, pseudoinputs);
    Kxp = feval(covfunc{:}, logtheta, inputs, pseudoinputs);
    Kx = feval(covfunc{:}, logtheta, inputs, []);
   case 2
    [Kpp, dKpp_logtheta] = feval(covfunc{:}, logtheta, pseudoinputs, ...
                                   pseudoinputs);
    [Kxp, dKxp_logtheta] = feval(covfunc{:}, logtheta, inputs, pseudoinputs);
    [Kx, dKx_logtheta] = feval(covfunc{:}, logtheta, inputs, []);
   case 3
    [Kpp, dKpp_logtheta, dKpp_pseudo] = feval(covfunc{:}, logtheta, ...
                                                pseudoinputs, pseudoinputs);
    [Kxp, dKxp_logtheta, dKxp_pseudo] = feval(covfunc{:}, logtheta, ...
                                                inputs, pseudoinputs);
    [Kx, dKx_logtheta] = feval(covfunc{:}, logtheta, inputs, []);
   otherwise
    error('No defined action here')
  end

% $$$ if ~iscell(covfunc), covfunc = {covfunc}; end
% $$$ [K,tmp] = feval(covfunc{:}, logtheta, pseudoinputs, pseudoinputs);
% $$$ figure
% $$$ imagesc(K-Kpp)
% $$$ return

  Im = diag(sparse(ones(m,1)));
% $$$   norm_Kpp = normest(Kpp)
% $$$   Kpp = Kpp + 1e-14*norm_Kpp*Im; % SET PROPERLY
%  Kpp = Kpp + 1e-3*Im; % SET PROPERLY
% $$$   figure
% $$$   imagesc(Kpp);
% $$$   error('jou')

  % DEBUG: REGULARIZE
  %Kpp = regularize(Kpp);

  % Helpful variables
  Kpp = regularize(Kpp);
  try
    Lp = chol(Kpp, 'lower');
  catch
    warning('Covariance matrix ill conditioned');
    bound = -inf;
    dlogtheta = nan * zeros(length(logtheta),1);
    dpseudoinputs = nan * zeros(d,m);
    return
  end
%  [Lp,Kpp] = safechol(Kpp, 1e-10, 'lower');
  %Kpp = Lp*Lp';

  R = solve_tril(Lp, Kxp' * LV);
  S = Im + R*R';
  LS = chol(S, 'lower');
%  LS = safechol(S, 1e-16, 'lower');
  LA = Lp * LS;
  z = LV \ c;
  H = solve_tril(LS, R);
  T = solve_triu(LA', H);
  beta = H * z;
  b = T * z;
  invLpR = solve_triu(Lp', R);
  diagV = full(diag(V));
  dif = c - V*(Kxp*b);

  % Lower bound for loglikelihood
  bound = -logdettri(LS) - 0.5*z'*z + 0.5*beta'*beta ...
          - 0.5*Kx'*diagV ...
          + 0.5*traceprod(R,R,true);

  % General gradient coefficients
  Tpp = 0.5 * solve_triu(LA', solve_triu(LA', LS'*LS-eye(m))') ...
        - 0.5 * invLpR*invLpR';
  Txp = -LV * T' ...
        + LV*invLpR';

  % Gradient for hyperparameters
  if nargout >= 2
    n = length(logtheta);
    dlogtheta = zeros(n,1);
    for i=1:n
      dlogtheta(i) = traceprod(dKpp_logtheta(:,:,i), Tpp, true) ...
          - 0.5 * b' * dKpp_logtheta(:,:,i) * b ...
          + traceprod(dKxp_logtheta(:,:,i), Txp, true) ...
          + dif' * dKxp_logtheta(:,:,i) * b ...
          - 0.5 * dKx_logtheta(:,:,i)' * diagV;
    end
  end

  if nargout >= 3
    % Gradient for pseudo-inputs
    dpseudoinputs = zeros(d,m);
    Dpp = spalloc(m,m,2*m);
    for i=1:m
      for j=1:d
        Dpp(:) = 0;
        Dpp(i,:) = dKpp_pseudo(j,:,i);
        Dpp(:,i) = Dpp(:,i) + dKpp_pseudo(j,:,i)';

        dpseudoinputs(j,i) = traceprod(Dpp, Tpp, true) ...
            - 0.5 * b' * Dpp * b ...
            + dKxp_pseudo(j,:,i) * Txp(:,i) ...
            + dif' * dKxp_pseudo(j,:,i)' * b(i);

      end
    end
  end

else

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % FULL CASE (NO PSEUDO INPUTS)
% $$$   disp('Full case in gplearn > pseudobound');
  d = rows(inputs); % dimensionality of pseudo-inputs
  n = cols(inputs);
  %m = cols(pseudoinputs); % number of pseudo-inputs

  % GP-prior (co)variances and their gradients
  switch nargout
   case 1
    K = feval(covfunc{:}, logtheta, inputs, inputs);
   case 2
    [K, dK_logtheta] = feval(covfunc{:}, logtheta, inputs, inputs);
   otherwise
    error('No pseudo inputs given');
  end

  In = diag(sparse(ones(n,1)));
% $$$   norm_K = normest(K);
% $$$   K = K + 1e-14*norm_K*In; % SET PROPERLY
%  K = K + 1e-6*In;

  % DEBUG: REGULARIZE
  %K = regularize(K);

  % ASSUME THAT V is DIAGONAL
  V = sparse(V);
  Ky = inv(V) + K;
  try
    L = chol(Ky, 'lower');
  catch
    warning('Covariance matrix ill conditioned');
    bound = -inf;
    dlogtheta = nan * zeros(length(logtheta),1);
    return
  end
%  [L,Ky] = safechol(Ky, 1e-10, 'lower');
  % DEBUG:
  %Ky = L*L';
  y = sparse(V) \ c;

  z = solve_tril(L, y);

  % Lower bound for loglikelihood
  bound = -logdettri(L) - 0.5*z'*z;

% $$$   % General gradient coefficients
% $$$   Tpp = 0.5 * solve_triu(LA', solve_triu(LA', LS'*LS-eye(m))') ...
% $$$         - 0.5 * invLpR*invLpR';
% $$$   Txp = -LV * T' ...
% $$$         + LV*invLpR';

  % Gradient for hyperparameters
  if nargout >= 2
    b = solve_triu(L',z);
    n = length(logtheta);
    dlogtheta = zeros(n,1);
    if issparse(Ky)
      invKy = sinv(Ky);
    else
      invKy = inv(Ky);
    end
    for i=1:n
      dlogtheta(i) = 0.5 * b' * dK_logtheta(:,:,i) * b ...
          - 0.5 * traceprod( invKy, dK_logtheta(:,:,i), true );
% $$$       dlogtheta(i) = traceprod(dKpp_logtheta(:,:,i), Tpp, true) ...
% $$$           - 0.5 * b' * dKpp_logtheta(:,:,i) * b ...
% $$$           + traceprod(dKxp_logtheta(:,:,i), Txp, true) ...
% $$$           + dif' * dKxp_logtheta(:,:,i) * b ...
% $$$           - 0.5 * dKx_logtheta(:,:,i)' * diagV;
    end
  end

end