1 files changed, 84 insertions, 9 deletions

diff --git a/src/control.cpp b/src/control.cpp
index f57baec..4abc899 100644
--- a/src/control.cpp
+++ b/src/control.cpp
@@ -1,16 +1,30 @@
 #include "control.h"
 #include <armadillo>
+#include <cassert>
+#include <cmath>
 
 
 namespace ct
 {
-	TransferFn::TransferFn(complex gain)
-		: gain(gain)
-		, num(math::Poly<complex>{1})
-		, den(math::Poly<complex>{1})
+	TransferFn::TransferFn()
+		: num(1)
+		, den(1)
 	{
-		num(0) = 1.;
-		den(0) = 1.;
+		num(0) = 1;
+		den(0) = 1;
+	}
+
+	TransferFn::TransferFn(math::PolyCx num, math::PolyCx den)
+		: num(num)
+		, den(den)
+	{}
+
+	complex TransferFn::dc_gain() const
+	{
+		arma::cx_vec z(1, arma::fill::zeros);
+		arma::cx_vec n = arma::polyval(num.coeffs, z);
+		arma::cx_vec d = arma::polyval(den.coeffs, z);
+		return n(0) / d(0);
 	}
 
 	bool TransferFn::is_proper() const
@@ -23,9 +37,70 @@ namespace ct
 		return den.degree() >= num.degree();
 	}
 
-	TransferFn feedback(const TransferFn& tf, double k)
+	TransferFn operator + (const TransferFn& g, const TransferFn& h)
+	{
+		TransferFn tf(g.num * h.den + h.num * g.den, g.den * g.den);
+		tf.canonicalize();
+		return tf;
+	}
+
+	TransferFn operator - (const TransferFn& g, const TransferFn& h)
 	{
-		// TransferFn tf_cl;
+		TransferFn tf = g + (-1. * h);
+		tf.canonicalize();
+		return tf;
+	}
+
+	TransferFn operator * (const TransferFn& g, const TransferFn& h)
+	{
+		TransferFn tf(g.num * h.num, g.den * h.den);
+		tf.canonicalize();
+		return tf;
+	}
+
+	TransferFn operator / (const TransferFn& g, const TransferFn& h)
+	{
+		TransferFn hinv = TransferFn(h.den, h.num);
+		hinv.canonicalize();
+		TransferFn tf = g * hinv;
+		tf.canonicalize();
+		return tf;
+	}
+
+	TransferFn operator * (const complex k, const TransferFn& h)
+	{
+		TransferFn tf(k * h.num, h.den);
+		tf.canonicalize();
+		return tf;
+	}
+
+	TransferFn operator / (const TransferFn& h, const complex k)
+	{
+		TransferFn tf((1. / k) * h.num, h.den);
+		tf.canonicalize();
+		return tf;
+	}
+
+
+	TransferFn feedback(const TransferFn& tf, complex k)
+	{
+		const TransferFn unit(math::PolyCx({1.}), math::PolyCx({1.}));
+		const TransferFn ol = k * tf;
+		const TransferFn bo = unit - ol;
+		const TransferFn fb = unit / bo;
+		const TransferFn tf_cl = ol * fb;
+		// const TransferFn tf_cl = (k * tf) / (unit - k * tf);
+		return tf_cl;
+	}
+
+	void TransferFn::canonicalize()
+	{
+		complex an = den(0);
+		if (std::abs(an) > 0)
+		{
+			num.coeffs /= an;
+			den.coeffs /= an;
+		}
 	}
 
 	LocusSeries::LocusSeries(double start, double end, size_t nsamples)
@@ -77,7 +152,7 @@ namespace ct
 		{
 			ss.A(ord - 1, i) = tf.den(ord - i);
 			if (i < tf.num.coeffs.n_elem)
-				ss.C(i) = tf.num(i) * tf.gain;
+				ss.C(i) = tf.num(i);
 		}
 	
 		return ss;