function [p,J]=kernelspline(centres1,centres2,pt,mode)
% kernelspline(centres1,centres2,pt,mode)
%
% Compute the value of a kernel spline.  Used for testing kernel
% spline transform in ITK.
%
% Rupert Brooks, March 2007
if(nargin<4)
    mode=0;
end

dim=size(centres1,2);
np=size(centres1,1);

% elastic body spline elasticity
alpha= 12.0 * ( 1 -  0.25) - 1.0 ;

K=zeros(np*dim);
P=zeros(np*dim,dim*dim+dim);
for i=1:np
    for j=1:np
       diffv=centres1(i,:)-centres1(j,:); % difference vectore between points i,j
       r=sqrt(sum(diffv.^2));
       % indices of this submatrix of K
       ii=dim*(i-1)+1;
       jj=dim*(j-1)+1;
       switch mode
           case 0
               % thin plate spline
               for k=0:(dim-1)
               K(ii+k,jj+k)=r;
               end
           case 1
               % thin plate r2logr
               % if >1e-8 r2logr else 0
               for k=0:(dim-1)
                   if(r>10^-8)
                        K(ii+k,jj+k)=r*r*log(r);
                   end
               end
           case 2
               % elastic body
               for k=0:(dim-1)
               for l=0:(dim-1)
                   K(ii+k,jj+l)=-3*diffv(k+1)*diffv(l+1)*r;
                   if(k==l)
                       K(ii+k,jj+l)=K(ii+k,jj+l)+alpha*r*r*r;
                   end
               end
               end
               
           case 3
               % elastic body reciprocal
               for k=0:(dim-1)
               for l=0:(dim-1)
                   if(r>10^-8)
                   K(ii+k,jj+l)=-1*diffv(k+1)*diffv(l+1)/r;
                   end
                   if(k==l)
                       K(ii+k,jj+l)=K(ii+k,jj+l)+alpha*r;
                   end
               end
               end
           case 4
               % volume
               % r^3
               for k=0:(dim-1)
                   K(ii+k,jj+k)=r*r*r;
               end

           otherwise
       end
       %K(2*i-1,2*j-1)=sqrt(sum(((centres1(i,:)-centres1(j,:)).^2),2));
       %K(2*i,2*j)=sqrt(sum(((centres1(i,:)-centres1(j,:)).^2),2));
%       if(K(i,j)~=0)
%          K(i,j)=K(i,j)*K(i,j)*log(K(i,j));
%       end
    end
    for k=0:(dim-1)
        for l=0:(dim-1)
        P(ii+k,l*dim+k+1)=centres1(i,l+1);
        end
        P(ii+k,dim*dim+k+1)=1;
    end
    
end

L=[K P;P' zeros((dim+1)*dim)];
Linv=inv(L);

delta=[centres2-centres1;zeros(dim+1,dim)]';

w=Linv*delta(:);
%w=reshape(w,[2,np+3])';
%diffs=repmat(pt,[size(centres1,1),1])-centres1;
%% now we need to compute the kernels at each point
%% 
Gvec=zeros(np*dim,dim);
for i=1:np
   diffv=pt-centres1(i,:); % difference vectore between points and lmk i
   r=sqrt(sum(diffv.^2));
       % indices of this submatrix of Gvec
       ii=dim*(i-1)+1;
       jj=1;
       switch mode
           case 0
               % thin plate spline
               for k=0:(dim-1)
               Gvec(ii+k,jj+k)=r;
               end
           case 1
               % thin plate r2logr
               % if >1e-8 r2logr else 0
               for k=0:(dim-1)
                   if(r>10^-8)
                        Gvec(ii+k,jj+k)=r*r*log(r);
                   end
               end
           case 2
               % elastic body
               for k=0:(dim-1)
               for l=0:(dim-1)
                   Gvec(ii+k,jj+l)=-3*diffv(k+1)*diffv(l+1)*r;
                   if(k==l)
                       Gvec(ii+k,jj+l)=Gvec(ii+k,jj+l)+alpha*r*r*r;
                   end
               end
               end
               
           case 3
               % elastic body reciprocal
               for k=0:(dim-1)
               for l=0:(dim-1)
                   if(r>10^-8)
                   Gvec(ii+k,jj+l)=-1*diffv(k+1)*diffv(l+1)/r;
                   end
                   if(k==l)
                       Gvec(ii+k,jj+l)=Gvec(ii+k,jj+l)+alpha*r;
                   end
               end
               end
           case 4
               % volume
               % r^3
               for k=0:(dim-1)
                   Gvec(ii+k,jj+k)=r*r*r;
               end

           otherwise
       end
end    

FullV=Gvec;
for i=1:dim
    FullV=[FullV;pt(i)*eye(dim)];
end
FullV=[FullV;eye(dim)];

J=Linv*FullV;
p=pt+w'*FullV;