/*
 * File: sdsplu2.c
 *
 * Abstract:
 *      S-function for LU decomposition
 *
 * Copyright 1995-2000 The MathWorks, Inc.
 * $Revision: 1.12 $
 * $Date: 2000/06/11 23:24:14 $
 */

#define S_FUNCTION_NAME  sdsplu2
#define S_FUNCTION_LEVEL 2

#include "dsp_sim.h"

enum {INPORT_A=0, NUM_INPORTS};
enum {OUTPORT_LU=0, OUTPORT_P, NUM_OUTPORTS};

enum {NUM_PARAMS=0};

static void lu_cplx(
    creal_T    *A,
    const int_T n,
    real_T     *piv
)
{
    int_T k;
    
    /* initialize row-pivot indices: */
    for (k = 0; k < n; k++) {
        piv[k] = (real_T)(k+1);
    }
    
    /* Loop over each column: */
    for (k = 0; k < n; k++) {
        const int_T kn = k*n;
        int_T	p = k;
        
        /*
         * Scan the lower triangular part of this column only
         * Record row of largest value
         */
        {
            int_T  i;
            real_T Amax = CQABS(A[p+kn]);	/* approx mag-squared value */
            
            for (i = k+1; i < n; i++) {
                real_T q = CMAGSQ(A[i+kn]);
                if (q > Amax) {p = i; Amax = q;}
            }
        }
            
        /* swap rows if required */
        if (p != k) {
            int_T j;
            for (j = 0; j < n; j++) {
                creal_T c;
                const int_T pjn = p+j*n;
                const int_T kjn = k+j*n;
                
                c      = A[pjn];
                A[pjn] = A[kjn];
                A[kjn] = c;
            }
                
            /* Swap pivot row indices */
            {
                real_T t = piv[p]; piv[p] = piv[k]; piv[k] = t;
            }
        }
            
        /* column reduction */
        {
            creal_T Adiag;
            int_T i, j;
        
            Adiag = A[k+kn];
        
            if (!((Adiag.re == 0.0) && (Adiag.im == 0.0))) {	/* non-zero diagonal entry */
                /*
                 * divide lower triangular part of column by max
                 * First, form reciprocal of Adiag:
                 *	    recip = conj(Adiag)/(|Adiag|^2)
                 */
                 CRECIP(Adiag, Adiag);
                        
                /* Multiply: A[i+kn] *= Adiag: */
                for (i = k+1; i < n; i++) {
                    real_T t   = CMULT_RE(A[i+kn], Adiag);
                    A[i+kn].im = CMULT_IM(A[i+kn], Adiag);
                    A[i+kn].re = t;
                }
                        
                /* subtract multiple of column from remaining columns */
                for (j = k+1; j < n; j++) {
                    int_T jn = j*n;
                    for (i = k+1; i < n; i++) {
                        /* Multiply: c = A[i+kn] * A[k+jn]: */
                        creal_T c;
                        c.re = CMULT_RE(A[i+kn], A[k+jn]);
                        c.im = CMULT_IM(A[i+kn], A[k+jn]);
                        
                        /* Subtract A[i+jn] -= A[i+kn]*A[k+jn]: */
                        A[i+jn].re -= c.re;
                        A[i+jn].im -= c.im;
                    }
                }
            }
        }
    }
}

