%% File : sfun_lookupnd.tlc generated from sfun_lookupnd.ttlc revsion 1.6
%% $Date: 2000/05/17 20:17:04 $
%%
%% Rob Aberg, 13-Aug-1999
%% Copyright 1990-2000 The MathWorks, Inc.
%%
%% Abstract: N-D Lookup table with various options.

%implements sfun_lookupnd "C"
 
%% Function: BlockInstanceSetup ===================================================
%% Abstract:
%%      Have mdlhdr.tlc include rtlibsrc.h
%%
%function BlockInstanceSetup(block, system) void
  %if SFcnParamSettings.vectorInputFlag == 0
    %assign numDims = NumDataInputPorts
  %else
    %assign numDims = LibBlockInputSignalWidth(0)
  %endif
  %switch SFcnParamSettings.interpMethod
    %case 2
      %if numDims > 2
	%assign ::CompiledModel.IncludeLibsrc = 1
      %endif
      %break
    %case 3
      %assign ::CompiledModel.IncludeLibsrc = 1
      %break
    %default
      %break
  %endswitch
%endfunction
  
%% BlockInstanceData ==========================================================
%% Abstract:
%%      Create persistant data for PWork Vector.
%%
%function BlockInstanceData(block, system) Output
  /* Look-Up Table (N-D) (LookupND) */
  /*   Block: %<Name> */
  %%
  %assign tabSize = LibBlockParameterSize(table)
  %if SFcnParamSettings.vectorInputFlag == 0
    %assign numDims = NumDataInputPorts
  %else
    %assign numDims = LibBlockInputSignalWidth(0)
  %endif
  %assign ttype = LibBlockOutputSignalDataTypeName(0,"")
  %assign dims = SFcnParamSettings.tabDims
  %assign biggestDim = dims[0]
  %if numDims > 1
    %foreach idx = %<numDims-1>
      %if dims[%<idx+1>] > biggestDim
	%assign biggestDim = dims[%<idx+1>]
      %endif
    %endforeach
  %endif
  %assign biggestDim = CAST("Number", biggestDim)
  %assign tnumYWorkElements = [0:%<numDims+1>]
  %assign tnumYWorkElements[numDims]   = 0
  %assign tnumYWorkElements[numDims-1] = 1
  %foreach idx = %<numDims-1>
    %assign yidx = numDims-2 - idx
    %assign tnumYWorkElements[%<yidx>] = tnumYWorkElements[%<yidx+1>] * \
    dims[%<yidx+1>]
  %endforeach
  %assign largestVec = CAST("Number", tnumYWorkElements[0])
  %if UsingMalloc
    %<LibBlockPWork(uLambdas, "", "", 0)> = malloc(%<numDims> * sizeof(%<ttype>));
    rt_VALIDATE_MEMORY(%<tSimStruct>, %<LibBlockPWork(uLambdas, "", "", 0)>);
    %<LibBlockPWork(uVect, "", "", 0)> = malloc(%<numDims> * sizeof(%<ttype>));
    rt_VALIDATE_MEMORY(%<tSimStruct>, %<LibBlockPWork(uVect, "", "", 0)>);
    %if SFcnParamSettings.interpMethod == 3
      %%
      %% work vectors for spline interpolation
      %%
      %<LibBlockPWork(splWork, "", "", 0)> = malloc( sizeof(SplineWorkVect) );
      rt_VALIDATE_MEMORY(%<tSimStruct>, %<LibBlockPWork(splWork, "", "", 0)>);
      %<LibBlockPWork(yyA, "", "", 0)> = malloc(%<largestVec> * ...
	sizeof(%<ttype>));
      rt_VALIDATE_MEMORY(%<tSimStruct>, %<LibBlockPWork(yyA, "", "", 0)>);
      %<LibBlockPWork(yyB, "", "", 0)> = malloc(%<largestVec> * ...
	sizeof(%<ttype>));
      rt_VALIDATE_MEMORY(%<tSimStruct>, %<LibBlockPWork(yyB, "", "", 0)>);
      %<LibBlockPWork(yy2, "", "", 0)> = malloc(%<largestVec> * ...
	sizeof(%<ttype>));
      rt_VALIDATE_MEMORY(%<tSimStruct>, %<LibBlockPWork(yy2, "", "", 0)>);
      %<LibBlockPWork(up,  "", "", 0)> = malloc(%<biggestDim> * ...
	sizeof(%<ttype>));
      rt_VALIDATE_MEMORY(%<tSimStruct>, %<LibBlockPWork(up, "", "", 0)>);
      %<LibBlockPWork(y2,  "", "", 0)> = malloc(%<tabSize[1]> * ...
	sizeof(%<ttype>));
      rt_VALIDATE_MEMORY(%<tSimStruct>, %<LibBlockPWork(y2, "", "", 0)>);
    %endif
  %else
    {
    static %<ttype> uLambdasBuffer[%<numDims>] ;
    static %<ttype> uVectBuffer[%<numDims>] ;
    %if SFcnParamSettings.interpMethod == 3
      static  SplineWorkVect  splWork;
      static %<ttype> yyABuffer[%<largestVec>] ;
      static %<ttype> yyBBuffer[%<largestVec>] ;
      static %<ttype> yy2Buffer[%<largestVec>] ;
      static %<ttype> upBuffer[%<biggestDim>] ;
      static %<ttype> y2Buffer[%<tabSize[1]>] ;
    %endif
    %assign vCast = "(void *) "
    %<LibBlockPWork(uLambdas, "", "", 0)> = %<vCast>&uLambdasBuffer[0];
    %<LibBlockPWork(uVect, "", "", 0)> = %<vCast>&uVectBuffer[0];
    %if SFcnParamSettings.interpMethod == 3
      %<LibBlockPWork(splWork, "", "", 0)> = %<vCast>&splWork;
      %<LibBlockPWork(yyA, "", "", 0)> = %<vCast>&yyABuffer[0];
      %<LibBlockPWork(yyB, "", "", 0)> = %<vCast>&yyBBuffer[0];
      %<LibBlockPWork(yy2, "", "", 0)> = %<vCast>&yy2Buffer[0];
      %<LibBlockPWork(up,  "", "", 0)> = %<vCast>&upBuffer[0];
      %<LibBlockPWork(y2,  "", "", 0)> = %<vCast>&y2Buffer[0];
    %endif
    }
  %endif

