/******************************************************************************

                                Calcs.cpp

                Copyright (c) Roger Burghall 2014..2017

******************************************************************************/


/** \file
'Calcs.cpp' is one of the files intended to hold the functions for 
calculation purposes.

It is not involved with GUIs and so it is expected that it will not (or 
barely) need editting for porting to a different OS.
*/

// ***************************
// #include <vcl\vcl.h>
// #pragma hdrstop
#include <cmath>
#include "math.h"
#include "Calcs.h"
#include <iostream>
#include <complex>
#include <stdlib.h>
#include <sstream>
// #include "filter.h"
// #include "control.h"
// ***************************

#if SHOW_FILE_COMPILING
#pragma message "Compiling Calcs.cpp"
#endif

TFilter filter;

TFilter::TFilter( )
{
	fclass = Lowpass;
	sclass = Bessel;
	cclass = SallKey;
	frequency = 1.0;
	bandwidth = 0.25;
  	gain = 1.0;
	poles = 3;
	zeroes = 0;
	fmax = tmax = 2.0;
	ripple = 0.1;
	a0 = 0.70794578;
	type = '0';		// 'Prototype' filter.
}



double isinh(double x)
{
	double y=1.0;
	do {
		y+=(x-sinh(y))/cosh(y);
	} while(((sinh(y)-x)/x) >=0.001);
	return(y);
}



double n_to_freq(int n, double f1, int nmax=FSTEPS, int decades=2)
{
	return(f1 * exp10(double(n) * decades / nmax));
}




using namespace std;

// void bessel(TFilter& filter);
// void butterworth(TFilter& filter);
// void chebyshev(TFilter& filter);
// void lowpass(TFilter& filter, double f);
// void highpass(TFilter& filter);

void geffe(double sigma, double omega, double w0, double bw, double &a1, double &b1, double &a2, double &b2);

double atof(string s)
{
	double d;
	sscanf(s.c_str(), "%lf", &d);
	return(d);
}



void
TFilter::_pole(double a, double w)
{
// Draw pole at (a,w).
int aa, ww;

#if VERBOSE
	cout << "Pole at " << a << ", " << w << "\n";
#endif
}



void TFilter::show_filter(void)
{
	int i;
#if VERBOSE
	cout << "No of poles:" << poles << "\n";
	cout << "No of zeroes:" << zeroes << "\n";

	for(i=0; i<poles; i++) cout << "i: " << st[i].pole << "\n";
#endif
}



void TFilter::show_filter(string title)
{
	int i;
#if VERBOSE
	cout << title << "\n";
	cout << "No of poles:" << poles << "\n";
	cout << "No of zeroes:" << zeroes << "\n";

	for(i=0; i<poles; i++) cout << "i: " << st[i].pole << "\n";
#endif
}



/// Assignment operator; don't actually need to copy all data for this application.
stage& stage::operator= (stage& f1)
{
	this->fclass = f1.fclass;
	this->cclass = f1.cclass;
	this->T = f1.T;
	this->q = f1.q;
	this->pole = f1.pole;
	this->zero = f1.zero;
	return(*this);
}



/// Put the poles into falling order of imaginary part. As only a few objects are to be sorted, use a bubble sort.
void TFilter::sort(void)
{
	stage s0;
	int i, j;
	for(j=0; j < poles-1; j++) for(i=0; i < poles-1; i++) {
		if(st[i].pole.imag( ) < st[i+1].pole.imag( )) {
			// poles in wrong order, swap them.
			s0.pole = st[i].pole;
			st[i].pole = st[i+1].pole;
			st[i+1].pole = s0.pole;
		}
	}
}



double TFilter::fgain(double f)
{
	unsigned i, k;
	double t, q;
	complex<double> j = std::complex<double>(0.0, 1.0);
	complex<double> s, r;
	double rf;
	rf = 1.0;
	for(k=0; k<(poles/2); k++) {
		t = st[k].T, q = st[k].q;
		s = std::complex<double>(0.0, (2.0*M_PI)*f);
		r = 1.0/(1.0+s*t/q+s*s*t*t);
		rf *= abs(r);
	}
	if(k < (poles+1)/2) {
		s = std::complex<double>(0.0, (2.0*M_PI)*f);
		r = 1.0/(1.0+s*tau);
		rf *= abs(r);
	}
	return(rf);
}



