// phasevocoder.C

/******************************************************************************
 *
 *  MiXViews - an X window system based sound & data editor/processor
 *
 *  Copyright (c) 1993, 1994 Regents of the University of California
 *
 *  Author:     Douglas Scott
 *  Date:       December 13, 1994
 *
 *  Permission to use, copy and modify this software and its documentation
 *  for research and/or educational purposes and without fee is hereby granted,
 *  provided that the above copyright notice appear in all copies and that
 *  both that copyright notice and this permission notice appear in
 *  supporting documentation. The author reserves the right to distribute this
 *  software and its documentation.  The University of California and the author
 *  make no representations about the suitability of this software for any 
 *  purpose, and in no event shall University of California be liable for any
 *  damage, loss of data, or profits resulting from its use.
 *  It is provided "as is" without express or implied warranty.
 *
 *****************************************************************************/

#ifdef __GNUG__
#pragma implementation
#endif

#include "application.h"
#include "framedata.h"
#include "envelope.h"
#include "localdefs.h"
#include "phasevocoder.h"
#include "pipeaction.h"

extern "C" {
	int fft_(float*, float*, int*, int*, int*, int*);
	int reals_(float*, float*, int*, int*);
	int kaiser_(int*, float*, int*, int*, float*);
}

PhaseVocoder::Info::Info(double sampRate, float scale, Mode mode,
                         int N, int F, int M, int D,
                         float T, float P, boolean useKaiser)
	: fftSize(N), fundFreq(F),
	  inputFrameSize(M), inputFrameOffset(D),
	  outputFrameSize(0), outputFrameOffset(0),
	  pchScaleFactor(P), timeScaleFactor(T), warp(0),
	  firstBand(0), lastBand(0), K(useKaiser),
	  inputScalingFactor(scale), samplingRate(sampRate),
	  runMode(mode) {}

PhaseVocoder::Info::Info(double samprate, float scale,
                         int N, int F, int D, int L, int I,
                         float T, int i, int j, float warpFactor,
						 boolean useKaiser)
	: fftSize(N), fundFreq(F),
	  inputFrameSize(0), inputFrameOffset(D), 
	  outputFrameSize(L), outputFrameOffset(I),
	  pchScaleFactor(1.0), timeScaleFactor(T),
	  warp(warpFactor), firstBand(i), lastBand(j), K(useKaiser),
	  inputScalingFactor(scale), samplingRate(samprate),
	  runMode(PhaseVocoder::Synthesis) {}

float PhaseVocoder::Pi = 4. * atan(1.);
float PhaseVocoder::HalfPi = 2. * atan(1.);
float PhaseVocoder::TwoPi = 8. * atan(1.);
float PhaseVocoder::beta = 6.8;				// for Kaiser window

// ctor for analysis only

PhaseVocoder::PhaseVocoder(Info& info) : I(info), initialized(false) {
	zeroPointers();
	setDefaults();
	setUpLog();
	switch(I.runMode) {
		case Spectrum:
			E = 1;
			break;
		case Magnitudes:
			X = 1;
			break;
		default:
			break;
	}
	if(checkAndSetValues())
		initialize();
}

void
PhaseVocoder::setDefaults() {
	E = X = 0;
	analWinLen = synWinLen = 0;
	startingOffset = 0;
	verbose = true;
	ftot = 0.;					// scale factor for calculating statistics
	inputFrameEven = 0;			// flag for even inputFrameSize
	outputFrameEven = 0;		// flag for even outputFrameSize
	C = 0;
	bandsLimited = 0;
}

PhaseVocoder::~PhaseVocoder() {
//	printToLog();
	Resource::unref(timeScaleEnvelope);
	delete [] output,
	delete [] anal,
	delete [] syn,
	delete [] analWindowBuf,
	delete [] synWindowBuf,
	delete [] maxAmp,
	delete [] avgAmp,
	delete [] avgFrq,
	delete [] env,
	delete [] oldInPhase,
	delete [] oldOutPhase;
}

// zero these to avoid deleting uninitialized pointers if in error condition

void
PhaseVocoder::zeroPointers() {
	timeScaleEnvelope = nil;
	output = nil;
	anal = nil;
	syn = nil;
	analWindowBuf = nil;
	synWindowBuf = nil;
	maxAmp = nil;
	avgAmp = nil;
	avgFrq = nil;
	env = nil;
	oldInPhase = nil;
	oldOutPhase = nil;
}

