// lpcdata.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 "localdefs.h"
#include "application.h"
#include "controller.h"
#include "envelope.h"
#include "header.h"
#include "lpcdata.h"
#include "request.h"
#include <Complex.h>
#include <stream.h>

// class data initialization

int LPCData::default_NumberOfPoles = 34;
double LPCData::default_FrameRate = 220.5;	// (44100 / 200)

// public methods

LPCData::LPCData(int length, int npoles, int srate, double frate)
	: FrameData(FloatData, length,
		(npoles ? npoles : defaultNumberOfPoles())+4,
		srate ? srate : 44100,
		frate ? frate : defaultFrameRate()) {
}

Data *
LPCData::newData() { return new LPCData(this); }

Data *
LPCData::newData(int length) { return new LPCData(this, length); }

Data *
LPCData::clone(const Range &r) { return new LPCData(this, r); }

Data *
LPCData::clone(const Range &r, const Range &c) {
	return new LPCData(this, r, c);
}

// number of poles is always 4 less than the real nchans for the underlying
// object (independent of cloning)

int
LPCData::nPoles() const { return rep->realChannels() - 4; }

void
LPCData::print(FILE *out) {
	int nframes = length();
	int chans = channels();
	fprintf(out, "LPC Data Dump\n");
	for(int i = 0; i < nframes; ++i) {
		fprintf(out, "Frame %d:  RMS1: %0.5f RMS2: %0.5f ERROR: %f PTCH: %0.2f\n",
		i, get(i, 0), get(i, 1), get(i, 2), get(i, 3));
		fprintf(out, "Coeffs:");
		for(int j = 4; j < chans; ++j) {
			if(!((j-4) % 8)) fprintf(out, "\n");
			fprintf(out, "%f ", get(i, j));
		}
		fprintf(out, "\n\n");
	}
	fflush(out);
}

void
LPCData::information(Controller *controller) {
	AlertRequest request("LPC Data Information:");
	request.setBell(false);
	char str[128];
	request.appendLabel("------------");
	request.appendLabel("Filename: ", controller->fileName());
	request.appendLabel("Sample Rate: ", toString(str, sRate()));
	request.appendLabel("Length (frames): ",
		toString(str, length()));
	request.appendLabel("Original Duration (seconds): ",
		toString(str, duration()));
	request.appendLabel("File Size (bytes): ",
		toString(str, sizeInBytes()));
	request.appendLabel("NPoles: ", toString(str, nPoles()));
	request.appendLabel("Frame Rate: ", toString(str, frameRate()));
	controller->handleRequest(request);
}

const char *
LPCData::channelName(int chan) const {
	static char *chan_names[4] =
		{ "Resid. RMS", "Signal RMS", "Error", "Freq. in Hz" };
	static char string[16];
	char *label;
	if(chan < 4) label = chan_names[chan];
	else {
		sprintf(string, "Coeff %d", chan - 4);
		label = string;
	}
	return label;
}

// returns 0 - max for first 3 bands, and normal range for other bands

Range
LPCData::limits(int chan, boolean real) const {
	Range rangeLimits = Super::limits(chan, real);
	if (chan > 2) 
		return rangeLimits;
	else
		return Range(0.0, rangeLimits.absoluteMax());
}

// protected methods

Header *
LPCData::createHeader(DataFile *, boolean reading) {
	LPCHeader *h = new LPCHeader(nPoles(), frameRate(), sRate(), duration());
	if(!reading) configureHeader(h);
	return h;
}

void
LPCData::mergePitchData(Data *pitches) {
	Envelope pchEnv(nFrames());
	static const int pitchChannel = 0;		// first channel is pitch data...
	pitches->getEnvelope(&pchEnv, pitchChannel, AbsoluteMagnitude);
	replaceWith(&pchEnv);
}

void
LPCData::stabilizeFrames() {
	for(int framenum = 0; framenum < length(); ++framenum) {
		Data* frameData = cloneFrame(framenum);
		frameData->ref();
		if(!stabilize(frameData)) {
			char report[32];
			sprintf(report, "Stabilized frame %d",framenum);
			Application::inform(report);
		}
		Resource::unref(frameData);
	}
	Notify();
}