/// We want to normalise the filter so the cut-off is at 1.0 radians per second.
void TFilter::Proto_normalise(void)
{
	int k;
	double f, f1;
	double g, g1;
	char logstr[256];
	log("\n<Normalise prototype>");
	for(f = 0.1; f < 10.0; f *= 1.01) {
		g = fgain(f);
		if(g < a0) break;
		g1 = g;
		f1 = f;
	}
//	std::cout << "Gain is " << g1 << " at " << f1 << ", and " << g << " at " << f << "\n";
	sprintf(logstr, "Prototype gain is %f at %f, and %f at %f", g1, f1, g, f);
	log(logstr);
	f = 2.0 * M_PI * (f * (-a0 + g1) + f1 * (-g + a0)) / (-g + g1);
	for(k=0; k<(poles); k++) {
		st[k].pole /= f;
	}

	std::cout << "f = " << f << "\n";
	show_filter("Normalised prototype");
	log("</Normalise prototype>\n");
}



/**
Calculate the t and q values for each stage from the poles with
positive imaginary parts (i.e. one of each pair), and if the 
number of poles is odd, calculate the tau value from the remaining 
pole.
*/
void TFilter::ts_and_qs(void)
{
	unsigned i, j, k;
	double theta;
	double tt, qq;

	log("<Determine Ts and qs>");

#if VERBOSE
	cout << "ts_and_qs\n";
#endif
	switch(fclass) {
		case Lowpass: k = poles/2; break;
		case Highpass: k = poles/2; break;
		case Bandpass: k = poles; break;
	}

	for(i=0, j=0; i<k; i++) {
		if(st[i].pole.imag( ) < 0.0) continue;
		tt=1.0/abs(st[i].pole),
		qq=-1.0/(2.0*tt*real(st[i].pole));
		st[j].T = tt;
		st[j].q = qq;
		log("T=", tt);
		log("q=", qq);
		++j;
	}
	if(poles%2) {
		tau = 1.0 / st[poles/2].pole.real();
		log("Tau=", -1.0/tau);
		st[j].T = tau;
	}
#if VERBOSE
	cout << "end ts_and_qs\n";
#endif
	log("</Determine Ts and qs>");
}



void TFilter::butterworth(void)
{
	unsigned i;
	double theta;
	double tt, qq;
	log("<Butterworth>");
	theta=M_PI/(2.0*poles);
	for(i=0; i<poles; i++) {
		if((poles%2) && (i == poles/2)) {
//			pole[i] = std::complex <double>(-sin(theta)*2.0*M_PI, 0.0);
			st[i].pole = std::complex <double>(-sin(theta), 0.0);
		} else {
//			pole[i] = std::complex <double>(-sin(theta)*2.0*M_PI, cos(theta)*2.0*M_PI);
			st[i].pole = std::complex <double>(-sin(theta), cos(theta));
		}
		log("pole at ", st[i].pole);
		theta += M_PI/poles;
	}

	type = 'P';

	log("/Butterworth");
}



