function [hrv, R_t, R_amp, R_index, S_t, S_amp]  = rpeakdetect(data,samp_freq,thresh,testmode)

% [hrv, R_t, R_amp, R_index, S_t, S_amp]  = rpeakdetect(data, samp_freq, thresh, testmode);
% R_t == RR points in time, R_amp == amplitude
% of R peak in bpf data & S_amp == amplitude of
% following minmum. sampling frequency (samp_freq = 256Hz
% by default) only needed if no time vector is
% specified (assumed to be 1st column or row).
% The 'triggering' threshold 'thresh' for the peaks in the 'integrated'
% waveform is 0.2 by default.  testmode = 0 (default) indicates
% no graphics diagnostics. Otherwise, you get to scan through each segment.
%
% A batch QRS detector based upon that of Pan, Hamilton and Tompkins:
% J. Pan \& W. Tompkins - A real-time QRS detection algorithm
% IEEE Transactions on Biomedical Engineering, vol. BME-32 NO. 3. 1985.
% P. Hamilton \& W. Tompkins. Quantitative Investigation of QRS
% Detection  Rules Using the MIT/BIH Arrythmia Database.
% IEEE Transactions on Biomedical Engineering, vol. BME-33, NO. 12.1986.
%
% Similar results reported by the authors above were achieved, without
% having to tune the thresholds on the MIT DB. An online version in C
% has also been written.
%
% Written by G. Clifford gari@ieee.org and made available under the
% GNU general public license. If you have not received a copy of this
% license, please download a copy from http://www.gnu.org/
%
% Please distribute (and modify) freely, commenting
% where you have added modifications.
% The author would appreciate correspondence regarding
% corrections, modifications, improvements etc.
%
% gari@ieee.org

%%%%%%%%%%% make threshold default 0.2 -> this was 0.15 on MIT data
if nargin < 4
   testmode = 0;
end
%%%%%%%%%%% make threshold default 0.2 -> this was 0.15 on MIT data
if nargin < 3
   thresh = 0.2;
end
%%%%%%%%%%% make sample frequency default 256 Hz
if nargin < 2
   samp_freq = 256;
   if(testmode==1)
       fprintf('Assuming sampling frequency of %iHz\n',samp_freq);
   end
end

%%%%%%%%%%% check format of data %%%%%%%%%%
[a b] = size(data);
if(a>b)
 len =a;
end
if(b>a)
 len =b;
end

%%%%%%%%%% if there's no time axis - make one
if (a | b == 1);
% make time axis
  tt = 1/samp_freq:1/samp_freq:ceil(len/samp_freq);
  t = tt(1:len);
  x = data;
end
%%%%%%%%%% check if data is in columns or rows
if (a == 2)
  x=data(:,1);
  t=data(:,2);
end
if (b == 2)
  t=data(:,1);
  x=data(:,2);
end

%%%%%%%%% bandpass filter data - assume 256hz data %%%%%
 % remove mean
 x = x-mean(x);

 % FIR filtering stage
 bpf=x; %Initialise
if( (samp_freq==128) & (exist('filterECG128Hz')~=0) )
        bpf = filterECG128Hz(x);
end
if( (samp_freq==256) & (exist('filterECG256Hz')~=0) )
        bpf = filterECG256Hz(x);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%% differentiate data %%%%%%%%%%%%%%%%%%%%%%%%%%%
 dff = diff(bpf);  % now it's one datum shorter than before

%%%%%%%%% square data    %%%%%%%%%%%%%%%%%%%%%%%%%%%
 sqr = dff.*dff;   %
 len = len-1; % how long is the new vector now?

%%%%%%%%% integrate data over window 'd' %%%%%%%%%%%%%%%%%%%%%%%%%
 d=[1 1 1 1 1 1 1]; % window size - intialise
 if (samp_freq>=256) % adapt for higher sampling rates
   d = [ones(1,round(7*samp_freq/256))];
 end
 % integrate
 mdfint = medfilt1(filter(d,1,sqr),10);
 % remove filter delay for scanning back through ECG
 delay = ceil(length(d)/2);
 mdfint = mdfint(delay:length(mdfint));
%%%%%%%%% segment search area %%%%%%%%%%%%%%%%%%%%%%%
 %%%% first find the highest bumps in the data %%%%%%
 max_h = max (mdfint(round(len/4):round(3*len/4)));

 %%%% then build an array of segments to look in %%%%%
 %thresh = 0.2;
 poss_reg = mdfint>(thresh*max_h);

