/*
 *  SDSPBURG2 - Burg method of AR parameter estimation.
 *
 *  Copyright (c) 1995-2000 The MathWorks, Inc.
 *  $Revision: 1.12 $ $Date: 2000/08/18 16:21:15 $
 */
#define S_FUNCTION_NAME sdspburg2
#define S_FUNCTION_LEVEL 2

#include "dsp_sim.h"

enum {INPORT=0, NUM_INPORTS};
#define NUM_OUTPORTS_MINIMUM 2
#define NUM_OUTPORTS_MAXIMUM 3

enum {FCN_TYPE_ARGC=0, INHERIT_ORDER_ARGC, ORDER_ARGC, NUM_PARAMS};
enum {FERR_IDX=0, BERR_IDX, ANEW_IDX, ACOEF_IDX, NUM_DWORKS};

#define FCN_TYPE_ARG        ssGetSFcnParam(S,FCN_TYPE_ARGC)
#define INHERIT_ORDER_ARG   ssGetSFcnParam(S,INHERIT_ORDER_ARGC)
#define ORDER_ARG           ssGetSFcnParam(S,ORDER_ARGC)

typedef enum {fcnAandK=1, fcnA, fcnK} FcnType;

/*
 * Determine output port number of reflection coefficients,
 * gain, or polynomial.  If not output, return -1.
 */

/* Polynomial port: */
static int_T getAPort(SimStruct *S)
{
    const FcnType ftype = (FcnType)((int_T)mxGetPr(FCN_TYPE_ARG)[0]);
    int_T port = (ftype == fcnK) ? -1 : 0;
    return(port);
}

/* Reflection coefficient port: */
static int_T getRCPort(SimStruct *S)
{
    const FcnType ftype = (FcnType)((int_T)mxGetPr(FCN_TYPE_ARG)[0]);
    int_T port = -1;

    if (ftype == fcnAandK) {
        port = 1;
    } else if (ftype == fcnK) {
        port = 0;
    }

    return(port);
}

/* Gain port: */
static int_T getGPortIdx(SimStruct *S)
{
    const FcnType ftype = (FcnType)((int_T)mxGetPr(FCN_TYPE_ARG)[0]);
    int_T port = (ftype == fcnAandK) ? 2 : 1;
    return(port);
}