boolean
PhaseVocoder::isGood() { return initialized; }

void
PhaseVocoder::setUpLog() {
	if (verbose)
		fp = stderr;				// write out to stderr for now
//		fp = fopen("pvoc.log","w");
}

void
PhaseVocoder::printToLog() {
	if (verbose && isGood() && analyzing()) {	// dont do this if never configured!
		if (ftot != 0.)
			ftot = 1. / ftot;

		fprintf(fp,"\n   i         band       max amp     ");
		fprintf(fp,"avg amp    avg frq\n\n");
		int F2 = I.fundFreq / 2;
		for (int i = 0; i <= fftPoints; i++) fprintf(fp,
			"%4d   %5d - %5d   %8.5f    %8.5f   %8.1f\n",
			(i<<1)+1, i*I.fundFreq-F2, i*I.fundFreq+F2, *(maxAmp+i),
			*(avgAmp+i)*ftot, *(avgFrq+i)*ftot);
		fprintf(fp,"\n");
	}
}

	/*
	user friendliness: Try to catch bad parameter specifications and correct
	them if possible. The basic idea is that we step through the
	signal applying an inputFrameSize point window and taking an fftSize
	point FFT at a spacing of inputFrameOffset samples. The synthesis consists
	of taking an invFftSize point inverse FFT, applying an outputFrameSize
	point window, and overlap-adding at a spacing of outputFrameOffset samples.
	And in the middle, we linearly rescale the phase by the time-scale
	expansion factor timeScaleFactor = outputFrameOffset / inputFrameOffset.
	The default is outputFrameSize = inputFrameSize = fftSize and
	outputFrameOffset = inputFrameOffset, which gives an analysis-
	synthesis identity. NOTE: the actual timeScaleFactor will not necessarily
	equal the specified T - if this is important, then specify inputFrameOffset
	and outputFrameOffset directly. NOTE: pitch transposition is performed by
	changing the size of the inverse FFT relative to the forward
	FFT - this is a cheap trick which works best on octave
	transpositions. NOTE: the spectral envelope estimation and
	warping is relatively crude.
	*/

