%% $RCSfile: sdspvfdly2.tlc,v $
%% $Revision: 1.10 $ 
%% $Date: 2000/08/29 22:27:00 $
%%
%% Dale Shpak, R. Firtion
%% Copyright (c) 1995-2000 The MathWorks, Inc. 
%%
%% Abstract: Variable Fractional Delay
%%

%implements sdspvfdly2 "C"

%include "dsplib.tlc"
%include "dsp_ic.tlc"


%% Function: BlockTypeSetup ================================================
%% Abstract:
%%
%function BlockTypeSetup(block, system) void

%% Render the clipDelayValue function

%% First, cache the function prototype for clipDelayValue:
%%
%openfile clipDelayBuff
extern int clipDelayValue(real_T *delay, int_T dmax);
%closefile clipDelayBuff
%<DSPAddToFileHeader(clipDelayBuff)>

%% Next, cache the clipDelayValue function itself:
%%
%openfile clipDelayBuff
/* Function: clipDelayValue
 *	Rounds the delay values if they are greater than the maximum
 *		or are not whole numbers.
 */
extern int clipDelayValue( real_T *delay, int_T dmax)
{
    /* Clip delay time to legal range: [0,dmax] */
    real_T dly = *delay;
    if (dly < 0.0) {
        dly = 0.0;
    } else if (dly > (real_T)dmax) {
        dly = (real_T)dmax;
    }
    *delay = dly;
    return((int_T)dly);  /* Integer part of delay time */

} /* clipDelayValue */

%closefile clipDelayBuff
%<DSPAddToFile(clipDelayBuff)>

%endfunction  %% BlockTypeSetup


%% Function: BlockInstanceSetup ===============================================
%%
%% Abstract:
%%		Name the S-function parameters
%function BlockInstanceSetup (block, system) void
    %%
    %assign INPORT      = 0
    %assign DELAY_PORT  = 1
    %assign OUTPORT     = 0
    %assign FILT        = SFcnParamSettings.FILTER
    %assign portWidth   = LibDataInputPortWidth(INPORT)
    %assign cplx_in     = LibBlockInputSignalIsComplex(INPORT)
    %assign sl_dtype_in = LibBlockInputSignalDataTypeName(INPORT,"%<tRealPart>")
    %assign dtype_in    = (cplx_in) ? "c" + sl_dtype_in : sl_dtype_in
	%%
    %assign dmax      = SFcnParamSettings.maxDelay
	%assign isFrame   = LibBlockInputSignalIsFrameData(INPORT) 
	%assign nChans    = SFcnParamSettings.ICs_nChans
	%assign nSamps    = portWidth / nChans
    %assign buffLen   = LibBlockDWorkWidth(BUFF)/nChans
    %assign filterLen = SIZE(FILT,1)
	%%
	%assign incrementDelayPerChannel = SFcnParamSettings.incrementDelayPerChannel
	%assign incrementDelayPerSample  = SFcnParamSettings.incrementDelayPerSample
    %%
    %assign numPhases = SFcnParamSettings.NUM_PHASES
    %if (filterLen == 1 && FILT[0] == 0.0)
        %assign hLen = 0
    %else
        %assign hLen = filterLen / (2 * numPhases)
    %endif

    %assign block = block + INPORT + OUTPORT + DELAY_PORT \
					+ FILT + hLen + portWidth + dtype_in + cplx_in + dmax \
					+ buffLen + filterLen + isFrame + nChans + nSamps + numPhases \
					+ incrementDelayPerChannel + incrementDelayPerSample 

    %% Setup the initial condition handler:
    %%
    %<CopyICsInstanceSetup(block)>

%endfunction %% BlockInstanceSetup


%% Function: InitializeConditions =============================================
%%
%% Abstract:
%%      Initialize the DWork vector (Buffer) to the initial values specified.
%%
%function InitializeConditions(block, system) Output
    /* DSP Blockset Variable Fractional Delay (%<ParamSettings.FunctionName>) - %<Name> */
	%%
	%<LibBlockDWork(BUFF_OFFSET, "", "", 0)> = %<dmax + hLen - 1>;
	%%
	%<CallCopyICsFcn(block)>

