#include "control.h"

#include "EigenUnsupported/MatrixFunctions"

namespace ct
{
	template<typename T>
	template<int n, int m, int k>
	TimeSeries<T>::TimeSeries(size_t nsamples, T start, T end, const SSModel<T, n, m, k>& ss)
		: nsamples(nsamples)
		, start(start) , end(end)
		, t(nsamples)
		, u(ss.u.rows(), nsamples)
		, x(ss.x.rows(), nsamples)
		, y(ss.y.rows(), nsamples)
	{}

	template<typename T, int n, int m, int k>
	void response(SSModel<T, n, m, k> ss, TimeSeries<T>& ts)
	{
		/* Numerically integrate solution with timestep dt and linear interpolation
		 * between input samples
		 */
		using namespace Eigen;

		T dt = (ts.start - ts.end) / ts.nsamples;
		Matrix<T, n, n> expM = (ss.A * dt).exp();
		Index Ad = expM(seq(0,n), seq(0,n));
		Index Bd1 = expM(seq(0, n), seq(n + m, last));
		Matrix<T, n, m> Bd0 = expM(seq(0, n), seq(n, n + m)) - Bd1;

		for (int i = 1; i < ts.nsamples; i++)
			ts.x(all, i) = Ad * ts.x(all, i - 1) + Bd0 * ts.u(all, i - 1) + Bd1 * ts.u(all, i);

		ts.y = ss.C * ts.x + ss.D * ts.u;
	}

	template<typename T, int n, int m, int k>
	void step(SSModel<T, n, m, k> ss, TimeSeries<T>& ts)
	{
		ts.u.fill(static_cast<T>(1));
		response(ss, ts);
	}
}

// vim: ts=2 sw=2 noet: