#ifndef lint
static char sccsid[] = "@(#)disprep.c	1.2 (mg@ukc.ac.uk) 5/24/90";
#endif
#include "cs.h"		/*							DISPREP.C	*/
#include <math.h>
#include "disprep.h"

static	float	*ftcoefs = NULL;      /* malloc for fourier coefs, mag or db */
static	int	ftc_nelem = 0;	/* number of points allocated to ftcoefs */

printv(p)
 register PRINTV *p;
{
register int	nargs = p->INCOUNT;
register char	**txtp = p->ORTXT.inlist->arg;
register float	**valp = p->iargs;

	printf("instr %d:", p->h.insdshead->insno);
	do printf("  %s = %5.3f", *txtp++, **valp++);
	while (--nargs);
	printf("\n");
}

dspset(p)
 register DSPLAY *p;
{
register int	npts, size;
register char	*auxp;

	if (p->h.optext->t.pftype == 'k')
		npts = *p->iprd * ekr;
	else npts = *p->iprd * esr;
	if (npts <= 0) {
		initerror("illegal iprd");
		return;
	}
	size = npts * sizeof(float);
	if ((auxp = p->auxds) == NULL || npts != p->npts) {
		auxp = auxalloc((long)size,&p->auxds);
		p->npts = npts;
		p->auxend = (float *) (auxp + size);
	}
	p->nxtp = (float *) auxp;
}

kdsplay(p)
 DSPLAY *p;
{
register float	*fp = p->nxtp;

	*fp++ = *p->signal;
	if (fp >= p->auxend) {
		register char	*s = strmsg;
		fp = (float *) p->auxds;
		sprintf(s,"instr %d, signal %s:",
			p->h.insdshead->insno,p->h.optext->t.inlist->arg[0]);
		display(fp,p->npts,s,&p->dwid);
	}
	p->nxtp = fp;
}

dsplay(p)
 DSPLAY *p;
{
register float	*fp = p->nxtp, *sp = p->signal;
register int	nsmps = ksmps;

	do {
		*fp++ = *sp++;
		if (fp >= p->auxend) {
			register char *s = strmsg;
			fp = (float *) p->auxds;
			sprintf(s,"instr %d, signal %s:",
				p->h.insdshead->insno,p->h.optext->t.inlist->arg[0]);
			display(fp,p->npts,s,&p->dwid);
		}
	}
	while (--nsmps);
	p->nxtp = fp;
}

dftset(p)	/* dspdft -- calcs discrete Fourier transform of collected */
 DSPDFT *p;	/*	samples and displays coefficients (mag or db)	   */
{
	double	theta, a;
	double	onedws, pidws, twopidws, n2pidws;
	int	stepsiz, windsiz, hanning, ncoefs;
	long	nbytes;
	char	*auxp;
register int	n, k;
register float	*sinp, *cosp, *hanp;
	static float	*hanwindow = NULL;
	static long	hanw_nelem = 0;

	stepsiz = *p->iprd * esr;
	windsiz = *p->inpts;
	hanning = *p->ihann;
	p->dbout = *p->idbout;
	ncoefs = windsiz / 2;

	if (windsiz > WINDMAX) {
		initerror("too many points requested");
		return;
	}

	if (ncoefs > ftc_nelem) {
		/* Bigger (or first) buffer required */
		if (ftcoefs != NULL) free((char *) ftcoefs);
		ftcoefs = (float *) mmalloc((long) ncoefs * sizeof(float));
		ftc_nelem = ncoefs;
	}

	if (windsiz > hanw_nelem) {
		/* Bigger (or first) buffer required */
		if (hanwindow != NULL) free((char *) hanwindow);
		hanwindow = (float *) mmalloc((long) windsiz * sizeof(float));
		hanw_nelem = windsiz;
	}

	if (p->auxds != NULL && ncoefs == p->ncoefs && hanning == p->hanning)
		goto dftfin;
	p->ncoefs = ncoefs;
	p->hanning = hanning;
	onedws = 1.0 / windsiz;
	twopidws = 6.2831853 / windsiz;
	pidws = 3.1415927 / windsiz;
	if (hanning) {			/* if hanning requested,    */
		hanp = hanwindow;
		for (k=0; k<windsiz; k++) {	/*    for this window size  */
			a = sin(k*pidws);	/*    create hanning points */
			*hanp++ = a * a;
		}
	}
	fprintf(stderr,"dft: %s window, %s out, making tables ...\n",
		(hanning) ? "hanning":"rect", (p->dbout) ? "db":"magnitude");
	nbytes = ncoefs * windsiz * sizeof(float) * 2;
	auxp = auxalloc(nbytes, &p->auxds);		/* alloc 2-dim   */
	p->sinp = sinp = (float *) auxp;		/*  weighted sin  */
	p->cosp = cosp = (float *) (auxp + nbytes/2);	/*  and cos tabls */
	for (n=0; n < ncoefs; n++) {			/* now fill tables */
		n2pidws = n * twopidws;
		if (hanning) {
			hanp = hanwindow;
			for (k=0; k<windsiz; k++) {	  /*   with sines    */
				theta = k * n2pidws;
				a = onedws * *hanp++;	  /*   times hanning */
				*sinp++ = a * sin(theta);
				*cosp++ = a * cos(theta);
			}
		}
		else {
			for (k=0; k<windsiz; k++) {	   /* or sines alone */
				theta = k * n2pidws;	   /*  (rect window) */
				*sinp++ = onedws*sin(theta);
				*cosp++ = onedws*cos(theta);
			}
		}
	}
dftfin:	p->bufp = p->sampbuf;
	p->endp = p->bufp + windsiz;
	p->overlap = windsiz - stepsiz;
}

