/*
 * Windowed sinc lowpass/bandpass/highpass filter.
 */

/*
 * November 18, 1999
 * Copyright 1994 Julius O. Smith
 * Copyright 1991 (?) Lance Norskog (?)
 * Copyright 1999 Stan Brooks <stabro@megsinet.net>
 *
 * -------------------------------------------------------------------
 * This source code is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * -------------------------------------------------------------------
 *
 * REMARKS: (Stan Brooks speaking)
 * This code is heavily based on the resample.c code which was
 * apparently itself a rewrite (by Lance Norskog?) of code originally
 * by Julius O. Smith, and distributed under the GPL license...
 *
 */

#include <math.h>
#include <string.h>
#include <stdlib.h>

#include "st_i.h"

#ifndef HAVE_MEMMOVE
#define memmove(dest,src,len) bcopy((src),(dest),(len))
#endif

/* this Float MUST match that in resample.h */
#define Float double/*float*/

#define ISCALE 0x10000
#define BUFFSIZE 8192

/* Private data for Lerp via LCM file */
typedef struct filterstuff {
	st_rate_t rate;
	st_sample_t freq0;/* low  corner freq */
	st_sample_t freq1;/* high corner freq */
	double beta;/* >2 is kaiser window beta, <=2 selects nuttall window */
	long Nwin;
	Float *Fp;/* [Xh+1] Filter coefficients */
	long Xh;/* number of past/future samples needed by filter  */
	long Xt;/* target to enter new data into X */
	Float *X, *Y;/* I/O buffers */
} *filter_t;

/* makeFilter() declared in resample.c */
extern int 
makeFilter(Float Fp[], long Nwing, double Froll, double Beta, long Num, int Normalize);

static void FiltWin(filter_t f, long Nx);

/*
 * Process options
 */
int st_filter_getopts(eff_t effp, int n, char **argv)
{
	filter_t f = (filter_t) effp->priv;

	f->beta = 16;  /* Kaiser window, beta 16 */
	f->Nwin = 128;
	
	f->freq0 = f->freq1 = 0;
	if (n >= 1) {
		char *p;
		p = argv[0];
		if (*p != '-') {
			f->freq1 = strtol(p, &p, 10);
		}
		if (*p == '-') {
			f->freq0 = f->freq1;
			f->freq1 = strtol(p+1, &p, 10);
		}
		if (*p) f->freq1 = f->freq0 = 0;
	}
	/* fprintf(stderr,"freq: %d-%d\n", f->freq0, f->freq1);fflush(stderr); */
	if (f->freq0 == 0 && f->freq1 == 0)
	{
		st_fail("Usage: filter low-high [ windowlength [ beta ] ]");
		return (ST_EOF);
	}

	if ((n >= 2) && !sscanf(argv[1], "%ld", &f->Nwin))
	{
		st_fail("Usage: filter low-high [ windowlength ]");
		return (ST_EOF);
	}
	else if (f->Nwin < 4) {
		st_fail("filter: window length (%ld) <4 is too short", f->Nwin);
		return (ST_EOF);
	}

	if ((n >= 3) && !sscanf(argv[2], "%lf", &f->beta))
	{
		st_fail("Usage: filter low-high [ windowlength [ beta ] ]");
		return (ST_EOF);
	}

	st_report("filter opts: %d-%d, window-len %d, beta %f\n", f->freq0, f->freq1, f->Nwin, f->beta);
	return (ST_SUCCESS);
}

/*
 * Prepare processing.
 */
int st_filter_start(eff_t effp)
{
	filter_t f = (filter_t) effp->priv;
	Float *Fp0, *Fp1;
	long Xh0, Xh1, Xh;
	int i;

	f->rate = effp->ininfo.rate;

	/* adjust upper frequency to Nyquist if necessary */
	if (f->freq1 > f->rate/2 || f->freq1 <= 0)
		f->freq1 = f->rate/2;

	if ((f->freq0 < 0) || (f->freq0 > f->freq1))
	{
		st_fail("filter: low(%d),high(%d) parameters must satisfy 0 <= low <= high <= %d",
					f->freq0, f->freq1, f->rate/2);
		return (ST_EOF);
	}
	
	Xh = f->Nwin/2;
	Fp0 = (Float *) malloc(sizeof(Float) * (Xh + 2)) + 1;
	if (f->freq0 > f->rate/200) {
		Xh0 = makeFilter(Fp0, Xh, 2.0*(double)f->freq0/f->rate, f->beta, 1, 0);
		if (Xh0 <= 1)
		{
			st_fail("filter: Unable to make low filter\n");
			return (ST_EOF);
		}
	} else {
		Xh0 = 0;
	}
	Fp1 = (Float *) malloc(sizeof(Float) * (Xh + 2)) + 1;
	/* need Fp[-1] and Fp[Xh] for makeFilter */
	if (f->freq1 < f->rate/2) {
		Xh1 = makeFilter(Fp1, Xh, 2.0*(double)f->freq1/f->rate, f->beta, 1, 0);
		if (Xh1 <= 1)
		{
			st_fail("filter: Unable to make high filter\n");
			return (ST_EOF);
		}
	} else {
		Fp1[0] = 1.0;
		Xh1 = 1;
	}
	/* now subtract Fp0[] from Fp1[] */
	Xh = (Xh0>Xh1)?  Xh0:Xh1; /* >=1, by above */
	for (i=0; i<Xh; i++) {
		Float c0,c1;
		c0 = (i<Xh0)? Fp0[i]:0;
		c1 = (i<Xh1)? Fp1[i]:0;
		Fp1[i] = c1-c0;
	}

	free(Fp0 - 1); /* all done with Fp0 */
	f->Fp = Fp1;

	Xh -= 1;       /* Xh = 0 can only happen if filter was identity 0-Nyquist */
	if (Xh<=0)
		st_warn("filter: adjusted freq %d-%d is identity", f->freq0, f->freq1);

	f->Nwin = 2*Xh + 1;  /* not really used afterwards */
	f->Xh = Xh;
	f->Xt = Xh;

	f->X = (Float *) malloc(sizeof(Float) * (2*BUFFSIZE + 2*Xh));
	f->Y = f->X + BUFFSIZE + 2*Xh;

	/* Need Xh zeros at beginning of X */
	for (i = 0; i < Xh; i++)
		f->X[i] = 0;
	return (ST_SUCCESS);
}