void
TFilter::bessel(void)
{
int i, j, k, nn;
double a[20], b[20], bb[20], bbb[20], aa[10], ww[10], limit, p, q,
	qq, tt, dd, t, w;
	std::ofstream file;
	int n;

	log("<Bessel>");
	n = poles;
	/// Bessel-Thomson filter design.
	if((n<3)||(n>19)) {
		cout << "Unreasonable order.\n";
		return;
	}
	nn=n;
	limit=exp(std::log(3.5)*n)*0.00001;
	/// Evaluate the terms of the polynomial.
	for(i=0; i<=19; i++) a[i]=b[i]=bb[i]=bbb[i]=0;
	bbb[0]=bbb[1]=1.0;
	bb[0]=bb[1]=3.0, bb[2]=1.0;
	for(i=3; i<=n; i++) {
		for(j=0; j<=n; j++)b[j]=(2*i-1)*bb[j];
		for(j=2; j<=n; j++) b[j]+=bbb[j-2];
		for(j=0; j<=n; j++) {
			bbb[j]=bb[j];
			bb[j]=b[j];
		}
	}
#if VERBOSE
	cout << "Bessel polynomial:\n";
	for(i=0; i<=n; i++) cout << "a[" << i << "]=" << b[i] << " "; cout << "\n";
#endif
	for(i=0; i<=n; i++) a[n-i]=b[i];
	k=0;
	file.open("pfile.z");
	if(!file) {
		cout << "\nUnable to open 'pfile.z'\n";
		return;
	}
	file << "Bessel: P " << n << " " << 0;
	// 440
	int pp = 0;
	do {
		p=q=0.01;
		do {
			b[0]=a[0];
			b[1]=a[1]-b[0]*p;
			for(i=2; i<=n;i++) b[i]=a[i]-b[i-2]*q-b[i-1]*p;
			if(b[n-2]==0) b[n-2]=0.000001;
			q=a[n]/b[n-2];	//530
			p=(a[n-1]-b[n-3]*q)/b[n-2];
		} while((abs(b[n-1]) >= limit) || (abs(b[n]) >= limit));
		cout << "Solution at " << -p/2.0 << " +/- j*" << sqrt(q-(p*p/4.0)) << "\n";
		file << " " << -p/2.0 << " " << sqrt(q-(p*p/4.0)) << "\n";
		st[pp++].pole = complex<double>(-p/2.0, sqrt(q-(p*p/4.0)));
		st[pp++].pole = complex<double>(-p/2.0, -sqrt(q-(p*p/4.0)));
		aa[k]=-p/2.0, ww[k]=sqrt(q-(p*p/4.0));
		k++;
		for(i=0; i<=n; i++) a[i]=b[i];
		n-=2;
	} while(n>2);
	if(n==1) {
		// 720
		cout << "Solution at " << -b[1]/b[0] << "\n";
		file << " " << -b[1]/b[0];
		st[pp++].pole = complex<double>(-b[1]/b[0], 0.0);
	} else {
		//660
		cout << "Solution' at " << -a[1]/(2.0*a[0]);
		cout << " +/-j*" << (sqrt(-a[1]*a[1]+4.0*a[0]*a[2])/(2.0*a[0]));
		file << " " << -a[1]/(2.0*a[0]) << " " << sqrt(-a[1]*a[1]+4.0*a[0]*a[2])/(2.0*a[0]);
		st[pp++].pole = complex<double>(-a[1]/(2.0*a[0]), sqrt(-a[1]*a[1]+4.0*a[0]*a[2])/(2.0*a[0]));
		st[pp++].pole = complex<double>(-a[1]/(2.0*a[0]), -sqrt(-a[1]*a[1]+4.0*a[0]*a[2])/(2.0*a[0]));
		k++;
	}
	file.close();

	for(i=0; i<=nn/2; i++) {
		_pole(aa[i]/3.0, ww[i]/3.0);
		_pole(aa[i]/3.0, -ww[i]/3.0);
	}
	if(nn%2) {
		_pole(-b[1]/(3.0*b[0]), 0);
//		pole[nn - 1] = complex<double>(-b[1]/(3.0*b[0], 0.0));
	}

#if VERBOSE
	cout << "\nEndBessel\n";
#endif

	// 940
	if(dd!='D') ;
	type = 'P';

	log("</Bessel>");
}



void
TFilter::chebyshev(void)
{
char dump;
int i;
double aa[20], ww[20], g, a, w, tt, qq;

	// Chebyshev filter design.
	log("<Chebyshev>");
	unsigned n = poles;
// Version 2.11: correct ripple factor from dB.
//	g = ripple;
	g = sqrt(pow(10.0, ripple / 10.0) - 1.0);

	for(i=1; i<=n; i++) {
		a=-sin((2*i-1)*M_PI/(2*n))*sinh(isinh(1.0/g)/n);

		w=cos((2*i-1)*M_PI/(2*n))*cosh(isinh(1.0/g)/n);

		if(fabs(w)<1e-9) w=0;

		st[i-1].pole = complex<double>(a, w);

		cout << "\n" << a << ", " << w;
		aa[i]=-a, ww[i]=w;
	}

	type = 'P';

#if VERBOSE
	cout << "EndChebyshev\n";
#endif

	log("</Chebyshev>");
}



