/*
 * SFTABLE2  A two dimensional table lookup in S-Function form.
 *           (see manual under Advanced Topics).
 *
 *           Syntax:  [sys, x0] = sftable2(t,x,u,flag) 
 * 
 * This 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 in the mdlOutputs function returns 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. 
 * 
 * Ned Gulley 12-19-91 
 * Gary Levenson 5-31-94
 * Copyright (c) 1991-94 by the MathWorks, Inc. 
 * All Rights Reserved 
 */ 
 
/* 
 * The following #define is used to specify the name of your S-Function. 
 */ 
 
#define S_FUNCTION_NAME sftable2 

/* 
 * need to include simstruc.h for the definition of the SimStruct and 
 * its associated macro definitions. 
 */ 
#include "simstruc.h" 
 
/* 
 * Defines for easy access of the XINDEX, YINDEX, and TABLE matrices 
 * that are passed in. 
 */ 
#define XINDEX  ssGetArg(S,0) 
#define YINDEX  ssGetArg(S,1) 
#define TABLE   ssGetArg(S,2) 
 
/* 
 * mdlInitializeSizes - initialize the sizes array 
 * 
 * The sizes array is used by SIMULINK to determine the S-function block's 
 * characteristics (number of inputs, outputs, states, etc.). 
 */ 
 
static void mdlInitializeSizes (S) 
    SimStruct *S; 
{ 
    ssSetNumContStates(    S, 0);      /* number of continuous states */ 
    ssSetNumDiscStates(    S, 0);      /* number of discrete states */ 
    ssSetNumInputs(        S, 2);      /* number of inputs */ 
    ssSetNumOutputs(       S, 1);      /* number of outputs */ 
    ssSetDirectFeedThrough(S, 1);      /* direct feedthrough flag */ 
    ssSetNumSampleTimes(   S, 1);      /* number of sample times */ 
    ssSetNumInputArgs(     S, 3);      /* number of input arguments */ 
    ssSetNumRWork(         S, 0);      /* number of real work vector elements */ 
    ssSetNumIWork(         S, 0);      /* number of integer work vector elements */ 
    ssSetNumPWork(         S, 0);      /* number of pointer work vector elements */ 
} 
 
/* 
 * mdlInitializeSampleTimes - initialize the sample times array 
 * 
 * This function is used to specify the sample time(s) for your S-function. 
 * If your S-function is continuous, you must specify a sample time of 0.0. 
 * Sample times must be registered in ascending order.  If your S-function 
 * is to acquire the sample time of the block that is driving it, you must 
 * specify the sample time to be INHERITED_SAMPLE_TIME. 
 */ 
 
static void mdlInitializeSampleTimes(S) 
    SimStruct *S; 
{ 
    ssSetSampleTimeEvent(S, 0, INHERITED_SAMPLE_TIME); 
    ssSetOffsetTimeEvent(S, 0, 0.0); 
} 
 
/* 
 * mdlInitializeConditions - initialize the states 
 * 
 * In this function, you should initialize the continuous and discrete 
 * states for your S-function block.  The initial states are placed 
 * in the x0 variable.  You can also perform any other initialization 
 * activities that your S-function may require. 
 */ 
 
static void mdlInitializeConditions(x0, S) 
    double *x0; 
    SimStruct *S; 
{ 
} 
 
/* 
 * mdlOutputs - compute the outputs 
 * 
 * In this function, you compute the outputs of your S-function 
 * block.  The outputs are placed in the y variable. 
 */ 
 
