%% $RCSfile: sdspunwrap2.tlc,v $
%% $Revision: 1.11 $  $Date: 2000/05/11 18:43:37 $
%%
%% Copyright (c) 1995-2000 The MathWorks, Inc.
%%
%% Abstract: Unwrap a vector of radian phase angles.

%implements sdspunwrap2 "C"

%% Function: BlockInstanceSetup ===============================================
%%
%function BlockInstanceSetup(block, system) void

    %assign INPORT  = 0
    %assign OUTPORT = 0
 
    %assign framebased = LibBlockInputSignalIsFrameData(INPORT)

    %assign numDims = LibBlockInputSignalNumDimensions(INPORT)
    %assign inDims  = LibBlockInputSignalDimensions(INPORT)
    %assign inCols  = (numDims == 2) ? inDims[1] : 1
    %assign inRows  = inDims[0]
    %assign inplace = (LibBlockInputSignalBufferDstPort(INPORT) == OUTPORT)
    %assign cutoff  = FEVAL("abs",SFcnParamSettings.Cutoff)

    %% If the input is a sample-based row, then we treat it
    %% as if it were a column instead of unwraping sclars. 
    %%
    %if(!framebased && inRows==1)
        %assign inRows = inCols
        %assign inCols = 1
    %endif

    %assign block = block  + INPORT + OUTPORT + framebased + ...
                    inCols + inRows + inplace + cutoff

%endfunction %% BlockInstanceSetup


%% Function: Start =============================================================
%% Abstract:
%%
%function Start(block, system) Output
%%
  %if !LibIsEqual(TID, "constant")
    %if framebased
        %<LibBlockDWork(FirstStep,"","",0)> = (boolean_T)1;
    %endif
  %endif
%%
%endfunction  %% Start


%% Function: Outputs ==========================================================
%%
%function Outputs(block, system) Output
    /* DSP Blockset Unwrap (%<ParamSettings.FunctionName>) - %<Name> */
    %%
    %<InitOnFirstStep()> \
    %%
    %if(!framebased && (inRows == 1) && inplace)
        /* No code needed for when number of input rows is 1 with inplace algorithm. */
    %else
        {
          %if inRows > 1
            const real_T cutoff = %<cutoff>;
            const int_T  inRows = %<inRows>;
          %endif
          %%
          %if inCols > 1
            const int_T  inCols = %<inCols>;
          %endif
          %%
          real_T *y = %<LibBlockOutputSignalAddr(OUTPORT,"","",0)>;
          %%
          %if(inplace)
              %if !framebased
                %<UnwrapInPlace(inRows, inCols)>
              %else
                %<UnwrapInPlaceRunning(inRows, inCols)>
              %endif
          %else
              %if !framebased
                %<Unwrap(inRows, inCols)>
              %else
                %<UnwrapRunning(inRows, inCols)>
              %endif
          %endif
        }
    %endif

%endfunction


%% Function: InitOnFirstStep ==========================================================
%%
%function InitOnFirstStep( ) Output 
    %%
    %if framebased
        %assign prev   = LibBlockDWorkAddr(Prev,"","",0)
        %assign cumsum = LibBlockDWorkAddr(Cumsum,"","",0)
        %assign u      = LibBlockInputSignalAddr(INPORT,"","",0)
        %%
       /* First sample in channel will = prev, thus the first
        * sample in the channel will remain unchanged because
        * the incremental phase variation will be zero. 
        * (current - previous) 
        */
        if(%<LibBlockDWork(FirstStep,"","",0)>==1)
        {
            %if inCols > 1
                real_T  *u        = %<u>;
                real_T  *prev     = %<prev>;
                real_T  *cumsum   = %<cumsum>;
                int_T   nChans    = %<inCols>;
                int_T   frameSize = %<inRows>;
                int_T   i;

                for(i=0; i<nChans; i++) {
                    *cumsum++ = 0.0;
                    *prev++   = *u;
                    u += frameSize;
                }
            %else
                %<LibBlockDWork(Cumsum,"","",0)> = 0.0;
                %<LibBlockDWork(Prev,"","",0)> = %<LibBlockInputSignal(INPORT,"","",0)>;
            %endif
            %%
            %<LibBlockDWork(FirstStep,"","",0)>= (boolean_T)0;
        }
    %endif
    %%
%endfunction %% InitOnFirstStep