void
TFilter::lowpass(void)
{
	unsigned i;
	log("<lowpass>");
// Mod 2016-09-01-RB
//	for(i=0; i<poles; i++) st[i].pole *= 2.0 * M_PI * frequency;
	for(i=0; i<poles; i++) st[i].pole *= frequency;
	fclass = Lowpass;
	zeroes = 0;
	type = 'L';
	log("</lowpass>");
}



/**
Convert the continuous filter to discrete form via the bilinear transformation
*/
void TFilter::bilinear(void)
{
	std::complex<double> _bilinear(std::complex<double> s, double Ts);
	double normalise(double fc, double fs);
	unsigned i;
	double fc, fs, omega;

	log("<bilinear transformation>");

	for(i=0; i<poles/2; i++) {
		fc = 1.0 / (2.0*M_PI*st[i].T);
		fs = samplingfreq;
		if(fc >= fs/2.0) {
			std::cout << "That doesn't make sense! Cut-off must be less than sampling freq / 2.\n" <<
			 "Here fc = " << fc << " and fs = " << fs << "\n";
			return;
		}
		omega = normalise(fc, fs);
		std::cout << "\nStage " << i << "\nomega =" << omega << "\n";
		double omega_ac = tan(omega/2.0);
		std::cout << "omega_ac =" << omega_ac << "\n\n";
		st[i].iir1.transform(fclass, omega_ac, st[i].q);
		st[i].iir1.write(i);
/*		st[i].iir1.step(0.0);
		std::cout << "\n";
		std::cout << "Direct Form 1 realisation.\n--------------------------\n";
		for(i=0; i<200; i++) std::cout << st[i].iir1.step(1.0) << "\n";
*/
//		st[i].z = _bilinear(st[i].pole, 1000.0);
	}

	log("</bilinear transformation>");
}


/**
Calculate the frequency response of the filter.
*/
void
bode_calc(TFilter& filter)
{
//	extern TControl_window *Control_window;
	unsigned i, k;
	double f, w;
	double t, q;
	std::complex<double> s, r, j;
	double temp;
	double max_gain = 0.0;
	ofstream bode_file;

	filter.log("<bode_calc>");
	j = std::complex<double>(0.0, 1.0);
	
//	filter.fmax = 1.0 * filter.frequency * M_PI / 2.0;
	filter.fmax = T_MULTIPLE_HP * filter.frequency;
	for(i=0; i<FSTEPS; i++) filter.freq_resp[i] = 1.0;
	for(k=0; k<(filter.poles/2); k++) {
//		t = 1.0 / abs(filter.pole[k]);
//		q = 1.0 / (2.0*t*-real(filter.pole[k]));
		t = filter.st[k].T, q = filter.st[k].q;
		for(i=0; i<FSTEPS; i++) {
#if LINEAR_FREQ
			f = i*filter.fmax/FSTEPS;
#else
			f = n_to_freq(i, filter.fmax);
#endif
			s = std::complex<double>(0.0, (2.0*M_PI)*f);
			r = 1.0/(1.0+s*t/q+s*s*t*t);
			if(filter.fclass == Bandpass) r *= s * t;
			if(filter.fclass == Highpass) r *= s * s * t * t;
			filter.freq_resp[i] *= abs(r);
		}
	}
	if(k < (filter.poles+1)/2) {
//		t = 1.0 / abs(filter.pole[k]);
		for(i=0; i<FSTEPS; i++) {
#if LINEAR_FREQ
			f = i*filter.fmax/FSTEPS;
#else
			f = n_to_freq(i, filter.fmax);
#endif
			s = std::complex<double>(0.0, (2.0*M_PI)*f);
			r = 1.0/(1.0+s*filter.tau);
			if(filter.fclass == Bandpass) { cout << "How do we have a bandpass filter with an odd pole?\n"; r *= s * filter.tau; }
			if(filter.fclass == Highpass) r *= s * filter.tau;
			filter.freq_resp[i] *= abs(r);
		}
	}

/// Normalise gain
	if(filter.fclass == Bandpass) {
		for(i=0, max_gain=0; i<FSTEPS; i++) {
			if(filter.freq_resp[i] > max_gain) max_gain = filter.freq_resp[i];
		}
		for(i=0; i<FSTEPS; i++) {
			filter.freq_resp[i] /= max_gain;
		}
	}

	bode_file.open("./bode.csv");
	if(bode_file.is_open( )) {
		bode_file << "Frequency, Gain\n";
		for(i=0; i<FSTEPS; i++) {
#if LINEAR_FREQ
			f = i*filter.fmax/FSTEPS;
#else
			f = n_to_freq(i, filter.frequency);
#endif
			bode_file << f << ", " << filter.freq_resp[i] << "\n";
		}
		bode_file.close( );
	}
	filter.log("</bode_calc>");
}