/*
 * Processed signed long samples from ibuf to obuf.
 * Return number of samples processed.
 */
int st_filter_flow(eff_t effp, st_sample_t *ibuf, st_sample_t *obuf, 
                   st_size_t *isamp, st_size_t *osamp)
{
	filter_t f = (filter_t) effp->priv;
	long i, Nx, Nproc;

	/* constrain amount we actually process */
	/* fprintf(stderr,"Xh %d, Xt %d, isamp %d, ",f->Xh, f->Xt, *isamp);fflush(stderr); */
	Nx = BUFFSIZE + 2*f->Xh - f->Xt;
	if (Nx > *isamp) Nx = *isamp;
	if (Nx > *osamp) Nx = *osamp;
	*isamp = Nx;

	{
		Float *xp, *xtop;
		xp = f->X + f->Xt;
		xtop = xp + Nx;
		if (ibuf != NULL) {
			while (xp < xtop)
				*xp++ = (Float)(*ibuf++) / ISCALE;
		} else {
			while (xp < xtop)
				*xp++ = 0;
		}
	}

	Nproc = f->Xt + Nx - 2*f->Xh;

	if (Nproc <= 0) {
		f->Xt += Nx;
		*osamp = 0;
		return (ST_SUCCESS);
	}
	/* fprintf(stderr,"flow Nproc %d\n",Nproc); */
	FiltWin(f, Nproc);

	/* Copy back portion of input signal that must be re-used */
	Nx += f->Xt;
	if (f->Xh)
		memmove(f->X, f->X + Nx - 2*f->Xh, sizeof(Float)*2*f->Xh); 
	f->Xt = 2*f->Xh;

	for (i = 0; i < Nproc; i++)
		*obuf++ = f->Y[i] * ISCALE;

	*osamp = Nproc;
	return (ST_SUCCESS);
}

/*
 * Process tail of input samples.
 */
int st_filter_drain(eff_t effp, st_sample_t *obuf, st_size_t *osamp)
{
	filter_t f = (filter_t) effp->priv;
	long isamp_res, osamp_res;
	st_sample_t *Obuf;

	/* fprintf(stderr,"Xh %d, Xt %d  <--- DRAIN\n",f->Xh, f->Xt); */

	/* stuff end with Xh zeros */
	isamp_res = f->Xh;
	osamp_res = *osamp;
	Obuf = obuf;
	while (isamp_res>0 && osamp_res>0) {
		st_sample_t Isamp, Osamp;
		Isamp = isamp_res;
		Osamp = osamp_res;
		st_filter_flow(effp, NULL, Obuf, &Isamp, &Osamp);
	  /* fprintf(stderr,"DRAIN isamp,osamp  (%d,%d) -> (%d,%d)\n",
		 * isamp_res,osamp_res,Isamp,Osamp); */
		Obuf += Osamp;
		osamp_res -= Osamp;
		isamp_res -= Isamp;
	};
	*osamp -= osamp_res;
	/* fprintf(stderr,"DRAIN osamp %d\n", *osamp); */
	if (isamp_res)
		st_warn("drain overran obuf by %d\n", isamp_res); fflush(stderr);
	return (ST_SUCCESS);
}

/*
 * Do anything required when you stop reading samples.  
 * Don't close input file! 
 */
int st_filter_stop(eff_t effp)
{
	filter_t f = (filter_t) effp->priv;

	free(f->Fp - 1);
	free(f->X);
	return (ST_SUCCESS);
}

static double jprod(const Float *Fp, const Float *Xp, long ct)
{
	const Float *fp, *xp, *xq;
	double v = 0;
	
	fp = Fp + ct;	/* so sum starts with smaller coef's */
	xp = Xp - ct;
	xq = Xp + ct;
	while (fp > Fp) {   /* ct = 0 can happen */
		v += *fp * (*xp + *xq);
		xp++; xq--; fp--;
	}
	v += *fp * *xp;
	return v;
}

static void FiltWin(filter_t f, long Nx)
{
	Float *Y;
	Float *X, *Xend;

	Y = f->Y;
	X = f->X + f->Xh;			/* Ptr to current input sample */
	Xend = X + Nx;
	while (X < Xend) {
		*Y++ = jprod(f->Fp, X, f->Xh);
		X++;
	}
}