/*
 * File: sdspchol2.c
 *
 * Abstract:
 *      S-function for Cholesky decomposition
 *
 * Copyright 1995-2000 The MathWorks, Inc. 
 * $Revision: 1.16 $
 * $Date: 2000/07/21 18:32:38 $
 */

/*
 * The algorithm used is "Bordered Form Cholesky Factorization".
 * See Exercise 1.5.13 in "Fundamentals of Matrix Computations", by
 *  David S. Watkins, '91, Wiley and Sons.
 *
 * The basic idea of the algorithm is to square root successively
 *  larger prinicipal sub-matrices, starting with the 1x1 in the
 *  upper left corner. This cholesky factorization is embedded in
 *  the larger factorization and grows from 1x1, to 2x2, etc. At
 *  each stage a triangular linear system must be solved for the
 *  update.
 */

#define S_FUNCTION_NAME  sdspchol2
#define S_FUNCTION_LEVEL 2

#include "dsp_sim.h"

enum {INPORT_A=0, NUM_INPORTS};
enum {OUTPORT_L=0, NUM_OUTPORTS};

enum {ERR_ARGC=0, NUM_PARAMS};
#define ERR_ARG(S) (ssGetSFcnParam(S,ERR_ARGC))

typedef enum {ERR_IGNORE=1, ERR_WARNING, ERR_ERROR} ErrMode;

#define MDL_CHECK_PARAMETERS
static void mdlCheckParameters(SimStruct *S) {
#ifdef MATLAB_MEX_FILE
    if (!IS_FLINT_IN_RANGE(ERR_ARG(S),1,3)) {
        THROW_ERROR(S, "Error mode must be 1=IGNORE, 2=WARNING, or 3=ERROR");
    }
#endif
}



/*
 *  Decompose A into L*L'
 *  Put L on lower triangle, and L' on upper.
 *
 *  This code does not check for realness on the main diagonal,
 *  but to prevent any imaginary part from affecting the results,
 *  we set it to zero. The code could take this into account, but
 *  it does not.
 *
 *  Only the real part of the diagonal and the upper triangle of the
 *  input matrix is used.
 */
static boolean_T chol_cplx(
    creal_T     *A,
    const int_T  n
)
{
    int_T j;
    creal_T *A_jn = A;

    for (j=0; j<n; j++) {
        real_T   s = 0.0;
        creal_T *A_kn = A;
        int_T    k;

        A_jn[j].im = 0.0; /* Remove imaginary part on diagonal since */
		                  /* this matrix is supposed to be Hermitian. */

        for (k=0; k<j; k++) {
            creal_T t = {0.0, 0.0};
            {
                /* Inner product */
                creal_T *x1 = A_kn;
                creal_T *x2 = A_jn;
                int_T   kk = k;
                while(kk-- > 0) {
                    t.re += CMULT_XCONJ_RE(*x1, *x2);
                    t.im += CMULT_XCONJ_IM(*x1, *x2);
                    x1++; x2++;
                }
            }
            t.re = A_jn[k].re - t.re;
            t.im = A_jn[k].im - t.im;
			t.re /= A_kn[k].re;  /* Diagonal elements A_kn[k] are real */
			t.im /= A_kn[k].re;
            A_jn[k] = t;

            /* Copy transpose of upper triangle (Hermitian) to lower triangle: */
            A_kn[j].re =  t.re;
            A_kn[j].im = -t.im;

            s += CMAGSQ(t);
            A_kn += n;
        }
        s = A_jn[j].re - s;

        /* Check for positive definiteness */
        if (s <= 0.0) {
            return((boolean_T)1);  /* Return with error flag set */
        }

        A_jn[j].re = sqrt(s);
        A_jn += n;
    }
    return((boolean_T)0);
}


/*
 *  Only the upper triangle of the input matrix is used.
 */
static boolean_T chol_real(
    real_T     *A,
    const int_T n
)
{
    boolean_T err = (boolean_T)0;
    int_T   j;
    real_T *A_jn = A;

    for (j=0; j<n; j++) {
        real_T s = 0.0;
        int_T   k;
        real_T *A_kn = A;

        for (k=0; k<j; k++) {
            real_T t = 0.0;
            {
                /* Inner product */
                real_T *x1 = A_kn;
                real_T *x2 = A_jn;
                int_T   kk = k;
                while(kk-- > 0) {
                    t += *x1++ * *x2++;
                }
            }
            t = (A_jn[k] - t) / A_kn[k];
            A_jn[k] = t;
            A_kn[j] = t;  /* Copy upper triangle to lower */
            s += t*t;
            A_kn += n;
        }
        s = A_jn[j] - s;
        if (s <= 0.0) {
            return((boolean_T)(1));
        }
        A_jn[j] = sqrt(s);
        A_jn += n;
    }
    return((boolean_T)0);
}