/**
'step_calc' convolves the impulse response of each stage with a step function
so as to derive the step response of the entire filter.
*/
void
TFilter::step_calc(void)
{
//	extern TControl_window *Control_window;
	char logstr[256];
	unsigned i, k, u;
	double ti, w, w0;
	double tt, qq, zeta;
	double a, b;
	double input[TSTEPS], output[TSTEPS], stage_n[TSTEPS];
	double integral, max;
	double k1, dt;
	std::complex<double> s, r, j;
	ofstream step_file;

	log("<step_calc>");
	j = std::complex<double>(0.0, 1.0);

	switch(fclass) {
		case Lowpass: tmax = T_MULTIPLE_LP/frequency; break;
		case Bandpass: tmax = T_MULTIPLE_BP/bandwidth; break;
		case Highpass: tmax = T_MULTIPLE_HP/frequency; break;
		default: cout << "Unknown fclass\n"; exit(-1);
	}
	dt = tmax/TSTEPS;

	sprintf(logstr, "tmax = %f seconds", tmax);
	log(logstr);

//	c = tmax / 5.0;
	/// Start with step function
	for(i=0; i<TSTEPS; i++) input[i] = 1.0, output[i] = 0.0;
	for(k=0; k<(poles/2); k++) {
		/// Calculate constants for stage 'k'
		tt = st[k].T;
		qq = st[k].q;
		a = -1.0 / (2 * qq * tt);
		b = sqrt(1.0 / (tt * tt) - (a * a));

#if VERBOSE
		cout << "T = " << tt << ", q = " << qq << "\n";
		cout << "a = " << a << ", b = " << b << "\n";
#endif

		w0 = 1.0 / tt;
		log("'step_calc' tt = ", tt);
		log("'step_calc' q = ", qq);
//		log("'step_calc' w0 = ", w0);
		k1 = 1.0 / (b*tt);
//		k1 = 1.0 / (b*tt*tt);
		for(i=0; i<TSTEPS; i++) {
			/// Calculate time for point 'i'
			ti = i*dt;		// CHECK THIS
			/// Calculate the impulse response of stage 'k'
			switch(fclass) {
				case Lowpass: stage_n[i] = k1*exp(a * ti) * sin(b * ti) * b; break;
				case Highpass: stage_n[i] = -exp(a * ti) * sin(b * ti) * k1;
				 if(i == 0) stage_n[0] += tt*TSTEPS/tmax; break;
				case Bandpass: stage_n[i] = (sin(b * ti) * a * exp(a * ti) + exp(a * ti) * b * cos(b * ti)) / b; break;
				default: cout << "\nUnknown 'fclass'!\n"; exit(-1);
			}
		}

		/// initialise the output array...
		for(i=0; i<TSTEPS; i++) output[i] = 0.0;

		/// Convolve with input to stage 'k'
		for(i=0; i<TSTEPS; i++) {
			// Calculate time for point 'i'
			ti = i*tmax/TSTEPS;		// CHECK THIS
			for(u=0; u<TSTEPS; u++) {
				if((i+u) < TSTEPS) output[i+u]+=input[i]*stage_n[u]*tmax/TSTEPS;		// CHECK THIS
				else break;
//				if((i+u) < TSTEPS) output[i+u]+=input[i]*stage_n[u];		// CHECK THIS
			}
		}

#ifdef SHOW_STAGES
		log("'step calc' stage[] = ", stage_n, TSTEPS);
		log("'step-calc' input[] = ", input, TSTEPS);
		log("’stepicalc' output[] = ", output, TSTEPS);
#endif

		/// Copy output of stage 'k' to input for stage 'k+1'
		for(i=0; i<TSTEPS; i++) {
			input[i] = output[i];
		}
	}

	/// If order is odd, process first order stage.
	if(poles%2) {
		if(fclass == Bandpass) {
			log("Impossible! Bandpass can't have odd order!");
			cout << "Impossible! Bandpass can't have odd order!\n";
			return;
		}
		tt = -st[k].T;
#if VERBOSE
		cout << "Tau = " << tt << "\n";
#endif
		w0 = 1.0 / tt;
		log("'step_calc' tt = ", tt);
		for(i=0; i<TSTEPS; i++) {
			/// Calculate time for point 'i'
			ti = i*tmax/TSTEPS;		// CHECK THIS
			/// Calculate the impulse response of 1st order stage 'k'
			switch(fclass) {
				case Lowpass: stage_n[i] = exp(-ti/tt); break;
//				case Highpass: stage_n[i] = -exp(-ti/tt)/tt; 
				case Highpass: stage_n[i] = -exp(-ti/tt);
				 if(i == 0) stage_n[0] += tt*TSTEPS/tmax; break;
				case Bandpass: std::cout << "*** ERROR! Band-pass filter cannot have odd order! ***\n"; break;
				default: cout << "\nUnknown 'fclass'!\n"; exit(-1);
			}
		}

		// initialise the output array...
		for(i=0; i<TSTEPS; i++) output[i] = 0.0;

		/// Convolve with input to stage 'k'
		for(i=0; i<TSTEPS; i++) {
			// Calculate time for point 'i'
			ti = i*tmax/TSTEPS;		// CHECK THIS
			for(u=0; u<TSTEPS; u++) {
				if((i+u) < TSTEPS) output[i+u]+=input[i]*stage_n[u]*tmax/TSTEPS;		// CHECK THIS
				else break;
//				if((i+u) < TSTEPS) output[i+u]+=input[i]*stage_n[u];		// CHECK THIS
			}
		}

#ifdef SHOW_STAGES
		log("'step calc' stage[] = ", stage_n, TSTEPS);
		log("'step-calc' input[] = ", input, TSTEPS);
		log("’stepicalc' output[] = ", output, TSTEPS);
#endif

		/// Copy output of stage 'k' to input for stage 'k+1'
		for(i=0; i<TSTEPS; i++) {
			input[i] = output[i];
		}

	}

	/// Adjust gain - I hope this is a temporary frig!
	switch(fclass) {
		case Highpass: for(i=1, max = 0.0; i<TSTEPS; i++) if(fabs(output[i]) > max) max = fabs(output[i]); break;
		default: for(i=0, max = 0.0; i<TSTEPS; i++) if(output[i] > max) max = output[i]; break;
	}
	for(i=0; i<TSTEPS; i++) output[i] /= max;

	/// Write to './step.csv' time and step response
	step_file.open("./step.csv");
	for(i=0, integral=0.0; i<TSTEPS; i++) {
		step_resp[i] = output[i];
		integral += output[i];
		step_file << i*tmax/TSTEPS << ", " << step_resp[i] /* << ", " << integral */ << "\n";
	}
	step_file.close( );
	log("</step_calc>");
}



