/*
 *  SDSPACFC.c - complex autocorrelation function.
 *  DSP Blockset S-Function to compute the complex autocorrelation function.
 *
 *  Computes positive lags (0,1,...,M-1) for a real length-N input
 *  with M <= N.
 *
 *  Bias flag: 1=none, 2=biased, 3=unbiased,
 *             4="unity at zero-lag" (corresponds to "coeff" in xcorr.m)
 *
 *  Copyright 1995-2000 The MathWorks, Inc.
 *  $Revision: 1.12 $ $Date: 2000/06/14 14:27:58 $
 */

#define S_FUNCTION_NAME sdspacfc

#include "dsp_sim.h"

#define MAXLAG_ARG ssGetArg(S,0)
#define BIAS_ARG   ssGetArg(S,1)
#define NUM_ARGS   2

#define MAXLAG_IDX 0
#define NUM_IWORKS 1

#define NO_BIAS  1
#define BIASED   2
#define UNBIASED 3
#define COEFF    4

#ifdef MATLAB_MEX_FILE
#define MDL_CHECK_PARAMETERS
static void mdlCheckParameters(SimStruct *S) {
    const char *msg = NULL;
    int_T       i;
    real_T      d;

    /* Check MAXLAG_ARG: */
    if ((mxGetM(MAXLAG_ARG) != 1) ||
        (mxGetN(MAXLAG_ARG) != 1) ||
        !mxIsDouble(MAXLAG_ARG)    ) {
        msg = "Maximum lag must be a scalar integer >= 0.  (Use -1 for all positive lags)";
        goto FCN_EXIT;
    }
    d = mxGetPr(MAXLAG_ARG)[0];
    i = (int_T)d;
    if ((d != i) || (d < -1)) {
        msg = "Maximum lag must be a scalar integer >= 0.  (Use -1 for all positive lags)";
        goto FCN_EXIT;
    }
    
    /* Check BIAS_ARG: */
    if ((mxGetM(BIAS_ARG) != 1) ||
        (mxGetN(BIAS_ARG) != 1) ||
        !mxIsDouble(BIAS_ARG)    ) {
        msg = "Bias type must be specified as a scalar";
        goto FCN_EXIT;
    }
    d = mxGetPr(BIAS_ARG)[0];
    i = (int_T)d;
    if ((d != i) || (d<1) || (d>4)) {
        msg = "Bias flag be one of the following:\n"
              "1=None, 2=Biased, 3=Unbiased, or 4=Coeff";
        goto FCN_EXIT;
    }

FCN_EXIT:
    ssSetErrorStatus(S,msg);
}
#endif


static void mdlInitializeSizes(SimStruct *S)
{
    ssSetNumSFcnParams(S, NUM_ARGS);

#if defined(MATLAB_MEX_FILE)
    if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) return;
    mdlCheckParameters(S);
    if (ssGetErrorStatus(S) != NULL) return;
#endif

    ssSetNumInputs(        S, DYNAMICALLY_SIZED);
    ssSetNumOutputs(       S, DYNAMICALLY_SIZED);
    ssSetDirectFeedThrough(S, 1);
    ssSetNumSampleTimes(   S, 1);
    ssSetNumIWork(         S, NUM_IWORKS);

    /* NOTE! For this block, we will NOT be using UPtrs,
     *       since the extra in-order copy of inputs values
     *       will be used to help algorithmic efficiency.
     */
    ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE);
}


static void mdlInitializeSampleTimes(SimStruct *S)
{
    ssSetSampleTimeEvent(S, 0, INHERITED_SAMPLE_TIME);
    ssSetOffsetTimeEvent(S, 0, 0.0);
}


static void mdlInitializeConditions(real_T *x0, SimStruct *S)
{
    int_T N      = ssGetNumInputs(S);
    int_T maxlag = (int_T)(mxGetPr(MAXLAG_ARG)[0]);

    /*
     * Check number of requested ACF coefficients.
     * Don't automatically zero-pad the input sequence.
     */
    if (maxlag+1 > N) {  /* Works even when maglag=-1 (i.e., inherit input width) */
        ssSetErrorStatus(S, "Maximum lag exceeds number of input samples.");
        return;
    }

    ssSetIWorkValue(S, MAXLAG_IDX, maxlag);
}


static void mdlOutputs(real_T *y, const real_T *x, const real_T *u, 
                       SimStruct *S, int_T tid)
{
    const int_T  Nin    = ssGetNumInputs(S);
    const int_T  N      = Nin/2;  /* # complex inputs */
    const int_T  maxlag = ssGetIWorkValue(S, MAXLAG_IDX);
    const int_T  nlags  = (maxlag == -1) ? N : maxlag+1;
    const int_T  bias   = (int_T)(mxGetPr(BIAS_ARG)[0]);
    real_T       norm   = (bias == BIASED) ? 1.0/N : 1.0;
    real_T      *yi     = y + nlags;  /* imag part of output */
    int_T        i;

    for (i=0; i<nlags; i++) {
        const real_T *r0     = u;           /* real part of input */
        const real_T *r1     = u+i;
        const real_T *i0     = r0 + N;  /* imag part of input */
        const real_T *i1     = r1 + N;
        real_T        r_sum  = 0.0;
        real_T        i_sum  = 0.0;
        int_T         jcnt   = N-i;

        while(jcnt-- > 0) {
            /* add: conj(u0) * u1 */
            r_sum += *r0   * *r1   + *i0   * *i1;
            i_sum += *r0++ * *i1++ - *r1++ * *i0++;
        }

        if ((i==0) && (bias == COEFF)) {
            norm = 1.0/r_sum;  /* 1 / R(0), R(0) is purely real */
        } 
        
        if (bias == UNBIASED) {
            *y++  = r_sum / (N-i);
            *yi++ = i_sum / (N-i);
        } else {
            *y++  = r_sum * norm;
            *yi++ = i_sum * norm;
        }
    }
}


static void mdlUpdate(real_T *x, const real_T *u, SimStruct *S, int_T tid)
{
}


static void mdlDerivatives(real_T *dx, const real_T *x, const real_T *u, 
                           SimStruct *S, int_T tid)
{
}


static void mdlTerminate(SimStruct *S)
{
}

#ifdef MATLAB_MEX_FILE
#define MDL_GET_INPUT_PORT_WIDTH
/* Function: mdlGetInputPortWidth =============================================
 *
 */
static int_T mdlGetInputPortWidth(SimStruct *S, int_T outputPortWidth)
{
    int_T maxlag = (int_T) (mxGetPr(MAXLAG_ARG)[0]);
    if (outputPortWidth % 2 != 2) {
        ssSetErrorStatus(S,"Output from block must be complex");
    }
    return( (maxlag == -1) ? outputPortWidth : DYNAMICALLY_SIZED);
}

#define MDL_GET_OUTPUT_PORT_WIDTH
/* Function: mdlGetOutputPortWidth ============================================
 *
 */
static int_T mdlGetOutputPortWidth(SimStruct *S, int_T inputPortWidth)
{
    int_T maxlag = (int_T) (mxGetPr(MAXLAG_ARG)[0]);
    if (inputPortWidth % 2 != 0) {
        ssSetErrorStatus(S,"Input to block must be complex");
        return(1);
    }
    return( (maxlag == -1) ? inputPortWidth : 2*(maxlag+1));
}
#endif


#include "dsp_trailer.c"
 
/* [EOF]: sdspacfc.c */