static void lu_real(
    real_T     *A,
    const int_T n,
    real_T     *piv
)
{
    int_T k;
    
    /* initialize row-pivot indices: */
    for (k = 0; k < n; k++) {
        piv[k] = (real_T)(k+1);
    }
    
    /* Loop over each column: */
    for (k = 0; k < n; k++) {
        const int_T kn = k*n;
        int_T p = k;
        
        /* Scan the lower triangular part of this column only
         * Record row of largest value
         */
        {
            int_T i;
            real_T Amax = fabs(A[p+kn]); /* assume diag is max */
            for (i = k+1; i < n; i++) {
                real_T q = fabs(A[i+kn]);
                if (q > Amax) {p = i; Amax = q;}
            }
        }
            
        /* swap rows if required */
        if (p != k) {
            int_T j;
            real_T t;
            for (j = 0; j < n; j++) {
                const int_T jn = j*n;
                t = A[p+jn]; A[p+jn] = A[k+jn]; A[k+jn] = t;
            }
            /* swap pivot row indices */
            t = piv[p]; piv[p] = piv[k]; piv[k] = t;
        }
            
        /* column reduction */
        {
            real_T Adiag = A[k+kn];
            int_T i,j;
            if (Adiag != 0.0) {  /* non-zero diagonal entry */
                    
                /* divide lower triangular part of column by max */
                Adiag = 1.0/Adiag;
                for (i = k+1; i < n; i++) {
                    A[i+kn] *= Adiag;
                }
                    
                /* subtract multiple of column from remaining columns */
                for (j = k+1; j < n; j++) {
                    int_T jn = j*n;
                    for (i = k+1; i < n; i++) {
                        A[i+jn] -= A[i+kn]*A[k+jn]; 
                    }
                }
            }
        }
    }
}


static void mdlInitializeSizes(SimStruct *S)
{
    ssSetNumSFcnParams(S, NUM_PARAMS);
    
#if defined(MATLAB_MEX_FILE)
    if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) return;
#endif
        
    if (!ssSetNumInputPorts(S, 1)) return;
    /* Can use the following when set methods are implemented:
     *
     *    if (!ssSetInputPortMatrixDimensions(S, INPORT_A, DYNAMICALLY_SIZED, DYNAMICALLY_SIZED)) return;
     *
     * The difference is seen with an unoriented input vector. Instead of getting
     * the error "Input must be square matrix", you'll get "can't set default method"
     */
    if (!ssSetInputPortDimensionInfo(S, INPORT_A, DYNAMIC_DIMENSION)) return;
    ssSetInputPortFrameData(         S, INPORT_A, FRAME_INHERITED);
    ssSetInputPortDirectFeedThrough( S, INPORT_A, 1);
    ssSetInputPortComplexSignal(     S, INPORT_A, COMPLEX_INHERITED);
    ssSetInputPortReusable(          S, INPORT_A, 1);
    ssSetInputPortOverWritable(      S, INPORT_A, 1);
    ssSetInputPortDataType(          S, INPORT_A, SS_DOUBLE);

    if (!ssSetNumOutputPorts(S,2)) return;
    if (!ssSetOutputPortDimensionInfo(S, OUTPORT_LU, DYNAMIC_DIMENSION)) return;
    ssSetOutputPortFrameData(         S, OUTPORT_LU, FRAME_NO);
    ssSetOutputPortComplexSignal(     S, OUTPORT_LU, COMPLEX_INHERITED);
    ssSetOutputPortReusable(          S, OUTPORT_LU, 1);
    ssSetOutputPortDataType(          S, OUTPORT_LU, SS_DOUBLE);

    if (!ssSetOutputPortDimensionInfo(S, OUTPORT_P, DYNAMIC_DIMENSION)) return;
    ssSetOutputPortFrameData(         S, OUTPORT_P, FRAME_NO);
    ssSetOutputPortComplexSignal(     S, OUTPORT_P, COMPLEX_NO);
    ssSetOutputPortReusable(          S, OUTPORT_P, 1);
    ssSetOutputPortDataType(          S, OUTPORT_P, SS_DOUBLE);
        
    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, 0.0);
}