double scale(TFilter filter)
{
	double max = 0;
	int i;
	for(i = 0; i < filter.poles; i++) {
		if(-(double)filter.st[i].pole.real() > max) max = -(double)filter.st[i].pole.real();
		if((double)filter.st[i].pole.imag() > max) max = (double)filter.st[i].pole.imag();
	}
	return((max * 4.0) / 3.0);
}



void TFilter::Calculate(void)
{
	char str[256];
	int i;
	log("<Calculate>");

/// Create prototype filter.
	switch(sclass) {
		case Butterworth: butterworth( ); break;
		case Bessel: bessel( ); sort( ); break;
		case Chebyshev: chebyshev( ); break;
		default: cout << "Unknown filter!" << "\n"; return;
	}
	/// Adjust prototype filter so attenuation is x dB at 1 Hertz.
	std::cout << "***** Prototype filter *****\n";
	for(i=0; i<poles; i++) {
		std::cout << "Pole: " << st[i].pole << "\n";
		sprintf(str, "Pole: %f + i.%f", st[i].pole.real( ), st[i].pole.imag( ));
		log(str);
	}
	std::cout << "****************************\n";
/// Calculate T and q values from poles.
	ts_and_qs( );
#ifdef NORMALISE
	Proto_normalise( );
#endif
/// Transform prototype to desired filter class (e.g. high-pass etc.).
	transform( );
	ts_and_qs( );
	show_filter( );
	log("</Calculate>");
}



