/* Pitch_to_PointProcess.c */
/* Paul Boersma, March 25, 2001 */

#include "Pitch_to_PointProcess.h"
#include "PitchTier_to_PointProcess.h"
#include "Pitch_to_PitchTier.h"
#include <ctype.h>

PointProcess Pitch_to_PointProcess (Pitch pitch) {
	PitchTier pitchTier = Pitch_to_PitchTier (pitch);
	PointProcess point;
	if (! pitchTier) return NULL;
	if (! (point = PitchTier_Pitch_to_PointProcess (pitchTier, pitch)))
		{ forget (pitchTier); return NULL; }
	return point;
}

static int Pitch_getVoicedIntervalAfter (Pitch me, double after, double *tleft, double *tright) {
	long ileft = Sampled_xToHighIndex (me, after), iright;
	if (ileft > my nx) return 0;   /* Offright. */
	if (ileft < 1) ileft = 1;   /* Offleft. */

	/* Search for first voiced frame. */
	for (; ileft <= my nx; ileft ++)
		if (Pitch_isVoiced_i (me, ileft)) break;
	if (ileft > my nx) return 0;   /* Offright. */

	/* Search for last voiced frame. */
	for (iright = ileft; iright <= my nx; iright ++)
		if (! Pitch_isVoiced_i (me, iright)) break;
	iright --;

	*tleft = Sampled_indexToX (me, ileft) - 0.5 * my dx;   /* The whole frame is considered voiced. */
	*tright = Sampled_indexToX (me, iright) + 0.5 * my dx;
	return 1;
}

static double NUMfindAbsoluteExtremum_3 (float *x, long n) {
	double extremum = 0.0;
	long imax = 0, i;
	if (n < 3) {
		if (n <= 0) return 0.0;   /* Outside. */
		else if (n == 1) return 1.0;
		else {   /* n == 2 */
			double xleft = fabs (x [1]), xright = fabs (x [2]);
			if (xleft > xright) return 1.0;
			else if (xleft < xright) return 2.0;
			else return 1.5;
		}
	}
	for (i = 1; i <= n; i ++)
		if (fabs (x [i]) > extremum) { extremum = fabs (x [i]); imax = i; }
	if (imax == 0) return 0.5 * (n + 1.0);   /* All zeroes. */
	else if (imax == 1) return 1.0;
	else if (imax == n) return (double) n;
	else {
		/* Parabolic interpolation. */
		/* We do NOT need fabs here: we look for a genuine extremum. */
		return imax + 0.5 * (x [imax + 1] - x [imax - 1]) / (2 * x [imax] - x [imax - 1] - x [imax + 1]);
	}
}

static double Sound_findExtremum (Sound me, double tmin, double tmax) {
	long imin = Sampled_xToLowIndex (me, tmin), imax = Sampled_xToHighIndex (me, tmax);
	double iextremum;
	Melder_assert (NUMdefined (tmin));
	Melder_assert (NUMdefined (tmax));
	if (imin < 1) imin = 1;
	if (imax > my nx) imax = my nx;
	iextremum = NUMfindAbsoluteExtremum_3 (my z [1] + imin - 1, imax - imin + 1);
	if (iextremum)
		return my x1 + (imin - 1 + iextremum - 1) * my dx;
	else
		return (tmin + tmax) / 2;
}

