function [K] = ku0u0(x, xp, hyp, ubar, ubarp, dt, i)

logsigma = hyp(1);
logtheta = hyp(2);

a1 = hyp(3);
a2 = hyp(4);
a3 = hyp(5);
a4 = hyp(6);

n_x = size(x,1);
n_xp = size(xp,1);

x = repmat(x,1,n_xp);
xp = repmat(xp',n_x,1);

ubar = repmat(ubar,1,n_xp);
ubarp = repmat(ubarp',n_x,1);

switch i

case 0

K=exp(1).^(logsigma+(-8).*logtheta+(-1/2).*exp(1).^((-1).*logtheta).*(x+( ...
  -1).*xp).^2).*(exp(1).^(2.*logtheta).*(exp(1).^(2.*logtheta).*(exp(1).^( ...
  2.*logtheta).*(exp(1).^(2.*logtheta)+a1.*dt.*exp(1).^logtheta.*(a1.*dt.* ...
  ubar.*ubarp+(-1).*(ubar+(-1).*ubarp).*(x+(-1).*xp))+(-1).*a1.^2.*dt.^2.* ...
  ubar.*ubarp.*(x+(-1).*xp).^2)+a2.*dt.*exp(1).^logtheta.*((-2).*exp(1).^( ...
  2.*logtheta)+exp(1).^logtheta.*(3.*a1.*dt.*(ubar+(-1).*ubarp)+2.*(x+(-1) ...
  .*xp)).*(x+(-1).*xp)+(-1).*a1.*dt.*(ubar+(-1).*ubarp).*(x+(-1).*xp).^3)+ ...
  a2.^2.*dt.^2.*(3.*exp(1).^(2.*logtheta)+(-6).*exp(1).^logtheta.*(x+(-1) ...
  .*xp).^2+(x+(-1).*xp).^4))+(-1).*a1.*a3.*dt.^2.*exp(1).^(2.*logtheta).*( ...
  ubar+ubarp).*(3.*exp(1).^(2.*logtheta)+(-6).*exp(1).^logtheta.*(x+(-1).* ...
  xp).^2+(x+(-1).*xp).^4)+a3.^2.*dt.^2.*(15.*exp(1).^(3.*logtheta)+(-45).* ...
  exp(1).^(2.*logtheta).*(x+(-1).*xp).^2+15.*exp(1).^logtheta.*(x+(-1).* ...
  xp).^4+(-1).*(x+(-1).*xp).^6))+a4.*dt.*exp(1).^(2.*logtheta).*((-2).* ...
  a2.*dt.*(15.*exp(1).^(3.*logtheta)+(-45).*exp(1).^(2.*logtheta).*(x+(-1) ...
  .*xp).^2+15.*exp(1).^logtheta.*(x+(-1).*xp).^4+(-1).*(x+(-1).*xp).^6)+ ...
  exp(1).^logtheta.*(6.*exp(1).^(3.*logtheta)+(-3).*exp(1).^(2.*logtheta) ...
  .*(5.*a1.*dt.*(ubar+(-1).*ubarp)+4.*(x+(-1).*xp)).*(x+(-1).*xp)+(-1).* ...
  a1.*dt.*(ubar+(-1).*ubarp).*(x+(-1).*xp).^5+2.*exp(1).^logtheta.*(x+(-1) ...
  .*xp).^3.*(5.*a1.*dt.*(ubar+(-1).*ubarp)+x+(-1).*xp)))+a4.^2.*dt.^2.*( ...
  105.*exp(1).^(4.*logtheta)+(-420).*exp(1).^(3.*logtheta).*(x+(-1).*xp) ...
  .^2+210.*exp(1).^(2.*logtheta).*(x+(-1).*xp).^4+(-28).*exp(1) ...
  .^logtheta.*(x+(-1).*xp).^6+(x+(-1).*xp).^8));

case 1 % logsigma

K=exp(1).^(logsigma+(-8).*logtheta+(-1/2).*exp(1).^((-1).*logtheta).*(x+( ...
  -1).*xp).^2).*(exp(1).^(2.*logtheta).*(exp(1).^(2.*logtheta).*(exp(1).^( ...
  2.*logtheta).*(exp(1).^(2.*logtheta)+a1.*dt.*exp(1).^logtheta.*(a1.*dt.* ...
  ubar.*ubarp+(-1).*(ubar+(-1).*ubarp).*(x+(-1).*xp))+(-1).*a1.^2.*dt.^2.* ...
  ubar.*ubarp.*(x+(-1).*xp).^2)+a2.*dt.*exp(1).^logtheta.*((-2).*exp(1).^( ...
  2.*logtheta)+exp(1).^logtheta.*(3.*a1.*dt.*(ubar+(-1).*ubarp)+2.*(x+(-1) ...
  .*xp)).*(x+(-1).*xp)+(-1).*a1.*dt.*(ubar+(-1).*ubarp).*(x+(-1).*xp).^3)+ ...
  a2.^2.*dt.^2.*(3.*exp(1).^(2.*logtheta)+(-6).*exp(1).^logtheta.*(x+(-1) ...
  .*xp).^2+(x+(-1).*xp).^4))+(-1).*a1.*a3.*dt.^2.*exp(1).^(2.*logtheta).*( ...
  ubar+ubarp).*(3.*exp(1).^(2.*logtheta)+(-6).*exp(1).^logtheta.*(x+(-1).* ...
  xp).^2+(x+(-1).*xp).^4)+a3.^2.*dt.^2.*(15.*exp(1).^(3.*logtheta)+(-45).* ...
  exp(1).^(2.*logtheta).*(x+(-1).*xp).^2+15.*exp(1).^logtheta.*(x+(-1).* ...
  xp).^4+(-1).*(x+(-1).*xp).^6))+a4.*dt.*exp(1).^(2.*logtheta).*((-2).* ...
  a2.*dt.*(15.*exp(1).^(3.*logtheta)+(-45).*exp(1).^(2.*logtheta).*(x+(-1) ...
  .*xp).^2+15.*exp(1).^logtheta.*(x+(-1).*xp).^4+(-1).*(x+(-1).*xp).^6)+ ...
  exp(1).^logtheta.*(6.*exp(1).^(3.*logtheta)+(-3).*exp(1).^(2.*logtheta) ...
  .*(5.*a1.*dt.*(ubar+(-1).*ubarp)+4.*(x+(-1).*xp)).*(x+(-1).*xp)+(-1).* ...
  a1.*dt.*(ubar+(-1).*ubarp).*(x+(-1).*xp).^5+2.*exp(1).^logtheta.*(x+(-1) ...
  .*xp).^3.*(5.*a1.*dt.*(ubar+(-1).*ubarp)+x+(-1).*xp)))+a4.^2.*dt.^2.*( ...
  105.*exp(1).^(4.*logtheta)+(-420).*exp(1).^(3.*logtheta).*(x+(-1).*xp) ...
  .^2+210.*exp(1).^(2.*logtheta).*(x+(-1).*xp).^4+(-28).*exp(1) ...
  .^logtheta.*(x+(-1).*xp).^6+(x+(-1).*xp).^8));

case 2 % logtheta

K=exp(1).^(logsigma+(-8).*logtheta+(-1/2).*exp(1).^((-1).*logtheta).*(x+( ...
  -1).*xp).^2).*(exp(1).^(3.*logtheta).*(exp(1).^(2.*logtheta).*(6.* ...
  a2.^2.*dt.^2.*(exp(1).^logtheta+(-1).*(x+(-1).*xp).^2)+exp(1) ...
  .^logtheta.*(4.*exp(1).^(2.*logtheta)+3.*a1.*dt.*exp(1).^logtheta.*(a1.* ...
  dt.*ubar.*ubarp+(-1).*(ubar+(-1).*ubarp).*(x+(-1).*xp))+(-2).*a1.^2.* ...
  dt.^2.*ubar.*ubarp.*(x+(-1).*xp).^2)+a2.*dt.*((-6).*exp(1).^(2.* ...
  logtheta)+2.*exp(1).^logtheta.*(3.*a1.*dt.*(ubar+(-1).*ubarp)+2.*(x+(-1) ...
  .*xp)).*(x+(-1).*xp)+(-1).*a1.*dt.*(ubar+(-1).*ubarp).*(x+(-1).*xp).^3)) ...
  +2.*exp(1).^logtheta.*(exp(1).^(2.*logtheta).*(exp(1).^(2.*logtheta)+ ...
  a1.*dt.*exp(1).^logtheta.*(a1.*dt.*ubar.*ubarp+(-1).*(ubar+(-1).*ubarp) ...
  .*(x+(-1).*xp))+(-1).*a1.^2.*dt.^2.*ubar.*ubarp.*(x+(-1).*xp).^2)+a2.* ...
  dt.*exp(1).^logtheta.*((-2).*exp(1).^(2.*logtheta)+exp(1).^logtheta.*( ...
  3.*a1.*dt.*(ubar+(-1).*ubarp)+2.*(x+(-1).*xp)).*(x+(-1).*xp)+(-1).*a1.* ...
  dt.*(ubar+(-1).*ubarp).*(x+(-1).*xp).^3)+a2.^2.*dt.^2.*(3.*exp(1).^(2.* ...
  logtheta)+(-6).*exp(1).^logtheta.*(x+(-1).*xp).^2+(x+(-1).*xp).^4))+(-6) ...
  .*a1.*a3.*dt.^2.*exp(1).^(2.*logtheta).*(ubar+ubarp).*(exp(1).^logtheta+ ...
  (-1).*(x+(-1).*xp).^2)+15.*a3.^2.*dt.^2.*(3.*exp(1).^(2.*logtheta)+(-6) ...
  .*exp(1).^logtheta.*(x+(-1).*xp).^2+(x+(-1).*xp).^4)+(-2).*a1.*a3.* ...
  dt.^2.*exp(1).^logtheta.*(ubar+ubarp).*(3.*exp(1).^(2.*logtheta)+(-6).* ...
  exp(1).^logtheta.*(x+(-1).*xp).^2+(x+(-1).*xp).^4))+2.*exp(1).^(2.* ...
  logtheta).*(exp(1).^(2.*logtheta).*(exp(1).^(2.*logtheta).*(exp(1).^(2.* ...
  logtheta)+a1.*dt.*exp(1).^logtheta.*(a1.*dt.*ubar.*ubarp+(-1).*(ubar+( ...
  -1).*ubarp).*(x+(-1).*xp))+(-1).*a1.^2.*dt.^2.*ubar.*ubarp.*(x+(-1).*xp) ...
  .^2)+a2.*dt.*exp(1).^logtheta.*((-2).*exp(1).^(2.*logtheta)+exp(1) ...
  .^logtheta.*(3.*a1.*dt.*(ubar+(-1).*ubarp)+2.*(x+(-1).*xp)).*(x+(-1).* ...
  xp)+(-1).*a1.*dt.*(ubar+(-1).*ubarp).*(x+(-1).*xp).^3)+a2.^2.*dt.^2.*( ...
  3.*exp(1).^(2.*logtheta)+(-6).*exp(1).^logtheta.*(x+(-1).*xp).^2+(x+(-1) ...
  .*xp).^4))+(-1).*a1.*a3.*dt.^2.*exp(1).^(2.*logtheta).*(ubar+ubarp).*( ...
  3.*exp(1).^(2.*logtheta)+(-6).*exp(1).^logtheta.*(x+(-1).*xp).^2+(x+(-1) ...
  .*xp).^4)+a3.^2.*dt.^2.*(15.*exp(1).^(3.*logtheta)+(-45).*exp(1).^(2.* ...
  logtheta).*(x+(-1).*xp).^2+15.*exp(1).^logtheta.*(x+(-1).*xp).^4+(-1).*( ...
  x+(-1).*xp).^6))+2.*a4.*dt.*exp(1).^(2.*logtheta).*((-2).*a2.*dt.*(15.* ...
  exp(1).^(3.*logtheta)+(-45).*exp(1).^(2.*logtheta).*(x+(-1).*xp).^2+15.* ...
  exp(1).^logtheta.*(x+(-1).*xp).^4+(-1).*(x+(-1).*xp).^6)+exp(1) ...
  .^logtheta.*(6.*exp(1).^(3.*logtheta)+(-3).*exp(1).^(2.*logtheta).*(5.* ...
  a1.*dt.*(ubar+(-1).*ubarp)+4.*(x+(-1).*xp)).*(x+(-1).*xp)+(-1).*a1.*dt.* ...
  (ubar+(-1).*ubarp).*(x+(-1).*xp).^5+2.*exp(1).^logtheta.*(x+(-1).*xp) ...
  .^3.*(5.*a1.*dt.*(ubar+(-1).*ubarp)+x+(-1).*xp)))+(exp(1).^(2.*logtheta) ...
  .*(exp(1).^(2.*logtheta).*(exp(1).^(2.*logtheta).*(exp(1).^(2.*logtheta) ...
  +a1.*dt.*exp(1).^logtheta.*(a1.*dt.*ubar.*ubarp+(-1).*(ubar+(-1).*ubarp) ...
  .*(x+(-1).*xp))+(-1).*a1.^2.*dt.^2.*ubar.*ubarp.*(x+(-1).*xp).^2)+a2.* ...
  dt.*exp(1).^logtheta.*((-2).*exp(1).^(2.*logtheta)+exp(1).^logtheta.*( ...
  3.*a1.*dt.*(ubar+(-1).*ubarp)+2.*(x+(-1).*xp)).*(x+(-1).*xp)+(-1).*a1.* ...
  dt.*(ubar+(-1).*ubarp).*(x+(-1).*xp).^3)+a2.^2.*dt.^2.*(3.*exp(1).^(2.* ...
  logtheta)+(-6).*exp(1).^logtheta.*(x+(-1).*xp).^2+(x+(-1).*xp).^4))+(-1) ...
  .*a1.*a3.*dt.^2.*exp(1).^(2.*logtheta).*(ubar+ubarp).*(3.*exp(1).^(2.* ...
  logtheta)+(-6).*exp(1).^logtheta.*(x+(-1).*xp).^2+(x+(-1).*xp).^4)+ ...
  a3.^2.*dt.^2.*(15.*exp(1).^(3.*logtheta)+(-45).*exp(1).^(2.*logtheta).*( ...
  x+(-1).*xp).^2+15.*exp(1).^logtheta.*(x+(-1).*xp).^4+(-1).*(x+(-1).*xp) ...
  .^6))+a4.*dt.*exp(1).^(2.*logtheta).*((-2).*a2.*dt.*(15.*exp(1).^(3.* ...
  logtheta)+(-45).*exp(1).^(2.*logtheta).*(x+(-1).*xp).^2+15.*exp(1) ...
  .^logtheta.*(x+(-1).*xp).^4+(-1).*(x+(-1).*xp).^6)+exp(1).^logtheta.*( ...
  6.*exp(1).^(3.*logtheta)+(-3).*exp(1).^(2.*logtheta).*(5.*a1.*dt.*(ubar+ ...
  (-1).*ubarp)+4.*(x+(-1).*xp)).*(x+(-1).*xp)+(-1).*a1.*dt.*(ubar+(-1).* ...
  ubarp).*(x+(-1).*xp).^5+2.*exp(1).^logtheta.*(x+(-1).*xp).^3.*(5.*a1.* ...
  dt.*(ubar+(-1).*ubarp)+x+(-1).*xp)))+a4.^2.*dt.^2.*(105.*exp(1).^(4.* ...
  logtheta)+(-420).*exp(1).^(3.*logtheta).*(x+(-1).*xp).^2+210.*exp(1).^( ...
  2.*logtheta).*(x+(-1).*xp).^4+(-28).*exp(1).^logtheta.*(x+(-1).*xp).^6+( ...
  x+(-1).*xp).^8)).*((-8)+(1/2).*exp(1).^((-1).*logtheta).*(x+(-1).*xp) ...
  .^2)+28.*a4.^2.*dt.^2.*exp(1).^logtheta.*(15.*exp(1).^(3.*logtheta)+( ...
  -45).*exp(1).^(2.*logtheta).*(x+(-1).*xp).^2+15.*exp(1).^logtheta.*(x+( ...
  -1).*xp).^4+(-1).*(x+(-1).*xp).^6)+a4.*dt.*exp(1).^(3.*logtheta).*(6.* ...
  exp(1).^(3.*logtheta)+(-30).*a2.*dt.*(3.*exp(1).^(2.*logtheta)+(-6).* ...
  exp(1).^logtheta.*(x+(-1).*xp).^2+(x+(-1).*xp).^4)+2.*exp(1).^logtheta.* ...
  (9.*exp(1).^(2.*logtheta)+(-3).*exp(1).^logtheta.*(5.*a1.*dt.*(ubar+(-1) ...
  .*ubarp)+4.*(x+(-1).*xp)).*(x+(-1).*xp)+(x+(-1).*xp).^3.*(5.*a1.*dt.*( ...
  ubar+(-1).*ubarp)+x+(-1).*xp))+(-3).*exp(1).^(2.*logtheta).*(5.*a1.*dt.* ...
  (ubar+(-1).*ubarp)+4.*(x+(-1).*xp)).*(x+(-1).*xp)+(-1).*a1.*dt.*(ubar+( ...
  -1).*ubarp).*(x+(-1).*xp).^5+2.*exp(1).^logtheta.*(x+(-1).*xp).^3.*(5.* ...
  a1.*dt.*(ubar+(-1).*ubarp)+x+(-1).*xp)));

case 3 % a1

K=dt.*exp(1).^(logsigma+(-5).*logtheta+(-1/2).*exp(1).^((-1).*logtheta).*( ...
  x+(-1).*xp).^2).*(exp(1).^(2.*logtheta).*(exp(1).^logtheta.*(2.*a1.*dt.* ...
  ubar.*ubarp.*(exp(1).^logtheta+(-1).*(x+(-1).*xp).^2)+(-1).*exp(1) ...
  .^logtheta.*(ubar+(-1).*ubarp).*(x+(-1).*xp))+(-1).*a2.*dt.*(ubar+(-1).* ...
  ubarp).*((-3).*exp(1).^logtheta+(x+(-1).*xp).^2).*(x+(-1).*xp))+(-1).* ...
  a3.*dt.*exp(1).^logtheta.*(ubar+ubarp).*(3.*exp(1).^(2.*logtheta)+(-6).* ...
  exp(1).^logtheta.*(x+(-1).*xp).^2+(x+(-1).*xp).^4)+a4.*dt.*(ubar+(-1).* ...
  ubarp).*((-15).*exp(1).^(2.*logtheta)+10.*exp(1).^logtheta.*(x+(-1).*xp) ...
  .^2+(-1).*(x+(-1).*xp).^4).*(x+(-1).*xp));

case 4 % a2

K=dt.*exp(1).^(logsigma+(-6).*logtheta+(-1/2).*exp(1).^((-1).*logtheta).*( ...
  x+(-1).*xp).^2).*(exp(1).^(2.*logtheta).*(exp(1).^logtheta.*((-2).*exp( ...
  1).^(2.*logtheta)+exp(1).^logtheta.*(3.*a1.*dt.*(ubar+(-1).*ubarp)+2.*( ...
  x+(-1).*xp)).*(x+(-1).*xp)+(-1).*a1.*dt.*(ubar+(-1).*ubarp).*(x+(-1).* ...
  xp).^3)+2.*a2.*dt.*(3.*exp(1).^(2.*logtheta)+(-6).*exp(1).^logtheta.*(x+ ...
  (-1).*xp).^2+(x+(-1).*xp).^4))+(-2).*a4.*dt.*(15.*exp(1).^(3.*logtheta)+ ...
  (-45).*exp(1).^(2.*logtheta).*(x+(-1).*xp).^2+15.*exp(1).^logtheta.*(x+( ...
  -1).*xp).^4+(-1).*(x+(-1).*xp).^6));

case 5 % a3

K=dt.^2.*exp(1).^(logsigma+(-6).*logtheta+(-1/2).*exp(1).^((-1).*logtheta) ...
  .*(x+(-1).*xp).^2).*((-1).*a1.*exp(1).^(2.*logtheta).*(ubar+ubarp).*(3.* ...
  exp(1).^(2.*logtheta)+(-6).*exp(1).^logtheta.*(x+(-1).*xp).^2+(x+(-1).* ...
  xp).^4)+2.*a3.*(15.*exp(1).^(3.*logtheta)+(-45).*exp(1).^(2.*logtheta).* ...
  (x+(-1).*xp).^2+15.*exp(1).^logtheta.*(x+(-1).*xp).^4+(-1).*(x+(-1).*xp) ...
  .^6));

case 6 % a4

K=dt.*exp(1).^(logsigma+(-8).*logtheta+(-1/2).*exp(1).^((-1).*logtheta).*( ...
  x+(-1).*xp).^2).*(exp(1).^(2.*logtheta).*((-2).*a2.*dt.*(15.*exp(1).^( ...
  3.*logtheta)+(-45).*exp(1).^(2.*logtheta).*(x+(-1).*xp).^2+15.*exp(1) ...
  .^logtheta.*(x+(-1).*xp).^4+(-1).*(x+(-1).*xp).^6)+exp(1).^logtheta.*( ...
  6.*exp(1).^(3.*logtheta)+(-3).*exp(1).^(2.*logtheta).*(5.*a1.*dt.*(ubar+ ...
  (-1).*ubarp)+4.*(x+(-1).*xp)).*(x+(-1).*xp)+(-1).*a1.*dt.*(ubar+(-1).* ...
  ubarp).*(x+(-1).*xp).^5+2.*exp(1).^logtheta.*(x+(-1).*xp).^3.*(5.*a1.* ...
  dt.*(ubar+(-1).*ubarp)+x+(-1).*xp)))+2.*a4.*dt.*(105.*exp(1).^(4.* ...
  logtheta)+(-420).*exp(1).^(3.*logtheta).*(x+(-1).*xp).^2+210.*exp(1).^( ...
  2.*logtheta).*(x+(-1).*xp).^4+(-28).*exp(1).^logtheta.*(x+(-1).*xp).^6+( ...
  x+(-1).*xp).^8));

otherwise

        K = zeros(n_x, n_xp);
end

if K == 0

    K = zeros(n_x, n_xp);

end

end


function r = subsasgn(r,s,b)
%SUBSASGN     Implements subscripted assignments for hessians
%
%  example  r(2,:) = b
%

% written  04/04/04     S.M. Rump
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 05/09/07     S.M. Rump  assignment r(:)=...
% modified 08/26/12     S.M. Rump  global variables removed
% modified 09/26/12     S.M. Rump  index handling (thanks to Matthew Weinstein)
%

  N = getappdata(0,'INTLAB_HESSIAN_NUMVAR');

  if length(s)>1
    error('multiple indexing for hessian assignment not allowed')
  end

  if strcmp(s.type,'()')     % assignment r(i) = b

    rEmpty = isempty(r);     % assignment r(i) = b for empty r
    if isempty(b)
      % does not work in Matlab 5.3 for sparse r
      r.x(s.subs{:}) = [];
      value = zeros(size(r.x));
      value(s.subs{:}) = 1;
      index = find(value);
      r.dx( : , index ) = [];
      r.hx( : , index ) = [];
      return
    end

    if ~isa(b,'hessian')
      b = hessian(b);
    end

    if ~rEmpty
      resultIsintval = isa(r.x,'intval');
      if ~resultIsintval & isa(b.x,'intval')
        r = intval(r);
        resultIsintval = 1;
      end
      sizeincreased = 0;
      if length(s.subs)==1              % single index
        if ~isequal(s.subs{1},':')      % not r(:)=...
          if size(r.x,1)==1             % row vector
            sizeincreased = ( s.subs{1}>size(r.x,2) );
          else
            sizeincreased = ( s.subs{1} > prod(size(r.x)) );
          end
          if sizeincreased
            srx = size(r.x);
            if length(srx)==2
              if all(prod(srx)~=srx)
                error('matrix cannot be resized by assignment a(I) = b')
              end
            else
              error('attempt to grow size of array along ambiguous dimension')
            end
          end
        end
      else                            % multiple index
        for i=1:length(s.subs)
          if ~isequal(s.subs{i},':')
            sizeincreased = sizeincreased | ( s.subs{i} > size(r.x,i) );
          end
        end
      end
      if sizeincreased                % size increased, adapt .x and .dx and .hx
        rx = r.x;
        rdx = r.dx;
        rhx = r.hx;
        value = ones(size(r.x));
        value( s.subs{:} ) = 0;
        r.x = zeros(size(value));
        INTLAB_HESSIAN_SPARSE = getappdata(0,'INTLAB_HESSIAN_SPARSE');
        if N<INTLAB_HESSIAN_SPARSE
          r.dx = zeros(N,prod(size(value)));
          r.hx = zeros(N*N,prod(size(value)));
        else
          r.dx = sparse([],[],[],N,prod(size(value)));
          r.hx = sparse([],[],[],N*N,prod(size(value)));
      end
        if resultIsintval
          r.x = intval(r.x);
          r.dx = intval(r.dx);
          r.hx = intval(r.hx);
        end
        index = find(value);
        r.x( index ) = rx;
        r.dx( : , index ) = rdx;
        r.hx( : , index ) = rhx;
      end
    else                     % assignment r(i) = b for empty r
      resultIsintval = isa(b.x,'intval');
      r.x(s.subs{:}) = 0;
      INTLAB_HESSIAN_SPARSE = getappdata(0,'INTLAB_HESSIAN_SPARSE');
      if N<INTLAB_HESSIAN_SPARSE
        r.dx = zeros(N,prod(size(r.x)));
        r.hx = zeros(N*N,prod(size(r.x)));
      else
        r.dx = sparse([],[],[],N,prod(size(r.x)));
        r.hx = sparse([],[],[],N*N,prod(size(r.x)));
      end
      if resultIsintval
        r.x = intval(r.x);
        r.dx = intval(r.dx);
        r.hx = intval(r.hx);
      end
    end
    r.x( s.subs{:} ) = b.x;
    value = reshape(1:prod(size(r.x)),size(r.x));
    index = value( s.subs{:} );
    if ( prod(size(b.x))==1 ) & ( length(index)~=1 )
      r.dx(:,index) = b.dx(:,ones(1,length(index)));
      r.hx(:,index) = b.hx(:,ones(1,length(index)));
    else
      r.dx(:,index) = b.dx;
      r.hx(:,index) = b.hx;
    end
    if rEmpty
      r = class(r,'hessian');
    end
  else
    error('invalid index reference for hessian')
  end

  % avoid Matlab 6.5f bug:
  % a = sparse([],[],[],1,1); a = reshape(a,1,1); abs(a)
  % produces  9.6721e-317  or similar number in underflow range
  if prod(size(r.x))==1
    r.x = full(r.x);
  end
  if prod(size(r.dx))==1
    r.dx = full(r.dx);
  end
  if prod(size(r.hx))==1
    r.hx = full(r.hx);
  end


function bemcp_example
% Simple function to test/demonstrate how the Boundary element functions are
% used in combination with Fildtrip/Forwinv routines.
%
% 1. A model is created as 3 concentric meshed spheres (using FT's
% icosahedron routines),
% 2. then random electrodes are placed on the upper part of the outer
% sphere.
% 3. the model is then "prepared" with 'ft_prepare_bemmodel', this bits
% takes most time as it requires LOTS of calculation.
% 4. sensors and volumes are plugged together by 'forwinv_prepare_vol_sens'
% 5. Finally the leadfiled for 3 orthogonal sources placed at one location
% is calculated with 'forwinv_compute_leadfield.m'
% 6. Display the 3 leadfields
%
% NOTE:
% this bit of code needs access to low level fieldtrip/forwinv routines
% which have been copy/pasted here under.
% Be aware that this way of programming is generally NOT advisable!
% I used it only to ensure a quick & dirty check of the BEM module...

% Christophe Phillips
% $Id: bemcp_example.m 4793 2012-07-19 11:35:24Z spm $

% create volume conductor starting from unit sphere
[pnt, tri] = icosahedron162;

vol = [];
vol.cond = [1 1/80 1];
vol.source = 1; % index of source compartment
vol.skin_surface   = 3; % index of skin surface
% inner_skull_surface
vol.bnd(1).pnt = pnt*88;
vol.bnd(1).tri = tri;
% outer_skull_surface
vol.bnd(2).pnt = pnt*92;
vol.bnd(2).tri = tri;
% skin_surface
vol.bnd(3).pnt = pnt*100;
vol.bnd(3).tri = tri;

% create the BEM system matrix
cfg = [];
cfg.method = 'bemcp';
vol1 = ft_prepare_bemmodel(cfg, vol);

% create some random electrodes
pnt = randn(200,3);
pnt = pnt(pnt(:,3)>0, :);  % only those on the upper half
sens = [];
for i=1:size(pnt,1)
  sens.pnt(i,:) = pnt(i,:) / norm(pnt(i,:)); % scale towards the skin surface
  sens.label{i} = sprintf('%02d', i);
end

% prepare the sensor array and volume conduction, i.e. set up the linear
% interpolation from vertices to electrodes
[vol2, sens] = forwinv_prepare_vol_sens(vol1, sens);

lf = forwinv_compute_leadfield([0 0 50], sens, vol2);

figure; triplot(sens.pnt, [], lf(:,1)); colorbar
figure; triplot(sens.pnt, [], lf(:,2)); colorbar
figure; triplot(sens.pnt, [], lf(:,3)); colorbar

return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Subfunctions from FieldTrip
function [pnt, dhk] = icosahedron162

% ICOSAHEDRON162 creates a 2-fold refined icosahedron

% Copyright (C) 2003, Robert Oostenveld
%
% $Log: icosahedron162.m,v $
% Revision 1.3  2003/11/28 09:40:12  roberto
% added a single help line
%
% Revision 1.2  2003/03/04 21:46:18  roberto
% added CVS log entry and synchronized all copyright labels
%

[pnt, dhk] = icosahedron;
[pnt, dhk] = refine(pnt, dhk);
[pnt, dhk] = refine(pnt, dhk);

pnt = pnt ./ repmat(sqrt(sum(pnt.^2,2)), 1,3);

return

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

function [pnt, dhk] = icosahedron

% ICOSAHEDRON creates an icosahedron
%
% [pnt, dhk] = icosahedron
% creates an icosahedron with 12 vertices and 20 triangles
%
% See also OCTAHEDRON, ICOSAHEDRON42, ICOSAHEDRON162, ICOSAHEDRON642, ICOSAHEDRON2562

% Copyright (C) 2002, Robert Oostenveld
%
% $Log: icosahedron.m,v $
% Revision 1.4  2006/07/26 11:03:38  roboos
% added "see also octahedron"
%
% Revision 1.3  2003/03/11 15:35:20  roberto
% converted all files from DOS to UNIX
%
% Revision 1.2  2003/03/04 21:46:18  roberto
% added CVS log entry and synchronized all copyright labels
%

dhk = [
   1   2   3
   1   3   4
   1   4   5
   1   5   6
   1   6   2
   2   8   3
   3   9   4
   4  10   5
   5  11   6
   6   7   2
   7   8   2
   8   9   3
   9  10   4
  10  11   5
  11   7   6
  12   8   7
  12   9   8
  12  10   9
  12  11  10
  12   7  11
];

pnt = zeros(12, 3);

rho=0.4*sqrt(5);
phi=2*pi*(0:4)/5;

pnt( 1, :) = [0 0  1];			% top point

pnt(2:6, 1) = rho*cos(phi)';
pnt(2:6, 2) = rho*sin(phi)';
pnt(2:6, 3) = rho/2;

pnt(7:11, 1) = rho*cos(phi - pi/5)';
pnt(7:11, 2) = rho*sin(phi - pi/5)';
pnt(7:11, 3) = -rho/2;

pnt(12, :) = [0 0 -1];			% bottom point

return

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

function [pntr, dhkr] = refine(pnt, dhk, method, varargin)

% REFINE a 3D surface that is described by a triangulation
%
% Use as
%   [pnt, tri] = refine(pnt, tri)
%   [pnt, tri] = refine(pnt, tri, 'updown', numtri)
%
% The default method is to refine the mesh globally by inserting a vertex at
% each edge according to the algorithm described in Banks, 1983.
%
% The alternative 'updown' method refines the mesh a couple of times
% using Banks' algorithm, followed by a downsampling using the REDUCEPATCH
% function.

% The Banks method is a memory efficient implementation which remembers
% the previously inserted vertices. The refinement algorithm executes in
% linear time with the number of triangles.

% Copyright (C) 2002-2005, Robert Oostenveld
%
% $Log: refine.m,v $
% Revision 1.4  2005/05/06 08:54:31  roboos
% added 'updown method, using matlab reducepatch function
%
% Revision 1.3  2003/03/11 15:35:20  roberto
% converted all files from DOS to UNIX
%
% Revision 1.2  2003/03/04 21:46:19  roberto
% added CVS log entry and synchronized all copyright labels
%

if nargin<3
  method = 'banks';
end

switch lower(method)
case 'banks'
  npnt   = size(pnt,1);
  ndhk   = size(dhk,1);
  insert = spalloc(3*npnt,3*npnt,3*ndhk);

  dhkr  = zeros(4*ndhk,3);		% allocate memory for the new triangles
  pntr  = zeros(npnt+3*ndhk,3);		% allocate memory for the maximum number of new vertices
  pntr(1:npnt,:) = pnt;			% insert the original vertices
  current = npnt;

  for i=1:ndhk

    if ~insert(dhk(i,1),dhk(i,2))
      current = current + 1;
      pntr(current,:) = (pnt(dhk(i,1),:) + pnt(dhk(i,2),:))/2;
      insert(dhk(i,1),dhk(i,2)) = current;
      insert(dhk(i,2),dhk(i,1)) = current;
      v12 = current;
    else
      v12 = insert(dhk(i,1),dhk(i,2));
    end

    if ~insert(dhk(i,2),dhk(i,3))
      current = current + 1;
      pntr(current,:) = (pnt(dhk(i,2),:) + pnt(dhk(i,3),:))/2;
      insert(dhk(i,2),dhk(i,3)) = current;
      insert(dhk(i,3),dhk(i,2)) = current;
      v23 = current;
    else
      v23 = insert(dhk(i,2),dhk(i,3));
    end

    if ~insert(dhk(i,3),dhk(i,1))
      current = current + 1;
      pntr(current,:) = (pnt(dhk(i,3),:) + pnt(dhk(i,1),:))/2;
      insert(dhk(i,3),dhk(i,1)) = current;
      insert(dhk(i,1),dhk(i,3)) = current;
      v31 = current;
    else
      v31 = insert(dhk(i,3),dhk(i,1));
    end

    % add the 4 new triangles with the correct indices
    dhkr(4*(i-1)+1, :) = [dhk(i,1) v12 v31];
    dhkr(4*(i-1)+2, :) = [dhk(i,2) v23 v12];
    dhkr(4*(i-1)+3, :) = [dhk(i,3) v31 v23];
    dhkr(4*(i-1)+4, :) = [v12 v23 v31];

  end

  % remove the space for the vertices that was not used
  pntr = pntr(1:current, :);

case 'updown'
  ndhk = size(dhk,1);
  while ndhk<varargin{1}
    % increase the number of triangles by a factor of 4
    [pnt, dhk] = refine(pnt, dhk, 'banks');
    ndhk = size(dhk,1);
  end
  % reduce number of triangles using Matlab function
  [dhkr, pntr] = reducepatch(dhk, pnt, varargin{1});

otherwise
  error(['unsupported method: ' method]);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [hs, hc, contour] = triplot(pnt, tri, val, mode, levels)

% TRIPLOT make 2D or 3D plot of triangulated surface and interpolated values
% The surface can be displayed with linear interpolated values or with
% linear interpolated contours of a potential distribution.
%
% Use as
%   triplot(pnt, tri, value)
%   triplot(pnt, tri, value, mode)
%   triplot(pnt, tri, value, mode, levels)
% This will make a plot of value on the surface described by triangles
% tri with vertices pnt. The matrix tri can be [], in which case a
% it will be computed using a delaunay triangulation.
%
% The visualization mode can be
%   'surface'     make interpolated plot of value on surface (default)
%   'faces'       plot white triangles only        (value can be [])
%   'faces_red'   plot red triangles only          (value can be [])
%   'faces_blue'  plot blue triangles only         (value can be [])
%   'faces_skin'  plot skin-colored triangles only (value can be [])
%   'face_index'  plot index of each triangle      (value can be [])
%   'nodes'       plot black vertices only         (value can be [])
%   'node_index'  plot index of each vertex        (value can be [])
%   'node_label'  plot label of each vertex        (value should be cell array)
%   'edges'       plot black edges only            (value can be [])
%   'contour'     make interpolated contour plot of value on surface
%   'contour_bw'  make interpolated black-white contour plot
%   'contour_rb'  make interpolated contour plot with red-blue
%
% With the optional levels, you can specify the levels at which contours will
% be plotted
%
% See also PATCH, COLORMAP, VIEW (general Matlab commands)

% updated on Mon Jul 23 12:41:44 MET DST 2001
% updated on Fri Jan 31 11:47:28 CET     2003

% Copyright (C) 2001=2006, Robert Oostenveld
%
% $Log: triplot.m,v $
% Revision 1.7  2008/06/24 13:37:51  roboos
% added option faces_blue
% always return handle to objects that were plotted
%
% Revision 1.6  2007/01/03 17:00:35  roboos
% updated documentation, changed layout of code and comments
%
% Revision 1.5  2006/09/19 16:11:35  roboos
% added support for line segments, removed "axis equal" at end
%
% Revision 1.4  2006/05/02 19:15:28  roboos
% added 'faces_red' style
%
% Revision 1.3  2004/06/28 07:51:39  roberto
% improved documentation, added faces_skin
%
% Revision 1.2  2003/03/17 10:37:29  roberto
% improved general help comments and added copyrights
%

% start with empty return values
hs      = [];
hc      = [];
contour = [];

% everything is added to the current figure
holdflag = ishold;
hold on

% check the input variables
if ~isempty(val)
  val = val(:);
end

if nargin<4
  mode = 'surface';
end

if isempty(tri) & ~strcmp(mode, 'nodes')
  % no triangulation was specified but a triangulation is needed
  if size(pnt,2)==2
    % make a 2d triangulation of the points using delaunay
    tri = delaunay(pnt(:,1), pnt(:,2));
  else
    % make a 2d triangulation of the projected points using delaunay
    prj = elproj(pnt);
    tri = delaunay(prj(:,1), prj(:,2));
  end
end

if size(tri,2)==2
  % lines are specified instead of triangles, convert to triangles
  tri(:,3) = tri(:,2);
end

if nargin<3
  warning('only displaying triangle edges')
  mode='edges';
  val = [];
elseif nargin<4
  % warning('default displaying surface')
  mode='surface';
elseif nargin<5
  % determine contour levels
  if ~isempty(val) & ~iscell(val)
    absmax = max(abs([min(val) max(val)]));
    levels = linspace(-absmax,absmax,21);
  else
    levels = [];
  end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% compute contours for 2D or 3D triangulation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if strcmp(mode, 'contour') || strcmp(mode, 'contour_bw') || strcmp(mode, 'contour_rb')
  triangle_val = val(tri);
  triangle_min = min(triangle_val, [], 2);
  triangle_max = max(triangle_val, [], 2);

  for cnt_indx=1:length(levels)
    cnt = levels(cnt_indx);
    use = cnt>=triangle_min & cnt<=triangle_max;
    counter = 0;
    intersect1 = [];
    intersect2 = [];

    for tri_indx=find(use)'
      pos  = pnt(tri(tri_indx,:), :);
      v(1) = triangle_val(tri_indx,1);
      v(2) = triangle_val(tri_indx,2);
      v(3) = triangle_val(tri_indx,3);
      la(1) = (cnt-v(1)) / (v(2)-v(1));	% abcissa between vertex 1 and 2
      la(2) = (cnt-v(2)) / (v(3)-v(2));	% abcissa between vertex 2 and 3
      la(3) = (cnt-v(3)) / (v(1)-v(3));	% abcissa between vertex 1 and 2
      abc(1,:) = pos(1,:) + la(1) * (pos(2,:) - pos(1,:));
      abc(2,:) = pos(2,:) + la(2) * (pos(3,:) - pos(2,:));
      abc(3,:) = pos(3,:) + la(3) * (pos(1,:) - pos(3,:));
      counter = counter + 1;
      sel     = find(la>=0 & la<=1);
      intersect1(counter, :) = abc(sel(1),:);
      intersect2(counter, :) = abc(sel(2),:);
    end

    % remember the details for external reference
    contour(cnt_indx).level = cnt;
    contour(cnt_indx).n     = counter;
    contour(cnt_indx).intersect1 = intersect1;
    contour(cnt_indx).intersect2 = intersect2;
  end

  % collect all different contourlevels for plotting
  intersect1 = [];
  intersect2 = [];
  cntlevel   = [];
  for cnt_indx=1:length(levels)
    intersect1 = [intersect1; contour(cnt_indx).intersect1];
    intersect2 = [intersect2; contour(cnt_indx).intersect2];
    cntlevel   = [cntlevel; ones(contour(cnt_indx).n,1) * levels(cnt_indx)];
  end

  X = [intersect1(:,1) intersect2(:,1)]';
  Y = [intersect1(:,2) intersect2(:,2)]';
  C = [cntlevel(:)     cntlevel(:)]';

  if size(pnt,2)>2
    Z = [intersect1(:,3) intersect2(:,3)]';
  else
    Z = zeros(2, length(cntlevel));
  end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% plot the desired detail
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
switch lower(mode)

  case 'faces'
    % plot the faces of the 2D or 3D triangulation
    hs = patch('Vertices', pnt, 'Faces', tri);
    set(hs, 'FaceColor', 'white');
    set(hs, 'EdgeColor', 'none');

  case 'faces_skin'
    % plot the faces of the 2D or 3D triangulation
    skin_surface   = [255 213 119]/255;
    inner_skull_surface  = [202 100 100]/255;
    cortex = [255 213 119]/255;
    hs = patch('Vertices', pnt, 'Faces', tri);
    set(hs, 'FaceColor', skin_surface);
    set(hs, 'EdgeColor', 'none');
    lighting gouraud
    material shiny
    camlight

  case 'faces_red'
    % plot the faces of the 2D or 3D triangulation
    hs = patch('Vertices', pnt, 'Faces', tri);
    set(hs, 'FaceColor', [1 0 0]);
    set(hs, 'EdgeColor', 'none');
    lighting gouraud
    material shiny
    camlight

  case 'faces_blue'
    % plot the faces of the 2D or 3D triangulation
    hs = patch('Vertices', pnt, 'Faces', tri);
    set(hs, 'FaceColor', [0 0 1]);
    set(hs, 'EdgeColor', 'none');
    lighting gouraud
    material shiny
    camlight

  case 'face_index'
    % plot the triangle indices (numbers) at each face
    for face_indx=1:size(tri,1)
      str = sprintf('%d', face_indx);
      tri_x = (pnt(tri(face_indx,1), 1) +  pnt(tri(face_indx,2), 1) +  pnt(tri(face_indx,3), 1))/3;
      tri_y = (pnt(tri(face_indx,1), 2) +  pnt(tri(face_indx,2), 2) +  pnt(tri(face_indx,3), 2))/3;
      tri_z = (pnt(tri(face_indx,1), 3) +  pnt(tri(face_indx,2), 3) +  pnt(tri(face_indx,3), 3))/3;
      h   = text(tri_x, tri_y, tri_z, str, 'HorizontalAlignment', 'center', 'VerticalAlignment', 'middle');
      hs  = [hs; h];
    end

  case 'edges'
    % plot the edges of the 2D or 3D triangulation
    hs = patch('Vertices', pnt, 'Faces', tri);
    set(hs, 'FaceColor', 'none');
    set(hs, 'EdgeColor', 'black');

  case 'nodes'
    % plot the nodes (vertices) only as points
    if size(pnt, 2)==2
      hs = plot(pnt(:,1), pnt(:,2), 'k.');
    else
      hs = plot3(pnt(:,1), pnt(:,2), pnt(:,3), 'k.');
    end

  case 'nodes_blue'
    % plot the nodes (vertices) only as points
    if size(pnt, 2)==2
      hs = plot(pnt(:,1), pnt(:,2), 'b.', 'MarkerSize', 20);
    else
      hs = plot3(pnt(:,1), pnt(:,2), pnt(:,3), 'b.', 'MarkerSize', 20);
    end

  case 'nodes_red'
    % plot the nodes (vertices) only as points
    if size(pnt, 2)==2
      hs = plot(pnt(:,1), pnt(:,2), 'r.', 'MarkerSize', 20);
    else
      hs = plot3(pnt(:,1), pnt(:,2), pnt(:,3), 'r.', 'MarkerSize', 20);
    end

  case 'node_index'
    % plot the vertex indices (numbers) at each node
    for node_indx=1:size(pnt,1)
      str = sprintf('%d', node_indx);
      if size(pnt, 2)==2
        h   = text(pnt(node_indx, 1), pnt(node_indx, 2), str, 'HorizontalAlignment', 'center', 'VerticalAlignment', 'middle');
      else
        h   = text(pnt(node_indx, 1), pnt(node_indx, 2), pnt(node_indx, 3), str, 'HorizontalAlignment', 'center', 'VerticalAlignment', 'middle');
      end
      hs  = [hs; h];
    end

  case 'node_label'
    % plot the vertex indices (numbers) at each node
    for node_indx=1:size(pnt,1)
      str = val{node_indx};
      if ~isempty(str)
        if size(pnt, 2)==2
          h   = text(pnt(node_indx, 1), pnt(node_indx, 2), str, 'HorizontalAlignment', 'center', 'VerticalAlignment', 'middle');
        else
          h   = text(pnt(node_indx, 1), pnt(node_indx, 2), pnt(node_indx, 3), str, 'HorizontalAlignment', 'center', 'VerticalAlignment', 'middle');
        end
      else
        h = -1;
      end
      hs  = [hs; h];
    end

  case 'surface'
    % plot a 2D or 3D triangulated surface with linear interpolation
    if length(val)==size(pnt,1)
      hs = patch('Vertices', pnt, 'Faces', tri, 'FaceVertexCData', val, 'FaceColor', 'interp');
    else
      hs = patch('Vertices', pnt, 'Faces', tri, 'CData', val, 'FaceColor', 'flat');
    end
    set(hs, 'EdgeColor', 'none');

  case 'contour_bw'
    % make black-white contours
    hc = [];
    for i=1:length(cntlevel)
      if cntlevel(i)>0
        linestyle = '-';
        linewidth = 1;
      elseif cntlevel(i)<0
        linestyle = '--';
        linewidth = 1;
      else
        linestyle = '-';
        linewidth = 2;
      end
      h1 = patch('XData', X(:,i), 'Ydata', Y(:,i), ...
        'ZData', Z(:,i), 'CData', C(:,i), ...
        'facecolor','none','edgecolor','black', ...
        'linestyle', linestyle, 'linewidth', linewidth, ...
        'userdata',cntlevel(i));
      hc = [hc; h1];
    end

  case 'contour_rb'
    % make red-blue contours
    hc = [];
    for i=1:length(cntlevel)
      if cntlevel(i)>0
        edgecolor = 'red';
      elseif cntlevel(i)<0
        edgecolor = 'blue';
      else
        edgecolor = 'black';
      end
      h1 = patch('XData', X(:,i), 'Ydata', Y(:,i), ...
        'ZData', Z(:,i), 'CData', C(:,i), ...
        'facecolor','none','edgecolor',edgecolor, ...
        'linestyle', '-', 'linewidth', 3, ...
        'userdata',cntlevel(i));
      hc = [hc; h1];
    end

  case 'contour'
    % make full-color contours
    hc = [];
    for i=1:length(cntlevel)
      h1 = patch('XData', X(:,i), 'Ydata', Y(:,i), ...
        'ZData', Z(:,i), 'CData', C(:,i), ...
        'facecolor','none','edgecolor','flat',...
        'userdata',cntlevel(i));
      hc = [hc; h1];
    end

end	% switch

axis off
axis vis3d
axis equal

if nargout==0
  clear contour hc hs
end

if ~holdflag
  hold off
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [proj] = elproj(pos, method);

% ELPROJ makes a azimuthal projection of a 3D electrode cloud
%  on a plane tangent to the sphere fitted through the electrodes
%  the projection is along the z-axis
%
%  [proj] = elproj([x, y, z], 'method');
%
% Method should be one of these:
%	  'gnomic'
%	  'stereographic'
%	  'ortographic'
%	  'inverse'
%	  'polar'
%
% Imagine a plane being placed against (tangent to) a globe. If
% a light source inside the globe projects the graticule onto
% the plane the result would be a planar, or azimuthal, map
% projection. If the imaginary light is inside the globe a Gnomonic
% projection results, if the light is antipodal a Sterographic,
% and if at infinity, an Orthographic.
%
% The default projection is a polar projection (BESA like).
% An inverse projection is the opposite of the default polar projection.

% Copyright (C) 2000-2008, Robert Oostenveld
%
% $Log: elproj.m,v $
% Revision 1.4  2008/05/15 10:54:24  roboos
% updated documentation
%
% Revision 1.3  2007/03/20 10:29:35  roboos
% renamed method 'default' into 'polar'
%
% Revision 1.2  2003/03/17 10:37:28  roberto
% improved general help comments and added copyrights
%

x = pos(:,1);
y = pos(:,2);
if size(pos, 2)==3
  z = pos(:,3);
end

if nargin<2
  method='polar';
end

if nargin<3
  secant=1;
end

if strcmp(method, 'orthographic')
  % this method compresses the lowest electrodes very much
  % electrodes on the bottom half of the sphere are folded inwards
  xp = x;
  yp = y;
  num = length(find(z<0));
  str = sprintf('%d electrodes may be folded inwards in orthographic projection\n', num);
  if num
    warning(str);
  end
  proj = [xp yp];

elseif strcmp(method, 'gnomic')
  % the lightsource is in the middle of the sphere
  % electrodes on the equator are projected at infinity
  % electrodes below the equator are not projected at all
  rad = mean(sqrt(x.^2 + y.^2 + z.^2));
  phi = cart2pol(x, y);
  th  = atan(sqrt(x.^2 + y.^2) ./ z);
  xp  = cos(phi) .* tan(th) .* rad;
  yp  = sin(phi) .* tan(th) .* rad;
  num = length(find(th==pi/2 | z<0));
  str = sprintf('removing %d electrodes from gnomic projection\n', num);
  if num
    warning(str);
  end
  xp(find(th==pi/2 | z<0)) = NaN;
  yp(find(th==pi/2 | z<0)) = NaN;
  proj = [xp yp];

elseif strcmp(method, 'stereographic')
  % the lightsource is antipodal (on the south-pole)
  rad = mean(sqrt(x.^2 + y.^2 + z.^2));
  z   = z + rad;
  phi = cart2pol(x, y);
  th  = atan(sqrt(x.^2 + y.^2) ./ z);
  xp  = cos(phi) .* tan(th) .* rad * 2;
  yp  = sin(phi) .* tan(th) .* rad * 2;
  num = length(find(th==pi/2 | z<0));
  str = sprintf('removing %d electrodes from stereographic projection\n', num);
  if num
    warning(str);
  end
  xp(find(th==pi/2 | z<0)) = NaN;
  yp(find(th==pi/2 | z<0)) = NaN;
  proj = [xp yp];

elseif strcmp(method, 'inverse')
  % compute the inverse projection of the default angular projection
  [th, r] = cart2pol(x, y);
  [xi, yi, zi] = sph2cart(th, pi/2 - r, 1);
  proj = [xi, yi, zi];

else
  % use default angular projection
  [az, el, r] = cart2sph(x, y, z);
  [x, y] = pol2cart(az, pi/2 - el);
  proj = [x, y];
end


classdef prtRvUniformImproper < prtRv
    % prtRvUniformImproper  Improper uniform random variable
    %
    %   RV = prtRvUniformImproper creates a prtRvUniformImproper object
    %   with unknown dimensionality nDimensions. nDimensions can be set
    %   manually or using the MLE method. A prtRvUniformImproper
    %   models an improper pdf that always yields a value of 1 no matter
    %   the input. prtRvUniformImproper is sometimes useful for creating
    %   one class classifiers. See the examples below for more information
    %
    %   The draw method of prtRvUniformImproper draws values uniformly
    %   distributed from realmin to realmax in each dimension.
    %
    %   RV = prtRvUniformImproper(PROPERTY1, VALUE1,...) creates a
    %   prtRvMultinomial object RV with properties as specified by
    %   PROPERTY/VALUE pairs.
    %
    %   A prtRvUniformImproper object inherits all properties from the
    %   prtRv class. In addition, it has the following properties:
    %
    %   nDimensions - dimensionality of the data modeled by this RV.
    %
    %  A prtRvUniformImproper object inherits all methods from the prtRv
    %  class. The MLE  method can be used to set the parameters from data.
    %
    %  Example:
    %
    %  % In this example we show that the PDF of a prtRvUniformImproper is
    %  % always 1
    %  dataSet = prtDataGenUnimodal;        % Load a dataset consisting of
    %                                       % 2 features
    %  dataSet = retainFeatures(dataSet,1); % Retain only the first feature
    %                                       % only for the example.
    %
    %  RV = prtRvUniformImproper;           % Create a prtRvUniform object
    %  RV = RV.mle(dataSet);                % Compute the bounds
    %
    %  RV.plotPdf([-10 10]);                % We must manually specify
    %                                       % plot limits since
    %                                       % prtRvUniformImproper does not
    %                                       % have actual plot limits
    %
    %
    %  % In this example we show how to build a one class MAP classifier
    %  dataSet = prtDataGenUnimodal;        % Load a dataset consisting of
    %                                       % 2 features
    %
    %  % Create and train a GLRT classifier that uses a
    %  % prtRvUniformImproper to model class 0 and a prtRvMvn to model
    %  % class 1
    %  glrtClass = train(prtClassGlrt('rvH0',prtRvUniformImproper,'rvH1',prtRvMvn),dataSet);
    %
    %  plot(glrtClass) % Contours only show the log-likelihood of class 1
    %
    %   See also: prtRv, prtRvMvn, prtRvGmm, prtRvMultinomial,
    %   prtRvVq, prtRvKde

    properties (SetAccess = private)
        name = 'Improper Uniform Random Variable'
        nameAbbreviation = 'RVImUnif';
    end

    properties (SetAccess = protected)
        isSupervised = false;
        isCrossValidateValid = true;
    end

    properties (Hidden = true, SetAccess = 'private', GetAccess = 'private')
        nDimensionsPrivate
    end

    properties (Hidden = true, Dependent = true)
        nDimensions
    end

    methods
        % The Constructor
        function R = prtRvUniformImproper(varargin)
            R = constructorInputParse(R,varargin{:});
        end

        function R = mle(R,X)
            X = R.dataInputParse(X); % Basic error checking etc
           R.nDimensionsPrivate = size(X,2);
        end

        function vals = pdf(R,X)
            X = R.dataInputParse(X); % Basic error checking etc

            [isValid, reasonStr] = R.isValid;
            assert(isValid,'PDF cannot yet be evaluated. This RV is not yet valid %s.',reasonStr);

            vals = ones(size(X,1),1);
        end

        function vals = logPdf(R,X)
            X = R.dataInputParse(X); % Basic error checking etc
            [isValid, reasonStr] = R.isValid;
            assert(isValid,'LOGPDF cannot yet be evaluated. This RV is not yet valid %s.',reasonStr);
            vals = log(pdf(R,X));
        end

        function vals = cdf(R,X)
            X = R.dataInputParse(X); % Basic error checking etc
            [isValid, reasonStr] = R.isValid;
            assert(isValid,'CDF cannot yet be evaluated. This RV is not yet valid %s.',reasonStr);
            assert(size(X,2) == R.nDimensions,'Data, RV dimensionality missmatch. Input data, X, has dimensionality %d and this RV has dimensionality %d.', size(X,2), R.nDimensions)
            vals = nan(size(X,1),1);
        end

        function vals = draw(R,N)
            if nargin < 2
                N = 1;
            end

            assert(numel(N)==1 && N==floor(N) && N > 0,'N must be a positive integer scalar.')

            [isValid, reasonStr] = R.isValid;
            assert(isValid,'DRAW cannot yet be evaluated. This RV is not yet valid %s.',reasonStr);

            vals = bsxfun(@times,bsxfun(@times,rand(N,R.nDimensions),realmax),sign(randn(N,1)));
        end

        function R = set.nDimensions(R,N)
            assert(numel(N)==1 && N==floor(N) && N > 0,'nDimensions must be a positive integer scalar.')
            R.nDimensionsPrivate = N;
        end

        function val = get.nDimensions(R)
            val = R.nDimensionsPrivate;
        end
    end

    methods (Hidden = true)
        function [val, reasonStr] = isValid(R)
            if numel(R) > 1
                val = false(size(R));
                for iR = 1:numel(R)
                    [val(iR), reasonStr] = isValid(R(iR));
                end
                return
            end

            val = true;
            reasonStr = '';
        end
        function val = plotLimits(R)
            [isValid, reasonStr] = R.isValid;
            if isValid
                val = zeros(2*R.nDimensions,1);
                val(1:2:end) = Inf;
                val(2:2:end) = -Inf;
                % We send backwards limits so that we don't effect plot
                % limits if you were to check the limits of a set of RVs.
                % In this case this RV would not change the resulting
                % max(upperBounds), min(lowerBounds)
            else
                error('prtRvUniformImproper:plotLimits','Plotting limits can not be determined for this RV. It is not yet valid %s.',reasonStr)
            end
        end
    end
end


function [K] = ku0u0(x, xp, hyp, ubar, ubarp, dt, i)

logsigma = hyp(1);
logtheta = hyp(2);

a1 = hyp(3);
a2 = hyp(4);
a3 = hyp(5);

n_x = size(x,1);
n_xp = size(xp,1);

x = repmat(x,1,n_xp);
xp = repmat(xp',n_x,1);

ubar = repmat(ubar,1,n_xp);
ubarp = repmat(ubarp',n_x,1);

switch i

case 0

K=exp(1).^(logsigma+(-8).*logtheta+(-1/2).*exp(1).^((-1).*logtheta).*(x+( ...
  -1).*xp).^2).*(exp(1).^(4.*logtheta).*(exp(1).^(2.*logtheta).*(exp(1).^( ...
  2.*logtheta)+a1.*dt.*exp(1).^logtheta.*(a1.*dt.*ubar.*ubarp+(-1).*(ubar+ ...
  (-1).*ubarp).*(x+(-1).*xp))+(-1).*a1.^2.*dt.^2.*ubar.*ubarp.*(x+(-1).* ...
  xp).^2)+a2.*dt.*exp(1).^logtheta.*((-2).*exp(1).^(2.*logtheta)+exp(1) ...
  .^logtheta.*(3.*a1.*dt.*(ubar+(-1).*ubarp)+2.*(x+(-1).*xp)).*(x+(-1).* ...
  xp)+(-1).*a1.*dt.*(ubar+(-1).*ubarp).*(x+(-1).*xp).^3)+a2.^2.*dt.^2.*( ...
  3.*exp(1).^(2.*logtheta)+(-6).*exp(1).^logtheta.*(x+(-1).*xp).^2+(x+(-1) ...
  .*xp).^4))+a3.*dt.*exp(1).^(2.*logtheta).*((-2).*a2.*dt.*(15.*exp(1).^( ...
  3.*logtheta)+(-45).*exp(1).^(2.*logtheta).*(x+(-1).*xp).^2+15.*exp(1) ...
  .^logtheta.*(x+(-1).*xp).^4+(-1).*(x+(-1).*xp).^6)+exp(1).^logtheta.*( ...
  6.*exp(1).^(3.*logtheta)+(-3).*exp(1).^(2.*logtheta).*(5.*a1.*dt.*(ubar+ ...
  (-1).*ubarp)+4.*(x+(-1).*xp)).*(x+(-1).*xp)+(-1).*a1.*dt.*(ubar+(-1).* ...
  ubarp).*(x+(-1).*xp).^5+2.*exp(1).^logtheta.*(x+(-1).*xp).^3.*(5.*a1.* ...
  dt.*(ubar+(-1).*ubarp)+x+(-1).*xp)))+a3.^2.*dt.^2.*(105.*exp(1).^(4.* ...
  logtheta)+(-420).*exp(1).^(3.*logtheta).*(x+(-1).*xp).^2+210.*exp(1).^( ...
  2.*logtheta).*(x+(-1).*xp).^4+(-28).*exp(1).^logtheta.*(x+(-1).*xp).^6+( ...
  x+(-1).*xp).^8));

case 1 % logsigma

K=exp(1).^(logsigma+(-8).*logtheta+(-1/2).*exp(1).^((-1).*logtheta).*(x+( ...
  -1).*xp).^2).*(exp(1).^(4.*logtheta).*(exp(1).^(2.*logtheta).*(exp(1).^( ...
  2.*logtheta)+a1.*dt.*exp(1).^logtheta.*(a1.*dt.*ubar.*ubarp+(-1).*(ubar+ ...
  (-1).*ubarp).*(x+(-1).*xp))+(-1).*a1.^2.*dt.^2.*ubar.*ubarp.*(x+(-1).* ...
  xp).^2)+a2.*dt.*exp(1).^logtheta.*((-2).*exp(1).^(2.*logtheta)+exp(1) ...
  .^logtheta.*(3.*a1.*dt.*(ubar+(-1).*ubarp)+2.*(x+(-1).*xp)).*(x+(-1).* ...
  xp)+(-1).*a1.*dt.*(ubar+(-1).*ubarp).*(x+(-1).*xp).^3)+a2.^2.*dt.^2.*( ...
  3.*exp(1).^(2.*logtheta)+(-6).*exp(1).^logtheta.*(x+(-1).*xp).^2+(x+(-1) ...
  .*xp).^4))+a3.*dt.*exp(1).^(2.*logtheta).*((-2).*a2.*dt.*(15.*exp(1).^( ...
  3.*logtheta)+(-45).*exp(1).^(2.*logtheta).*(x+(-1).*xp).^2+15.*exp(1) ...
  .^logtheta.*(x+(-1).*xp).^4+(-1).*(x+(-1).*xp).^6)+exp(1).^logtheta.*( ...
  6.*exp(1).^(3.*logtheta)+(-3).*exp(1).^(2.*logtheta).*(5.*a1.*dt.*(ubar+ ...
  (-1).*ubarp)+4.*(x+(-1).*xp)).*(x+(-1).*xp)+(-1).*a1.*dt.*(ubar+(-1).* ...
  ubarp).*(x+(-1).*xp).^5+2.*exp(1).^logtheta.*(x+(-1).*xp).^3.*(5.*a1.* ...
  dt.*(ubar+(-1).*ubarp)+x+(-1).*xp)))+a3.^2.*dt.^2.*(105.*exp(1).^(4.* ...
  logtheta)+(-420).*exp(1).^(3.*logtheta).*(x+(-1).*xp).^2+210.*exp(1).^( ...
  2.*logtheta).*(x+(-1).*xp).^4+(-28).*exp(1).^logtheta.*(x+(-1).*xp).^6+( ...
  x+(-1).*xp).^8));

case 2 % logtheta

K=exp(1).^(logsigma+(-8).*logtheta+(-1/2).*exp(1).^((-1).*logtheta).*(x+( ...
  -1).*xp).^2).*(exp(1).^(5.*logtheta).*(6.*a2.^2.*dt.^2.*(exp(1) ...
  .^logtheta+(-1).*(x+(-1).*xp).^2)+exp(1).^logtheta.*(4.*exp(1).^(2.* ...
  logtheta)+3.*a1.*dt.*exp(1).^logtheta.*(a1.*dt.*ubar.*ubarp+(-1).*(ubar+ ...
  (-1).*ubarp).*(x+(-1).*xp))+(-2).*a1.^2.*dt.^2.*ubar.*ubarp.*(x+(-1).* ...
  xp).^2)+a2.*dt.*((-6).*exp(1).^(2.*logtheta)+2.*exp(1).^logtheta.*(3.* ...
  a1.*dt.*(ubar+(-1).*ubarp)+2.*(x+(-1).*xp)).*(x+(-1).*xp)+(-1).*a1.*dt.* ...
  (ubar+(-1).*ubarp).*(x+(-1).*xp).^3))+4.*exp(1).^(4.*logtheta).*(exp(1) ...
  .^(2.*logtheta).*(exp(1).^(2.*logtheta)+a1.*dt.*exp(1).^logtheta.*(a1.* ...
  dt.*ubar.*ubarp+(-1).*(ubar+(-1).*ubarp).*(x+(-1).*xp))+(-1).*a1.^2.* ...
  dt.^2.*ubar.*ubarp.*(x+(-1).*xp).^2)+a2.*dt.*exp(1).^logtheta.*((-2).* ...
  exp(1).^(2.*logtheta)+exp(1).^logtheta.*(3.*a1.*dt.*(ubar+(-1).*ubarp)+ ...
  2.*(x+(-1).*xp)).*(x+(-1).*xp)+(-1).*a1.*dt.*(ubar+(-1).*ubarp).*(x+(-1) ...
  .*xp).^3)+a2.^2.*dt.^2.*(3.*exp(1).^(2.*logtheta)+(-6).*exp(1) ...
  .^logtheta.*(x+(-1).*xp).^2+(x+(-1).*xp).^4))+2.*a3.*dt.*exp(1).^(2.* ...
  logtheta).*((-2).*a2.*dt.*(15.*exp(1).^(3.*logtheta)+(-45).*exp(1).^(2.* ...
  logtheta).*(x+(-1).*xp).^2+15.*exp(1).^logtheta.*(x+(-1).*xp).^4+(-1).*( ...
  x+(-1).*xp).^6)+exp(1).^logtheta.*(6.*exp(1).^(3.*logtheta)+(-3).*exp(1) ...
  .^(2.*logtheta).*(5.*a1.*dt.*(ubar+(-1).*ubarp)+4.*(x+(-1).*xp)).*(x+( ...
  -1).*xp)+(-1).*a1.*dt.*(ubar+(-1).*ubarp).*(x+(-1).*xp).^5+2.*exp(1) ...
  .^logtheta.*(x+(-1).*xp).^3.*(5.*a1.*dt.*(ubar+(-1).*ubarp)+x+(-1).*xp)) ...
  )+(exp(1).^(4.*logtheta).*(exp(1).^(2.*logtheta).*(exp(1).^(2.*logtheta) ...
  +a1.*dt.*exp(1).^logtheta.*(a1.*dt.*ubar.*ubarp+(-1).*(ubar+(-1).*ubarp) ...
  .*(x+(-1).*xp))+(-1).*a1.^2.*dt.^2.*ubar.*ubarp.*(x+(-1).*xp).^2)+a2.* ...
  dt.*exp(1).^logtheta.*((-2).*exp(1).^(2.*logtheta)+exp(1).^logtheta.*( ...
  3.*a1.*dt.*(ubar+(-1).*ubarp)+2.*(x+(-1).*xp)).*(x+(-1).*xp)+(-1).*a1.* ...
  dt.*(ubar+(-1).*ubarp).*(x+(-1).*xp).^3)+a2.^2.*dt.^2.*(3.*exp(1).^(2.* ...
  logtheta)+(-6).*exp(1).^logtheta.*(x+(-1).*xp).^2+(x+(-1).*xp).^4))+a3.* ...
  dt.*exp(1).^(2.*logtheta).*((-2).*a2.*dt.*(15.*exp(1).^(3.*logtheta)+( ...
  -45).*exp(1).^(2.*logtheta).*(x+(-1).*xp).^2+15.*exp(1).^logtheta.*(x+( ...
  -1).*xp).^4+(-1).*(x+(-1).*xp).^6)+exp(1).^logtheta.*(6.*exp(1).^(3.* ...
  logtheta)+(-3).*exp(1).^(2.*logtheta).*(5.*a1.*dt.*(ubar+(-1).*ubarp)+ ...
  4.*(x+(-1).*xp)).*(x+(-1).*xp)+(-1).*a1.*dt.*(ubar+(-1).*ubarp).*(x+(-1) ...
  .*xp).^5+2.*exp(1).^logtheta.*(x+(-1).*xp).^3.*(5.*a1.*dt.*(ubar+(-1).* ...
  ubarp)+x+(-1).*xp)))+a3.^2.*dt.^2.*(105.*exp(1).^(4.*logtheta)+(-420).* ...
  exp(1).^(3.*logtheta).*(x+(-1).*xp).^2+210.*exp(1).^(2.*logtheta).*(x+( ...
  -1).*xp).^4+(-28).*exp(1).^logtheta.*(x+(-1).*xp).^6+(x+(-1).*xp).^8)).* ...
  ((-8)+(1/2).*exp(1).^((-1).*logtheta).*(x+(-1).*xp).^2)+28.*a3.^2.* ...
  dt.^2.*exp(1).^logtheta.*(15.*exp(1).^(3.*logtheta)+(-45).*exp(1).^(2.* ...
  logtheta).*(x+(-1).*xp).^2+15.*exp(1).^logtheta.*(x+(-1).*xp).^4+(-1).*( ...
  x+(-1).*xp).^6)+a3.*dt.*exp(1).^(3.*logtheta).*(6.*exp(1).^(3.*logtheta) ...
  +(-30).*a2.*dt.*(3.*exp(1).^(2.*logtheta)+(-6).*exp(1).^logtheta.*(x+( ...
  -1).*xp).^2+(x+(-1).*xp).^4)+2.*exp(1).^logtheta.*(9.*exp(1).^(2.* ...
  logtheta)+(-3).*exp(1).^logtheta.*(5.*a1.*dt.*(ubar+(-1).*ubarp)+4.*(x+( ...
  -1).*xp)).*(x+(-1).*xp)+(x+(-1).*xp).^3.*(5.*a1.*dt.*(ubar+(-1).*ubarp)+ ...
  x+(-1).*xp))+(-3).*exp(1).^(2.*logtheta).*(5.*a1.*dt.*(ubar+(-1).*ubarp) ...
  +4.*(x+(-1).*xp)).*(x+(-1).*xp)+(-1).*a1.*dt.*(ubar+(-1).*ubarp).*(x+( ...
  -1).*xp).^5+2.*exp(1).^logtheta.*(x+(-1).*xp).^3.*(5.*a1.*dt.*(ubar+(-1) ...
  .*ubarp)+x+(-1).*xp)));

case 3 % a1

K=dt.*exp(1).^(logsigma+(-5).*logtheta+(-1/2).*exp(1).^((-1).*logtheta).*( ...
  x+(-1).*xp).^2).*(exp(1).^(2.*logtheta).*(exp(1).^logtheta.*(2.*a1.*dt.* ...
  ubar.*ubarp.*(exp(1).^logtheta+(-1).*(x+(-1).*xp).^2)+(-1).*exp(1) ...
  .^logtheta.*(ubar+(-1).*ubarp).*(x+(-1).*xp))+(-1).*a2.*dt.*(ubar+(-1).* ...
  ubarp).*((-3).*exp(1).^logtheta+(x+(-1).*xp).^2).*(x+(-1).*xp))+a3.*dt.* ...
  (ubar+(-1).*ubarp).*((-15).*exp(1).^(2.*logtheta)+10.*exp(1).^logtheta.* ...
  (x+(-1).*xp).^2+(-1).*(x+(-1).*xp).^4).*(x+(-1).*xp));

case 4 % a2

K=dt.*exp(1).^(logsigma+(-6).*logtheta+(-1/2).*exp(1).^((-1).*logtheta).*( ...
  x+(-1).*xp).^2).*(exp(1).^(2.*logtheta).*(exp(1).^logtheta.*((-2).*exp( ...
  1).^(2.*logtheta)+exp(1).^logtheta.*(3.*a1.*dt.*(ubar+(-1).*ubarp)+2.*( ...
  x+(-1).*xp)).*(x+(-1).*xp)+(-1).*a1.*dt.*(ubar+(-1).*ubarp).*(x+(-1).* ...
  xp).^3)+2.*a2.*dt.*(3.*exp(1).^(2.*logtheta)+(-6).*exp(1).^logtheta.*(x+ ...
  (-1).*xp).^2+(x+(-1).*xp).^4))+(-2).*a3.*dt.*(15.*exp(1).^(3.*logtheta)+ ...
  (-45).*exp(1).^(2.*logtheta).*(x+(-1).*xp).^2+15.*exp(1).^logtheta.*(x+( ...
  -1).*xp).^4+(-1).*(x+(-1).*xp).^6));

case 5 % a3

K=dt.*exp(1).^(logsigma+(-8).*logtheta+(-1/2).*exp(1).^((-1).*logtheta).*( ...
  x+(-1).*xp).^2).*(exp(1).^(2.*logtheta).*((-2).*a2.*dt.*(15.*exp(1).^( ...
  3.*logtheta)+(-45).*exp(1).^(2.*logtheta).*(x+(-1).*xp).^2+15.*exp(1) ...
  .^logtheta.*(x+(-1).*xp).^4+(-1).*(x+(-1).*xp).^6)+exp(1).^logtheta.*( ...
  6.*exp(1).^(3.*logtheta)+(-3).*exp(1).^(2.*logtheta).*(5.*a1.*dt.*(ubar+ ...
  (-1).*ubarp)+4.*(x+(-1).*xp)).*(x+(-1).*xp)+(-1).*a1.*dt.*(ubar+(-1).* ...
  ubarp).*(x+(-1).*xp).^5+2.*exp(1).^logtheta.*(x+(-1).*xp).^3.*(5.*a1.* ...
  dt.*(ubar+(-1).*ubarp)+x+(-1).*xp)))+2.*a3.*dt.*(105.*exp(1).^(4.* ...
  logtheta)+(-420).*exp(1).^(3.*logtheta).*(x+(-1).*xp).^2+210.*exp(1).^( ...
  2.*logtheta).*(x+(-1).*xp).^4+(-28).*exp(1).^logtheta.*(x+(-1).*xp).^6+( ...
  x+(-1).*xp).^8));

otherwise

        K = zeros(n_x, n_xp);
end

if K == 0

    K = zeros(n_x, n_xp);

end

end


function obj = t2_pnp_func_new(in1,in2,in3)
coefs_tq1_1 = in2(1);
coefs_tq1_2 = in2(4);
coefs_tq2_1 = in2(2);
coefs_tq1_3 = in2(7);
coefs_tq2_2 = in2(5);
coefs_tq3_1 = in2(3);
coefs_tq1_4 = in2(10);
coefs_tq2_3 = in2(8);
coefs_tq3_2 = in2(6);
coefs_tq1_5 = in2(13);
coefs_tq2_4 = in2(11);
coefs_tq3_3 = in2(9);
coefs_tq1_6 = in2(16);
coefs_tq2_5 = in2(14);
coefs_tq3_4 = in2(12);
coefs_tq1_7 = in2(19);
coefs_tq2_6 = in2(17);
coefs_tq3_5 = in2(15);
coefs_tq1_8 = in2(22);
coefs_tq2_7 = in2(20);
coefs_tq3_6 = in2(18);
coefs_tq1_9 = in2(25);
coefs_tq2_8 = in2(23);
coefs_tq3_7 = in2(21);
coefs_tq2_9 = in2(26);
coefs_tq3_8 = in2(24);
coefs_tq3_9 = in2(27);
coefs_tq1_10 = in2(28);
coefs_tq2_10 = in2(29);
coefs_tq3_10 = in2(30);
pinvG2_1 = in1(2);
pinvG2_2 = in1(5);
pinvG2_3 = in1(8);
q0 = in3(1,:);
q1 = in3(2,:);
q2 = in3(3,:);
q3 = in3(4,:);
obj = q0.^2.*(coefs_tq1_1.*pinvG2_1+coefs_tq2_1.*pinvG2_2+coefs_tq3_1.*pinvG2_3)+q1.^2.*(coefs_tq1_5.*pinvG2_1+coefs_tq2_5.*pinvG2_2+coefs_tq3_5.*pinvG2_3)+q2.^2.*(coefs_tq1_8.*pinvG2_1+coefs_tq2_8.*pinvG2_2+coefs_tq3_8.*pinvG2_3)+q3.^2.*(coefs_tq1_10.*pinvG2_1+coefs_tq2_10.*pinvG2_2+coefs_tq3_10.*pinvG2_3)+q0.*q1.*(coefs_tq1_2.*pinvG2_1+coefs_tq2_2.*pinvG2_2+coefs_tq3_2.*pinvG2_3)+q0.*q2.*(coefs_tq1_3.*pinvG2_1+coefs_tq2_3.*pinvG2_2+coefs_tq3_3.*pinvG2_3)+q0.*q3.*(coefs_tq1_4.*pinvG2_1+coefs_tq2_4.*pinvG2_2+coefs_tq3_4.*pinvG2_3)+q1.*q2.*(coefs_tq1_6.*pinvG2_1+coefs_tq2_6.*pinvG2_2+coefs_tq3_6.*pinvG2_3)+q1.*q3.*(coefs_tq1_7.*pinvG2_1+coefs_tq2_7.*pinvG2_2+coefs_tq3_7.*pinvG2_3)+q2.*q3.*(coefs_tq1_9.*pinvG2_1+coefs_tq2_9.*pinvG2_2+coefs_tq3_9.*pinvG2_3);


function out = feval(F, x, varargin)
%FEVAL   Evaluate a CHEBFUN.
%   FEVAL(F, X) evaluates a CHEBFUN F at the points in X.  If F is a quasimatrix
%   with columns F1, ..., FN, then the result will be [F1(X), ..., FN(X)], the
%   horizontal concatenation of the results of evaluating each column at the
%   points in X.
%
%   FEVAL(F, 'left'), FEVAL(F, 'start'), and FEVAL(F, '-') return the value of F
%   at the left endpoint of its domain.  FEVAL(F, 'right'), FEVAL(F, 'end'), and
%   FEVAL(F, '+') do the same for the right endpoint.
%
%   FEVAL(F, X, 'left') and FEVAL(F, X, '-') evaluate F at the points in X,
%   using left-hand limits to evaluate F at any breakpoints. FEVAL(F, X,
%   'right') and FEVAL(F, X, '+') do the same but using right-hand limits.
%
%   F(X), F('left'), F(X, 'left'), etc, are equivalent syntaxes.
%
%   Example:
%     f = chebfun(@(x) 1./(1 + 25*x.^2));
%     y = feval(f, linspace(-1, 1, 100).');
%
% See also SUBSREF.

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

% If F or x are empty, there's nothing to do.
if ( isempty(F) )
    out = [];
    return
elseif ( isempty(x) )
    % Return empty matrix with dimensions of the appropriate size.
    out = zeros(size(x));
    return
end

% Deal with FEVAL(FUNCTION_HANDLE, CHEBFUN) type calls.
if ( isa(F, 'function_handle') )
    out = F(x, varargin{:});
    return
end

% Deal with FEVAL(CHEBFUN, CHEBFUN) type calls.
if ( isa(x, 'chebfun') )
    out = compose(x, F);
    return
end

%% LEFT / RIGHT / END VALUES:
% Support for feval(f, 'left') and feval(f, 'end'), etc.
if ( ischar(x) )
    if ( any(strcmpi(x, {'left', 'start' ,'-'})) )
        out = F.pointValues(1,:);
    elseif ( any(strcmpi(x, {'right', 'end', '+'})) )
        out = F.pointValues(end,:);
    else
        error('CHEBFUN:CHEBFUN:feval:strInput', ...
            'Unknown input argument "%s".', x);
    end
    if ( F(1).isTransposed )
        out = out.';
    end
    return
end

%% EVALUATE EACH COLUMN:
if ( numel(F) == 1 )
    out = columnFeval(F, x, varargin{:});
else
    % Deal with quasimatrices:
    out = cell(1, numel(F));
    for k = 1:numel(F)
        out{k} = columnFeval(F(k), x, varargin{:});
    end
    out = cell2mat(out);
end

%% DEAL WITH ROW CHEBFUNS:
if ( F(1).isTransposed )
    % We got a passed a row CHEBFUN. If X had more than two dimensions, we can't
    % simply transpose the output from above, instead, we need to use permute.
    ndimsx = ndims(x);
    if ( ndimsx <= 2 )
        out = out.';
    else
        % We define "transposition" in this case to mean the switching of the
        % first two dimensions.
        out = permute(out, [2 1 3:ndimsx]);
    end
end

end

function out = columnFeval(f, x, varargin)
% Act on each column. Note that columnFeval ignores the transpose state of F.

%% INITIALISE:

% Reshape x to be a column vector.
sizex = size(x);
ndimsx = ndims(x);
x = x(:);

% Initialise output:
numFuns = numel(f.funs);

funs = f.funs;
dom = f.domain;
isTrans = f.isTransposed;

%% LEFT AND RIGHT LIMITS:
% Deal with feval(f, x, 'left') and feval(f, x, 'right'):
leftFlag = 0; rightFlag = 0;
if ( nargin > 2 )
    lr = varargin{1};
    leftFlag = any(strcmpi(lr, {'left', '-'}));
    rightFlag = any(strcmpi(lr, {'right', '+'}));
    if ( ~(leftFlag || rightFlag) )
        if ( ischar(lr) )
            error('CHEBFUN:CHEBFUN:feval:leftRightChar',...
                'Unknown input argument "%s".', lr);
        else
            error('CHEBFUN:CHEBFUN:feval:leftRight', 'Unknown input argument.');
        end
    end
end

%% VALUES FROM FUNS:

if ( numFuns == 1 )

    % Things are simple when there is only a single FUN:
    out = feval(funs{1}, x(:), varargin{:});
    numCols = size(out, 2);

else

    % For multiple FUNs we must determine which FUN corresponds to each x.

    % Initialise output matrix:
    numCols = numColumns(f);
    out = zeros(numel(x), numCols);

    % Replace the first and last domain entries with +/-inf. (Since we want to
    % use FUN{1} if real(x) < dom(1) and FUN{end} if real(x) > dom(end)).
    domInf = [-inf, dom(2:end-1), inf];

    % Loop over each FUN. If real(x) is in [dom(k) dom(k+1)] then use FUN{k}.
    xReal = real(x);
    for k = 1:numFuns
        I = ( xReal >= domInf(k) ) & ( xReal < domInf(k+1) );
        if ( any(I(:)) )
            % Evaluate the appropriate fun:
            out(I,:) = feval(funs{k}, x(I), varargin{:});
        end
    end

end

%% POINTVALUES:
% If the evaluation point corresponds to a breakpoint, we get the value from

% f.pointValues. However, if the 'left' or 'right' flag is given, we first
% modify the entry in point values to take either the left- or right-sided
% limit, respectively.

[xAtBreaks, domIndices] = ismember(x, dom);
if ( any(xAtBreaks) )
    if ( leftFlag )
        % Note that for leftFlag we use 'rval-local', which corresponds to the
        % function value at the right part of the subdomain.
        rvals = get(f, 'rval-local'); if ( isTrans ), rvals = rvals.'; end
        pointValues = [f.pointValues(1,:); rvals];
    elseif ( rightFlag )
        % Similarly rightFlag uses lval-local.
        lvals = get(f, 'lval-local'); if ( isTrans ), lvals = lvals.'; end
        pointValues = [lvals; f.pointValues(end,:)];
    else
        pointValues = f.pointValues;
    end
    % Set the output values for any values of x at the breakpoints.
    out(xAtBreaks,:) = pointValues(domIndices(xAtBreaks),:);
end

%% RESHAPE FOR OUTPUT:
% Reshape fx, which is a column vector or horizontal concatenation of column
% vectors, to be of the appropriate size, and handle transposition.
sizefx = sizex;
sizefx(2) = numCols*sizex(2);
if ( ndimsx == 2 )
    % If x was just a matrix or vector, the reshape is straightforward.
    out = reshape(out, sizefx);
else
    % If x had more than two dimensions, we have to be more careful.  The
    % cell2mat(mat2cell(...).') effects a transpose which keeps certain
    % columnar blocks of the fx matrix intact, putting the entries in the
    % correct order for reshape().
    blockLength = sizex(1)*sizex(2);
    blocksPerCol = prod(sizex(3:end));
    out = reshape(cell2mat(mat2cell(out, blockLength*ones(1, blocksPerCol), ...
        ones(1, numCols)).'), sizefx);
end

end


%SerialLink.ikine Numerical inverse kinematics
%
% Q = R.ikine(T) are the joint coordinates (1xN) corresponding to the robot
% end-effector pose T (4x4) which is a homogenenous transform.
%
% Q = R.ikine(T, Q0, OPTIONS) specifies the initial estimate of the joint
% coordinates.
%
% This method can be used for robots with 6 or more degrees of freedom.
%
% Underactuated robots::
%
% For the case where the manipulator has fewer than 6 DOF the solution
% space has more dimensions than can be spanned by the manipulator joint
% coordinates.
%
% Q = R.ikine(T, Q0, M, OPTIONS) similar to above but where M is a mask
% vector (1x6) which specifies the Cartesian DOF (in the wrist coordinate
% frame) that will be ignored in reaching a solution.  The mask vector
% has six elements that correspond to translation in X, Y and Z, and rotation
% about X, Y and Z respectively.  The value should be 0 (for ignore) or 1.
% The number of non-zero elements should equal the number of manipulator DOF.
%
% For example when using a 3 DOF manipulator rotation orientation might be
% unimportant in which case  M = [1 1 1 0 0 0].
%
% For robots with 4 or 5 DOF this method is very difficult to use since
% orientation is specified by T in world coordinates and the achievable
% orientations are a function of the tool position.
%
% Trajectory operation::
%
% In all cases if T is 4x4xM it is taken as a homogeneous transform sequence
% and R.ikine() returns the joint coordinates corresponding to each of the
% transforms in the sequence.  Q is MxN where N is the number of robot joints.
% The initial estimate of Q for each time step is taken as the solution
% from the previous time step.
%
% Options::
% 'pinv'         use pseudo-inverse instead of Jacobian transpose (default)
% 'ilimit',L     set the maximum iteration count (default 1000)
% 'tol',T        set the tolerance on error norm (default 1e-6)
% 'alpha',A      set step size gain (default 1)
% 'varstep'      enable variable step size if pinv is false
% 'verbose'      show number of iterations for each point
% 'verbose=2'    show state at each iteration
% 'plot'         plot iteration state versus time
%
% References::
% - Robotics, Vision & Control, Section 8.4,
%   P. Corke, Springer 2011.
%
% Notes::
% - Solution is computed iteratively.
% - Solution is sensitive to choice of initial gain.  The variable
%   step size logic (enabled by default) does its best to find a balance
%   between speed of convergence and divergence.
% - Some experimentation might be required to find the right values of
%   tol, ilimit and alpha.
% - The pinv option leads to much faster convergence (default)
% - The tolerance is computed on the norm of the error between current
%   and desired tool pose.  This norm is computed from distances
%   and angles without any kind of weighting.
% - The inverse kinematic solution is generally not unique, and
%   depends on the initial guess Q0 (defaults to 0).
% - The default value of Q0 is zero which is a poor choice for most
%   manipulators (eg. puma560, twolink) since it corresponds to a kinematic
%   singularity.
% - Such a solution is completely general, though much less efficient
%   than specific inverse kinematic solutions derived symbolically, like
%   ikine6s or ikine3.
% - This approach allows a solution to be obtained at a singularity, but
%   the joint angles within the null space are arbitrarily assigned.
% - Joint offsets, if defined, are added to the inverse kinematics to
%   generate Q.
% - Joint limits are not considered in this solution.
%
% See also SerialLink.ikcon, SerialLink.ikunc, SerialLink.fkine, SerialLink.ikine6s.

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

function [qt,histout] = ikine(robot, tr, varargin)

    %  set default parameters for solution
    opt.ilimit = 1000;
    opt.tol = 1e-6;
    opt.alpha = 0.9;
    opt.plot = false;
    opt.pinv = true;
    opt.varstep = false;

    [opt,args] = tb_optparse(opt, varargin);

    n = robot.n;

    % robot.ikine(tr, q)
    if ~isempty(args)
        q = args{1};
        if isempty(q)
            q = zeros(1, n);
        else
            q = q(:)';
        end
    else
        q = zeros(1, n);
    end

    % robot.ikine(tr, q, m)
    if length(args) > 1
        m = args{2};
        m = m(:);
        if numel(m) ~= 6
            error('RTB:ikine:badarg', 'Mask matrix should have 6 elements');
        end
        if numel(find(m)) > robot.n
            error('RTB:ikine:badarg', 'Number of robot DOF must be >= the same number of 1s in the mask matrix')
        end
    else
        if n < 6
            error('RTB:ikine:badarg', 'For a manipulator with fewer than 6DOF a mask matrix argument must be specified');
        end
        m = ones(6, 1);
    end
    % make this a logical array so we can index with it
    m = logical(m);

    npoints = size(tr,3);    % number of points
    qt = zeros(npoints, n);  % preallocate space for results
    tcount = 0;              % total iteration count

    if ~ishomog(tr)
        error('RTB:ikine:badarg', 'T is not a homog xform');
    end

    J0 = jacob0(robot, q);
    J0 = J0(m, :);
    if cond(J0) > 100
        warning('RTB:ikine:singular', 'Initial joint configuration results in a (near-)singular configuration, this may slow convergence');
    end

    history = [];
    failed = false;
    e = zeros(6,1);
    revolutes = [robot.links.sigma] == 0;

    for i=1:npoints
        T = tr(:,:,i);

        nm = Inf;
        % initialize state for the ikine loop
        eprev = [Inf Inf Inf Inf Inf Inf];
        save.e = [Inf Inf Inf Inf Inf Inf];
        save.q = [];
        count = 0;

        while true
            % update the count and test against iteration limit
            count = count + 1;
            if count > opt.ilimit
                warning('ikine: iteration limit %d exceeded (row %d), final err %f', ...
                    opt.ilimit, i, nm);
                failed = true;
                %q = NaN*ones(1,n);
                break
            end

            % compute the error
            Tq = robot.fkine(q');

            e(1:3) = transl(T - Tq);
            Rq = t2r(Tq);
            [th,n] = tr2angvec(Rq'*t2r(T));
            e(4:6) = th*n;

            % optionally adjust the step size
            if opt.varstep
                % test against last best error, only consider the DOF of
                % interest
                if norm(e(m)) < norm(save.e(m))
                    % error reduced,
                    % let's save current state of solution and rack up the step size
                    save.q = q;
                    save.e = e;
                    opt.alpha = opt.alpha * (2.0^(1.0/8));
                    if opt.verbose > 1
                        fprintf('step %d: raise alpha to %f\n', count, opt.alpha);
                    end
                else
                    % rats!  error got worse,
                    % restore to last good solution and reduce step size
                    q = save.q;
                    e = save.e;
                    opt.alpha = opt.alpha * 0.5;
                    if opt.verbose > 1
                        fprintf('step %d: drop alpha to %f\n', count, opt.alpha);
                    end
                end
            end

            % compute the Jacobian
            J = jacob0(robot, q);

            % compute change in joint angles to reduce the error,
            % based on the square sub-Jacobian
            if opt.pinv
                %pinv( J(m,:) )
                dq = opt.alpha * pinv( J(m,:) ) * e(m);
            else
                dq = J(m,:)' * e(m);
                dq = opt.alpha * dq;
            end

            % diagnostic stuff
            if opt.verbose > 1
                fprintf('%d/%d: |e| = %f\n', i, count, nm);
                fprintf('       e  = '); disp(e');
                fprintf('       dq = '); disp(dq');
            end
            if opt.plot
                h.q = q';
                h.dq = dq;
                h.e = e;
                h.ne = nm;
                h.alpha = opt.alpha;
                history = [history; h]; %#ok<*AGROW>
            end

            % update the estimated solution
            q = q + dq';

            % wrap angles for revolute joints
            k = (q > pi) & revolutes;
            q(k) = q(k) - 2*pi;

            k = (q < -pi) & revolutes;
            q(k) = q(k) + 2*pi;

            nm = norm(e(m));

            if norm(e(m)) > 2*norm(eprev(m))
                warning('RTB:ikine:diverged', 'solution diverging at step %d, try reducing alpha', count);
            end
            eprev = e;

            if nm <= opt.tol
                break
            end

        end  % end ikine solution for tr(:,:,i)
        qt(i,:) = q';
        tcount = tcount + count;
        if opt.verbose
            fprintf('%d iterations\n', count);
        end
    end

    if opt.verbose && npoints > 1
        fprintf('TOTAL %d iterations\n', tcount);
    end

    % plot evolution of variables
    if opt.plot
        figure(1);
        subplot(511)
        plot([history.q]');
        ylabel('q');
        grid

        subplot(512)
        plot([history.dq]');
        ylabel('dq');
        grid

        subplot(513)
        plot([history.e]');
        ylabel('e');
        grid

        subplot(514)
        semilogy([history.ne]);
        ylabel('|e|');
        grid

        subplot(515)
        plot([history.alpha]);
        xlabel('iteration');
        ylabel('\alpha');
        grid

        if nargout > 1
            histout = history;
        end
    end
end


%% Create a TSE sequence and export for execution
%
% The |Sequence| class provides functionality to create magnetic
% resonance sequences (MRI or NMR) from basic building blocks.
%
% This provides an implementation of the open file format for MR sequences
% described here: http://pulseq.github.io/specification.pdf
%
% This example performs the following steps:
%
% # Create slice selective RF pulse for imaging.
% # Create readout gradient and phase encode strategy.
% # Loop through phase encoding and generate sequence blocks.
% # Write the sequence to an open file format suitable for execution on a
% scanner.
%
%   Juergen Hennig <juergen.hennig@uniklinik-freiburg.de>
%   Maxim Zaitsev  <maxim.zaitsev@uniklinik-freiburg.de>

%% Instantiation and gradient limits
% The system gradient limits can be specified in various units _mT/m_,
% _Hz/cm_, or _Hz/m_. However the limits will be stored internally in units
% of _Hz/m_ for amplitude and _Hz/m/s_ for slew. Unspecificied hardware
% parameters will be assigned default values.

dG=250e-6;
system = mr.opts('MaxGrad', 30, 'GradUnit', 'mT/m', ...
    'MaxSlew', 170, 'SlewUnit', 'T/m/s', 'rfRingdownTime', 100e-6, ...
    'rfDeadTime', 100e-6, 'adcDeadTime', 10e-6);

%%
% A new sequence object is created by calling the class constructor.
seq=mr.Sequence(system);

%% Sequence events
% Some sequence parameters are defined using standard MATLAB variables
fov=256e-3;
Nx=128; Ny=128; necho=16; Nslices=1;
rflip=180;
if (numel(rflip)==1), rflip=rflip+zeros([1 necho]); end
sliceThickness=5e-3;
TE=12e-3; TR=2000e-3;
TEeff=60e-3;
k0=round(TEeff/TE);
PEtype='linear';

samplingTime= 6.4e-3;
readoutTime = samplingTime + 2*system.adcDeadTime;
tEx=2.5e-3;
tExwd=tEx+system.rfRingdownTime+system.rfDeadTime;
tRef=2e-3;
tRefwd=tRef+system.rfRingdownTime+system.rfDeadTime;
tSp=0.5*(TE-readoutTime-tRefwd);
tSpex=0.5*(TE-tExwd-tRefwd);
fspR=1.0;
fspS=0.5;

rfex_phase=pi/2; % MZ: we need to maintain these as variables because we will overwrtite phase offsets for multiple slice positions
rfref_phase=0;

%%
%%% Base gradients
%%% Slice selection
% Key concepts in the sequence description are *blocks* and *events*.
% Blocks describe a group of events that are executed simultaneously. This
% hierarchical structure means that one event can be used in multiple
% blocks, a common occurrence in MR sequences, particularly in imaging
% sequences.
%
% First, the slice selective RF pulses (and corresponding slice gradient)
% are generated using the |makeSincPulse| function.
% Gradients are recalculated such that their flattime covers the pulse plus
% the rfdead- and rfringdown- times.
%
flipex=90*pi/180;
[rfex, gz] = mr.makeSincPulse(flipex,system,'Duration',tEx,...
    'SliceThickness',sliceThickness,'apodization',0.5,'timeBwProduct',4,'PhaseOffset',rfex_phase);
GSex = mr.makeTrapezoid('z',system,'amplitude',gz.amplitude,'FlatTime',tExwd,'riseTime',dG);
% plotPulse(rfex,GSex);

flipref=rflip(1)*pi/180;
[rfref, gz] = mr.makeSincPulse(flipref,system,'Duration',tRef,...
    'SliceThickness',sliceThickness,'apodization',0.5,'timeBwProduct',4,'PhaseOffset',rfref_phase,'use','refocusing');
GSref = mr.makeTrapezoid('z',system,'amplitude',GSex.amplitude,'FlatTime',tRefwd,'riseTime',dG);
% plotPulse(rfref,GSref);

AGSex=GSex.area/2;
GSspr = mr.makeTrapezoid('z',system,'area',AGSex*(1+fspS),'duration',tSp,'riseTime',dG);
GSspex = mr.makeTrapezoid('z',system,'area',AGSex*fspS,'duration',tSpex,'riseTime',dG);

%%
%%% Readout gradient
% To define the remaining encoding gradients we need to calculate the
% $k$-space sampling. The Fourier relationship
%
% $$\Delta k = \frac{1}{FOV}$$
%
% Therefore the area of the readout gradient is $n\Delta k$.
deltak=1/fov;
kWidth = Nx*deltak;

GRacq = mr.makeTrapezoid('x',system,'FlatArea',kWidth,'FlatTime',readoutTime,'riseTime',dG);
adc = mr.makeAdc(Nx,'Duration',samplingTime, 'Delay', system.adcDeadTime);%,'Delay',GRacq.riseTime);
GRspr = mr.makeTrapezoid('x',system,'area',GRacq.area*fspR,'duration',tSp,'riseTime',dG);
GRspex = mr.makeTrapezoid('x',system,'area',GRacq.area*(1+fspR),'duration',tSpex,'riseTime',dG);

AGRspr=GRspr.area;%GRacq.area/2*fspR;
AGRpreph = GRacq.area/2+AGRspr;%GRacq.area*(1+fspR)/2;
GRpreph = mr.makeTrapezoid('x',system,'Area',AGRpreph,'duration',tSpex,'riseTime',dG);

%%
%%% Phase encoding
% To move the $k$-space trajectory away from 0 prior to the readout a
% prephasing gradient must be used. Furthermore rephasing of the slice
% select gradient is required.

%[PEorder,Ny] = myTSE_PEorder(Ny,necho,k0,PEtype);
nex=floor(Ny/necho);
pe_steps=(1:(necho*nex))-0.5*necho*nex-1;
if 0==mod(necho,2)
    pe_steps=circshift(pe_steps,[0,-round(nex/2)]); % for odd number of echoes we have to apply a shift to avoid a contrast jump at k=0
end
PEorder=reshape(pe_steps,[nex,necho])';
phaseAreas = PEorder*deltak;

%% split gradients and recombine into blocks
% lets start with slice selection....
GS1times=[0 GSex.riseTime];
GS1amp=[0 GSex.amplitude];
GS1 = mr.makeExtendedTrapezoid('z','times',GS1times,'amplitudes',GS1amp);

GS2times=[0 GSex.flatTime];
GS2amp=[GSex.amplitude GSex.amplitude];
GS2 = mr.makeExtendedTrapezoid('z','times',GS2times,'amplitudes',GS2amp);

GS3times=[0 GSspex.riseTime GSspex.riseTime+GSspex.flatTime GSspex.riseTime+GSspex.flatTime+GSspex.fallTime];
GS3amp=[GSex.amplitude GSspex.amplitude GSspex.amplitude GSref.amplitude];
GS3 = mr.makeExtendedTrapezoid('z','times',GS3times,'amplitudes',GS3amp);

GS4times=[0 GSref.flatTime];
GS4amp=[GSref.amplitude GSref.amplitude];
GS4 = mr.makeExtendedTrapezoid('z','times',GS4times,'amplitudes',GS4amp);

GS5times=[0 GSspr.riseTime GSspr.riseTime+GSspr.flatTime GSspr.riseTime+GSspr.flatTime+GSspr.fallTime];
GS5amp=[GSref.amplitude GSspr.amplitude GSspr.amplitude 0];
GS5 = mr.makeExtendedTrapezoid('z','times',GS5times,'amplitudes',GS5amp);

GS7times=[0 GSspr.riseTime GSspr.riseTime+GSspr.flatTime GSspr.riseTime+GSspr.flatTime+GSspr.fallTime];
GS7amp=[0 GSspr.amplitude GSspr.amplitude GSref.amplitude];
GS7 = mr.makeExtendedTrapezoid('z','times',GS7times,'amplitudes',GS7amp);

% and now the readout gradient....

GR3=GRpreph;%GRspex;

GR5times=[0 GRspr.riseTime GRspr.riseTime+GRspr.flatTime GRspr.riseTime+GRspr.flatTime+GRspr.fallTime];
GR5amp=[0 GRspr.amplitude GRspr.amplitude GRacq.amplitude];
GR5 = mr.makeExtendedTrapezoid('x','times',GR5times,'amplitudes',GR5amp);

GR6times=[0 readoutTime];
GR6amp=[GRacq.amplitude GRacq.amplitude];
GR6 = mr.makeExtendedTrapezoid('x','times',GR6times,'amplitudes',GR6amp);

GR7times=[0 GRspr.riseTime GRspr.riseTime+GRspr.flatTime GRspr.riseTime+GRspr.flatTime+GRspr.fallTime];
GR7amp=[GRacq.amplitude GRspr.amplitude GRspr.amplitude 0];
GR7 = mr.makeExtendedTrapezoid('x','times',GR7times,'amplitudes',GR7amp);

% and filltimes
%tex=GS1.t(end)+GS2.t(end)+GS3.t(end);
%tref=GS4.t(end)+GS5.t(end)+GS7.t(end)+readoutTime;
%tend=GS4.t(end)+GS5.t(end);
tex=mr.calcDuration(GS1)+mr.calcDuration(GS2)+mr.calcDuration(GS3);
tref=mr.calcDuration(GS4)+mr.calcDuration(GS5)+mr.calcDuration(GS7)+readoutTime;
tend=mr.calcDuration(GS4)+mr.calcDuration(GS5);

tETrain=tex+necho*tref+tend;
TRfill=(TR-Nslices*tETrain)/Nslices;
% round to gradient raster
TRfill=system.gradRasterTime * round(TRfill / system.gradRasterTime);
if TRfill<0, TRfill=1e-3;
    disp(strcat('Warning!!! TR too short, adapted to include all slices to : ',num2str(1000*Nslices*(tETrain+TRfill)),' ms'));
else
    disp(strcat('TRfill : ',num2str(1000*TRfill),' ms'));
end
delayTR = mr.makeDelay(TRfill);

%% Define sequence blocks
% Next, the blocks are put together to form the sequence
for kex=0:nex % MZ: we start at 0 to have one dummy
    for s=1:Nslices
        rfex.freqOffset=GSex.amplitude*sliceThickness*(s-1-(Nslices-1)/2);
        rfref.freqOffset=GSref.amplitude*sliceThickness*(s-1-(Nslices-1)/2);
        rfex.phaseOffset=rfex_phase-2*pi*rfex.freqOffset*mr.calcRfCenter(rfex); % align the phase for off-center slices
        rfref.phaseOffset=rfref_phase-2*pi*rfref.freqOffset*mr.calcRfCenter(rfref); % dito

        seq.addBlock(GS1);
        seq.addBlock(GS2,rfex);
        seq.addBlock(GS3,GR3);
        %GS4.first=GS4f;
        %GS4.first=GS3.last;
        for kech=1:necho,
            if (kex>0)
                phaseArea=phaseAreas(kech,kex);
            else
                phaseArea=0;
            end
            GPpre = mr.makeTrapezoid('y',system,'Area',phaseArea,'Duration',tSp,'riseTime',dG);
            GPrew = mr.makeTrapezoid('y',system,'Area',-phaseArea,'Duration',tSp,'riseTime',dG);
            seq.addBlock(GS4,rfref);
            seq.addBlock(GS5,GR5,GPpre);
            if (kex>0)
                seq.addBlock(GR6,adc);
            else
                seq.addBlock(GR6);
            end
            seq.addBlock(GS7,GR7,GPrew);
            %GS4.first=GS7.last;
        end
        seq.addBlock(GS4);
        seq.addBlock(GS5);
        seq.addBlock(delayTR);
    end
end

%% check whether the timing of the sequence is correct
[ok, error_report]=seq.checkTiming;

if (ok)
    fprintf('Timing check passed successfully\n');
else
    fprintf('Timing check failed! Error listing follows:\n');
    fprintf([error_report{:}]);
    fprintf('\n');
end

%% k-space trajectory calculation
[ktraj_adc, t_adc, ktraj, t_ktraj, t_excitation, t_refocusing] = seq.calculateKspacePP();

% plot k-spaces
figure; plot(t_ktraj,ktraj'); title('k-space components as functions of time'); % plot the entire k-space trajectory
figure; plot(ktraj(1,:),ktraj(2,:),'b',...
             ktraj_adc(1,:),ktraj_adc(2,:),'r.'); % a 2D plot
axis('equal'); % enforce aspect ratio for the correct trajectory display
title('2D k-space');

%% Write to file

% The sequence is written to file in compressed form according to the file
% format specification using the |write| method.
seq.write('tse.seq')

%%
% Display the first few lines of the output file
% s=fileread('myTSE.seq');
% disp(s(1:300))
seq.plot();

% seq.install('siemens');


%TRPLOT Draw a coordinate frame
%
% TRPLOT(T, OPTIONS) draws a 3D coordinate frame represented by the homogeneous
% transform T (4x4).
%
% H = TRPLOT(T, OPTIONS) as above but returns a handle.
%
% TRPLOT(H, T) moves the coordinate frame described by the handle H to
% the pose T (4x4).
%
% TRPLOT(R, OPTIONS) as above but the coordinate frame is rotated about the
% origin according to the orthonormal rotation matrix R (3x3).
%
% H = TRPLOT(R, OPTIONS) as above but returns a handle.
%
% TRPLOT(H, R) moves the coordinate frame described by the handle H to
% the orientation R.
%
% Options::
% 'color',C          The color to draw the axes, MATLAB colorspec C
% 'noaxes'           Don't display axes on the plot
% 'axis',A           Set dimensions of the MATLAB axes to A=[xmin xmax ymin ymax zmin zmax]
% 'frame',F          The coordinate frame is named {F} and the subscript on the axis labels is F.
% 'text_opts', opt   A cell array of MATLAB text properties
% 'handle',H         Draw in the MATLAB axes specified by the axis handle H
% 'view',V           Set plot view parameters V=[az el] angles, or 'auto'
%                    for view toward origin of coordinate frame
% 'length',s         Length of the coordinate frame arms (default 1)
% 'arrow'            Use arrows rather than line segments for the axes
% 'width', w         Width of arrow tips (default 1)
% 'thick',t          Thickness of lines (default 0.5)
% '3d'               Plot in 3D using anaglyph graphics
% 'anaglyph',A       Specify anaglyph colors for '3d' as 2 characters for
%                    left and right (default colors 'rc'): chosen from
%                    r)ed, g)reen, b)lue, c)yan, m)agenta.
% 'dispar',D         Disparity for 3d display (default 0.1)
% 'text'             Enable display of X,Y,Z labels on the frame
% 'labels',L         Label the X,Y,Z axes with the 1st, 2nd, 3rd character of the string L
% 'rgb'              Display X,Y,Z axes in colors red, green, blue respectively
% 'rviz'             Display chunky rviz style axes
%
% Examples::
%
%       trplot(T, 'frame', 'A')
%       trplot(T, 'frame', 'A', 'color', 'b')
%       trplot(T1, 'frame', 'A', 'text_opts', {'FontSize', 10, 'FontWeight', 'bold'})
%       trplot(T1, 'labels', 'NOA');
%
%       h = trplot(T, 'frame', 'A', 'color', 'b');
%       trplot(h, T2);
%
% 3D anaglyph plot
%       trplot(T, '3d');
%
% Notes::
% - The 'rviz' option is equivalent to 'rgb', 'notext', 'noarrow',
%   'thick', 5.
% - The arrow option requires the third party package arrow3 from File
%   Exchange.
% - The handle H is an hgtransform object.
% - When using the form TRPLOT(H, ...) to animate a frame it is best to set
%   the axis bounds.
% - The '3d' option requires that the plot is viewed with anaglyph glasses.
% - You cannot specify 'color' and '3d' at the same time.
%
% See also TRPLOT2, TRANIMATE.

%TODO:
% 'rviz', chunky RGB lines, no arrows

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

% TODO
%  need to decide how to handle scaling
%  what does hold on mean?  don't touch scaling?

function hout = trplot(T, varargin)

    if isscalar(T) && ishandle(T)
        % trplot(H, T)
        H = T; T = varargin{1};
        if isrot(T)
            T = r2t(T);
        end
        set(H, 'Matrix', T);

        % for the 3D case retrieve the right hgtransform and set it
        hg2 = get(H, 'UserData');
        if ~isempty(hg2)
            set(hg2, 'Matrix', T);
        end

        return;
    end

    if size(T,3) > 1
        error('trplot cannot operate on a sequence');
    end
    if ~ishomog(T) && ~isrot(T)
        error('trplot operates only on transform (4x4) or rotation matrix (3x3)');
    end

    opt.color = [];
    opt.rgb = false;
    opt.axes = true;
    opt.axis = [];
    opt.frame = [];
    opt.text_opts = [];
    opt.view = [];
    opt.width = 1;
    opt.arrow = false;
    opt.labels = 'XYZ';
    opt.handle = [];
    opt.anaglyph = 'rc';
    opt.d_3d = false;
    opt.dispar = 0.1;
    opt.thick = 0.5;
    opt.length = 1;
    opt.text = true;
    opt.lefty = false;
    opt.rviz = false;

    opt = tb_optparse(opt, varargin);

    if opt.rviz
        opt.thick = 5;
        opt.arrow = false;
        opt.rgb = true;
        opt.text = false;
    end

    if ~isempty(opt.color) && opt.d_3d
        error('cannot specify ''color'' and ''3d'', use ''anaglyph'' option');
    end
    if isempty(opt.color)
        opt.color = 'b';
    end
    if isempty(opt.text_opts)
        opt.text_opts = {};
    end

    if opt.d_3d
        opt.color = ag_color(opt.anaglyph(1));
    end

    if isempty(opt.axis)
        % determine some default axis dimensions

        % get the origin of the frame
        if isrot(T)
            c = [0 0 0];  % at zero for a rotation matrix
        else
            c = transl(T);
        end

        d = 1.2;
        opt.axis = [c(1)-d c(1)+d c(2)-d c(2)+d c(3)-d c(3)+d];

    end

    % TODO: should do the 2D case as well

    if ~isempty(opt.handle)
        hax = opt.handle;
        hold(hax);
    else
        ih = ishold;
        if ~ih
            % if hold is not on, then clear the axes and set scaling
            cla
            if ~isempty(opt.axis)
                axis(opt.axis);
            end
            daspect([1 1 1]);

            if opt.axes
                xlabel( 'X');
                ylabel( 'Y');
                zlabel( 'Z');
                rotate3d on
            end
            new_plot = true;
        end
        hax = gca;
        hold on
    end
    % hax is the handle for the axis we will work with, either new or
    % passed by option 'handle'

    opt.text_opts = [opt.text_opts, 'Color', opt.color];

    hg = hgtransform('Parent', hax);

    % trplot( Q.R, fmt, color);
    if isrot(T)
        T = r2t(T);
    end

    % create unit vectors
    o =  [0 0 0]';
    x1 = opt.length*[1 0 0]';
    y1 = opt.length*[0 1 0]';
    if opt.lefty
        z1 = opt.length*[0 0 -1]';
    else
        z1 = opt.length*[0 0 1]';
    end

    % draw the axes

    mstart = [o o o]';
    mend = [x1 y1 z1]';

    if opt.rgb
        axcolors = {'r', 'g', 'b'};
    else
        axcolors = { opt.color, opt.color, opt.color};
    end

    if opt.arrow
%         % draw the 3 arrows
%         S = [opt.color num2str(opt.width)];
%         ha = arrow3(mstart, mend, S);
%         for h=ha'
%             set(h, 'Parent', hg);
%         end
          daspect([1,1,1])
          for i=1:3
              ha = arrow3(mstart(i,1:3), mend(i,1:3), [axcolors{i} num2str(opt.width)]);
              set(ha, 'Parent', hg);
          end
    else
        for i=1:3
            plot2([mstart(i,1:3); mend(i,1:3)], 'Color', axcolors{i}, ...
                'LineWidth', opt.thick, ...
                'Parent', hg);
        end
    end

    % label the axes
    if isempty(opt.frame)
        fmt = '%c';
    else
        fmt = sprintf('%%c_{%s}', opt.frame);
    end

    if opt.text
        % add the labels to each axis
        h = text(x1(1), x1(2), x1(3), sprintf(fmt, opt.labels(1)), 'Parent', hg);
        set(h, opt.text_opts{:});

        h = text(y1(1), y1(2), y1(3), sprintf(fmt, opt.labels(2)), 'Parent', hg);
        set(h, opt.text_opts{:});

        h = text(z1(1), z1(2), z1(3), sprintf(fmt, opt.labels(3)), 'Parent', hg);
        set(h, opt.text_opts{:});
    end

    % label the frame
    if ~isempty(opt.frame)
        h = text(o(1)-0.04*x1(1), o(2)-0.04*y1(2), o(3)-0.04*z1(3), ...
            ['\{' opt.frame '\}'], 'Parent', hg);
        set(h, 'VerticalAlignment', 'middle', ...
            'HorizontalAlignment', 'center', opt.text_opts{:});
    end

    if ~opt.axes
        set(gca, 'visible', 'off');
    end
    if ischar(opt.view) && strcmp(opt.view, 'auto')
        cam = x1+y1+z1;
        view(cam(1:3));
    elseif ~isempty(opt.view)
        view(opt.view);
    end
    if isempty(opt.handle) && ~ih
        grid on
        hold off
    end

    % now place the frame in the desired pose
    set(hg, 'Matrix', T);

    if opt.d_3d
        % 3D display.  The original axes are for the left eye, and we add
        % another set of axes to the figure for the right eye view and
        % displace its camera to the right of that of that for the left eye.
        % Then we recursively call trplot() to create the right eye view.

        left = gca;
        right = axes;

        % compute the offset in world coordinates
        off = -t2r(view(left))'*[opt.dispar 0 0]';
        pos = get(left, 'CameraPosition');

        set(right, 'CameraPosition', pos+off');
        set(right, 'CameraViewAngle', get(left, 'CameraViewAngle'));
        set(right, 'CameraUpVector', get(left, 'CameraUpVector'));
        target = get(left, 'CameraTarget');
        set(right, 'CameraTarget', target+off');

        % set perspective projections
                set(left, 'Projection', 'perspective');
        set(right, 'Projection', 'perspective');

        % turn off axes for right view
        set(right, 'Visible', 'Off');

        % set color for right view
        hg2 = trplot(T, 'color', ag_color(opt.anaglyph(2)));

        % the hgtransform for the right view is user data for the left
        % view hgtransform, we need to change both when we rotate the
        % frame.
        set(hg, 'UserData', hg2);
    end

    % optionally return the handle, for later modification of pose
    if nargout > 0
        hout = hg;
    end
end

function out = ag_color(c)

% map color character to an color triple, same as anaglyph.m

    % map single letter color codes to image planes
    switch c
    case 'r'
        out = [1 0 0];        % red
    case 'g'
        out = [0 1 0];        % green
    case 'b'
        % blue
        out = [0 0 1];
    case 'c'
        out = [0 1 1];        % cyan
    case 'm'
        out = [1 0 1];        % magenta
    case 'o'
        out = [1 1 0];        % orange
    end
end