#define MDL_CHECK_PARAMETERS
static void mdlCheckParameters(SimStruct *S) {
#ifdef MATLAB_MEX_FILE

    /* Function popup: */
    if (!IS_FLINT_IN_RANGE(FCN_TYPE_ARG, 1, 3)) {
        THROW_ERROR(S, "Function parameter must be a scalar in the range [1,3].");
    }

    /* Inherit order: */
    if(!IS_FLINT_IN_RANGE(INHERIT_ORDER_ARG,0,1)) {
        THROW_ERROR(S, "Inherit order argument must be 0 or 1");
    }    


    /* Specified order: */
    if (OK_TO_CHECK_VAR(S, ORDER_ARG)) {
        FcnType ftype = (FcnType)((int_T)mxGetPr(FCN_TYPE_ARG)[0]);

        if (!IS_FLINT_GE(ORDER_ARG, 0)) {
            THROW_ERROR(S, "Order must be a scalar integer >= 0.");
        }

        if ((ftype != fcnA) && (mxGetPr(ORDER_ARG)[0] < 2.0)) {
            THROW_ERROR(S, "Order must be > 1 when returning reflection coefficients.");
        }
    }
#endif
}



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

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

    ssSetSFcnParamNotTunable(S, FCN_TYPE_ARGC);
    ssSetSFcnParamNotTunable(S, ORDER_ARGC);
    ssSetSFcnParamNotTunable(S, INHERIT_ORDER_ARGC);

    if (!ssSetNumInputPorts(S, NUM_INPORTS)) return;
    if (!ssSetInputPortDimensionInfo(S, INPORT, DYNAMIC_DIMENSION)) return;
    ssSetInputPortFrameData(         S, INPORT, FRAME_INHERITED);
    ssSetInputPortComplexSignal(     S, INPORT, COMPLEX_INHERITED);
    ssSetInputPortDirectFeedThrough( S, INPORT, 1);
    ssSetInputPortReusable(          S, INPORT, 1);
    ssSetInputPortRequiredContiguous(S, INPORT, 1);

    /* Outputs: */
    {
        const FcnType ftype = (FcnType)((int_T)mxGetPr(FCN_TYPE_ARG)[0]);
        int_T gain_port = getGPortIdx(S);

        if (ftype == fcnAandK) {
            /* 3 outputs (both polynomial and reflection coefficients,
             *  plus gain) - set the 2nd port properties now:
             */
            if (!ssSetNumOutputPorts(S, NUM_OUTPORTS_MAXIMUM)) return;
            if (!ssSetOutputPortDimensionInfo(S, 1, DYNAMIC_DIMENSION)) return;
            ssSetOutputPortFrameData(         S, 1, FRAME_NO);
            ssSetOutputPortComplexSignal(     S, 1, COMPLEX_INHERITED); 
            ssSetOutputPortReusable(          S, 1, 1);

        } else {
            /* 2 Outputs (either polynomial or reflection coefficients,
             * plus gain port)
             */
            if (!ssSetNumOutputPorts(S, NUM_OUTPORTS_MINIMUM)) return;
        }

        /* Set the first port (same for any mode) */
        if (!ssSetOutputPortDimensionInfo(S, 0, DYNAMIC_DIMENSION)) return;
        ssSetOutputPortFrameData(         S, 0, FRAME_NO);
        ssSetOutputPortComplexSignal(     S, 0, COMPLEX_INHERITED); 
        ssSetOutputPortReusable(          S, 0, 1);

        /* Setup gain port: */
        if (!ssSetOutputPortVectorDimension(S, gain_port, 1)) return;
        ssSetOutputPortFrameData(           S, gain_port, FRAME_NO);
        ssSetOutputPortComplexSignal(       S, gain_port, COMPLEX_NO);
        ssSetOutputPortReusable(            S, gain_port, 1);
    }

    if(!ssSetNumDWork(  S, DYNAMICALLY_SIZED)) return;

    ssSetNumSampleTimes(S, 1);
    ssSetOptions(       S, SS_OPTION_EXCEPTION_FREE_CODE |
                        SS_OPTION_USE_TLC_WITH_ACCELERATOR);
}


static void mdlInitializeSampleTimes(SimStruct *S)
{
    ssSetSampleTime(S, 0, INHERITED_SAMPLE_TIME);
    ssSetOffsetTime(S, 0, FIXED_IN_MINOR_STEP_OFFSET);
}

#ifdef MATLAB_MEX_FILE
static void mdlCheckParametersPostProp(SimStruct *S)
{
    /* Check arguments relying on port widths, etc: */

    const int_T     N            = ssGetInputPortWidth(S, INPORT);
    const boolean_T inheritOrder = (boolean_T)mxGetPr(INHERIT_ORDER_ARG)[0];
    const int_T     order        = (inheritOrder) ? -1 : (int_T)(mxGetPr(ORDER_ARG)[0]);

    /*
     * Check number of requested ACF coefficients.
     * Don't automatically zero-pad the input sequence.
     */
    if (order+1 > N) {  /* Works even when order=-1 (i.e., inherit input width) */
        THROW_ERROR(S, "Order exceeds number of input samples.");
    }
}
#endif

#define MDL_START
static void mdlStart(SimStruct *S)
{
#ifdef MATLAB_MEX_FILE
    mdlCheckParametersPostProp(S);
#endif
}