int
LPCData::stabilize(Data* frameData) {
	static const int arraySize = LPCHeader::maxPoles + 4;
	static const int offset = 4;	// coeffs start at loc 4 in frame
	MXINT32 stable=0;
	MXINT32 npoles = nPoles();
	float frameIn[arraySize];
	frameData->getArray(frameIn, channels());
	float frameOut[arraySize];
	for(int i=0; i<npoles; i++)
		frameOut[i] = -frameIn[npoles+3-i];
	if(!(stable = isStable(frameOut, npoles))) {
		correct(frameIn, npoles, frameOut);
		((LPCData *)frameData)->setFromArray(frameOut, int(npoles), 0, offset); 
	}
	return stable;
}

double
LPCData::weightedAveragePitch(double threshold) const {
	double totalWeight = .001;
	double sum = .001;
	for(int frame = 0; frame < length(); frame++) {
		if(threshold < 0.0 || (get(frame, Error_Loc) <= threshold)) {
			double rms_amp = get(frame, RMS_Loc);
			totalWeight += rms_amp;
			sum += (get(frame, Pitch_Loc) * rms_amp);
		}
	}
	return (sum/totalWeight);
}

double
LPCData::averagePitchDeviation(double threshold) const {
	double weight = weightedAveragePitch(threshold);
	double sum = 0.0;
	double xweight = 0.0;
	for(int frame = 0; frame < length(); frame++) {
		if(threshold < 0.0 || get(frame, Error_Loc) <= threshold) {
			double rms_amp = get(frame, RMS_Loc);
			sum += abs((get(frame, Pitch_Loc) - weight)) * rms_amp;
			xweight += rms_amp;
		}
	}
	return (xweight > 0.0) ? (sum/xweight) : 0.0;
}

// this is new version of readjust() and adjust() from old LPC code

void
LPCData::scalePitchDeviation(double newDeviation, double threshold, double pitchCutoff) {
	double devFactor =
		newDeviation / max(0.001, averagePitchDeviation(threshold));
	if(devFactor == 1.0)
		return;
	double pitchAverage = weightedAveragePitch(threshold);
	for(int frame = 0; frame < length(); frame++) {
		double oldPitch = get(frame, Pitch_Loc);
		double newPitch = (oldPitch - pitchAverage) * devFactor + pitchAverage;
		if(newPitch > pitchCutoff)
			set(newPitch, frame, Pitch_Loc);
	}
	Notify();
}

// the remaining functions are UGLY because they were converted from Fortran

static Complex jay(0., 1.);
static Complex tmp;

const int LPCData::ArrSize = LPCHeader::maxPoles + 3;	// who knows what size??

int
LPCData::factor(double *b, MXINT32 *k4, double *rootr, double *rooti,
		MXINT32 *kinsid, MXINT32 *kprint, double *eps) {
    /* System generated locals */
    MXINT32 i_1, i_2, i_3;

    /* Local variables */
    static double amax;
    static MXINT32 kerr;
    static double dist, rmin, rmax;
    static MXINT32 i, j, k;
    static double r;
    static Complex z, res;
    static double parti, distr, partr, r2, resmag, resmax;
    static MXINT32 k4m;
    static double coe[ArrSize];

#ifdef LPC_DEBUG
	printf("in factor npoles = %d\n",*k4);
	for(i=0; i<36; i++) printf(" %g ",b[i]);
#endif
    /* Parameter adjustments */
    --b;
    --rootr;
    --rooti;
    /* Function Body */
/*       sets up problem, calls dproot, */
/*       and checks residual values at roots */
    i_1 = *k4;
    for (i = 1; i <= i_1; ++i) {
/* L550: */
		coe[i - 1] = b[i];
    }
    k4m = *k4 - 1;
    dproot(&k4m, coe, &rootr[1], &rooti[1], &kerr, kprint, eps);
    if (kerr > 0) {
    	return 0;
    }
    *kinsid = 0;
    resmax = 0.;
    rmax = 0.;
    rmin = 4294967296.;
    dist = 4294967296.;
    amax = 4294967296.;
    r2 = pow(amax, 1.0 / *k4);
    i_1 = k4m;
    for (j = 1; j <= i_1; ++j) {
		i_2 = j;
		i_3 = j;
		z = rootr[i_2] + (jay * rooti[i_3]);
	/* Computing 2nd power */
		r = hypot(rootr[j], rooti[j]);
	/*         skip residue calculation if root is too big */
		if (r > r2) {
			goto L713;
		}
		i_2 = *k4;
		res = b[i_2];
		i_2 = *k4;
		for (k = 2; k <= i_2; ++k) {
			i_3 = *k4 - k + 1;
			res = (res * z) + b[i_3];
		}
		partr = res.real();
		tmp = -jay * res;
		parti = tmp.real();
	/* Computing 2nd power */
		resmag = hypot(partr, parti);
		if (resmax <= resmag) {
			resmax = resmag;
		}
	L713:
		if (rmax < r) {
			rmax = r;
		}
		if (rmin > r) {
			rmin = r;
		}
		if (r < 1.) {
			++(*kinsid);
		}
		distr = abs(r - 1.);
		if (dist > distr) {
			dist = distr;
		}
    }
    return 1;
} /* factor */

