/*
 * SDSPSVD  Economy-sized Singular Value Decomposition
 *
 * Copyright 1995-2000 The MathWorks, Inc.
 * $Revision: 1.7 $ $Date: 2000/08/18 14:08:10 $
 */

#define S_FUNCTION_NAME  sdspsvd
#define S_FUNCTION_LEVEL 2

#include "dsp_sim.h"
#include "dspsvd_rt.h"

enum {X_IDX=0, E_IDX, WORK_IDX, S_IDX, NUM_DWORKS};
enum {INPORT_A=0, NUM_INPORTS};
enum {OUTPORT_U=0, OUTPORT_S, OUTPORT_V, NUM_OUTPORTS};
enum {SINGVECS_ARGC=0,NUM_PARAMS};

#define SINGVECS_ARG(S)  (ssGetSFcnParam(S, SINGVECS_ARGC))
#define WANTV(S)         (mxGetPr(SINGVECS_ARG(S))[0] == 1.0)


#ifdef MATLAB_MEX_FILE
#define MDL_CHECK_PARAMETERS
static void mdlCheckParameters(SimStruct *S) 
{
    /* Singular Vector Mode: */
    if (!IS_FLINT_IN_RANGE(SINGVECS_ARG(S), 0, 1)) {
        THROW_ERROR(S, "Singular vectors mode can be only 1 (compute singular vectors) or 0 (not).");
    }
}
#endif


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

    /* Checkbox is not tunable because it changes the number of output ports. */
    ssSetSFcnParamNotTunable(S, SINGVECS_ARGC);

    if (!ssSetNumInputPorts(S, NUM_INPORTS)) return;
    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, 0);
    ssSetInputPortDataType(          S, INPORT_A, SS_DOUBLE);
    ssSetInputPortRequiredContiguous(S, INPORT_A, 1);    

    if (WANTV(S)) {
	if (!ssSetNumOutputPorts(S, NUM_OUTPORTS)) return;

	if (!ssSetOutputPortDimensionInfo(S, OUTPORT_U, DYNAMIC_DIMENSION)) return;
	ssSetOutputPortFrameData(         S, OUTPORT_U, FRAME_NO);
	ssSetOutputPortComplexSignal(     S, OUTPORT_U, COMPLEX_INHERITED);
	ssSetOutputPortReusable(          S, OUTPORT_U, 0);
	ssSetOutputPortDataType(          S, OUTPORT_U, SS_DOUBLE);
    
	if (!ssSetOutputPortDimensionInfo(S, OUTPORT_S, DYNAMIC_DIMENSION)) return;
	ssSetOutputPortFrameData(         S, OUTPORT_S, FRAME_NO);
	ssSetOutputPortComplexSignal(     S, OUTPORT_S, COMPLEX_NO);
	ssSetOutputPortReusable(          S, OUTPORT_S, 0);
	ssSetOutputPortDataType(          S, OUTPORT_S, SS_DOUBLE);
    
	if (!ssSetOutputPortDimensionInfo(S, OUTPORT_V, DYNAMIC_DIMENSION)) return;
	ssSetOutputPortFrameData(         S, OUTPORT_V, FRAME_NO);
	ssSetOutputPortComplexSignal(     S, OUTPORT_V, COMPLEX_INHERITED);
	ssSetOutputPortReusable(          S, OUTPORT_V, 0);
	ssSetOutputPortDataType(          S, OUTPORT_V, SS_DOUBLE);

    } else {
	if (!ssSetNumOutputPorts(S, 1)) return;
	
        /* There is only one output (S) but we need to use OUTPORT_U
         * as the name because it is defined as outport 0
         */
	if (!ssSetOutputPortDimensionInfo(S, OUTPORT_U, DYNAMIC_DIMENSION)) return;
	ssSetOutputPortFrameData(         S, OUTPORT_U, FRAME_NO);
	ssSetOutputPortComplexSignal(     S, OUTPORT_U, COMPLEX_NO);
	ssSetOutputPortReusable(          S, OUTPORT_U, 0);
	ssSetOutputPortDataType(          S, OUTPORT_U, SS_DOUBLE);
    }
    
    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, 0.0);
}


#ifdef MATLAB_MEX_FILE
#define MDL_SET_WORK_WIDTHS
static void mdlSetWorkWidths(SimStruct *S)
{
    const CSignal_T Cplx = ssGetInputPortComplexSignal(S, INPORT_A);
    const int_T  numDimsA = ssGetInputPortNumDimensions(S, INPORT_A);
    const int_T *dims     = ssGetInputPortDimensions(S,INPORT_A);
    const int_T  M        = dims[0];
    const int_T  N        = (numDimsA ==2) ? dims[1] : 1;
    const int_T  MN       = M * N;
    int_T        Mnew=M, Nnew=N;

    /*  If M<N, we will be transposing the input matrix, so
     *  interchange M and N in that case. The routine that computes
     *  the SVD (dspsvd_rt.c) does not provide an economy sized SVD
     *  unless the input is tall and skinny, so we make sure that
     *  that is what it gets and adjust everything accordingly.
     */

    if (M < N) {
        Mnew = N;
        Nnew = M;
    }
    
    if(!ssSetNumDWork(      S, NUM_DWORKS)) return;
    
    ssSetDWorkWidth(        S, X_IDX, MN);
    ssSetDWorkDataType(     S, X_IDX, SS_DOUBLE);
    ssSetDWorkComplexSignal(S, X_IDX, Cplx);
    
    ssSetDWorkWidth(        S, E_IDX, Nnew);
    ssSetDWorkDataType(     S, E_IDX, SS_DOUBLE);
    ssSetDWorkComplexSignal(S, E_IDX, Cplx);
    
    ssSetDWorkWidth(        S, WORK_IDX, Mnew);
    ssSetDWorkDataType(     S, WORK_IDX, SS_DOUBLE);
    ssSetDWorkComplexSignal(S, WORK_IDX, Cplx);
    
    ssSetDWorkWidth(        S, S_IDX, MIN(Mnew+1,Nnew));
    ssSetDWorkDataType(     S, S_IDX, SS_DOUBLE);
    ssSetDWorkComplexSignal(S, S_IDX, Cplx);
 
}
#endif

/*
 * Compute the economy-sized svd of m-by-n input A:
 * MATLAB command: [U,S,V] = svd(A,0); and S = svd(A,0);
 * U is m-by-min(m,n) and V is n-by-n, when requested.
 * Unlike MATLAB, S is always a vector of length min(m,n).
 * U and V have the complexity of A; S is always real.
 * work and e are length m and n and they have A's complexity.
 * A working copy of S, complex if A is, needs to be allocated
 * and at the end of the algorithm, the real parts of the
 * first min(m,n) entries are copied to the output vector S.
 *
 * NOTE: THE BEHAVIOR OF THE BLOCK HAS BEEN CHANGED FROM THE
 * ABOVE BEHAVIOR AS FOLLOWS:
 *
 * ASSUME THAT A IS MxN and P=min(M,N). IN THE REVISED CODE,
 * U IS MxP, S IS PxP, and U is NxP. THIS BEHAVIOR IS ATTAINED
 * BY TRANSPOSING A IF M<N, AND INTERCHANGING U AND V AT THE
 * OTUPUT. THE SAME COMPUTATIONAL ENGINE IS USED AS BEFORE,
 * WHICH IS BASED ON LINPACK. R12 MATLAB IS BASED ON LAPACK,
 * WHICH DOES NOT WORK IDENTICALLY TO LINPACK FOR SVD.
 */

