1 files changed, 56 insertions, 15 deletions

diff --git a/src/control.h b/src/control.h
index 8ae9c5d..587b80e 100644
--- a/src/control.h
+++ b/src/control.h
@@ -1,9 +1,12 @@
 #pragma once
 
-#include <armadillo>
+#include "armadillo/include/armadillo"
+
+#include <utility>
 #include <complex>
 #include <cassert>
 #include <cmath>
+#include <cstddef>
 
 #ifndef CONTROL_H
 #define CONTROL_H
@@ -14,7 +17,8 @@
 
 namespace ct
 {
-	using complex = std::complex<double>;
+	typedef std::complex<double> complex;
+	typedef std::size_t size_t;
 
 	namespace math
 	{
@@ -23,11 +27,19 @@ namespace ct
 		{
 			arma::Col<T> coeffs;
 
-			Poly(size_t n_elem) : coeffs(n_elem) {};
+			Poly(size_t n_elem) : coeffs(n_elem) {}
 			Poly(arma::Col<T> c) : coeffs(c) {}
 
+			template<typename U>
+			Poly(std::initializer_list<U> l) : coeffs(l.size())
+			{
+				int i = 0;
+				for (const U& u : l)
+					coeffs(i++) = T(u);
+			}
+
 			int degree() const { return coeffs.n_elem - 1; }
-			void add_root(T r) { (*this) = (*this) * Poly({1, -r}); }
+			void add_root(T r) { (*this) = (*this) * Poly({1., -r}); }
 			arma::Col<T> roots() const { return arma::roots(coeffs); }
 
 			template<typename IndexT>
@@ -41,6 +53,7 @@ namespace ct
 		Poly<T> operator * (const Poly<T>& p, const Poly<T>& q)
 		{
 			arma::Col<T> coeffs = arma::conv(p.coeffs, q.coeffs);
+			CT_ASSERT(!coeffs.has_nan());
 			return Poly<T>(coeffs);
 		}
 
@@ -48,6 +61,15 @@ namespace ct
 		Poly<T> operator * (R scalar, const Poly<T>& p)
 		{
 			Poly<T> q(p.coeffs * scalar);
+			CT_ASSERT(!q.coeffs.has_nan());
+			return q;
+		}
+
+		template<typename T, typename R = T>
+		Poly<T> operator / (R scalar, const Poly<T>& p)
+		{
+			Poly<T> q(p.coeffs / scalar);
+			CT_ASSERT(!q.coeffs.has_nan());
 			return q;
 		}
 
@@ -57,34 +79,53 @@ namespace ct
 			const Poly<T>& big = (p.degree() > q.degree()) ? p : q;
 			const Poly<T>& small = (p.degree() > q.degree()) ? q : p;
 
-			Poly<T> s(big.degree() + 1);
-
-			for (int i = 0; i <= small.degree(); i++)
-				s(i) = big(i) + small(i);
+			Poly<T> s(small);
+			int rel = big.degree() - small.degree();
+			s.coeffs.insert_rows(0, rel);
 
-			for (int i = small.degree() + 1; i <= big.degree(); i++)
-				s(i) = big(i);
+			CT_ASSERT(!s.coeffs.has_nan());
+			return Poly<T>(big.coeffs + s.coeffs);
+		}
 
-			return s;
+		template<typename T>
+		Poly<T> operator - (const Poly<T>& p, const Poly<T>& q)
+		{
+			return p + (-1. * q);
 		}
+
+		typedef Poly<complex> PolyCx;
 	}
 
 	struct TransferFn
 	{
 		complex gain;
-		math::Poly<complex> num;
-		math::Poly<complex> den;
+		math::PolyCx num;
+		math::PolyCx den;
 
-		TransferFn(complex gain);
+		TransferFn(void);
+		TransferFn(math::PolyCx num, math::PolyCx den);
 
 		inline void add_pole(complex p) { den.add_root(p); }
 		inline void add_zero(complex z) { num.add_root(z); };
 
+		complex dc_gain() const;
+
 		bool is_proper() const;
 		bool is_strictly_proper() const;
+
+		void canonicalize();
 	};
 
-	TransferFn feedback(const TransferFn& tf, double k = -1);
+	TransferFn operator + (const TransferFn& g, const TransferFn& h);
+	TransferFn operator - (const TransferFn& g, const TransferFn& h);
+	TransferFn operator * (const TransferFn& g, const TransferFn& h);
+	TransferFn operator / (const TransferFn& g, const TransferFn& h);
+	TransferFn operator * (const complex k, const TransferFn& h);
+	TransferFn operator / (const TransferFn& h, const complex k);
+
+	// inline TransferFn operator / (const TransferFn&
+
+	TransferFn feedback(const TransferFn& tf, complex k = -1);
 
 	struct LocusSeries
 	{