/*
 * Copyright (c) 1991-1993 Regents of the University of California.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. All advertising materials mentioning features or use of this software
 *    must display the following acknowledgement:
 *	This product includes software developed by the Computer Systems
 *	Engineering Group at Lawrence Berkeley Laboratory.
 * 4. Neither the name of the University nor of the Laboratory may be used
 *    to endorse or promote products derived from this software without
 *    specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 */
static const char rcsid[] =
    "@(#) $Header: /work/projects/tove/cvs/src/testing/vat/ss.cc,v 1.1 1997/12/08 17:22:52 parnanen Exp $ (LBL)";

#include "config.h"
#include "ss.h"

extern "C" const u_char mulawsum[256*256];
extern "C" const u_char mugaintab[64*256];

static inline int ABS(int i) { return (i < 0? -i : i); }

#define ALL_ZERO ((ULAW_ZERO<<24)|(ULAW_ZERO<<16|(ULAW_ZERO<<8))|ULAW_ZERO)

#define	AGC_FILTER 4		/* 2^4 = 320ms time const. */
#define AGC_CUTOFF 16
#define AGC_GAIN (AGC_FILTER + 3)
#define	AGC_DEADTIME (8*160)		/* adjust every 160ms (time in samples) */

SampleStream::SampleStream(int size, int n, int p, int l) :
	samples(new u_char[n * size]),
	zero(new u_char[size]),
	now(p + size),
	sclock(p + size + random()),
	past(p + size),
	blksize(size),
	totsize(n * size),
	maxsamp((n - 1) * size - p),
	sltmean(0),
	smean(0),
	smax(0),
	doAGC(0),
	bias(0),
	oldbias(0),
	AGClevel(l),
	AGCerr(0),
	deadtime(0),
	ssthresh_(20)
{
	if (maxsamp < (int)blksize)
		abort();

	memset((void *)zero, ULAW_ZERO, blksize);
	memset(samples, ULAW_ZERO, totsize);
}

SampleStream::SampleStream(const SampleStream& s) :
	samples(new u_char[s.totsize]),
	zero(new u_char[s.blksize]),
	now(s.past),
	sclock(s.past + random()),
	past(s.past),
	blksize(s.blksize),
	totsize(s.totsize),
	maxsamp(s.maxsamp),
	sltmean(0),
	smean(0),
	smax(0),
	doAGC(0),
	bias(0),
	oldbias(0),
	AGClevel(s.AGClevel),
	AGCerr(0),
	deadtime(0),
	ssthresh_(s.ssthresh_)
{
	memset((void *)zero, ULAW_ZERO, blksize);
	memset(samples, ULAW_ZERO, totsize);
}

void SampleStream::Compute()
{
	/*
	 * compute short term max power (max of 8 sample means) and
	 * long term power (full frame mean passed through simple
	 * lowpass filter).  This is done for every input & output
	 * frame so it should be reasonably efficient.  We take advantage
	 * of the fact that frames are 8-byte aligned (at least) and
	 * of the u-law sign-magnitude representation to do several
	 * sums 'in parallel'.
	 */
	register int b = blksize;
	register int amt = b >> 3, max = 0, sum = 0;
	register const u_int *ip = (const u_int*)&samples[now];

	register int mask = 0x007f007f;
	do {
		register int s0 = ~ip[0];
		register int s1 = ~ip[1];
		ip += 2;
		register int p;
		p  = s0 & mask;
		p += (s0 >> 8) & mask;
		p += s1 & mask;
		p += (s1 >> 8) & mask;
		p = ((p >> 16) + p) & 0x3ff;
		if (p > max)
			max = p;
		sum += p;
	} while (--amt > 0);
	smax = max;
	sum /= b;
	smean = sum;
	register int sm = sltmean;
	register int ssm = sm >> MEAN_FILTER;
	sm += sum - ssm;
	sltmean = sm;
}

void SampleStream::_UpdateAGC()
{
	register int sm = sltmean;

	if (sm >= (AGC_CUTOFF << MEAN_FILTER) &&
	    smax - sm >= (ssthresh_ << MEAN_FILTER)) {
		register int ssm = sm >> MEAN_FILTER;
		register int err = AGClevel - ssm;
		AGCerr += err - (AGCerr >> AGC_FILTER);
		register int abserr = ABS(AGCerr);
		if (int(sclock - deadtime) >= 0 &&
		    (abserr >> (AGC_GAIN-1)) > 0) {
			deadtime = sclock + AGC_DEADTIME;
			AdjustScale((AGCerr + AGC_GAIN/2) >> AGC_GAIN);
		}
	}
}