int
LPCData::dproot(MXINT32 *mm, double *a, double *rootr, double *rooti, MXINT32 *kerr,
		MXINT32 *kprint, double *eps) {
    /* System generated locals */
    MXINT32 i_1, i_2, i_3, i_4;

    /* Local variables */
    static MXINT32 mdec;
    static double amin, amax, save[ArrSize];
    static MXINT32 kmax, kpol;
    static double temp, size;
    static MXINT32 ktry;
    static double real1, real2;
    static Complex b[ArrSize], c[ArrSize];
    static Complex p, w, z;
    static double parti;
    static MXINT32 kpolm;
    static double partr;
    static MXINT32 kount, newst, nroot;
    static double r1, r2;
    static MXINT32 ktrym;
    static Complex bb[ArrSize], cc[ArrSize];
    static MXINT32 mp;
    static Complex pp;
    static double sqteps, rkount, rr1, rr2;
    static MXINT32 mmp;

    int i, j, k, m;
	
    /* Parameter adjustments */
    --a;
    --rootr;
    --rooti;

    /* Function Body */
/*        mm=degree of polynomial */
/*        a=coefficient array, lowest to highest degree */
/*        kprint=1 for full printing */
/*        kerr=0 is normal return */
    mmp = *mm + 1;
    m = *mm;
    mp = mmp;
    i_1 = mp;
    for (i = 1; i <= i_1; ++i) {
	save[i - 1] = a[i];
    }
/*       kount is number of iterations so far */
    kount = 0;
/*       kmax is maximum total number of iterations allowed */
    kmax = m * 20;
/*       newst is number of re-starts */
    newst = 0;
/*       ktrym is number of attempted iterations before re-starting */
    ktrym = 20;
/*      kpolm is number of attempted iterations before polishing is stopped */
    kpolm = 20;
/*       amax is the largest number we allow */
    amax = 4294967296.;
    amin = 1. / amax;
/*       rr1 and rr2 are radii within which we work for polishing */
    rr1 = pow(amin, 1. / m);
    rr2 = pow(amax, 1. / m);
/*     eps is the tolerance for convergence */
    sqteps = sqrt(*eps);
/*        main loop; m is current degree */
L10:
    if (m <= 0) {
	goto L200;
    }
/*        new z, a point on the unit circle */
    rkount = double(kount);
    z = (jay * sin(rkount)) + cos(rkount);
    ktry = 0;
/*      r1 and r2 are boundaries of an expanding annulus within which we 
work*/
    r1 = pow(amin, 1. / m);
    r2 = pow(amax, 1. / m);
/*        inside loop */
L20:
    partr = z.real();
    tmp = -jay * z;
    parti = tmp.real();
/* Computing 2nd power */
    size = hypot(partr, parti);
    if (size < r1 || size > r2) {
	goto L300;
    }
    if (ktry >= ktrym) {
	goto L300;
    }
    ++ktry;
    if (kount >= kmax) {
	goto L400;
    }
    ++kount;
/*        get value of polynomial at z, synthetic division */
    i_1 = mp - 1;
    i_2 = mp;
    b[i_1] = a[i_2];
    i_1 = m;
    for (j = 1; j <= i_1; ++j) {
		k = m - j + 1;
		i_2 = k - 1;
		i_3 = k;
		i_4 = k;
		b[i_2] = (z * b[i_3]) + a[i_4];
    }
    p = b[0];
    partr = p.real();
    tmp = -jay * p;
    parti = tmp.real();
/* Computing 2nd power */
    if (hypot(partr, parti) > amax) {
	goto L300;
    }
/*        get value of derivative at z, synthetic division */
    i_2 = mp - 1;
    i_3 = mp - 1;
    c[i_2] = b[i_3];
    mdec = m - 1;
    i_2 = mdec;
    for (j = 1; j <= i_2; ++j) {
		k = m - j + 1;
		i_3 = k - 1;
		i_4 = k;
		i_1 = k - 1;
		c[i_3] = (z * c[i_4]) + b[i_1];
    }
    pp = c[1];
    partr = pp.real();
    tmp = -jay * pp;
    parti = tmp.real();
/* Computing 2nd power */
    if (hypot(partr, parti) < amin) {
	goto L300;
    }
/*        test for convergence */
    partr = p.real();
    tmp = -jay * p;
    parti = tmp.real();
/* Computing 2nd power */
    size = hypot(partr, parti);
    if (size > *eps) {
	goto L775;
    }
    nroot = *mm - m + 1;
    goto L500;
L775:
    z = z - (p / pp);
    goto L20;
/*        end of main loop */
/*        normal return */
L200:
    *kerr = 0;
    goto L600;
/*        new start */
L300:
    rkount = double(kount);
    z = (jay * sin(rkount)) + cos(rkount);
    ktry = 0;
    ++newst;
    goto L20;
/*        too many iterations */
L400:
    *kerr = 400;
    goto L600;
/*        root z located */
/*        polish z to get w */
L500:
    w = z;
    kpol = 0;
L510:
    partr = w.real();
    tmp = -jay * w;
    parti = tmp.real();
/* Computing 2nd power */
    size = hypot(partr, parti);
/*       give up polishing if w is outside annulus */
    if (size < rr1 || size > rr2) {
	goto L501;
    }
/*       give up polishing if kpol>=kpolm */
    if (kpol >= kpolm) {
	goto L501;
    }
    ++kpol;
    if (kount >= kmax) {
	goto L400;
    }
    ++kount;
    i_3 = mmp - 1;
    i_4 = mmp - 1;
    bb[i_3] = save[i_4];
    i_3 = *mm;
    for (j = 1; j <= i_3; ++j) {
		k = *mm - j + 1;
		i_4 = k - 1;
		i_1 = k;
		i_2 = k - 1;
		bb[i_4] = (w * bb[i_1]) + save[i_2];
    }
    p = bb[0];
    partr = p.real();
    tmp = -jay * p;
    parti = tmp.real();
/* Computing 2nd power */
    if (hypot(partr, parti) > amax) {
	goto L300;
    }
    i_4 = mmp - 1;
    i_1 = mmp - 1;
    cc[i_4] = bb[i_1];
    mdec = *mm - 1;
    i_4 = mdec;
    for (j = 1; j <= i_4; ++j) {
		k = *mm - j + 1;
		i_1 = k - 1;
		i_2 = k;
		i_3 = k - 1;
		cc[i_1] = (w * cc[i_2]) + bb[i_3];
    }
    pp = cc[1];
    partr = pp.real();
    tmp = -jay * pp;
    parti = tmp.real();
/* Computing 2nd power */
    if (hypot(partr, parti) < amin) {
	goto L300;
    }
    partr = p.real();
    tmp = -jay * p;
    parti = tmp.real();
/* Computing 2nd power */
    size = hypot(partr, parti);
/*       test for convergence of polishing */
    if (size <= *eps) {
	goto L501;
    }
    w = w - (p / pp);
    goto L510;
/*        deflate */
L501:
    i_1 = mp - 1;
    i_2 = mp;
    b[i_1] = a[i_2];
    i_1 = m;
    for (j = 1; j <= i_1; ++j) {
		k = m - j + 1;
		i_2 = k - 1;
		i_3 = k;
		i_4 = k;
		b[i_2] = (z * b[i_3]) + a[i_4];
    }
    p = b[0];
    i_2 = m;
    rootr[i_2] = w.real();
    i_2 = m;
    tmp = -jay * w;
    rooti[i_2] = tmp.real();
    --m;
    --mp;
    tmp = -jay * w;
    parti = tmp.real();
    if (abs(parti) > sqteps) {
	goto L140;
    }
/*        real root */
    rooti[m + 1] = 0.;
    i_2 = mp;
    for (j = 1; j <= i_2; ++j) {
	i_3 = j;
	i_4 = j;
	a[i_3] = b[i_4].real();
    }
    goto L10;
/*        complex root */
L140:
    partr = z.real();
    tmp = -jay * z;
    parti = tmp.real();
    z = partr - (parti * jay);
    i_3 = mp - 1;
    i_4 = mp;
    c[i_3] = b[i_4];
    i_3 = m;
    for (j = 1; j <= i_3; ++j) {
		k = m - j + 1;
		i_4 = k - 1;
		i_2 = k;
		i_1 = k;
		c[i_4] = (z * c[i_2]) + b[i_1];
    }
    i_4 = m;
    rootr[i_4] = w.real();
    i_4 = m;
    tmp = -(-jay * w);
    rooti[i_4] = tmp.real();
    --m;
    --mp;
    i_4 = mp;
    for (j = 1; j <= i_4; ++j) {
		i_2 = j;
		i_1 = j;
		a[i_2] = c[i_1].real();
    }
    goto L10;
/*        report and return */
L600:
    real1 = double(kount);
    real2 = double((*mm));
    temp = real1 / real2;
    return 1;
} /* dproot */