boolean
PhaseVocoder::checkAndSetValues() {
	char errmsg[128];
	errmsg[0] = '\0';
	if ((I.fftSize != 0) && (I.fundFreq != 0))
		sprintf(errmsg,"Don't specify both fftSize and fundFreq.");

	if ((I.fftSize == 0) && (I.fundFreq == 0))
		I.fftSize = 256;
	else if (I.fundFreq != 0)
		I.fftSize = int(I.samplingRate / I.fundFreq);

	if ((I.fftSize%2) != 0)
		I.fftSize += 1;		/* even values usually run faster */
	fftPoints = I.fftSize / 2;
	if (fftPoints > 16384) {
		sprintf(errmsg,"FFT size (%d) too large.", fftPoints);
		goto error;
	}

	I.fundFreq = int(I.samplingRate / I.fftSize);

	if (I.inputFrameSize == 0)
		I.inputFrameSize = I.fftSize;
	if ((I.inputFrameSize%2) == 0)
		inputFrameEven = 1;

	if (I.outputFrameSize == 0)
		I.outputFrameSize = I.inputFrameSize;
	if ((I.outputFrameSize%2) == 0)
		outputFrameEven = 1;

	if (I.inputFrameSize < 7) {
		sprintf(errmsg,"Input frame size (%d) is too small.", I.inputFrameSize);
		goto error;
	}

	obuflen = 4 * I.outputFrameSize;

	if (I.lastBand == 0) I.lastBand = fftPoints;
	if (I.lastBand > fftPoints) I.lastBand = fftPoints;
	if (I.firstBand < 0) I.firstBand = 0;
	bandsLimited = (I.firstBand != 0 || I.lastBand != fftPoints);

	if ((I.pchScaleFactor != 1.) && (I.timeScaleFactor != 1.)) 
		sprintf(errmsg, "Don't specify both time and pitch scaling -- using pitch scaling.");

	if ((I.pchScaleFactor != 1.) && variableTimeScaling()) {
		sprintf(errmsg, "Sorry, no pitch scaling with time-varying time scaling.");
		goto error;
	}

	if (I.pchScaleFactor != 1.)
		I.timeScaleFactor = I.pchScaleFactor;	/* pitch change is time change plus resamp */

	if (I.timeScaleFactor <= 0.){
		sprintf(errmsg,"Invalid time scale factor (%f).", I.timeScaleFactor);
		goto error;
	}

	if (I.inputFrameOffset == 0)
		if (I.timeScaleFactor > 1.)
			I.inputFrameOffset = int(I.inputFrameSize / (8.0 * I.timeScaleFactor));
		else
			I.inputFrameOffset = int(I.inputFrameSize / 8.0);

	if (I.inputFrameOffset == 0){
		sprintf(errmsg,"Time scale factor greater than inputFrameSize/8.");
		I.inputFrameOffset = 1;
	}

	// if doing resynthesis, remultiply outputFrameOffset by time factor
	// to return to "unity" value
	
	if (I.outputFrameOffset == 0)
		I.outputFrameOffset = synthesizing() ? 
			 int(I.inputFrameOffset / I.timeScaleFactor)
			 : int(I.timeScaleFactor * I.inputFrameOffset);

	if (I.outputFrameOffset == 0){
		sprintf(errmsg, "time factor * offset (or pitch factor * offset) < 1.   Increase input frame size.");
		goto error;
	}

	if (variableTimeScaling()) {
		// original timeScaleFactor was maximum - used to set outputFrameOffset
		I.timeScaleFactor = timeScaleEnvelope->get(0);	
		I.inputFrameOffset = int(I.outputFrameOffset / I.timeScaleFactor);
		if (I.inputFrameOffset < 1) {
			char msg[128];
			sprintf(msg, "Cannot expand by time scale factor %f.",
				I.timeScaleFactor);
			Application::alert(msg, "Setting to factor to maximum value.");
			I.inputFrameOffset = 1;
		}
		if (I.warp != 0.)
			I.warp = I.timeScaleFactor;	// warp varies with timeScaleFactor
	}

	if(variableTimeScaling() || !synthesizing())
		I.timeScaleFactor = (float(I.outputFrameOffset) / I.inputFrameOffset);

	if (I.pchScaleFactor != 1.)
		I.pchScaleFactor = I.timeScaleFactor;

	// synth transform will be invFftSize points
	invFftSize = int(I.fftSize / I.pchScaleFactor);

	if ((invFftSize % 2) != 0)
		invFftSize += 1;

	invFftPoints = invFftSize / 2;

	/* ideally, fftSize/invFftSize = I.outputFrameOffset/I.inputFrameOffset
		= pitch change
	*/
	
	I.pchScaleFactor = (float(I.fftSize) / invFftSize);

	if (I.warp == -1.)
		I.warp = I.pchScaleFactor;
	if ((E == 1) && (I.warp == 0.))
		I.warp = 1.;

	if ((I.pchScaleFactor != 1.) && (I.pchScaleFactor != I.timeScaleFactor))
		 sprintf(errmsg,"pchScaleFactor (%f) not equal to timeScaleFactor (%f).",I.pchScaleFactor,I.timeScaleFactor);

	scaledOutputOffset = int(I.outputFrameOffset / I.pchScaleFactor);

	if (verbose) {
		fprintf(fp,"\nfftSize: %d  I.inputFrameSize: %d  outputFrameSize: %d",I.fftSize,I.inputFrameSize,I.outputFrameSize); 
		fprintf(fp,"  inputFrameOffset: %d  outputFrameOffset: %d  fundFreq: %d",I.inputFrameOffset,I.outputFrameOffset,I.fundFreq);
		fprintf(fp,"  sampleRate: %d  pchScaleFactor: %5.2f  timeScaleFactor: %5.2f\n",int(I.samplingRate),I.pchScaleFactor,I.timeScaleFactor);
		if (bandsLimited)
			fprintf(fp,"C: %d    i: %d    j: %d\n",C,I.firstBand,I.lastBand);
		if (I.K)
			fprintf(fp,"---Kaiser Window---\n");
	}
	if(strlen(errmsg))
		Application::alert("Warning:", errmsg);
	return true;
error:
	Application::alert(errmsg);
	return false;
}

void
PhaseVocoder::initialize() {		
	createAnalysisWindow();
	createSynthesisWindow();
	createBuffers();
	reset();
	initialized = true;
}