static void mdlInitializeSizes(SimStruct *S)
{
    ssSetNumSFcnParams(S,  NUM_PARAMS);
    
#if defined(MATLAB_MEX_FILE)
    if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) return;
    mdlCheckParameters(S);
    if (ssGetErrorStatus(S) != NULL) return;
#endif

    if (!ssSetNumInputPorts(S, NUM_INPORTS)) return;
    if (!ssSetInputPortDimensionInfo(S, INPORT_A, DYNAMIC_DIMENSION)) return;
    ssSetInputPortFrameData(         S, INPORT_A, FRAME_INHERITED);
    ssSetInputPortComplexSignal(     S, INPORT_A, COMPLEX_INHERITED);
    ssSetInputPortDataType(          S, INPORT_A, SS_DOUBLE);
    ssSetInputPortDirectFeedThrough( S, INPORT_A, 1);
    ssSetInputPortReusable(          S, INPORT_A, 1);
    ssSetInputPortOverWritable(      S, INPORT_A, 1);

    if (!ssSetNumOutputPorts(S, NUM_OUTPORTS)) return;
    if (!ssSetOutputPortDimensionInfo(S, OUTPORT_L, DYNAMIC_DIMENSION)) return;
    ssSetOutputPortFrameData(         S, OUTPORT_L, FRAME_NO);
    ssSetOutputPortComplexSignal(     S, OUTPORT_L, COMPLEX_INHERITED);
    ssSetOutputPortDataType(          S, OUTPORT_L, SS_DOUBLE);
    ssSetOutputPortReusable(          S, OUTPORT_L, 1);
        
    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);
    const ErrMode err_mode = (ErrMode)((int_T)(mxGetPr(ERR_ARG(S))[0]));

    /* Copy input elements to output storage area: */
    {
        const boolean_T need_copy = (boolean_T)(ssGetInputPortBufferDstPort(S, INPORT_A) != OUTPORT_L);
        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_L);
                while(i-- > 0) {
                    *y++ = *((creal_T *)(*uPtrs++));
                }
            } else {
                InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S, INPORT_A);
                real_T           *y     = ssGetOutputPortRealSignal(S, OUTPORT_L);
                while(i-- > 0) {
                    *y++ = **uPtrs++;
                }
            }
        }
    }
    
    /* Compute Cholesky decomposition: */
    {
        const int_T *dims  = ssGetInputPortDimensions(S,INPORT_A);
        const int_T  N     = dims[0];
        void        *pA    = ssGetOutputPortSignal(S, OUTPORT_L);
        boolean_T    err;

        if (cA) {
            err = chol_cplx((creal_T *)pA, N);
        } else {
            err = chol_real((real_T  *)pA, N);
        }

#ifdef MATLAB_MEX_FILE
        if (err) {
            /* Input matrix not positive definite */
            static char header[] = "Input matrix to block '%s' is not positive definite.";
            static char msg_err[] = "Input matrix is not positive definite.";
            static char msg_warn[256];
			int_T i;
            if (err_mode == ERR_WARNING) {
				if (strlen(header) - 2 + strlen(ssGetPath(S)) > 255) {
					strcpy(msg_warn,"Input matrix to Cholesky Factorization block is not positive definite.");
                } else {
					sprintf(msg_warn,header, ssGetPath(S));
					for (i=strlen(msg_warn); i-- > 0; ) {
						if (msg_warn[i] == '\n') msg_warn[i] = ' ';
					}
					mexWarnMsgTxt(msg_warn);
				}
            } else if (err_mode == ERR_ERROR) {
                ssSetErrorStatus(S, msg_err);
            }
        }
#endif
    }
}


static void mdlTerminate(SimStruct *S)
{
}


#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 port */
    if (isOutputDynamicallySized(S, OUTPORT_L)) {
        if(!ssSetOutputPortMatrixDimensions(S, OUTPORT_L, dimsInfo->dims[0], 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;
    
    ErrorIfOutputIsNotSquareMatrix(S, port);

    /* Set input port */
    if (!isOutputScalar(S, port) && isInputDynamicallySized(S, INPORT_A)) {
        if (!ssSetInputPortDimensionInfo(S, INPORT_A, dimsInfo)) 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] sdspchol2.c */