function  [sys, x0]  = sftable2(t,x,u,flag,xindex,yindex,table)
%SFTABLE2 A two dimensional table lookup in S-Function form.
%
% This M-file is designed to be used in a SIMULINK S-Function block.
% It performs a 2-D lookup on the table passed in through the
% parameter list. It makes use of the variables XINDEX, YINDEX,
% TABLE, XO, and YO, where the first three of these are passed in
% through the parameter list, and the last two, XO and YO, are
% passed in as the inputs, respectively u(1) and u(2).
%
% The section of code under the FLAG == 3 call returns to SYS a linearly 
% interpolated intersection from the array TABLE, looking up X0
% first with the vector XINDEX, then looking up Y0 with the vector
% YINDEX.  The matrix TABLE has entries that correspond to the
% X and Y indices and therefore must have the same number of rows as the
% vector XINDEX and the same number of columns as the vector 
% YINDEX.  TABLE contains the output values to be interpolated among.
%
% Both XINDEX and YINDEX must increase monotonically.
% Extrapolation is used for values outside the limits of XINDEX
% and YINDEX based on the four closest points.

%       Copyright (c) 1990-94 by The MathWorks, Inc.
% 
%  Ned Gulley 12-19-91
%  Gary Levenson 6-3-94

if flag == 3,
        % Perform the desired 2-D table lookup
        x0 = u(1);
        y0 = u(2);

        % ***************************************
        % Now follows the entire 2-D table lookup routine...
        % ***************************************

% Use "find" to indicate what xindex values x0 lies between
        xptrhi = min(find(xindex > x0));
        xptrlo = xptrhi-1;
% Use the variable xptrhi to point to the first xindex value > x0
% Use the variable xptrlo to point to the first xindex value < x0
        if xptrhi==1,
           if length(yindex) ~= 1       
                % If x0 is less than all values of xindex, 
                % set xptrlo and xptrhi to the first two values of xindex 
                xptrlo = 1;
                xptrhi = 2;
	   end
        elseif length(xptrhi)==0,
                % If x0 is greater than all values of xindex, 
                % set xptrlo and xptrhi to the last two values of xindex 
                xptrhi = length(xindex);
                xptrlo = xptrhi-1;
        elseif length(xindex) == 1,
                % if xindex is a scalar, set the indeces to 
                % the first element of the index
                xprtlo = 1;
                xptrhi = 1;                   
        end;
% xlo and xhi are thus the table entries that bracket the value x0
% unless x0 lies off the table, in which case they are as close as 
% possible.
        xlo = xindex(xptrlo);
        xhi = xindex(xptrhi);

% Use "find" to indicate what yindex values y0 lies between
        yptrhi = min(find(yindex > y0));
        yptrlo = yptrhi-1;
        
        if  min(find(yindex > y0)) == []
            yptrlo = 1;
            yptrhi = 1;
        end
        
% Use the variable yptrhi to point to the first yindex value > y0
% Use the variable yptrlo to point to the first yindex value < y0        
        if (yptrhi==1)
           if length(yindex) ~= 1   
                % If y0 is less than all values of yindex, 
                % set yptrlo and yptrhi to the first two values of yindex. 
                yptrhi = 2;
                yptrlo = 1;
            end
        elseif length(yptrhi)==0,
                % If y0 is greater than all values of yindex, 
                % set yptrlo and yptrhi to the last two values of yindex 
                yptrhi = length(yindex);
                yptrlo = yptrhi-1;        
        elseif length(yindex) == 1,
                % if yindex is a scalar, set the indeces to 
                % the first element of the index
                yprtlo = 1;
                yptrhi = 1;
        end;
        
% ylo and yhi are thus the table entries that bracket the value y0
% unless y0 lies off the table, in which case they are as close as 
% possible.
        ylo = yindex(yptrlo);
        yhi = yindex(yptrhi);
        
% Now find the four z values on the table that bracket (x0,y0)
        zxloylo = table(xptrlo, yptrlo);
        zxloyhi = table(xptrlo, yptrhi);
        zxhiylo = table(xptrhi, yptrlo);
        zxhiyhi = table(xptrhi, yptrhi);

% Now interpolate to find the desired answer
% First the intermediate step at "ylo"
% Calculate xratio only once, as it is used twice below.
% Avoid a divide by zero
        if (xhi-xlo) ~= 0,
                xratio = (x0-xlo)/(xhi-xlo);
        else
                xratio = x0 - xlo;
        end;
        zx0ylo = xratio*(zxhiylo-zxloylo) + zxloylo;
% Then the intermediate value at "yhi"
        zx0yhi = xratio*(zxhiyhi-zxloyhi) + zxloyhi;
% Finally, interpolate to find the value at (x0,y0)
% Again, avoid a divide by zero
        if (yhi - ylo) ~= 0,
                sys = (y0-ylo)/(yhi-ylo)*(zx0yhi-zx0ylo) + zx0ylo;
        else 
                sys = (y0-ylo)*(zx0yhi-zx0ylo) + zx0ylo;
        end;
                        
% Where sys = zx0y0, the desired interpolated value;

        % ***************************************
        % Here ends the 2-D table lookup routine...
        % ***************************************

elseif flag  == 0,
        % This part takes care of all initialization; it is used only once.
        x0 = [];
        % The system has no states, one output, and two inputs.
        sys = [0 0 1 2 0 1]';

else 
        % Flags not considered here are treated as unimportant.
        % Output is set to [].
        sys = [];

end