// 	initialization: input time starts negative so that the rightmost
// 	edge of the analysis filter just catches the first non-zero
// 	input samples; output time is always timeScaleFactor times input time.

void
PhaseVocoder::reset() {
	outCount = 0;
	float inputFrameRate = I.samplingRate / I.inputFrameOffset;
	float outputFrameRate = I.samplingRate / I.outputFrameOffset;
	RoverTwoPi = inputFrameRate / TwoPi;
	TwoPioverR = TwoPi / outputFrameRate;
	// input time (in samples, starts negative)
	// inSamp is -1 * integer number of offsets per half-window
	inSamp = -(analWinLen/I.inputFrameOffset) * I.inputFrameOffset;
	// output time (in samps, also starts negative)
	outSamp = int(I.timeScaleFactor/I.pchScaleFactor * inSamp);
	sampsIn = analWinLen + inSamp + 1;		// number of new inputs to read
	startingOffset = sampsIn;				// cached here
	sampsOut = 0;
}

// 	set up analysis window: The window is assumed to be symmetric
// 	with inputFrameSize total points.  After the initial memory allocation,
// 	analWindow always points to the midpoint of the window
// 	(or one half sample to the right, if inputFrameSize is even);
// 	analWinLen is half the true window length (rounded down). Any low pass
// 	window will work; a Hamming window is generally fine,
// 	but a Kaiser is also available.  If the window duration is
// 	MXINT32er than the transform (inputFrameSize > fftSize), then the
// 	window is multiplied by a sin(x)/x function to meet the condition:
// 	analWindow[Ni] = 0 for i != 0.  In either case, the
// 	window is renormalized so that the phase vocoder amplitude
// 	estimates are properly scaled.

void
PhaseVocoder::createAnalysisWindow() {
	analWindowBuf = new float[I.inputFrameSize+inputFrameEven];
	bzero(analWindowBuf, (I.inputFrameSize+inputFrameEven) * sizeof(float));
	analWindow = analWindowBuf + (analWinLen = I.inputFrameSize/2);
	int one = 1;
	int i = 0;
	if (I.K)
		kaiser_(&I.inputFrameSize,analWindow,&analWinLen,&one,&beta);
	else
		hamming(analWindow,analWinLen,inputFrameEven);

	for (i = 1; i <= analWinLen; i++)
		*(analWindow - i) = *(analWindow + i - inputFrameEven);

	if (I.inputFrameSize > I.fftSize) {
		if (inputFrameEven)
			*analWindow *= I.fftSize * sin(HalfPi/I.fftSize) / HalfPi;
		for (i = 1; i <= analWinLen; i++) 
			*(analWindow + i) *=
				I.fftSize * sin(Pi*(i+.5*inputFrameEven)/I.fftSize) /
					(Pi*(i+.5*inputFrameEven));
		for (i = 1; i <= analWinLen; i++)
			*(analWindow - i) = *(analWindow + i - inputFrameEven);
	}

	/* normalize window for unity gain across unmodified
		analysis-synthesis procedure */

	sum = 0.;
	for (i = -analWinLen; i < analWinLen; i++)
		sum += *(analWindow + i);

	sum = 2. / sum;				// factor of 2 comes in later in trig identity

	if(analyzing())
		sum *= I.inputScalingFactor;	// input scaled by 1/(2^16) if short integer
	
	for (i = -analWinLen; i < analWinLen; i++)
		*(analWindow + i) *= sum;
}

// set up synthesis window:  For the minimal mean-square-error
// formulation (valid for fftSize >= inputFrameSize), the synthesis window
// is identical to the analysis window (except for a
// scale factor), and both are even in length.  If
// fftSize < inputFrameSize, then an interpolating synthesis window is used.