static void mdlOutputs(SimStruct *S, int_T tid)
{
    boolean_T    cA	  = (boolean_T)(ssGetInputPortComplexSignal(S,INPORT_A) == COMPLEX_YES);
    const int_T  numDimsA = ssGetInputPortNumDimensions(S, INPORT_A);
    const int_T *dimsA    = ssGetInputPortDimensions(S, INPORT_A); 
    int_T        M        = dimsA[0];
    int_T        N        = (numDimsA == 2) ? dimsA[1] : 1;
    int_T        MN       = M * N;
    int_T        Mnew=M, Nnew=N;

    if (M < N) {
        Mnew = N;
        Nnew = M;
    }

    { /* Copy input A to work array X, which will be diagonalized */
	if (cA) {
            creal_T *pA = (creal_T *)(ssGetInputPortSignal(S, INPORT_A));
            creal_T *pX = (creal_T *)(ssGetDWork(S, X_IDX));

	    if (M >= N) { /* Matrix is tall and skinny, copy as is. */
                while(MN-- > 0) {
                    *pX++ = *pA++;
                }
            }
            else { /* Matrix is short and fat, so transpose it. */
                int i,j;
	        for (i=0; i<M; i++) {
		    for (j=0; j<N; j++) {
		        *pX++ = *(pA + i + j*M);
		    }
		}
	    }
        }
        else {
            real_T *pA = (real_T *)(ssGetInputPortRealSignal(S, INPORT_A));
            real_T *pX = (real_T *)(ssGetDWork(S, X_IDX));

            if (M >= N) { /* Matrix is tall and skinny, copy as is. */
                while(MN-- > 0) {
                    *pX++ = *pA++;
		}
            }
            else { /* Matrix is short and fat, so transpose it. */
                int i,j;
	        for (i=0; i<M; i++) {
		    for (j=0; j<N; j++) {
		        *pX++ = *(pA + i + j*M); 
		    }
		}
	    }
	}
    }
    {
	/* Compute the svd */
        void  *pU = (void *)0;
        void  *pS    = ssGetDWork(S, S_IDX);
        void  *pV = (void *)0;
        void  *pe    = ssGetDWork(S, E_IDX);
        void  *pwork = ssGetDWork(S, WORK_IDX);
	int_T  info;

        if (WANTV(S)) {
            if (M >=N) { /* Input is tall and skinny, don't interchange U and V */
                pU    = ssGetOutputPortSignal(S, OUTPORT_U);
                pV    = ssGetOutputPortSignal(S, OUTPORT_V);
            }
            else { /* Input is short and fat, so interchange U and V */
                pU    = ssGetOutputPortSignal(S, OUTPORT_V);
                pV    = ssGetOutputPortSignal(S, OUTPORT_U);
            }
	}
        if (cA) {
	    creal_T *pX = (creal_T *)(ssGetDWork(S, X_IDX));
	    info = sdspsvd_cplx(pX, Mnew, Nnew, (creal_T *)pS, (creal_T *)pe,
		(creal_T *)pwork, (creal_T *)pU, (creal_T *)pV, WANTV(S));
        } else {
	    real_T *pX = (real_T *)(ssGetDWork(S, X_IDX));
	    info = sdspsvd_real(pX, Mnew, Nnew, (real_T *)pS, (real_T *)pe,
		(real_T *)pwork, (real_T *)pU, (real_T *)pV, WANTV(S));
        }
	if (info) {
	    THROW_ERROR(S,"SVD did not converge");
	}
    }

    { /* Copy the real part of the first min(m,n) entries of S_IDX to
       * OUTPORT_S for the three output case or OUTPORT_U for one output case.
       */
        real_T *pOS = (real_T *)(ssGetOutputPortSignal(S, (WANTV(S) ? OUTPORT_S : OUTPORT_U)));
	int_T   P   = MIN(M,N);

        if (cA) {
	    creal_T *pS = (creal_T *)(ssGetDWork(S, S_IDX));
	    while(P-- > 0) {
		*pOS++ = pS->re; pS++;
	    }
	} else {
	    real_T *pS = (real_T *)(ssGetDWork(S, S_IDX));
	    while(P-- > 0) {
		*pOS++ = *pS++;
	    }
	}
    }
}


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)
{
    if(!ssSetInputPortDimensionInfo(S, port, dimsInfo)) return;
    ErrorIfInputIsNot1or2D(S, port);
    {
        /* Unoriented inputs are interpreted as oriented column vectors. 
         * Set output ports now that M and N are known: */
        const int_T numDims = dimsInfo->numDims;
        const int_T M       = dimsInfo->dims[0];
        const int_T N       = (numDims == 2) ? dimsInfo->dims[1] : 1;
        const int_T P       = MIN(M,N);

	if (WANTV(S)) {
            if (isOutputDynamicallySized(S, OUTPORT_S)) {
                if (!ssSetOutputPortVectorDimension(S, OUTPORT_S, P)) return;
            }
            else {
	      /* Do error checking for S */
	      if (ssGetOutputPortWidth(S,OUTPORT_S) != P)
                THROW_ERROR(S,"Width of S output port must be equal to min(M,N), where M is the number of input rows and N is the number of input columns.\n");
            }

            if (isOutputDynamicallySized(S, OUTPORT_U)) {
                if (!ssSetOutputPortMatrixDimensions(S, OUTPORT_U, M, P)) return;
            }
	    else {
                /* Do error checking for U */
	        const int_T *U_dims = ssGetOutputPortDimensions(S, OUTPORT_U);
                if (!isOutputScalar(S, OUTPORT_U)) { /* U is not a scalar */
                    ErrorIfOutputIsNot2D(S, OUTPORT_U);
                    if (U_dims[0] != M)
                        THROW_ERROR(S,"U and A must have the same number of rows.\n");
                    if (U_dims[1] != P)
                        THROW_ERROR(S,"The number of columns in U must be min(M,N), where M is the number of input rows and N is the number of input columns.\n");
		}
                else { /* U is a scalar */
		    if (M != 1) THROW_ERROR(S,"For U to be a scalar, A must be a row vector.\n");
                }
	    }

            if (isOutputDynamicallySized(S, OUTPORT_V)) {
                if (!ssSetOutputPortMatrixDimensions(S, OUTPORT_V, N, P)) return;
            }
	    else {
                /* Do error checking for V */
	        const int_T *V_dims = ssGetOutputPortDimensions(S, OUTPORT_V);
                if (!isOutputScalar(S, OUTPORT_V)) { /* V is not a scalar */
                    ErrorIfOutputIsNot2D(S, OUTPORT_V);
                    if (V_dims[0] != N)
                        THROW_ERROR(S,"The number of rows in V must match the number of columns in A.\n");
                    if (V_dims[1] != P)
                        THROW_ERROR(S,"The number of columns in V must be min(M,N), where M is the number of input rows and N is the number of input columns.\n");
                }
                else { /* V is a scalar */
		    if (N != 1) THROW_ERROR(S,"For V to be a scalar, A must be a column vector.\n");
                }
	    }
        }
        else {
            /* Port is hard coded to 0 since output corresponds to S,
               but we use port 0 (which was OUTPORT_U) in this case. */
            if (isOutputDynamicallySized(S, 0)) {
                if (!ssSetOutputPortVectorDimension(S, 0, P)) return;
            }
            else {
                if (ssGetOutputPortWidth(S, 0) != P) {
                  THROW_ERROR(S,"S must have min(M,N) elements, where M is the number of input rows and N is the number of input columns.\n");
                }
            }
        }
    }
}