static void mdlOutputs(SimStruct *S, int_T tid)
{
    const FcnType   ftype = (FcnType)((int_T)mxGetPr(FCN_TYPE_ARG)[0]);
    const boolean_T cplx  = (boolean_T)(ssGetInputPortComplexSignal(S, INPORT) == COMPLEX_YES);
    const int_T     N     = ssGetInputPortWidth(S, INPORT);
    const boolean_T inheritOrder = (boolean_T)mxGetPr(INHERIT_ORDER_ARG)[0];
    const int_T     order        = (inheritOrder) ? -1 : (int_T)(mxGetPr(ORDER_ARG)[0]);
    real_T          E     = 0.0;

    if (cplx) {
        /*
         * Complex inputs:
         */
        const creal_T *u  = ssGetInputPortSignal(S, INPORT);
        creal_T       *ef = (creal_T *)ssGetDWork(S, FERR_IDX);
        creal_T       *eb = (creal_T *)ssGetDWork(S, BERR_IDX);
        int_T          k;

        creal_T *Acoef  = (ftype == fcnK)
                        ? (creal_T *)ssGetDWork(S, ACOEF_IDX)
                        : (creal_T *)ssGetOutputPortSignal(S, getAPort(S));

        creal_T *RCcoef = (ftype == fcnA)
                        ? NULL
                        : (creal_T *)ssGetOutputPortSignal(S, getRCPort(S));

        /* Preset AR coefficients: */
        {
            creal_T *a = Acoef;
            a->re      = 1.0;          /* First AR coefficient is 1 */
            (a++)->im  = 0.0;

            for(k=1; k <= order; k++) {
                a->re     = 0.0;
                (a++)->im = 0.0;       /* All remaining are zero */
            }
        }
        
        /* Copy inputs and compute initial prediction error: */
        for (k=0; k<N; k++) {
            creal_T x = *u++;
            ef[k] = eb[k] = x;
            E += CMAGSQ(x);
        }
        E /= N;
        
        for(k=1; k<=order; k++) {
            int_T   n;
            creal_T KK;

            /* Calculate reflection coefficient: */
            {
                creal_T *efp = ef+k;
                creal_T *ebp = eb+k-1;
                creal_T  num = {0.0, 0.0};
                real_T   den = 0.0;
                
                for (n=k; n <= N-1; n++) {
                    const creal_T v1 = *efp++;
                    const creal_T v2 = *ebp++;
                    den    += CMAGSQ(v1) + CMAGSQ(v2);
                    num.re += CMULT_YCONJ_RE(v1, v2);
                    num.im += CMULT_YCONJ_IM(v1, v2);
                }
                KK.re = -2 * num.re / den;
                KK.im = -2 * num.im / den;
            }

            /* Record reflection coefficient: */
            if (RCcoef != NULL) {
                RCcoef[k-1] = KK;
            }

            /* Update AR polynomial: */
            {
                int_T    i;
                creal_T *a    = Acoef;
                creal_T *anew = (creal_T *)ssGetDWork(S, ANEW_IDX);  /* point to output area + 1*/

                for (i=1; i <= k-1; i++) {
                    creal_T Ka;
                    Ka.re = CMULT_YCONJ_RE(KK, a[k-i]);
                    Ka.im = CMULT_YCONJ_IM(KK, a[k-i]);
                    anew[i].re = a[i].re + Ka.re;
                    anew[i].im = a[i].im + Ka.im;
                }
                anew[k] = KK;

                /* Update polynomial for next iteration: */
                for (i=1; i <= k; i++) {
                    a[i] = anew[i];
                }
            }
            
            /* Update prediction error terms: */
            for (n=N-1; n >= k+1; n--) {
                creal_T t;

                /* ef[j] += K * eb[n-1]; */
                t.re = CMULT_RE(KK, eb[n-1]);
                t.im = CMULT_IM(KK, eb[n-1]);
                ef[n].re += t.re;
                ef[n].im += t.im;

                /* eb[j]  = eb[n-1] + K * ef_old; */
                t.re = CMULT_XCONJ_RE(KK, ef[n-1]);
                t.im = CMULT_XCONJ_IM(KK, ef[n-1]);
                eb[n-1].re = eb[n-2].re + t.re;
                eb[n-1].im = eb[n-2].im + t.im;
            }
            
            /* Update prediction error: */
            E *= (1.0 - CMAGSQ(KK));
        }

    } else {
        /*
         * Real inputs:
         */
        const real_T *u  = ssGetInputPortRealSignal(S, INPORT);
        real_T       *ef = (real_T *)ssGetDWork(S, FERR_IDX);
        real_T       *eb = (real_T *)ssGetDWork(S, BERR_IDX);
        int_T         k;
        
        real_T *Acoef  = (ftype == fcnK)
                       ? (real_T *)ssGetDWork(S, ACOEF_IDX)
                       : ssGetOutputPortRealSignal(S, getAPort(S));

        real_T *RCcoef = (ftype == fcnA)
                       ? NULL
                       : ssGetOutputPortRealSignal(S, getRCPort(S));

        /* Preset AR coefficients: */
        {
            real_T *a = Acoef;
            *a++ = 1.0;             /* First AR coefficient is 1 */

            /* xxx Unnecessary? */
            for(k=1; k <= order; k++) {
                *a++ = 0.0;         /* All remaining are zero */
            }
        }
        
        /* Copy inputs and compute initial prediction error: */
        for (k=0; k<N; k++) {
            real_T x = *u++;
            ef[k] = eb[k] = x;
            E += x*x;
        }
        E /= N;
        
        for(k=1; k<=order; k++) {
            int_T   n;
            real_T KK;

            /* Calculate reflection coefficient: */
            {
                real_T *efp = ef+k;
                real_T *ebp = eb+k-1;
                real_T  num = 0.0;
                real_T  den = 0.0;
                
                for (n=k; n <= N-1; n++) {
                    const real_T v1 = *efp++;
                    const real_T v2 = *ebp++;
                    den    += v1*v1 + v2*v2;
                    num    += v1*v2;
                }
                KK = -2 * num / den;
            }

            /* Record reflection coefficient: */
            if (RCcoef != NULL) {
                RCcoef[k-1] = KK;
            }

            /* Update AR polynomial: */
            {
                int_T   i;
                real_T *a    = Acoef;
                real_T *anew = (real_T *)ssGetDWork(S, ANEW_IDX);  /* point to output area + 1*/

                for (i=1; i <= k-1; i++) {
                    anew[i]= a[i] + KK * a[k-i];
                }
                anew[k] = KK;

                /* Update polynomial for next iteration: */
                for (i=1; i <= k; i++) {
                    a[i] = anew[i];
                }
            }
            
            /* Update prediction error terms: */
            for (n=N-1; n >= k+1; n--) {
                ef[n] += KK * eb[n-1];
                eb[n-1] = eb[n-2] + KK * ef[n-1];
            }
            
            /* Update prediction error: */
            E *= (1.0 - KK*KK);
        }
    }

    /*
     * Save gain term:
     */
    *ssGetOutputPortRealSignal(S, getGPortIdx(S)) = E;
}


