/* simulateDSM.c - A MEX-file for simulating a delta-sigma modulator %[v,xn,xmax,y] = simulateDSM( u, ABCD, nlev[2], x0[0] )% or%[v,xn,xmax,y] = simulateDSM( u, ntf, nlev[2], x0[0] )%%Compute the output of a general delta-sigma modulator with input u,%a structure described by ABCD, an initial state x0 (default zero) and %a quantizer with a number of levels specified by nlev.%Multiple quantizers are implied by making nlev an array,% and multiple inputs are implied by the number of rows in u.%%Alternatively, the modulator may be described by an NTF.%The NTF is zpk object. (The STF is assumed to be 1.)%The structure that is simulated is the block-diagional structure used by%zp2ss.m.%%The obsolete NTF style is %a struct with fields 'zeros' and 'poles' ('k' is assumed to be 1).%the STF is assumed to be 1.%The structure that is simulated is the block-diagional structure used by%zp2ss.m.*/#include <stdio.h>#include <math.h>#include "mex.h"/* Global variables *//* In an effort to make the code more readable and to cut down on the overhead    associated with function calls, I have made many variables global. */char *cmdName = "simulateDSM";int    order,	/* The order of the modulator. */    nu,		/* The number of inputs, inferred from size(u,1). */    nq,		/* The number of quantizers, inferred from nlev */    N,		/* The number of time steps. */    ABCD_rows,	/* The number of rows in ABCD */    saveState;	/* Flag: keep track of the states. */double     *u,		/* Points into the input array. */    *v,		/* Points into the output array. */    *x,		/* The current state. */    *xn,	/* Points (in)to the (output) state array. */    *xMax,	/* Points to the state maxima output array. */    *py,	/* Points to the quantizer input output array. */    *ABCD,	/* The ABCD array (col-wise) description of the moduator. */    *nlev,	/* The number of quantizer levels. */    default_nlev=2;#ifdef __STDC__double quantize(double yy, int nLevels)#elsedouble quantize(yy, nLevels)double yy;int nLevels;#endif{     double vv;    if(nLevels%2) { /* Mid-tread quantizer */	vv = 2*floor(0.5*(yy+1));	if( vv > nLevels )	    vv = nLevels-1;	else if( vv < -nLevels )	    vv = 1-nLevels;    }    else { /* Mid-rise quantizer */	vv = 2*floor(0.5*yy)+1;	if( vv > nLevels )	    vv = nLevels-1;	else if( vv < -nLevels )	    vv = 1-nLevels;    }    return vv;}/* The following function is for debugging purposes only */#ifdef __STDC__void printMatrix(double *x, int m, int n)#elseprintMatrix(x, m, n)double *x;int m, n;#endif{int i,j;for(i=0; i<m; ++i){    for(j=0; j<n; ++j)	mexPrintf("%8.3f ",x[i+m*j]);    mexPrintf("\n");    }}#ifdef __STDC__void fatalError(char *s)#elsefatalError(s)char *s;#endif{    char msg[1024];    sprintf(msg, "%s: %s", cmdName, s);    mexErrMsgTxt(msg);}#ifdef __STDC__void initializeX(const mxArray *M_x0)#elseinitializeX(M_x0)mxArray *M_x0;#endif{    int i;    double *x0 = mxGetPr(M_x0);    if( mxGetM(M_x0)!=order || mxGetN(M_x0)!=1 )	fatalError("x0 must be an order x 1 column vector.");    for(i=0; i<order; ++i)	x[i] = *x0++;}#ifdef __STDC__void checkArgs(int nlhs, mxArray **plhs, int nrhs, const mxArray **prhs)#elsecheckArgs(nlhs, plhs, nrhs, prhs)int nlhs, nrhs;mxArray *plhs[], *prhs[];#endif{    int i;    int form;    double *pABCD;    const mxArray *arg2=prhs[1];    mxArray *zeros, *poles;    /* Verify the rhs (input) arguments */    if( nrhs < 2 )	fatalError("At least two input arguments are needed.");    if( !mxIsDouble(prhs[0]) )	fatalError("The input vector does not contain double-precision data.");    u = mxGetPr(prhs[0]);    nu = mxGetM(prhs[0]);    N = mxGetN(prhs[0]);    nq = 1;    nlev = &default_nlev;    if(nrhs>=3)	if( !( mxIsEmpty(prhs[2]) || mxIsNaN(*mxGetPr(prhs[2])) ) ){	    nq = mxGetM(prhs[2]) * mxGetN(prhs[2]);	    nlev = mxGetPr(prhs[2]);	}    /* Determine the form of the modulator */    if( mxIsClass(arg2,"zpk") ){		/* NTF in zpk form */	/* Matlab code: [z,p,k] = zpkdata(ntf); zeros = z{1}; poles=p{1} */	mxArray *lhs[3];	form = 0;	mexCallMATLAB(3,lhs,1,&arg2,"zpkdata"); 	zeros = mxGetCell(lhs[0],0);	poles = mxGetCell(lhs[1],0);	if( (order=mxGetNumberOfElements(zeros)) != mxGetNumberOfElements(poles) )	    fatalError("The number of poles must equal the number of zeros.");    }    else if( mxIsStruct(arg2) ){		/* Obsolete NTF form */	if( (zeros=mxGetField(arg2,0,"zeros"))==0 )	    fatalError("No zeros field in the NTF struct.");	if( !mxIsNumeric(zeros) )	    fatalError("The zeros field is not numeric.");	if( (poles=mxGetField(arg2,0,"poles"))==0 )	    fatalError("No poles field in the NTF struct.");	if( !mxIsNumeric(poles) )	    fatalError("The poles field is not numeric.");	if( (order=mxGetNumberOfElements(zeros)) != mxGetNumberOfElements(poles) )	    fatalError("The number of poles must equal the number of zeros.");	mexWarnMsgTxt("You appear to be using an old-style form of NTF specification.\nAutomatic converstion to the new form will be done for this release only.");	form = 2;    }    else if( mxIsNumeric(arg2) ){	if( mxGetN(arg2)==mxGetM(arg2)+nu && mxIsDouble(arg2) ){	    form = 1;			/* ABCD form */	    order = mxGetM(arg2)-nq;	}	else if( mxGetN(arg2)==2 ){	    mexWarnMsgTxt("You appear to be using the old-style form of NTF specification.\nAutomatic converstion to the new form will be done for this release only.");	    form = 3;		/* old NTF form */	    order = mxGetM(arg2);	}	else	    fatalError("ABCD must be an order+nq by order+nu+nq matrix.");    }    else	fatalError("The second argument is neither a proper ABCD matrix nor an NTF.");    if( form==1 ){		/* ABCD form */	ABCD = mxGetPr(arg2);    }    else if( form==0 || form==2 || form==3 ){	/* NTF form */	/* MATLAB code for computing ABCD 	   [A,B2,C,D2] = zp2ss(ntf.poles,ntf.zeros,-1);	   D2 = 0;	   % !!!! Assume stf=1	   B1 = -B2;	   D1 = 1;	   ABCD = [ A B1 B2; C D1 D2]	*/	mxArray *lhs[4],*pntf[3];	double *p1,*p2;	if( nu != 1 )	    fatalError("Fatal error. Number of inputs must be 1 for a modulator specified by its NTF\n");	if( nq != 1)	    fatalError("Fatal error. Number of quantizers must be 1 for a modulator specified by its NTF\n");	/* Copy the ntf (arg2) into the temporary pntf[] matrices. */	/* This could be accomplished more efficiently (but more dangerously)	   by directly setting the elements of the mxArray data structure. */	pntf[0] = mxCreateDoubleMatrix(order,1,mxCOMPLEX);	pntf[1] = mxCreateDoubleMatrix(order,1,mxCOMPLEX);	pntf[2] = mxCreateDoubleMatrix(1,1,mxREAL);	p2 = mxGetPr((form==0||form==2) ? zeros:arg2);	for( p1=mxGetPr(pntf[1]), i=0; i<order; ++i)	    *p1++ = *p2++;	if( form!=3 )	    p2 = mxGetPr(poles);	for( p1=mxGetPr(pntf[0]), i=0; i<order; ++i)	    *p1++ = *p2++;	p2 = mxGetPi((form==0||form==2) ? zeros:arg2);	if( p2 ) /* Non-null imaginary part. */	    for( p1=mxGetPi(pntf[1]),  i=0; i<order; ++i)		*p1++ = *p2++;	if( form!=3 )	    p2 = mxGetPi(poles);	if( p2 ) /* Non-null imaginary part. */	    for( p1=mxGetPi(pntf[0]), i=0; i<order; ++i)		*p1++ = *p2++;	*mxGetPr(pntf[2]) = -1;	mexCallMATLAB(4,lhs,3,pntf,"zp2ss"); 	p1 = mxGetPr(lhs[0]);	p2 = mxGetPr(lhs[2]);	ABCD = (double *)mxCalloc((order+nu)*(order+nu+nq),sizeof(double));	pABCD = ABCD;	for( i=0; i<order; ++i ){	    int j;	    for( j=0; j<order; ++j )		*pABCD++ = *p1++;	    *pABCD++ = *p2++;	}	p1 = mxGetPr(lhs[1]);	/* B1 = -B2 */	for( i=0; i<order; ++i )	    *pABCD++ = -*p1++;	*pABCD++ = 1;		/* D1 */	p1 = mxGetPr(lhs[1]);	/* B2 */	for( i=0; i<order; ++i )	    *pABCD++ = *p1++;	*pABCD = 0;		/* D2 */	for(i=0;i<4;++i) 	    mxDestroyArray(lhs[i]);	for(i=0;i<2;++i) 	    mxDestroyArray(pntf[i]);    }    else	fatalError("Internal error. form != 0, 1, 2 or 3!");    ABCD_rows = order + nq;    plhs[0] = mxCreateDoubleMatrix(nq,N,mxREAL);    v = mxGetPr(plhs[0]);    x = (double *)mxCalloc(order,sizeof(double));    if(nrhs>=4){	if( !( mxIsEmpty(prhs[3]) || mxIsNaN(*mxGetPr(prhs[3])) ) )		    initializeX(prhs[3]);	}        /* Verify the lhs (output) arguments */    saveState=0;     py=0;    xMax=0;    switch(nlhs){	case 4:	    plhs[3] = mxCreateDoubleMatrix(nq,N,mxREAL);	    py = mxGetPr(plhs[3]);	case 3:	    plhs[2] = mxCreateDoubleMatrix(order,1,mxREAL);	    xMax = mxGetPr(plhs[2]);	case 2:	    plhs[1] = mxCreateDoubleMatrix(order,N,mxREAL);	    xn = mxGetPr(plhs[1]);	    saveState=1;	    break;	case 1:	    break;	default:	    fatalError("Incorrect number of output arguments.");    }    if( !saveState )	xn = (double *)mxCalloc(order,sizeof(double));}/* Simulate the modulator using the difference equations. *//* For efficiency, store the state in xn and compute from x. *//* (These variables may be recycled internally, depending   on the output variables requested.) */#ifdef __STDC__void simulateDSM()#elsesimulateDSM()#endif{    int i,j,t, qi;    double *pABCD, *ptr, *pxn, tmp;    for( t=0; t<N; ++t ){ /* [xn;y] = ABCD*[x;u;v]; x=xn;*/	/* Compute y = C*x + D1*u and thence v for each quantizer */	for( qi=0; qi<nq; ++qi){	    tmp = 0;	    for(i=0, pABCD=ABCD+order+qi, ptr=x; i<order; ++i, pABCD+=ABCD_rows)		tmp += (*pABCD) * *ptr++;	    for(i=0, ptr=u; i<nu; ++i, pABCD+=ABCD_rows)		tmp += (*pABCD) * *ptr++;	    if( py!=0 )		*py++ = tmp;	    v[qi] = quantize(tmp, nlev[qi]);	}	/* Next compute xn = A*x + B*[u;v], */	for( i=0, pxn=xn; i<order; ++i ){	    tmp=0;	    pABCD=ABCD+i;	    for( ptr=x, j=0; j<order; ++j, pABCD += ABCD_rows )		tmp += *pABCD * *ptr++;	    for( ptr=u, j=0; j<nu; ++j, pABCD += ABCD_rows )		tmp += *pABCD * *ptr++;	    for( ptr=v, j=0; j<nq; ++j, pABCD += ABCD_rows )		tmp += *pABCD * *ptr++;	    *pxn++ = tmp;	    }	u += nu;	v += nq;	if(xMax!=0){	    for( i=0; i<order; ++i){		double abs=fabs(xn[i]);		if( abs > xMax[i] )		    xMax[i] = abs;	    }	}	if(saveState){	    x = xn;	    xn += order;	}	else { /* swap x and xn */	    double *xtmp = x;	    x = xn;	    xn = xtmp;	} /* if(saveState) */    } /* for( t=0 .... ) */}#ifdef __STDC__void mexFunction(int nlhs, mxArray **plhs, int nrhs, const mxArray **prhs)#elsemexFunction(nlhs, plhs, nrhs, prhs)int nlhs, nrhs;mxArray *plhs[], *prhs[];#endif{    checkArgs(nlhs, plhs, nrhs, prhs);    /* Print the variables being used     mexPrintf("x=\n");		printMatrix(x,order,1);    mexPrintf("\nABCD=\n");	printMatrix(ABCD,order+nq,order+nu+nq);    */    simulateDSM();}