void
PhaseVocoder::createSynthesisWindow() {
	synWindowBuf = new float[I.outputFrameSize+outputFrameEven];
	bzero(synWindowBuf, (I.outputFrameSize+outputFrameEven) * sizeof(float));
	synWindow = synWindowBuf + (synWinLen = I.outputFrameSize/2);
	int one = 1;
	int i = 0;
	
	if (I.inputFrameSize <= I.fftSize) {
		if (I.K)
			kaiser_(&I.inputFrameSize,synWindow,&synWinLen,&one,&beta);
		else
			hamming(synWindow,synWinLen,outputFrameEven);

		for (i = 1; i <= synWinLen; i++)
			*(synWindow - i) = *(synWindow + i - outputFrameEven);

		for (i = -synWinLen; i < synWinLen; i++)
			*(synWindow + i) *= sum;

	// normalize window for unity gain across unmodified
	//	analysis-synthesis procedure

		sum = 0.;
		for (i = -synWinLen; i <= synWinLen; i+=I.outputFrameOffset)
			sum += *(synWindow + i) * *(synWindow + i);
	}
	else {
		hamming(synWindow,synWinLen,outputFrameEven);
		for (i = 1; i <= synWinLen; i++)
			*(synWindow - i) = *(synWindow + i - outputFrameEven);

		if (outputFrameEven)
			*synWindow *= scaledOutputOffset * sin(HalfPi/scaledOutputOffset) /
				HalfPi;
		for (i = 1; i <= synWinLen; i++) 
			*(synWindow + i) *= scaledOutputOffset *
				sin(Pi*(i+.5*outputFrameEven) / scaledOutputOffset) /
					(Pi*(i+.5*outputFrameEven));
		for (i = 1; i <= synWinLen; i++)
			*(synWindow - i) = *(synWindow + i - outputFrameEven);
	}

	sum = 1. / sum;

	if(synthesizing())
		sum *= I.inputScalingFactor;	// output mult. by (2^16) if short integer

	for (i = -synWinLen; i < synWinLen; i++)
		*(synWindow + i) *= sum;
}

void
PhaseVocoder::hamming(float *win, int winLen, int even) {
	int i;
	float ftmp = Pi/winLen;

	if (even) {
		for (i=0; i<winLen; i++)
			*(win+i) = .54 + .46 * cos(ftmp*(i+.5));
		*(win+winLen) = 0.;
	}
	else {
		*(win) = 1.;
		for (i=1; i<=winLen; i++)
			*(win+i) = .54 + .46 * cos(ftmp*i);
	}
}

void
PhaseVocoder::createBuffers() {

	/* set up output buffer:  nextOut always points to the next word
		to be shifted out.  The shift is simulated by writing the
		value to the standard output and then setting that word
		of the buffer to zero.  When nextOut reaches the end of
		the buffer, it jumps back to the beginning.  */

	nextOut = output = new float[obuflen];
	bzero((char*) output, sizeof(float) * obuflen);

	/* set up analysis buffer for (fftSize/2 + 1) channels: The input is real,
		so the other channels are redundant. oldInPhase is used
		in the conversion to remember the previous phase when
		calculating phase difference between successive samples. */

	anal = new float[analysisChannels()];

	// allocate and zero these
	
	int fb = freqBands();
	oldInPhase = new float[fb];
	bzero((char*) oldInPhase, sizeof(float) * fb);
	maxAmp = new float[fb];
	bzero((char*) maxAmp, sizeof(float) * fb);
	avgAmp = new float[fb];
	bzero((char*) avgAmp, sizeof(float) * fb);
	avgFrq = new float[fb];
	bzero((char*) avgFrq, sizeof(float) * fb);
	env = new float[fb];
	bzero((char*) env, sizeof(float) * fb);

	/* set up synthesis buffer for (fftSize/2 + 1) channels: (This is included
		only for clarity.)  oldOutPhase is used in the re-
		conversion to accumulate angle differences (actually angle
		difference per second). */

	syn = new float[invFftSize+2];

	oldOutPhase = new float[invFftPoints + 1];
	bzero((char*) oldOutPhase, sizeof(float) * (invFftPoints + 1));
}

void
PhaseVocoder::updateAmps() {
	ftot++;

	for (int i = 0; i <= fftPoints; i++){
		if (*(anal+(i<<1)) > *(maxAmp+i))
			*(maxAmp+i) = *(anal+(i<<1));
		*(avgAmp + i) += *(anal + (i<<1));
		*(avgFrq + i) += *(anal + (i<<1) + 1);
	}
}

int
PhaseVocoder::runAnalysis(double* in, Data* frame) {
	applyInputWindow(in);
	analyze();
	convertToReal();
	detectAndWarpEnvelope();
	
//	if (verbose) updateAmps();
	outputAnalysis(frame);
	getTimeScaleFactor();
	calculateOffsetsAndIncrement();
	return true;
}