static inline const u_int* doMix(u_int* sp, const u_int* ip, int len)
{
	const u_char* const mixtab = mulawsum;
	register u_int bmask = 0xff00;
	register u_int allzero = ALL_ZERO;

	for (register u_int* ep = sp + (len >> 2); sp < ep; ++ip, ++sp) {
		register u_int mixsum;
		register u_int idat = *ip;
		register u_int sdat = *sp;

		// special case all zeros since they happen often
		if (idat == allzero)
			continue;
		if (sdat == allzero)
			mixsum = idat;
		else {
			register u_int sb = (sdat >> 16) & bmask;
			register u_char ib = idat >> 24;
			mixsum =  mixtab[sb | ib] << 24;
			sb = (sdat >> 8) & bmask;
			ib = idat >> 16;
			mixsum |=  mixtab[sb | ib] << 16;
			sb = sdat & bmask;
			ib = idat >> 8;
			mixsum |=  mixtab[sb | ib] << 8;
			sb = (sdat << 8) & bmask;
			ib = idat;
			mixsum |=  mixtab[sb | ib];
		}
		*sp = mixsum;
	}
	return (ip);
}

static inline const u_int* doBiasedMix(u_int* sp, const u_int* ip, int len,
				       int b)
{
	const u_char* const mixtab = mulawsum;
	const u_char* const gaintab = &mugaintab[(b + 32) << 8];
	register u_int bmask = 0xff00;
	register u_int allzero = ALL_ZERO;

	for (register u_int* ep = sp + (len >> 2); sp < ep; ++ip, ++sp) {
		register u_int mixsum;
		register u_int idat = *ip;
		register u_int sdat = *sp;

		// special case all zeros since they happen often
		if (idat == allzero)
			continue;

		register u_int sb = (sdat >> 16) & bmask;
		register u_char ib = idat >> 24;
		mixsum =  mixtab[sb | gaintab[ib]] << 24;
		sb = (sdat >> 8) & bmask;
		ib = idat >> 16;
		mixsum |=  mixtab[sb | gaintab[ib]] << 16;
		sb = sdat & bmask;
		ib = idat >> 8;
		mixsum |=  mixtab[sb | gaintab[ib]] << 8;
		sb = (sdat << 8) & bmask;
		ib = idat;
		mixsum |=  mixtab[sb | gaintab[ib]];
		*sp = mixsum;
	}
	return (ip);
}

#ifdef COMPRESSION
static inline const
u_int* doCompressedMix(u_int* sp, const u_int* ip, int len, int level)
{
	int power = 0;
	int i = len / 4;
	register int mask = 0x007f007f;
	const u_int *p = ip;
	do {
		register int s = ~*p++;
		power += s & mask;
		s >>= 8;
		power += s & mask;
	} while (--i > 0);
	double pin = (power >> 16) + (power & 0xffff);
	pin = pin / (16 * len) + 4.94692243;
	extern double CompressionSlope;
	double pout = pin * CompressionSlope + 
		(1 - CompressionSlope) * double(level) / 6.4;
	int gain = irint(32. / 4. * (pout - pin));
	if (gain < -32)
		gain = -32;
	else if (gain > 31)
		gain = 31;

	return doBiasedMix(sp, ip, len, gain);
}
#endif

void SampleStream::Mix(int delta, const u_char *in, int len)
{
	register u_int off;
	register const u_int* ip = (const u_int*)in;

	off = now + delta;
	if (off >= totsize)
		off -= totsize;

	register u_int* sp = (u_int*)&samples[off];
	if (off + len > totsize) {
		/*
		 * The thing we're mixing in overlaps the wrap at the
		 * end of the stream -- do the first piece (the one
		 * that goes to the end of the stream).
		 */
		register int i = totsize - off;
		len -= i;
#ifndef COMPRESSION
		if (bias)
			ip = doBiasedMix(sp, ip, i, bias);
#else
		if (doAGC)
			ip = doCompressedMix(sp, ip, i, AGClevel);
#endif
		else
			ip = doMix(sp, ip, i);
		sp = (u_int*)samples;
	}
#ifndef COMPRESSION
	if (bias)
		doBiasedMix(sp, ip, len, bias);
#else
	if (doAGC)
		doCompressedMix(sp, ip, len, AGClevel);
#endif
	else
		doMix(sp, ip, len);
}