static void mdlTerminate(SimStruct *S)
{
}


#if defined(MATLAB_MEX_FILE)
#define MDL_SET_INPUT_PORT_FRAME_DATA
static void mdlSetInputPortFrameData(SimStruct *S,int_T port,Frame_T frameData)
{
    ssSetInputPortFrameData(S, port, frameData);
}


#define MDL_SET_INPUT_PORT_DIMENSION_INFO
static void mdlSetInputPortDimensionInfo(SimStruct *S, 
                                      int_T port,
                                      const DimsInfo_T *dimsInfo)
{
    const FcnType   ftype        = (FcnType)((int_T)mxGetPr(FCN_TYPE_ARG)[0]);
    const boolean_T conn         = (boolean_T)ssGetInputPortConnected(S,0);
    const boolean_T inheritOrder = (boolean_T)mxGetPr(INHERIT_ORDER_ARG)[0];
    const int_T     order        = (int_T)(mxGetPr(ORDER_ARG)[0]);
    int_T A, K;

    if (port != INPORT) {
        THROW_ERROR(S,"Invalid call to input port dimension info propagation.");
    }

    if(!ssSetInputPortDimensionInfo(S, port, dimsInfo)) return;

    ErrorIfInputIsNot1or2D(S, INPORT);
    if((dimsInfo->numDims) == 2) {
        if ( isInputFullMatrix(S, INPORT) ||
                (isInputRowVector(S, INPORT) && isInputFrameDataOn(S, INPORT)) ) {
            THROW_ERROR(S, "Input cannot be a full matrix or a frame-based row vector."
                         "  Multi-channel operation is not currently supported.");
        }
    }
    
    
    if (conn) {
        if ( ((ftype != fcnA) && (dimsInfo->width <= 1))  ||
             ((ftype == fcnA) && (dimsInfo->width < 1)) ) {
             THROW_ERROR(S, "Cannot output reflection coefficients when input is a scalar. "
                               "Set output mode to 'A' instead.");
        }
    }


    if (inheritOrder) {
        A = dimsInfo->width;
        K = MAX(1, dimsInfo->width - 1);
    } else {
        A = order + 1;
        K = order;
    }

    switch (ftype) {
        case fcnAandK:
            if (isOutputDynamicallySized(S, getAPort(S))) {
                if(!ssSetOutputPortVectorDimension(S, getAPort(S), A)) return;
            }

            if (isOutputDynamicallySized(S, getRCPort(S))) {
                if(!ssSetOutputPortVectorDimension(S, getRCPort(S), K)) return;
            }
            break;

        case fcnA:
            if (isOutputDynamicallySized(S, getAPort(S))) {
                if(!ssSetOutputPortVectorDimension(S, getAPort(S), A)) return;
            }
            break;

        case fcnK:
            if (isOutputDynamicallySized(S, getRCPort(S))) {
                if(!ssSetOutputPortVectorDimension(S, getRCPort(S), K)) return;
            }
            break;
    }
}