/*
	analysis: The analysis subroutine computes the complex output at
	time n of (fftSize/2 + 1) of the phase vocoder channels.  It operates
	on input samples (n - analWinLen) thru (n + analWinLen) and
	expects to find these in input[0 - 2*analWinLen)]. 
	This subroutine expects analWindow to point to the center of a
	symmetric window of length (2 * analWinLen +1).  It is the
	responsibility of the main program to ensure that these values
	are correct!  The results are returned in anal as succesive
	pairs of real and imaginary values for the lowest (fftSize/2 + 1)
	channels.   The subroutines fft and reals together implement
	one efficient FFT call for a real input sequence.
*/

void
PhaseVocoder::applyInputWindow(double* input) {
	int i = 0;
	
	for (i = 0; i < analysisChannels(); i++)	// initialize
		*(anal + i) = 0.;

	int k = inSamp - analWinLen - 1;			// time shift
	while (k < 0)								// get rotated pointer
		k += I.fftSize;
	k = k % I.fftSize;

	for (i = -analWinLen; i <= analWinLen; i++) {
		if (++k >= I.fftSize)
			k = 0;
		*(anal + k) += *(analWindow + i) * *input++;
	}
}

// extern void rfft(float*, int, int);

void
PhaseVocoder::analyze() {
 	register float* banal = anal + 1;
 	int one = 1;
 	int negTwo = -2;
 	fft_(anal,banal,&one,&fftPoints,&one,&negTwo);
 	reals_(anal,banal,&fftPoints,&negTwo);
//	rfft(anal, fftPoints, true);
}

void
PhaseVocoder::convertToReal() {

	/*
	conversion: The real and imaginary values in anal are converted to
	magnitude and angle-difference-per-second (assuming an 
	intermediate sampling rate of inputFrameRate) and are returned in anal.
	*/

	register float* realptr = anal;
	register float* imagptr = anal + 1;
	register float* phaseptr = oldInPhase;

	for (int i = 0; i <= fftPoints;
			i++, realptr += 2, imagptr += 2, phaseptr++) {
		float real = *realptr;
		float imag = *imagptr;
		float angleDif = 0;
		*realptr = hypot(real,imag);
		
		if (*realptr == 0.)
			angleDif = 0.;
		else {
			float phase = 0;
			angleDif = (phase = atan2(imag,real)) - *phaseptr;
			*phaseptr = phase;
		}
	
		// unwrap phase differences

		while(angleDif > Pi)
			angleDif -= TwoPi;
		while(angleDif < -Pi)
			angleDif += TwoPi;

		// convert each phase difference to Hz

		*imagptr = angleDif * RoverTwoPi + (i * float(I.fundFreq));
	}
}

void
PhaseVocoder::outputAnalysis(Data* frame) {
	if(E)
		frame->setFrame(env, fftPoints);			// spectral envelope
	else if(X)
		for (int i=0; i <= fftPoints; i++)			// magnitudes only
			frame->set(*(anal + (i<<1)), 0, i);
	else
		frame->setFrame(anal, analysisChannels());	// pvoc analysis data
}