static void mdlOutputs(SimStruct *S, int_T tid)
{
    boolean_T cA = (boolean_T)(ssGetInputPortComplexSignal(S,INPORT_A) == COMPLEX_YES);

    /* Copy input elements to output storage area: */
    {
        const boolean_T need_copy = (boolean_T)(ssGetInputPortBufferDstPort(S, INPORT_A) != OUTPORT_LU);
        if (need_copy) {
            int_T i = ssGetInputPortWidth(S, INPORT_A);

            if (cA) {
                InputPtrsType uPtrs = ssGetInputPortSignalPtrs(S, INPORT_A);
                creal_T      *y     = (creal_T *)ssGetOutputPortSignal(S, OUTPORT_LU);
                while(i-- > 0) {
                    *y++ = *((creal_T *)(*uPtrs++));
                }
            } else {
                InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S, INPORT_A);
                real_T           *y     = ssGetOutputPortRealSignal(S, OUTPORT_LU);
                while(i-- > 0) {
                    *y++ = **uPtrs++;
                }
            }
        }
    }
    
    /* Compute LU decomposition: */
    {
        int_T   N = ssGetOutputPortWidth(     S, OUTPORT_P );
        real_T *p = ssGetOutputPortRealSignal(S, OUTPORT_P );
        void   *u = ssGetOutputPortSignal(    S, OUTPORT_LU);
        if (cA) {
            lu_cplx((creal_T *)u, N, p);
        } else {
            lu_real((real_T  *)u, N, p);
        }
    }
}


static void mdlTerminate(SimStruct *S)
{
}



/* Dimension width prop for QR Factorization: 
 * PA = LU
 *
 * A is N x N
 * LU is N x N
 * P has N elements 
 */
#if defined(MATLAB_MEX_FILE)
#define MDL_SET_INPUT_PORT_DIMENSION_INFO
static void mdlSetInputPortDimensionInfo(SimStruct *S, 
                                      int_T port,
                                      const DimsInfo_T *dimsInfo)
{
    if(!ssSetInputPortDimensionInfo(S, port, dimsInfo)) return;

    ErrorIfInputIsNotSquareMatrix(S, port);

    /* Set output ports */
    if (isOutputDynamicallySized(S, OUTPORT_LU)) {
        if (isInputOriented(S, port)) {
            if(!ssSetOutputPortDimensionInfo(S, OUTPORT_LU, dimsInfo)) return;    
        } else {
            if(!ssSetOutputPortMatrixDimensions(S, OUTPORT_LU, dimsInfo->dims[0], dimsInfo->dims[0])) return;
        }
    }

    if (isOutputDynamicallySized(S, OUTPORT_P)) {
        if(!ssSetOutputPortVectorDimension(S, OUTPORT_P, dimsInfo->dims[0])) return;
    }
}


# define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
static void mdlSetOutputPortDimensionInfo(SimStruct        *S, 
                                          int_T            port,
                                          const DimsInfo_T *dimsInfo)
{
    if(!ssSetOutputPortDimensionInfo(S, port, dimsInfo)) return;

    if (port == OUTPORT_LU) {

        ErrorIfOutputIsNotSquareMatrix(S, port);

        /* Set A if LU port is not a scalar */
        if (!isOutputScalar(S, port) && (isInputDynamicallySized(S,INPORT_A))) {
            if(!ssSetInputPortDimensionInfo(S, INPORT_A, dimsInfo)) return;
        }

        if (isOutputDynamicallySized(S,OUTPORT_P)) {
            if(!ssSetOutputPortVectorDimension(S, OUTPORT_P, dimsInfo->dims[0])) return;
        }

    } else {

        /* Port P is always unoriented. */
        ErrorIfOutputIsOriented(S, port);

        /* Set other ports */
        if (!isOutputScalar(S, port) && isInputDynamicallySized(S, INPORT_A)) {
            if(!ssSetInputPortMatrixDimensions(S, INPORT_A, dimsInfo->dims[0], dimsInfo->dims[0])) return;
        }

        if (isOutputDynamicallySized(S, OUTPORT_LU)) {
            if(!ssSetOutputPortMatrixDimensions(S, OUTPORT_LU, dimsInfo->dims[0], dimsInfo->dims[0])) return;
        }
    }
}


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

#include "dsp_cplxhs11.c"

#include "dsp_trailer.c"

/* [EOF] sdsplu2.c */