%% 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 "Ada"
 
%% 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
  %assign ttype = LibBlockOutputSignalDataTypeName(0,"")
  %switch SFcnParamSettings.interpMethod
    %case 2
      %if numDims > 2
	%if ttype == "TMW_Types.Real_T" || ttype == "TMW_Types.Real_T'Access"
	  %assign ::CompiledModel.WithRTLookupND = 1
	%else
	  %assign ::CompiledModel.WithRTLookupND32 = 1
	%endif
      %endif
      %break
    %case 3
      %if ttype == "TMW_Types.Real_T" || ttype == "TMW_Types.Real_T'Access"
	%assign ::CompiledModel.WithRTSplineND = 1
      %else
	%assign ::CompiledModel.WithRTSplineND32 = 1
      %endif
      %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
  %else
    declare
    uLambdasBuffer : array (0 .. %<numDims>) of %<ttype>;
    uVectBuffer : array (0 .. %<numDims>) of %<ttype>;
    %if SFcnParamSettings.interpMethod == 3
       splWork :  SplineWorkVect;
      yyABuffer : array (0 .. %<largestVec>) of %<ttype>;
      yyBBuffer : array (0 .. %<largestVec>) of %<ttype>;
      yy2Buffer : array (0 .. %<largestVec>) of %<ttype>;
      upBuffer : array (0 .. %<biggestDim>) of %<ttype>;
      y2Buffer : array (0 .. %<tabSize[1]>) of %<ttype>;
    %endif
    begin
    %assign vCast = ""
    %<LibBlockPWork(uLambdas, "", "", 0)> := %<vCast>uLambdasBuffer'access;
    %<LibBlockPWork(uVect, "", "", 0)> := %<vCast>uVectBuffer'access;
    %if SFcnParamSettings.interpMethod == 3
      %<LibBlockPWork(splWork, "", "", 0)> := %<vCast>splWork'access;
      %<LibBlockPWork(yyA, "", "", 0)> := %<vCast>yyABuffer'access;
      %<LibBlockPWork(yyB, "", "", 0)> := %<vCast>yyBBuffer'access;
      %<LibBlockPWork(yy2, "", "", 0)> := %<vCast>yy2Buffer'access;
      %<LibBlockPWork(up,  "", "", 0)> := %<vCast>upBuffer'access;
      %<LibBlockPWork(y2,  "", "", 0)> := %<vCast>y2Buffer'access;
    %endif
    end;
  %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)
    declare
    splWk : SplineWorkVect_Access := %<tsplWork>;

    begin
    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", "", "")>'access;
    splWk.numYWorkElements = %<LibBlockIWork(numYWorkElements,"0","","")>'access;
    splWk.maxIdx = %<LibBlockIWork(maxIdx, "0", "", "")>'access;
    splWk.bpIndices = %<LibBlockParameterAddr(bpIndices, "0", "", "")>;
    splWk.bpData = %<LibBlockParameterAddr(bpData, "0", "", "")>;
    splWk.tableData = %<LibBlockParameterAddr(table, "0", "", "")>;
    splWk.extrapMethod = %<CAST("Number", SFcnParamSettings.extrapMethod)>;
    end;
  %endif
  
%endfunction
 