static void mdlOutputs(y, x, u, S, tid) 
    double *y, *x, *u; 
    SimStruct *S; 
    int tid; 
{ 
    double *xindex = mxGetPr(XINDEX); 
    double *yindex = mxGetPr(YINDEX); 
    double *table  = mxGetPr(TABLE); 
     
#define NUMEL(m)    mxGetM(m)*mxGetN(m) 
    int xindexLen = NUMEL(XINDEX); 
    int yindexLen = NUMEL(YINDEX); 
#undef NUMEL 
     
    double x0, y0; 
    double xlo, xhi, ylo, yhi; 
    double zxloylo, zxloyhi, zxhiylo, zxhiyhi; 
    double xratio, zx0ylo, zx0yhi; 
    int i, xptrlo, xptrhi, yptrlo, yptrhi; 
     
    /* 
     * Perform the desired 2-D table lookup 
     */ 
     
    x0 = u[0]; 
    for (i = 0; (i < xindexLen) && (xindex[i] < x0); i++) 
        ; 
     
    /* 
     * if i is equal to the length of the xindex array, then x0 is less 
     * than all values in xindex, use the last two points in xindex 
     * else if i is zero, x0 is greater than all values in xindex, 
     * use the first two points in xindex. If the index is a scalar 
     * handle that as a special case of a 1-d look-up table 
     */ 
      
      
     
    if ((i == xindexLen) && (xindexLen != 1)) { 
        xptrlo = xindexLen - 2; 
        xptrhi = xindexLen - 1; 
    } else if ((i == 0) && (xindexLen != 1)) { 
        xptrlo = 0; 
        xptrhi = 1; 
    } else if (xindexLen == 1) { 
        xptrlo = 0; 
        xptrhi = 0; 
    }  else { 
        xptrlo = i - 1; 
        xptrhi = i; 
    } 
     
    xlo = xindex[xptrlo]; 
    xhi = xindex[xptrhi]; 
     
    /* 
     * Now repeat the process for yindex 
     */ 
     
    y0 = u[1]; 
    for (i = 0; (i < yindexLen) && (yindex[i] < y0); i++) 
        ; 
     
    /* 
     * if i is equal to the length of the yindex array, then y0 is less 
     * than all values in yindex, use the last two points in yindex 
     * else if i is zero, y0 is greater than all values in yindex, 
     * use the first two points in yindex. 
     */ 
     
   
    if ((i == yindexLen) && (yindexLen != 1)) { 
        yptrlo = yindexLen - 2; 
        yptrhi = yindexLen - 1; 
    } else if ((i == 0) && (yindexLen != 1)) { 
        yptrlo = 0; 
        yptrhi = 1; 
    } else if (yindexLen == 1) { 
        yptrlo = 0; 
        yptrhi = 0; 
    } else { 
        yptrlo = i - 1; 
        yptrhi = i; 
    } 
     
    ylo = yindex[yptrlo]; 
    yhi = yindex[yptrhi]; 
     
    /* 
     * Find the four z values on the table that bracket (x0,y0) 
     */ 
     
    zxloylo = table[xptrlo + yptrlo*xindexLen]; 
    zxloyhi = table[xptrlo + yptrhi*xindexLen]; 
    zxhiylo = table[xptrhi + yptrlo*xindexLen]; 
    zxhiyhi = table[xptrhi + yptrhi*xindexLen]; 
     
    /* 
     * Now interpolate to find the desired answer 
     * First the intermediate step at "ylo" 
     * Calculate xratio only once, as it is used twice below. 
     * Avoid divide by zero caused by scalar case of xindex 
     */ 
     
     if (xhi-xlo != 0) 
        xratio = (x0-xlo)/(xhi-xlo); 
     else   
        xratio = (x0-xlo); 
        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) 
     * Where y[0] = zx0y0, the desired interpolated value . 
     * Avoid divide by zero caused by scalar indeces 
     */ 
     
    if (yhi-ylo != 0) 
        y[0] = (y0-ylo)/(yhi-ylo)*(zx0yhi-zx0ylo) + zx0ylo; 
     
    else  
        y[0] = (y0-ylo) * (zx0yhi-zx0ylo) + zx0ylo; 
} 
 
/* 
 * mdlUpdate - perform action at major integration time step 
 * 
 * This function is called once for every major integration time step. 
 * Discrete states are typically updated here, but this function is useful 
 * for performing any tasks that should only take place once per integration 
 * step. 
 */ 
 
static void mdlUpdate(x, u, S, tid) 
    double *x, *u; 
    SimStruct *S; 
    int tid; 
{ 
} 
 
/* 
 * mdlDerivatives - compute the derivatives 
 * 
 * In this function, you compute the S-function block's derivatives. 
 * The derivatives are placed in the dx variable. 
 */ 
 
static void mdlDerivatives(dx, x, u, S, tid) 
    double *dx, *x, *u; 
    SimStruct *S; 
    int tid; 
{ 
} 
 
/* 
 * mdlTerminate - called when the simulation is terminated. 
 * 
 * In this function, you should perform any actions that are necessary 
 * at the termination of a simulation.  For example, if memory was allocated 
 * in mdlInitializeConditions, this is the place to free it. 
 */ 
 
static void mdlTerminate(S) 
    SimStruct *S; 
{ 
} 
 
#ifdef  MATLAB_MEX_FILE    /* Is this file being compiled as a MEX-file? */ 
#include "simulink.c"      /* MEX-file interface mechanism */ 
#else 
#include "cg_sfun.h"       /* Code generation registration function */ 
#endif        
