/*
 * Copyright (c) 1992 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.
 */

#ifndef lint
static const char rcsid[] =
    "@(#) $Header: /work/projects/tove/cvs/src/testing/vat/wiener.cc,v 1.1 1997/12/08 17:22:55 parnanen Exp $ (LBL)";
#endif

#include "wiener.h"

Wiener::Wiener(double *x, double *y, int slen, int o)
{
	order = o;
	h = new double[o];
	double* r_xx= new double[o];
	double* r_dx= new double[o];
	Correlate(r_xx, x, x, slen);
	Correlate(r_dx, y, x, slen);
	Solve(r_xx, r_dx);
	delete r_xx;
	delete r_dx;
}

/*
 * Compute the wiener cancellation filter coefficients, using an 
 * exact, iterative approach.
 */
void Wiener::Solve(double *r_xx, double *r_dx)
{
	double *g = new double[order];
	double *ng = new double[order];
	int k;

	for (k = 0; k < order; ++k)
		h[k] = 0;

	g[0] = 1 / r_xx[0];
	h[0] = r_dx[0] / r_xx[0];
	for (int m = 0; m + 1 < order; ++m) {
		double alpha = 0;
		double beta = 0;
		for (k = 0; k <= m; ++k) {
			alpha += r_xx[k + 1] * g[k];
			beta += r_xx[m + 1 - k] * h[k];
		}
		double t1 = 1 / (1 - (alpha * alpha));
		ng[0] = - (alpha * g[m]) / t1;
		for (k = 1; k <= m; ++k)
			ng[k] = (g[k - 1] - alpha * g[m - k]) / t1;
		ng[m + 1] = g[m] / t1;
		
		for (k = 0; k <= m; ++k)
			h[k] += (r_dx[m + 1] - beta) * ng[k];
		h[m + 1] = (r_dx[m + 1] - beta) * ng[m + 1];

		double *tmp = g;
		g = ng;
		ng = tmp;
	}
	delete g;
	delete ng;
}

void Wiener::Correlate(double *dx, double *d, double *x, int slen)
{
	int v;
	for (v = 0; v < order; ++v)
		dx[v] = 0;

	for (v = 0; v < order; ++v) {
		double sigma = 0;
		for (int n = v; n < slen; ++n)
			sigma += d[n] * x[n - v];
		dx[v] += sigma / slen;
	}
}