%endfunction    

%% Start ======================================================================
%% Abstract:
%%      Initialize the state information (first input & time)
%%
%function Start(block, system) Output
  /* Look-Up Table (N-D) (LookupNDInterp) */
  /*   Block: %<Name> */
  %if SFcnParamSettings.vectorInputFlag == 0
    %assign numEl = NumDataInputPorts
  %else
    %assign numEl = LibBlockInputSignalWidth(0)
  %endif
  
  /* index search result storage explicit initialization */
  %assign rollVars = ["Mode"]
  %assign rollRegion = [0:%<numEl-1>]
  %roll idx = rollRegion, lcv = RollThreshold, block, "Roller", rollVars
    %assign m = LibBlockMode("", lcv, idx)
    %<m> = 0;
  %endroll

  /* maxIdx initialization */
  %assign dims = SFcnParamSettings.tabDims
  %assign rollVars = ["IWork"]
  %assign rollRegion = [0:%<numEl-1>]
  %roll idx = rollRegion, lcv = RollThreshold, block, "Roller", rollVars
    %assign iw = LibBlockIWork(maxIdx, "", lcv, idx)
    %<iw> = %<CAST("Number", dims[idx]-1)>;
  %endroll

  %if numEl > 2 || SFcnParamSettings.interpMethod != 2
    /* dimSizes initialization */
    %assign tdimSizes = dims
    %assign tdimSizes[0] = 1
    %foreach i = %<SIZE(dims,1)*SIZE(dims,0)-1>
      %assign tdimSizes[i+1] = tdimSizes[i]*dims[i]
    %endforeach
    %assign rollVars = ["IWork"]
    %assign rollRegion = [0:%<numEl-1>]
    %roll idx = rollRegion, lcv = RollThreshold, block, "Roller", rollVars
      %assign iw = LibBlockIWork(dimSizes, "", lcv, idx)
      %<iw> = %<CAST("Number", tdimSizes[idx])>;
    %endroll
  %endif
  %if SFcnParamSettings.interpMethod == 3
    %%
    %% calculate needed size of work vectors 
    %if SFcnParamSettings.vectorInputFlag == 0
      %assign numDims = NumDataInputPorts
    %else
      %assign numDims = LibBlockInputSignalWidth(0)
    %endif
    %assign tnumYWorkElements = [0:%<numDims+1>]
    %assign tnumYWorkElements[numDims]   = 0
    %assign tnumYWorkElements[numDims-1] = 1
    %foreach idx = %<numDims-1>
      %assign yidx = numDims-2 - idx
      %assign tnumYWorkElements[%<yidx>] = tnumYWorkElements[%<yidx+1>] * \
      dims[%<yidx+1>]
    %endforeach

    /* numYWorkElements initialization */
    %assign rollVars = ["IWork"]
    %assign rollRegion = [0:%<numEl>]
    %roll idx = rollRegion, lcv = RollThreshold, block, "Roller", rollVars
      %assign iw = LibBlockIWork(numYWorkElements, "", lcv, idx)
      %<iw> = %<CAST("Number", tnumYWorkElements[idx])>;
    %endroll
    
    /* work vector bundle initialization */
    %assign tsplWork = LibBlockPWork(splWork, "", "", 0)
    {
    SplineWorkVect *splWk = (SplineWorkVect *) %<tsplWork>;
    
    splWk->uLambdas = %<LibBlockPWork(uLambdas, "", "", 0)>;
    splWk->uVect = %<LibBlockPWork(uVect, "", "", 0)>;
    splWk->yyA = %<LibBlockPWork(yyB, "", "", 0)>;
    splWk->yyB = %<LibBlockPWork(yyB, "", "", 0)>;
    splWk->yy2 = %<LibBlockPWork(yy2, "", "", 0)>;
    splWk->up  = %<LibBlockPWork(up,  "", "", 0)>;
    splWk->y2  = %<LibBlockPWork(y2,  "", "", 0)>;
    splWk->Indices = &%<LibBlockMode("0", "", "")>;
    splWk->numYWorkElements = &%<LibBlockIWork(numYWorkElements,"0","","")>;
    splWk->maxIdx = &%<LibBlockIWork(maxIdx, "0", "", "")>;
    splWk->bpIndices = %<LibBlockParameterAddr(bpIndices, "0", "", "")>;
    splWk->bpData = %<LibBlockParameterAddr(bpData, "0", "", "")>;
    splWk->tableData = %<LibBlockParameterAddr(table, "0", "", "")>;
    splWk->extrapMethod = %<CAST("Number", SFcnParamSettings.extrapMethod)>;
    }
  %endif
  