static double Sound_findMaximumCorrelation (Sound me, double t1, double windowLength, double tmin2, double tmax2, double *tout) {
	double maximumCorrelation = -1.0, r1 = 0.0, r2 = 0.0, r3 = 0.0, ir;
	double halfWindowLength = 0.5 * windowLength;
	long i1, i2, ileft2;
	long ileft1 = Sampled_xToNearestIndex ((Sampled) me, t1 - halfWindowLength);
	long iright1 = Sampled_xToNearestIndex ((Sampled) me, t1 + halfWindowLength);
	long ileft2min = Sampled_xToLowIndex ((Sampled) me, tmin2 - halfWindowLength);
	long ileft2max = Sampled_xToHighIndex ((Sampled) me, tmax2 - halfWindowLength);
	float *amp = my z [1];
	for (ileft2 = ileft2min; ileft2 <= ileft2max; ileft2 ++) {
		double norm1 = 0.0, norm2 = 0.0, product = 0.0;
		for (i1 = ileft1, i2 = ileft2; i1 <= iright1; i1 ++, i2 ++) {
			if (i1 < 1 || i1 > my nx || i2 < 1 || i2 > my nx) continue;
			norm1 += amp [i1] * amp [i1];
			norm2 += amp [i2] * amp [i2];
			product += amp [i1] * amp [i2];
		}
		r1 = r2;
		r2 = r3;
		r3 = product ? product / (sqrt (norm1 * norm2)) : 0.0;
		if (r2 > maximumCorrelation && r2 >= r1 && r2 >= r3) {
			double dr = 0.5 * (r3 - r1), d2r = 2 * r2 - r1 - r3;
			ir = (ileft2 - 1) + (d2r ? dr / d2r : 0);
			maximumCorrelation = r2 + (d2r ? 0.5 * dr * dr / d2r : 0);
			*tout = t1 + (ir - ileft1) * my dx;
		}
	}
	return maximumCorrelation;
}

PointProcess Sound_Pitch_to_PointProcess (Sound sound, Pitch pitch) {
	PointProcess point = PointProcess_create (sound -> xmin, sound -> xmax, 10);
	double t = pitch -> xmin;
	double addedRight = -1e30;
	/*
	 * Cycle over all voiced intervals.
	 */

	for (;;) {
		double tleft, tright, tmiddle, f0middle, tmax, tsave;
		if (! Pitch_getVoicedIntervalAfter (pitch, t, & tleft, & tright)) break;
		/*
		 * Go to the middle of the voiced interval.
		 */
		tmiddle = (tleft + tright) / 2;
		if (! Melder_progress ((tmiddle - sound -> xmin) / (sound -> xmax - sound -> xmin), "Sound & Pitch to PointProcess"))
			goto error;
		f0middle = Pitch_getFrequency_t (pitch, tmiddle);

		/*
		 * Our first point is near this middle.
		 */
		Melder_assert (f0middle != 0.0);
		tmax = Sound_findExtremum (sound, tmiddle - 0.5 / f0middle, tmiddle + 0.5 / f0middle);
		Melder_assert (NUMdefined (tmax));
		if (! PointProcess_addPoint (point, tmax)) goto error;

		tsave = tmax;
		for (;;) {
			double f0 = Pitch_getFrequency_t (pitch, tmax), correlation;
			if (! f0) break;
			correlation = Sound_findMaximumCorrelation (sound, tmax, 1.0 / f0, tmax - 1.2 / f0, tmax - 0.8 / f0, & tmax);
			if (correlation == -1) /*break*/ tmax -= 1.0 / f0;   /* This one period will drop out. */
			if (tmax < tleft) {
				if (correlation > 0.7 && tmax - addedRight > 0.8 / f0)
					if (! PointProcess_addPoint (point, tmax)) goto error;
				break;
			}
			if (correlation > 0.3) {
				if (tmax - addedRight > 0.8 / f0)   /* Do not fill in a short originally unvoiced interval twice. */
					if (! PointProcess_addPoint (point, tmax)) goto error;
			}
		}
		tmax = tsave;
		for (;;) {
			double f0 = Pitch_getFrequency_t (pitch, tmax), correlation;
			if (! f0) break;
			correlation = Sound_findMaximumCorrelation (sound, tmax, 1.0 / f0, tmax + 0.8 / f0, tmax + 1.2 / f0, & tmax);
			if (correlation == -1) /*break*/ tmax += 1.0 / f0;
			if (tmax > tright) {
				if (correlation > 0.7) {
					if (! PointProcess_addPoint (point, tmax)) goto error;
					addedRight = tmax;
				}
				break;
			}
			if (correlation > 0.3) {
				if (! PointProcess_addPoint (point, tmax)) goto error;
				addedRight = tmax;
			}
		}
		t = tright;
	}
	Melder_progress (1.0, NULL);
	return point;
error:
	Melder_progress (1.0, NULL);
	forget (point);
	return NULL;
}
	
/* End of file Pitch_to_PointProcess.c */