void TFilter::log(string s)
{
#if LOG
	ofstream logfile;
	logfile.open("./rbfilter.log", ios::app);
	logfile << s << "\n";
	logfile.close( );
#endif
}



void TFilter::log(string s, double d)
{
	ostringstream s1;
	std::string s2;
	s1 << s << d;
	s2 = s1.str( );
	log(s2);
}




void TFilter::log(string s, double *pd)
{
	ostringstream s1;
	std::string s2;
	s1 << s << *pd;
	s2 = s1.str( );
	log(s2);
}



/// Log the array pd but no more than 100 elements of it.
void TFilter::log(string s, double *pd, int n)
{
	int i;
#if LOG
	char s1[128];
	n = n>100? 100: n;
	for(i=0; i<(n-1); i++) {
		sprintf(s1, "%s %f", s.c_str( ), *(pd+i));
		log(s1);
	}
	log("");
#endif
}




void TFilter::log(string s, std::complex<double> c)
{
	ostringstream s1;
	std::string s2;
	s1 << s << c;
	s2 = s1.str( );
	log(s2);
}




void TFilter::print_T_q(void)
{
//	cout << s << "\n";
}



/// 'transform( )' transforms the prototype filter to the desired type (e.g. bandpass) and frequency.
void TFilter::transform(void)
{
char t, type;
int i;
double p[10], z[10];
ifstream ifile;
ofstream ofile;

	type = TFilter::type;
	log("<transform>");
	if(type != 'P') {
		cout << "\nFile has already been transformed.\n";
		return;
	}
	/// 220; Transform to the chosen type.
	do {
		switch(fclass) {
			case Lowpass:
			 lowpass( );
			 type = 'L';
			 for(i=0; i<poles; i++) st[i].pole *= 2.0 * M_PI;
			 break;
			case Highpass:
			 highpass( );
			 type = 'H';
//			 for(i=0; i<poles; i++) st[i].pole *= 2.0 * M_PI;
			 break;
			case Bandpass:
			 bandpass( );
			 type = 'B';
			 for(i=0; i<poles; i++) st[i].pole *= 2.0 * M_PI;
			 break;
			default: cout << "Unknown filter class\n"; continue;
		}
		break;
	} while(true);
	/// 330; Rewrite the parameter file.
	ofile.open("pfile.z");
	if(!ofile) {
		cout << "\nUnable to open 'pfile.z'\n";
//		exit(0);
	}
	ofile << type << " " << poles << " " << zeroes << "\n";
	for(i=0; i<poles; i++) ofile << st[i].pole << " ";
	if(zeroes) {
		for(i=0; i<zeroes; i++) ofile << st[i].zero << "\n";
	}
	ofile.close();

//	ts_and_qs(filter);

	log("</transform>");
}



/** Transform to highpass 
Keep the q values unaltered but take the reciprocal of the magnitude; adjust the magnitudes for
frequency; add zeroes at the origin.
*/
void
TFilter::highpass(void)
{
int i, type;
double dsq, d, ratio;
double a1, w1;

	log("<highpass>");
	for(i=0; i<poles; i++) {
		std::cout << "From " << st[i].pole;
		dsq = st[i].pole.real( ) * st[i].pole.real( ) + st[i].pole.imag( ) *st[i].pole.imag( );
		d = sqrt(dsq);
		std::cout << " (Magnitude " << d << ") ";
		ratio = st[i].pole.imag( ) / st[i].pole.real( );
		if(st[i].pole.imag( ) == 0.0) {
			w1 = 0.0;
			a1 = 1.0 / st[i].pole.real( );
		} else {
			d = 1.0 / d;
			dsq = 1.0 / dsq;
			w1 = st[i].pole.imag( ) * dsq;
			a1 = st[i].pole.real( ) * dsq;
		}
		st[i].pole = complex<double>(a1, w1);
		st[i].zero = complex<double>(0.0, 0.0);
		st[i].pole *= 2.0 * M_PI * frequency;
//		st[i].zero *= 2.0 * M_PI * frequency;
		std::cout << " to " << st[i].pole << "\n";
	}

	fclass = Highpass;
	zeroes = poles;
	type='H';

	log("</highpass>");
}