#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;

    /* Output error checking:
     *  If only one output port - cannot backprop dims.
     *
     *  If 3 output ports (U, S, V) - we will try
     *    Port S may be unoriented, 2-D row vector, or 2-D column vector.
     *    Ports U and V must be 2-D.
     */
    if (WANTV(S)) {
        if  (port != OUTPORT_S) {
            /* U or V output port */
            if  (!isOutputScalar(S, port)) ErrorIfOutputIsNot2D(S, port);
        }
        else {
            /* S output port */
            ErrorIfOutputIsNotVector(S, port);
        }


        /* Are U and V both already set? If so, we can set A & S. */
        if  ((!isOutputDynamicallySized(S, OUTPORT_U)) && (!isOutputDynamicallySized(S, OUTPORT_V))) {
            const int_T *dims_U  = ssGetOutputPortDimensions(S, OUTPORT_U);
            const int_T *dims_V  = ssGetOutputPortDimensions(S, OUTPORT_V);
            const int_T  M       = dims_U[0];
            const int_T  N       = dims_V[0]; /* A (= U S V^T) is MxN */
            const int_T  P       = MIN(M,N);
            const int_T  U_numDims = ssGetOutputPortNumDimensions(S, OUTPORT_U);
            const int_T  P_U     = (U_numDims == 2) ? dims_U[1] : 1;
            const int_T  V_numDims = ssGetOutputPortNumDimensions(S, OUTPORT_V);
            const int_T  P_V     = (V_numDims == 2) ? dims_V[1] : 1;

            if ((P != P_U) || (P != P_V))
                THROW_ERROR(S,"The number of columns in both U and V must match min(M,N), where M is the number of rows in U and N is the number of rows in V.\n");

            if (isOutputDynamicallySized(S, OUTPORT_S)) {
                if (!ssSetOutputPortVectorDimension(S, OUTPORT_S, P)) return;
            }
            else {
              /* Do error checking for S */
              if (ssGetOutputPortWidth(S,OUTPORT_S) != P)
                THROW_ERROR(S,"Width of S output port must be equal to min(M,N), where M is the number of rows in U and N is the number of rows in V.\n");
            }

            if (!ssSetInputPortMatrixDimensions(S, INPORT_A, M, N)) return;
            /* No need to check dimensions of A. They are not set, else we
               would not be in this routine. */

        }
        /* Else, are S and U set? If so, and if U is rectangular, then we can set A and V. */
	else if ((!isOutputDynamicallySized(S, OUTPORT_S)) && (!isOutputDynamicallySized(S, OUTPORT_U))) {
            const int_T *dims_U  = ssGetOutputPortDimensions(S, OUTPORT_U);
            const int_T  M       = dims_U[0];
            const int_T  P       = ssGetOutputPortWidth(S,OUTPORT_S);
            const int_T  U_numDims = ssGetOutputPortNumDimensions(S, OUTPORT_U);
            const int_T  P_U     = (U_numDims == 2) ? dims_U[1] : 1;

            if (P != P_U)
                THROW_ERROR(S,"The number of columns in U must match the length of S.\n");

            if (M > P) { /* Is U rectangular? If so, we can set A and V. */
                if (isOutputDynamicallySized(S, OUTPORT_V)) {
                    if (!ssSetOutputPortMatrixDimensions(S, OUTPORT_V, P, P)) return;
                }
                else {
                    /* Do error checking for V */
	            const int_T *V_dims = ssGetOutputPortDimensions(S, OUTPORT_V);
                    if (!isOutputScalar(S, OUTPORT_V)) { /* V is not a scalar */
                        ErrorIfOutputIsNot2D(S, OUTPORT_V);
                        if (V_dims[0] != P)
                            THROW_ERROR(S,"V must have the same number of rows as S has elements.\n");
                        if (V_dims[1] != P)
                            THROW_ERROR(S,"V must have the same number of columns as S has elements.\n");
                    }
                    else { /* V is a scalar */
	               if (P != 1) THROW_ERROR(S,"For V to be a scalar, S must also be a scalar.\n");
		    }
		}
                if (!ssSetInputPortMatrixDimensions(S, INPORT_A, M, P)) return;
                /* No need to check dimensions of A. They are not set, else we
                   would not be in this routine. */
	    }
	}
        /* Else, are S and V set? If so, and if V is rectangular, then we can set A and U. */
	else if ((!isOutputDynamicallySized(S, OUTPORT_S)) && (!isOutputDynamicallySized(S, OUTPORT_V))) {
            const int_T *dims_V  = ssGetOutputPortDimensions(S, OUTPORT_V);
            const int_T  N       = dims_V[0];
            const int_T  P       = ssGetOutputPortWidth(S,OUTPORT_S);
            const int_T  V_numDims = ssGetOutputPortNumDimensions(S, OUTPORT_V);
            const int_T  P_V     = (V_numDims == 2) ? dims_V[1] : 1;


            if (P != P_V)
                THROW_ERROR(S,"The number of columns in V must match the length of S.\n");

            if (N > P) { /* Is V rectangular? If so, we can set A and U. */
                if (isOutputDynamicallySized(S, OUTPORT_U)) {
                    if (!ssSetOutputPortMatrixDimensions(S, OUTPORT_U, P, P)) return;
                }
                else {
                    /* Do error checking for U */
	            const int_T *U_dims = ssGetOutputPortDimensions(S, OUTPORT_U);
                    if (!isOutputScalar(S, OUTPORT_U)) { /* U is not a scalar */
                        ErrorIfOutputIsNot2D(S, OUTPORT_U);
                        if (U_dims[0] != P)
                            THROW_ERROR(S,"U must have the same number of rows as S has elements.\n");
                        if (U_dims[1] != P)
                            THROW_ERROR(S,"U must have the same number of columns as S has elements.\n");
                    }
                    else { /* U is a scalar */
	               if (P != 1) THROW_ERROR(S,"For U to be a scalar, S must also be a scalar.\n");
		    }
		}
                if (!ssSetInputPortMatrixDimensions(S, INPORT_A, P, N)) return;
                /* No need to check dimensions of A. They are not set, else we
                   would not be in this routine. */
	    }
	}
    }
}

#endif

#include "dsp_trailer.c"

/* [EOF] sdspsvd.c */