void
PhaseVocoder::detectAndWarpEnvelope() {

	/* 
	spectral envelope detection: this is a very crude peak picking algorithm
	which is used to detect and pre-warp the spectral envelope so that
	pitch transposition can be performed without altering timbre.
	The basic idea is to disallow large negative slopes between
	successive values of magnitude vs. frequency.
	*/

	if (I.warp != 0.){

		float lastmag = *anal;
		float mag = *(anal + 2);
		float nextmag = 0;
		float eps = -64. / I.fftSize;
		float pkOld = lastmag;
		*env = pkOld;
		int pkcnt = 1;
		float slope = 0;
		int i = 0;

		for (i = 1; i <= fftPoints; i++) {			// step thru spectrum

			if (i<fftPoints)
				nextmag = *(anal + (i<<1) + 2);
			else nextmag = 0.;

			if (pkOld != 0.)
				slope = (float(mag - pkOld)/(pkOld * pkcnt));
			else
				slope = -10.;

													// look for peaks

			if ((mag>=lastmag)&&(mag>nextmag)&&(slope>eps)) {
				*(env + i) = mag;
				pkcnt--;
				for (int j = 1; j <= pkcnt; j++)
					//*(env + i - pkcnt + j - 1) = pkOld * (1. + slope * j);
					env[i - pkcnt + j - 1] = pkOld * (1. + slope * j);
				pkOld = mag;
				pkcnt = 1;
			}	
			else pkcnt++;							// not a peak

			lastmag = mag;
			mag = nextmag;
		}

		if (pkcnt > 1) {							// get final peak
			mag = *(anal + I.fftSize);
			slope = (float(mag - pkOld) / pkcnt);
			*(env + fftPoints) = mag;
			pkcnt--;
			for (int j = 1; j <= pkcnt; j++)
				//*(env + fftPoints - pkcnt + j - 1) = pkOld + slope * j;
				env[fftPoints - pkcnt + j - 1] = pkOld + slope * j;
		}

		for (i = 0; i <= fftPoints; i++) {			// warp spectral env.
			int j = int(i * I.warp);
			if ((j <= fftPoints) && (*(env + i) != 0.))
				*(anal + (i<<1)) *= *(env + j) / *(env + i);
			else
				*(anal + (i<<1)) = 0.;
		}
	}
}

int
PhaseVocoder::runSynthesis(Data* frame, InPipeAction* pipe) {
	BUG("PhaseVocoder::runSynthesis");
	loadAnalysis(frame);
	limitBands();
	detectAndWarpEnvelope();	// try this here!!
	convertFromReal();
	synthesize();
	applyOutputWindow();
	int status = shiftOut(pipe);
	getTimeScaleFactor();
	calculateOffsetsAndIncrement();
	return status;
}

void
PhaseVocoder::loadAnalysis(Data* frame) {
	BUG("PhaseVocoder::loadAnalysis");
	frame->getFrame(anal, analysisChannels());
}

void
PhaseVocoder::limitBands() {
	/* resynthesize only selected channels */
	if (bandsLimited){
		int i;
		for (i = 0; i < I.firstBand; i++)
			*(anal+(i<<1)) = 0.;
		for (i = I.lastBand+1; i <= fftPoints; i++)
			*(anal+(i<<1)) = 0.;
		if (C == 1)
			for (i = I.firstBand; i <= I.lastBand; i++)
				if (i%2 == 0)
					*(anal+(i<<1)) = 0.;
		if (C == 2)
			for (i = I.firstBand; i <= I.lastBand; i++)
				if (i%2 != 0)
					*(anal+(i<<1)) = 0.;
	}
}

void
PhaseVocoder::convertFromReal() {
	/*
	reconversion: The magnitude and angle-difference-per-second in syn
	(assuming an intermediate sampling rate of outputFrameRate) are
	converted to real and imaginary values and are returned in syn.
	This automatically incorporates the proper phase scaling for
	time modifications.
	*/

	int i = 0;
	
	if (invFftSize <= I.fftSize){
		for (i = 0; i < invFftSize+2; i++)
			*(syn+i) = *(anal+i);
	}
	else {
		for (i = 0; i <= I.fftSize+1; i++)
			*(syn+i) = *(anal+i);
		for (i = analysisChannels(); i < invFftSize+2; i++)
			*(syn+i) = 0.;
	}

	register float* i0 = syn;
	register float* i1 = syn + 1;

	for (i = 0; i <= invFftPoints; i++, i0 += 2, i1 += 2) {
		float mag = *i0;
		*(oldOutPhase + i) += *i1 - ( i * float(I.fundFreq));
		float phase = *(oldOutPhase + i) * TwoPioverR;
		*i0 = mag * cos(phase);
		*i1 = mag * sin(phase);
	}

	// scale values by pitch factor if present
	
	if (I.pchScaleFactor != 1.) {
		float Pinv = 1.0 / I.pchScaleFactor;
		for (i = 0; i < invFftSize+2; i++)
			*(syn+i) *= Pinv;
	}

}

/*
	synthesis: The synthesis subroutine uses the Weighted Overlap-Add
	technique to reconstruct the time-domain signal.  The (fftSize/2 + 1)
	phase vocoder channel outputs at time n are inverse Fourier
	transformed, windowed, and added into the output array.  The
	subroutine thinks of output as a shift register in which 
	locations are referenced modulo obuflen.  Therefore, the main
	program must take care to zero each location which it "shifts"
	out (to standard output). The subroutines reals and fft
	together perform an efficient inverse FFT.
*/