%endfunction


%% Function: Outputs ==========================================================
%%
%function Outputs(block, system) Output
{ 
  /* DSP Blockset Variable Fractional Delay (%<ParamSettings.FunctionName>) - %<Name> */
  %%
  %% Block could be multirate if and only if it has more than
  %% one input port (eg, a "Delay" input).  If it is multirate,
  %% generate "sample hit" conditional so code executes at the
  %% block's base rate, and not the overall model base rate.
  %%
  %assign differentPortRates = !LibIsSFcnSingleRate(block)
  %%
  %if differentPortRates
    if (%<LibIsSFcnSampleHit("InputPortIdx0")>) { %<LibTaskSFcnComment("InputPortIdx0")>
  %endif
    %%
    %assign delayLen = LibDataInputPortWidth(DELAY_PORT)
    %%
    %<dtype_in> *u    = %<LibBlockInputSignalAddr(INPORT, "", "", 0)>;
    %<dtype_in> *buff = %<LibBlockDWorkAddr(BUFF, "", "", 0)>;
    %<dtype_in> *y    = %<LibBlockOutputSignalAddr(OUTPORT, "", "", 0)>;
    %<dtype_in> *in_curr;
    real_T       t  = 0.0; 
    int_T        ti = 0;   
    int_T        i,j;
    real_T *dptr      = %<LibBlockInputSignalAddr(DELAY_PORT, "", "", 0)>;
               
	for (i=0; i++ < %<nChans>; ) {
            int_T   buffstart = %<LibBlockDWork(BUFF_OFFSET, "", "", 0)>;

            /* For a scalar delay input, we need to reset t and ti for each channel: */
            t  = *dptr;
            ti =  clipDelayValue(&t, %<dmax>);

	    for (j=0; j++ < %<nSamps>; ) {
                if (++buffstart == %<buffLen>) buffstart = 0;  /* Rotate circular buffer */
                *(buff + buffstart) = *u++;

                %if (incrementDelayPerSample) 
                    t  = *dptr++;  /* Get delay time from 2nd input port */                
                    ti =  clipDelayValue(&t, %<dmax>);
                %else
    		    t  = *dptr;
	            ti =  clipDelayValue(&t, %<dmax>);
		%endif
            
            /* Output new interpolation point */
            in_curr = buff;

            %if (hLen > 0)
                /* Check if we need to use linear interp: */
                if (ti < %<hLen - 1>) {
            %else
                {
            %endif
                /* Linear interpolation: */
                real_T frac  = t - ti;    /* Fractional part of delay time */
                real_T frac1 = 1 - frac;
                
                /* Add offset to current input pos'n in buffer, backing up by
                * the integer number of delay samples.  If we've backed up too
                * far past the start of the buffer, wrap to the end:
                */
                ti = buffstart - ti;
                if (ti < 0) ti += %<buffLen>;
                in_curr += ti; /* Get pointer into buffer */
                
                if (ti > 0) {
                    /* The two points are adjacent in memory: */
                    %if (cplx_in)
                        y->re     = in_curr[0].re * frac1 + in_curr[-1].re * frac;
                        (y++)->im = in_curr[0].im * frac1 + in_curr[-1].im * frac;
                    %else
                        *y++ = in_curr[0] * frac1 + in_curr[-1] * frac;
                    %endif
                } else {
                    /* The two points are at the end and start of buffer: */
                    %if (cplx_in)
                        y->re     = in_curr[0].re * frac1 + in_curr[%<buffLen - 1>].re * frac;
                        (y++)->im = in_curr[0].im * frac1 + in_curr[%<buffLen - 1>].im * frac;
                    %else
                        *y++ = in_curr[0] * frac1 + in_curr[%<buffLen - 1>] * frac;
                    %endif
                }
            %if (hLen == 0)
                }
            %else
                } else {
                /* FIR Interpolation: */
                %assign firFilterLen    = filterLen / numPhases
                %assign coeffs        = ""
                %assign count       = 0
                const real_T filter[%<filterLen>] = {
                %foreach Idx = numPhases
                    %foreach Idx2 = firFilterLen
                        %assign coeffs = coeffs + "%<FILT[count]>"
                        %assign count = count + 1
                        %if (count < filterLen) 
                            %assign coeffs = coeffs + ", "
                        %else
                            %assign coeffs = coeffs + " };"
                        %endif
                    %endforeach
                    %<coeffs>
                    %assign coeffs = ""        
                %endforeach
                const real_T frac = t - ti;    /* Fractional part of delay time */
                int_T phase       = (int_T) (%<numPhases> * frac + 0.5); /* [0,nphases] */
                
                if (phase == %<numPhases>) { ++ti;  phase = 0; }
           
                /* Add offset to current input pos'n in buffer, backing up by the
                 * integer number of delay samples then incrementing by half the
                 * filter length.  If we've backed up too far past the start of the
                 * buffer, wrap:
                 */
                ti = buffstart - ti + %<hLen - 1>;
                
                if (ti < 0) ti += %<buffLen>;
                in_curr += ti; /* Get pointer into buffer */
                
                if (ti+1 >= %<firFilterLen>) {
                    /* Contiguous input samples in buffer: */
                    /* Point to correct polyphase filter */
                    const real_T *filt = filter + phase*%<firFilterLen>; 
					int_T kn;

                    %if (cplx_in)
                        y->re = y->im = 0.0;
                        for(kn=0; kn< %<firFilterLen>; kn++) {
                            y->re += in_curr[-kn].re * *filt;
                            y->im += in_curr[-kn].im * *filt++;
                        }
                    %else
                        *y = 0.0;
                        for(kn=0; kn <%<firFilterLen>; kn++) {  
                            *y += in_curr[-kn] * *filt++;
                        }
                    %endif
                    ++y;
                } else {
                    /* Discontiguous input samples in buffer: */
                    const real_T     *filt     = filter + phase*%<firFilterLen>;
                    const int_T        k1      = ti + 1;
                    const int_T        k2      = %<firFilterLen> - k1;
                    const %<dtype_in> *in_last = in_curr - ti + %<buffLen - 1>;
                    int_T kn;
                    
                    %if (cplx_in)
                        y->re = y->im = 0.0;
                        for(kn=0; kn  <k1; kn++) {
                            y->re += in_curr[-kn].re * *filt;
                            y->im += in_curr[-kn].im * *filt++;
                        }
                        for(kn=0; kn < k2; kn++) {
                            y->re += in_last[-kn].re * *filt;
                            y->im += in_last[-kn].im * *filt++;
                        }
                    %else
                        *y = 0.0;
                        for(kn=0; kn  <k1; kn++) {
                            *y += in_curr[-kn] * *filt++;
                        }
                        for(kn=0; kn < k2; kn++) {
                            *y += in_last[-kn] * *filt++;
                        }
                    %endif
                    ++y;
                }
             } /* FIR interpolation */
            %endif
        }  %% nSamps loop
		%%
		buff += %<buffLen>;
		%if (incrementDelayPerSample && delayLen == nSamps) 
			dptr -= %<delayLen>;
		%endif
		%if (incrementDelayPerChannel)
			dptr++;  
		%endif
    }
	%%
    %<LibBlockDWork(BUFF_OFFSET, "", "", 0)> += %<nSamps>;
    while (%<LibBlockDWork(BUFF_OFFSET, "", "", 0)> >= %<buffLen>) %<LibBlockDWork(BUFF_OFFSET, "", "", 0)> -= %<buffLen>;
  %if differentPortRates
    }
  %endif
}

%endfunction  %% Outputs

%% [EOF] sdspvfdly2.tlc