%endfunction
 
%% Function: Outputs ==========================================================
%% Abstract:
%%    Perform indicated interpolation, flat or linear
%%
%function Outputs(block, system) Output
  /* Look-Up Table (N-D) (LookupNDInterp) */
  /*   Block: %<Name> */
  {
  %assign ttype = LibBlockOutputSignalDataTypeName(0,"")
  
  %if SFcnParamSettings.vectorInputFlag == 0
    %assign numEl = NumDataInputPorts
  %else
    %assign numEl = LibBlockInputSignalWidth(0)
  %endif
  /* retrieve work vectors */
  %assign declULambda = "%<ttype> *uLambda"
  %assign declUVect   = "%<ttype> *uVect"
  %assign vCast       = "(%<ttype> *) "
  %<declULambda> = %<vCast>%<LibBlockPWork(uLambdas, "", "", 0)>;
  %if numEl > 1
    %<declUVect> = %<vCast>%<LibBlockPWork(uVect, "", "", 0)>;
  %endif
  %if (numEl > 1 && SFcnParamSettings.interpMethod == 1) || ...
      (numEl > 2 && SFcnParamSettings.interpMethod == 2)
    int_T *dimSizes = &%<LibBlockIWork(dimSizes, "", "", 0)>;
  %endif
  
  %if numEl > 1
  /* collect inputs */
    %foreach idx = numEl
      %if SFcnParamSettings.vectorInputFlag == 0
	uVect[%<idx>] = %<LibBlockInputSignal(idx, "", "", 0)>;
      %else
	uVect[%<idx>] = %<LibBlockInputSignal(0, "", "", "%<idx>")>;
      %endif
    %endforeach
  %endif
    
  
  /* do index search for each input */
  {
  %assign declBpData = "const %<ttype> *bpData"
  %if numEl > 1
    %assign iVar = "i"
    %if SFcnParamSettings.searchMode > 1
      %<ttype> lambda;
    %endif
    int_T %<iVar>;
    
    for (%<iVar>= 0; %<iVar> <  %<numEl>; %<iVar>++) {
      
      int_T bpBaseIdx = \
                         %<LibBlockParameter(bpIndices, "%<iVar>", "", "")>;
      %<declBpData> = %<LibBlockParameterAddr(bpData,"bpBaseIdx","","")>;
      int_T maxIdx = \
            %<LibBlockIWork(maxIdx, "%<iVar>", "", "")>;
      %assign istr   = "%<iVar>" 
      %assign istr2  = "2*%<iVar>"
      %assign istr21 = "2*%<iVar>+1"
  %else
    %<declBpData> = %<LibBlockParameterAddr(bpData,"","",0)>;
    int_T maxIdx = \
             %<LibBlockIWork(maxIdx, "0", "", "")>;
    %assign istr   = "0"
    %assign istr2  = "0"
    %assign istr21 = "1"
  %endif
  %%
  %if SFcnParamSettings.rangeErrorMode < 3 || ...
    SFcnParamSettings.searchMode > 1
    %if numEl == 1
      %<ttype> lambda;
    %endif
    %if SFcnParamSettings.searchMode != 1 || ...
      SFcnParamSettings.extrapMethod != 1
      boolean_T bpFound = false;
    %endif
    %if numEl == 1
      %assign idx = 0
      %if SFcnParamSettings.vectorInputFlag == 0
	%<ttype> u = %<LibBlockInputSignal(idx, "", "", 0)>;
      %else
	%<ttype> u = %<LibBlockInputSignal(0, "", "", "%<idx>")>;
      %endif
    %else
	%<ttype> u = uVect[%<istr>];
    %endif
    %if SFcnParamSettings.searchMode > 1  %% is this an actual search?
      %if SFcnParamSettings.cacheBpFlag == 1
	int_T index = \
	                        %<LibBlockMode("%<istr>", "", "")>; \
        /* get previous index */
      %else
	int_T index = 0;
      %endif
    %elseif SFcnParamSettings.extrapMethod != 1
      int_T index;
    %endif
  %endif
  
  %if SFcnParamSettings.extrapMethod == 1
    /* clipping */
    %if SFcnParamSettings.rangeErrorMode != 3
      %if SFcnParamSettings.searchMode == 1
	u = (u < bpData[0]) ? bpData[0] : 
	((u > bpData[maxIdx]) ? bpData[maxIdx] : u);
      %else
	if (u < bpData[0]) {
	  index        = 0;
	  lambda       = 0.0;
	  bpFound      = true;
	} else if (u > bpData[maxIdx]) {
	  index        = maxIdx - 1;
	  lambda       = 1.0;
	  bpFound      = true;
	}
      %endif
    %endif
  %else
    /* extrapolation (check) */
    if (u < bpData[0]) {
      index        = 0;
      lambda       = (u - bpData[0]) / (bpData[1] - bpData[0]);
      bpFound      = true;
    } else if (u > bpData[maxIdx]) {
      index        = maxIdx - 1;
      lambda       = (u - bpData[maxIdx-1]) / ...
	(bpData[maxIdx] - bpData[maxIdx-1]);
      bpFound      = true;
    }
  %endif
  %%  
  %switch (SFcnParamSettings.searchMode)
    %case 1
      
      %if SFcnParamSettings.extrapMethod > 1
      if (bpFound == false) {
      %endif
        /* directly calculate index and fraction */
        uLambda[%<istr>] = (u - bpData[0]) /
                          (bpData[1] - bpData[0]);
        %<LibBlockMode("%<istr>", "", "")> = (int_T) uLambda[%<istr>];
	if ( %<LibBlockMode("%<istr>", "", "")> == maxIdx ) {
	    %<LibBlockMode("%<istr>", "", "")>--;
	  uLambda[%<istr>] = 1.0;
	} else {
            uLambda[%<istr>] -= (%<ttype>) %<LibBlockMode("%<istr>", "", "")>;
	}
      %if SFcnParamSettings.extrapMethod > 1
        } else {
        %<LibBlockMode("%<istr>", "", "")> = index;
	uLambda[i] = lambda;
        }
      %endif
      %break
    %case 2
      
      /* in-range linear search for [ u ) */
      while (bpFound == false) {
	
	lambda = (u - bpData[index]) /
	(bpData[index+1] - bpData[index]);
	
	/* check result */
	if (lambda >= 0.0 && lambda < 1.0 ) {
	  bpFound = true;
	} else if (lambda > 1.0) {
	  index++;
	} else if (lambda < 0.0) {
	  index--;
	} else if (lambda == 1.0) {
	  bpFound = true;
	  if ( index < (maxIdx-1) ) {
	    index++;
	    lambda = 0.0;
	  }
	}
      }

      %<LibBlockMode("%<istr>", "", "")> = index;
      uLambda[%<istr>] = lambda;

      %break
    %case 3

      /* in-range binary search for [ u ) */
      if (bpFound == false) {
	int_T bottom = 0;
	int_T top = maxIdx;

	while (bpFound == false) {
	  if (u < bpData[index]) {
	    top = index - 1;
	    index = (top + bottom) / 2;
	  } else if (u <= bpData[index+1]) {
	    bpFound = true;
	    if ( u == bpData[index+1] ) {
	      index++;
	    }
	  } else {
	    bottom = index + 1;
	    index = (top + bottom) / 2;
	  }
	}

        if (index == maxIdx) {
	  index--;
	  lambda = 1.0;
        } else {
	  lambda = (u - bpData[index]) / 
	  (bpData[index+1] - bpData[index]);
        }
      }

    %<LibBlockMode("%<istr>", "", "")> = index;
    uLambda[%<istr>] = lambda;


%endswitch
%%
%if numEl > 1

}
%endif
}

%assign numDims = SIZE(SFcnParamSettings.tabDims,1)
%assign y       = LibBlockOutputSignal(0, "0", "", "")
%assign dims = SFcnParamSettings.tabDims
%assign tdimSizes = dims
%assign tdimSizes[0] = 1
%foreach i = %<SIZE(dims,1)*SIZE(dims,0)-1>
  %assign tdimSizes[i+1] = tdimSizes[i]*dims[i]
%endforeach
%switch SFcnParamSettings.interpMethod
  %case 1
    %%
    %% FLAT interpolation (i.e., don't interpolate)
    %%
    %if NumDataInputPorts > 1
      {
	/* flat, no interpolation - use point at beginning of interval */
	int_T offset;

	%foreach idx = NumDataInputPorts
	  %if tdimSizes[idx]==1
	    %if idx == 0
	      offset = %<LibBlockMode("0", "", "")>;
	    %else
	      offset = offset + %<LibBlockMode("0", "", "")>;
	    %endif
	  %else
	    offset = offset + dimSizes[%<idx>] * %<LibBlockMode("%<idx>", "", "")>;
	  %endif
	%endforeach
	%<y> = %<LibBlockParameter(table, "offset", "", "")>;
      }
    %else
      %<y> = %<LibBlockParameter(table, "%<LibBlockMode("0", "", "")>", "", "")>;
    %endif
    %break
  %case 2
    %%
    %% LINEAR interpolation
    %%
    %switch numDims
      %case 1
	/* Linear 1-D interpolation */
	{
	  int_T offset = \
	           %<LibBlockMode("0", "", "")>;

	  %<y> = %<LibBlockParameter(table, "offset", "", "")> + ...
	    uLambda[0]*(%<LibBlockParameter(table, "offset+1", "", "")> - ...
	    %<LibBlockParameter(table, "offset", "", "")>);
	}
	%break
      %case 2
	%assign ttype = LibBlockOutputSignalDataTypeName(0,"")
	%assign t1 = LibBlockParameter(table, "offset", "", "")
	%assign t2 = LibBlockParameter(table, "offset+1", "", "")
	/* Linear 2-D interpolation */
	{
	  int_T offset = \
	    %<LibBlockMode("0", "", "")> + ...
	    %<tdimSizes[1]> * %<LibBlockMode("1", "", "")>;
	  %<ttype> ylower;
	  %<ttype> yupper;

	  ylower = %<t1> + uLambda[0]*
	            (%<t2> - %<t1>);
	  offset += %<tdimSizes[1]>;
	  yupper = %<t1> + uLambda[0]*
	            (%<t2> - %<t1>);
	  
	  %<y> = ylower + uLambda[1]*(yupper - ylower);
	}
	%break
      %default
	%assign t = LibBlockParameterAddr(table,"","",0)
	/* Linear %<numDims>-D interpolation */
	%<y> = rt_LookupLinearInterpND_%<ttype>(%<numDims-1>, 0,
	       &%<LibBlockMode("0", "", "")>, uLambda, dimSizes, %<t>);
    %endswitch
    %break
  %case 3
    %%
    %% SPLINE interpolation
    %% 
    %%
    /* Natural Spline %<numDims>-D interpolation */
    %<y> = rt_LookupSplineInterpND_%<ttype>(%<numEl>, 
           (SplineWorkVect *)%<LibBlockPWork(splWork, "", "", 0)>);
    %break
%endswitch
}

%endfunction
%%
%% Function: Terminate =========================================================
%% Abstract:
%%    For malloc'ed work vectors, free memory
%%
%function Terminate(block, system) Output
  %if UsingMalloc
    /* Look-Up Table (N-D) (LookupNDInterp) */
    /*   Block: %<Name> */

    #if defined(RT_MALLOC) || defined(MATLAB_MEX_FILE)
    rt_FREE(%<LibBlockPWork(uLambdas, "", "", 0)>);
    rt_FREE(%<LibBlockPWork(uVect, "", "", 0)>);
    %if SFcnParamSettings.interpMethod == 3
      rt_FREE(%<LibBlockPWork(splWork, "", "", 0)>);
      rt_FREE(%<LibBlockPWork(yyA,     "", "", 0)>);
      rt_FREE(%<LibBlockPWork(yyB,     "", "", 0)>);
      rt_FREE(%<LibBlockPWork(yy2,     "", "", 0)>);
      rt_FREE(%<LibBlockPWork(up,      "", "", 0)>);
      rt_FREE(%<LibBlockPWork(y2,      "", "", 0)>);
    %endif
    #endif
  %endif

%endfunction
%% [EOF] sfun_lookupnd.tlc