## Copyright (C) 2000  Kai Habel
##
## This program is free software; you can redistribute it and/or modify
## it under the terms of the GNU General Public License as published by
## the Free Software Foundation; either version 2 of the License, or
## (at your option) any later version.
##
## This program is distributed in the hope that it will be useful,
## but WITHOUT ANY WARRANTY; without even the implied warranty of
## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
## GNU General Public License for more details.
##
## You should have received a copy of the GNU General Public License
## along with this program; if not, write to the Free Software
## Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

## -*- texinfo -*-
## @deftypefn {Function File} {@var{zi}=} 
##  (@var{x},@var{y},@var{z},@var{xi},@var{yi},@var{method})
## @deftypefnx {Function File} {@var{zi}=}
##  (@var{x},@var{y},@var{z},@var{xi},@var{yi})
## @deftypefnx {Function File} {@var{zi}=} (@var{Z},@var{xi},@var{yi})
## Two-dimensional interpolation
## @end deftypefn

## Author:	Kai Habel <kai.habel@gmx.de>

function ZI = interp2 (X, Y, Z, XI, YI, method)

	if (nargin < 3 || nargin > 6 || nargin == 4)
		usage ("interp2 (X, Y, Z, XI, YI, method)");
	endif

	if !( (size (X) == size (Y)) && (size (X) == size (Z)) )
		error ("X,Y and Z must be matrices of same size");
	endif

	if (is_vector (XI) && is_vector (YI))
		if (length(XI) != length(YI))
			error ("If XI and YI are vectors they must have same length");
		else
			[XI, YI] = meshgrid(XI,YI);
		endif
	elseif !(size(XI) == size(YI))
		error ("XI and YI must be matrices of same size");
	endif

	xtable = X(1,:);
	ytable = Y(:,1);

	if (nargin < 6)
		method = "linear";
	endif

	if (nargin == 3)
		[XI, YI] = meshgrid (XI, YI);
	endif

	ytlen = length (ytable);
	xtlen = length (xtable);

	## get x index
	[m, n] = sort ([xtable(:); XI(1, :)']);
	o = cumsum (n <= xtlen);
	xidx = o([find(n > xtlen)])';

	## get y index
	[m, n]=sort ([ytable(:); YI(:, 1)]);
	o = cumsum (n <= ytlen);
	yidx = o([find(n > ytlen)]);
	
	[zr, zc]=size (Z);

	## mark values outside the lookup table
	xfirst_val = find (XI(1,:) <= xtable(1));
	xlast_val = find (XI(1,:) >= xtable(xtlen));
	yfirst_val = find (YI(:,1) <= ytable(1));
	ylast_val = find (YI(:,1) >= ytable(ytlen));

	yidx(yfirst_val) = 1;
	xidx(xfirst_val) = 1;
	yidx(ylast_val) = zr - 1;
	xidx(xlast_val) = zc - 1;

	if strcmp (method, "linear")
		## xlast_val = find (XI(1,:) == xtable(xtlen))
		## ylast_val = find (YI(1,:) == ytable(ytlen))
		## xidx(xlast_val) = z
		## each quad satisfies the equation z(x,y)=a+b*x+c*y+d*xy
		a = Z(1:zr - 1, 1:zc - 1);
		b = Z(1:zr - 1, 2:zc) - a;
		c = Z(2:zr, 1:zc - 1) - a;
		d = Z(2:zr, 2:zc) - a - b - c;

		ZI = a(xidx,yidx) \
		.+ b(xidx, yidx) .* (XI .- X(xidx, yidx)) \
		.+ c(xidx, yidx) .* (YI .- Y(xidx, yidx)) \
		.+ d(xidx, yidx) .* (XI .- X(xidx, yidx)) .* (YI .- Y(xidx, yidx));

	elseif strcmp (method, "nearest")
		
		i = XI(1, :) - xtable(xidx) > xtable(xidx + 1) - XI(1, :);
		j = YI(:, 1) - ytable(yidx) > ytable(yidx + 1) - YI(:, 1);
		ZI = Z(yidx + j, xidx + i);

	elseif strcmp (method, "cubic")
		## the cubic polynom is computed using Lagrange's interpolation
		## method
		##			3	3
		##			-	-
		## Zi(x,y)=	\	\	L_i(x)*L_j(y)*z(xi,yi)
		##			/	/
		##			-	-
		##			i=0	j=0

		## adjust index for points in boundary area so that general
		## formular can be applied

		xidx(find (xidx == 1)) = 2;
		xidx(find (xidx >= (xtlen - 1))) = xtlen - 2;
		xidx
		X(1,xidx)
		X(1,xidx+1)
		XI(1,:)
		Numx = (XI(1,:) - X(1, xidx - 1))\
			 .* (XI(1, :) - X(1, xidx))\
			 .* (XI(1, :) - X(1, xidx + 1))\
			 .* (XI(1, :) - X(1, xidx + 2))
		L0x = Numx ./ ( (X(1, xidx) - X(1, xidx - 1))\
						.* (X(1, xidx + 1) - X(1, xidx - 1))\
						.* (X(1, xidx + 2) - X(1, xidx - 1)))
		L1x = Numx ./ ( (X(1, xidx - 1) - X(1, xidx))\
						.*(X(1, xidx + 1) - X(1, xidx))\
						.*(X(1, xidx + 2) - X(1,xidx)))
		L2x = Numx ./ ( (X(1, xidx - 1) - X(1, xidx + 1))\
						.*(X(1, xidx) - X(1, xidx + 1))\
						.*(X(1, xidx + 2) - X(1, xidx + 1)))
		L3x = Numx ./ ( (X(1, xidx - 1) - X(1, xidx + 2))\
						.*(X(1, xidx) - X(1, xidx + 2))\
						.*(X(1, xidx + 1) - X(1, xidx + 2)))

		Numy = (YI(:, 1) - Y(yidx - 1, 1))\
			 .* (YI(:, 1) - Y(yidx, 1))\
			 .* (YI(:, 1) - Y(yidx + 1, 1))\
			 .* (YI(:, 1) - Y(yidx + 2, 1))
		L0y = Numy ./ ( (Y(yidx, 1) - Y(yidx - 1, 1))\
					.*(Y(yidx + 1, 1) - Y(yidx - 1, 1))\
					.*(Y(yidx + 2, 1) - Y(yidx - 1, 1)))
		L1y = Numy ./ ( (Y(yidx - 1, 1) - Y(yidx, 1))\
					.*(Y(yidx + 1, 1) - Y(yidx, 1))\
					.*(Y(yidx + 2, 1) - Y(yidx, 1)))
		L2y = Numy ./ ( (Y(yidx - 1, 1) - Y(yidx + 1, 1))\
					.*(Y(yidx, 1) - Y(yidx + 1, 1))\
					.*(Y(yidx + 2, 1) - Y(yidx + 1, 1)))
		L3y = Numy ./ ( (Y(yidx - 1, 1) - Y(yidx + 2, 1))\
					.*(Y(yidx, 1) - Y(yidx + 2, 1))\
					.*(Y(yidx + 1, 1) - Y(yidx + 2, 1)))
		ZI = L0x.*L0y.*Z(xidx-2,yidx-2)+\
			  L1x.*L0y.*Z(xidx-1,yidx-2)+\
			  L2x.*L0y.*Z(xidx,yidx-2)+\
			  L3x.*L0y.*Z(xidx+1,yidx-2)+\
			  L0x.*L1y.*Z(xidx-2,yidx-1)+\
			  L1x.*L1y.*Z(xidx-1,yidx-1)+\
			  L2x.*L1y.*Z(xidx,yidx-1)+\
			  L3x.*L1y.*Z(xidx+1,yidx-1)+\
			  L0x.*L2y.*Z(xidx-2,yidx)+\
			  L1x.*L2y.*Z(xidx-1,yidx)+\
			  L2x.*L2y.*Z(xidx,yidx)+\
			  L3x.*L2y.*Z(xidx+1,yidx)+\
			  L0x.*L3y.*Z(xidx-2,yidx+1)+\
			  L1x.*L3y.*Z(xidx-1,yidx+1)+\
			  L2x.*L3y.*Z(xidx,yidx+1)+\
			  L3x.*L3y.*Z(xidx+1,yidx+1);
	else
		error ("interpolation method not (yet) supported");
	endif
	## ZI(xnan_val,:) = NaN;
	## ZI(:,ynan_val) = NaN;
endfunction