function lik = lik_coxph(varargin)
%LIK_COXPH    Create a Cox proportional hazard likelihood structure
%
%  Description
%    LIK = LIK_COXPH('PARAM1',VALUE1,'PARAM2,VALUE2,...)
%    creates a proportional hazard model where a piecewise log-constant
%    baseline hazard is assumed.
%
%    The likelihood contribution for the ith observation is
%
%      l_i = h_i(y_i)^(1-z_i)*[exp(-int_0^y_i*h_i dt)],
%
%    where hazard is h_i=h_0(y_i)*exp(f_i). A zero mean Gaussian process
%    prior is placed for f = [f_1, f_2,...,f_n] ~ N(0, C). C is the
%    covariance matrix, whose elements are given as C_ij = c(x_i, x_j |
%    th). The function c(x_i, x_j| th) is covariance function and th its
%    parameters, hyperparameters. We place a hyperprior for
%    hyperparameters, p(th).
%
%    The time axis is partioned into K intervals with equal lengths:
%    0 = s_0 < s_1 < ... < s_K, where s_K > y_i for all i. The baseline
%    hazard rate function h_0 is piecewise constant,
%
%      h_0(t) = la_k,
%
%    when t belongs to the interval (s_{k-1},s_k] and where ft_k=log(la_k).
%    The hazard rate function is smoothed by assuming another Gaussian
%    process prior ft = [ft_1, ft_2,...,ft_K] ~ N(0, C).
%
%    z is a vector of censoring indicators with z = 0 for uncensored event
%    and z = 1 for right censored event.
%
%    When using the Coxph likelihood you need to give the vector z
%    as an extra parameter to each function that requires also y.
%    For example, you should call gpla_e as follows: gpla_e(w, gp,
%    x, y, 'z', z)
%
%  See also
%    GP_SET, LIK_*, PRIOR_*
%

% Copyright (c) 2007-2010 Jarno Vanhatalo & Jouni Hartikainen
% Copyright (c) 2010 Aki Vehtari
% Copyright (c) 2011 Jaakko Riihimäki

% 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_COXPH';
  ip.addOptional('lik', [], @isstruct);
  ip.addParamValue('S', linspace(0,1.001,50), @(x) isvector(x));
  ip.addParamValue('stratificationVariables', [], @(x) isvector(x) && all(rem(x,1)==0));
  ip.addParamValue('removeStratificationVariables', [], @(x) ismember(x, {'on' 'off'}));
  ip.parse(varargin{:});
  lik=ip.Results.lik;

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

  if init || ~ismember('S', ip.UsingDefaults)
    s=ip.Results.S;
    xtmp=zeros(length(s)-1,1);
    for i2=1:(length(s)-1)
      xtmp(i2,1)=mean([s(i2) s(i2+1)]);
    end
    lik.xtime=xtmp;
    lik.stime=s;
  end

  if ~ismember('stratificationVariables', ip.UsingDefaults)
    lik.stratificationVariables = ip.Results.stratificationVariables;
    if ~ismember('stratificationVariables', ip.UsingDefaults)
      if isequal(ip.Results.removeStratificationVariables, 'on')
        lik.removeStratificationVariables=true;
      end
    end
  end

  if init
    % Set the function handles to the nested functions
    lik.fh.pak = @lik_coxph_pak;
    lik.fh.unpak = @lik_coxph_unpak;
    lik.fh.ll = @lik_coxph_ll;
    lik.fh.llg = @lik_coxph_llg;
    lik.fh.llg2 = @lik_coxph_llg2;
    lik.fh.llg3 = @lik_coxph_llg3;
    lik.fh.tiltedMoments = @lik_coxph_tiltedMoments;
    lik.fh.predy = @lik_coxph_predy;
    lik.fh.invlink = @lik_coxph_invlink;
    lik.fh.recappend = @lik_coxph_recappend;
    lik.fh.predcdf= @lik_coxph_predcdf;
  end

  function [w,s] = lik_coxph_pak(lik)
  %LIK_COXPH_PAK  Combine likelihood parameters into one vector.
  %
  %  Description
  %    W = LIK_COXPH_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.disper)
  %
  %   See also
  %   LIK_COXPH_UNPAK, GP_PAK

    w=[];s={};
  end

  function [lik, w] = lik_coxph_unpak(lik, w)
  %LIK_COXPH_UNPAK  Extract likelihood parameters from the vector.
  %
  %  Description
  %    [LIK, W] = LIK_COXPH_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.disper)
  %
  %   See also
  %   LIK_COXPH_PAK, GP_UNPAK

    lik=lik;
    w=w;

  end

  function ll = lik_coxph_ll(lik, y, f, z)
  %LIK_COXPH_LL  Log likelihood
  %
  %  Description
  %    LL = LIK_COXPH_LL(LIK, Y, F, Z) takes a likelihood
  %    structure LIK, incedence counts Y, expected counts Z, 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_COXPH_LLG, LIK_COXPH_LLG3, LIK_COXPH_LLG2, GPLA_E

    if isempty(z)
      error(['lik_coxph -> lik_coxph_ll: missing z!    '...
             'Coxph likelihood needs the expected number of    '...
             'occurrences as an extra input z. See, for         '...
             'example, lik_coxph and gpla_e.               ']);
    end

    [n,ny]=size(y);
    ntime=size(lik.xtime,1);
    i3v=ones(size(y,1),1);
    if isfield(lik, 'stratificationVariables')
      f1=f(1:ntime*lik.n_u);
      f2=f((ntime*lik.n_u+1):(ntime*lik.n_u+n));
      for i=1:lik.n_u
        i3v(lik.stratind{i})=i;
      end
    else
      f1=f(1:ntime);
      f2=f((ntime+1):(ntime+n));
    end

    la1=exp(f1);
    eta2=exp(f2);

    nu=1-z;
    sd=lik.stime(2)-lik.stime(1);

    if ny==1
      ll=0;
      for i1=1:n
        i3=i3v(i1);
        nft=(i3-1)*ntime;
        si=sum(y(i1)>lik.stime);
        ll=ll + nu(i1).*(f1(si+nft)+f2(i1)) - (y(i1)-lik.stime(si)).*la1(si+nft).*eta2(i1) ...
              - sum(sd.*la1((1+nft):(si-1+nft)).*eta2(i1));
      end
    else

      ll=0;
      sb=sum(bsxfun(@gt,y(:,1),lik.stime),2);
      se=sum(bsxfun(@gt,y(:,2),lik.stime),2);
      for i1=1:n
        i3=i3v(i1);
        nft=(i3-1)*ntime;
        if sb(i1)==0
          ll=ll + nu(i1).*(f1(se(i1)+nft)+f2(i1)) - (y(i1,2)-lik.stime(se(i1))).*la1(se(i1)+nft).*eta2(i1) ...
                - sum(sd.*la1((1+nft):(se(i1)-1+nft)).*eta2(i1));
        else

          if se(i1)==sb(i1)
            ll=ll + nu(i1).*(f1(se(i1)+nft)+f2(i1)) - (y(i1,2)-y(i1,1)).*la1(se(i1)+nft).*eta2(i1);
          else
            ll=ll + nu(i1).*(f1(se(i1)+nft)+f2(i1)) - (y(i1,2)-lik.stime(se(i1))).*la1(se(i1)+nft).*eta2(i1) ...
                - sum(sd.*la1((sb(i1)+1+nft):(se(i1)-1+nft)).*eta2(i1)) - (lik.stime(sb(i1)+1)-y(i1,1)).*la1(sb(i1)+nft).*eta2(i1);
          end
        end
      end
    end
  end

  function llg = lik_coxph_llg(lik, y, f, param, z)
  %LIK_COXPH_LLG  Gradient of the log likelihood
  %
  %  Description
  %    LLG = LIK_COXPH_LLG(LIK, Y, F, PARAM) takes a likelihood
  %    structure LIK, incedence counts Y, expected counts Z and
  %    latent values F. Returns the gradient of the 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_COXPH_LL, LIK_COXPH_LLG2, LIK_COXPH_LLG3, GPLA_E

    if isempty(z)
      error(['lik_coxph -> lik_coxph_llg: missing z!    '...
             'Coxph likelihood needs the expected number of    '...
             'occurrences as an extra input z. See, for         '...
             'example, lik_coxph and gpla_e.               ']);
    end

    ntime=size(lik.xtime,1);

    [n,ny]=size(y);
    i3v=ones(size(y));
    if isfield(lik, 'stratificationVariables')
      f1=f(1:ntime*lik.n_u);
      f2=f((ntime*lik.n_u+1):(ntime*lik.n_u+n));
      llg=zeros(ntime*lik.n_u+n,1);
      for i=1:lik.n_u
        i3v(lik.stratind{i})=i;
      end
      nf1=lik.n_u*ntime;
    else
      f1=f(1:ntime);
      f2=f((ntime+1):(ntime+n));
      llg=zeros(ntime+n,1);
      nf1=ntime;
    end

    la1=exp(f1(:));
    eta2=exp(f2(:));

    nu=1-z;
    sd=lik.stime(2)-lik.stime(1);

    switch param
      case 'latent'

      if ny==1
        if ~isfield(lik, 'stratificationVariables')
          for i1=1:ntime
            ind=y>=lik.stime(i1) & y<lik.stime(i1+1);
            llg(i1)= sum(nu(ind) - (y(ind)-lik.stime(i1)).*la1(i1).*eta2(ind)) - sum(sum(sd.*la1(i1)).*eta2(~ind & y>=lik.stime(i1+1)));
          end
        else
          for j=1:lik.n_u
            for i1=1:ntime
              ind=y>=lik.stime(i1) & y<lik.stime(i1+1);
              ind2=lik.stratind{j};
              llg(i1+(j-1)*ntime)= sum(nu(ind & ind2) - (y(ind & ind2)-lik.stime(i1)).*la1(i1+(j-1)*ntime).*eta2(ind & ind2)) - sum(sum(sd.*la1(i1+(j-1)*ntime)).*eta2((~ind & ind2) & y>=lik.stime(i1+1)));
            end
          end
        end
        for i1=1:n
          i3=i3v(i1);
          nft=(i3-1)*ntime;
          si=sum(y(i1)>lik.stime);
          llg(i1+nf1)= nu(i1) - (y(i1)-lik.stime(si)).*la1(si+nft).*eta2(i1) - sum(sd.*la1((1+nft):(si-1+nft)).*eta2(i1));
        end
      else
        for i1=1:ntime

          % left truncated + follow-up entry: (1)
          ind_vkst = y(:,1)>=lik.stime(i1) & y(:,1)<lik.stime(i1+1) & y(:,2)>=lik.stime(i1+1);
          % follow-up entry + follow-up exit: (2)
          ind_stsp = y(:,1)>=lik.stime(i1) & y(:,1)<lik.stime(i1+1) & y(:,2)>=lik.stime(i1) & y(:,2)<lik.stime(i1+1);
          % follow-up: (3)
          ind_s = y(:,1)<lik.stime(i1) & y(:,2)>=lik.stime(i1+1);
          % follow-up exit: (4)
          ind_sp = y(:,1)<lik.stime(i1) & y(:,2)>=lik.stime(i1) & y(:,2)<lik.stime(i1+1);

          if ~isfield(lik, 'stratificationVariables')
            % (1)
            s2b=sum(-(lik.stime(i1+1)-y(ind_vkst,1)).*la1(i1).*eta2(ind_vkst));
            % (2)
            s3b=sum(nu(ind_stsp)) - sum((y(ind_stsp,2)-y(ind_stsp,1)).*la1(i1).*eta2(ind_stsp));
            % (3)
            s4= - sum(sd.*la1(i1).*eta2(ind_s));
            % (4)
            s5=sum(nu(ind_sp)) - sum((y(ind_sp,2)-lik.stime(i1)).*la1(i1).*eta2(ind_sp));

            llg(i1) = s2b+s3b+s4+s5;
          else
            for j=1:lik.n_u
              ind2 = lik.stratind{j};
              nft=(j-1)*ntime;
              % (1)
              s2b=sum(-(lik.stime(i1+1)-y(ind_vkst & ind2,1)).*la1(i1+nft).*eta2(ind_vkst & ind2));
              % (2)
              s3b=sum(nu(ind_stsp & ind2)) - sum((y(ind_stsp & ind2,2)-y(ind_stsp & ind2,1)).*la1(i1+nft).*eta2(ind_stsp & ind2));
              % (3)
              s4= - sum(sd.*la1(i1+nft).*eta2(ind_s & ind2));
              % (4)
              s5=sum(nu(ind_sp & ind2)) - sum((y(ind_sp & ind2,2)-lik.stime(i1)).*la1(i1+nft).*eta2(ind_sp & ind2));

              llg(i1+nft) = s2b+s3b+s4+s5;
            end
          end

        end

        sb=sum(bsxfun(@gt,y(:,1),lik.stime),2);
        se=sum(bsxfun(@gt,y(:,2),lik.stime),2);
        for i1=1:n
          i3=i3v(i1);
          nft=(i3-1)*ntime;
          if sb(i1)==0
            llg(i1+nf1)= nu(i1) - (y(i1,2)-lik.stime(se(i1))).*la1(se(i1)+nft).*eta2(i1) - sum(sd.*la1((1+nft):(se(i1)-1+nft)).*eta2(i1));
          else
            if se(i1)==sb(i1)
              llg(i1+nf1) = nu(i1) - (y(i1,2)-y(i1,1)).*la1(se(i1)+nft).*eta2(i1);
            else
              llg(i1+nf1) = nu(i1) - (y(i1,2)-lik.stime(se(i1))).*la1(se(i1)+nft).*eta2(i1) - sum(sd.*la1((sb(i1)+1+nft):(se(i1)-1+nft)).*eta2(i1)) - (lik.stime(sb(i1)+1)-y(i1,1)).*la1(sb(i1)+nft).*eta2(i1);
            end
          end
        end

      end

    end
  end

  function [llg2,llg2mat] = lik_coxph_llg2(lik, y, f, param, z)
  %function [pi_vec, pi_mat] = lik_coxph_llg2(lik, y, ff, param, z)
  %LIK_COXPH_LLG2  Second gradients of the log likelihood
  %
  %  Description
  %    LLG2 = LIK_COXPH_LLG2(LIK, Y, F, PARAM) takes a likelihood
  %    structure LIK, incedence counts Y, expected counts Z, and
  %    latent values F. Returns the Hessian of the 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_COXPH_LL, LIK_COXPH_LLG, LIK_COXPH_LLG3, GPLA_E

    if isempty(z)
      error(['lik_coxph -> lik_coxph_llg2: missing z!   '...
             'Coxph likelihood needs the expected number of    '...
             'occurrences as an extra input z. See, for         '...
             'example, lik_coxph and gpla_e.               ']);
    end

    [n,ny]=size(y);
    ntime=size(lik.xtime,1);
    i3v=ones(size(y,1),1);
    if isfield(lik, 'stratificationVariables')
      f1=f(1:ntime*lik.n_u);
      f2=f((ntime*lik.n_u+1):(ntime*lik.n_u+n));
      llg2=zeros(ntime*lik.n_u+n,1);
      llg2mat=zeros(ntime*lik.n_u,n);
      for i=1:lik.n_u
        i3v(lik.stratind{i})=i;
      end
      nf1=ntime*lik.n_u;
    else
      f1=f(1:ntime);
      f2=f((ntime+1):(ntime+n));
      llg2=zeros(ntime+n,1);
      llg2mat=zeros(ntime,n);
      nf1=ntime;
    end

    la1=exp(f1);
    eta2=exp(f2);

    %nu=1-z;
    sd=lik.stime(2)-lik.stime(1);
    switch param
      case 'latent'

        if ny==1
          % 11
          if ~isfield(lik, 'stratificationVariables')
            for i1=1:ntime
              ind=y>=lik.stime(i1) & y<lik.stime(i1+1);
              llg2(i1)= sum(-(y(ind)-lik.stime(i1)).*la1(i1).*eta2(ind)) - sum(sum(sd.*la1(i1)).*eta2(~ind & y>=lik.stime(i1+1)));
              %llg2(i1,i1)= sum(-(y(ind)-lik.stime(i1)).*la1(i1).*eta2(ind)) - sum(sum(sd.*la1(i1)).*eta2(~ind & y>=lik.stime(i1+1)));
            end
          else
            for j=1:lik.n_u
              for i1=1:ntime
                ind=y>=lik.stime(i1) & y<lik.stime(i1+1);
                ind2=lik.stratind{j};
                llg2(i1+(j-1)*ntime)= sum(-(y(ind & ind2)-lik.stime(i1)).*la1(i1+ntime*(j-1)).*eta2(ind & ind2)) - sum(sum(sd.*la1(i1+ntime*(j-1))).*eta2((~ind & ind2) & y>=lik.stime(i1+1)));
                %llg2(i1,i1)= sum(-(y(ind)-lik.stime(i1)).*la1(i1).*eta2(ind)) - sum(sum(sd.*la1(i1)).*eta2(~ind & y>=lik.stime(i1+1)));
              end
            end
          end

          % 22
          for i1=1:n
            si=sum(y(i1)>lik.stime);
            i3=i3v(i1);
            llg2(i1+nf1)= -(y(i1)-lik.stime(si)).*la1(si+(i3-1)*ntime).*eta2(i1) - sum(sd.*la1((1+(i3-1)*ntime):(si-1+(i3-1)*ntime)).*eta2(i1));
            %llg2(i1+ntime,i1+ntime)= -(y(i1)-lik.stime(si)).*la1(si).*eta2(i1) - sum(sd.*la1(1:(si-1)).*eta2(i1));
          end

          % derivative wrt f1 and f2:
          if ~isfield(lik, 'stratificationVariables')
            for i1=1:ntime
              ind=y>=lik.stime(i1) & y<lik.stime(i1+1);
              llg2mat(i1,find(ind))= -(y(ind)-lik.stime(i1)).*la1(i1).*eta2(ind);
              llg2mat(i1,find((~ind & y>=lik.stime(i1+1)))) = - sd.*la1(i1).*eta2((~ind & y>=lik.stime(i1+1)));
              %llg2(i1,ntime+find(ind))= -(y(ind)-lik.stime(i1)).*la1(i1).*eta2(ind);
              %llg2(i1,ntime+find((~ind & y>=lik.stime(i1+1)))) = - sd.*la1(i1).*eta2((~ind & y>=lik.stime(i1+1)));
              %llg2(ntime+find(ind),i1)=llg2(i1,ntime+find(ind));
              %llg2(ntime+find((~ind & y>=lik.stime(i1+1))),i1)=llg2(i1,ntime+find((~ind & y>=lik.stime(i1+1))));
            end
          else
            for j=1:lik.n_u
              ind2=lik.stratind{j};
              for i1=1:ntime
                ind=y>=lik.stime(i1) & y<lik.stime(i1+1);
                llg2mat(i1+(j-1)*ntime,find(ind & ind2))= -(y(ind & ind2)-lik.stime(i1)).*la1(i1+(j-1)*ntime).*eta2(ind & ind2);
                llg2mat(i1+(j-1)*ntime,find(((~ind & ind2) & y>=lik.stime(i1+1)))) = - sd.*la1(i1+(j-1)*ntime).*eta2(((~ind & ind2) & y>=lik.stime(i1+1)));
                %llg2(i1,ntime+find(ind))= -(y(ind)-lik.stime(i1)).*la1(i1).*eta2(ind);
                %llg2(i1,ntime+find((~ind & y>=lik.stime(i1+1)))) = - sd.*la1(i1).*eta2((~ind & y>=lik.stime(i1+1)));
                %llg2(ntime+find(ind),i1)=llg2(i1,ntime+find(ind));
                %llg2(ntime+find((~ind & y>=lik.stime(i1+1))),i1)=llg2(i1,ntime+find((~ind & y>=lik.stime(i1+1))));
              end
            end
          end

        else

          % 11
          for i1=1:ntime

            % left truncated + follow-up entry: (1)
            ind_vkst = y(:,1)>=lik.stime(i1) & y(:,1)<lik.stime(i1+1) & y(:,2)>=lik.stime(i1+1);
            % follow-up entry + follow-up exit: (2)
            ind_stsp = y(:,1)>=lik.stime(i1) & y(:,1)<lik.stime(i1+1) & y(:,2)>=lik.stime(i1) & y(:,2)<lik.stime(i1+1);
            % follow-up: (3)
            ind_s = y(:,1)<lik.stime(i1) & y(:,2)>=lik.stime(i1+1);
            % follow-up exit: (4)
            ind_sp = y(:,1)<lik.stime(i1) & y(:,2)>=lik.stime(i1) & y(:,2)<lik.stime(i1+1);

            if ~isfield(lik, 'stratificationVariables')
              % (1)
              s2b=sum(-(lik.stime(i1+1)-y(ind_vkst,1)).*la1(i1).*eta2(ind_vkst));
              % (2)
              s3b=-sum((y(ind_stsp,2)-y(ind_stsp,1)).*la1(i1).*eta2(ind_stsp));
              % (3)
              s4=-sum(sd.*la1(i1).*eta2(ind_s));
              % (4)
              s5=-sum((y(ind_sp,2)-lik.stime(i1)).*la1(i1).*eta2(ind_sp));
              llg2(i1) = s2b+s3b+s4+s5;
            else
              for j=1:lik.n_u
                ind2=lik.stratind{j};
                nft=(j-1)*ntime;
                % (1)
                s2b=sum(-(lik.stime(i1+1)-y(ind_vkst & ind2,1)).*la1(i1+nft).*eta2(ind_vkst & ind2));
                % (2)
                s3b=-sum((y(ind_stsp & ind2,2)-y(ind_stsp & ind2,1)).*la1(i1+nft).*eta2(ind_stsp & ind2));
                % (3)
                s4=-sum(sd.*la1(i1+nft).*eta2(ind_s & ind2));
                % (4)
                s5=-sum((y(ind_sp & ind2,2)-lik.stime(i1)).*la1(i1+nft).*eta2(ind_sp & ind2));
                llg2(i1+nft) = s2b+s3b+s4+s5;
              end
            end
          end

          % 22
          sb=sum(bsxfun(@gt,y(:,1),lik.stime),2);
          se=sum(bsxfun(@gt,y(:,2),lik.stime),2);
          for i1=1:n
            i3=i3v(i1);
            nft=(i3-1)*ntime;
            if sb(i1)==0
              llg2(i1+nf1)= -(y(i1,2)-lik.stime(se(i1))).*la1(se(i1)+nft).*eta2(i1) - sum(sd.*la1((1+nft):(se(i1)-1+nft)).*eta2(i1));
              %llg2(i1+ntime,i1+ntime)= -(y(i1,2)-lik.stime(se)).*la1(se).*eta2(i1) - sum(sd.*la1(1:(se-1)).*eta2(i1));
            else
              if se(i1)==sb(i1)
                llg2(i1+nf1) = -(y(i1,2)-y(i1,1)).*la1(se(i1)+nft).*eta2(i1);
                %llg2(i1+ntime,i1+ntime) = -(y(i1,2)-y(i1,1)).*la1(se).*eta2(i1);
              else
                llg2(i1+nf1) = -(y(i1,2)-lik.stime(se(i1))).*la1(se(i1)+nft).*eta2(i1) - sum(sd.*la1((sb(i1)+1+nft):(se(i1)-1+nft)).*eta2(i1)) - (lik.stime(sb(i1)+1)-y(i1,1)).*la1(sb(i1)+nft).*eta2(i1);
                %llg2(i1+ntime,i1+ntime) = -(y(i1,2)-lik.stime(se)).*la1(se).*eta2(i1) - sum(sd.*la1((sb+1):(se-1)).*eta2(i1)) - (lik.stime(sb+1)-y(i1,1)).*la1(sb).*eta2(i1);
              end
            end
          end

          % derivative wrt f1 and f2:
          for i1=1:ntime

            % left truncated + follow-up entry: (1)
            ind_vkst = y(:,1)>=lik.stime(i1) & y(:,1)<lik.stime(i1+1) & y(:,2)>=lik.stime(i1+1);
            % follow-up entry + follow-up exit: (2)
            ind_stsp = y(:,1)>=lik.stime(i1) & y(:,1)<lik.stime(i1+1) & y(:,2)>=lik.stime(i1) & y(:,2)<lik.stime(i1+1);
            % follow-up: (3)
            ind_s = y(:,1)<lik.stime(i1) & y(:,2)>=lik.stime(i1+1);
            % follow-up exit: (4)
            ind_sp = y(:,1)<lik.stime(i1) & y(:,2)>=lik.stime(i1) & y(:,2)<lik.stime(i1+1);

            if ~isfield(lik, 'stratificationVariables')
              % (1)
              llg2mat(i1,ind_vkst)=-(lik.stime(i1+1)-y(ind_vkst,1)).*la1(i1).*eta2(ind_vkst);
              % (2)
              llg2mat(i1,ind_stsp)=-(y(ind_stsp,2)-y(ind_stsp,1)).*la1(i1).*eta2(ind_stsp);
              % (3)
              llg2mat(i1,ind_s)= -sd.*la1(i1).*eta2(ind_s);
              % (4)
              llg2mat(i1,ind_sp)=-(y(ind_sp,2)-lik.stime(i1)).*la1(i1).*eta2(ind_sp);
            else
              for j=1:lik.n_u
                ind2=lik.stratind{j};
                nft=(j-1)*ntime;
                % (1)
                llg2mat(i1+nft,ind_vkst & ind2)=-(lik.stime(i1+1)-y(ind_vkst & ind2,1)).*la1(i1+nft).*eta2(ind_vkst & ind2);
                % (2)
                llg2mat(i1+nft,ind_stsp & ind2)=-(y(ind_stsp & ind2,2)-y(ind_stsp & ind2,1)).*la1(i1+nft).*eta2(ind_stsp & ind2);
                % (3)
                llg2mat(i1+nft,ind_s & ind2)= -sd.*la1(i1+nft).*eta2(ind_s & ind2);
                % (4)
                llg2mat(i1+nft,ind_sp & ind2)=-(y(ind_sp & ind2,2)-lik.stime(i1)).*la1(i1+nft).*eta2(ind_sp & ind2);
              end
            end
          end
      end
    end
  end

  function [llg3,llg3mat] = lik_coxph_llg3(lik, y, f, param, z, j1)
  %LIK_COXPH_LLG3  Third gradients of the log likelihood
  %
  %  Description
  %    LLG3 = LIK_COXPH_LLG3(LIK, Y, F, PARAM) takes a likelihood
  %    structure LIK, incedence counts Y, expected counts Z and
  %    latent values F and returns the third gradients of the log
  %    likelihood with respect to PARAM. At the moment PARAM can be
  %    only 'latent'. LLG3 is a vector with third gradients. This
  %    subfunction is needed when using Laplace approximation for
  %    inference with non-Gaussian likelihoods.
  %
  %  See also
  %    LIK_COXPH_LL, LIK_COXPH_LLG, LIK_COXPH_LLG2, GPLA_E, GPLA_G

    if isempty(z)
      error(['lik_coxph -> lik_coxph_llg3: missing z!   '...
             'Coxph likelihood needs the expected number of    '...
             'occurrences as an extra input z. See, for         '...
             'example, lik_coxph and gpla_e.               ']);
    end

    ntime=size(lik.xtime,1);

    [n,ny]=size(y);
    if isfield(lik, 'stratificationVariables')
      f1=f(1:ntime*lik.n_u);
      f2=f((ntime*lik.n_u+1):(ntime*lik.n_u+n));
      llg3=zeros(ntime*lik.n_u+n,1);
      llg3mat=zeros(ntime*lik.n_u,n);
      nf1=ntime*lik.n_u;
      if j1>nf1
        for j=1:lik.n_u
          if lik.stratind{j}(j1-nf1)==1;
            i3=j;
            break;
          end
        end
      end
    else
      f1=f(1:ntime);
      f2=f((ntime+1):(ntime+n));
      llg3=zeros(ntime+n,1);
      llg3mat=zeros(ntime,n);
      nf1=ntime;
      i3=1;
    end

    la1=exp(f1);
    eta2=exp(f2);

    %nu=1-z;
    sd=lik.stime(2)-lik.stime(1);

    switch param
      case 'latent'

        if ny==1

          if j1<=nf1

            indt=rem(j1,ntime);
            if indt==0
              indt=ntime;
            end

            % 11
            ind=y>=lik.stime(indt) & y<lik.stime(indt+1);

            if isfield(lik, 'stratificationVariables')
              ind2=lik.stratind{(j1-indt)/ntime+1};
            else
              ind2=ones(size(ind));
            end

            fi=find(ind & ind2);
            fni=find((~ind & ind2) & y>=lik.stime(indt));

            llg3(j1) = sum(-(y(ind & ind2)-lik.stime(indt)).*la1(j1).*eta2(ind & ind2)) - sum(sum(sd.*la1(j1)).*eta2((~ind & ind2) & y>=lik.stime(indt+1)));

            if ~isempty(fi)
              valtmp=(-(y(ind & ind2)-lik.stime(indt)).*la1(j1).*eta2(ind & ind2));
              for m2i=1:length(valtmp)
                llg3( nf1+fi(m2i))  = valtmp(m2i);
              end
            end
            if ~isempty(fni)
              valtmp2=(-sd.*la1(j1).*eta2(((~ind & ind2) & y>=lik.stime(indt+1))));
              for m2i=1:length(valtmp2)
                llg3( nf1+fni(m2i))  = valtmp2(m2i);
              end
            end

            % 12/21
            % derivative wrt f1 and f2:
            val1tmp=-(y(ind & ind2)-lik.stime(indt)).*la1(j1).*eta2(ind & ind2);
            llg3mat(j1,fi)= val1tmp;

            val2tmp = - sd.*la1(j1).*eta2(((~ind & ind2) & y>=lik.stime(indt+1)));
            llg3mat(j1,fni) = val2tmp;

          else

            % 11
            s1=sum(y(j1-nf1)>lik.stime);
            llg3((1+(i3-1)*ntime):(s1-1+(i3-1)*ntime)) = - sd.*la1((1+(i3-1)*ntime):(s1-1+(i3-1)*ntime)).*eta2(j1-nf1);
            llg3(s1+(i3-1)*ntime) = -(y(j1-nf1)-lik.stime(s1)).*la1(s1+(i3-1)*ntime).*eta2(j1-nf1);

            % 22
            llg3(j1) = -(y(j1-nf1)-lik.stime(s1)).*la1(s1+(i3-1)*ntime).*eta2(j1-nf1) ...
                        - sum(sd.*la1((1+(i3-1)*ntime):(s1-1+(i3-1)*ntime)).*eta2(j1-nf1));

            % 12/21
            % derivative wrt f1 and f2:
            val3tmp = - sd.*la1((1+(i3-1)*ntime):(s1-1+(i3-1)*ntime)).*eta2(j1-nf1);
            llg3mat((1+(i3-1)*ntime):(s1-1+(i3-1)*ntime),j1-nf1)= val3tmp;
            llg3mat(s1+(i3-1)*ntime,j1-nf1) = -(y(j1-nf1)-lik.stime(s1)).*la1(s1+(i3-1)*ntime).*eta2(j1-nf1);

          end

        else

          if j1<=nf1

            indt=rem(j1,ntime);
            if indt==0
              indt=ntime;
            end
            if isfield(lik, 'stratificationVariables')
              ind2=lik.stratind{(j1-indt)/ntime+1};
            else
              ind2=ones(size(y,1),1);
            end

            % 11
            % left truncated + follow-up entry: (1)
            ind_vkst = y(:,1)>=lik.stime(indt) & y(:,1)<lik.stime(indt+1) & y(:,2)>=lik.stime(indt+1);
            % follow-up entry + follow-up exit: (2)
            ind_stsp = y(:,1)>=lik.stime(indt) & y(:,1)<lik.stime(indt+1) & y(:,2)>=lik.stime(indt) & y(:,2)<lik.stime(indt+1);
            % follow-up: (3)
            ind_s = y(:,1)<lik.stime(indt) & y(:,2)>=lik.stime(indt+1);
            % follow-up exit: (4)
            ind_sp = y(:,1)<lik.stime(indt) & y(:,2)>=lik.stime(indt) & y(:,2)<lik.stime(indt+1);

            % (1)
            s2b=sum(-(lik.stime(indt+1)-y(ind_vkst & ind2,1)).*la1(j1).*eta2(ind_vkst & ind2));
            % (2)
            s3b=-sum((y(ind_stsp & ind2,2)-y(ind_stsp & ind2,1)).*la1(j1).*eta2(ind_stsp & ind2));
            % (3)
            s4=-sum(sd.*la1(j1).*eta2(ind_s & ind2));
            % (4)
            s5=-sum((y(ind_sp & ind2,2)-lik.stime(indt)).*la1(j1).*eta2(ind_sp & ind2));

            llg3(j1) = s2b+s3b+s4+s5;

            % 22
            % (1)
            llg3(nf1+find(ind_vkst & ind2))=-(lik.stime(indt+1)-y(ind_vkst & ind2,1)).*la1(j1).*eta2(ind_vkst & ind2);
            % (2)
            llg3(nf1+find(ind_stsp & ind2))=-(y(ind_stsp & ind2,2)-y(ind_stsp & ind2,1)).*la1(j1).*eta2(ind_stsp & ind2);
            % (3)
            llg3(nf1+find(ind_s & ind2))= -sd.*la1(j1).*eta2(ind_s & ind2);
            % (4)
            llg3(nf1+find(ind_sp & ind2))=-(y(ind_sp & ind2,2)-lik.stime(indt)).*la1(j1).*eta2(ind_sp & ind2);

            % 12/21
            llg3mat(j1,find(ind_vkst & ind2))=-(lik.stime(indt+1)-y(ind_vkst & ind2,1)).*la1(j1).*eta2(ind_vkst & ind2);
            % (2)
            llg3mat(j1,find(ind_stsp & ind2))=-(y(ind_stsp & ind2,2)-y(ind_stsp & ind2,1)).*la1(j1).*eta2(ind_stsp & ind2);
            % (3)
            llg3mat(j1,find(ind_s & ind2))= -sd.*la1(j1).*eta2(ind_s & ind2);
            % (4)
            llg3mat(j1,find(ind_sp & ind2))=-(y(ind_sp & ind2,2)-lik.stime(indt)).*la1(j1).*eta2(ind_sp & ind2);
          else

            sb=sum(y(j1-nf1,1)>lik.stime); % begin
            se=sum(y(j1-nf1,2)>lik.stime); % end

            nft=(i3-1)*ntime;
            % 11
            if sb==0
              llg3((1+nft):(se-1+nft))= -sd.*la1((1+nft):(se-1+nft)).*eta2(j1-nf1);
              llg3(se+nft)= -(y(j1-nf1,2)-lik.stime(se)).*la1(se+nft).*eta2(j1-nf1);
            else
              if se==sb
                llg3(se+nft) = -(y(j1-nf1,2)-y(j1-nf1,1)).*la1(se+nft).*eta2(j1-nf1);
              else
                llg3(sb+nft) = - (lik.stime(sb+1)-y(j1-nf1,1)).*la1(sb+nft).*eta2(j1-nf1);
                llg3((sb+1+nft):(se-1+nft)) = - sd.*la1((sb+1+nft):(se-1+nft)).*eta2(j1-nf1);
                llg3(se+nft) = -(y(j1-nf1,2)-lik.stime(se)).*la1(se+nft).*eta2(j1-nf1);
              end
            end

            % 12/21
            if sb==0
              llg3mat((1+nft):(se-1+nft),j1-nf1) = -sd.*la1((1+nft):(se-1+nft)).*eta2(j1-nf1);
              llg3mat(se+nft,j1-nf1)= -(y(j1-nf1,2)-lik.stime(se)).*la1(se+nft).*eta2(j1-nf1);
            else
              if se==sb
                llg3mat(se+nft,j1-nf1) = -(y(j1-nf1,2)-y(j1-nf1,1)).*la1(se+nft).*eta2(j1-nf1);
              else
                llg3mat(sb+nft,j1-nf1) = - (lik.stime(sb+1)-y(j1-nf1,1)).*la1(sb+nft).*eta2(j1-nf1);
                llg3mat((sb+1+nft):(se-1+nft),j1-nf1) = - sd.*la1((sb+1+nft):(se-1+nft)).*eta2(j1-nf1);
                llg3mat(se+nft,j1-nf1) = -(y(j1-nf1,2)-lik.stime(se)).*la1(se+nft).*eta2(j1-nf1);
              end
            end

            % 22
            if sb==0
              llg3(j1)= -(y(j1-nf1,2)-lik.stime(se)).*la1(se+nft).*eta2(j1-nf1) - sum(sd.*la1((1+nft):(se-1+nft)).*eta2(j1-nf1));
            else
              if se==sb
                llg3(j1) = -(y(j1-nf1,2)-y(j1-nf1,1)).*la1(se+nft).*eta2(j1-nf1);
              else
                llg3(j1) = -(y(j1-nf1,2)-lik.stime(se)).*la1(se+nft).*eta2(j1-nf1) - sum(sd.*la1((sb+1+nft):(se-1+nft)).*eta2(j1-nf1)) - (lik.stime(sb+1)-y(j1-nf1,1)).*la1(sb+nft).*eta2(j1-nf1);
              end
            end

          end
        end
    end
  end

  function [logM_0, m_1, sigm2hati1] = lik_coxph_tiltedMoments(lik, y, i1, S2_i, M_i, z)
    %LIK_COXPH_TILTEDMOMENTS  Returns the marginal moments for EP algorithm
    %
    %  Description
    %    [M_0, M_1, M2] = LIK_COXPH_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

      [n,ny]=size(y);

      % M_i(end);
      % S2_i(end,end);
      fgrid=M_i(end)+sqrt(S2_i(end,end))*[-6 6];
      fg=linspace(fgrid(1),fgrid(2),15);
      ng=length(fg);

      if isfield(gp.lik, 'stratificationVariables')
        ntime=size(lik.xtime,1)*gp.lik.n_u;
      else
        ntime=size(lik.xtime,1);
      end

      %f11=f(1:ntime);
      %f2=f((ntime+1):(ntime+n));
      %la1=exp(f1);
      %eta2=exp(f2);

      nu=1-z;
      sd=lik.stime(2)-lik.stime(1);
      if ny==1
        sb=1;
        se=sum(bsxfun(@gt,y(i1,1),lik.stime),2);
      end
      indf=sb:se;
      sdvec=[ones(se-1,1)*sd; y(i1)-lik.stime(se)];
      nutmp=zeros(se,1);
      nutmp(se)=nu(i1);

      for j1=1:ng

        % conditional distribution
        myy=M_i(indf)+S2_i(indf,end)*(1./S2_i(end,end))*(fg(j1)-M_i(end));
        myy0=myy;
        Sigm=S2_i(indf,indf)-S2_i(indf,end)*(1./S2_i(end,end))*S2_i(end,indf);
        Sigm0=Sigm;

        nu_prior=Sigm\myy;

        nt=size(myy,1);
        c1=exp(fg(j1));

        % site parameters
        tautilde=zeros(nt,1);
        nutilde=zeros(nt,1);
        ztilde=zeros(nt,1);

        max_small_ep_iter=50;
        tol=1e-9;
        small_ep_iter=1;

        tautilde0=Inf; nutilde0=Inf; ztilde0=Inf;

        logZep_tmp=0; logZep=Inf;
        %while small_ep_iter <= max_small_ep_iter && (sum(abs(tautilde0-tautilde)>tol) || sum(abs(nutilde0-nutilde)>tol) || sum(abs(ztilde0-ztilde)>tol))
        while small_ep_iter<=max_small_ep_iter && abs(logZep_tmp-logZep)>tol
          logZep_tmp=logZep;

          %tautilde0=tautilde; nutilde0=nutilde; ztilde0=ztilde;

          for k1=1:nt

            tau_i=Sigm(k1,k1)^-1-tautilde(k1);
            nu_i = Sigm(k1,k1)^-1*myy(k1)-nutilde(k1);
            myy_i=nu_i/tau_i;
            sigm2_i=tau_i^-1;

            % marginal moments
            [logM0(k1), muhati, sigm2hati] = coxph_tiltedMoments(sigm2_i, myy_i, nutmp(k1), sdvec(k1), c1);
            %[M0, muhati, sigm2hati] = coxph_tiltedMoments(lik, y(i1,:), k1, sigm2_i, myy_i, c1, sd_vec(i1), ztmp);

            deltatautilde=sigm2hati^-1-tau_i-tautilde(k1);
            tautilde(k1)=tautilde(k1)+deltatautilde;
            nutilde(k1)=sigm2hati^-1*muhati-nu_i;

            apu = deltatautilde/(1+deltatautilde*Sigm(k1,k1));
            Sigm = Sigm - apu*(Sigm(:,k1)*Sigm(:,k1)');

            % The below is how Rasmussen and Williams
            % (2006) do the update. The above version is
            % more robust.
            %apu = deltatautilde^-1+Sigm(k1,k1);
            %apu = (Sigm(:,k1)/apu)*Sigm(:,k1)';
            %Sigm = Sigm - apu;
            %Sigm=Sigm-(deltatautilde^-1+Sigm(k1,k1))^-1*(Sigm(:,k1)*Sigm(:,k1)');

            %myy=Sigm*nutilde;
            myy=Sigm*(nutilde+nu_prior);

            muvec_i(k1,1)=myy_i;
            sigm2vec_i(k1,1)=sigm2_i;

          end

          if tautilde > 0
            Stilde=tautilde;
            Stildesqroot=diag(sqrt(tautilde));
            B=eye(nt)+Stildesqroot*Sigm0*Stildesqroot;
            L=chol(B,'lower');

            V=(L\Stildesqroot)*Sigm0;
            Sigm=Sigm0-V'*V;
            %myy=Sigm*nutilde;
            myy=Sigm*(nutilde+nu_prior);

            %Ls = chol(Sigm);

            % Compute the marginal likelihood
            % Direct formula (3.65):
            % Sigmtilde=diag(1./tautilde);
            % mutilde=inv(Stilde)*nutilde;
            %
            % logZep=-0.5*log(det(Sigmtilde+K))-0.5*mutilde'*inv(K+Sigmtilde)*mutilde+
            %         sum(log(normcdf(y.*muvec_i./sqrt(1+sigm2vec_i))))+
            %         0.5*sum(log(sigm2vec_i+1./tautilde))+
            %         sum((muvec_i-mutilde).^2./(2*(sigm2vec_i+1./tautilde)))

            % 4. term & 1. term
            term41=0.5*sum(log(1+tautilde.*sigm2vec_i))-sum(log(diag(L)));

            % 5. term (1/2 element) & 2. term
            T=1./sigm2vec_i;
            Cnutilde = Sigm0*nutilde;
            L2 = V*nutilde;
            term52 = nutilde'*Cnutilde - L2'*L2 - (nutilde'./(T+Stilde)')*nutilde;
            term52 = term52.*0.5;

            % 5. term (2/2 element)
            term5=0.5*muvec_i'.*(T./(Stilde+T))'*(Stilde.*muvec_i-2*nutilde);

            % 3. term
            term3 = sum(logM0);

            V_tmp=(L\Stildesqroot);
            Sigm_inv_tmp=V_tmp'*V_tmp;

            term_add1=-0.5*myy0'*Sigm_inv_tmp*myy0;
            term_add2=myy0'*(eye(nt)-Sigm_inv_tmp*Sigm0)*nutilde;
            logZep = -(term41+term52+term5+term3+term_add1+term_add2);

            %logZep = -(term41+term52+term5+term3);

            small_ep_iter=small_ep_iter+1;
            %iter=iter+1;
          else
            error('tautilde <= 0')
          end
        end

        ZZ(j1,1)=exp(-logZep);
        MM(:,j1)=myy;
        SS2(:,:,j1)=Sigm;

      end

      %m_0=zeros(1,1);
      %m_1=zeros(nt+1,1);
      %sigm2hati1=zeros(nt+1,nt+1);
      % indf

      W=normpdf(fg,M_i(end),sqrt(S2_i(end,end)))*(fg(2)-fg(1));

      qw=W.*ZZ';
      m_0=sum(qw);
      m_1=[sum(bsxfun(@times,qw,MM),2); sum(qw.*fg)]./m_0;

      m_211=zeros(nt,nt);
      for k1=1:ng
        m_211=m_211+qw(k1)*(SS2(:,:,k1)+MM(:,k1)*MM(:,k1)');
      end
      m_212=(qw.*fg)*MM';
      m_222=(qw.*fg)*fg';

      m_2=[m_211 m_212'; m_212 m_222]./m_0;

      sigm2hati1=m_2 - m_1*m_1';
      logM_0 = log(m_0);

      %figure(1),hold on, plot(fg(j1),logZep,'.')
      %figure(2),hold on, plot(fg(j1),exp(-logZep),'.')

  end

  function [lpyt,Ey, Vary] = lik_coxph_predy(lik, Ef, Covf, yt, zt)
  %LIK_COXPH_PREDY  Returns the predictive mean, variance and density of y
  %
  %  Description
  %    [EY, VARY] = LIK_COXPH_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 predictive
  %    distributions for future observations.
  %
  %    [Ey, Vary, PY] = LIK_COXPH_PREDY(LIK, EF, VARF YT, ZT)
  %    Returns also the predictive density of YT, that is
  %        p(yt | zt) = \int p(yt | f, zt) p(f|y) df.
  %    This requires also the incedence counts YT, expected counts ZT.
  %    This subfunction is needed when computing posterior predictive
  %    distributions for future observations.
  %
  %  See also
  %    GPLA_PRED, GPEP_PRED, GPMC_PRED

    if isempty(zt)
      error(['lik_coxph -> lik_coxph_predy: missing zt!'...
             'Coxph likelihood needs the expected number of    '...
             'occurrences as an extra input zt. See, for         '...
             'example, lik_coxph and gpla_e.               ']);
    end
    ntime=size(lik.xtime,1);

    ntest=size(zt,1);
    ny=size(yt,2);

    if isfield(lik, 'stratificationVariables')
      nf1=lik.n_u*ntime;
      i3v=zeros(ntest,1);
      for ii=1:length(lik.stratindt)
        i3v(lik.stratindt{ii})=ii;
      end
    else
      nf1=ntime;
      i3v=ones(ntest,1);
    end

    Py = zeros(size(zt));
    %Ey = zeros(size(zt));
    %EVary = zeros(size(zt));
    %VarEy = zeros(size(zt));

    S=10000;
    sd=lik.stime(2)-lik.stime(1);
    nu=1-zt;

    [nn1,nn2,c] =size(Covf);
    if (c>1) || (nn1==nn2)
      mcmc=false;
    else
      mcmc=true;
    end

    for i1=1:ntest
      i3=i3v(i1);
      if mcmc
        Sigm_tmp=([Covf(1:ntime,:); Covf(i1+ntime,:)]);
        f_star=bsxfun(@plus,Ef([(1+(i3-1)*ntime):(i3*ntime) i1+nf1])', ...
          bsxfun(@times,sqrt(Sigm_tmp'),randn(S,nf1+1)));
      else
        Sigm_tmp=Covf([(1+(i3-1)*ntime):(i3*ntime) i1+nf1],[(1+(i3-1)*ntime):(i3*ntime) i1+nf1]);
        Sigm_tmp=(Sigm_tmp+Sigm_tmp')./2;
        f_star=mvnrnd(Ef([(1+(i3-1)*ntime):(i3*ntime) i1+nf1]), Sigm_tmp, S);
      end

      f1=f_star(:,1:ntime);
      f2=f_star(:,(ntime+1):end);

      la1=exp(f1);
      eta2=exp(f2);

      if ny==1
        si=sum(yt(i1)>lik.stime);
        Py(i1)=mean(exp(nu(i1).*(f1(:,si)+f2) - (yt(i1)-lik.stime(si)).*la1(:,si).*eta2 ...
                     - sum(sd.*la1(:,1:(si-1)),2).*eta2));
      else

        sb=sum(bsxfun(@gt,yt(i1,1),lik.stime),2);
        se=sum(bsxfun(@gt,yt(i1,2),lik.stime),2);

        if sb==0
          Py(i1) = mean(exp(nu(i1).*(f1(:,se)+f2) - (yt(i1,2)-lik.stime(se)).*la1(:,se).*eta2 - sum(sd.*la1(:,1:(se-1)),2).*eta2));
        else
          if se==sb
            Py(i1) = mean(exp(nu(i1).*(f1(:,se)+f2) - (yt(i1,2)-yt(i1,1)).*la1(:,se).*eta2));
          else
            Py(i1) = mean(exp(nu(i1).*(f1(:,se)+f2) - (yt(i1,2)-lik.stime(se)).*la1(:,se).*eta2 - sum(sd.*la1(:,(sb+1):(se-1)),2).*eta2 - (lik.stime(sb+1)-yt(i1,1)).*la1(:,sb).*eta2));
          end
        end
      end
    end
    Ey = [];
    Vary = [];
    lpyt=log(Py);

    %     % Evaluate Ey and Vary
%     for i1=1:length(Ef)
%       %%% With quadrature
%       myy_i = Ef(i1);
%       sigm_i = sqrt(Varf(i1));
%       minf=myy_i-6*sigm_i;
%       maxf=myy_i+6*sigm_i;
%
%       F = @(f) exp(log(avgE(i1))+f+norm_lpdf(f,myy_i,sigm_i));
%       Ey(i1) = quadgk(F,minf,maxf);
%
%       F2 = @(f) exp(log(avgE(i1).*exp(f)+((avgE(i1).*exp(f)).^2/r))+norm_lpdf(f,myy_i,sigm_i));
%       EVary(i1) = quadgk(F2,minf,maxf);
%
%       F3 = @(f) exp(2*log(avgE(i1))+2*f+norm_lpdf(f,myy_i,sigm_i));
%       VarEy(i1) = quadgk(F3,minf,maxf) - Ey(i1).^2;
%     end
%     Vary = EVary + VarEy;
%
%     % Evaluate the posterior predictive densities of the given observations
%     if nargout > 2
%       for i1=1:length(Ef)
%         % get a function handle of the likelihood times posterior
%         % (likelihood * posterior = Negative-binomial * Gaussian)
%         % and useful integration limits
%         [pdf,minf,maxf]=init_coxph_norm(...
%           yt(i1),Ef(i1),Varf(i1),avgE(i1),r);
%         % integrate over the f to get posterior predictive distribution
%         Py(i1) = quadgk(pdf, minf, maxf);
%       end
%     end
  end

  function [logM_0, m_1, sigm2hati1] = coxph_tiltedMoments(sigm2_i, myy_i, nutmp, sd, c1)

  integrand = @(f) exp(-c1.*exp(f).*sd + nutmp*(f+log(c1)) - log(sigm2_i)/2 - log(2*pi)/2 - 0.5*(f-myy_i).^2./sigm2_i);
  RTOL = 1.e-6;
  ATOL = 1.e-10;
  minf=myy_i+sqrt(sigm2_i)*(-6);
  maxf=myy_i+sqrt(sigm2_i)*(6);

  [m_0, m_1, m_2] = quad_moments(integrand, minf, maxf, RTOL, ATOL);
  sigm2hati1 = m_2 - m_1.^2;

  % If the second central moment is less than cavity variance
  % integrate more precisely. Theoretically for log-concave
  % likelihood should be sigm2hati1 < sigm2_i.

  if sigm2hati1 >= sigm2_i
    ATOL = ATOL.^2;
    RTOL = RTOL.^2;
    [m_0, m_1, m_2] = quad_moments(tf, minf, maxf, RTOL, ATOL);
    sigm2hati1 = m_2 - m_1.^2;
    if sigm2hati1 >= sigm2_i
      error('lik_poisson_tilted_moments: sigm2hati1 >= sigm2_i');
    end
  end
  logM_0 = log(m_0);
  end

  function [df,minf,maxf] = init_coxph_norm(yy,myy_i,sigm2_i,avgE,r)
  %INIT_COXPH_NORM
  %
  %  Description
  %    Return function handle to a function evaluating
  %    Negative-Binomial * Gaussian which is used for evaluating
  %    (likelihood * cavity) or (likelihood * posterior) Return
  %    also useful limits for integration. This is private function
  %    for lik_coxph.
  %
  %  See also
  %    LIK_COXPH_TILTEDMOMENTS, LIK_COXPH_SITEDERIV,
  %    LIK_COXPH_PREDY

  % avoid repetitive evaluation of constant part
    ldconst = -gammaln(r)-gammaln(yy+1)+gammaln(r+yy)...
              - log(sigm2_i)/2 - log(2*pi)/2;
    % Create function handle for the function to be integrated
    df = @coxph_norm;
    % use log to avoid underflow, and derivates for faster search
    ld = @log_coxph_norm;
    ldg = @log_coxph_norm_g;
    ldg2 = @log_coxph_norm_g2;

    % Set the limits for integration
    % Negative-binomial likelihood is log-concave so the coxph_norm
    % function is unimodal, which makes things easier
    if yy==0
      % with yy==0, the mode of the likelihood is not defined
      % use the mode of the Gaussian (cavity or posterior) as a first guess
      modef = myy_i;
    else
      % use precision weighted mean of the Gaussian approximation
      % of the Negative-Binomial likelihood and Gaussian
      mu=log(yy/avgE);
      s2=(yy+r)./(yy.*r);
      modef = (myy_i/sigm2_i + mu/s2)/(1/sigm2_i + 1/s2);
    end
    % find the mode of the integrand using Newton iterations
    % few iterations is enough, since the first guess in the right direction
    niter=4;       % number of Newton iterations
    mindelta=1e-6; % tolerance in stopping Newton iterations
    for ni=1:niter
      g=ldg(modef);
      h=ldg2(modef);
      delta=-g/h;
      modef=modef+delta;
      if abs(delta)<mindelta
        break
      end
    end
    % integrand limits based on Gaussian approximation at mode
    modes=sqrt(-1/h);
    minf=modef-8*modes;
    maxf=modef+8*modes;
    modeld=ld(modef);
    iter=0;
    % check that density at end points is low enough
    lddiff=20; % min difference in log-density between mode and end-points
    minld=ld(minf);
    step=1;
    while minld>(modeld-lddiff)
      minf=minf-step*modes;
      minld=ld(minf);
      iter=iter+1;
      step=step*2;
      if iter>100
        error(['lik_coxph -> init_coxph_norm: ' ...
               'integration interval minimun not found ' ...
               'even after looking hard!'])
      end
    end
    maxld=ld(maxf);
    step=1;
    while maxld>(modeld-lddiff)
      maxf=maxf+step*modes;
      maxld=ld(maxf);
      iter=iter+1;
      step=step*2;
      if iter>100
        error(['lik_coxph -> init_coxph_norm: ' ...
               'integration interval maximun not found ' ...
               'even after looking hard!'])
      end
    end

%     while minld>(modeld-lddiff)
%       minf=minf-modes;
%       minld=ld(minf);
%       iter=iter+1;
%       if iter>100
%         error(['lik_coxph -> init_coxph_norm: ' ...
%                'integration interval minimun not found ' ...
%                'even after looking hard!'])
%       end
%     end
%     maxld=ld(maxf);
%     while maxld>(modeld-lddiff)
%       maxf=maxf+modes;
%       maxld=ld(maxf);
%       iter=iter+1;
%       if iter>100
%         error(['lik_coxph -> init_coxph_norm: ' ...
%                'integration interval maximum not found ' ...
%                'even after looking hard!'])
%       end
%
%     end

    function integrand = coxph_norm(f)
    % Negative-binomial * Gaussian
      mu = avgE.*exp(f);
      integrand = exp(ldconst ...
                      +yy.*(log(mu)-log(r+mu))+r.*(log(r)-log(r+mu)) ...
                      -0.5*(f-myy_i).^2./sigm2_i);
    end

    function log_int = log_coxph_norm(f)
    % log(Negative-binomial * Gaussian)
    % log_coxph_norm is used to avoid underflow when searching
    % integration interval
      mu = avgE.*exp(f);
      log_int = ldconst...
                +yy.*(log(mu)-log(r+mu))+r.*(log(r)-log(r+mu))...
                -0.5*(f-myy_i).^2./sigm2_i;
    end

    function g = log_coxph_norm_g(f)
    % d/df log(Negative-binomial * Gaussian)
    % derivative of log_coxph_norm
      mu = avgE.*exp(f);
      g = -(r.*(mu - yy))./(mu.*(mu + r)).*mu ...
          + (myy_i - f)./sigm2_i;
    end

    function g2 = log_coxph_norm_g2(f)
    % d^2/df^2 log(Negative-binomial * Gaussian)
    % second derivate of log_coxph_norm
      mu = avgE.*exp(f);
      g2 = -(r*(r + yy))/(mu + r)^2.*mu ...
           -1/sigm2_i;
    end

  end

  function p = lik_coxph_invlink(lik, f, z)
  %LIK_COXPH_INVLINK  Returns values of inverse link function
  %
  %  Description
  %    P = LIK_COXPH_INVLINK(LIK, F) takes a likelihood structure LIK and
  %    latent values F and returns the values of inverse link function P.
  %    This subfunction is needed when using function gp_predprcty.
  %
  %     See also
  %     LIK_COXPH_LL, LIK_COXPH_PREDY

    p = exp(f);
  end

  function [cdf,Ey,Vary] = lik_coxph_predcdf(lik,Ef,Covf,yt)
  %LIK_LOGLOGISTIC_PREDCDF  Returns the predictive cdf evaluated at yt
  %
  %  Description
  %    CDF = LIK_LOGLOGISTIC_PREDCDF(LIK, EF, VARF, YT)
  %    Returns the predictive cdf evaluated at YT given likelihood
  %    structure LIK, posterior mean EF and posterior Variance VARF
  %    of the latent variable. This subfunction is needed when using
  %    functions gp_predcdf or gp_kfcv_cdf.
  %
  %  See also
  %    GP_PREDCDF

    if isfield(lik, 'stratificationVariables')
      ntime=size(lik.xtime,1)*lik.n_u;
    else
      ntime=size(lik.xtime,1);
    end
    Ef1 = Ef(1:ntime); Ef(1:ntime) = []; Ef2 = Ef;
    nsamps = 10000;
    sd=lik.stime(2)-lik.stime(1);
    Sigm_tmp=Covf;
    [nn1,nn2,cc]=size(Sigm_tmp);
    if cc==1 && nn1~=nn2
      f_star=bsxfun(@plus,[Ef1;Ef2]', ...
        bsxfun(@times,sqrt(Sigm_tmp'),randn(nsamps,ntime+size(Ef2,1))));
    else
      Sigm_tmp=(Sigm_tmp+Sigm_tmp')./2;
      % f_star=mvnrnd(Ef1, Sigm_tmp(1:ntime,1:ntime), nsamps);
      f_star=mvnrnd([Ef1;Ef2], Sigm_tmp, nsamps);
    end

    f1=f_star(:,1:ntime);
    f2=f_star(:,(ntime+1):end);

    la1=exp(f1);
    eta2=exp(f2);

    mST=zeros(size(eta2,2),1);
    if size(yt,2) == 1
      % Integrate from zero to yt
      cumsumtmp=cumsum(la1'*sd)';
      %   t=binsgeq(gp.lik.xtime,yt(i));
      for i1=1:size(eta2,2)
        Stime=exp(-bsxfun(@times,cumsumtmp,eta2(:,i1)));
        mStime=mean(Stime);
        % for i=1:size(yt,1)
        mST(i1)=mStime(binsgeq(lik.xtime,yt(i1)));
        %end
      end
      cdf = 1- mST;

    else
      error('Size(yt,2) ~= 1');
    end

  end

  function reclik = lik_coxph_recappend(reclik, ri, lik)
  %RECAPPEND  Append the parameters to the record
  %
  %  Description
  %    RECLIK = GPCF_COXPH_RECAPPEND(RECLIK, RI, LIK) takes a
  %    likelihood record structure RECLIK, record index RI and
  %    likelihood structure LIK with the current MCMC samples of
  %    the parameters. Returns RECLIK which contains all the old
  %    samples and the current samples from LIK. This subfunction
  %    is needed when using MCMC sampling (gp_mc).
  %
  %  See also
  %    GP_MC

  % Initialize record
    if nargin == 2
      reclik=ri;

      % Set the function handles
      reclik.fh.pak = @lik_coxph_pak;
      reclik.fh.unpak = @lik_coxph_unpak;
      reclik.fh.lp = @lik_coxph_lp;
      reclik.fh.lpg = @lik_coxph_lpg;
      reclik.fh.ll = @lik_coxph_ll;
      reclik.fh.llg = @lik_coxph_llg;
      reclik.fh.llg2 = @lik_coxph_llg2;
      reclik.fh.llg3 = @lik_coxph_llg3;
      reclik.fh.tiltedMoments = @lik_coxph_tiltedMoments;
      reclik.fh.predy = @lik_coxph_predy;
      reclik.fh.invlink = @lik_coxph_invlink;
      reclik.fh.recappend = @lik_coxph_recappend;
      return
    else

      reclik.xtime=lik.xtime;
      reclik.stime=lik.stime;

      if isfield(lik, 'stratificationVariables')
        reclik.stratificationVariables=lik.stratificationVariables;
        if isfield(lik,removeStratificationVariables)
          reclik.removeStratificationVariables=lik.removeStratificationVariables;
        end
      end
    end
  end

end


function ierror = edge_check ( n_node, n_edge, t )

%*****************************************************************************80
%
%% EDGE_CHECK checks a graph stored by edges.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 January 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer  N_NODE, the number of nodes in the graph.
%    N_NODE must be positive.
%
%    Input, integer N_EDGE, the number of edges in the graph.
%    N_EDGE must be positive.
%
%    Input, integer T(2,N_EDGE), describes the edges of the tree
%    as pairs of nodes.
%
%    Output, integer IERROR, error flag.
%    -1, N_NODE is not positive.
%    -2, N_EDGE is not positive.
%    0, no error.
%    I, edge T(1,I), T(2,I) is illegal.
%
  ierror = 0;

  if ( n_node < 1 )
    ierror = -1;
    return
  end

  if ( n_edge < 1 )
    ierror = -2;
    return
  end
%
%  Every edge must join two legal nodes.
%
  for i = 1 : 2
    for j = 1 : n_edge
      if ( t(i,j) < 1 || n_node < t(i,j) )
        ierror = i;
        return
      end
    end
  end
%
%  Every edge must join distinct nodes.
%
  for j = 1 : n_edge
    if ( t(1,j) == t(2,j) )
      ierror = i;
      return
    end
  end
%
%  Every edge must be distinct.
%
  for j = 1 : n_edge - 1
    for j2 = j + 1 : n_edge
      if ( t(1,j) == t(1,j2) && t(2,j) == t(2,j2) )
        ierror = j2;
        return
      elseif ( t(1,j) == t(2,j2) && t(2,j) == t(1,j2) )
        ierror = j2;
        return
      end
    end
  end

  return
end


function edgenb=edgeneighbors(t,opt)
%
% edgenb=edgeneighbors(t,opt)
%
% to find neighboring triangular elements in a triangule surface
%
% author: Qianqian Fang, <q.fang at neu.edu>
%
% input:
%     t: a triangular surface element list, 3 columns of integers
%     opt: if opt='general', return the edge neighbors for a general
%          triangular surface: each edge can be shared by more than 2
%          triangles; if ignored, we assume all triangles are shared by no
%          more than 2 triangles.
%
% output:
%     edgenb: if opt is not supplied, edgenb is a size(t,1) by 3 array with
%     each element being the triangle ID of the edge neighbor of that
%     triangle. For each row, the order of the neighbors is listed as those
%     sharing edges [1 2], [2 3] and [3 1] between the triangle nodes.
%
%     when opt='general', edgenb is a cell array with a length of size(t).
%     each member of the cell array is a list of edge neighbors (the order
%     is not defined).
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

edges=[t(:,[1,2]);
       t(:,[2,3]);
       t(:,[3 1])];
edges=sort(edges,2);
[foo,ix,jx]=unique(edges,'rows');

if(nargin==2)
  if(strcmp(opt,'general'))
        ne=size(t,1);
        edgenb=cell(ne,1);
        for i=1:ne
            % this is very slow, need to be optimized
            nb=unique(mod([find(jx==jx(i) | jx==jx(i+ne) | jx==jx(i+2*ne))]',ne),'first');
            nb(nb==0)=ne;
            edgenb{i}=nb(nb~=i);
        end
        return;
  else
        error(['supplied option "' opt '" is not supported.']);
  end
end

if(isoctavemesh)
        u=unique(jx);
        qx=u(hist(jx,u)==2);
else
        vec=histc(jx,1:max(jx));
        qx=find(vec==2);
end

nn=max(t(:));
ne=size(t,1);
edgenb=zeros(size(t));

% now I need to find all repeatitive elements
% that share a face, to do this, unique('first')
% will give me the 1st element, and 'last' will
% give me the second. There will be no more than 2

% doing this is 60 times faster than doing find(jx==qx(i))
% inside a loop

[ujx,ii]=unique(jx,'first');
[ujx,ii2]=unique(jx,'last');

% iddup is the list of all pairs that share a common face

iddup=[ii(qx) ii2(qx)];
faceid=ceil(iddup/ne);
eid=mod(iddup,ne);
eid(eid==0)=ne;

% now rearrange this list into an element format

for i=1:length(qx)
	edgenb(eid(i,1),faceid(i,1))=eid(i,2);
	edgenb(eid(i,2),faceid(i,2))=eid(i,1);
end

% edgenb may contain 0s, that just means the corresponding
% face is a boundary face and has no neighbor.

% if the second option is 'surface', I am going to find
% and return surface patches only


function [ n_data, tc, sigma ] = surten_values ( n_data )

%*****************************************************************************80
%
%% SURTEN_VALUES returns some values of the surface tension.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Lester Haar, John Gallagher and George Kell,
%    NBS/NRC Steam Tables:
%    Thermodynamic and Transport Properties and Computer Programs
%    for Vapor and Liquid States of Water in SI Units,
%    Hemisphere Publishing Corporation, Washington, 1984,
%    TJ270.H3, pages 267.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real TC, the temperature, in degrees Celsius.
%
%    Output, real SIGMA, the surface tension,
%    in Pascal * m = Newton / m.
%
  n_max = 14;

  sigma_vec = [ ...
     74.22E+00, ...
     72.74E+00, ...
     71.20E+00, ...
     69.60E+00, ...
     67.95E+00, ...
     58.92E+00, ...
     48.75E+00, ...
     37.68E+00, ...
     26.05E+00, ...
     14.37E+00, ...
      8.78E+00, ...
      3.67E+00, ...
      0.40E+00, ...
      0.00E+00 ];

  tc_vec = [ ...
      10.000E+00, ...
      20.000E+00, ...
      30.000E+00, ...
      40.000E+00, ...
      50.000E+00, ...
     100.000E+00, ...
     150.000E+00, ...
     200.000E+00, ...
     250.000E+00, ...
     300.000E+00, ...
     325.000E+00, ...
     350.000E+00, ...
     370.000E+00, ...
     373.976E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    tc = 0.0;
    sigma = 0.0;
  else
    tc = tc_vec(n_data);
    sigma = sigma_vec(n_data);
  end

  return
end


function r = rdivide(a,b)
%RDIVIDE      Hessian elementwise right division  a ./ b
%

% written  04/04/04     S.M. Rump
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
% modified 08/26/12     S.M. Rump  global variables removed
%

  if prod(size(b))==1               % scalar denominator
    r = a / b;
    return
  end

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  scalar_a = ( prod(size(a))==1 );
  if ( ~scalar_a ) & ( ~isequal(size(a),size(b)) )
    error('dimensions do not match in hessian elementwise division')
  end

  N = getappdata(0,'INTLAB_HESSIAN_NUMVAR');

  % check for emptyness: cures Matlab bug
  % a=sparse([],[],[],2,1), [i,j,s]=find(a), s(i).*s(:)  yields error
  % throughout this routine use e.g.  sparse(ib,jb,reshape(D(jb),size(sb)).*sb,N,m);
  % rather than  sparse(ib,jb,D(jb).*sb(:),N,m);

  if ~isa(a,'hessian')              % non-hessian (scalar or array) ./ hessian array
    if issparse(b.hx)
      m = prod(size(b.x));
      r.x = a ./ b.x;
      D = -r.x ./ b.x;
      [ib,jb,sb] = find(b.dx);
      r.dx = sparse(ib,jb,reshape(D(jb),size(sb)).*sb,N,m);
      bdx = sparse(ib,jb,reshape(1./b.x(jb),size(sb)).*sb,N,m);
      r.hx = adx2rhx(N,m,bdx,b.dx);
      [ib,jb,sb] = find(b.hx-r.hx);
      r.hx = sparse(ib,jb,reshape(D(jb),size(sb)).*sb,N^2,m);
    else
      r.x = a ./ b.x;
      D = r.x ./ b.x;
      D = repmat(-D(:).',N^2,1);
      r.dx = D(1:N,:) .* b.dx;
      bdx1 = b.dx./repmat(b.x(:).',N,1);
      index = repmat(1:N,N,1);
      r.hx = D .* ( b.hx - bdx1(index,:).*b.dx(index',:) );
    end
  elseif ~isa(b,'hessian')          % hessian ./ non-hessian array
    r.x = a.x ./ b;
    bb = 1 ./ (b(:).');
    if scalar_a                     % hessian scalar ./ non-hessian array
      r.dx = a.dx * bb;
      r.hx = a.hx * bb;
    else                            % hessian array ./ non-hessian array
      if issparse(a.hx)
        m = prod(size(a.x));
        [ia,ja,sa] = find(a.dx);
        r.dx = sparse(ia,ja,reshape(bb(ja),size(sa)).*sa,N,m);
        [ia,ja,sa] = find(a.hx);
        r.hx = sparse(ia,ja,reshape(bb(ja),size(sa)).*sa,N^2,m);
      else
        bb = repmat(bb,N^2,1);
        r.dx = a.dx .* bb(1:N,:);
        r.hx = a.hx .* bb;
      end
    end
  else                              % hessian ./ hessian array
    m = prod(size(b.x));
    r.x = a.x ./ b.x;
    if scalar_a                     % hessian scalar ./ hessian array
      if issparse(a.hx) | issparse(b.hx)
        bx = sparse(b.x(:));
        D = 1 ./ sqr(bx);
        Num = a.dx * bx.' - a.x * b.dx;
        [i,j,s] = find(Num);
        r.dx = sparse(i,j,s.*reshape(D(j),size(s)),N,m);
        Num = sparse(i,j,s./reshape(bx(j),size(s)),N,m);
        r.hx = a.hx * sparse(bx.') - adx2rhx(N,m,b.dx,Num) - a.x * b.hx;
        [i,j,s] = find(r.hx);
        r.hx = sparse(i,j,s.*reshape(D(j),size(s)),N^2,m);
      else
        Num = a.dx * b.x(:).' - a.x * b.dx;
        D = repmat( sqr((1./b.x(:)).') , N^2 , 1 );
        r.dx = Num .* D(1:N,:);
        index = repmat(1:N,N,1);
        Num = Num ./ repmat(b.x(:).',N,1);
        r.hx = ( a.hx * b.x(:).' - reshape(b.dx(index,:) .* Num(index',:),size(b.hx)) - a.x * b.hx ) .* D;
      end
    else                            % hessian array ./ hessian array
      if issparse(a.hx) | issparse(b.hx)
        [ia,ja,sa] = find(a.dx);
        [ib,jb,sb] = find(b.dx);
        ax = a.x(:);
        bx = b.x(:);
        Num = sparse(ia,ja,reshape(bx(ja),size(sa)).*sa,N,m) - sparse(ib,jb,reshape(ax(jb),size(sb)).*sb,N,m);
        D = sqr( 1 ./ bx );
        [ia,ja,sa] = find(Num);
        r.dx = sparse(ia,ja,reshape(D(ja),size(sa)).*sa,N,m);
        Num = sparse(ia,ja,sa./reshape(bx(ja),size(sa)),N,m);
        [ia,ja,sa] = find(a.hx);
        [ib,jb,sb] = find(b.hx);
        r.hx = sparse(ia,ja,reshape(bx(ja),size(sa)).*sa,N^2,m) - adx2rhx(N,m,b.dx,Num) - ...
               sparse(ib,jb,reshape(ax(jb),size(sb)).*sb,N^2,m);
        [ia,ja,sa] = find(r.hx);
        r.hx = sparse(ia,ja,sa.*reshape(D(ja),size(sa)),N^2,m);
      else
        ax = repmat(a.x(:).',N^2,1);
        bx = repmat(b.x(:).',N^2,1);
        Num = a.dx .* bx(1:N,:) - ax(1:N,:) .* b.dx;
        D = repmat( sqr((1./b.x(:)).') , N^2 , 1 );
        r.dx = Num .* D(1:N,:);
        index = repmat(1:N,N,1);
        Num = Num ./ bx(1:N,:);
        r.hx = ( a.hx .* bx - reshape(b.dx(index,:) .* Num(index',:),size(a.hx)) - ax .* b.hx ) .* D;
      end
    end
  end

  r = class(r,'hessian');

  if rndold
    setround(rndold)
  end


function [ fxx, fxy, fyy ] = f00_f2 ( fi, n, x, y )

%*****************************************************************************80
%
%% F00_F2 returns second derivatives of any function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 August 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer FI, the index of the function.
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real X(N,1), Y(N,1), the evalution points.
%
%    Output, real FXX(N,1), FXY(N,1), FYY(N,1), the second derivatives.
%
  if ( fi == 1 )
    [ fxx, fxy, fyy ] = f01_f2 ( n, x, y );
  elseif ( fi == 2 )
    [ fxx, fxy, fyy ] = f02_f2 ( n, x, y );
  elseif ( fi == 3 )
    [ fxx, fxy, fyy ] = f03_f2 ( n, x, y );
  elseif ( fi == 4 )
    [ fxx, fxy, fyy ] = f04_f2 ( n, x, y );
  elseif ( fi == 5 )
    [ fxx, fxy, fyy ] = f05_f2 ( n, x, y );
  elseif ( fi == 6 )
    [ fxx, fxy, fyy ] = f06_f2 ( n, x, y );
  elseif ( fi == 7 )
    [ fxx, fxy, fyy ] = f07_f2 ( n, x, y );
  elseif ( fi == 8 )
    [ fxx, fxy, fyy ] = f08_f2 ( n, x, y );
  elseif ( fi == 9 )
    [ fxx, fxy, fyy ] = f09_f2 ( n, x, y );
  elseif ( fi == 10 )
    [ fxx, fxy, fyy ] = f10_f2 ( n, x, y );
  elseif ( fi == 11 )
    [ fxx, fxy, fyy ] = f11_f2 ( n, x, y );
  elseif ( fi == 12 )
    [ fxx, fxy, fyy ] = f12_f2 ( n, x, y );
  elseif ( fi == 13 )
    [ fxx, fxy, fyy ] = f13_f2 ( n, x, y );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, 'F00_F2 - Fatal error!\n' );
    fprintf ( 1, '  Illegal function index FI = %d\n', fi );
    error ( 'F00_F2 - Fatal error!' );
  end

  return
end


function [pdag, G] = learn_struct_pdag_pc_mod(cond_indep, n, k, varargin)
% LEARN_STRUCT_PDAG_PC Learn a partially oriented DAG (pattern) using the PC algorithm
% P = learn_struct_pdag_pc(cond_indep, n, k, ...)
%
% n is the number of nodes.
% k is an optional upper bound on the fan-in (default: n)
% cond_indep is a boolean function that will be called as follows:
%   feval(cond_indep, x, y, S, ...)
% where x and y are nodes, and S is a set of nodes (positive integers),
% and ... are any optional parameters passed to this function.
%
% The output P is an adjacency matrix, in which
% P(i,j) = -1 if there is an i->j edge.
% P(i,j) = P(j,i) = 1 if there is an undirected edge i <-> j
%
% The PC algorithm does structure learning assuming all variables are observed.
% See Spirtes, Glymour and Scheines, "Causation, Prediction and Search", 1993, p117.
% This algorithm may take O(n^k) time if there are n variables and k is the max fan-in,
% but this is quicker than the Verma-Pearl IC algorithm, which is always O(n^n).

sep = cell(n,n);
ord = 0;
done = 0;
G = ones(n,n);
G=setdiag(G,0);
while ~done
 done = 1;
 [X,Y] = find(G);
 for i=1:length(X)
   x = X(i); y = Y(i);
   %nbrs = mysetdiff(myunion(neighbors(G, x), neighbors(G,y)), [x y]);
   nbrs = mysetdiff(neighbors(G, y), x);  % bug fix by Raanan Yehezkel <raanany@ee.bgu.ac.il> 6/27/04
   if length(nbrs) >= ord & G(x,y) ~= 0
     done = 0;
     %SS = subsets(nbrs, ord, ord); % all subsets of size ord
     SS = subsets1(nbrs, ord);
     for si=1:length(SS)
       S = SS{si};
       if feval(cond_indep, x, y, S, varargin{:})
         %if isempty(S)
         %  fprintf('%d indep of %d ', x, y);
         %else
         %  fprintf('%d indep of %d given ', x, y); fprintf('%d ', S);
         %end
         %fprintf('\n');

         % diagnostic
         %[CI, r] = cond_indep_fisher_z(x, y, S, varargin{:});
         %fprintf(': r = %6.4f\n', r);

         G(x,y) = 0;
         G(y,x) = 0;
         sep{x,y} = myunion(sep{x,y}, S);
         sep{y,x} = myunion(sep{y,x}, S);
         break; % no need to check any more subsets
       end
     end
   end
 end
 ord = ord + 1;
end

% Create the minimal pattern,
% i.e., the only directed edges are V structures.
pdag = G;
[X, Y] = find(G);
% We want to generate all unique triples x,y,z
% This code generates x,y,z and z,y,x.
for i=1:length(X)
 x = X(i);
 y = Y(i);
 Z = find(G(y,:));
 Z = mysetdiff(Z, x);
 for z=Z(:)'
   if G(x,z)==0 & ~ismember(y, sep{x,z}) & ~ismember(y, sep{z,x})
     %fprintf('%d -> %d <- %d\n', x, y, z);
     pdag(x,y) = -1; pdag(y,x) = 0;
     pdag(z,y) = -1; pdag(y,z) = 0;
   end
 end
end

% Convert the minimal pattern to a complete one,
% i.e., every directed edge in P is compelled
% (must be directed in all Markov equivalent models),
% and every undirected edge in P is reversible.
% We use the rules of Pearl (2000) p51 (derived in Meek (1995))

old_pdag = zeros(n);
iter = 0;
while ~isequal(pdag, old_pdag)
 iter = iter + 1;
 old_pdag = pdag;
 % rule 1
 [A,B] = find(pdag==-1); % a -> b
 for i=1:length(A)
   a = A(i); b = B(i);
   C = find(pdag(b,:)==1 & G(a,:)==0); % all nodes adj to b but not a
   if ~isempty(C)
     pdag(b,C) = -1; pdag(C,b) = 0;
     %fprintf('rule 1: a=%d->b=%d and b=%d-c=%d implies %d->%d\n', a, b, b, C, b, C);
   end
 end
 % rule 2
 [A,B] = find(pdag==1); % unoriented a-b edge
 for i=1:length(A)
   a = A(i); b = B(i);
   if any( (pdag(a,:)==-1) & (pdag(:,b)==-1)' );
     pdag(a,b) = -1; pdag(b,a) = 0;
     %fprintf('rule 2: %d -> %d\n', a, b);
   end
 end
 % rule 3
 [A,B] = find(pdag==1); % a-b
 for i=1:length(A)
   a = A(i); b = B(i);
   % Bug fix by Imme Ebert-Uphoff (ebert@tree.com), Jan 2007
   % C = find( (G(a,:)==1) & (pdag(:,b)==-1)' );
   C = find( (pdag(a,:)==1) & (pdag(:,b)==-1)' );
   % C contains nodes c s.t. a-c->ba
   G2 = setdiag(G(C, C), 1);
   if any(G2(:)==0) % there are 2 different non adjacent elements of C
     pdag(a,b) = -1; pdag(b,a) = 0;
     %fprintf('rule 3: %d -> %d\n', a, b);
   end
 end
end

%  % Test Rule 3 of PC algorithm
%
%  % Define PDAG
%  pdag = zeros(4);
%  pdag(2,1)=-1;
%  pdag(3,1)=-1;
%  pdag(2,4)=-1;
%  pdag(3,4)=-1;
%  pdag(1,4)=1;
%  pdag(4,1)=1;
%
%  fprintf('\nSample input PDAG:\n');
%  pdag
%
%  fprintf('Sample DAG generated from PDAG:\n');
%  dag = abs(pdag_to_dag(pdag))
%
%  fprintf('Output from current PC algorithm:\n');
%  pdag_PC = learn_struct_pdag_pc('dsep', 4, 3, dag)
%
%  % Problem can be fixed by changing Line 120 of learn_struct_pdag_pc.m
%  %    C = find( (G(a,:)==1) & (pdag(:,b)==-1)' );
%  % to
%  %    C = find( (pdag(a,:)==1) & (pdag(:,b)==-1)' );
%
%  fprintf('Correct version:\n');
%  pdag_PC_mod = learn_struct_pdag_pc_mod('dsep', 4, 3, dag)


  function img = feldkamp(cg, ig, proj, varargin)
%|function img = feldkamp(cg, ig, proj, varargin)
%|
%| version modified by james balter and others to try accelereys "jacket"
%| UNDER DEVELOPMENT!
%|
%| FBP reconstruction of cone-beam tomography data collected with
%| a circular source trajectory.
%| See feldkamp_example.m for example.
%|
%| in:
%|	cg			ct_geom()
%|	ig			image_geom()
%|	proj	[ns,nt,na]	cone-beam projection views (line integrals)
%|
%| options:
%|	'window' [npad]		'ramp' (default), or 'hann', or array.
%|				if array, then use samples [-K/2, K/2).
%|	'offset_source'		distance from isocenter to perpendicular ray
%|				[the same units (e.g., mm) as pixel_size etc.]
%|				caution: probably should not be used
%|	'ia_skip' [int]		downsample in angle to save time for tests
%|	'use_mex' 0|1		backprojector: 0 for matlab, 1 for mex (default)
%|	'nthread'		default: jf('ncore')
%|
%| out:
%|	img	[nx,ny,nz]	reconstructed image
%|
%| References: Feldkamp, Davis, Kress, JOSA-A, 1(6):612-9, June 1984.
%| Notation here follows Fessler tomography book chapter (ask if interested).
%|
%| Copyright 2004-8-28 Nicole Caparanis, Patty Laskowsky, Taka Masuda,
%| and Jeff Fessler, University of Michigan
%| arc detector case contributed by Yingying Zhang 2005-6-13

if nargin == 1 && streq(cg, 'test')
	run_mfile_local('feldkamp_example')
return
end
if nargin < 3, ir_usage, end

if isnumeric(cg)
	img = feldkamp_old(cg, ig, proj, varargin{:});
return
end

% defaults
arg.use_mex = 1;
arg.window = 'ramp';
arg.offset_source = 0; % r_off, distance between rotation iso-center
			% and ray from source that is orthogonal to detector.
arg.ia_skip = 1;
arg.nthread = jf('ncore');
arg = vararg_pair(arg, varargin);

if cg.pitch ~= 0 || any(cg.zshifts ~= 0)
	fail('sorry, helical CT unsupported')
end

img = feldkamp_do(proj, ...
	cg.ds, cg.dt, cg.offset_s, cg.offset_t, arg.offset_source, ...
	cg.dsd, cg.dso, cg.dfs, cg.orbit, cg.orbit_start, ...
	ig.mask_or, ig.nz, ig.dx, ig.dy, ig.dz, ...
	[ig.offset_x ig.offset_y ig.offset_z], ...
	arg.window, arg.ia_skip, arg.use_mex, arg.nthread);
end % feldkamp()

 function img = feldkamp_old(proj, mask, varargin)
%function img = feldkamp_old(proj, mask, [options])
%
% FBP reconstruction of cone-beam tomography data collected with
% a circular source trajectory.
% See feldkamp_example.m for example.
%
% in:
%	proj	[ns,nt,na]	cone-beam projection views (line integrals)
%	mask	[nx,ny]		logical array of desired (2d) fov support
%
% options:
%	window	[npad]		'ramp' (default), or 'hann', or array.
%				if array, then use samples [-K/2, K/2).
%	'dx' 'dy' 'dz'		voxel dimensions
%	'nz'			# of slices
%	'center_xyz'		image center [pixels] (default: 0)
%	'ds' 'dt'		detector sample spacing (horiz, vert)
%	'offset_st'		detector offset_s,t [integer number of samples
%				or fraction thereof, for 1/4-detector offset.
%	'orbit'			default 360 degrees
%	'orbit_start'		default 0 degrees
%	'dis_src_det'
%	'dis_iso_det'
%	'dis_foc_src'		default inf (flat), or 0 for arc
%	'offset_source'		distance from isocenter to perpendicular ray
%				[the same units (e.g., mm) as pixel_size etc.]
%	'ia_skip' [int]		downsample in angle to save time for tests
%	'use_mex' 0|1		backprojector: 0 for matlab, 1 for mex (default)
%
% out:
%	img	[nx,ny,nz]		reconstructed image
%
% offset_s is an integer or fraction thereof, e.g., 0.25,
% relative to centerline between two central channels.
%
% References: Feldkamp, Davis, Kress, JOSA-A, 1(6):612-9, June 1984.
% Fessler tomography chapter.
%
% Copyright 2004-8-28 Nicole Caparanis, Patty Laskowsky, Taka Masuda,
% and Jeff Fessler, University of Michigan
% arc detector case contributed by Yingying Zhang 2005-6-13

warn 'this is obsolete use of feldkamp.m - use new style!'

% defaults
arg.use_mex = 1; % force MATLAB
arg.window = 'ramp';
arg.dx = nan;
arg.dy = [];
arg.dz = [];
arg.nz = size(proj,2);
arg.center_xyz = [0 0 0]; % center_x,y,z (pixels)
arg.flip_y = 1;

arg.dsd = []; % 'dis_src_det'
arg.dod	= []; %	'dis_iso_det'
arg.dfs = inf; % 'dis_foc_src' default flat
arg.offset_source = 0; % r_off, distance between rotation iso-center
			% and ray from source that is orthogonal to detector.

arg.ds = nan;
arg.dt = [];
arg.offset_st = [0 0]; % offset_s,t
arg.ia_skip = 1;
arg.orbit = 360;
arg.orbit_start = 0;
arg = vararg_pair(arg, varargin, 'subs', ...
	{'dis_src_det', 'dsd';
	'dis_iso_det', 'dod';
	'dis_foc_src', 'dfs'});
if isnan(arg.ds) || isnan(arg.dx), fail('ds and dx required'), end

if isempty(arg.dy), arg.dy = -arg.dx; end
if isempty(arg.dz), arg.dz = arg.dx; end
if isempty(arg.dt), arg.dt = arg.ds; end

arg.dso = arg.dsd - arg.dod; % src to "isocenter" distance

if ~isempty(arg.zshifts), error 'todo: helical feldkamp not done', end

img = feldkamp_do(proj, ...
	arg.ds, arg.dt, arg.offset_st(1), arg.offset_st(2), ...
	arg.offset_source, ...
	arg.dsd, arg.dso, arg.dfs, ...
	arg.orbit, arg.orbit_start, ...
	mask, arg.nz, arg.dx, arg.dy, arg.dz, arg.center_xyz, ...
	arg.window, arg.ia_skip, arg.use_mex, 1);

end % feldkamp_old()

%
% feldkamp_do()
%
function img = feldkamp_do(proj, ...
	ds, dt, offset_s, offset_t, offset_source, ...
	dsd, dso, dfs, orbit, orbit_start, ...
	mask, nz, dx, dy, dz, offset_xyz, ...
	window, ia_skip, use_mex, nthread)

% step 1: weight the projections as in fan-beam case
proj = feldkamp_weight1(proj, ds, dt, offset_s, offset_t, dsd, dso, dfs);

% step 2: filter the (zero padded) projections
[ns nt na] = size(proj);
if use_mex
	pad1 = 0;
else
	pad1 = 2;
end
proj = feldkamp_filter(proj, window, dsd, dfs, ds, pad1);

if pad1 % trick: zero at end saves indexing in loop
%	proj = [proj; zeros(2, nt, na)];
	proj(:,end+1,:) = 0;
end

% step 3: cone-beam backprojection of the filtered views
cpu etic
img = feldkamp_back(proj, ns, nt, na, ...
	ds, dt, offset_s, offset_t, offset_source, ...
	dsd, dso, dfs, orbit, orbit_start, ...
	mask, nz, dx, dy, dz, offset_xyz, ...
	ia_skip, use_mex, nthread);
cpu etoc 'fdk backprojection cpu time:'

end % feldkamp_do()

%
% feldkamp_weight1()
% step 1: weight the projections as in fan-beam case
%
function proj = feldkamp_weight1(proj, ds, dt, offset_s, offset_t, ...
	dsd, dso, dfs);
[ns nt na] = size(proj);
ss = ([-(ns-1)/2:(ns-1)/2]' - offset_s) * ds;
tt = ([-(nt-1)/2:(nt-1)/2]' - offset_t) * dt;

[ss tt] = ndgrid(ss, tt);
if isinf(dfs) % flat
	ww1 = dso * sqrt(1 + (tt/dsd).^2) ./ sqrt(dsd^2 + ss.^2 + tt.^2);
elseif dfs == 0 % arc
	ww1 = (dso/dsd) * cos(ss ./ (dsd * sqrt(1 + (tt/dsd).^2)));
else
	error 'other configurations not implemented'
end

for ia=1:na % same weighting for each view angle
	proj(:,:,ia) = proj(:,:,ia) .* ww1;
end
end % feldkamp_weight1()

%
% feldkamp_filter()
% step 2: filter the (zero padded) projections
%
function proj = feldkamp_filter(proj, window, dsd, dfs, ds, pad1)
[ns nt na] = size(proj);
npadh = 2^ceil(log2(2*ns-1));
printf('ns=%d npadh=%d', ns, npadh)

if isinf(dfs)
	H = fan_filter('flat', npadh, ds, [], window);	% [nb,1]
elseif dfs == 0
	H = fan_filter('arc', npadh, ds, dsd, window);
end
H = ds * H; % differential for discrete-space convolution vs integral

%% START JACKET
GPU = 1;
if GPU,
    proj = gsingle( proj );
    H = gsingle( H );
    proj_large = gzeros( npadh, size(proj,2), size(proj,3) );
else,
    proj_large = zeros( npadh, size(proj,2), size(proj,3) );
end
%% END JACKET

tic;
proj_large(1:size(proj,1),:,:) = proj;
proj = ifft( fft(proj_large) .* repmat(H, [1 nt na]) );
proj = proj(1:(ns+pad1),:,:); % trick: extra zero at end saves indexing in loop
toc

%% START JACKET
if GPU,
    proj = double( proj );
end
%% END JACKET

end % feldkamp_filter()

%
% fan_filter()
% apodized filter frequency response
%
function H = fan_filter(type, n, ds, dsd, window)

if streq(type, 'flat')
	h = fbp_ramp('flat', n, ds);
else
	h = fbp_ramp('arc', n, ds, dsd);
end
H = reale(fft(fftshift(h)));

if ischar(window)
	if streq(window, 'ramp')
		window = ones(n,1);
	elseif streq(window, 'hann')
		window = ir_hann_periodic(n);
	else
		error 'unknown window'
	end
elseif length(window) ~= n
	error 'bad window length'
end

H = H .* fftshift(window);
end % fan_filter()

%
% feldkamp_back()
% step 3: cone-beam backprojection of the filtered views
%
function img = feldkamp_back(proj, ns, nt, na, ...
	ds, dt, offset_s, offset_t, offset_source, ...
	dsd, dso, dfs, orbit, orbit_start, ...
	mask, nz, dx, dy, dz, offset_xyz, ...
	ia_skip, use_mex, nthread)
[nx ny] = size(mask);

betas = deg2rad(orbit_start + orbit * [0:na-1] / na); % [na] source angles

% mex backprojector
if use_mex
	proj = single(proj);
	nthread = int32(nthread);

	if use_mex == 1 % loop in mex
		proj = permute(proj, [2 1 3]); % ts
		tmp = jf_mex('fdk,ts,back', int32([nx ny nz]), [dx dy dz], ...
				offset_xyz, uint8(mask), ...
				dso, dsd, dfs, [ds dt], [offset_s offset_t], ...
				proj, betas, nthread);
		img = double6(tmp);
		img = permute(img, [2 3 1]); % zxy -> xyz

	elseif use_mex == 2 % loop in matlab (for testing)
		proj = permute(proj, [2 1 3]); % ts
		img = 0;
		for ia=1:na
			ticker(mfilename, ia, na)
			% note: 2006-5-30: replaced -dy with dy
			tmp = jf_mex('fdk,ts,back', ...
				int32([nx ny nz]), [dx dy dz], ...
				offset_xyz, uint8(mask), ...
				dso, dsd, dfs, [ds dt], [offset_s offset_t], ...
				proj(:,:,ia), betas(ia), nthread);
			tmp = double6(tmp);
			img = img + tmp;
		end
		img = permute(img, [2 3 1]); % zxy -> xyz

	elseif use_mex == 3 % fdk,st (for testing only - slower!)
		tmp = jf_mex('fdk,st,back', int32([nx ny nz]), [dx dy dz], ...
				offset_xyz, uint8(mask), ...
				dso, dsd, dfs, [ds dt], [offset_s offset_t], ...
				proj, betas, nthread);
		img = double6(tmp);

	else
		fail 'bug'
	end

	% final "\der angle" scale:
	img = (0.5 * deg2rad(abs(orbit)) / (na/ia_skip)) * img;
return
end

% matlab backprojector (slower)

fprintf('We are using slower version!\n');

% precompute as much as possible
wx = (nx-1)/2 + offset_xyz(1);
wy = (ny-1)/2 + offset_xyz(2);
wz = (nz-1)/2 + offset_xyz(3);
[xc yc] = ndgrid(([0:nx-1] - wx) * dx, ([0:ny-1] - wy) * dy);
zc = ([0:nz-1] - wz) * dz;

if 0 % limit back-projection to FOV?  removed 2008-10-9
	rr = sqrt(xc.^2 + yc.^2); % [nx,ny]
	smax = ((ns-1)/2-abs(offset_s)) * ds; % maximum detector s coordinate

	if isinf(dfs)
		gamma_max = atan(smax/dsd);
	elseif dfs == 0
		gamma_max = smax / dsd;
	end

	rmax = dso * sin(gamma_max);
	mask = mask & (rr < rmax);
end
clear wx wy wz rr smax rmax

xc = xc(mask); % [np] pixels within mask
yc = yc(mask);

% loop over slices
GPU = 0;
if GPU,
    img = gzeros([size(mask) nz]) + i;
    img = img - i;
else,
    img = zeros([size(mask) nz]);
end;
ticker reset
tic;
for iz=1:nz

	% loop over each projection angle
	img2 = 0;
	for ia=1:na
		ticker(mfilename, [iz ia], [nz na])
		beta = betas(ia);

		x_beta = +xc * cos(beta) + yc * sin(beta);
		y_beta = -xc * sin(beta) + yc * cos(beta);

		% detector indices
		mag = dsd ./ (dso - y_beta);

		if isinf(dfs)
			sprime = mag .* x_beta;
		elseif (dfs == 0)
			d_loop = dso - y_beta;
			r_loop = x_beta - offset_source;
			sprime = dsd * atan2(r_loop, d_loop);
		end

		tprime = mag * zc(iz);
		bh = sprime / ds + (ns+1)/2 + offset_s;
		bv = tprime / dt + (nt+1)/2 + offset_t;

		% bi-linear interpolation:
		ih = floor(bh); % left bin
		iv = floor(bv);
		igood = 1<=ih & ih<ns & 1<= iv & iv<nt;

		ih(~igood) = ns+1; % trick! point at harmless zeros
		iv(~igood) = 1;

		wr = bh - ih;	% left weight
		wl = 1 - wr;	% right weight
		wu = bv - iv;	% upper weight
		wd = 1 - wu;	% lower weight

		sdim = size(proj);
		ia1 = ia * ones(size(ih));

		p1 =	wl .* proj(sub2ind(sdim, ih,iv,ia1)) + ...
			wr .* proj(sub2ind(sdim, ih+1,iv,ia1));
		p2 =	wl .* proj(sub2ind(sdim, ih,iv+1,ia1)) + ...
			wr .* proj(sub2ind(sdim, ih+1,iv+1,ia1));

		p0 = wu .* p1 + wd .* p2; % vertical interpolation

		if isinf(dfs)
			p0 = p0 .* mag.^2; % back-projection weighting for flat
		elseif dfs == 0
			p0 = p0 .* (dsd.^2) ./ (r_loop.^2 + d_loop.^2);
		end

		img2 = img2 + p0;
	end % ia

	img(:,:,iz) = embed(img2, mask);
end % iz
toc

img = (0.5 * deg2rad(orbit) / (na/ia_skip)) * img; % final "\der angle" scale
img = double( img );

end % feldkamp_back()


function w = K_symmetrised(psi,q1,q2,CS,SS,varargin)
% evaluate kernel modulo symmetries
%
% Input
%  psi    - @SO3Kernel
%  q1, q2 - @quaternion(s)
%  CS, SS - crystal , specimen @symmetry
%
% Options
%  exact -
%  epsilon -
%
% Description
% K(q1,q2) = Sum(S) Sum(l) A_l Tr T_l(s1^-1 q1 s2)

% only the pur rotational part is of interest
% TODO
qCS = unique(quaternion(CS));
qSS = unique(quaternion(SS));

if check_option(varargin,'exact')
  epsilon = pi;
else
  epsilon = min(pi,get_option(varargin,'epsilon',psi.halfwidth*3.5));
end

% how to use sparse matrix representation
if isa(q1,'SO3Grid')
  lg1 = length(q1);
else
  lg1 = -length(q1);
end
if isa(q2,'SO3Grid')
  lg2 = length(q2);
else
  lg2 = -length(q2);
end

if epsilon>2*pi/CS.Laue.multiplicityZ % full matrixes

  q1 = quaternion(q1);
  q2 = quaternion(q2);
  w = zeros(length(q1),length(q2));

	for iks = 1:length(qCS)
		for ips = 1:length(qSS) % for all symmetries

			sg    = qSS(ips) * q1 * qCS(iks);  % rotate g1
      omega = abs(dot_outer(sg,q2));      % calculate full distance matrix
      w = w + psi.eval(omega);

		end
  end

elseif (lg1>0 || lg2>0) && ~check_option(varargin,'old')

  w = sparse(abs(lg1),abs(lg2));

  % sum over specimen symmetry
  if (lg1 >= lg2)              % first argument is SO3Grid
    for issq = 1:length(qSS)
      d = abs(dot_outer(q1,qSS(issq)*quaternion(q2),'epsilon',epsilon,...
        'nospecimensymmetry'));
        w = w + spfun(@psi.eval,d);
    end
  else                         % second argument is SO3Grid
    for issq = 1:length(qSS)
      d = abs(dot_outer(q2,qSS(issq)*quaternion(q1),'epsilon',epsilon,...
        'nospecimensymmetry'));
      w = w + spfun(@psi.eval,d.');
    end
  end

else

	q1 = quaternion(q1);
	q2 = quaternion(q2);

	w = sparse(length(q1),length(q2));

	for iks = 1:length(qCS)
		for ips = 1:length(qSS) % for all symmetries

      if abs(lg1) > abs(lg2)
        sg    = qSS(ips) * q2 * qCS(iks);  % rotate g1
        omega = abs(dot_outer(q1,sg));      % calculate full distance matrix
      else
        sg    = qSS(ips) * q1 * qCS(iks);  % rotate g1
        omega = abs(dot_outer(sg,q2));      % calculate full distance matrix
      end

%  z = find(omega>cos(epsilon));
%  if length(z) > length(omega)/length(CS)/10, w = full(w); end
%  w(z) = w(z) +  kk.K(omega(z));

%  if length(z) > numel(omega)/length(CS)/10, w = full(w); end

      [y,x] = find(omega>cos(epsilon));
      dummy = sparse(y,x,psi.eval(omega(sub2ind(size(w),y,x))),length(q1),length(q2));

      w = w + dummy;

		end
  end

end
%nnz(w)
w = w / length(qCS) / length(qSS);


function amin_index = i4vec_amin_index ( n, a )

%*****************************************************************************80
%
%% I4VEC_AMIN_INDEX returns the index of the smallest magnitude in an I4VEC.
%
%  Discussion:
%
%    An I4VEC is a vector of integer values.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 November 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries to be checked.
%
%    Input, integer A(N), the vector to be checked.
%
%    Output, integer AMIN_INDEX, the entry of the smallest magnitude.
%
  if ( n <= 0 )

    amin_index = 0;

  else

    aamin = a(1);
    amin_index = 1;

    for i = 2 : n

      if ( abs ( a(i) ) < aamin )
        aamin = abs ( a(i) );
        amin_index = i;
      end

    end

  end

  return
end


function Ft=frametight(F);
%FRAMETIGHT  Construct the canonical tight frame
%   Usage: Ft=frametight(F);
%
%   `Ft=frametight(F)` returns the canonical tight frame of *F*.
%
%   The canonical tight frame can be used to get perfect reconstruction if
%   it is used for both analysis and synthesis. This is demonstrated in the
%   following example:::
%
%     % Create a frame and its canonical tight
%     F=frame('dgt','hamming',32,64);
%     Ft=frametight(F);
%
%     % Compute the frame coefficients and test for perfect
%     % reconstruction
%     f=gspi;
%     c=frana(Ft,f);
%     r=frsyn(Ft,c);
%     norm(r(1:length(f))-f)
%
%   See also: frame, framepair, framedual

complainif_notenoughargs(nargin,1,'FRAMETIGHT');
complainif_notvalidframeobj(F,'FRAMETIGHT');

% Default operation, works for a lot of frames
Ft=F;

% Handle the windowed transforms
switch(F.type)
  case {'dgt','dgtreal','dwilt','wmdct','filterbank','ufilterbank',...
        'nsdgt','unsdgt','nsdgtreal','unsdgtreal'}

    Ft=frame(F.type,{'tight',F.g},F.origargs{2:end});

  case {'filterbankreal','ufilterbankreal'}
    Ft=frame(F.type,{'realtight',F.g},F.origargs{2:end});

  case 'gen'
    [U,sv,V] = svd(F.g,'econ');
    Ft=frame('gen',U*V');

  case 'tensor'
    for ii=1:F.Nframes
        tight_frames{ii}=frametight(F.frames{ii});
    end;
    F=frame('tensor',tight_frames{:});

  case 'fusion'
    tight_w=1./F.w;
    for ii=1:F.Nframes
        tight_frames{ii}=frametight(F.frames{ii});
    end;
    Ft=frame('fusion',tight_w,tight_frames{:});

  case 'ufwt'
    % The canonical tight made from ufwt might not keep the iterated
    % filterbank structure
    [g,a] = wfbt2filterbank({F.g,F.J,'dwt'});
    g = comp_filterbankscale(g,a,F.flags.scaling);

    Ft = frametight(frame('filterbank',g,ones(numel(g),1),numel(g)));

  case 'uwfbt'
    % The canonical tight made from uwfbt might not keep the iterated
    % filterbank structure
    [g,a] = wfbt2filterbank(F.g,F.J);
    g = comp_filterbankscale(g,a,F.flags.scaling);

    Ft = frametight(frame('filterbank',g,ones(numel(g),1),numel(g)));

  case 'uwpfbt'
    % The canonical tight made from uwpfbt might not keep the iterated
    % filterbank structure
    [g, a] = wpfbt2filterbank(F.g,F.flags.interscaling);
    g = comp_filterbankscale(g,a,F.flags.scaling);

    Ft = frametight(frame('filterbank',g,ones(numel(g),1),numel(g)));

  case 'fwt'
    is_basis = abs(sum(1./F.g.a)-1)<1e-6;
    is_tight = F.info.istight;

    if is_basis && is_tight
        Ft = F;
    else
        error(['%s: Cannot create the canonical tight frame with the ',...
               'same structure. Consider casting the system to an ',...
               'uniform filterbank.'],...
               upper(mfilename));
    end

  case 'wfbt'
    is_basis = all(cellfun(@(nEl) abs(sum(1./nEl.a)-1)<1e-6,F.g.nodes));
    is_tight = F.info.istight;

    if is_basis && is_tight
        Ft = F;
    else
        error(['%s: Cannot create the canonical tight frame with the ',...
               'same structure. Consider casting the system to an ',...
               'uniform filterbank.'],...
               upper(mfilename));
    end

   case 'wpfbt'
     % WPFBT is too wierd.
     error(['%s: Canonical tight frame of wpfbt might not keep the ',...
           'same structure. '],upper(mfilename))

end;

switch(F.type)
    case {'ufwt','uwfbt','uwpfbt'}
      warning(sprintf(['%s: The canonical tight system does not preserve ',...
                     'the iterated filterbank structure.'],...
                      upper(mfilename)));
end

% Treat the fixed length frames
if isfield(F,'fixedlength') && F.fixedlength && isfield(F,'L')
   Ft = frameaccel(Ft,F.L);
   Ft.fixedlength = 1;
end


function Bh = hmxMldivide(Mh,Bh)
%+========================================================================+
%|                                                                        |
%|         OPENHMX - LIBRARY FOR H-MATRIX COMPRESSION AND ALGEBRA         |
%|           openHmx is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : hmxMldivide.m                                 |
%|    #    |   VERSION    : 0.51                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 14.01.2019                                    |
%| ( === ) |   SYNOPSIS   : Solve Mh x = Bh with the rule                 |
%|  `---'  |                Compr > Full > H-Matrix                       |
%+========================================================================+

% Check dimensions
if (size(Mh,1) ~= size(Bh,1))
    error('hmxMldivide.m : matrix dimensions must agree.')
end

% Check input data
if ~isa(Mh,'hmx')
    error('hmxMldivide.m : unavailable case.')
end

%%% Lower H-Matrix \ H-Matrix -> H-Matrix
if islower(Mh) && isa(Bh,'hmx')
    % H-Matrix \ H-Matrix -> H-Matrix (recursion)
    if (Mh.typ == 0) && (Bh.typ == 0)
        % X11 -> L11 \ B11
        Bh.chd{1} = hmxMldivide(Mh.chd{1},Bh.chd{1});

        % X12 -> L11 \ B12
        Bh.chd{2} = hmxMldivide(Mh.chd{1},Bh.chd{2});

        % X21 -> L22 \ (B21 - L21*X11)
        Bh.chd{3} = Bh.chd{3} - Mh.chd{3} * Bh.chd{1};
        Bh.chd{3} = hmxMldivide(Mh.chd{4},Bh.chd{3});

        % X22 -> L22 \ (B22 - L21 * X12)
        Bh.chd{4} = Bh.chd{4} - Mh.chd{3} * Bh.chd{2};
        Bh.chd{4} = hmxMldivide(Mh.chd{4},Bh.chd{4});

        % Fusion
        Bh = hmxFusion(Bh);

    % H-Matrix \ Compr -> Compr
    elseif (Mh.typ == 0) && (Bh.typ == 1)
        Bh.dat = { hmxMldivide(Mh,Bh.dat{1}) , Bh.dat{2} };

    % H-Matrix \ Full -> Full
    elseif (Mh.typ == 0) && (Bh.typ == 2)
        Bh.typ = 2;
        Bh.dat = hmxMldivide(Mh,Bh.dat);

    % Compr \ --- -> ---
    elseif (Mh.typ == 1)
        error('hmxMldivide : unavailable case')

    % Full \ H-Matrix -> Unavailable
    elseif (Mh.typ == 2) && (Bh.typ == 0)
        error('hmxMldivide : unavailable case')

    % Full \ Compr -> Compr
    elseif (Mh.typ == 2) && (Bh.typ == 1)
        Bh.dat = { Mh.dat\Bh.dat{1} , Bh.dat{2} };

    % Full \ Full -> Full
    elseif (Bh.typ == 2)
        Bh.dat = Mh.dat \ Bh.dat;

    else
        error('hmxMldivide : unavailable case')
    end

%%% Upper H-Matrix \ H-Matrix -> H-Matrix
elseif isupper(Mh) && isa(Bh,'hmx')
    % H-Matrix \ H-Matrix -> H-Matrix (recursion)
    if (Mh.typ == 0) && (Bh.typ == 0)
        % X22 -> U22 \ B22
        Bh.chd{4} = hmxMldivide(Mh.chd{4},Bh.chd{4});

        % X21 -> U22 \ B21
        Bh.chd{3} = hmxMldivide(Mh.chd{4},Bh.chd{3});

        % X12 -> U11 \ (B12 - U12*X22)
        Bh.chd{2} = Bh.chd{2} - Mh.chd{2} * Bh.chd{4};
        Bh.chd{2} = hmxMldivide(Mh.chd{1},Bh.chd{2});

        % X11 -> U11 \ (B11 - U12 * X21)
        Bh.chd{1} = Bh.chd{1} - Mh.chd{2} * Bh.chd{3};
        Bh.chd{1} = hmxMldivide(Mh.chd{1},Bh.chd{1});

        % Fusion
        Bh = hmxFusion(Bh);

    % H-Matrix \ Compr -> Compr
    elseif (Mh.typ == 0) && (Bh.typ == 1)
        Bh.dat = { hmxMldivide(Mh,Bh.dat{1}) , Bh.dat{2} };

    % H-Matrix \ Full -> Full
    elseif (Mh.typ == 0) && (Bh.typ == 2)
        Bh.typ = 2;
        Bh.dat = hmxMldivide(Mh,Bh.dat);

    % Compr \ --- -> ---
    elseif (Mh.typ == 1)
        error('hmxMldivide : unavailable case')

    % Full \ H-Matrix -> Unavailable
    elseif (Mh.typ == 2) && (Bh.typ == 0)
        error('hmxMldivide : unavailable case')

    % Full \ Compr -> Compr
    elseif (Mh.typ == 2) && (Bh.typ == 1)
        Bh.dat = { Mh.dat\Bh.dat{1} , Bh.dat{2} };

    % Full \ Full -> Full
    elseif (Bh.typ == 2)
        Bh.dat = Mh.dat \ Bh.dat;

    else
        error('hmxMldivide : unavailable case')
    end

%%% Lower H-Matrix \ Matrix -> Matrix
elseif islower(Mh)
    if (size(Bh,2) > 0)
        % H-Matrix (recursion)
        if (Mh.typ == 0)
            % X1 -> L11 \ B1
            X1 = hmxMldivide(Mh.chd{1},Bh(Mh.row{1},:));

            % X2 -> L22 \ (B2 - L21*X1)
            X2 = Bh(Mh.row{3},:) - Mh.chd{3}*X1;
            X2 = hmxMldivide(Mh.chd{4},X2);

            % Bh = [X1 X2]
            if issparse(X1)
                Bh = sparse(size(Mh,1),size(Bh,2));
            else
                Bh = zeros(size(Mh,1),size(Bh,2),class(X1));
            end
            Bh(Mh.col{1},:) = X1;
            Bh(Mh.col{2},:) = X2;

        % Compressed leaf
        elseif (Mh.typ == 1)
            error('hmxMldivide : unavailable case')

        % Full leaf
        elseif (Mh.typ == 2)
            Bh = Mh.dat \ Bh;

        % Unknown type
        else
            error('hmxMldivide : unavailable case')
        end
    else
        Bh = zeros(size(Mh,1),0);
    end

%%% Upper H-Matrix \ Matrix -> Matrix
elseif isupper(Mh)
    if (size(Bh,2) > 0)
        % H-Matrix (recursion)
        if (Mh.typ == 0)
            % X2 -> U22 \ B2
            X2 = hmxMldivide(Mh.chd{4},Bh(Mh.row{4},:));

            % X1 -> U11 \ (B1 - U12*X2)
            X1 = Bh(Mh.row{1},:) - Mh.chd{2}*X2;
            X1 = hmxMldivide(Mh.chd{1},X1);

            % Bh = [X1 ; X2]
            if issparse(X1)
                Bh = sparse(size(Mh,1),size(Bh,2));
            else
                Bh = zeros(size(Mh,1),size(Bh,2),class(X1));
            end
            Bh(Mh.col{1},:) = X1;
            Bh(Mh.col{2},:) = X2;

        % Compressed leaf
        elseif (Mh.typ == 1)
            error('hmxMldivide : unavailable case')

        % Full leaf
        elseif (Mh.typ == 2)
            Bh = Mh.dat \ Bh;

        % Unknown type
        else
            error('hmxMldivide : unavailable case')
        end
    else
        Bh = zeros(size(Mh,1),0);
    end

%%% H-Matrix \ (H-)Matrix -> (H-)Matrix
else
    [Lh,Uh] = lu(Mh);
    Bh      = Uh\(Lh\Bh);
end