%% Function: Outputs ==========================================================
%% Abstract:
%%    Perform indicated interpolation, flat or linear
%%
%function Outputs(block, system) Output
  -- Look-Up Table (N-D) (LookupNDInterp)
  --   Block: %<Name>
  declare
  %assign ttype = LibBlockOutputSignalDataTypeName(0,"")
  
  %if SFcnParamSettings.vectorInputFlag == 0
    %assign numEl = NumDataInputPorts
  %else
    %assign numEl = LibBlockInputSignalWidth(0)
  %endif
  
  -- retrieve work vectors
  %assign declULambda = "uLambda : %<ttype>_Access"
  %assign declUVect   = "uVect : %<ttype>_Access"
  %assign vCast       = ""
  %<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)
    dimSizes : TMW_Types.Int_T_Access := %<LibBlockIWork(dimSizes, "", "", 0)>'access;
  %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
    
  begin
  -- do index search for each input
  declare
  %assign declBpData = "bpData : %<ttype>_Access"
  %if numEl > 1
    %assign iVar = "I"
    %if SFcnParamSettings.searchMode > 1
      lambda : %<ttype>;
    %endif
    %<iVar> : TMW_Types.Int_T;
    
    begin
    for %<iVar> in  0 ..  %<numEl> loop
      declare
      bpBaseIdx : TMW_Types.Int_T := \
                         %<LibBlockParameter(bpIndices, "%<iVar>", "", "")>;
      %<declBpData> := %<LibBlockParameterAddr(bpData,"bpBaseIdx","","")>;
      maxIdx : TMW_Types.Int_T := \
            %<LibBlockIWork(maxIdx, "%<iVar>", "", "")>;
      %assign istr   = "%<iVar>" 
      %assign istr2  = "2*%<iVar>"
      %assign istr21 = "2*%<iVar>+1"
  %else
    %<declBpData> := %<LibBlockParameterAddr(bpData,"","",0)>;
    maxIdx : TMW_Types.Int_T := \
             %<LibBlockIWork(maxIdx, "0", "", "")>;
    %assign istr   = "0"
    %assign istr2  = "0"
    %assign istr21 = "1"
  %endif
  %%
  %if SFcnParamSettings.rangeErrorMode < 3 || ...
    SFcnParamSettings.searchMode > 1
    %if numEl == 1
      lambda : %<ttype>;
    %endif
    %if SFcnParamSettings.searchMode != 1 || ...
      SFcnParamSettings.extrapMethod != 1
      bpFound : boolean := false;
    %endif
    %if numEl == 1
      %assign idx = 0
      %if SFcnParamSettings.vectorInputFlag == 0
	u : %<ttype> := %<LibBlockInputSignal(idx, "", "", 0)>;
      %else
	u : %<ttype> := %<LibBlockInputSignal(0, "", "", "%<idx>")>;
      %endif
    %else
	u : %<ttype> := uVect(%<istr>);
    %endif
    %if SFcnParamSettings.searchMode > 1  %% is this an actual search?
      %if SFcnParamSettings.cacheBpFlag == 1
	index : TMW_Types.Int_T := \
	                        %<LibBlockMode("%<istr>", "", "")>; \
        -- get previous index
      %else
	index : TMW_Types.Int_T := 0;
      %endif
    %elseif SFcnParamSettings.extrapMethod != 1
      index : TMW_Types.Int_T;
    %endif
  %endif
  
  begin
  %if SFcnParamSettings.extrapMethod == 1
    -- clipping
    %if SFcnParamSettings.rangeErrorMode != 3
      %if SFcnParamSettings.searchMode == 1
	if (u < bpData(0)) then
	  u := bpData(0);
	elsif (u > bpData(maxIdx)) then
	  u := bpData(maxIdx);
	end if;
      %else
	if (u < bpData(0)) then
	  index        := 0;
	  lambda       := 0.0;
	  bpFound      := true;
	elsif (u > bpData(MaxIdx)) then
	  index        := maxIdx - 1;
	  lambda       := 1.0;
	  bpFound      := true;
	end if;
      %endif
    %endif
  %else
    -- extrapolation (check)
    if (u < bpData(0)) then
      index        := 0;
      lambda       := (u - bpData(0)) / (bpData(1) - bpData(0));
      bpFound      := true;
    elsif (u > bpData(MaxIdx)) then
      index        := maxIdx - 1;
      lambda       := (u - bpData(MaxIdx-1)) / ...
	(bpData(MaxIdx) - bpData(MaxIdx-1));
      bpFound      := true;
    end if;
  %endif
  %%  
  %switch (SFcnParamSettings.searchMode)
    %case 1
      
      %if SFcnParamSettings.extrapMethod > 1
      if (bpFound = false) then
      %endif
        -- directly calculate index and fraction
        uLambda(%<istr>) := (u - bpData(0)) /
                          (bpData(1) - bpData(0));
        %<LibBlockMode("%<istr>", "", "")> := TMW_Types.int_T(uLambda(%<istr>));
	if ( %<LibBlockMode("%<istr>", "", "")> = maxIdx ) then
	    %<LibBlockMode("%<istr>", "", "")> := %<LibBlockMode("%<istr>", "", "")> - 1;
	  uLambda(%<istr>) := 1.0;
	else
            uLambda(%<istr>) := uLambda(%<istr>) - ...
	                  %<ttype>(%<LibBlockMode("%<istr>", "", "")>);
	end if;
      %if SFcnParamSettings.extrapMethod > 1
        else
        %<LibBlockMode("%<istr>", "", "")> := index;
	uLambda(I) := lambda;
        end if;
      %endif
      %break
    %case 2
      
      -- in-range linear search for [ u )
      while (bpFound = false) loop
	
	lambda := (u - bpData(Index)) /
	(bpData(Index+1) - bpData(Index));
	
	-- check result
	if (lambda >= 0.0 and then lambda < 1.0 ) then
	  bpFound := true;
	elsif (lambda > 1.0) then
	  index := index + 1;
	elsif (lambda < 0.0) then
	  index := index - 1;
	elsif (lambda = 1.0) then
	  bpFound := true;
	  if ( index < (maxIdx-1) ) then
	    index := index + 1;
	    lambda := 0.0;
	  end if;
	end if;
      end loop;

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

      %break
    %case 3

      -- in-range binary search for [ u )
      if (bpFound = false) then
      declare
	bottom : TMW_Types.Int_T := 0;
	top : TMW_Types.Int_T := maxIdx;

      begin
	while (bpFound = false) loop
	  if (u < bpData(Index)) then
	    top := index - 1;
	    index := (top + bottom) / 2;
	  elsif (u <= bpData(Index+1)) then
	    bpFound := true;
	    if ( u = bpData(Index+1) ) then
	      index := index + 1;
	    end if;
	  else
	    bottom = index + 1;
	    index := (top + bottom) / 2;
	  end if;
	end loop;

        if (index = maxIdx) then
	  index := index -1;
	  lambda := 1.0;
        else
	  lambda := (u - bpData(Index)) / 
	  (bpData(Index+1) - bpData(Index));
        end if;
      end;
      end if;

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