%%%%%%%%% and find peaks %%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %%%% find indices into boudaries of each segment %%%
 left  = find(diff([0 poss_reg'])==1); % remember to zero pad at start
 right = find(diff([poss_reg' 0])==-1); % remember to zero pad at end

 %%%% loop through all possibilities
 for(i=1:length(left))
    [maxval(i) maxloc(i)] = max( bpf(left(i):right(i)) );
    [minval(i) minloc(i)] = min( bpf(left(i):right(i)) );
    maxloc(i) = maxloc(i)-1+left(i); % add offset of present location
    minloc(i) = minloc(i)-1+left(i); % add offset of present location
 end

 R_index = maxloc;
 R_t   = t(maxloc);
 R_amp = maxval;
 S_amp = minval;   %%%% Assuming the S-wave is the lowest
                   %%%% amp in the given window
 S_t   = t(minloc);

%%%%%%%%%% check for lead inversion %%%%%%%%%%%%%%%%%%%
 % i.e. do minima precede maxima?
 if (minloc(length(minloc))<maxloc(length(minloc)))
  R_t   = t(minloc);
  R_amp = minval;
  S_t   = t(maxloc);
  S_amp = maxval;
 end

%%%%%%%%%%%%
hrv  = diff(R_t);
resp = R_amp-S_amp;

%%%%%%%%%%%%%%%%%%%%
if (testmode~=0)
figure(1)
hold off
subplot(4,1,1)
plot(t,x);hold on;plot(t,bpf,'r')
title('raw ECG (blue) and zero-pahse FIR filtered ECG (red)')
ylabel('ECG')
hold off;
subplot(4,1,2)
plot(t(1:length(mdfint)),mdfint);hold on;
%plot(t(1:length(sqr)),sqr);hold on;
plot(t,max(mdfint)*bpf/(2*max(bpf)),'r')
plot(t(left),mdfint(left),'og')
plot(t(right),mdfint(right),'om')
title('Integrated data with scan boundaries over scaled ECG')
ylabel('Int ECG')
hold off;
subplot(4,1,3)
plot(t,bpf,'r');hold on;
plot(R_t,R_amp,'+k');
plot(S_t,S_amp,'+g');
title('ECG with R-peaks (black) and S-points (green) over ECG')
ylabel('ECG+R+S')
hold off;
subplot(4,1,4)
hold off
plot(R_t(1:length(hrv)),hrv,'r+')
hold on
title('RR intervals')
ylabel('RR (s)')
hold off
fprintf('Press any key for next block of data\n');
pause
end


function boxesAll = extendBBoxStickmen(stickmen)
boxesAll = zeros(length(stickmen),4);
for ridx = 1:length(stickmen)
    box = stickmen(ridx).det;
    h = box(4) - box(2);
    % 2x extension in Y
    box(4) = box(4) + h;
    % pad in from all sides
    delta1 = 0.1*h;
    delta2 = 0.1*h;
    box = box + [-delta1 -delta2 delta1 delta2];
    boxesAll(ridx,:) = box;
end

end


function lik = lik_t(varargin)
%LIK_T  Create a Student-t likelihood structure
%
%  Description
%    LIK = LIK_T('PARAM1',VALUE1,'PARAM2,VALUE2,...)
%    creates Student-t likelihood structure in which the named
%    parameters have the specified values. Any unspecified
%    parameters are set to default values.
%
%    LIK = LIK_T(LIK,'PARAM1',VALUE1,'PARAM2,VALUE2,...)
%    modify a likelihood structure with the named parameters
%    altered with the specified values.
%
%    Parameters for Student-t likelihood [default]
%      sigma2       - scale squared [1]
%      nu           - degrees of freedom [4]
%      sigma2_prior - prior for sigma2 [prior_logunif]
%      nu_prior     - prior for nu [prior_fixed]
%
%    Note! If the prior is 'prior_fixed' then the parameter in
%    question is considered fixed and it is not handled in
%    optimization, grid integration, MCMC etc.
%
%    The likelihood is defined as follows:
%                  __ n
%      p(y|f, z) = || i=1 C(nu,s2) * (1 + 1/nu * (y_i - f_i)^2/s2 )^(-(nu+1)/2)
%
%      where nu is the degrees of freedom, s2 the scale and f_i the
%      latent variable defining the mean. C(nu,s2) is constant
%      depending on nu and s2.
%
%  See also
%    GP_SET, LIK_*, PRIOR_*
%

% Copyright (c) 2009-2010 Jarno Vanhatalo
% Copyright (c) 2010 Aki Vehtari
% Copyright (c) 2011 Pasi Jyl�nki

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

  ip=inputParser;
  ip.FunctionName = 'LIK_T';
  ip.addOptional('lik', [], @isstruct);
  ip.addParamValue('sigma2',0.1, @(x) isscalar(x) && x>0);
  ip.addParamValue('sigma2_prior',prior_logunif(), @(x) isstruct(x) || isempty(x));
  ip.addParamValue('nu',4, @(x) isscalar(x) && x>0);
  ip.addParamValue('nu_prior',prior_fixed, @(x) isstruct(x) || isempty(x));
  ip.parse(varargin{:});
  lik=ip.Results.lik;

  if isempty(lik)
    init=true;
    lik.type = 'Student-t';
  else
    if ~isfield(lik,'type') || ~isequal(lik.type,'Student-t')
      error('First argument does not seem to be a valid likelihood function structure')
    end
    init=false;
  end

  % Initialize parameters
  if init || ~ismember('sigma2',ip.UsingDefaults)
    lik.sigma2 = ip.Results.sigma2;
  end
  if init || ~ismember('nu',ip.UsingDefaults)
    lik.nu = ip.Results.nu;
  end
  % Initialize prior structure
  if init
    lik.p=[];
  end
  if init || ~ismember('sigma2_prior',ip.UsingDefaults)
    lik.p.sigma2=ip.Results.sigma2_prior;
  end
  if init || ~ismember('nu_prior',ip.UsingDefaults)
    lik.p.nu=ip.Results.nu_prior;
  end

  if init
    % Set the function handles to the subfunctions
    lik.fh.pak = @lik_t_pak;
    lik.fh.unpak = @lik_t_unpak;
    lik.fh.lp = @lik_t_lp;
    lik.fh.lpg = @lik_t_lpg;
    lik.fh.ll = @lik_t_ll;
    lik.fh.llg = @lik_t_llg;
    lik.fh.llg2 = @lik_t_llg2;
    lik.fh.llg3 = @lik_t_llg3;
    lik.fh.tiltedMoments = @lik_t_tiltedMoments;
    lik.fh.tiltedMoments2 = @lik_t_tiltedMoments2;
    lik.fh.siteDeriv = @lik_t_siteDeriv;
    lik.fh.siteDeriv2 = @lik_t_siteDeriv2;
    lik.fh.optimizef = @lik_t_optimizef;
    lik.fh.upfact = @lik_t_upfact;
    lik.fh.invlink = @lik_t_invlink;
    lik.fh.predy = @lik_t_predy;
    lik.fh.predprcty = @lik_t_predprcty;
    lik.fh.recappend = @lik_t_recappend;
  end

end

function [w, s] = lik_t_pak(lik)
%LIK_T_PAK  Combine likelihood parameters into one vector.
%
%  Description
%    W = LIK_T_PAK(LIK) takes a likelihood structure LIK and
%    combines the parameters into a single row vector W. This
%    is a mandatory subfunction used for example in energy and
%    gradient computations.
%
%       w = [ log(lik.sigma2)
%             (hyperparameters of lik.sigma2)
%             log(log(lik.nu))
%             (hyperparameters of lik.nu)]'
%
%  See also
%    LIK_T_UNPAK, GP_PAK

  w = []; s = {};
  if ~isempty(lik.p.sigma2)
    w = [w log(lik.sigma2)];
    s = [s; 'log(lik.sigma2)'];
    [wh sh] = lik.p.sigma2.fh.pak(lik.p.sigma2);
    w = [w wh];
    s = [s; sh];
  end
  if ~isempty(lik.p.nu)
    w = [w log(log(lik.nu))];
    s = [s; 'loglog(lik.nu)'];
    [wh sh] = lik.p.nu.fh.pak(lik.p.nu);
    w = [w wh];
    s = [s; sh];
  end
end

function [lik, w] = lik_t_unpak(lik, w)
%LIK_T_UNPAK  Extract likelihood parameters from the vector.
%
%  Description
%    W = LIK_T_UNPAK(W, LIK) takes a likelihood structure LIK and
%    extracts the parameters from the vector W to the LIK
%    structure. This is a mandatory subfunction used for example
%    in energy and gradient computations.
%
%    Assignment is inverse of
%       w = [ log(lik.sigma2)
%             (hyperparameters of lik.sigma2)
%             log(log(lik.nu))
%             (hyperparameters of lik.nu)]'
%
%   See also
%   LIK_T_PAK, GP_UNPAK

  if ~isempty(lik.p.sigma2)
    lik.sigma2 = exp(w(1));
    w = w(2:end);
    [p, w] = lik.p.sigma2.fh.unpak(lik.p.sigma2, w);
    lik.p.sigma2 = p;
  end
  if ~isempty(lik.p.nu)
    lik.nu = exp(exp(w(1)));
    w = w(2:end);
    [p, w] = lik.p.nu.fh.unpak(lik.p.nu, w);
    lik.p.nu = p;
  end
end

function lp = lik_t_lp(lik)
%LIK_T_LP  log(prior) of the likelihood parameters
%
%  Description
%    LP = LIK_T_LP(LIK) takes a likelihood structure LIK and
%    returns log(p(th)), where th collects the parameters.
%    This subfunction is needed when there are likelihood parameters.
%
%  See also
%    LIK_T_LLG, LIK_T_LLG3, LIK_T_LLG2, GPLA_E

  v = lik.nu;
  sigma2 = lik.sigma2;
  lp = 0;

  if ~isempty(lik.p.sigma2)
    lp = lp + lik.p.sigma2.fh.lp(sigma2, lik.p.sigma2) +log(sigma2);
  end
  if ~isempty(lik.p.nu)
    lp = lp + lik.p.nu.fh.lp(lik.nu, lik.p.nu)  +log(v) +log(log(v));
  end
end

function lpg = lik_t_lpg(lik)
%LIK_T_LPG  d log(prior)/dth of the likelihood parameters th
%
%  Description
%    LPG = LIK_T_LPG(LIK) takes a likelihood structure LIK
%    and returns d log(p(th))/dth, where th collects the
%    parameters. This subfunction is needed when there are
%    likelihood parameters.
%
%  See also
%    LIK_T_LLG, LIK_T_LLG3, LIK_T_LLG2, GPLA_G

% Evaluate the gradients of log(prior)

  v = lik.nu;
  sigma2 = lik.sigma2;
  lpg = [];
  i1 = 0;

  if ~isempty(lik.p.sigma2)
    i1 = i1+1;
    lpg(i1) = lik.p.sigma2.fh.lpg(lik.sigma2, lik.p.sigma2).*sigma2 + 1;
  end
  if ~isempty(lik.p.nu)
    i1 = i1+1;
    lpg(i1) = lik.p.nu.fh.lpg(lik.nu, lik.p.nu).*v.*log(v) +log(v) + 1;
  end
end

function ll = lik_t_ll(lik, y, f, z)
%LIK_T_LL  Log likelihood
%
%  Description
%    LL = LIK_T_LL(LIK, Y, F) takes a likelihood structure LIK,
%    observations Y, and latent values F. Returns the log
%    likelihood, log p(y|f,z). This subfunction is needed when
%    using Laplace approximation or MCMC for inference with
%    non-Gaussian likelihoods. This subfunction is also used in
%    information criteria (DIC, WAIC) computations.
%
%  See also
%    LIK_T_LLG, LIK_T_LLG3, LIK_T_LLG2, GPLA_E

  r = y-f;
  v = lik.nu;
  sigma2 = lik.sigma2;

  term = gammaln((v + 1) / 2) - gammaln(v/2) -log(v.*pi.*sigma2)/2;
  ll = term + log(1 + (r.^2)./v./sigma2) .* (-(v+1)/2);
  ll = sum(ll);
end

function llg = lik_t_llg(lik, y, f, param, z)
%LIK_T_LLG  Gradient of the log likelihood
%
%  Description
%    LOKLIKG = LIK_T_LLG(LIK, Y, F, PARAM) takes a likelihood
%    structure LIK, observations Y, and latent values F. Returns
%    the gradient of log likelihood with respect to PARAM. At the
%    moment PARAM can be 'param' or 'latent'. This subfunction is
%    needed when using Laplace approximation or MCMC for inference
%    with non-Gaussian likelihoods.
%
%  See also
%    LIK_T_LL, LIK_T_LLG2, LIK_T_LLG3, GPLA_E

  r = y-f;
  v = lik.nu;
  sigma2 = lik.sigma2;

  switch param
    case 'param'
      n = length(y);

      i1=0;
      if ~isempty(lik.p.sigma2)
        i1=i1+1;
        % Derivative with respect to sigma2
        llg(i1) = -n./sigma2/2 + (v+1)./2.*sum(r.^2./(v.*sigma2.^2+r.^2*sigma2));
        % correction for the log transformation
        llg(i1) = llg(i1).*sigma2;
      end
      if ~isempty(lik.p.nu)
        i1=i1+1;
        % Derivative with respect to nu
        llg(i1) = 0.5.* sum(psi((v+1)./2) - psi(v./2) - 1./v - log(1+r.^2./(v.*sigma2)) + (v+1).*r.^2./(v.^2.*sigma2 + v.*r.^2));

        % correction for the log transformation
        llg(i1) = llg(i1).*v.*log(v);
      end
    case 'latent'
      llg  = (v+1).*r ./ (v.*sigma2 + r.^2);
  end

end

function llg2 = lik_t_llg2(lik, y, f, param, z)
%LIK_T_LLG2  Second gradients of log likelihood
%
%  Description
%    LLG2 = LIK_T_LLG2(LIK, Y, F, PARAM) takes a likelihood
%    structure LIK, observations Y, and latent values F. Returns
%    the Hessian of log likelihood with respect to PARAM. At the
%    moment PARAM can be only 'latent'. LLG2 is a vector with
%    diagonal elements of the Hessian matrix (off diagonals are
%    zero). This subfunction is needed when using Laplace
%    approximation or EP for inference with non-Gaussian likelihoods.
%
%  See also
%    LIK_T_LL, LIK_T_LLG, LIK_T_LLG3, GPLA_E

  r = y-f;
  v = lik.nu;
  sigma2 = lik.sigma2;

  switch param
    case 'param'

    case 'latent'
      % The Hessian d^2 /(dfdf)
      llg2 =  (v+1).*(r.^2 - v.*sigma2) ./ (v.*sigma2 + r.^2).^2;
    case 'latent+param'
      % gradient d^2 / (dfds2)
      llg2 = -v.*(v+1).*r ./ (v.*sigma2 + r.^2).^2;

      % Correction for the log transformation
      llg2 = llg2.*sigma2;
      if ~isempty(lik.p.nu)
        % gradient d^2 / (dfdnu)
        llg2(:,2) = r./(v.*sigma2 + r.^2) - sigma2.*(v+1).*r./(v.*sigma2 + r.^2).^2;

        % Correction for the log transformation
        llg2(:,2) = llg2(:,2).*v.*log(v);
      end
  end
end

function llg3 = lik_t_llg3(lik, y, f, param, z)
%LIK_T_LLG3  Third gradients of log likelihood (energy)
%
%  Description
%    LLG3 = LIK_T_LLG3(LIK, Y, F, PARAM) takes a likelihood
%    structure LIK, observations Y and latent values F and
%    returns the third gradients of log likelihood with respect
%    to PARAM. At the moment PARAM can be only 'latent'. G3 is a
%    vector with third gradients. This subfunction is needed when
%    using Laplace approximation for inference with non-Gaussian
%    likelihoods.
%
%  See also
%    LIK_T_LL, LIK_T_LLG, LIK_T_LLG2, GPLA_E, GPLA_G

  r = y-f;
  v = lik.nu;
  sigma2 = lik.sigma2;

  switch param
    case 'param'

    case 'latent'
      % Return the diagonal of W differentiated with respect to latent values / dfdfdf
      llg3 = (v+1).*(2.*r.^3 - 6.*v.*sigma2.*r) ./ (v.*sigma2 + r.^2).^3;
    case 'latent2+param'
      % Return the diagonal of W differentiated with respect to
      % likelihood parameters / dfdfds2
      llg3 = (v+1).*v.*( v.*sigma2 - 3.*r.^2) ./ (v.*sigma2 + r.^2).^3;
      llg3 = llg3.*sigma2;
      if ~isempty(lik.p.nu)
        % dfdfdnu
        llg3(:,2) = (r.^2-2.*v.*sigma2-sigma2)./(v.*sigma2 + r.^2).^2 - 2.*sigma2.*(r.^2-v.*sigma2).*(v+1)./(v.*sigma2 + r.^2).^3;
        llg3(:,2) = llg3(:,2).*v.*log(v);
      end
  end
end

function [logM_0, m_1, sigm2hati1] = lik_t_tiltedMoments(lik, y, i1, sigm2_i, myy_i, z)
%LIK_T_TILTEDMOMENTS  Returns the marginal moments for EP algorithm
%
%  Description
%    [M_0, M_1, M2] = LIK_T_TILTEDMOMENTS(LIK, Y, I, S2, MYY, Z)
%    takes a likelihood structure LIK, incedence counts Y,
%    expected counts Z, index I and cavity variance S2 and mean
%    MYY. Returns the zeroth moment M_0, mean M_1 and variance
%    M_2 of the posterior marginal (see Rasmussen and Williams
%    (2006): Gaussian processes for Machine Learning, page 55).
%    This subfunction is needed when using EP for inference with
%    non-Gaussian likelihoods.
%
%  See also
%    GPEP_E

  zm = @zeroth_moment;

  tol = 1e-8;
  yy = y(i1);
  nu = lik.nu;
  sigma2 = lik.sigma2;

  % Set the limits for integration and integrate with quad
  % -----------------------------------------------------
  mean_app = myy_i;
  sigm_app = sqrt(sigm2_i);

  lambdaconf(1) = mean_app - 8.*sigm_app; lambdaconf(2) = mean_app + 8.*sigm_app;
  test1 = zm((lambdaconf(2)+lambdaconf(1))/2) > zm(lambdaconf(1));
  test2 = zm((lambdaconf(2)+lambdaconf(1))/2) > zm(lambdaconf(2));
  testiter = 1;
  if test1 == 0
    lambdaconf(1) = lambdaconf(1) - 3*sigm_app;
    test1 = zm((lambdaconf(2)+lambdaconf(1))/2)>zm(lambdaconf(1));
    if test1 == 0
      go=true;
      while testiter<10 & go
        lambdaconf(1) = lambdaconf(1) - 2*sigm_app;
        lambdaconf(2) = lambdaconf(2) - 2*sigm_app;
        test1 = zm((lambdaconf(2)+lambdaconf(1))/2)>zm(lambdaconf(1));
        test2 = zm((lambdaconf(2)+lambdaconf(1))/2)>zm(lambdaconf(2));
        if test1==1&test2==1
          go=false;
        end
        testiter=testiter+1;
      end
    end
    mean_app = (lambdaconf(2)+lambdaconf(1))/2;
  elseif test2 == 0
    lambdaconf(2) = lambdaconf(2) + 3*sigm_app;
    test2 = zm((lambdaconf(2)+lambdaconf(1))/2)>zm(lambdaconf(2));
    if test2 == 0
      go=true;
      while testiter<10 & go
        lambdaconf(1) = lambdaconf(1) + 2*sigm_app;
        lambdaconf(2) = lambdaconf(2) + 2*sigm_app;
        test1 = zm((lambdaconf(2)+lambdaconf(1))/2)>zm(lambdaconf(1));
        test2 = zm((lambdaconf(2)+lambdaconf(1))/2)>zm(lambdaconf(2));
        if test1==1&test2==1
          go=false;
        end
        testiter=testiter+1;
      end
    end
    mean_app = (lambdaconf(2)+lambdaconf(1))/2;
  end
  RTOL = 1.e-6;
  ATOL = 1.e-10;

  % Integrate with quadrature
  [m_0, m_1, m_2] = quad_moments(zm,lambdaconf(1), lambdaconf(2), RTOL, ATOL);

  sigm2hati1 = m_2 - m_1.^2;
  logM_0 = log(m_0);
  function integrand = zeroth_moment(f)
    r = yy-f;
    term = gammaln((nu + 1) / 2) - gammaln(nu/2) -log(nu.*pi.*sigma2)/2;
    integrand = exp(term + log(1 + r.^2./nu./sigma2) .* (-(nu+1)/2));
    integrand = integrand.*exp(- 0.5 * (f-myy_i).^2./sigm2_i - log(sigm2_i)/2 - log(2*pi)/2); %
  end
end

function [g_i] = lik_t_siteDeriv(lik, y, i1, sigm2_i, myy_i, z)
%LIK_T_SITEDERIV  Evaluate the expectation of the gradient
%                 of the log likelihood term with respect
%                 to the likelihood parameters for EP
%
%  Description
%    [M_0, M_1, M2] = LIK_T_TILTEDMOMENTS(LIK, Y, I, S2, MYY)
%    takes a likelihood structure LIK, observations Y, index I
%    and cavity variance S2 and mean MYY. Returns E_f [d log
%    p(y_i|f_i) /d a], where a is the likelihood parameter and
%    the expectation is over the marginal posterior. This term is
%    needed when evaluating the gradients of the marginal
%    likelihood estimate Z_EP with respect to the likelihood
%    parameters (see Seeger (2008): Expectation propagation for
%    exponential families).  This subfunction is needed when using
%    EP for inference with non-Gaussian likelihoods and there are
%    likelihood parameters.
%
%  See also
%    GPEP_G

  zm = @zeroth_moment;
  znu = @deriv_nu;
  zsigma2 = @deriv_sigma2;

  tol = 1e-8;
  yy = y(i1);
  nu = lik.nu;
  sigma2 = lik.sigma2;

  % Set the limits for integration and integrate with quad
  mean_app = myy_i;
  sigm_app = sqrt(sigm2_i);

  lambdaconf(1) = mean_app - 6.*sigm_app; lambdaconf(2) = mean_app + 6.*sigm_app;
  test1 = zm((lambdaconf(2)+lambdaconf(1))/2)>zm(lambdaconf(1));
  test2 = zm((lambdaconf(2)+lambdaconf(1))/2)>zm(lambdaconf(2));
  testiter = 1;
  if test1 == 0
    lambdaconf(1) = lambdaconf(1) - 3*sigm_app;
    test1 = zm((lambdaconf(2)+lambdaconf(1))/2)>zm(lambdaconf(1));
    if test1 == 0
      go=true;
      while testiter<10 & go
        lambdaconf(1) = lambdaconf(1) - 2*sigm_app;
        lambdaconf(2) = lambdaconf(2) - 2*sigm_app;
        test1 = zm((lambdaconf(2)+lambdaconf(1))/2)>zm(lambdaconf(1));
        test2 = zm((lambdaconf(2)+lambdaconf(1))/2)>zm(lambdaconf(2));
        if test1==1&test2==1
          go=false;
        end
        testiter=testiter+1;
      end
    end
    mean_app = (lambdaconf(2)+lambdaconf(1))/2;
  elseif test2 == 0
    lambdaconf(2) = lambdaconf(2) + 3*sigm_app;
    test2 = zm((lambdaconf(2)+lambdaconf(1))/2)>zm(lambdaconf(2));
    if test2 == 0
      go=true;
      while testiter<10 & go
        lambdaconf(1) = lambdaconf(1) + 2*sigm_app;
        lambdaconf(2) = lambdaconf(2) + 2*sigm_app;
        test1 = zm((lambdaconf(2)+lambdaconf(1))/2)>zm(lambdaconf(1));
        test2 = zm((lambdaconf(2)+lambdaconf(1))/2)>zm(lambdaconf(2));
        if test1==1&test2==1
          go=false;
        end
        testiter=testiter+1;
      end
    end
    mean_app = (lambdaconf(2)+lambdaconf(1))/2;
  end

  % Integrate with quad
  [m_0, fhncnt] = quadgk(zm, lambdaconf(1), lambdaconf(2));

  %         t=linspace(lambdaconf(1),lambdaconf(2),100);
  %         plot(t,zm(t))
  %         keyboard

  [g_i(1), fhncnt] = quadgk( @(f) zsigma2(f).*zm(f) , lambdaconf(1), lambdaconf(2));
  g_i(1) = g_i(1)/m_0*sigma2;

  if ~isempty(lik.p.nu)
    [g_i(2), fhncnt] = quadgk(@(f) znu(f).*zm(f) , lambdaconf(1), lambdaconf(2));
    g_i(2) = g_i(2)/m_0.*nu.*log(nu);
  end

  function integrand = zeroth_moment(f)
    r = yy-f;
    term = gammaln((nu + 1) / 2) - gammaln(nu/2) -log(nu.*pi.*sigma2)/2;
    integrand = exp(term + log(1 + r.^2./nu./sigma2) .* (-(nu+1)/2));
    integrand = integrand.*exp(- 0.5 * (f-myy_i).^2./sigm2_i - log(sigm2_i)/2 - log(2*pi)/2);
  end

  function g = deriv_nu(f)
    r = yy-f;
    temp = 1 + r.^2./nu./sigma2;
    g = psi((nu+1)/2)./2 - psi(nu/2)./2 - 1./(2.*nu) - log(temp)./2 + (nu+1)./(2.*temp).*(r./nu).^2./sigma2;
  end

  function g = deriv_sigma2(f)
    r = yy-f;
    g  = -1/sigma2/2 + (nu+1)./2.*r.^2./(nu.*sigma2.^2 + r.^2.*sigma2);
  end

end

function [lnZhat, muhat, sigm2hat] = lik_t_tiltedMoments2(likelih, y, yi, sigm2_i, myy_i, z, eta)
%LIKELIH_T_TILTEDMOMENTS    Returns the marginal moments for EP algorithm
%
%   Description
%   [M_0, M_1, M2] = LIKELIH_T_TILTEDMOMENTS(LIKELIH, Y, I, S2, MYY, Z)
%   takes a likelihood data structure LIKELIH, incedence counts Y,
%   expected counts Z, index I and cavity variance S2 and mean
%   MYY. Returns the zeroth moment M_0, mean M_1 and variance M_2
%   of the posterior marginal (see Rasmussen and Williams (2006):
%   Gaussian processes for Machine Learning, page 55). This subfunction
%   is needed when using robust-EP for inference with non-Gaussian
%   likelihoods.
%
%   See also
%   GPEP_E

  if nargin<7
    eta=1;
  end

  yy = y(yi);
  nu = likelih.nu;
  sigma2 = likelih.sigma2;
  sigma = sqrt(sigma2);

  nuprime = eta*nu+eta-1;
  a=nuprime/2; %a=nu/2;

  u=linspace(log(1e-8),5,200);
  du=u(2)-u(1);
  lnpu=(a-1)*u -a*exp(u)+u;

  %   sigma2 t-likelihood parameter, scale squared
  %   sigm2_i cavity variance
  %   myy_i cavity mean

  sigma2prime = sigma2*nu/nuprime;
  Vu = sigm2_i + (sigma2prime)./exp(u);
  lnZu = 0.5*(-log(2*pi*Vu)) -0.5 * (yy-myy_i)^2 ./Vu;
  lnZt = eta*gammaln((nu+1)/2) - eta/2*log(nu*pi*sigma2) - eta*gammaln(nu/2) - gammaln((nuprime+1)/2) + 0.5*log(nuprime*pi*sigma2prime) + gammaln(nuprime/2);

  ptu=exp(lnpu+lnZu+lnZt);

  Z_0=sum(ptu)*du;
  lnZhat=log(Z_0) + a*log(a)-gammaln(a);

  Vtu=1./(1/sigm2_i +(1/sigma2prime)*exp(u));
  mtu=Vtu.*(myy_i/sigm2_i + (yy/sigma2prime)*exp(u));

  muhat=sum(mtu.*ptu)*du/Z_0;
  sigm2hat=sum((Vtu+mtu.^2).*ptu)*du/Z_0-muhat^2;

  % limiting distribution (nu -> infinity)
%   Vg=1/(1/sigm2_i +eta/sigma2);
%   mg=Vg*(myy_i/sigm2_i +yy*eta/sigma2);
%   sigm_i=sqrt(sigm2_i);
%   sg=sqrt(Vg);
%
%   % set integration limits and scaling
%   nu_lim=1e10;
%   if nu<nu_lim
%
%     if sqrt(sigma2/sigm2_i)<0.05
%       % set the integration limits when the likelihood is very narrow
%
%       % grid resolution
%       dd=10;
%       df = [12*sigm_i/100 2*dd*sigma/100];
%
%       if yy>=myy_i
%         % grid break points
%         bp=[min(myy_i-6*sigm_i,yy-dd*sigma) myy_i-6*sigm_i, ...
%           min(myy_i+6*sigm_i,yy-dd*sigma), yy-dd*sigma, yy+dd*sigma,...
%           max(myy_i+6*sigm_i,yy+dd*sigma)];
%
%         % grid values
%         a=1e-6;
%         fvec =[ bp(1):df(2):bp(2)-a, bp(2):df(1):bp(3)-a, bp(3):max(df):bp(4)-a, ...
%           bp(4):df(2):bp(5)-a, bp(5):df(1):bp(6)];
%       else
%         % grid break points
%         bp=[min(myy_i-6*sigm_i,yy-dd*sigma), yy-dd*sigma, yy+dd*sigma,...
%           max(myy_i-6*sigm_i,yy+dd*sigma), myy_i+6*sigm_i, ...
%           max(myy_i+6*sigm_i,yy+dd*sigma)];
%
%         % grid values
%         a=1e-6;
%         fvec =[ bp(1):df(1):bp(2)-a, bp(2):df(2):bp(3)-a, bp(3):max(df):bp(4)-a, ...
%           bp(4):df(1):bp(5)-a, bp(5):df(2):bp(6)];
%       end
%
%       np=numel(fvec);
%       logpt = lpt(fvec,0);
%       lpt_max = max([logpt lpt([myy_i mg],0)]);
%       lambdaconf=[fvec(1), fvec(end)];
%       for i1=2:np-1
%         if logpt(i1) < lpt_max+log(1e-7) %(exp(logpt(i1))/exp(lpt_max) < 1e-7)
%           lambdaconf(1) = fvec(i1);
%         else
%           break;
%         end
%       end
%       for i1=1:np-2
%         if logpt(end-i1) < lpt_max+log(1e-7) %(exp(logpt(end-i1))/exp(lpt_max) < 1e-7)
%           lambdaconf(2) = fvec(end-i1);
%         else
%           break;
%         end
%       end
%     else
%       % set the integration limits in easier cases
%       np=20;
%       if mg>myy_i
%         lambdaconf=[myy_i-6*sigm_i,max(mg+6*sg,myy_i+6*sigm_i)];
%         fvec=linspace(myy_i,mg,np);
%       else
%         lambdaconf=[min(mg-6*sg,myy_i-6*sigm_i),myy_i+6*sigm_i];
%         fvec=linspace(mg,myy_i,np);
%       end
%       lpt_max=max(lpt(fvec,0));
%     end
%     C=log(1)-lpt_max; % scale the log-density for the quadrature tolerance
%   else
%     lambdaconf=[mg-6*sg,mg+6*sg];
%     C=log(1)-lpt(mg,0);
%   end
%
%   if nu>nu_lim
%     % the limiting Gaussian case
%     Vz=sigm2_i+sigma2/eta;
%     lnZhat = 0.5*(-log(eta) +(1-eta)*log(2*pi*sigma2) -log(2*pi*Vz)) -(0.5/Vz)*(yy-myy_i)^2;
%     muhat = mg;
%     sigm2hat = Vg;
%   else
%     % Integrate with quadrature
%     RTOL = 1.e-6;
%     ATOL = 1.e-7;
%     tic
%     [m_0, m_1, m_2] = quad_moments(@(f) exp(lpt(f,C)),lambdaconf(1), lambdaconf(2), RTOL, ATOL);toc
%     muhat = m_1;
%     sigm2hat = m_2 - m_1.^2;
%     lnZhat = log(m_0) -C;
%   end

  function lpdf = lpt(f,C)
    % logarithm of the tilted distribution
    r = yy-f;
    lpdf = gammaln((nu + 1) / 2) - gammaln(nu/2) -log(nu.*pi.*sigma2)/2;
    lpdf = lpdf + log(1 + r.^2./nu./sigma2) .* (-(nu+1)/2);
    lpdf = lpdf*eta - (0.5/sigm2_i) * (f-myy_i).^2 + (C-log(2*pi*sigm2_i)/2);
  end
end

function [g_i] = lik_t_siteDeriv2(likelih, y, yi, sigm2_i, myy_i, z, eta, lnZhat)
%LIKELIH_T_SITEDERIV   Evaluate the expectation of the gradient
%                           of the log likelihood term with respect
%                           to the likelihood parameters for EP
%
%   Description
%   [M_0, M_1, M2] = LIKELIH_T_TILTEDMOMENTS(LIKELIH, Y, I, S2, MYY)
%   takes a likelihood data structure LIKELIH, observations Y, index I
%   and cavity variance S2 and mean MYY. Returns E_f [d log
%   p(y_i|f_i) /d a], where a is the likelihood parameter and the
%   expectation is over the marginal posterior. This term is
%   needed when evaluating the gradients of the marginal
%   likelihood estimate Z_EP with respect to the likelihood
%   parameters (see Seeger (2008): Expectation propagation for
%   exponential families).  This subfunction is needed when using
%   robust-EP for inference with non-Gaussian likelihoods and there
%   are likelihood parameters.
%
%   See also
%   GPEP_G

  if nargin<7
    eta=1;
  end

  yy = y(yi);
  nu = likelih.nu;
  sigma2 = likelih.sigma2;
  sigma = sqrt(sigma2);

  % limiting distribution (nu -> infinity)
  Vg=1/(1/sigm2_i +eta/sigma2);
  mg=Vg*(myy_i/sigm2_i +yy*eta/sigma2);
  sigm_i=sqrt(sigm2_i);
  sg=sqrt(Vg);

  % set integration limits and scaling
  nu_lim=1e10;
  if nu<nu_lim

    if sqrt(sigma2/sigm2_i)<0.05
      % set the integration limits when the likelihood is very narrow

      % grid resolution
      dd=10;
      df = [12*sigm_i/100 2*dd*sigma/100];

      if yy>=myy_i
        % grid break points
        bp=[min(myy_i-6*sigm_i,yy-dd*sigma) myy_i-6*sigm_i, ...
          min(myy_i+6*sigm_i,yy-dd*sigma), yy-dd*sigma, yy+dd*sigma,...
          max(myy_i+6*sigm_i,yy+dd*sigma)];

        % grid values
        a=1e-6;
        fvec =[ bp(1):df(2):bp(2)-a, bp(2):df(1):bp(3)-a, bp(3):max(df):bp(4)-a, ...
          bp(4):df(2):bp(5)-a, bp(5):df(1):bp(6)];
      else
        % grid break points
        bp=[min(myy_i-6*sigm_i,yy-dd*sigma), yy-dd*sigma, yy+dd*sigma,...
          max(myy_i-6*sigm_i,yy+dd*sigma), myy_i+6*sigm_i, ...
          max(myy_i+6*sigm_i,yy+dd*sigma)];

        % grid values
        a=1e-6;
        fvec =[ bp(1):df(1):bp(2)-a, bp(2):df(2):bp(3)-a, bp(3):max(df):bp(4)-a, ...
          bp(4):df(1):bp(5)-a, bp(5):df(2):bp(6)];
      end

      np=numel(fvec);
      logpt = lpt(fvec,0);
      lpt_max = max([logpt lpt([myy_i mg],0)]);
      lambdaconf=[fvec(1), fvec(end)];
      for i1=2:np-1
        if logpt(i1) < lpt_max+log(1e-7) %(exp(logpt(i1))/exp(lpt_max) < 1e-7)
          lambdaconf(1) = fvec(i1);
        else
          break;
        end
      end
      for i1=1:np-2
        if logpt(end-i1) < lpt_max+log(1e-7) %(exp(logpt(end-i1))/exp(lpt_max) < 1e-7)
          lambdaconf(2) = fvec(end-i1);
        else
          break;
        end
      end
    else
      % set the integration limits in easier cases
      np=20;
      if mg>myy_i
        lambdaconf=[myy_i-6*sigm_i,max(mg+6*sg,myy_i+6*sigm_i)];
        fvec=linspace(myy_i,mg,np);
      else
        lambdaconf=[min(mg-6*sg,myy_i-6*sigm_i),myy_i+6*sigm_i];
        fvec=linspace(mg,myy_i,np);
      end
      lpt_max=max(lpt(fvec,0));
    end
    C=log(1)-lpt_max; % scale the log-density for the quadrature tolerance
  else
    lambdaconf=[mg-6*sg,mg+6*sg];
    C=log(1)-lpt(mg,0);
  end

  if nu>nu_lim
    % the limiting normal observation model
    Vz=sigm2_i+sigma2/eta;
    g_i(1) = 0.5*( (1-eta)/sigma2 -1/Vz/eta  + (yy-myy_i)^2 /Vz^2 /eta ) *sigma2/eta;

    if (isfield(likelih,'p') && ~isempty(likelih.p.nu))
      g_i(2) = 0;
    end
  else

    % Integrate with quadrature
    RTOL = 1.e-6;
    ATOL = 1e-7;

    % Integrate with quad
    %zm=@(f) exp(lpt(f,C));
    %[m_0, fhncnt] = quadgk(zm, lambdaconf(1), lambdaconf(2),'AbsTol',ATOL,'RelTol',RTOL)

    % Use the normalization determined in the lik_t_tiltedMoments2
    m_0=exp(lnZhat+C);

    zm=@(f) deriv_sigma2(f).*exp(lpt(f,C))*sigma2;
    [g_i(1), fhncnt] = quadgk( zm, lambdaconf(1), lambdaconf(2),'AbsTol',ATOL,'RelTol',RTOL);
    g_i(1) = g_i(1)/m_0;

    if (isfield(likelih,'p') && ~isempty(likelih.p.nu))
      zm=@(f) deriv_nu(f).*exp(lpt(f,C));
      [g_i(2), fhncnt] = quadgk( zm, lambdaconf(1), lambdaconf(2),'AbsTol',ATOL,'RelTol',RTOL);
      g_i(2) = g_i(2)/m_0.*nu.*log(nu);
    end

  end

  function lpdf = lpt(f,C)
    % logarithm of the tilted distribution
    r = yy-f;
    lpdf = gammaln((nu + 1) / 2) - gammaln(nu/2) -log(nu.*pi.*sigma2)/2;
    lpdf = lpdf + log(1 + r.^2./nu./sigma2) .* (-(nu+1)/2);
    lpdf = lpdf*eta - (0.5/sigm2_i) * (f-myy_i).^2 + (C-log(2*pi*sigm2_i)/2);
  end

  function g = deriv_nu(f)
    % derivative of the log-likelihood wrt nu
    r = yy-f;
    temp = r.^2 ./(nu*sigma2);
    g = psi((nu+1)/2) - psi(nu/2) - 1/nu;
    g = g + (1+1/nu).*temp./(1+temp);

    % for small values use a more accurate method for log(1+x)
    ii = temp<1e3;
    g(ii) = g(ii)  - log1p(temp(ii));
    g(~ii) = g(~ii) - log(1+temp(~ii));
    g = g*0.5;

  end

  function g = deriv_sigma2(f)
    % derivative of the log-likelihood wrt sigma2
    r = yy-f;
    temp = r.^2 /sigma2;
    g  = -1/sigma2/2 + ((1+1/nu)/2) * temp ./ (1 + temp/nu) /sigma2;
  end

end

function [f, a] = lik_t_optimizef(gp, y, K, Lav, K_fu)
%LIK_T_OPTIMIZEF  function to optimize the latent variables
%                 with EM algorithm
%
%  Description:
%    [F, A] = LIK_T_OPTIMIZEF(GP, Y, K, Lav, K_fu) Takes Gaussian
%    process structure GP, observations Y and the covariance
%    matrix K. Solves the posterior mode of F using EM algorithm
%    and evaluates A = (K + W)\Y as a sideproduct. Lav and K_fu
%    are needed for sparse approximations. For details, see
%    Vanhatalo, Jyl�nki and Vehtari (2009): Gaussian process
%    regression with Student-t likelihood. This subfunction is
%    needed when using lik_specific optimization method for mode
%    finding in Laplace algorithm.
%

  iter = 1;
  sigma2 = gp.lik.sigma2;
%  if sigma2==0
%    f=NaN;a=NaN;
%    return
%  end
  nu = gp.lik.nu;
  n = length(y);

  switch gp.type
    case 'FULL'
      iV = ones(n,1)./sigma2;
      siV = sqrt(iV);
      B = eye(n) + siV*siV'.*K;
      [L,notpositivedefinite] = chol(B);
      if notpositivedefinite
        f=NaN;a=NaN;
        return
      end
      B=B';
      b = iV.*y;
      a = b - siV.*(L'\(L\(siV.*(K*b))));
      f = K*a;
      while iter < 200
        fold = f;
        iV = (nu+1) ./ (nu.*sigma2 + (y-f).^2);
        siV = sqrt(iV);
        B = eye(n) + siV*siV'.*K;
        L = chol(B)';
        b = iV.*y;
        ws=warning('off','MATLAB:nearlySingularMatrix');
        a = b - siV.*(L'\(L\(siV.*(K*b))));
        warning(ws);
        f = K*a;

        if max(abs(f-fold)) < 1e-8
          break
        end
        iter = iter + 1;
      end
    case 'FIC'
      K_uu = K;

      Luu = chol(K_uu)';
      B=Luu\(K_fu');       % u x f

      K = diag(Lav) + B'*B;

      iV = ones(n,1)./sigma2;
      siV = sqrt(iV);
      B = eye(n) + siV*siV'.*K;
      L = chol(B)';
      b = iV.*y;
      a = b - siV.*(L'\(L\(siV.*(K*b))));
      f = K*a;
      while iter < 200
        fold = f;
        iV = (nu+1) ./ (nu.*sigma2 + (y-f).^2);
        siV = sqrt(iV);
        B = eye(n) + siV*siV'.*K;
        L = chol(B)';
        b = iV.*y;
        a = b - siV.*(L'\(L\(siV.*(K*b))));
        f = K*a;

        if max(abs(f-fold)) < 1e-8
          break
        end
        iter = iter + 1;
      end
  end

end

function upfact = lik_t_upfact(gp, y, mu, ll, z)
  nu = gp.lik.nu;
  sigma = sqrt(gp.lik.sigma2);
  sll = sqrt(ll);

  fh_e = @(f) t_pdf(f, nu, y, sigma).*norm_pdf(f, mu, sll);
  EE = quadgk(fh_e, -40, 40);

  fm = @(f) f.*t_pdf(f, nu, y, sigma).*norm_pdf(f, mu, sll)./EE;
  mm  = quadgk(fm, -40, 40);

  fV = @(f) (f - mm).^2.*t_pdf(f, nu, y, sigma).*norm_pdf(f, mu, sll)./EE;
  Varp = quadgk(fV, -40, 40);

  upfact = -(Varp - ll)./ll^2;
end

function [lpy, Ey, Vary] = lik_t_predy(lik, Ef, Varf, y, z)
%LIK_T_PREDY    Returns the predictive mean, variance and density of y
%
%  Description
%    LPY = LIK_T_PREDY(LIK, EF, VARF YT)
%    Returns logarithm of the predictive density PY of YT, that is
%       p(yt | zt) = \int p(yt | f, zt) p(f|y) df.
%    This requires also the observations YT. This subfunction is
%    needed when computing posterior preditive distributions for
%    future observations.
%
%    [LPY, EY, VARY] = LIK_T_PREDY(LIK, EF, VARF) takes a likelihood
%    structure LIK, posterior mean EF and posterior Variance
%    VARF of the latent variable and returns the posterior
%    predictive mean EY and variance VARY of the observations
%    related to the latent variables. This subfunction is needed when
%    computing posterior preditive distributions for future observations.
%

%
%  See also
%    GPLA_PRED, GPEP_PRED, GPMC_PRED

  nu = lik.nu;
  sigma2 = lik.sigma2;
  sigma = sqrt(sigma2);

  Ey = zeros(size(Ef));
  EVary = zeros(size(Ef));
  VarEy = zeros(size(Ef));
  lpy = zeros(size(Ef));
  if nargout > 1
%       for i1=1:length(Ef)
%         %%% With quadrature
%         ci = sqrt(Varf(i1));
%
%         F = @(x) x.*norm_pdf(x,Ef(i1),sqrt(Varf(i1)));
%         Ey(i1) = quadgk(F,Ef(i1)-6*ci,Ef(i1)+6*ci);
%
%         F2 = @(x) (nu./(nu-2).*sigma2).*norm_pdf(x,Ef(i1),sqrt(Varf(i1)));
%         EVary(i1) = quadgk(F2,Ef(i1)-6*ci,Ef(i1)+6*ci);
%
%         F3 = @(x) x.^2.*norm_pdf(x,Ef(i1),sqrt(Varf(i1)));
%         VarEy(i1) = quadgk(F3,Ef(i1)-6*ci,Ef(i1)+6*ci) - Ey(i1).^2;
%       end
%       Vary = EVary + VarEy;

      Ey = Ef;
      if nu>2
        Vary=nu./(nu-2).*sigma2 +Varf;
      else
        warning('Variance of Student''s t-distribution is not defined for nu<=2')
        Vary=NaN+Varf;
      end
  end

  lpy = zeros(length(y),1);
  for i2 = 1:length(y)
    mean_app = Ef(i2);
    sigm_app = sqrt(Varf(i2));

    pd = @(f) t_pdf(y(i2), nu, f, sigma).*norm_pdf(f,Ef(i2),sqrt(Varf(i2)));
    lpy(i2) = log(quadgk(pd, mean_app - 12*sigm_app, mean_app + 12*sigm_app));
  end

end

function prctys = lik_t_predprcty(lik, Ef, Varf, zt, prcty)
%LIK_T_PREDPRCTY  Returns the percentiles of predictive density of y
%
%  Description
%    PRCTY = LIK_T_PREDPRCTY(LIK, EF, VARF YT, ZT)
%    Returns percentiles of the predictive density PY of YT. This
%    subfunction is needed when using function gp_predprcty.
%
%  See also
%    GP_PREDPCTY

  opt=optimset('TolX',1e-5,'Display','off');
  nt=size(Ef,1);
  prctys = zeros(nt,numel(prcty));
  prcty=prcty/100;
  nu = lik.nu;
  nu_p=max(2.5,nu);
  sigma2 = lik.sigma2;
  Vary=nu_p./(nu_p-2).*sigma2 +Varf;
  for i1=1:nt
    ci = sqrt(Varf(i1));
    for i2=1:numel(prcty)
      minf=sqrt(Vary(i1))*tinv(prcty(i2),nu)+(Ef(i1)-2.5*sqrt(Vary(i1)));
      maxf=sqrt(Vary(i1))*tinv(prcty(i2),nu)+(Ef(i1)+2.5*sqrt(Vary(i1)));
      a=(fminbnd(@(a) (quadgk(@(f) tcdf((a-f)/sqrt(Vary(i1)),nu).*norm_pdf(f,Ef(i1),ci),Ef(i1)-6*ci,Ef(i1)+6*ci,'AbsTol',1e-4)-prcty(i2)).^2,minf,maxf,opt));
%       a=(fminbnd(@(a) (quadgk(@(f) quadgk(@(y) t_pdf(y,nu,Ef(i1),sqrt(Vary(i1))),Ef(i1)-12*sqrt(Vary(i1)),a).*norm_pdf(f,Ef(i1),ci),Ef(i1)-6*ci,Ef(i1)+6*ci,'AbsTol',1e-4)-prcty(i2)).^2,minf,maxf,opt));
      prctys(i1,i2)=a;
      close all;
    end
  end
end

function mu = lik_t_invlink(lik, f, z)
%LIK_T_INVLINK  Returns values of inverse link function
%
%  Description
%    P = LIK_T_INVLINK(LIK, F) takes a likelihood structure LIK and
%    latent values F and returns the values MU of inverse link function.
%    This subfunction is needed when using gp_predprctmu.
%
%     See also
%     LIK_T_LL, LIK_T_PREDY

  mu = f;
end

function reclik = lik_t_recappend(reclik, ri, lik)
%RECAPPEND  Record append
%  Description
%    RECCF = GPCF_SEXP_RECAPPEND(RECCF, RI, GPCF) takes old
%    covariance function record RECCF, record index RI, RECAPPEND
%    returns a structure RECCF. This subfunction is needed when
%    using MCMC sampling (gp_mc).

  if nargin == 2
    % Initialize the record
    reclik.type = 'Student-t';

    % Initialize parameters
    reclik.nu = [];
    reclik.sigma2 = [];

    % Set the function handles
    reclik.fh.pak = @lik_t_pak;
    reclik.fh.unpak = @lik_t_unpak;
    reclik.fh.lp = @lik_t_lp;
    reclik.fh.lpg = @lik_t_lpg;
    reclik.fh.ll = @lik_t_ll;
    reclik.fh.llg = @lik_t_llg;
    reclik.fh.llg2 = @lik_t_llg2;
    reclik.fh.llg3 = @lik_t_llg3;
    reclik.fh.tiltedMoments = @lik_t_tiltedMoments;
    reclik.fh.tiltedMoments2 = @lik_t_tiltedMoments2;
    reclik.fh.siteDeriv = @lik_t_siteDeriv;
    reclik.fh.siteDeriv2 = @lik_t_siteDeriv2;
    reclik.fh.optimizef = @lik_t_optimizef;
    reclik.fh.upfact = @lik_t_upfact;
    reclik.fh.invlink = @lik_t_invlink;
    reclik.fh.predy = @lik_t_predy;
    reclik.fh.predprcty = @lik_t_predprcty;
    reclik.fh.recappend = @lik_t_recappend;
    reclik.p.nu=[];
    if ~isempty(ri.p.nu)
      reclik.p.nu = ri.p.nu;
    end
    reclik.p.sigma2=[];
    if ~isempty(ri.p.sigma2)
      reclik.p.sigma2 = ri.p.sigma2;
    end
  else
    % Append to the record
    likp = lik.p;

    % record sigma2
    reclik.sigma2(ri,:) = lik.sigma2;
    if isfield(likp,'sigma2') && ~isempty(likp.sigma2)
      reclik.p.sigma2 = likp.sigma2.fh.recappend(reclik.p.sigma2, ri, likp.sigma2);
    end
    % record nu
    reclik.nu(ri,:) = lik.nu;
    if isfield(likp,'nu') && ~isempty(likp.nu)
      reclik.p.nu = likp.nu.fh.recappend(reclik.p.nu, ri, likp.nu);
    end
  end

end


function y = log_sum_exp( x, dim )

%LOG_SUM_EXP   CVX internal version.

error( nargchk( 1, 2, nargin ) );
cvx_expert_check( 'log_sum_exp', x );

sx = size( x );
if nargin < 2 || isempty( dim ),
    dim = cvx_default_dimension( sx );
elseif ~cvx_check_dimension( dim, true ),
    error( 'Second argument must be a valid dimension.' );
end

%
% Quick exits
%

sx( end + 1 : dim ) = 1;
nx = sx( dim );
sy = sx;
sy( dim ) = 1;
if nx == 0,
    sx( dim ) = 1; %#ok
    y = -Inf * ones( sy );
    return
elseif nx == 1,
    y = x;
    return;
elseif any( sx == 0 ),
    y = zeros( sy );
    return
end

%
% Determine the expression types
%

persistent remap
if isempty( remap ),
    remap_2 = cvx_remap( 'real' );
    remap_1 = cvx_remap( 'convex' ) & ~remap_2;
    remap = remap_1 + 2 * remap_2;
end
v = reshape( remap( cvx_classify( x ) ), sx );
v = min( v, [], dim );

%
% Process each type of expression one piece at a time
%

vu = sort( v(:) );
vu = vu([true;diff(vu)~=0]);
nv = length( vu );
if nv > 1,
    y = cvx( sy, [] );
    if prod(sx(1:dim+1))>1 && prod(sx(dim+1:end))>1,
        perm = [ dim, 1:dim-1, dim+1:length(sx) ];
        x  = permute( x, perm );
        v  = permute( v, perm );
        y  = permute( y, perm );
        dim = 1;
    end
end
for k = 1 : nv,

    %
    % Select the category of expression to compute
    %

    vk = vu( k );
    if nv == 1,
        xt = x;
        sz = sy; %#ok
    else
        t = v == vk;
        xt = cvx_subsref( x, cvx_expand_dim( t, dim, nx ) );
        sx = size( xt );
        sz = sx;
        sz( dim ) = 1; %#ok
    end

    %
    % Perform the computations
    %

    switch vk,
        case 0,
            % Invalid
            error( 'Disciplined convex programming error:\n    Illegal operation: log_sum_exp( {%s} ).', cvx_class( xt ) );
        case 1,
            % Affine, convex
            cvx_begin
                variable w( sx )
                epigraph variable z( sz )
                { cvx_accept_convex( x ) - cvx_expand_dim( z, dim, nx ), 1, w } == exponential( sx );
                sum( w, dim ) == 1;
            cvx_end
        case 2,
            % Constant
            cvx_optval = cvx( log_sum_exp( cvx_constant( xt ) ) );
        otherwise,
            error( 'Shouldn''t be here.' );
    end

    %
    % Store the results
    %

    if nv == 1,
        y = cvx_optval;
    else
        y = cvx_subsasgn( y, t, cvx_optval );
    end

end

% Reshape again, just in case
y = reshape( y, sy );

% Copyright 2010 Michael C. Grant and Stephen P. Boyd.
% See the file COPYING.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function y = vl_nnsoftmaxloss(x,c,dzdy)
%VL_NNSOFTMAXLOSS CNN combined softmax and logistic loss.
%   **Deprecated: use `vl_nnloss` instead**
%
%   Y = VL_NNSOFTMAX(X, C) applies the softmax operator followed by
%   the logistic loss the data X. X has dimension H x W x D x N,
%   packing N arrays of W x H D-dimensional vectors.
%
%   C contains the class labels, which should be integers in the range
%   1 to D. C can be an array with either N elements or with dimensions
%   H x W x 1 x N dimensions. In the fist case, a given class label is
%   applied at all spatial locations; in the second case, different
%   class labels can be specified for different locations.
%
%   DZDX = VL_NNSOFTMAXLOSS(X, C, DZDY) computes the derivative of the
%   block projected onto DZDY. DZDX and DZDY have the same dimensions
%   as X and Y respectively.

% Copyright (C) 2014-15 Andrea Vedaldi.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

% work around a bug in MATLAB, where native cast() would slow
% progressively
if isa(x, 'gpuArray')
  switch classUnderlying(x) ;
    case 'single', cast = @(z) single(z) ;
    case 'double', cast = @(z) double(z) ;
  end
else
  switch class(x)
    case 'single', cast = @(z) single(z) ;
    case 'double', cast = @(z) double(z) ;
  end
end

%X = X + 1e-6 ;
sz = [size(x,1) size(x,2) size(x,3) size(x,4)] ;

if numel(c) == sz(4)
  % one label per image
  c = reshape(c, [1 1 1 sz(4)]) ;
end
if size(c,1) == 1 & size(c,2) == 1
  c = repmat(c, [sz(1) sz(2)]) ;
end

% one label per spatial location
sz_ = [size(c,1) size(c,2) size(c,3) size(c,4)] ;
assert(isequal(sz_, [sz(1) sz(2) sz_(3) sz(4)])) ;
assert(sz_(3)==1 | sz_(3)==2) ;

% class c = 0 skips a spatial location
mass = cast(c(:,:,1,:) > 0) ;
if sz_(3) == 2
  % the second channel of c (if present) is used as weights
  mass = mass .* c(:,:,2,:) ;
  c(:,:,2,:) = [] ;
end

% convert to indexes
c = c - 1 ;
c_ = 0:numel(c)-1 ;
c_ = 1 + ...
  mod(c_, sz(1)*sz(2)) + ...
  (sz(1)*sz(2)) * max(c(:), 0)' + ...
  (sz(1)*sz(2)*sz(3)) * floor(c_/(sz(1)*sz(2))) ;

% compute softmaxloss
xmax = max(x,[],3) ;
ex = exp(bsxfun(@minus, x, xmax)) ;

%n = sz(1)*sz(2) ;
if nargin <= 2
  t = xmax + log(sum(ex,3)) - reshape(x(c_), [sz(1:2) 1 sz(4)]) ;
  y = sum(sum(sum(mass .* t,1),2),4) ;
else
  y = bsxfun(@rdivide, ex, sum(ex,3)) ;
  y(c_) = y(c_) - 1;
  y = bsxfun(@times, y, bsxfun(@times, mass, dzdy)) ;
end


function [t,conse,allt,glm] = designsim(noisetype,dmodel,HRF,ISI,TR,noise_var,c,beta,S,varargin)
% function [t,modelse,allt,glm] = designsim(noisetype,dmodel,HRF,ISI,TR,noise_var,c,betas,S,xcfunction [if using 'myxc'],num2avgover)
%  [t,modelse,allt,glm] = designsim('myxc',M.modelatTR,HRF,ISI,TR,noise_var,c,beta,fullS,xc)
%
% noisetype:
%   '1overf' : use Luis Hernandez' 1/f model with your specified noise variance
%   'myxc'   : use your own autocorrelation function and noise variance
%
% model = matrix of regressors (cols) sampled at frequency of ISI
% noise_var = variance of 1/f noise from your scanner
% c = a contrast across the regressors
% beta = a list of betas to test
%   each beta should be a row vector, and you can have multiple rows
%
% S: smoothing matrix, or 0 for no smoothing.
% _______________________________________________________________________
% Output is a distribution of t-scores in a vector and the maximum correlation between predictors
% t-scores are in a column vector, one for each set of betas.
%
% Special mode: if 4 output arguments specified, 'plot mode'.  Produces graph and extended output.
% outputs:
%	t 		mean of t-scores for specified number of noise vectors tested, numnoise in script
%	allt	vector of all t values generated for each noise vector
%
% specifying a 4th output puts designsim.m into 'verbose' mode.
%
% Last modified   5/24/01 Tor Wager		add noise first, then smooth and filter, etc.

% if no stimuli for one condition, gives error - create extra col of zeros if this happens.
while size(beta,2) > size(dmodel,2)
 	warning('      Designsim.m : dmodel has too few columns or beta is too long! No stim. in one cond.?')
	whos dmodel
     	dmodel = [dmodel zeros(size(dmodel,1),1)];
end

if isempty(c)
	if nargout > 3,disp('	...designsim.m: No contrasts found.  Using first predictor for one sample T-test.'),end
	c = zeros(1,size(dmodel,2));
	c(1) = 1;
end

c = c';

done = 0;
%if nargout > 3,numnoise = 1;,else numnoise = 1;,end
% if average over noise models is given, return the average - otherwise return each individual t and se score.
if nargin > 10, numnoise = varargin{2};,else numnoise = 1;,end

% make the noise vectors
% =====================================================================================
switch noisetype
case '1overf'
   noise = make1overf(size(dmodel,1), 1/ISI) * sqrt(noise_var);
case 'myxc'

    for i = 1:numnoise
        noise(:,i) = noisevector(size(dmodel,1),varargin{1},noise_var)';
    end
otherwise error('unsupported noise type.')
end

   		% Create the response data by weighting all the regressors by a beta parameter
   		% adding all the regressors together
   		% and adding noise to the result
 beta = beta';
 if nargout > 3,
    	%figure;,
    	%disp('testing with only one noise model, returning 1st beta vector only in glm.')
    	%beta = beta(:,1); noise = noise(:,1);
	c = c(:,1);	% test first contrast here - this should be changed back for normal use. this is for rundesignsimMAP
 else c = c(:,1); % tests only first contrast if you specify many iterations.
 end

% make the data vectors and fit
% =====================================================================================
for i = 1:size(beta,2)

	if nargout > 3
        	subplot(4,size(beta,2),3*i-2);hold on
        	plot(dmodel * beta(:,i),'r');title(['ideal model, betas = ' num2str(beta(:,i)')]);drawnow
        	clear glm
	end

	% make data
	% --------------------------------------------------
	for j = 1:size(noise,2)

		try
			myfit = dmodel * beta(:,i);
		catch
			disp(['	designsim.m: dmodel and beta are not the same size - wrong beta size?'])
			whos dmodel
			beta
			error('exiting.')
		end
		myfit = (myfit - mean(myfit));		% mean center so betas are not 'correlated' with intercept.
		data(:,j) = myfit + noise(:,j);

		% testing stuff -
		%disp(['designsim.m: beta = '])
		%beta(:,i)
		%figure;subplot(3,1,1);plot(noise(:,j))
		%subplot(3,1,2);plot(dmodel * beta(:,i))
		%subplot(3,1,3);plot(data(:,j));
	end

	% smoothing
	% --------------------------------------------------
	if S,
		dmodel = S * dmodel;
		data = S * data;
		glm.smoothed = 'yes';
	end

	% fit model to data and save t and se
	% --------------------------------------------------
	for j = 1:size(noise,2)
        	[myt,myconse] = my_glm(dmodel,data(:,j),c);		% fit model
        	allt(i,j) = myt(1);
		allse(i,j) = myconse(1);

		% make glm structure if necessary
		% ---------------------------------------------
        	if nargout > 3
            		glm.y = data;
            		glm.X = [dmodel ones(size(dmodel,1),1)];
            		glm.betas = (glm.X \ glm.y)';
            		glm.fit = glm.X * glm.betas';
            		glm.e = glm.y - glm.fit;
            		glm.xtxi = inv(glm.X' * glm.X);
            		glm.df = (size(glm.y,1) - size(glm.X,2));
            		glm.evar = glm.e' * glm.e / glm.df;
            		glm.se = sqrt(diag(glm.xtxi .* glm.evar))';
            		glm.t = (glm.betas ./ glm.se);
            		glm.p = tdist(glm.t,glm.df);
            		glm.c = [c;zeros(1,size(c,2))];
            		glm.con_se = diag(sqrt(glm.evar .* (glm.c' * glm.xtxi * glm.c)))';
            		glm.con_t = glm.betas * glm.c ./ glm.con_se;
            		glm.con_p = tdist(glm.con_t,glm.df);

	    		if j == 1							% plot the first noise model.
        			subplot(4,size(beta,2),3*i-1);hold on
        			plot(data(:,j));hold on;plot(glm.fit,'r--'); title(['Data with noise added and fit']);drawnow
        			subplot(4,size(beta,2),3*i);hold on
        			plot(data(:,j) - glm.fit); title('Residuals');drawnow
 	    		end
        	end

	end
	% loop thru noise models.
 end	% loop thru betas.

t = mean(allt,2);
conse = mean(allse,2);

return

% rmat = abs(corrcoef(dmodel));%rmat(rmat == 1) = 0;		% zero diagonals%r = max(max(rmat));


classdef StressNormShapeFuncCreator < handle

    properties (Access = private)
        outFile
        stressShape
        stressShapeDB
    end

    methods (Access = public)

        function obj = StressNormShapeFuncCreator(d)
            obj.init(d)
            obj.createStressShapeDataBase();
            obj.createStressShape();
        end

        function s = getStressNormShape(obj)
            s = obj.stressShape;
        end

    end

    methods (Access = private)

        function init(obj,d)
            obj.outFile       = d.outFile;
        end

        function createStressShape(obj)
            dB = obj.stressShapeDB();
            sF = ShFunc_StressNorm(dB);
%             sF.filter.preProcess();
            obj.stressShape = sF;
        end

        function createStressShapeDataBase(obj)
            dB.filename    = obj.outFile;
            dB.TOL         = obj.createMaterialProperties();
            dB.material    = 'ISOTROPIC';
            dB.method      = 'SIMPALL';
            dB.pdim        = '2D';
            dB.stressHomog = [1,0,0]';
            dB.filter      = 'P1';
            dB.optimizer   = 'SLERP';
            dB.pdim        = '2D';
            dB.scale       = 'MICRO';
            obj.stressShapeDB = dB;
        end
    end

    methods (Access = private, Static)

        function matProp = createMaterialProperties()
            matProp.rho_plus  =  1;
            matProp.rho_minus =  0;
            matProp.E_plus    =  1;
            matProp.E_minus   =  1.0000e-03;
            matProp.nu_plus   =  0.3333;
            matProp.nu_minus  =  0.3333;
        end

    end

end


function [res]=tt_smv(sttm, vec)
%TT-matrix with sparse factors by TT-vector multiplication
%   [res]=TT_SMV(STTM,VEC) Multiplies the TT-matrix stored in the TT1.0
%   format but with "sparse" cores by a vector VEC
%
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------

d=size(vec,1);
res=cell(d,1);

n=sttm{d+2}(1);
% m=size(vec{1},1);
rm1=sttm{d+1}(1);
rv1=size(vec{1},2);
%mat{1} is (n x m x rm1 , vec{1} is mxrv1)
%            n x rm1 x rv1
res{1}=sttm{1}*vec{1};
res{1}=reshape(permute(reshape(res{1},[n,rm1,rv1]),[1,3,2]),[n,rv1*rm1]);

if (d>1)
    n=sttm{d+2}(d);
    % m=size(mat{d},2);
    rm1=sttm{d+1}(d-1);
    rv1=size(vec{d},2);
    %mat{1} is (n x rm1 x rv1 , vec{1} is mxrv1)
    res{d}=sttm{d}*vec{d};
    res{d}=reshape(permute(reshape(res{d},[n,rm1,rv1]),[1,3,2]),[n,rv1*rm1]);
end;

for i=2:d-1
    n=sttm{d+2}(i);
    m=size(vec{i},1);
    rm1=sttm{d+1}(i-1);
    rm2=sttm{d+1}(i);
    rv1=size(vec{i},2);
    rv2=size(vec{i},3);
    %mat{i} is n x m x rm1 x rm2, vec{i} is m x rv1 x rv2
    %n x (rv1*rv2)*rm1*rm2
    %n x rm2 x rm1 x m, n x rm2 x rm1 x (rv1*rv2)
    %n x rm2 x rmx1 x rv1 x rv2
    %want: n x rv1*rm1 x rv2*rm2
    res{i}=sttm{i}*reshape(vec{i},[m,rv1*rv2]);
    res{i}=reshape(res{i},[n,rm2,rm1,rv1,rv2]);
    res{i}=permute(res{i},[1,4,3,5,2]);
    res{i}=reshape(res{i},[n,rv1*rm1,rv2*rm2]);
    %res{i}=permute(reshape(permute(mat{i},[1,3,4,2])*reshape(vec{i},[m,rv1*rv2]),[n,rv1,rv2,rm1,rm2]),[1,2,4,3,5]);
end

end


function magic3 ( n )

%*****************************************************************************80
%
%% MAGIC3 is a "main program" for a MATLAB compiler example.
%
%  Discussion:
%
%    This example is similar to magicsquare_print, but now it
%    includes calls to the timestamp() function, which is a separate
%    .M file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 April 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer/string N, the order of the magic square.
%
  if ( ischar ( n ) )
    n = str2num ( n );
  end

  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'MAGIC3\n' );
  fprintf ( 1, '  MATLAB version.\n' );
%
%  Compute the magic square.
%
  m = magic ( n );
%
%  Print the magic square.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Magic square of order %d:\n', n );
  fprintf ( 1, '\n' );
  for i = 1 : n
    for j = 1 : n
      fprintf ( 1, '  %2d', m(i,j) );
    end
    fprintf ( 1, '\n' );
  end
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'MAGIC3\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


function h = conv(f, g)
%CONV   Convolution of DELTAFUN objects.
%   H = CONV(F, G) produces the convolution of DELTAFUN objects F and G:
%                     -
%                    /
%           H(x) =   |    F(t) G(x-t) dt,  x in [a + c, b + d]
%                    /
%   The output H is a cell array of DELTAFUNs or CLASSICFUNS. If the result of
%   the convolution of F and G is zero, an empty cell is returned. The cell
%   array returned is used by higher level convolutions.

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

% Return empty for an empty input:
if ( isempty(f) || isempty(g) )
    h = deltafun();
    return
end

if ( ~isa(f, 'deltafun') )
    deltaMagF = [];
    deltaLocF = [];
    funF = f;
else
    f = simplifyDeltas(f);
    if ( isa(f, 'deltafun') )
        deltaMagF = f.deltaMag;
        deltaLocF = f.deltaLoc;
        funF = f.funPart;
    else
        deltaMagF = [];
        deltaLocF = [];
        funF = f;
    end
end

if ( ~isa(g, 'deltafun') )
    deltaMagG = [];
    deltaLocG = [];
    funG = g;
else
    g = simplifyDeltas(g);
    if ( isa(g, 'deltafun') )
        deltaMagG = g.deltaMag;
        deltaLocG = g.deltaLoc;
        funG = g.funPart;
    else
        deltaMagG = [];
        deltaLocG = [];
        funG = g;
    end
end

% Extract the domains of f and g:
domF = funF.domain;
domG = funG.domain;
a = domF(1);
b = domF(end);
c = domG(1);
d = domG(end);

% Get the threshold for deltafunctions:
pref = chebfunpref();
deltaTol = pref.deltaPrefs.deltaTol;

% Compute the convolution of funParts and append it to the output cell:
h = conv(funF, funG);

% Remove zero funs for simplicity:
zeroIndices = cellfun( @(hk) iszero(hk), h);
h(zeroIndices) = [];

%% Get all the deltafunction contributions
% (f + df) * (g + dg) = f*g + dg * (f + df) + df * (g + dg) - df * dg
%                     = f*g + dg * (f + df/2) + df * (g + dg/2)

% Contributions due to deltafunctions in F:
% df * (g + dg/2):
h = convDeltas(deltaMagF, deltaLocF, g, c, d, deltaTol, h);

% Contributions due to delta functions in G:
% dg * (f + df/2);
h = convDeltas(deltaMagG, deltaLocG, f, a, b, deltaTol, h);

% Check for emptiness:
if ( isempty(h) )
    h = {fun.constructor(0, struct('domain', [a, b]))};
    return
end

%% Make sure that h is a cell array with non-overlapping funs.

% First extract the breakpints from funs forming h:
doms = cellfun(@(hk) hk.domain, h, 'uniformoutput', 0);
breakPoints = sort(unique(horzcat(doms{:})));

% Coalesce breakpoints that are extremely close together.
if ( any(isinf(breakPoints)) )
    tol = 2*eps;  % hscale of functions on unbounded domains is 1.
else
    tol = 2*eps*norm(breakPoints, Inf);
end

% TODO:  Take the average of points which are close together instead of just
% picking one more or less arbitrarily?
closeBreaks = abs(diff(breakPoints)) < tol;
breakPoints(closeBreaks) = [];

% Restrict each FUN to lie within the new breakpoints.
nFuns = length(h);
H = {};
deltaTol = pref.deltaPrefs.proximityTol;
for k = 1:nFuns
    hk = h{k};
    dom = hk.domain;

    % The domain endpoints of each computed FUN must lie _exactly_ within the
    % set of breakpoints or RESTRICT will fail.
    idx1 = find(abs(breakPoints - dom(1)) < tol);
    idx2 = find(abs(breakPoints - dom(2)) < tol);
    hk = changeMap(hk, breakPoints([idx1, idx2]));

    % If we just changed the map of a DELTAFUN, the locations of deltas at the
    % domain endpoints also need to be updated.
    if ( isa(hk, 'deltafun') )
        deltaLoc = hk.deltaLoc;
        domk = hk.domain;
        if( ~isempty(deltaLoc) )
            if ( abs(deltaLoc(1) - domk(1) ) < deltaTol )
                deltaLoc(1) = domk(1);
            end
            if ( abs(deltaLoc(end) - domk(2)) < deltaTol )
                deltaLoc(end) = domk(2);
            end
            hk.deltaLoc = deltaLoc;
        end
    end

    % Do the restriction.
    hk = restrict(hk, breakPoints(idx1:idx2));
    if ( ~isa(hk, 'cell') )
        hk = {hk};
    end
    H = [H, hk]; %#ok<AGROW>
end

% If H has the same number of funs, nothing further needs to be done:
if ( length(H) == length(breakPoints) - 1 )
    return
end

% H has more funs as a result of restrict. Re-initialize h with 0 BNDFUNS:
nFuns = length(breakPoints) - 1;
h = {};
for k = 1:nFuns
    data.domain = breakPoints(k:k+1);
    h = [h, {bndfun(0, data)}]; %#ok<AGROW>
end

% Loop through H and add each fun to the relevant domain piece in h:
for k = 1:length(H)
    hk = H{k};
    dom = hk.domain;
    if ( abs(dom(2) - dom(1)) > tol )
        idx = find(breakPoints == dom(1));
        h{idx} = h{idx} + hk; %#ok<AGROW>
    end
end

end

function h = convDeltas(deltaMagF, deltaLocF, g, c, d, deltaTol, h)
%   H = CONVDELTAS() convolves the function G with deltafunctions described by
%   deltaMagF and deltaLocF. G originally lives on [c, d] and the output is
%   returned in H. This function is called twice so deltafunctions in g are
%   scaled by half to get the correct delta-delta contributions.

%%
% Contributions due to deltafunctions in F:
% df * (g + dg/2):
[m, n] = size(deltaMagF);
% Loop through the delta function matrix:
for i = 1:m
    for j = 1:n
        if ( abs(deltaMagF(i, j)) > deltaTol )
            % Take appropriate derivative, scale and shift the function:
            hij = deltaMagF(i, j) * changeMap(diff(g,i-1), deltaLocF(j) + [c d]);
            % The half below is to make sure that delta-delta interaction
            % is not counted twice:
            if ( isa(hij, 'deltafun') )
                hij.deltaMag = hij.deltaMag/2;
            end

            % If the result is non-zero, append it:
            if ( ~iszero(hij) )
                h = [h, {hij}]; %#ok<AGROW>
            end
        end
    end
end
end


% Demo of using different line codings

bits = [1 0 1 0 0 0 1 1 0];
bitrate = 1; % bits per second

figure;
[t,s] = unrz(bits,bitrate);
plot(t,s,'LineWidth',3);
axis([0 t(end) -0.1 1.1])
grid on;
title(['Unipolar NRZ: [' num2str(bits) ']']);

figure;
[t,s] = urz(bits,bitrate);
plot(t,s,'LineWidth',3);
axis([0 t(end) -0.1 1.1])
grid on;
title(['Unipolar RZ: [' num2str(bits) ']']);

figure;
[t,s] = prz(bits,bitrate);
plot(t,s,'LineWidth',3);
axis([0 t(end) -1.1 1.1])
grid on;
title(['Polar RZ: [' num2str(bits) ']']);

figure;
[t,s] = manchester(bits,bitrate);
plot(t,s,'LineWidth',3);
axis([0 t(end) -1.1 1.1])
grid on;
title(['Manchester: [' num2str(bits) ']']);


% Wrapper for pca_apply that allows for application to large X.
%
% Wrapper for pca_apply that splits and processes X in parts, this may be
% useful if processing cannot be done fully in parallel because of memory
% constraints. See pca_apply for usage.
%
% USAGE
%  same as pca_apply
%
% INPUTS
%  same as pca_apply
%
% OUTPUTS
%  same as pca_apply
%
% EXAMPLE
%
% See also PCA, PCA_APPLY, PCA_VISUALIZE

% Piotr's Image&Video Toolbox      Version 1.5
% Written and maintained by Piotr Dollar    pdollar-at-cs.ucsd.edu
% Please email me if you find bugs, or have suggestions or questions!

function [ Yk, Xhat, avsq ] = pca_apply_large( X, U, mu, vars, k )

siz = size(X); nd = ndims(X);  [N,r]  = size(U);
if(N==prod(siz) && ~(nd==2 && siz(2)==1)); siz=[siz, 1]; nd=nd+1; end
inds = {':'}; inds = inds(:,ones(1,nd-1));
d= prod(siz(1:end-1));

% some error checking
if(d~=N); error('incorrect size for X or U'); end
if(isa(X,'uint8')); X = double(X); end
if( k>r )
  warning(['Only ' int2str(r) '<k comp. available.']); %#ok<WNTAG>
  k=r;
end

% Will run out of memory if X has too many elements.  Hence, run
% pca_apply on parts of X and recombine.
maxwidth = ceil( (10^7) / d );
if(maxwidth > siz(end))
  if (nargout==1)
    Yk = pca_apply( X, U, mu, vars, k );
  elseif (nargout==2)
    [Yk, Xhat] = pca_apply( X, U, mu, vars, k );
  else
    [ Yk, Xhat, avsq ] = pca_apply( X, U, mu, vars, k );
  end
else
  Yk = zeros( k, siz(end) );  Xhat = zeros( siz );
  avsq = 0;  avsqOrig = 0;  last = 0;
  while(last < siz(end))
    first=last+1;  last=min(first+maxwidth-1,siz(end));
    Xi = X(inds{:}, first:last);
    if( nargout==1 )
      Yki = pca_apply( Xi, U, mu, vars, k );
    else
      if( nargout==2 )
        [Yki,Xhati] = pca_apply( Xi, U, mu, vars, k );
      else
        [Yki,Xhati,avsqi,avsqOrigi] = pca_apply( Xi, U, mu, vars, k );
        avsq = avsq + avsqi;  avsqOrig = avsqOrig + avsqOrigi;
      end;
      Xhat(inds{:}, first:last ) = Xhati;
    end
    Yk( :, first:last ) = Yki;
  end;
  if( nargout==3); avsq = avsq / avsqOrig; end
end


% SVARGPLVMPREPAREDATA Load and prepare different kinds of data for the shared variational GPLVM.
% DESC Load and prepare different kinds of data for the shared variational GPLVM.
%
% COPYRIGHT: Andreas C. Damianou, Carl Henrik Ek,  2011
% SEEALSO : demSharedVargplvm1
%
% VARGPLVM

%-- Toy (random) data (samples from GPs)
if isempty(dataSetNames) || (isstr(dataSetNames) && strcmp(dataSetNames,'toy'))
    fprintf('# Creating toy data of type %s...\n', toyDataCreate);
    toyData = 1;
    %toyDataCreate = 'vargplvm'; % options: 'vargplvm', 'human','sampling', 'fols'
    dataType = toyDataCreate;
    switch toyDataCreate
        case 'humanPose'
            load humanPose;
            %ind2 = floor(1:2:size(Y,1));
            %%%__silhouettes are whole images
            if exist('imageSilhouette') && imageSilhouette
                load sil_images
            end
            % Remove the 'drunk' sequence
            Y1 = Y(1:100,:);
            Y2 = Y(337:end,:);
            Y = [Y1; Y2];
            clear('Y1','Y2');
            Z1 = Z(1:100,:);
            Z2 = Z(337:end,:);
            Z = [Z1; Z2];
            clear('Z1','Z2');
            % Subsample
            ind2 = floor(1:2:size(Y,1));
            Ytoy{1} = Y(ind2,:);
            Ytoy{2} = Z(ind2,:);
            dataSetNames={'silhouette', 'pose'};
            % X_init = Xp;
            %mappingKern = {'linard2', 'white'};
            %mappingKern = {'rbfard2', 'white'};
            latentDim = 5; % Anything > 2 and < 10
            %xyzankurAnim(Z_test, 3);
        case 'fols'
            %             addpath('../../ncca/matlab/');
            %             addpath('../../sgplvm/matlab/');
            %             addpath('../../fgplvm(svn)/matlab/');
            %             dem_sgplvm_fols; % This creates the dataset
            %             clear model
            %             clear options
            %alpha = linspace(0,2*pi,100);
            alpha = linspace(0,4*pi,100);
            Z1 = cos(alpha)';
            Z2 = sin(alpha)';
            Z3= (cos(alpha)').^2;
            % Z3 = 2*cos(2*alpha)' + 2*sin(2*alpha)' ; %

            % Scale and center data
            bias_Z1 = mean(Z1);
            Z1 = Z1 - repmat(bias_Z1,size(Z1,1),1);
            scale_Z1 = max(max(abs(Z1)));
            Z1 = Z1 ./scale_Z1;

            bias_Z2 = mean(Z2);
            Z2 = Z2 - repmat(bias_Z2,size(Z2,1),1);
            scale_Z2 = max(max(abs(Z2)));
            Z2 = Z2 ./ scale_Z2;

            Z3 = Z3 - repmat(mean(Z3),size(Z3,1),1);%
            Z3 = Z3 ./ (max(max(abs(Z3))));%

            noiseLevel = 0.3; % Default: 0.1
            % Map 1-D to 10-D and add some noise
            Z2p = Z2*rand(1,10);
            Z2p = Z2p + noiseLevel.*randn(size(Z2p));
            Z1p = Z1*rand(1,10);
            Z1p = Z1p + noiseLevel.*randn(size(Z1p));

            Z3p = Z3*rand(1,10);%
            Z3p = Z3p + noiseLevel.*randn(size(Z3p));%

            % pca(Z2p) % This shows that it is actually a 1-D dataset

            % Y = [Z1p Z2p];
            % We like the numer of latent dims to be 2+numSharedDims, ideally 3. With
            % vargplvm we set Q=6 and expect the other 3 or 4 to be switched off.
            % [U,V] = pca(Y,6);
            % Xp = Y*V;
            % pca(Xp)

            %---
            numSharedDims = 5;
            Z1p(:,1:numSharedDims) = Z3p(:,1:numSharedDims);
            Z2p(:,1:numSharedDims) = Z3p(:,1:numSharedDims);
            bar(pca([Z1p Z2p]))
            % return
            %---
            Ytoy{1} = Z1p;
            Ytoy{2} = Z2p;
            %Ytoy{3}= Z3p;%
            dataSetNames={'fols_cos', 'fols_sin'};
            % X_init = Xp;
            mappingKern = {'linard2', 'white'};
            % mappingKern = {'rbfard2', 'white'};
            latentDim = 6; % Anything > 2 and < 10
            % indPoints = 80;
        case 'vargplvm'
            % In the 'vargplvm' case, we generate X from a big Y (which
            % contains signal from rbf in the first half, and from a matern32
            % in the second half of the dims). Then, we set manually the
            % lengthscales for model_i to favor the i partition of X, and
            % generate a new Y_i from the posterior, hopefully associated with
            % the partition X_i. In the end (after optim), we would expect the lengthscales
            % found to be like the ones we manually set here.

            % Constants
            n = dataToKeep;
            D = 20;
            numSubModels = 2;
            latentDimPerModel = 3;
            numSharedDims = 0; % 2;
            %
            latentDim = latentDimPerModel * numSubModels + numSharedDims;
            x = linspace(-1, 1, n)';
            indPoints = round(2/3 * n);

            % Create toy Y as a combination of an rbf and a matern signal
            krn = 'rbf';
            invWidth = 10;
            kern = kernCreate(x,krn);
            kern.inverseWidth = invWidth;
            Kshared = kernCompute(kern, x);
            Y = zeros(n, D);
            for d=1:round(D/2)
                Y(:,d) = real(gsamp(zeros(1, size(x, 1)), Kshared, 1))';
            end

            krn = 'matern32';
            invWidth = 10;
            kern = kernCreate(x,krn);
            kern.inverseWidth = invWidth;
            Kshared = kernCompute(kern, x);
            for d=round(D/2+1):D
                Y(:,d) = real(gsamp(zeros(1, size(x, 1)), Kshared, 1))';
            end

            % Learn a model and X that fit Y
            options = vargplvmOptions('dtcvar');
            options.kern = 'rbfardjit';
            options.numActive = indPoints;
            options.optimiser = 'scg';
            d = size(Y, 2);
            model = vargplvmCreate(latentDim, d, Y, options);
            model = vargplvmParamInit(model, model.m, model.X);
            model.vardist.covars = 0.5*ones(size(model.vardist.covars)) + 0.001*randn(size(model.vardist.covars));
            % Optional____
            model.initVardist = 1;
            model = vargplvmOptimise(model, 1, 100);
            model.initVardist = 0;
            model = vargplvmOptimise(model, 1, 300);
            %_____
            % Create new Y_i by manipulating the ard parameters
            % latentIndices = randperm(size( model.vardist.means,2));
            latentIndices = 1:size(model.vardist.means,2);
            allIndices = 1:size(latentIndices,2);
            startVal = 1;
            endVal = numSharedDims;
            sharedIndices = latentIndices(startVal:endVal);
            dataSetNames = {};
            for i=1:numSubModels
                modelTemp = model;
                startVal = endVal+1;
                endVal = startVal + latentDimPerModel - 1;
                curIndices = [latentIndices(startVal:endVal) sharedIndices];
                switchOffIndices = setdiff(allIndices, curIndices);
                modelTemp.kern.inputScales(switchOffIndices) = 0;
                modelTemp.kern.comp{1}.inputScales(switchOffIndices) = 0;
                modelTemp.kern.comp{1}.inputScales(sharedIndices) = (1/2)* modelTemp.kern.comp{1}.inputScales(sharedIndices); % Optional
                modelTemp.kern.inputScales(sharedIndices) = (1/2)* modelTemp.kern.inputScales(sharedIndices); % Optional
                figure, bar(modelTemp.kern.inputScales)
                Ytoy{i} = vargplvmPosteriorMeanVar(modelTemp, modelTemp.vardist.means);
                dataSetNames = {dataSetNames{:}, ['datasetNo' num2str(i)]};
            end
            numberOfDatasets = length(dataSetNames);
            clear('model','modelTemp','options','x','D','n','Y','kern','Kshared','d');
            %{
        case 'sampling' % DOESN'T WORK
            %-- 2nd way: Generate (sample) X from N(0,I), then arbitrarily split X into
            % X_1, X_2,..., X_n, X_s  and fit a GP to then find the posteriors
            % p(Y_1 | X_1, Xs), ..., p(Y_n | X_n, X_s), ... , p(Y_s | X_s). Then, use
            % these Y's as datasets and see if you can get the X's back!!
            n = dataToKeep; % N
            indPoints = round((2/3)*n);
            subModelsNum = 2;
            numSharedDims = 2;
            Q = subModelsNum * (latentDimPerModel+numSharedDims);
            Xsamp = zeros(n,Q);
            for i=1:n
                Xsamp(i,:) = randn(1,Q);
            end
            latentIndices = randperm(size(Xsamp,2));
            startVal = 1;
            endVal =  numSharedDims;
            sharedIndices = latentIndices(startVal:endVal);
            for i=1:subModelsNum
                startVal = endVal+1;
                endVal = startVal + latentDimPerModel;
                X_i = [Xsamp(startVal:endVal) Xsamp(sharedIndices)];
                % Fit a gp
                % ...
                % Ytoy{i} =
            end
            %}
        case 'human'
            load('../../sgplvm/matlab/nccaDemoData.mat')
            Ytoy{1} = Y_train;
            Ytoy{2} = Z_train;
            numberOfDatasets=2;
        case 'rbfOu'
            %-- For private signals
            dataSetNames = {'rand1','rand2','rand3','rand4'};
            kernels = {'rbf', 'rbf', 'ou','ou'};
            n = dataToKeep; % N
            indPoints = round((2/3)*n);
            dims = {40,40,40,40};
            invWidths = {10,10,15,15};
            numberOfDatasets = length(dataSetNames);
            x = linspace(-1, 1, n)';

            %-- For shared signal
            krn = 'rbf';
            invWidth = 100;
            kern = kernCreate(x,krn);
            kern.inverseWidth = invWidth;
            Kshared = kernCompute(kern, x);
            maxD = max(cell2mat(dims));
            Yshared = zeros(n, maxD);
            for d=1:maxD
                Yshared(:,d) = real(gsamp(zeros(1, size(x, 1)), Kshared, 1))';
            end

            %-- Build the datasets by adding the shared with the private signals
            for i=1:numberOfDatasets
                Ytoy{i} = zeros(n, dims{i});
                krn = kernels{i};
                for d=1:dims{i}
                    kern = kernCreate(x, krn);
                    kern.inverseWidth = invWidths{i};
                    K = kernCompute(kern,x);
                    Ytoy{i}(:,d) = real(gsamp(zeros(1, size(x, 1)), K, 1))';
                end
                % Add the shared signal
                Ytoy{i} = Ytoy{i} + Yshared(:,1:dims{i});
            end

            %-- We can add one extra dataset: the shared one
            Ytoy{i+1} = Yshared;
            numberOfDatasets = numberOfDatasets + 1;
            dataSetNames = {dataSetNames{:}, 'shared'};

            clear('n','K','x','kernels','krn','kern','d');
        otherwise
            %...
    end
    if ~exist('numberOfDatasets')
        numberOfDatasets = length(Ytoy);
    end
    %     for i=1:numberOfDatasets
    %         figure
    %         plot(Ytoy{i})
    %     end
    Yall = Ytoy;
    clear('Ytoy');
else
    toyData = 0;
    if isstruct(dataSetNames)
        for i=1:length(dataSetNames)
            Y{i} = svargplvmLoadData(dataSetNames{i});
        end
        numberOfDatasets = length(dataSetNames);
    else
        [Yall, lbls] = svargplvmLoadData(dataSetNames);
        if ~isempty(lbls)
            height = lbls(1); width = lbls(2);
        end
        numberOfDatasets = length(Yall);
    end
end
%--

if isempty(dataType)
    if length(dataSetNames) ==1
        dataType = dataSetNames{1};
    end
end


function [m,n] = size(a,dim)
%SIZE         Implements  size(a)  for slope
%
%   [m,n] = size(a,dim)
%
% functionality as Matlab function size, second argument dim optional
%

% written  12/06/98     S.M. Rump
% modified 09/28/01     S.M. Rump  matrices and multi-dimensional arrays
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
%

  if nargout==2
    [m,n] = size(zeros(a.size));
  elseif nargin==1
    m = a.size;
  else
    m = size(zeros(a.size),dim);
  end


% Plots of temperature, pressure and density with altitude increments.
sldata;
chart;
A=chart;

A(:,2)=A(:,2)*T1;
A(:,3)=A(:,3)*p1;
A(:,4)=A(:,4)*rho1;

% Absolute variables are separated in different vectors.

h=A(:,1);
T=A(:,2);
p=A(:,3);
rho=A(:,4);

plott;
figure;
plotp;
figure;
plotrho;


function drawAcrobot(t,z,p)

clf; hold on;

length = p.l1+p.l2;
axis equal; axis(length*[-1,1,-1,1]); axis off;

[p1,p2] = acrobotKinematics(z,p);
pos = [[0;0],p1,p2];

plot(0,0,'ks','MarkerSize',25,'LineWidth',4)
plot(pos(1,:),pos(2,:),'Color',[0.1, 0.8, 0.1],'LineWidth',4)
plot(pos(1,:),pos(2,:),'k.','MarkerSize',50)

title(sprintf('Acrobot Animation,  t = %6.4f', t));

drawnow; pause(0.001);

end


function S = modexpon(S,p)
% Reduces the exponents of a sympoly modulo p
% Usage: S = modexpon(S,p)
%
% S is any sympoly
% p a scalar numeric value
%

if numel(S) > 1
  for i = 1:numel(S)
    S(i) = modexpon(S(i),p);
  end
else
  % a scalar sympoly
  S.Exponent = mod(S.Exponent,p);

  S = clean_sympoly(S);
end


function [xY,word,NI] = spm_voice_get_xY(PATH)
% Create word arrays from sound file exemplars
% FORMAT [xY,word,NI] = spm_voice_get_xY(PATH)
%
% PATH      -  directory containing sound files of exemplar words
%
% xY(nw,ns) -  structure array for ns samples of nw words
% word(nw)  -  cell array of word names
% NI(nw,ns) -  numeric array of number of minima
%
%  This routine uses a library of sound files, each containing 32 words
%  spoken with varying prosody. The name of the sound file labels the word
%  in question. These exemplars are then transformed (using a series of
%  discrete cosine and Hilbert transforms) into a set of parameters, which
%  summarise the lexical content and prosody. The inverse transform
%  generates  timeseries that can be played to articulate a word. The
%  transform operates on a word structure xY to create lexical and prosody
%  parameters (Q and P respectively).
%__________________________________________________________________________
% Copyright (C) 2019 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_voice_get_xY.m 7766 2020-01-05 21:37:39Z karl $

% get corpus
%==========================================================================
rng('default')

% get the list of words and sampling frequency (FS)
%--------------------------------------------------------------------------
spm_figure('GetWin','voice'); clf
cd(PATH)
wfile    = dir('*.wav');
try
    audioread(wfile(1).name,[1,1]);
    read = @audioread;
catch
    read = @wavread;
end
[Y,FS]   = read(wfile(1).name,[1,1]);

% assemble cell array of word structures for subsequent characterisation
%==========================================================================
nw    = numel(wfile);                               % number of words
ns    = 32;                                         % number of samples
nj    = 16;                                         % number of jitters
sj    = FS/64;                                      % s.d. of jitter
for w = 1:nw

    % get lexicon name and create structure
    %----------------------------------------------------------------------
    wname   = wfile(w).name;                        % name of word
    [d,str] = fileparts(wname);
    word{w} = str; disp(str)

    % get the midpoint of words from the (maxima) of successive exemplars
    %----------------------------------------------------------------------
    G     = spm_voice_check(read(wname),FS,1/4);
    I     = find((diff(G(1:end - 1)) > 0) & (diff(G(2:end)) < 0));
    [i,j] = sort(G(I),'descend');
    I     = sort(I(j(1:ns)));

    % and plot
    %----------------------------------------------------------------------
    subplot(2,1,1), plot(G), hold on, plot(I,G(I),'ro'), hold off
    title(sprintf('Power peaks - %s',word{w}),'FontSize',16)
    xlabel('frequency (hertz)'), ylabel('power'), box off
    drawnow

    for s = 1:ns

        % retrieve (one second) epoch around midpoint and transform
        %------------------------------------------------------------------
        i     = round([-1/2 1/2]*FS + I(s));
        if i(1) < 1
            i = i - i(1) + 1;
        end
        Y     = read(wname,i);
        i     = spm_voice_onsets(Y,FS);
        i     = i{end};
        ni    = numel(Y);
        for j = 1:nj

            % jitter interval
            %--------------------------------------------------------------
            k       = (s - 1)*nj + j;
            ik      = fix((i(1) + sj*randn):(i(end) + sj*randn));
            ik      = ik(ik < ni & ik > 1);
            xY(w,k) = spm_voice_ff(Y(ik),FS);

            % record interval
            %--------------------------------------------------------------
            xY(w,k).i(1) = ik(1)/FS   - 1/2;
            xY(w,k).i(2) = ik(end)/FS - 1/2;
        end

        % apply inverse transform and play, if requested
        %------------------------------------------------------------------
        spm_voice_iff(xY(w,k));

        % record number of spectral minima intervals
        %------------------------------------------------------------------
        [Y,IS]  = spm_voice_get_next(Y);
        j       = fix((0:FS + FS/4) + IS);
        j       = j(j < ni & j > 1);
        NI(w,s) = numel(spm_voice_onsets(Y(j),FS));

    end

end


% The contact lib by Mathworks, may change to another lib written by
% Professor Hartmut Geyer
addpath(genpath(fullfile(pwd,'contat_lib')));
%parameters for ground plane geometry and contract
ground.stiff = 5e4;
ground.damp = 1000;
ground.height = 0.4;

%robot body size
body.x_length = 0.6;
body.y_length = 0.3;
body.z_length = 0.15;
body.shoulder_size = 0.07;
body.upper_length = 0.16;
body.lower_length = 0.25;
body.foot_radius = 0.035;
body.shoulder_distance = 0.2;
body.max_stretch = body.upper_length + body.lower_length;
body.knee_damping = 0.1;

% parameters for leg control
ctrl.pos_kp = 30;
ctrl.pos_ki = 0;
ctrl.pos_kd = 0;
ctrl.vel_kp = 4.4;
ctrl.vel_ki = 0;
ctrl.vel_kd = 0.0;

% parameters for high level plan
planner.touch_down_height = body.foot_radius; % from foot center to ground

planner.stand_s = 0;
planner.stand_u = 30*pi/180;
planner.stand_k = -60*pi/180;
stance_pos = forward_kinematics(planner.stand_s, planner.stand_u, planner.stand_k, body);
planner.stand_height = stance_pos(3);

planner.flight_height = 1.1*planner.stand_height;% when leg is flying in the air

% gait control parameters
planner.time_circle = 3;
planner.swing_ang = 12/180*pi;
planner.init_shake_ang = 3/180*pi;

%some gait control goals
planner.tgt_body_ang = 0;
planner.tgt_body_vx = 0.3;  % tested 0.15-0.45
planner.Ts = 0.1; % for x_direction leg place
planner.Kv = 0.3;  % for x_dreiction leg place

planner.y_Ts = 0.21; % for y_direction leg place
planner.y_Kv = -0.34;  % for y_direction leg place

% state transition thredsholds
planner.state0_vel_thres = 0.05;
% the sample time of the ctrl block is set to be 0.0025 (400Hz);
planner.state0_trans_thres = 300; %(0.75 seconds)
planner.state0_swing_ang  = 10*pi/180;
planner.state0_swing_T = 1200;
planner.state12_trans_speed = 0.2;
planner.leg_swing_time = 70; %(use 0.25s to generate a motion)

%robot weight
body_weight = 600*body.x_length*body.y_length*body.z_length;
leg_density = 660;
should_weight = leg_density*0.07^3;
upperleg_weight = leg_density*0.04*0.04*body.upper_length;
lowerleg_weight = leg_density*0.04*0.04*body.lower_length;
foot_weight = 1000*4/3*pi*body.foot_radius^3;

total_weight = body_weight + 4*(should_weight+upperleg_weight+lowerleg_weight+foot_weight);

body_inertia = diag([0.151875;0.516375;0.6075]);


function [st3, rez] = standalone_detector(rez, spkTh)
% Detects spikes across the entire recording using generic templates.
% Each generic template has rank one (in space-time).
% In time, we use the 1D template prototypes found in wTEMP.
% In space, we use Gaussian weights of several sizes, centered
% on (x,y) positions that are part of a super-resolution grid covering the
% entire probe (pre-specified in the calling function).
% In total, there ~100x more generic templates than channels.

NrankPC = size(rez.wPCA, 2);
ops = rez.ops;

% minimum/base sigma for the Gaussian.
sig = 10;

% grid of centers for the generic tempates
[ycup, xcup] = meshgrid(ops.yup, ops.xup);

% determine prototypical timecourses by clustering of simple threshold crossings.
wTEMP = gpuArray(rez.wTEMP);
wPCA = gpuArray(rez.wPCA);

% Get nearest channels for every template center.
% Template products will only be computed on these channels.
NchanNear = 10;
[iC, dist] = getClosestChannels2(ycup, xcup, rez.yc, rez.xc, NchanNear);

% Templates with centers that are far from an active site are discarded
dNearActiveSite = median(diff(unique(rez.yc)));
igood = dist(1,:)<dNearActiveSite;
iC = iC(:, igood);
dist = dist(:, igood);
ycup = ycup(igood);
xcup = xcup(igood);

% number of nearby templates to compare for local template maximum
NchanNearUp =  min(numel(xcup), 10*NchanNear);
[iC2, dist2] = getClosestChannels2(ycup, xcup, ycup, xcup, NchanNearUp);

% pregenerate the Gaussian weights used for spatial components
nsizes = 5;
v2 = gpuArray.zeros(nsizes, size(dist,2), 'single');
for k = 1:nsizes
    v2(k, :) = sum(exp( - 2 * dist.^2 / (sig * k)^2), 1);
end

% build up Params
NchanUp = size(iC,2);

% preallocate the results
st3 = zeros(1000000, 6);
t0 = 0; %ceil(rez.ops.trange(1) * ops.fs); % I think this should be 0 all the time.
nsp = 0; % counter for total number of spikes
%%
tic
for k = 1:ops.Nbatch
    % get a batch of whitened and filtered data
    dataRAW = get_batch(rez.ops, k);

    Params = [size(dataRAW,1) ops.Nchan ops.nt0 NchanNear NrankPC ops.nt0min spkTh NchanUp NchanNearUp sig];

    % run the CUDA function on this batch
    [dat, kkmax, st, cF] = spikedetector3(Params, dataRAW, wTEMP, iC-1, dist, v2, iC2-1, dist2);

    % upsample the y position using the center of mass of template products
    % coming out of the CUDA function.
    ys = rez.yc(iC);
    cF0 = max(0, cF);
    cF0 = cF0 ./ sum(cF0, 1);
    iChan = st(2, :) + 1;
    yct = sum(cF0 .* ys(:, iChan), 1);

    % build st for the current batch
    st = double(gather(st));
    st(6, :) = st(2,:);
    st(2,:) = gather(yct);

    toff = ops.nt0min + t0 + ops.NT*(k-1);
    st(1,:) = st(1,:) + toff; % these offsets ensure the times are computed correctly

    st(5,:) = k; % batch number

    nsp0 = size(st,2);
    if nsp0 + nsp > size(st3,1)
       st3(nsp + 1e6, 1) = 0; % if we need to preallocate more space
    end

    st3(nsp + [1:nsp0], :) = st';
    nsp = nsp + nsp0;

    if rem(k,100)==1 || k==ops.Nbatch
        fprintf('%2.2f sec, %d batches, %d spikes \n', toc, k, nsp)
    end
end

st3 = st3(1:nsp, :);
%%

rez.iC = gather(iC);
rez.dist =  gather(dist);


function ori = exp(m,ori_ref,varargin)
% misorientation vector to misorientation
%
% Description
%
% Syntax
%
%   mori = exp(m)        % misorientation
%
%   ori = exp(m,ori_ref) % orientation update
%
% Input
%  m - @Miller misorientation vector in crystal coordinates
%  ori_ref - @orientation
%
% Output
%  ori - @orientation
%
% See also
% Miller/exp orientation/log

rot = exp@vector3d(m);

ori = orientation(rot,m.CS,m.CS);

if nargin >= 2
  if check_option(varargin,'left')
    ori =  ori * ori_ref;
  else
    ori =  ori_ref .* ori;
  end
end


function [ sepScene, wallPanoNormal] = compSurfaceLabel( rotImg )%compSurfaceLabel( data, folderName )
%COMPSURFACELABEL Compute geometric context surface label for panorama
%   sepScene: surface label for each seprate view, 6 channels
%   wallPanoNormal: surface label for panorama, 7 channels

% [rotImg, data, R] = loadBuffer( data, folderName );
%% emprical good separate of panorama
cutSize = 640;
% fov = pi*2/3;
fov = [pi/2*ones(1,8) pi/2*ones(1,8)];
xh = [-pi:(pi/4):(3/4*pi) -pi:(pi/4):(3/4*pi)];
yh = [zeros(1, 8) -1/6*pi*ones(1,8)];

% fov = [pi/2*ones(1,36) pi/2*ones(1,36)];
% xh = [-pi:(pi/18):(17/18*pi) -pi:(pi/18):(17/18*pi)];
% yh = [zeros(1,36) -1/6*pi*ones(1,36)];

% fov = [pi/2*ones(1,8)];
% xh = [-pi:(pi/4):(3/4*pi)];
% yh = [zeros(1,8)];

% fov = [pi/2*ones(1,36)];
% xh = [-pi:(pi/18):(17/18*pi)];
% yh = [zeros(1,36)];
[sepScene] = separatePano( rotImg, fov, xh, yh, cutSize);

%%
% imdir = './roomModel/SpatialLayout_shrink/Images_resized/';
% workspcdir = './roomModel/SpatialLayout_shrink/tempworkspace/';
% imsegdir = './roomModel/SpatialLayout_shrink/Imsegs/';
% moveFolder = './roomModel/SpatialLayout_shrink/';
sepScene(1).cimages = [];
sepScene(1).maxCimage = [];
numScene = length(sepScene);
for sid = 1:numScene
    img = sepScene(sid).img;
    img = imresize(img, [640 640]);
    try
        [ cimages ] = getSurfaceLabelInitial( img );
    catch
        cimages{1} = 1/7*ones(cutSize,cutSize,7);
    end
%     [cimages, Polyg] = getSurfaceLabelLayout( sepScene(sid), data  )
    sepScene(sid).cimages = cimages;
    [~, indd] = max(cimages{1}, [], 3);
    sepScene(sid).maxCimage = indd;
end

%% combine
% 1 middle wall=yellow; 2 left wall=red; 3 right wall=cyan; 4 floor=green; 5 ceiling=blue; 6 objects=pink
width = 2048;   height = 1024;
wallPano = zeros(height, width, 7);
wallPanoWeight = zeros(height, width);
for vid = 1:length(sepScene)
    vx = sepScene(vid).vx;

    [curPano, curPanoValid] = ...
    im2Sphere( sepScene(vid).cimages{1}, sepScene(vid).fov, width, height, ...
                sepScene(vid).vx, sepScene(vid).vy);
    curPano(~curPanoValid) = 0;
    wallPanoWeight(curPanoValid(:,:,1)) = wallPanoWeight(curPanoValid(:,:,1)) + 1;

    normMiddle = vx+pi;
    normMLR = normValue([normMiddle normMiddle-pi/2 normMiddle+pi/2], -pi, pi);
    normGlobal = [-pi -pi/2 0 pi/2];
    for i = 1:3
        angle = abs(normValue(normGlobal - normMLR(i), -pi, pi));
        angle(angle>pi/2) = pi/2; % cos^2 weight
%         [~,I] = sort(angle, 'ascend'); % nearest neighbor
%         angle(I(1)) = 0;
%         angle(I(2:4)) = pi/2;
        for j = find(angle<pi/2)
            wallPano(:,:,j) = wallPano(:,:,j) + curPano(:,:,i)*(cos(angle(j)).^2);
        end
    end

    wallPano(:,:,5) = wallPano(:,:,5) + curPano(:,:,4); % floor
    wallPano(:,:,6) = wallPano(:,:,6) + curPano(:,:,5); % ceiling
    wallPano(:,:,7) = wallPano(:,:,7) + curPano(:,:,6); % object
end
wallPanoNormal = wallPano./repmat(wallPanoWeight+0.000001,[1 1 7]);

%% more informative combine
% % Left wall: view+(90-FOV/2) ~ view+(90-FOV/2)+180
% % Right wall: view-(90-FOV/2)-180 ~ view-(90-FOV/2)
% % Middle wall: Left wall + Right wall
% width = 1024;   height = 512;
% wallPano = zeros(512, 1024, 4);
% for vid = 1:length(sepScene)
%     vx = sepScene(vid).vx;
%     fov = sepScene(vid).fov;
%     [curPano, curPanoValid] = ...
%         im2Sphere( sepScene(vid).cimages{1}, sepScene(vid).fov, width, height, ...
%                     sepScene(vid).vx, sepScene(vid).vy);
%     curPano(~curPanoValid) = 0;
%     %Left
%     lowBound = vx+(pi/2-fov/2);
%     uppBound = vx+(pi/2-fov/2)+pi;
%     range = normRange(lowBound, uppBound, -pi, pi);
%     Linside = insideRange([-pi -pi/2 0 pi/2], range);
% %     Lweight = [1 1 1 1];
%     %Right
%     lowBound = vx-(pi/2-fov/2)-pi;
%     uppBound = vx-(pi/2-fov/2);
%     range = normRange(lowBound, uppBound, -pi, pi);
%     Rinside = insideRange([-pi -pi/2 0 pi/2], range);
% %     Rweight = [1 1 1 1];
%     %Middle
%     lowBound = vx+(pi/2-fov/2);
%     uppBound = vx-(pi/2-fov/2);
%     range = normRange(lowBound, uppBound, -pi, pi);
%     Minside = insideRange([-pi -pi/2 0 pi/2], range);
% %     Mweight = [1 1 1 1];
%     %Vote Left
%     score = curPano(:,:,2);
%     voteMatrix = repmat(reshape(Linside, [1 1 4]), [512 1024 1]);
%     voteMatrix = voteMatrix .* repmat(score, [1 1 4]);
%     wallPano = wallPano + voteMatrix;
%
%     %Vote Right
%     score = curPano(:,:,3);
%     voteMatrix = repmat(reshape(Linside, [1 1 4]), [512 1024 1]);
%     voteMatrix = voteMatrix .* repmat(score, [1 1 4]);
%     wallPano = wallPano + voteMatrix;
%
%     %Vote Middle
%     score = curPano(:,:,1);
%     voteMatrix = repmat(reshape(Linside, [1 1 4]), [512 1024 1]);
%     voteMatrix = voteMatrix .* repmat(score, [1 1 4]);
%     wallPano = wallPano + voteMatrix;
%
% end

%% heuristic combine
% MASK = zeros(640,640);
% MASK(213:427,1:320) = 1;
% MASK(213:427,321:640) = 2;
% All_MLR2ABCD = zeros(8,3);
% for vid = [2 4 6 8]
%     histLabelL = hist(sepScene(vid).maxCimage(MASK<1.5 & MASK>0.5), 1:3);
%     histLabelR = hist(sepScene(vid).maxCimage(MASK>1.5 & MASK<2.5), 1:3);
%     histLabelL = histLabelL./sum(histLabelL) + 0.0001;
%     histLabelR = histLabelR./sum(histLabelR) + 0.0001;
%     entropyL = -sum(histLabelL.*log2(histLabelL));
%     entropyR = -sum(histLabelR.*log2(histLabelR));
%     MLR2ABCD = [0 0 0]; %[M L R], 1234 for ABCD, 0 for Null
%     histLabel = hist(sepScene(vid).maxCimage(MASK<2.5 & MASK>0.5), 1:3);
%     [~, IW] = sort(histLabel, 'descend');
%     if entropyL<entropyR
%         [~, I] = max(histLabelL);
%         MLR2ABCD(I) = vid/2;
%         if ~ismember(I, IW(1:2))
%             fprintf('Probably some problems\n');
%         end
%         if I==1
%             MLR2ABCD(3) = rem(vid/2,4)+1;
%         else
%             MLR2ABCD(setdiff(IW(1:2),I)) = rem(vid/2,4)+1;
%         end
%     else
%         [~, I] = max(histLabelR);
%         MLR2ABCD(I) = rem(vid/2,4)+1;
%         if ~ismember(I, IW(1:2))
%             fprintf('Probably some problems\n');
%         end
%         if I==1
%             MLR2ABCD(2) = vid/2;
%         else
%             MLR2ABCD(setdiff(IW(1:2),I)) = vid/2;
%         end
%     end
%     All_MLR2ABCD(vid,:) = MLR2ABCD;
% end
% width = 1024;   height = 512;
% for vid = [1 3 5 7]
%     preID = vid - 1; preID(preID==0) = 8;
%     latID = vid + 1;
%
%     curMaxCimage = sepScene(vid).maxCimage;
%     curMaxCimage(MASK<0.5) = 0;
%     [curPano, curPanoValid] = ...
%         im2Sphere( curMaxCimage, sepScene(vid).fov, width, height, ...
%                     sepScene(vid).vx, sepScene(vid).vy);
%     curPano(~curPanoValid) = 0;
%
%     preMaxCimage = sepScene(preID).maxCimage;
%     preMaxCimage(MASK<0.5) = 0;
%     [prePano, prePanoValid] = ...
%         im2Sphere( preMaxCimage, sepScene(preID).fov, width, height, ...
%                     sepScene(preID).vx, sepScene(preID).vy);
%     prePano(~prePanoValid) = 0;
%
%     latMaxCimage = sepScene(latID).maxCimage;
%     latMaxCimage(MASK<0.5) = 0;
%     [latPano, latPanoValid] = ...
%         im2Sphere( latMaxCimage, sepScene(latID).fov, width, height, ...
%                     sepScene(latID).vx, sepScene(latID).vy);
%     latPano(~latPanoValid) = 0;
%
%     preConsist = [0 0 0];
%     preNumber = [0 0 0];
%     for i = 1:3
%         mask = curPano>i-0.5 & curPano<i+0.5 & prePano>0.5 & prePano<3.5;
%         h = hist(prePano(mask),1:3);
%         [B,I] = max(h);
%         J = All_MLR2ABCD(preID,I);
%         preConsist(i) = J;
%         preNumber(i) = B;
%
%         mask = curPano>i-0.5 & curPano<i+0.5;
%         if sum(mask(:))<250 || B/sum(mask(:))<0.05
%             preConsist(i) = 0;
%         end
%     end
%     latConsist = [0 0 0];
%     latNumber = [0 0 0];
%     for i = 1:3
%         mask = curPano>i-0.5 & curPano<i+0.5 & latPano>0.5 & latPano<3.5;
%         h = hist(latPano(mask),1:3);
%         [B,I] = max(h);
%         J = All_MLR2ABCD(latID,I);
%         latConsist(i) = J;
%         latNumber(i) = B;
%
%         mask = curPano>i-0.5 & curPano<i+0.5;
%         if sum(mask(:))<250 || B/sum(mask(:))<0.05
%             latConsist(i) = 0;
%         end
%     end
%     for i = 1:3
%         if preNumber(i)>latNumber(i)
%             All_MLR2ABCD(vid, i) = preConsist(i);
%         else
%             All_MLR2ABCD(vid, i) = latConsist(i);
%         end
%     end
% end
% wallPano = zeros(512, 1024, 4);
% for vid = [2 4 6 8]
%     [curPano, curPanoValid] = ...
%     im2Sphere( sepScene(vid).cimages{1}, sepScene(vid).fov, width, height, ...
%                 sepScene(vid).vx, sepScene(vid).vy);
%     curPano(~curPanoValid) = 0;
%     voteMat = zeros(512, 1024, 4);
%     for i = find(All_MLR2ABCD(vid,:)~=0)
%         voteMat(:,:,All_MLR2ABCD(vid,i)) = curPano(:,:,i);
%     end
%     wallPano = wallPano + voteMat;
% end

end

function [nv] = normValue( v, minBound, maxBound)
nv = rem((v-minBound)+2*(maxBound-minBound), maxBound-minBound) + minBound;
end

function [range] = normRange( low, upp, minBound, maxBound)
normLow = rem((low-minBound)+2*(maxBound-minBound), maxBound-minBound) + minBound;
normUpp = rem((upp-minBound)+2*(maxBound-minBound), maxBound-minBound) + minBound;
if normLow<=normUpp
    range = [normLow normUpp];
else
    range = [normLow maxBound; minBound normUpp];
end
end

function [inside] = insideRange(values, range)
inside = false(size(values));
for i = 1:size(range,1)
    inside = inside | (values>=range(i,1) & values<=range(i,2));
end
end