# define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
static void mdlSetOutputPortDimensionInfo(SimStruct        *S, 
                                          int_T            port,
                                          const DimsInfo_T *dimsInfo)
{
    const FcnType   ftype        = (FcnType)((int_T)mxGetPr(FCN_TYPE_ARG)[0]);
    const boolean_T conn         = (boolean_T)ssGetInputPortConnected(S,0);
    const boolean_T inheritOrder = (boolean_T)mxGetPr(INHERIT_ORDER_ARG)[0];
    const int_T     order        = (int_T)(mxGetPr(ORDER_ARG)[0]);
    int_T A, K;

    if(!ssSetOutputPortDimensionInfo(S, port, dimsInfo)) return;
    ErrorIfOutputIsFullMatrix(S, port);
    ErrorIfOutputIsRowVector(S, port);

    if (port >= 2) {
        /* Should not get a call for gain (3rd) output port, since it is declared (scalar) */
        THROW_ERROR(S,"Invalid call to output port dimension info propagation.");
    }

    if (conn) {
        /* size(A)==1 implies size(INPUT)==1, which does not allow
         * reflection coefficients to be output (they would be empty)
         */
        if ((ftype != fcnA) && (port==0) && (dimsInfo->width == 1)) {
            THROW_ERROR(S,"Cannot output reflection coefficients when input is a scalar. "
                        "Set output mode to 'A' instead.");
        }
    } 

    if (inheritOrder) {
        if (port == getAPort(S)) {
            A = dimsInfo->width;
            K = MAX(1, A-1);
        } else {
            K = dimsInfo->width;
            A = K+1;
        }
    } else {
        A = order+1;
        K = MAX(1, order);
    }

    if (!isInputDynamicallySized(S, INPORT) && (ssGetInputPortWidth(S, INPORT) != A)) {
        THROW_ERROR(S, "Invalid input port width computed during output port dimension info.");
    }

    switch (ftype) {
        case fcnAandK:
            if (port == getAPort(S)) {
                if (isOutputDynamicallySized(S, getRCPort(S))) {
                    if(!ssSetOutputPortVectorDimension(S, getRCPort(S), K)) return;
                }

            } else {
                if (isOutputDynamicallySized(S, getAPort(S))) {
                    if(!ssSetOutputPortVectorDimension(S, getAPort(S), A)) return;
                }
            }
            break;

        case fcnA:
        case fcnK:
            /* Only need to set input port -- already done */
            break;
    }
}




