/*
 * SDSPPINV  Moore-Penrose Pseudoinverse
 *
 * Copyright 1995-2000 The MathWorks, Inc.
 * $Revision: 1.3 $ $Date: 2000/08/03 21:17:24 $
 */

#define S_FUNCTION_NAME  sdsppinv
#define S_FUNCTION_LEVEL 2

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

enum {X_IDX=0, E_IDX, WORK_IDX, S_IDX, U_IDX, V_IDX, NUM_DWORKS};
enum {INPORT_A=0, NUM_INPORTS};
enum {OUTPORT_X=0, NUM_OUTPORTS};


static void mdlInitializeSizes(SimStruct *S)
{

    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 (!ssSetNumOutputPorts(S, NUM_OUTPORTS)) return;

    if (!ssSetOutputPortDimensionInfo(S, OUTPORT_X, DYNAMIC_DIMENSION)) return;
    ssSetOutputPortFrameData(         S, OUTPORT_X, FRAME_NO);
    ssSetOutputPortComplexSignal(     S, OUTPORT_X, COMPLEX_INHERITED);
    ssSetOutputPortReusable(          S, OUTPORT_X, 0);
    ssSetOutputPortDataType(          S, OUTPORT_X, 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);
}


/*
 * Compute the Moore-Penrose Pseudoinverse of m-by-n input A:
 * X is n-by-m and satisfies:
 *   A*X*A = A
 *   X*A*X = X
 *   A*X and X*A are Hermitian
 */
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;

    { /* 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));

	    while(MN-- > 0) {
		*pX++ = *pA++;
	    }
	} else {
	    real_T *pA = (real_T *)(ssGetInputPortRealSignal(S, INPORT_A));
	    real_T *pX = (real_T *)(ssGetDWork(S, X_IDX));
	    
	    while(MN-- > 0) {
		*pX++ = *pA++;
	    }
	}
    }

    {
	/* Compute the svd */
	void  *pU    = ssGetDWork(S, U_IDX);
	void  *pS    = ssGetDWork(S, S_IDX);
	void  *pV    = ssGetDWork(S, V_IDX);
	void  *pe    = ssGetDWork(S, E_IDX);
	void  *pwork = ssGetDWork(S, WORK_IDX);
	int_T  info;
	int_T  wantv = 1;

	if (cA) {
	    creal_T *pX = (creal_T *)(ssGetDWork(S, X_IDX));

	    info = sdspsvd_cplx(pX, M, N, (creal_T *)pS, (creal_T *)pe,
		(creal_T *)pwork, (creal_T *)pU, (creal_T *)pV, wantv);
	} else {
	    real_T *pX = (real_T *)(ssGetDWork(S, X_IDX));
	    
	    info = sdspsvd_real(pX, M, N, (real_T *)pS, (real_T *)pe,
		(real_T *)pwork, (real_T *)pU, (real_T *)pV, wantv);
	}
	if (info) {
	    THROW_ERROR(S,"SVD did not converge");
	}
    }
    
    { /*
       * tol = max(m,n) * S(1,1) * eps
       * rank = sum(S(1:min(m,n)) > tol)
       * X = V(:,1:rank)*diag(1./S(1:rank))*U(:,1:rand)';
       */
	void *pX = ssGetOutputPortSignal(S, OUTPORT_X);
	real_T tol, cabsS;
	int_T rank, i;
	int_T P = MIN(M,N);
	
	if (cA) {
	    creal_T *pS = (creal_T *)(ssGetDWork(S, S_IDX));
	    creal_T *pU = (creal_T *)(ssGetDWork(S, U_IDX));
	    creal_T *pV = (creal_T *)(ssGetDWork(S, V_IDX));
	    creal_T *ppS;
	    
	    tol = (real_T)MAX(M,N) * (pS->re) * mxGetEps();
	    ppS = pS+P-1;
	    for(i=P; i>0; i--) {
		CABS(*ppS,cabsS);
		ppS--;
		if (cabsS >= tol) {
		    rank = i;
		    break;
		}
	    }
	    if (rank != 0) {
		creal_T *ppX, *ppU, *ppV, temp;
		int_T j,k;
		
		ppX = pX;

                /* Initialize output array */
                for (i=0; i<N*M; i++) {
		  ppX->re = 0.0;
		  ppX->im = 0.0;
                  ppX++;
		}
		ppX = pX;

		for(j=0; j<M; j++) {
		    for(i=0; i<N; i++) {
			ppV = pV+i;
			ppU = pU+j;
			ppS = pS;
			for(k=0; k<rank; k++) {
			    temp.re = CMULT_YCONJ_RE(*ppV,*ppU);
			    temp.im = CMULT_YCONJ_IM(*ppV,*ppU);
			    CDIV(temp,*ppS,temp);
			    ppX->re += temp.re;
			    ppX->im += temp.im;
			    ppS++;
			    ppV += N;
			    ppU += M;
			}
			ppX++;
		    }
		}
	    }
	} else {
	    real_T *pS = (real_T *)(ssGetDWork(S, S_IDX));
	    real_T *pU = (real_T *)(ssGetDWork(S, U_IDX));
	    real_T *pV = (real_T *)(ssGetDWork(S, V_IDX));
	    real_T *ppS;

	    tol = (real_T)MAX(M,N) * (*pS) * mxGetEps();
	    ppS = pS+P-1;
	    for(i=P; i>0; i--) {
		if (fabs(*ppS--) >= tol) {
		    rank = i;
		    break;
		}
	    }
	    if (rank != 0) {
		real_T *ppX, *ppU, *ppV;
		int_T j,k;
		
		ppX = pX;
                /* Initialize output array */
                for (i=0; i<N*M; i++) {
		  *ppX++ = 0.0;
		}
		ppX = pX;

		for(j=0; j<M; j++) {
		    for(i=0; i<N; i++) {
			ppV = pV+i;
			ppU = pU+j;
			ppS = pS;
			for(k=0; k<rank; k++) {
			    *ppX += *ppV * *ppU / *ppS++;
			    ppV += N;
			    ppU += M;
			}
			ppX++;
		    }
		}
	    }
	}
    }
}


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;

    /* Make sure input is not 3-D or higher: */
    if(!isInput1or2D(S, port)) {
        THROW_ERROR(S, "Input must be 1-D or 2-D.");
    }
    {
        /* Unoriented inputs are interpreted as an oriented column vectors. 
         * Set output port 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;

	if (isOutputDynamicallySized(S, OUTPORT_X)) {
	    if (!ssSetOutputPortMatrixDimensions(S, OUTPORT_X, N, M)) 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;

    /*  Output error checking: 
     *  Port X must be oriented.
     */
    ErrorIfOutputIsUnoriented(S, port);

    if (!isInputDynamicallySized(S, OUTPORT_X)) {
	const int_T numDimsX = ssGetOutputPortNumDimensions(S, OUTPORT_X);
	const int_T *dims_X  = ssGetOutputPortDimensions(S, OUTPORT_X);
	const int_T N        = dims_X[0];
	const int_T M        = (numDimsX == 2) ? dims_X[1] : 1;

	if (!ssSetInputPortMatrixDimensions(S, INPORT_A, M, N)) return;
    }
}


