#include "brick_link.h"


BrickLinkTransferFunction::BrickLinkTransferFunction(const PIString & num, const PIString & denom): BrickLinkBase(1, 1, 2) {
	type = "TransferFunction";
	setName(type);
	paramNames[1] = "Numerator";
	paramNames[2] = "Denominator";
	parameters[1] = num;
	parameters[2] = denom;
	interactive = true;
	parameterChanged(1);
}


void BrickLinkTransferFunction::parameterChanged(int index) {
	if (index == 0) {
		KF.setMethod((PIMathSolver::Method)parameters[0].toInt());
		return;
	}
	PIString ts;
	note_ = "Your function:\n\n";
	PIStringList sl = parameters[1].toString().split(" ");
	sl.removeStrings(""); sl.removeStrings("\t");
	TF.vector_Bm.resize(sl.size());
	for (int i = sl.size_s() - 1; i >= 0; --i) {
		TF.vector_Bm[i] = sl[sl.size_s() - 1 - i].toDouble();
		ts += PIString::fromNumber(TF.vector_Bm[i]);
		if (i > 1) {
			ts += "s^{" + PIString::fromNumber(i) + "} + ";
			continue;
		}
		if (i > 0) ts += "s" + PIString(" + ");
	}
	if (ts.left(2) == "1s") ts.pop_front();
	if (ts.left(3) == "-1s") ts.replace(0, 2, "-");
	note_ += ts + "\n---\n";
	sl = parameters[2].toString().split(" ");
	sl.removeStrings(""); sl.removeStrings("\t");
	TF.vector_An.resize(sl.size());
	ts.clear();
	for (int i = sl.size_s() - 1; i >= 0; --i) {
		TF.vector_An[i] = sl[sl.size_s() - 1 - i].toDouble();
		ts += PIString::fromNumber(TF.vector_An[i]);
		if (i > 1) {
			ts += "s^{" + PIString::fromNumber(i) + "} + ";
			continue;
		}
		if (i > 0) ts += "s" + PIString(" + ");
	}
	if (ts.left(2) == "1s") ts.pop_front();
	if (ts.left(3) == "-1s") ts.replace(0, 2, "-");
	note_ += ts;
	note_.replaceAll("+ -", "- ");
	note_.replaceAll(" + 1s", " + s");
	note_.replaceAll(" - 1s", " - s");
}


bool BrickLinkTransferFunction::tick_body(double time) {
	KF.setTime(time);
	KF.solve(inputs[0], dt);
	outputs[0] = KF.X[0];
	return true;
}


BrickLinkFilter1Degree::BrickLinkFilter1Degree(double t, double k): BrickLinkBase(3, 2) {
	setParameters(t, k);
	type = "Filter1Degree";
	setName(type);
	inNames[1] = "T";
	inNames[2] = "K";
	outNames[0] = "Low Pass";
	outNames[1] = "High Pass";
	s.setPreamp(1, -1.);
	makeConnections();
}


bool BrickLinkFilter1Degree::tick_body(double time) {
	if (inputs[T] == 0.) return false;
	a.setGain(1. / inputs[T]);
	s.setPreamp(0, inputs[K]);
	s.setInputValue(0, inputs[Input]);
	s.tick(time);
	s.proceedConnections();
	a.tick(time);
	a.proceedConnections();
	i.tick(time);
	i.proceedConnections();
	outputs[LowPass] = i.output();
	outputs[HighPass] = s.output();
	return true;
}


BrickLinkFilterBandpass::BrickLinkFilterBandpass(double lf, double hf): BrickLinkBase(3, 1) {
	TF.vector_Bm.resize(2);
	TF.vector_An.resize(3, 1.);
	type = "FilterBandpass";
	setName(type);
	inNames[1] = "Low Freq";
	inNames[2] = "High Freq";
	setParameters(lf, hf);
	pl = ph = 0.;
}


bool BrickLinkFilterBandpass::tick_body(double time) {
	if (pl != inputs[LF] || ph != inputs[HF]) {
		pl = inputs[LF];
		ph = inputs[HF];
		if (pl != 0.) t0 = 1. / pl;
		else t0 = 0.;
		if (ph != 0.) t1 = 1. / ph;
		else t1 = 0.;
		TF.vector_Bm[0] = 0.;
		TF.vector_Bm[1] = t0;
		TF.vector_An[0] = 1.;
		TF.vector_An[1] = t0 + t1;
		TF.vector_An[2] = t0 * t1;
		KF.fromTF(TF);
	}
	KF.solve(inputs[0], dt);
	outputs[0] = KF.X[0];
	return true;
}



BrickLinkFilterMedian::BrickLinkFilterMedian(int count): BrickLinkBase(1, 1, 0) {
	type = "FilterMedian";
	setName(type);
	paramNames[0] = "Count";
	parameters[0].setValue(count);
	note_ = "Size of window";
	setParameters(count);
}


bool BrickLinkFilterMedian::tick_body(double time) {
	for (int i = 0; i < hist_.size_s() - 1; ++i)
		hist_[i] = hist_[i + 1];
	hist_.back() = inputs[0];
	hsort = hist_;
	hsort.sort();
	int size = hsort.size_s();
	if (size == 0) {
		outputs[0] = 0.;
	} else {
		if (size % 2 == 1)
			outputs[0] = hsort[size / 2];
		else
			outputs[0] = (hsort[size / 2 - 1] + hsort[size / 2]) / 2.;
	}
	return true;
}