dspdft(p)
 register DSPDFT *p;
{
register float *sigp = p->signal, *bufp = p->bufp, *endp = p->endp;
register int	nsmps = ksmps;
	double	a, b, c;
	
	do {
		if (bufp < p->sampbuf) {	/* skip any spare samples   */
			bufp++; sigp++;
		}
		else {				/*    then start collecting */
			*bufp++ = *sigp++;
			if (bufp >= endp) {	/* when full, do dft:	    */
				register float *coefp = ftcoefs;
				register float *sinp = p->sinp, *cosp = p->cosp;
				register int	n;
				char *s = strmsg;
			
				for (n = 0; n < p->ncoefs; n++) {
					a = 0.0;
					b = 0.0;	  /* calculate coefs */
					bufp = p->sampbuf;
					do {
						a += *bufp * *cosp++;
						b += *bufp * *sinp++;
					}
					while (++bufp < endp);
					c = sqrt( a*a + b*b );
					if (!p->dbout)	   /* stor mag or db */
						*coefp++ = c;
					else *coefp++ = 20. * log10(c);
				}
				sprintf(s,"instr %d, signal %s, dft (%s):",
					p->h.insdshead->insno,p->h.optext->t.inlist->arg[0],
					p->dbout ? "db" : "mag");
				display(ftcoefs, p->ncoefs, s, &p->dwid);
				if (p->overlap > 0) {
					bufp = p->sampbuf;
					sinp = endp - p->overlap;
					do *bufp++ = *sinp++;
					while (sinp < endp);
				}
				else bufp = p->sampbuf + p->overlap;
			}
		}
	}
	while (--nsmps);
	p->bufp = bufp;
}


/* fftset, dspfft -- calcs Fast Fourier Transform of collected	*/
/*      samples and displays coefficients (mag or db)		*/
/* networks of a given size will be created only once		*/
/*	& used for each call of the same size (see fft_net.c)	*/

fftset(p)
 register DSPFFT *p;
{
 register int	window_size, step_size;

	window_size = *p->inpts;
	if (window_size > WINDMAX)  initerror("too many points requested");
	if (window_size < WINDMIN)  initerror("too few points requested");
	if (!power_of_two(window_size))
		initerror("window size must be power of two");
	if (inerrcnt)
		return;
	step_size  = *p->iprd * esr;
	p->ncoefs  = window_size / 2;
	p->overlap = window_size - step_size;
	p->hanning = *p->ihann;
	p->dbout   = *p->idbout;
	p->bufp    = p->sampbuf;
	p->endp    = p->bufp + window_size;
	p->windsize = window_size;
	p->overN    = 1./(*p->inpts);

	if (window_size > ftc_nelem) {	
		/* Bigger (or first)  buffer required */
		if (ftcoefs != NULL) free((char *) ftcoefs);
		/*
		 * *Was* p->window_size/2 for some reason.
		 * This caused core dumps.
		 */
		ftcoefs = (float *) mmalloc((long) window_size * sizeof(float));
		ftc_nelem = p->ncoefs;
	}
}

dspfft(p)
 register DSPFFT *p;
{
 register float *sigp = p->signal, *bufp = p->bufp, *endp = p->endp;
 register int   nsmps = ksmps;

	do {
		if (bufp < p->sampbuf) {	/* skip any spare samples */
			bufp++; sigp++;
		}
		else {				/* then start collecting  */
			*bufp++ = *sigp++;
			if (bufp >= endp) {	/* when full, do fft:     */
				char *s = strmsg;
				register float *tp, *tplim;
				fft(FORWARD, p->windsize, p->hanning,
				   p->sampbuf, REAL, LINEAR,
				   ftcoefs, MAG, p->dbout);	/* calc fft */
				tp = ftcoefs;
				tplim = tp + p->ncoefs;
				do  *tp *= p->overN;		/* scale 1/N */
				while (++tp < tplim);

				sprintf(s, "instr %d, signal %s, fft (%s):",
					p->h.insdshead->insno,
					p->h.optext->t.inlist->arg[0],
					p->dbout ? "db" : "mag");
				display(ftcoefs, p->ncoefs, s, &p->dwid);
			 	if (p->overlap > 0) {
					bufp = p->sampbuf;
					tp   = endp - p->overlap;
					do *bufp++ = *tp++;
					while(tp < endp);
				}
				else bufp = p->sampbuf + p->overlap;
			}
		}
	}
	while (--nsmps);
	p->bufp = bufp;
}