void
PhaseVocoder::synthesize() {
	register float* bsyn = syn + 1;
	int one = 1;
	int two = 2;
	reals_(syn,bsyn,&invFftPoints,&two);
	fft_(syn,bsyn,&one,&invFftPoints,&one,&two);
}

void
PhaseVocoder::applyOutputWindow() {
	BUG("PhaseVocoder::applyOutputWindow");
	// set up pointers to current locations in syn and output
	
	int j = outSamp - synWinLen - 1;
	while (j < 0)
		j += obuflen;
	j = j % obuflen;

	int k = outSamp - synWinLen - 1;
	while (k < 0)
		k += invFftSize;
	k = k % invFftSize;

	// overlap-add windowed synth buffer into output array, wrapping pointers
	// as required
	
	for (int i = -synWinLen; i <= synWinLen; i++) {
		if (++j >= obuflen)
			j -= obuflen;
		if (++k >= invFftSize)
			k -= invFftSize;
		*(output + j) += *(syn + k) * *(synWindow + i);
	}
}

int
PhaseVocoder::shiftOut(InPipeAction* pipe) {
	BUG("PhaseVocoder::shiftOut");
	// shift out next sampsOut values
	pipe->ref();
	int keepGoing = true;
	for (int i = 0; i < sampsOut && keepGoing; i++) {
		keepGoing = pipe->add(*nextOut);
		*(nextOut++) = 0.;
		if (nextOut >= (output + obuflen))
			nextOut -= obuflen;
		outCount++;
	}
	Resource::unref(pipe);
	return keepGoing;
}

void
PhaseVocoder::getTimeScaleFactor() {

	// time-varying time-scaling: get linearly interpolated time values
	// from envelope object

	if (variableTimeScaling() && inSamp > 0) { // if window midpt. is positive
		I.timeScaleFactor = timeScaleEnvelope->next();
		if (I.timeScaleFactor < (8.0 * I.outputFrameOffset / I.inputFrameSize)) {
			char msg[64];
			sprintf(msg, "Cannot contract by time scaling %f.",
				I.timeScaleFactor);
			Application::inform(msg, true);		// display for 1 second
			I.timeScaleFactor = (8.0 * I.outputFrameOffset / (I.inputFrameSize + 1));
		}
		I.inputFrameOffset = int(I.outputFrameOffset / I.timeScaleFactor);
		if (I.inputFrameOffset < 1) {
			char msg[64];
			sprintf(msg,"Cannot expand by time scaling %f.", I.timeScaleFactor);
			Application::inform(msg, true);		// display for 1 second
			I.inputFrameOffset = 1;
		}
		I.timeScaleFactor = (float(I.outputFrameOffset) / I.inputFrameOffset);
		float inputFrameRate = (I.samplingRate / I.inputFrameOffset);
		RoverTwoPi = inputFrameRate / TwoPi;
		if (I.warp != 0.)
			I.warp = I.timeScaleFactor;
	}
}

void
PhaseVocoder::calculateOffsetsAndIncrement() {

/*	I.inputFrameOffset = some_function(inSamp);	for variable time-scaling */
/*	float inputFrameRate = (I.samplingRate / I.inputFrameOffset); for variable time-scaling */
/*	RoverTwoPi =  inputFrameRate / TwoPi;	for variable time-scaling */

	inSamp += I.inputFrameOffset;				/* increment time */
	outSamp += scaledOutputOffset;

	sampsIn = I.inputFrameOffset;	// always, after first time

	float Ii = 0.0;
	if (outSamp > (synWinLen + I.outputFrameOffset))
		Ii = I.outputFrameOffset;
	else if (outSamp > synWinLen)
		Ii = outSamp - synWinLen;
	else {
		Ii = 0;
		for (int i=outSamp+synWinLen; i<obuflen; i++)
			if (i > 0)
				*(output+i) = 0.;
	}
	sampsOut = int(Ii / I.pchScaleFactor);
}

int
PhaseVocoder::calculateAnalysisLength(int inputLen) {
	return max(1,
		1 + (inputLen + I.inputFrameSize-startingOffset) / I.inputFrameOffset
	);
}