static const Complex one = 1, zero = 0;

void
LPCData::correct(float *frame, MXINT32 npoles, float *a) {
    /* System generated locals */
    MXINT32 i,i_1, i_2, i_3, i_4, i_5;

    /* Local variables */
    static MXINT32 nall;
    static MXINT32 j, k;
    static double r[ArrSize];
    static Complex w[ArrSize];
    static MXINT32 ndata;
    static double y[97], rooti[96];
    static MXINT32 l1, k4;
    static double rootr[96];
    static MXINT32 ii;
    static double th[ArrSize];
    static Complex ww;
    static MXINT32 kinsid;
    static double zz;
    static MXINT32 kprint, k4m;
    static double eps;

#ifdef LPC_DEBUG
	printf("\nin correct npoles = %d \n",npoles);
	for(i=0; i<36; i++) printf(" %g ",frame[i]);
#endif
    /* Parameter adjustments */
    --frame; 
    --a;
    /* Function Body */
    k4 = npoles + 1;
    k4m = k4 - 1;
    nall = k4 + 4;
    i_1 = k4m;
    for (ii = 1; ii <= i_1; ++ii) {
		y[ii - 1] = -frame[ii + 4];
    }
    y[k4 - 1] = 1.;
    eps = 1.0000000000000008e-8;
    factor(y, &k4, rootr, rooti, &kinsid, &kprint, &eps);
    i_1 = k4m;
    for (j = 1; j <= i_1; ++j) {
	/* Computing 2nd power */
		r[j - 1] = hypot(rootr[j - 1], rooti[j - 1]);
		th[j - 1] = atan2(rooti[j - 1], rootr[j - 1]);
		if (r[j - 1] >= 1.) {
			r[j - 1] = 1. / r[j - 1];
		}
    }
    i_1 = k4m;
    for (k = 1; k <= i_1; ++k) {
		i_2 = k - 1;
		w[i_2] = zero;
    }
    i_2 = k4 - 1;
    w[i_2] = one;
    i_2 = k4m;
    for (k = 1; k <= i_2; ++k) {
	/* 	ww=dcmplx(rootr(k),rooti(k)) */
		ww = polar(r[k - 1], th[k - 1]);
		l1 = k4 - k;
		i_1 = k4m;
		for (j = l1; j <= i_1; ++j) {
			i_3 = j - 1;
			i_4 = j;
			i_5 = j - 1;
			w[i_3] = w[i_4] - (ww * w[i_5]);
		}
		i_3 = k4 - 1;
		i_4 = k4 - 1;
		w[i_3] = -ww * w[i_4];
    }
    i_3 = k4;
    for (j = 2; j <= i_3; ++j) {
		i_4 = j - 1;
		zz = w[i_4].real();
		a[k4 + ndata + 1 - j] = -zz;
    }
}