/// Transform the filter to bandpass using the Geffe algotithm.
void TFilter::bandpass(void)
{
int i, j;
double f, w1, a1, d, bw, w0, b1, a2, b2;
ofstream ofile;
TFilter filter1;

	log("<bandpass>");
/// Transformation to bandpass doubles the number of poles!
//	if(poles % 2) log("Odd order! Impossible");

	for(i=0, j=0; i<poles; i++) {
		if(st[i].pole.imag( ) < -0.0001) continue;
		geffe(st[i].pole.real( ), st[i].pole.imag( ), frequency, bandwidth, a1, b1, a2, b2);
#if VERBOSE
		cout << i << ": " << a1 << ", +-" << b1 << " \\ " << a2 << ", +/-" << b2 << "\n";
#endif
		filter1.st[j++].pole = complex<double>(a1, b1);
		filter1.st[j++].pole = complex<double>(a1, -b1);
		if(i != poles - 1) {
			filter1.st[j++].pole = complex<double>(a2, b2);
			filter1.st[j++].pole = complex<double>(a2, -b2);
			ofile << -a1 << " " << b1 << " " << -a2 << " " << b2 << "\n";
		} else ofile << -a1 << " " << b1 << "\n";
	}
	zeroes = poles;
	filter1.poles = 2 * poles;
	type = 'B';
	filter1.type = type;
/*	if(n%2)
	 ofile << p[n]*bw/2.0 << " " << sqrt(w0*w0-p[n]*p[n]*bw*bw/4.0) << "\n";
	ofile.close();
*/

	for(i=0; i<filter1.poles; i++) st[i].pole = filter1.st[i].pole;
	poles = filter1.poles;

	fclass = Bandpass;

	log("</bandpass>");
}



double arccos(double x)
{
	return(2.0 * atan(sqrt(1.0 - x*x) / (1.0 + x)));
}



/**
'geffe' is an implementation of the "Geffe" algorithm which transforms a low pass prototype filter into a bandpass filter. The "Geffe" algorithm is described in "Analogue Filter Design" by M. E. van Valkenburg.
*/
void
geffe(double sigma, double omega, double w0, double bw,
 double &a1, double &b1, double &a2, double &b2)
{
double c, d, e, g, q, k, w, w01, w02, qc, a, b;
double q1, q2;		// Used for checking.

	filter.log("<geffe>");

	qc=w0/bw;
	c=sigma*sigma+omega*omega;
	d=-2.0*sigma/qc;
	e=4.0+c/(qc*qc);
	g=sqrt(e*e-4.0*d*d);
	q=sqrt((e+g)/2.0)/d;
	k=sigma*q/qc;
	// As originally written, 'w' would often be evaluated as 'nan'. This is intended to avoid that problem.
	if(fabs((k*k-1.0) < 0.0001)) w = k;
	else w=k+sqrt(k*k-1.0);
	w01=w*w0;
	w02=w0/w;
	a1=w01/(2.0*q);
	a2=w02/(2.0*q);
	a=1.0/(2.0*q);
#if 1
	b=sin(atan(sqrt((1.0-a*a)/(a*a))));
	b1=w01*b;
	b2=w02*b;
#else
	b1 = -w01*sin(arccos(1.0 / (2.0 * q)));
	b2 = -w02*sin(arccos(1.0 / (2.0 * q)));
#endif
	// Check:
	q1 = (a1*a1+b1*b1)/(2.0*a1);
	q2 = (a2*a2+b2*b2)/(2.0*a2);
//	cout << "q=" << q1 << " & " << q2 << "\n";	TODO: WHAT has gone wrong here?!!!

	filter.log("</geffe>");
}