%endswitch
%%
%if numEl > 1
end;
end loop;
%endif
end;

%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
      declare
	-- flat, no interpolation - use point at beginning of interval
	offset : TMW_Types.Int_T;

        begin
	%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", "", "")>;
      end;
    %else
      %<y> := %<LibBlockParameter(table, "%<LibBlockMode("0", "", "")>", "", "")>;
    %endif
    %break
  %case 2
    %%
    %% LINEAR interpolation
    %%
    %switch numDims
      %case 1
	-- Linear 1-D interpolation
	declare
	  offset : TMW_Types.Int_T := \
	           %<LibBlockMode("0", "", "")>;

	  begin
	  %<y> := %<LibBlockParameter(table, "offset", "", "")> + ...
	    uLambda(0)*(%<LibBlockParameter(table, "offset+1", "", "")> - ...
	    %<LibBlockParameter(table, "offset", "", "")>);
	end;
	%break
      %case 2
	%assign ttype = LibBlockOutputSignalDataTypeName(0,"")
	%assign t1 = LibBlockParameter(table, "offset", "", "")
	%assign t2 = LibBlockParameter(table, "offset+1", "", "")
	-- Linear 2-D interpolation
	declare
	  offset : TMW_Types.Int_T := \
	    %<LibBlockMode("0", "", "")> + ...
	    %<tdimSizes[1]> * %<LibBlockMode("1", "", "")>;
	  ylower : %<ttype>;
	  yupper : %<ttype>;

	  begin
	  ylower := %<t1> + uLambda(0)*
	            (%<t2> - %<t1>);
	  offset := offset + %<tdimSizes[1]>;
	  yupper := %<t1> + uLambda(0)*
	            (%<t2> - %<t1>);
	  
	  %<y> := ylower + uLambda(1)*(yupper - ylower);
	end;
	%break
      %default
	%assign t = LibBlockParameterAddr(table,"","",0)
	-- Linear %<numDims>-D interpolation
	%% xxx(g#####) REMOVE NEXT 3 LINES FOR ADA CODE GEN
	%assign errTxt = "Ada code generation not fully implemented at this time"+\
	                 " for Look-Up Table (N-D) (LookupND) block (G#####)"
	%<LibBlockReportFatalError(block, errTxt)>
	%<y> := rt_LookupLinearInterpND_%<ttype>(%<numDims-1>, 0,
	       &%<LibBlockMode("0", "", "")>, uLambda, dimSizes, %<t>);
    %endswitch
    %break
  %case 3
    %%
    %% SPLINE interpolation
    %% 
    %%
    %% xxx(g#####) REMOVE NEXT 3 LINES FOR ADA CODE GEN
    %assign errTxt = "Ada code generation not fully implemented at this time"+\
                     " for Look-Up Table (N-D) (LookupND) block (G#####)"
    %<LibBlockReportFatalError(block, errTxt)>
    -- Natural Spline %<numDims>-D interpolation
    %<y> := rt_LookupSplineInterpND_%<ttype>(%<numEl>, 
           )%<LibBlockPWork(splWork, "", "", 0)>);
    %break
%endswitch
end;

%endfunction
%%
%% Function: Terminate =========================================================
%% Abstract:
%%    For malloc'ed work vectors, free memory
%%
%function Terminate(block, system) Output
%endfunction
%% [EOF] sfun_lookupnd.tlc