boolean
LPCData::isStable(float *frame, MXINT32 npoles) {
    /* System generated locals */
    double r_1;
	static const int Size = 150;
	static const int SizeP1 = Size + 1;
    /* Local variables */
    static float a[Size * Size]	/* was [150][150] */;
    MXINT32 i, m, mm;
    static float rk[249];
    /* Parameter adjustments */
    --frame;

    /* Function Body */
    a[(npoles + 1) * Size - Size] = 1.0;
    for (i = 1; i <= npoles; ++i) {
		a[i + 1 + (npoles + 1) * Size - SizeP1] = *(frame + i);
    }
    for (mm = 1; mm <= npoles; ++mm) {
		m = npoles - mm + 1;
		rk[m - 1] = a[m + 1 + (m + 1) * Size - SizeP1];
		if ((r_1 = rk[m - 1], abs(r_1)) < 1.) {
			goto L20;
		}
		return false;
	L20:
		for (i = 1; i <= m; ++i) {
	/* L25: */
	/* Computing 2nd power */
			r_1 = rk[m - 1];
			a[i + m * Size - SizeP1] = (a[i + (m + 1) * Size - SizeP1]
				- rk[m - 1] * a[m - i + 2 + (m + 1) * Size - SizeP1])
			/ (1.0 - r_1 * r_1);
		}
    }
    return true;
}