%% Function: UnwrapCore ==========================================================
%%
%function UnwrapCore( ) Output 
%%
    /* Equivalent phase variations in [-pi,pi).
     * floor and fmod have precision problmes so we use 
     * our own inlined fmod amd floor code to replace
     * dp_shift = (fmod(dp+pi, TWO_PI_DOUBLE)) - pi;  and
     * dp_shift =  (dp+pi - TWO_PI_DOUBLE * floor((dp+pi)/TWO_PI_DOUBLE)) - pi;
     */
    real_T dp_tmp   = (dp+PI_DOUBLE)/TWO_PI_DOUBLE;
    real_T dp_floor = (int_T)( (dp_tmp > 0.0) ? dp_tmp : (dp_tmp - 0.9999999999) );
    real_T dp_shift = (dp + PI_DOUBLE - TWO_PI_DOUBLE * dp_floor) - PI_DOUBLE;            

    /* Preserve variation sign for pi vs. -pi */
    if(dp_shift==-PI_DOUBLE && dp>0.0) {
        dp_shift = PI_DOUBLE;
    }
    /* Incremental phase corrections */
    dp_correction = dp_shift - dp;

    /* Ignore correction when incr. variation is not greater than CUTOFF */
    if((fabs(dp_correction) > cutoff)) {
        cumsum += dp_correction;
    }
%%
%endfunction %% UnwrapCore


%% Function: UnwrapInPlace ==========================================================
%%
%function UnwrapInPlace(inRows, inCols) Output 
%%      
    %if inCols > 1
            int_T   j;
            for(j=0; j<inCols; j++) {
    %endif
    %%
    real_T cumsum        = 0.0;  
    real_T dp_correction = 0.0;
    real_T prev          = *y++;
    int_T i;

    for(i=0; i<(inRows-1); i++) { 
        /* Incremental phase variations */ 
        real_T dp = *y - prev;  
   
        %<UnwrapCore()>\
        prev = *y;
        (*y++) += cumsum;
    }  /* for inRows */
    %%
    %if inCols > 1
      } /* for inCols */
    %endif 
    %%
%endfunction  %% UnwrapInPlace


%% Function: UnwrapInPlaceRunning  ============================================
%%
%function UnwrapInPlaceRunning(inRows, inCols) Output 
%%      
    real_T *prev    = %<LibBlockDWorkAddr(Prev,"","",0)>;
    real_T *pcumsum = %<LibBlockDWorkAddr(Cumsum,"","",0)>;  
    %%
    %if inCols > 1
        int_T   j;

        for(j=0; j<inCols; j++) {
    %endif
    %%
        real_T cumsum        = *pcumsum;
        real_T y_prev        = *prev;
        real_T dp_correction = 0.0; 
        int_T i;

    for(i=0; i<inRows; i++) { 
        /* Incremental phase variations */ 
        real_T dp = *y - y_prev;  
   
        %<UnwrapCore()>\
        y_prev = *y;
        (*y++) += cumsum;
    }
    *prev++    = y_prev;  /* Save last value in frame     */
    *pcumsum++ = cumsum;  /* Save cumsum for each channel */

    %%
    %if inCols > 1
      }
    %endif 
    %%
%endfunction  %% UnwrapInPlaceRunning


%% Function: Unwrap ==========================================================
%%
%function Unwrap(inRows, inCols) Output 
    %%      
    %% Non-inplace algorithm
    %%
    real_T *u = %<LibBlockInputSignalAddr(INPORT,"","",0)>;
    %%
    %if inCols > 1
      int_T   j;
      for(j=0; j<inCols; j++) {
    %endif
        %%
        %if inRows == 1
            *y++ = *u++;
        %else
            real_T cumsum        = 0.0;  
            real_T dp_correction = 0.0;
            int_T i;

            *y++ = *u++;  /* The first value is unchanged */
        
            for(i=0; i<(inRows-1); i++) { 
                /* Incremental phase variations. (current - previous) */ 
                real_T dp = *u - *(u-1);  
                
                %<UnwrapCore()> \            
                *y++ = *u++ + cumsum;
            }  
        %endif
        %%
    %if inCols > 1
      }
    %endif
    %%
%endfunction  %% Unwrap


%% Function: UnwrapRunning ==========================================================
%%
%function UnwrapRunning(inRows, inCols) Output 
    %%      
    real_T *u       = %<LibBlockInputSignalAddr(INPORT,"","",0)>;
    real_T *prev    = %<LibBlockDWorkAddr(Prev,"","",0)>;
    real_T *pcumsum = %<LibBlockDWorkAddr(Cumsum,"","",0)>;  
    %%
    %if inCols > 1
      int_T   j;
      for(j=0; j<inCols; j++) {
    %endif
        %%
        %if inRows == 1
            *y++ = *u++;
        %else
            real_T cumsum        = *pcumsum;
            real_T dp_correction = 0.0;
            real_T dp;
            int_T i;

            for(i=0; i<inRows; i++) { 
                if(i==0) {
                    dp = *u - *prev; /* Done once for each channel */
                } else {
                    dp = *u - *(u-1); 
                }
                {
                	%<UnwrapCore()> \            
                }
                *y++ = *u++ + cumsum;
            }  
            *prev++    = *(u-1);  /* Save last value in frame     */
            *pcumsum++ = cumsum; /* Save cumsum for each channel */
            %%
        %endif
        %%
    %if inCols > 1
      }
    %endif
    %%
%endfunction  %% UnwrapRunning


%% [EOF] sdspunwrap2tlc