#define MDL_SET_WORK_WIDTHS
static void mdlSetWorkWidths(SimStruct *S)
{
    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;

    CSignal_T Cplx = ssGetInputPortComplexSignal(S, INPORT_A);
    
    if(!ssSetNumDWork(      S, NUM_DWORKS)) return;
    
    ssSetDWorkWidth(        S, X_IDX, M*N);
    ssSetDWorkDataType(     S, X_IDX, SS_DOUBLE);
    ssSetDWorkComplexSignal(S, X_IDX, Cplx);
    
    ssSetDWorkWidth(        S, E_IDX, N);
    ssSetDWorkDataType(     S, E_IDX, SS_DOUBLE);
    ssSetDWorkComplexSignal(S, E_IDX, Cplx);
    
    ssSetDWorkWidth(        S, WORK_IDX, M);
    ssSetDWorkDataType(     S, WORK_IDX, SS_DOUBLE);
    ssSetDWorkComplexSignal(S, WORK_IDX, Cplx);
    
    ssSetDWorkWidth(        S, S_IDX, MIN(M+1,N));
    ssSetDWorkDataType(     S, S_IDX, SS_DOUBLE);
    ssSetDWorkComplexSignal(S, S_IDX, Cplx);
 
    ssSetDWorkWidth(        S, U_IDX, M*MIN(M,N));
    ssSetDWorkDataType(     S, U_IDX, SS_DOUBLE);
    ssSetDWorkComplexSignal(S, U_IDX, Cplx);

    ssSetDWorkWidth(        S, V_IDX, N*N);
    ssSetDWorkDataType(     S, V_IDX, SS_DOUBLE);
    ssSetDWorkComplexSignal(S, V_IDX, Cplx);
}
#endif

#include "dsp_trailer.c"

/* [EOF] sdsppinv.c */