#define MDL_SET_INPUT_PORT_COMPLEX_SIGNAL
static void mdlSetInputPortComplexSignal(SimStruct *S,
                                         int_T      portIdx,
                                         CSignal_T  portComplexSignal)
{
    ssSetInputPortComplexSignal(S, INPORT, portComplexSignal);
    {
        /* NOTE: The last output port index is always the G (gain) port, */
        /*       which is set to COMPLEX_NO in mdlInitializeSizes().     */
        int_T outportIdx;
        for (outportIdx=0; outportIdx < getGPortIdx(S); outportIdx++) {
            ssSetOutputPortComplexSignal(S, outportIdx, portComplexSignal);
        }
    }
}


#define MDL_SET_OUTPUT_PORT_COMPLEX_SIGNAL
static void mdlSetOutputPortComplexSignal(SimStruct *S,
                                          int_T      portIdx, 
                                          CSignal_T  portComplexSignal)
{
    const int_T gPortIndex = getGPortIdx(S);

    if (portIdx < gPortIndex) {
        /* NOTE: The last output port index is always the G (gain) port, */
        /*       which is set to COMPLEX_NO in mdlInitializeSizes().     */
        int_T outportIdx;
        for (outportIdx=0; outportIdx < gPortIndex; outportIdx++) {
            ssSetOutputPortComplexSignal(S, outportIdx, portComplexSignal);
        }
        
        ssSetInputPortComplexSignal(S, INPORT, portComplexSignal);
    }
    else {
        THROW_ERROR(S, "Invalid call to mdlSetOutputPortComplexSignal.");
    }
}


#define MDL_SET_WORK_WIDTHS
static void mdlSetWorkWidths(SimStruct *S)
{
    /* Allocate storage for:
     *     ef: N  (forward error)
     *     eb: N  (backward error)
     *   anew: temp scratchpad for reversed poly coeff's
     *  ACoef: Polynomial coeffs if they are not output directly
     */
    const int_T     N         = ssGetInputPortWidth(S,INPORT);
    const CSignal_T cplx      = ssGetInputPortComplexSignal(S,INPORT);
    const FcnType   ftype     = (FcnType)((int_T)mxGetPr(FCN_TYPE_ARG)[0]);
    int_T           numDWork  = NUM_DWORKS;

    /* If we are outputting A, we don't need extra DWork
     * storage for it (it will be stored directly in the
     * output area).
     */
    const boolean_T needACoef = (boolean_T)(ftype == fcnK);

    if (!needACoef) numDWork--;

    if(!ssSetNumDWork(      S, numDWork)) return;

    ssSetDWorkWidth(        S, FERR_IDX, N);
    ssSetDWorkDataType(     S, FERR_IDX, SS_DOUBLE);
    ssSetDWorkComplexSignal(S, FERR_IDX, cplx);

    ssSetDWorkWidth(        S, BERR_IDX, N);
    ssSetDWorkDataType(     S, BERR_IDX, SS_DOUBLE);
    ssSetDWorkComplexSignal(S, BERR_IDX, cplx);

    ssSetDWorkWidth(        S, ANEW_IDX, N);
    ssSetDWorkDataType(     S, ANEW_IDX, SS_DOUBLE);
    ssSetDWorkComplexSignal(S, ANEW_IDX, cplx);

    if (needACoef) {
        ssSetDWorkWidth(        S, ACOEF_IDX, N);
        ssSetDWorkDataType(     S, ACOEF_IDX, SS_DOUBLE);
        ssSetDWorkComplexSignal(S, ACOEF_IDX, cplx);
    }
}

#endif /* MATLAB_MEX_FILE */


#include "dsp_trailer.c"

/* [EOF] sdspburg2.c */