/**
 * \file dcs/testbed/lq_application_manager.hpp
 *
 * \brief Linear-Quadratic (LQ) system manager.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2012 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef DCS_TESTBED_LQ_SYSTEM_MANAGER_HPP
#define DCS_TESTBED_LQ_SYSTEM_MANAGER_HPP

#include <algorithm>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_expression.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/operation/all.hpp>
#include <boost/numeric/ublasx/operation/any.hpp>
#include <boost/numeric/ublasx/operation/inv.hpp>
#include <boost/numeric/ublasx/operation/isfinite.hpp>
#include <boost/numeric/ublasx/operation/num_columns.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>
#include <boost/smart_ptr.hpp>
#include <cmath>
#include <cstddef>
#include <dcs/assert.hpp>
#include <dcs/control/analysis/controllability.hpp>
#include <dcs/control/analysis/detectability.hpp>
#include <dcs/control/analysis/observability.hpp>
#include <dcs/control/analysis/stabilizability.hpp>
#include <dcs/control/design/dlqry.hpp>
#include <dcs/debug.hpp>
#include <dcs/exception.hpp>
#include <dcs/logging.hpp>
#include <dcs/macro.hpp>
#include <dcs/math/traits/float.hpp>
#include <dcs/testbed/application_performance_category.hpp>
#include <dcs/testbed/base_application_manager.hpp>
#include <dcs/testbed/system_identification_strategies.hpp>
#ifdef DCS_TESTBED_EXP_LQ_APP_MGR_USE_ARX_B0_SIGN_HEURISTIC
# include <functional>
#endif // DCS_TESTBED_EXP_LQ_APP_MGR_USE_ARX_B0_SIGN_HEURISTIC
#include <limits>
#include <map>
#include <stdexcept>

namespace dcs { namespace testbed {

namespace detail { namespace /*<unnamed>*/ {

/**
 * \brief Convert a discrete model from ARX structure to a state-space model in
 *  the canonical controllable form.
 *
 * Given the following input-output system:
 * \f[
 *  y(k)+\sum_{i=1}^{n_a}y(k-i)=\sum_{i=1}^{n_b}u(k-d-i)
 * \f]
 * create the following state-space system:
 * \f{align}{
 *  x(k+1) &= Ax(k)+Bu(k),\\
 *  y(k)   &= Cx(k)+Du(k)
 * \f}
 * such that:
 * \f{align}{
 *  x(k) &=\begin{pmatrix}
 *          u(k-d-n_b) \\
 *          .          \\
 *          .          \\
 *          .          \\
 *          u(k-d-2)   \\
 *          y(k-n_a)   \\
 *          .          \\
 *          .          \\
 *          .          \\
 *          y(k-1)
 *      \end{pmatrix},\\
 *  u(k) &=\begin{pmatrix}
 *          u(k-d)
 *      \end{pmatrix},\\
 *  A &=\begin{pmatrix}
         0        & I         & 0         & \ldots & 0  \\
         0        & 0         & I         & \ldots & 0  \\
         .        & .         & 0         & \ldots & .  \\
         .        & .         & .         & \ldots & .  \\
         .        & .         & .         & \ldots & .  \\
         0        & 0         & 0         & \ldots & I  \\
         -A_{n_a} &-A_{n_a-1} &-A_{n_a-2} & \ldots &-A_1
 *      \end{pmatrix},\\
 *  B &=\begin{pmatrix}
 *       0  \\
 *       .	\\
 *       .	\\
 *       .	\\
 *       0  \\
 *       I
 *      \end{pmatrix},\\
 *  C &=\begin{pmatrix}
	     B_n-B_0*A_n & \ldots & B_0-B_0*A_0
 *      \end{pmatrix},\\
 *  D &=\begin{pmatrix}
 *       B_0
 *      \end{pmatrix}
 * \f}
 * NOTE: in our case \f$B_0=0\f$, so
 * \f{align}
 *  D &=\begin{pmatrix}
 *       B_n & \ldots & B_1 & 0
 *      \end{pmatrix},\\
 *  D &=\begin{pmatrix}
 *       0
 *      \end{pmatrix}
 * \f}.
 */
template <
	typename SysIdentStrategyT,
	typename AMatrixExprT,
	typename BMatrixExprT,
	typename CMatrixExprT,
	typename DMatrixExprT
>
void make_controllable_ss(SysIdentStrategyT const& sys_ident_strategy,
						  ::boost::numeric::ublas::matrix_container<AMatrixExprT>& A,
						  ::boost::numeric::ublas::matrix_container<BMatrixExprT>& B,
						  ::boost::numeric::ublas::matrix_container<CMatrixExprT>& C,
						  ::boost::numeric::ublas::matrix_container<DMatrixExprT>& D)
{
//DCS_DEBUG_TRACE("BEGIN make_ss");//XXX
	namespace ublas = ::boost::numeric::ublas;

	typedef typename ublas::promote_traits<
				typename ublas::promote_traits<
					typename ublas::promote_traits<
						typename ublas::matrix_traits<AMatrixExprT>::value_type,
						typename ublas::matrix_traits<BMatrixExprT>::value_type
					>::promote_type,
					typename ublas::matrix_traits<CMatrixExprT>::value_type
				>::promote_type,
				typename ublas::matrix_traits<DMatrixExprT>::value_type
			>::promote_type value_type;
	typedef ::std::size_t size_type; //FIXME: use type-promotion?

	const size_type rls_n_a(sys_ident_strategy.output_order());
	const size_type rls_n_b(sys_ident_strategy.input_order());
//	const size_type rls_d(sys_ident_strategy.input_delay());
	const size_type rls_n_y(sys_ident_strategy.num_outputs());
	const size_type rls_n_u(sys_ident_strategy.num_inputs());
	const size_type n_x(rls_n_a*rls_n_y);
	const size_type n_u(rls_n_b*rls_n_u);
//	const size_type n(::std::max(n_x,n_u));
	const size_type n_y(1);

	DCS_ASSERT(
			rls_n_y <= 1 && rls_n_u <= 1,
			DCS_EXCEPTION_THROW(
				::std::runtime_error,
				"Actually, only SISO cases are hanlded"
			)
		);
	DCS_ASSERT(
			rls_n_y == rls_n_u,
			DCS_EXCEPTION_THROW(
				::std::runtime_error,
				"Actually, only the same number of channel are treated"
			)
		);

	// Create the state matrix A
	// A=[ 0        I          0         ...  0  ;
	//     0        0          I         ...  0  ;
	//     .        .          .         ...  .
	//     .        .          .         ...  .
	//     .        .          .         ...  .
	// 	   0        0          0         ...  I  ;
	// 	  -A_{n_a} -A_{n_a-1} -A_{n_a-2} ... -A_1]
	if (n_x > 0)
	{
		size_type broffs(n_x-rls_n_y); // The bottom row offset

		A().resize(n_x, n_x, false);

		// The upper part of A is set to [0_{k,rls_n_y} I_{k,k}],
		// where: k=n_x-rls_n_y.
		ublas::subrange(A(), 0, broffs, 0, rls_n_y) = ublas::zero_matrix<value_type>(broffs,rls_n_y);
		ublas::subrange(A(), 0, broffs, rls_n_y, n_x) = ublas::identity_matrix<value_type>(broffs,broffs);

		if (rls_n_a > 0)
		{
			// Fill A with A_1, ..., A_{n_a}
			for (size_type i = 0; i < rls_n_a; ++i)
			{
				// Copy matrix -A_i from \hat{\Theta} into A.
				// In A the matrix A_i has to go in (rls_n_a-i)-th position:
				//   A(k:(k+n),((rls_n_a-i-1)*rls_n_y):((rls_n_a-i)*rls_n_y)) <- -A_i

				size_type c2((rls_n_a-i)*rls_n_y);
				size_type c1(c2-rls_n_y);

				////ublas::subrange(A(), broffs, n_x, c1, c2) = sys_ident_strategy.A(i+1);
				ublas::subrange(A(), broffs, n_x, c1, c2) = -sys_ident_strategy.A(i+1);
				//ublas::subrange(A(), broffs, n_x, c1, c2) = sys_ident_strategy.A(i+1);
			}
		}
		else
		{
			ublas::subrange(A(), broffs, n_x, 0, n_x) = ublas::zero_matrix<value_type>(rls_n_y,n_x);
		}
	}
	else
	{
		A().resize(0, 0, false);
	}
//DCS_DEBUG_TRACE("A="<<A);//XXX

	// Create the input matrix B
	// B=[0;
	//    .;
	//    .;
	//    .;
	//    0;
	//    I]
	if (n_x > 0 && rls_n_b > 0)
	{
		size_type broffs(n_x-rls_n_u); // The bottom row offset

		B().resize(n_x, n_u, false);

		// The upper part of B is set to 0_{k,n_u}
		// where: k=n_x-rls_n_u.
		ublas::subrange(B(), 0, broffs, 0, n_u) = ublas::zero_matrix<value_type>(broffs, n_u);
		// The lower part of B is set to I_{n_u,n_u}
		ublas::subrange(B(), broffs, n_x, 0, n_u) = ublas::identity_matrix<value_type>(n_u, n_u);
	}
	else
	{
		B().resize(0, 0, false);
	}
//DCS_DEBUG_TRACE("B="<<B);//XXX

	// Create the output matrix C
	// C=[M_n ... M_0]
	// where M_i=B_i-B_0*A_i
	// NOTE: in our case B_0=0, so M_i=B_i
	if (n_x > 0)
	{
		C().resize(n_y, n_x, false);

		for (size_type i = 0; i < rls_n_b; ++i)
		{
			size_type c2((rls_n_b-i)*rls_n_u);
			size_type c1(c2-rls_n_u);

			ublas::subrange(C(), 0, n_y, c1, c2) = sys_ident_strategy.B(i+1);
		}
	}
	else
	{
		C().resize(0, 0, false);
	}
//DCS_DEBUG_TRACE("C="<<C);//XXX

	// Create the transmission matrix D
	// D=[B0]
	// NOTE: in our case B_0=0, so D=[0]
	{
		D().resize(n_y, n_u, false);

		D() = ublas::zero_matrix<value_type>(n_y, n_u);
	}
//DCS_DEBUG_TRACE("D="<<D);//XXX

//DCS_DEBUG_TRACE("END make_ss");//XXX
}

/// Convert an ARX structure to a state-space model in the canonical observable form.
template <
    typename SysIdentStrategyT,
    typename AMatrixExprT,
    typename BMatrixExprT,
    typename CMatrixExprT,
    typename DMatrixExprT
>
inline
void make_observable_ss(SysIdentStrategyT const& sys_ident_strategy,
						::boost::numeric::ublas::matrix_container<AMatrixExprT>& A,
						::boost::numeric::ublas::matrix_container<BMatrixExprT>& B,
						::boost::numeric::ublas::matrix_container<CMatrixExprT>& C,
						::boost::numeric::ublas::matrix_container<DMatrixExprT>& D)
{
	make_controllable_ss(sys_ident_strategy, A, B, C, D);
	A() = ::boost::numeric::ublas::trans(A);
	B() = ::boost::numeric::ublas::trans(C);
	C() = ::boost::numeric::ublas::trans(B);
}

#if defined(DCS_TESTBED_EXP_LQ_APP_MGR_USE_ALT_SS) && DCS_TESTBED_EXP_LQ_APP_MGR_USE_ALT_SS == 'X'

/**
 * \brief Convert a discrete model from ARX structure to a state-space model.
 *
 * Given the following input-output system:
 * \f[
 *  y(k)+\sum_{i=1}^{n_a}y(k-i)=\sum_{i=1}^{n_b}u(k-d-i)
 * \f]
 * create the following state-space system:
 * \f{align}{
 *  x(k+1) &= Ax(k)+Bu(k),\\
 *  y(k)   &= Cx(k)+Du(k)
 * \f}
 * such that:
 * \f{align}{
 *  x(k) &=\begin{pmatrix}
 *          u(k-d-n_b) \\
 *          .          \\
 *          .          \\
 *          .          \\
 *          u(k-d-2)   \\
 *          y(k-n_a)   \\
 *          .          \\
 *          .          \\
 *          .          \\
 *          y(k-1)
 *      \end{pmatrix},\\
 *  u(k) &=\begin{pmatrix}
 *          u(k-d)
 *      \end{pmatrix},\\
 *  A &=\begin{pmatrix}
 *       0       & I         & 0         & \ldots & 0   & 0       & I         & 0         & \ldots & 0  \\
 *       0       & 0         & I         & \ldots & 0   & 0       & 0         & I         & \ldots & 0  \\
 *       .       & .         & .         & \ldots & .   & .       & .         & 0         & \ldots & .  \\
 *       .       & .         & .         & \ldots & .   & .       & .         & 0         & \ldots & .  \\
 *       .       & .         & .         & \ldots & .   & .       & .         & 0         & \ldots & .  \\
 *       0       & 0         & 0         & \ldots & I   & 0       & 0         & 0         & \ldots & I  \\
 *       B_{n_b} & B_{n_b-1} & B_{n_b-2} & \ldost & B_2 &-A_{n_a} &-A_{n_a-1} &-A_{n_a-2} & \ldots &-A_1
 *      \end{pmatrix},\\
 *  B &=\begin{pmatrix}
 *       I  \\
 *       0  \\
 *       .	\\
 *       .	\\
 *       .	\\
 *       0  \\
 *       B_1
 *      \end{pmatrix},\\
 *  C &=\begin{pmatrix}
 *       0 & \ldots & 0 & 1 & \ldots & 1 \\
 *       0 & \ldots & 0 & 1 & \ldots & 1 \\
 *       . & \ldots & . & . & \ldots & . \\
 *       . & \ldots & . & . & \ldots & . \\
 *       . & \ldots & . & . & \ldots & . \\
 *       0 & \ldots & 0 & 1 & \ldots & 1
 *      \end{pmatrix},\\
 *  D &=\begin{pmatrix}
 *       0 & \ldots & 0 \\
 *       0 & \ldots & 0 \\
 *       . & \ldots & . \\
 *       . & \ldots & . \\
 *       . & \ldots & . \\
 *       0 & \ldots & 0
 *      \end{pmatrix}
 * \f}
 */
template <
	typename SysIdentStrategyT,
	typename AMatrixExprT,
	typename BMatrixExprT,
	typename CMatrixExprT,
	typename DMatrixExprT
>
void make_ss(SysIdentStrategyT const& sys_ident_strategy,
			 ::boost::numeric::ublas::matrix_container<AMatrixExprT>& A,
			 ::boost::numeric::ublas::matrix_container<BMatrixExprT>& B,
			 ::boost::numeric::ublas::matrix_container<CMatrixExprT>& C,
			 ::boost::numeric::ublas::matrix_container<DMatrixExprT>& D)
{
//DCS_DEBUG_TRACE("BEGIN make_ss");//XXX
	namespace ublas = ::boost::numeric::ublas;

	typedef typename ublas::promote_traits<
				typename ublas::promote_traits<
					typename ublas::promote_traits<
						typename ublas::matrix_traits<AMatrixExprT>::value_type,
						typename ublas::matrix_traits<BMatrixExprT>::value_type
					>::promote_type,
					typename ublas::matrix_traits<CMatrixExprT>::value_type
				>::promote_type,
				typename ublas::matrix_traits<DMatrixExprT>::value_type
			>::promote_type value_type;
	typedef ::std::size_t size_type; //FIXME: use type-promotion?

	const size_type rls_n_a(sys_ident_strategy.output_order());
	const size_type rls_n_b(sys_ident_strategy.input_order());
//	const size_type rls_d(sys_ident_strategy.input_delay());
	const size_type rls_n_y(sys_ident_strategy.num_outputs());
	const size_type rls_n_u(sys_ident_strategy.num_inputs());
	const size_type n_x(rls_n_a*rls_n_y+(rls_n_b-1)*rls_n_u);
	const size_type n_u(rls_n_u);
//	const size_type n(::std::max(n_x,n_u));
	const size_type n_y(1);

	// Create the state matrix A
	// A=[ 0        I          0         ...  0    0        I          0         ...  0  ;
	//     0        0          I         ...  0    0        0          I         ...  0  ;
	//     .        .          .         ...  .    .        .          0         ...  .
	//     .        .          .         ...  .    .        .          0         ...  .
	//     .        .          .         ...  .    .        .          0         ...  .
	// 	   0        0          0         ...  I    0        0          0         ...  I  ;
	// 	   B_{n_b}  B_{n_b-1}  B_{n_b-2} ...  B_2 -A_{n_a} -A_{n_a-1} -A_{n_a-2} ... -A_1]
	if (n_x > 0)
	{
		size_type broffs(n_x-rls_n_y); // The bottom row offset
		size_type cboffs0(rls_n_u);
		size_type cboffs1(cboffs0+((rls_n_b > 2) ? (rls_n_b-2)*rls_n_u : 0));
		size_type caoffs0(cboffs1+rls_n_y);
		size_type caoffs1(caoffs0+((rls_n_a > 1) ? (rls_n_a-1)*rls_n_y : 0));

		A().resize(n_x, n_x, false);

		// The upper part of A is set to [0_{k,rls_n_u} I_{k,kb} 0_{k,rls_n_y} I_{k,ka}],
		// where: k=n_x-rls_n_y, kb=(rls_n_b-2)*rls_n_u, ka=(rls_n_a-1)*rls_n_y.
		if (cboffs0 > 0)
		{
			ublas::subrange(A(), 0, broffs, 0, cboffs0) = ublas::zero_matrix<value_type>(broffs,rls_n_u);
		}
		if (cboffs1 > cboffs0)
		{
			ublas::subrange(A(), 0, broffs, cboffs0, cboffs1) = ublas::identity_matrix<value_type>(broffs,cboffs1-cboffs0);
		}
		if (caoffs0 > cboffs1)
		{
			ublas::subrange(A(), 0, broffs, cboffs1, caoffs0) = ublas::zero_matrix<value_type>(broffs,caoffs0-cboffs1);
		}
		if (caoffs1 > caoffs0)
		{
			ublas::subrange(A(), 0, broffs, caoffs0, caoffs1) = ublas::identity_matrix<value_type>(broffs,caoffs1-caoffs0);
		}

		// Fill A with B_2, ..., B_{n_b}
		for (size_type i = 1; i < rls_n_b; ++i)
		{
			// Copy matrix B_i from \hat{\Theta} into A.
			// In A the matrix B_i has to go in (rls_n_b-i)-th position:
			//   A(k:(k+n),((rls_n_b-i-1)*rls_n_u):((rls_n_b-i)*rls_n_u)) <- B_i

			size_type c2((rls_n_b-i)*rls_n_u);
			size_type c1(c2-rls_n_u);

			ublas::subrange(A(), broffs, n_x, c1, c2) = sys_ident_strategy.B(i+1);
		}

		// Fill A with A_1, ..., A_{n_a}
		for (size_type i = 0; i < rls_n_a; ++i)
		{
			// Copy matrix -A_i from \hat{\Theta} into A.
			// In A the matrix A_i has to go in ((rls_n_b-1)*rls_n_u+rls_n_a-i)-th position:
			//   A(k:(k+n),((rls_n_b-1)*rls_n_u+(rls_n_a-i-1)*rls_n_y):((rls_n_b-1)*rls_n_u+(rls_n_a-i)*rls_n_y)) <- -A_i

			size_type c2(cboffs1+(rls_n_a-i)*rls_n_y);
			size_type c1(c2-rls_n_y);

			////ublas::subrange(A(), broffs, n_x, c1, c2) = sys_ident_strategy.A(i+1);
			ublas::subrange(A(), broffs, n_x, c1, c2) = -sys_ident_strategy.A(i+1);
			//ublas::subrange(A(), broffs, n_x, c1, c2) = sys_ident_strategy.A(i+1);
		}
	}
	else
	{
		A().resize(0, 0, false);
	}
//DCS_DEBUG_TRACE("A="<<A);//XXX

	// Create the input matrix B
	// B=[I  ;
	//    0  ;
	//    .	 ;
	//    .	 ;
	//    .	 ;
	//    0  ;
	//    B_1]
	if (n_x > 0)
	{
		size_type broffs(n_x-rls_n_u); // The bottom row offset

		B().resize(n_x, n_u, false);

		// The upper part of B is set to [I_{n_u,n_u} 0_{k,n_u}]
		// where: k=n_x-rls_n_u.
		ublas::subrange(B(), 0, n_u, 0, n_u) = ublas::identity_matrix<value_type>(n_u,n_u);
		ublas::subrange(B(), n_u, broffs, 0, n_u) = ublas::zero_matrix<value_type>(broffs-n_u,n_u);
		// The bottom part of B with B_1
		ublas::subrange(B(), broffs, n_x, 0, n_u) = sys_ident_strategy.B(1);
	}
	else
	{
		B().resize(0, 0, false);
	}
//DCS_DEBUG_TRACE("B="<<B);//XXX

	// Create the output matrix C
	// C=[0 0 ... 1 ... 1]
	if (n_x > 0)
	{
		size_type rcoffs(n_x-rls_n_y); // The right most column offset

		C().resize(n_y, n_x, false);

		ublas::subrange(C(), 0, n_y, 0, rcoffs) = ublas::zero_matrix<value_type>(n_y,rcoffs);
		ublas::subrange(C(), 0, n_y, rcoffs, n_x) = ublas::scalar_matrix<value_type>(n_y, rls_n_y, 1);
	}
	else
	{
		C().resize(0, 0, false);
	}
//DCS_DEBUG_TRACE("C="<<C);//XXX

	// Create the transmission matrix D
	{
		D().resize(n_y, n_u, false);

		D() = ublas::zero_matrix<value_type>(n_y, n_u);
	}
//DCS_DEBUG_TRACE("D="<<D);//XXX

//DCS_DEBUG_TRACE("END make_ss");//XXX
}

#elif defined(DCS_TESTBED_EXP_LQ_APP_MGR_USE_ALT_SS) && DCS_TESTBED_EXP_LQ_APP_MGR_USE_ALT_SS == 'Y'

/**
 * \brief Convert a discrete model from ARX structure to a state-space model.
 *
 * Given the following input-output system:
 * \f[
 *  y(k)+\sum_{i=1}^{n_a}y(k-i)=\sum_{i=1}^{n_b}u(k-d-i)
 * \f]
 * create the following state-space system:
 * \f{align}{
 *  x(k+1) &= Ax(k)+Bu(k),\\
 *  y(k)   &= Cx(k)+Du(k)
 * \f}
 * such that:
 * \f{align}{
 *  x(k) &=\begin{pmatrix}
 *          u(k-d-n_b+1) \\
 *          .            \\
 *          .            \\
 *          .            \\
 *          u(k-d-1)     \\
 *          y(k-n_a+1)   \\
 *          .            \\
 *          .            \\
 *          .            \\
 *          y(k)
 *      \end{pmatrix},\\
 *  u(k) &=\begin{pmatrix}
 *          u(k-d)
 *      \end{pmatrix},\\
 *  A &=\begin{pmatrix}
 *       0       & I         & 0         & \ldots & 0   & 0       & I         & 0         & \ldots & 0  \\
 *       0       & 0         & I         & \ldots & 0   & 0       & 0         & I         & \ldots & 0  \\
 *       .       & .         & .         & \ldots & .   & .       & .         & 0         & \ldots & .  \\
 *       .       & .         & .         & \ldots & .   & .       & .         & 0         & \ldots & .  \\
 *       .       & .         & .         & \ldots & .   & .       & .         & 0         & \ldots & .  \\
 *       0       & 0         & 0         & \ldots & I   & 0       & 0         & 0         & \ldots & I  \\
 *       B_{n_b} & B_{n_b-1} & B_{n_b-2} & \ldost & B_2 &-A_{n_a} &-A_{n_a-1} &-A_{n_a-2} & \ldots &-A_1
 *      \end{pmatrix},\\
 *  B &=\begin{pmatrix}
 *       0  \\
 *       .  \\
 *       .	\\
 *       .	\\
 *       0	\\
 *       I	\\
 *       0	\\
 *       .	\\
 *       .	\\
 *       .	\\
 *       0  \\
 *       B_1
 *      \end{pmatrix},\\
 *  C &=\begin{pmatrix}
 *       0 & \ldots & 0 & I
 *      \end{pmatrix},\\
 *  D &=\begin{pmatrix}
 *       0
 *      \end{pmatrix}
 * \f}
 */
template <
	typename SysIdentStrategyT,
	typename AMatrixExprT,
	typename BMatrixExprT,
	typename CMatrixExprT,
	typename DMatrixExprT
>
void make_ss(SysIdentStrategyT const& sys_ident_strategy,
			 ::boost::numeric::ublas::matrix_container<AMatrixExprT>& A,
			 ::boost::numeric::ublas::matrix_container<BMatrixExprT>& B,
			 ::boost::numeric::ublas::matrix_container<CMatrixExprT>& C,
			 ::boost::numeric::ublas::matrix_container<DMatrixExprT>& D)
{
//DCS_DEBUG_TRACE("BEGIN make_ss");//XXX
	namespace ublas = ::boost::numeric::ublas;

	typedef typename ublas::promote_traits<
				typename ublas::promote_traits<
					typename ublas::promote_traits<
						typename ublas::matrix_traits<AMatrixExprT>::value_type,
						typename ublas::matrix_traits<BMatrixExprT>::value_type
					>::promote_type,
					typename ublas::matrix_traits<CMatrixExprT>::value_type
				>::promote_type,
				typename ublas::matrix_traits<DMatrixExprT>::value_type
			>::promote_type value_type;
	typedef ::std::size_t size_type; //FIXME: use type-promotion?

	const size_type rls_n_a(sys_ident_strategy.output_order());
	const size_type rls_n_b(sys_ident_strategy.input_order());
//	const size_type rls_d(sys_ident_strategy.input_delay());
	const size_type rls_n_y(sys_ident_strategy.num_outputs());
	const size_type rls_n_u(sys_ident_strategy.num_inputs());
	const size_type n_x(rls_n_a*rls_n_y+(rls_n_b-1)*rls_n_u);
	const size_type n_u(rls_n_u);
//	const size_type n(::std::max(n_x,n_u));
	const size_type n_y(1);

	// Create the state matrix A
	// A=[ 0        I          0         ...  0    0        I          0         ...  0  ;
	//     0        0          I         ...  0    0        0          I         ...  0  ;
	//     .        .          .         ...  .    .        .          0         ...  .
	//     .        .          .         ...  .    .        .          0         ...  .
	//     .        .          .         ...  .    .        .          0         ...  .
	// 	   0        0          0         ...  I    0        0          0         ...  I  ;
	// 	   B_{n_b}  B_{n_b-1}  B_{n_b-2} ...  B_2 -A_{n_a} -A_{n_a-1} -A_{n_a-2} ... -A_1]
	if (n_x > 0)
	{
		const size_type broffs(n_x-rls_n_y); // The bottom row offset
		const size_type cboffs0(rls_n_b > 1 ? rls_n_u : 0); // The column offset where to write the second B_i matrix (i.e., B_{n_b}-1 matrix)
		const size_type cboffs1(cboffs0+((rls_n_b > 2) ? (rls_n_b-2)*rls_n_u : 0)); // The column offset where to write the last B_i matrix (i.e., B_2 matrix)
		const size_type caoffs0(cboffs1+rls_n_y); // The column offset where to write the first A_i matrix (i.e., A_{n_a} matrix)
		const size_type caoffs1(caoffs0+((rls_n_a > 1) ? (rls_n_a-1)*rls_n_y : 0)); // The column offset where to write the last A_i matrix (i.e., A_1 matrix)

		A().resize(n_x, n_x, false);

		// The upper part of A is set to [0_{k,rls_n_u} I_{k,kb} 0_{k,rls_n_y} I_{k,ka}],
		// where: k=n_x-rls_n_y, kb=(rls_n_b-2)*rls_n_u, ka=(rls_n_a-1)*rls_n_y.
		if (cboffs0 > 0)
		{
			ublas::subrange(A(), 0, broffs, 0, cboffs0) = ublas::zero_matrix<value_type>(broffs,rls_n_u);
		}
		if (cboffs1 > cboffs0)
		{
			ublas::subrange(A(), 0, broffs, cboffs0, cboffs1) = ublas::identity_matrix<value_type>(broffs,cboffs1-cboffs0);
		}
		if (caoffs0 > cboffs1)
		{
			ublas::subrange(A(), 0, broffs, cboffs1, caoffs0) = ublas::zero_matrix<value_type>(broffs,caoffs0-cboffs1);
		}
		if (caoffs1 > caoffs0)
		{
			ublas::subrange(A(), 0, broffs, caoffs0, caoffs1) = ublas::identity_matrix<value_type>(broffs,caoffs1-caoffs0);
		}

		// Fill A with B_2, ..., B_{n_b}
		for (size_type i = 1; i < rls_n_b; ++i)
		{
			// Copy matrix B_i from \hat{\Theta} into A.
			// In A the matrix B_i has to go in (rls_n_b-i)-th position:
			//   A(k:(k+n),((rls_n_b-i-1)*rls_n_u):((rls_n_b-i)*rls_n_u)) <- B_i

			size_type c2((rls_n_b-i)*rls_n_u);
			size_type c1(c2-rls_n_u);

			ublas::subrange(A(), broffs, n_x, c1, c2) = sys_ident_strategy.B(i+1);
		}

		// Fill A with A_1, ..., A_{n_a}
		for (size_type i = 0; i < rls_n_a; ++i)
		{
			// Copy matrix -A_i from \hat{\Theta} into A.
			// In A the matrix A_i has to go in ((rls_n_b-1)*rls_n_u+rls_n_a-i)-th position:
			//   A(k:(k+n),((rls_n_b-1)*rls_n_u+(rls_n_a-i-1)*rls_n_y):((rls_n_b-1)*rls_n_u+(rls_n_a-i)*rls_n_y)) <- -A_i

			size_type c2(cboffs1+(rls_n_a-i)*rls_n_y);
			size_type c1(c2-rls_n_y);

			////ublas::subrange(A(), broffs, n_x, c1, c2) = sys_ident_strategy.A(i+1);
			ublas::subrange(A(), broffs, n_x, c1, c2) = -sys_ident_strategy.A(i+1);
			//ublas::subrange(A(), broffs, n_x, c1, c2) = sys_ident_strategy.A(i+1);
		}
	}
	else
	{
		A().resize(0, 0, false);
	}
//DCS_DEBUG_TRACE("A="<<A);//XXX

	// Create the input matrix B
	// B=[0  ;
	//    0  ;
	//    .	 ;
	//    .	 ;
	//    .	 ;
	//    0  ;
	//    I  ;
	//    0  ;
	//    .	 ;
	//    .	 ;
	//    .	 ;
	//    0  ;
	//    B_1]
	if (n_x > 0 && n_u > 0)
	{
		const size_type iroffs((rls_n_b > 2) ? rls_n_u*(rls_n_b-2) : 0); // The row offset where to write the identity matrix
		const size_type zroffs(iroffs+rls_n_u); // The row offset where to write the second zero matrix
		const size_type broffs(n_x-rls_n_u); // The bottom row offset

		B().resize(n_x, n_u, false);

		// The upper part of B is set to [0_{h,n_u} I_{n_u,n_u} 0_{k,n_u}]
		// where: h=rls_n_u*(rls_n_b-2) and k=rls_n_y*(rls_n_a-1)
		if (iroffs > 0)
		{
			ublas::subrange(B(), 0, iroffs, 0, n_u) = ublas::zero_matrix<value_type>(iroffs,n_u);
		}
		ublas::subrange(B(), iroffs, zroffs, 0, n_u) = ublas::identity_matrix<value_type>(n_u,n_u);
		if (broffs > zroffs)
		{
			ublas::subrange(B(), zroffs, broffs, 0, n_u) = ublas::zero_matrix<value_type>(broffs-zroffs,n_u);
		}
		// The bottom part of B with B_1
		ublas::subrange(B(), broffs, n_x, 0, n_u) = sys_ident_strategy.B(1);
	}
	else
	{
		B().resize(0, 0, false);
	}
//DCS_DEBUG_TRACE("B="<<B);//XXX

	// Create the output matrix C
	// C=[0 0 ... I]
	if (n_x > 0)
	{
		size_type rcoffs(n_x-rls_n_y); // The right most column offset

		C().resize(n_y, n_x, false);

		ublas::subrange(C(), 0, n_y, 0, rcoffs) = ublas::zero_matrix<value_type>(n_y,rcoffs);
		ublas::subrange(C(), 0, n_y, rcoffs, n_x) = ublas::identity_matrix<value_type>(rls_n_y,rls_n_y);
	}
	else
	{
		C().resize(0, 0, false);
	}
//DCS_DEBUG_TRACE("C="<<C);//XXX

	// Create the transmission matrix D
	if (n_u > 0)
	{
		D().resize(n_y, n_u, false);

		D() = ublas::zero_matrix<value_type>(n_y, n_u);
	}
	else
	{
		D().resize(0, 0, false);
	}
//DCS_DEBUG_TRACE("D="<<D);//XXX

//DCS_DEBUG_TRACE("END make_ss");//XXX
}

#elif defined(DCS_TESTBED_EXP_LQ_APP_MGR_USE_ALT_SS) && DCS_TESTBED_EXP_LQ_APP_MGR_USE_ALT_SS == 'C'

/// Convert an ARX structure to a state-space model in the canonical controllable form.
template <
	typename SysIdentStrategyT,
	typename AMatrixExprT,
	typename BMatrixExprT,
	typename CMatrixExprT,
	typename DMatrixExprT
>
inline
void make_ss(SysIdentStrategyT const& sys_ident_strategy,
			 ::boost::numeric::ublas::matrix_container<AMatrixExprT>& A,
			 ::boost::numeric::ublas::matrix_container<BMatrixExprT>& B,
			 ::boost::numeric::ublas::matrix_container<CMatrixExprT>& C,
			 ::boost::numeric::ublas::matrix_container<DMatrixExprT>& D)
{
	make_controllable_ss(sys_ident_strategy, A, B, C, D);
}

#elif defined(DCS_TESTBED_EXP_LQ_APP_MGR_USE_ALT_SS) && DCS_TESTBED_EXP_LQ_APP_MGR_USE_ALT_SS == 'O'

/// Convert an ARX structure to a state-space model in the canonical observable form.
template <
	typename SysIdentStrategyT,
	typename AMatrixExprT,
	typename BMatrixExprT,
	typename CMatrixExprT,
	typename DMatrixExprT
>
inline
void make_ss(SysIdentStrategyT const& sys_ident_strategy,
			 ::boost::numeric::ublas::matrix_container<AMatrixExprT>& A,
			 ::boost::numeric::ublas::matrix_container<BMatrixExprT>& B,
			 ::boost::numeric::ublas::matrix_container<CMatrixExprT>& C,
			 ::boost::numeric::ublas::matrix_container<DMatrixExprT>& D)
{
	make_observable_ss(sys_ident_strategy, A, B, C, D);
}

#else // DCS_DES_TESTBED_EXP_LQ_APP_MGR_ALT_SS

template <
	typename SysIdentStrategyT,
	typename AMatrixExprT,
	typename BMatrixExprT,
	typename CMatrixExprT,
	typename DMatrixExprT
>
//void make_ss(rls_ff_mimo_proxy<TraitsT> const& sys_ident_strategy,
void make_ss(SysIdentStrategyT const& sys_ident_strategy,
			 ::boost::numeric::ublas::matrix_container<AMatrixExprT>& A,
			 ::boost::numeric::ublas::matrix_container<BMatrixExprT>& B,
			 ::boost::numeric::ublas::matrix_container<CMatrixExprT>& C,
			 ::boost::numeric::ublas::matrix_container<DMatrixExprT>& D)
{
//DCS_DEBUG_TRACE("BEGIN make_ss");//XXX
	namespace ublas = ::boost::numeric::ublas;

	typedef typename ublas::promote_traits<
				typename ublas::promote_traits<
					typename ublas::promote_traits<
						typename ublas::matrix_traits<AMatrixExprT>::value_type,
						typename ublas::matrix_traits<BMatrixExprT>::value_type
					>::promote_type,
					typename ublas::matrix_traits<CMatrixExprT>::value_type
				>::promote_type,
				typename ublas::matrix_traits<DMatrixExprT>::value_type
			>::promote_type value_type;
	typedef ::std::size_t size_type; //FIXME: use type-promotion?

	const size_type rls_n_a(sys_ident_strategy.output_order());
	const size_type rls_n_b(sys_ident_strategy.input_order());
//	const size_type rls_d(sys_ident_strategy.input_delay());
	const size_type rls_n_y(sys_ident_strategy.num_outputs());
	const size_type rls_n_u(sys_ident_strategy.num_inputs());
	const size_type n_x(rls_n_a*rls_n_y);
	const size_type n_u(rls_n_b*rls_n_u);
//	const size_type n(::std::max(n_x,n_u));
	const size_type n_y(1);

	// Create the state matrix A
	// A=[ 0        I          0         ...  0  ;
	//     0        0          I         ...  0  ;
	//     .        .          .         ...  .
	//     .        .          .         ...  .
	//     .        .          .         ...  .
	// 	   0        0          0         ...  I  ;
	// 	  -A_{n_a} -A_{n_a-1} -A_{n_a-2} ... -A_1]
	if (n_x > 0)
	{
		size_type broffs(n_x-rls_n_y); // The bottom row offset

		A().resize(n_x, n_x, false);

		// The upper part of A is set to [0_{k,rls_n_y} I_{k,k}],
		// where: k=n_x-rls_n_y.
		ublas::subrange(A(), 0, broffs, 0, rls_n_y) = ublas::zero_matrix<value_type>(broffs,rls_n_y);
		ublas::subrange(A(), 0, broffs, rls_n_y, n_x) = ublas::identity_matrix<value_type>(broffs,broffs);

		// Fill A with A_1, ..., A_{n_a}
		for (size_type i = 0; i < rls_n_a; ++i)
		{
			// Copy matrix -A_i from \hat{\Theta} into A.
			// In A the matrix A_i has to go in (rls_n_a-i)-th position:
			//   A(k:(k+n),((rls_n_a-i-1)*rls_n_y):((rls_n_a-i)*rls_n_y)) <- -A_i

			size_type c2((rls_n_a-i)*rls_n_y);
			size_type c1(c2-rls_n_y);

			////ublas::subrange(A(), broffs, n_x, c1, c2) = sys_ident_strategy.A(i+1);
			ublas::subrange(A(), broffs, n_x, c1, c2) = -sys_ident_strategy.A(i+1);
			//ublas::subrange(A(), broffs, n_x, c1, c2) = sys_ident_strategy.A(i+1);
		}
	}
	else
	{
		A().resize(0, 0, false);
	}
//DCS_DEBUG_TRACE("A="<<A);//XXX

	// Create the input matrix B
	// B=[0 ... 0;
	//    .	... .
	//    .	... .
	//    .	... .
	//    0 ... 0;
	//    B_{n_b} ... B_1]
	if (n_x > 0)
	{
		size_type broffs(n_x-rls_n_u); // The bottom row offset

		B().resize(n_x, n_u, false);

		// The upper part of B is set to 0_{k,n_u}
		// where: k=n_x-rls_n_u.
		ublas::subrange(B(), 0, broffs, 0, n_u) = ublas::zero_matrix<value_type>(broffs,n_u);

		// Fill B with B_1, ..., B_{n_b}
		for (size_type i = 0; i < rls_n_b; ++i)
		{
			// Copy matrix B_i from \hat{\Theta} into B.
			// In \hat{\Theta} the matrix B_i stays at:
			//   B_i <- (\hat{\Theta}(((n_a*n_y)+i):n_b:n_u,:))^T
			// but in B the matrix B_i has to go in (n_b-i)-th position:
			//   B(k:(k+n_x),((n_b-i-1)*n_u):((n_a-i)*n_u)) <- B_i

			size_type c2((rls_n_b-i)*rls_n_u);
			size_type c1(c2-rls_n_u);

			ublas::subrange(B(), broffs, n_x, c1, c2) = sys_ident_strategy.B(i+1);
		}
	}
	else
	{
		B().resize(0, 0, false);
	}
//DCS_DEBUG_TRACE("B="<<B);//XXX

	// Create the output matrix C
	if (n_x > 0)
	{
		size_type rcoffs(n_x-rls_n_y); // The right most column offset

		C().resize(n_y, n_x, false);

		ublas::subrange(C(), 0, n_y, 0, rcoffs) = ublas::zero_matrix<value_type>(n_y,rcoffs);
		ublas::subrange(C(), 0, n_y, rcoffs, n_x) = ublas::scalar_matrix<value_type>(n_y, rls_n_y, 1);
	}
	else
	{
		C().resize(0, 0, false);
	}
//DCS_DEBUG_TRACE("C="<<C);//XXX

	// Create the transmission matrix D
	{
		D().resize(n_y, n_u, false);

		D() = ublas::zero_matrix<value_type>(n_y, n_u);
	}
//DCS_DEBUG_TRACE("D="<<D);//XXX

//DCS_DEBUG_TRACE("END make_ss");//XXX
}

#endif // DCS_DES_TESTBED_EXP_LQ_APP_MGR_ALT_SS

}} // Namespace detail::<unnamed>

template <typename TraitsT>
class lq_application_manager: public base_application_manager<TraitsT>
{
	private: typedef base_application_manager<TraitsT> base_type;
	public: typedef typename base_type::traits_type traits_type;
	public: typedef typename base_type::uint_type uint_type;
	public: typedef typename traits_type::real_type real_type;
	private: typedef typename base_type::app_type app_type;
	private: typedef typename base_type::app_pointer app_pointer;
	private: typedef typename app_type::sensor_type sensor_type;
	private: typedef typename app_type::sensor_pointer sensor_pointer;
	protected: typedef ::boost::numeric::ublas::vector<real_type> numeric_vector_type;
	protected: typedef ::boost::numeric::ublas::matrix<real_type> numeric_matrix_type;
	private: typedef base_arx_system_identification_strategy<traits_type> sysid_strategy_type;
	private: typedef ::boost::shared_ptr<sysid_strategy_type> sysid_strategy_pointer;
	private: typedef ::std::vector<real_type> observation_container;
	private: typedef ::std::map<application_performance_category,observation_container> observation_map;
	private: typedef ::std::map<application_performance_category,real_type> target_map;
	private: typedef ::std::map<application_performance_category,sensor_pointer> sensor_map;

	private: static const uint_type default_sampling_time;
	private: static const uint_type default_control_time;
	private: static const real_type default_min_share;
	private: static const real_type default_max_share;
	private: static const real_type default_ewma_smoothing_factor;

	public: lq_application_manager()
	: nx_(0),
	  nu_(0),
	  ny_(0),
	  x_offset_(0),
	  u_offset_(0),
	  ctl_count_(0),
	  ctl_skip_count_(0),
	  ctl_fail_count_(0),
	  sysid_fail_count_(0),
	  ewma_sf_(default_ewma_smoothing_factor)
	{
		this->sampling_time(default_sampling_time);
		this->control_time(default_control_time);
	}

	public: void sysid_strategy(sysid_strategy_pointer const& p_strategy)
	{
		p_sysid_alg_ = p_strategy;
	}

	public: sysid_strategy_pointer sysid_strategy()
	{
		return p_sysid_alg_;
	}

	public: sysid_strategy_pointer sysid_strategy() const
	{
		return p_sysid_alg_;
	}

	protected: numeric_vector_type const& state_vector() const
	{
		return x_;
	}

	protected: numeric_vector_type& state_vector()
	{
		return x_;
	}

	protected: numeric_vector_type const& input_vector() const
	{
		return u_;
	}

	protected: numeric_vector_type& input_vector()
	{
		return u_;
	}

	protected: numeric_vector_type const& output_vector() const
	{
		return y_;
	}

	protected: numeric_vector_type& output_vector()
	{
		return y_;
	}

	private: numeric_vector_type lq_control(numeric_matrix_type const& A,
											numeric_matrix_type const& B,
											numeric_matrix_type const& C,
											numeric_matrix_type const& D)
	{
		return do_lq_control(A, B, C, D);
	}

	private: void do_reset()
	{
		// pre: p_sysid_alg_ != null
		DCS_ASSERT(p_sysid_alg_,
				   DCS_EXCEPTION_THROW(::std::runtime_error,
									   "System identification strategy is not set"));

		//[FIXME]
		DCS_ASSERT(tgt_map_.size() == 1,
				   DCS_EXCEPTION_THROW(::std::logic_error,
									   "Currently, only one application performace category is handled"));
		//[/FIXME]

		p_sysid_alg_->init();

		const ::std::size_t np(p_sysid_alg_->num_outputs());
		const ::std::size_t ns(p_sysid_alg_->num_inputs());
		const ::std::size_t na(p_sysid_alg_->output_order());
		const ::std::size_t nb(p_sysid_alg_->input_order());

#if defined(DCS_TESTBED_EXP_LQ_APP_MGR_USE_ALT_SS) && DCS_TESTBED_EXP_LQ_APP_MGR_USE_ALT_SS == 'X'
		nx_ = np*na+ns*(nb-1);
		nu_ = ns;
		ny_ = np;
		x_offset_ = (nx_ > 0) ? (nx_-np) : 0;
		u_offset_ = 0;
#elif defined(DCS_TESTBED_EXP_LQ_APP_MGR_USE_ALT_SS) && DCS_TESTBED_EXP_LQ_APP_MGR_USE_ALT_SS == 'Y'
		nx_ = np*na+ns*(nb-1);
		nu_ = ns;
		ny_ = np;
		x_offset_ = (nx_ > 0) ? (nx_-np) : 0;
		u_offset_ = 0;
#elif defined(DCS_TESTBED_EXP_LQ_APP_MGR_USE_ALT_SS) && DCS_TESTBED_EXP_LQ_APP_MGR_USE_ALT_SS == 'C'
		nx_ = np*na;
		nu_ = ns;
		ny_ = np;
		x_offset_ = (nx_ > 0) ? (nx_-np) : 0;
		u_offset_ = 0;
#elif defined(DCS_TESTBED_EXP_LQ_APP_MGR_USE_ALT_SS) && DCS_TESTBED_EXP_LQ_APP_MGR_USE_ALT_SS == 'O'
		nx_ = np*na;
		nu_ = ns;
		ny_ = np;
		x_offset_ = (nx_ > 0) ? (nx_-np) : 0;
		u_offset_ = 0;
#else // DCS_TESTBED_EXP_LQ_APP_MGR_ALT_SS
		nx_ = np*na;
		nu_ = ns*nb;
		ny_ = np;
		x_offset_ = (nx_ > 0) ? (nx_-np) : 0;
		u_offset_ = (nu_ > 0) ? (nu_-ns) : 0;
#endif // DCS_TESTBED_EXP_LQ_APP_MGR_ALT_SS
		x_ = numeric_vector_type(nx_, ::std::numeric_limits<real_type>::quiet_NaN());
		u_ = numeric_vector_type(nu_, ::std::numeric_limits<real_type>::quiet_NaN());
		y_ = numeric_vector_type(ny_, ::std::numeric_limits<real_type>::quiet_NaN());
		//yr_ = ::boost::numeric::ublas::scalar_vector<real_type>(ny_, tgt_map_.at(response_time_application_performance));
		yr_ = numeric_vector_type(ny_, ::std::numeric_limits<real_type>::quiet_NaN());
		typedef typename target_map::const_iterator target_iterator;
		target_iterator tgt_end_it = tgt_map_.end();
		for (target_iterator tgt_it = tgt_map_.begin();
			 tgt_it != tgt_end_it;
			 ++tgt_it)
		{
			application_performance_category cat(tgt_it->first);

			yr_ = numeric_vector_type(ny_, tgt_it->second);
			out_sens_map_[cat] = p_app_->sensor(cat);
		}
		ewma_s_ = numeric_vector_type(ns, ::std::numeric_limits<real_type>::quiet_NaN());
		ewma_p_ = numeric_vector_type(np, ::std::numeric_limits<real_type>::quiet_NaN());
		ctl_count_ = ctl_skip_count_
				   = ctl_fail_count_
				   = sysid_fail_count_
				   = 0;
	}

	private: void do_sample()
	{
		typedef typename sensor_type::observation_type obs_type;
		typedef ::std::vector<obs_type> obs_container;
		typedef typename obs_container::const_iterator obs_iterator;
		typedef typename sensor_map::const_iterator sensor_iterator;

		DCS_DEBUG_TRACE("(" << this << ") BEGIN Do SAMPLE - Count: " << ctl_count_ << "/" << ctl_skip_count_ << "/" << sysid_fail_count_ << "/" << ctl_fail_count_);

		sensor_iterator sens_end_it = out_sens_map_.end();
		for (sensor_iterator sens_it = out_sens_map_.begin();
			 sens_it != sens_end_it;
			 ++sens_it)
		{
			application_performance_category cat(sens_it->first);
			sensor_pointer p_sens(sens_it->second);

			p_sens->sense();
			if (p_sens->has_observations())
			{
				obs_container obs = p_sens->observations();
				obs_iterator end_it = obs.end();
				for (obs_iterator it = obs.begin();
					 it != end_it;
					 ++it)
				{
					out_obs_map_[cat].push_back(it->value());
				}
			}
		}

		DCS_DEBUG_TRACE("(" << this << ") END Do SAMPLE - Count: " << ctl_count_ << "/" << ctl_skip_count_ << "/" << sysid_fail_count_ << "/" << ctl_fail_count_);
	}

	private: void do_control()
	{
		namespace ublas = ::boost::numeric::ublas;
		namespace ublasx = ::boost::numeric::ublasx;

		typedef typename app_type::vm_pointer vm_pointer;
		typedef ::std::vector<vm_pointer> vm_container;
		typedef typename vm_container::iterator vm_iterator;
		typedef typename vm_container::const_iterator vm_citerator;

		DCS_DEBUG_TRACE("(" << this << ") BEGIN Do CONTROL - Count: " << ctl_count_ << "/" << ctl_skip_count_ << "/" << sysid_fail_count_ << "/" << ctl_fail_count_);

		const ::std::size_t np = p_sysid_alg_->num_outputs();
		const ::std::size_t ns = p_sysid_alg_->num_inputs();
		const ::std::size_t na = p_sysid_alg_->output_order();
		const ::std::size_t nb = p_sysid_alg_->input_order();
		const ::std::size_t nk = p_sysid_alg_->input_delay();

		bool skip_ctl = false;
		numeric_vector_type p(np, 0); // model output (performance measure)
		numeric_vector_type s(ns, 0); // model input (resource share)

		vm_container vms = this->app()->vms();

		++ctl_count_;

		// Update measures
		if (out_obs_map_.size() > 0)
		{
			typedef typename observation_map::const_iterator obs_map_iterator;
			typedef typename observation_container::const_iterator obs_iterator;

#if defined(DCS_TESTBED_APP_MGR_APPLY_EWMA_TO_EACH_OBSERVATION)
			//bool init_check = (ctl_count_ < 1) ? true : false;
			obs_map_iterator map_end_it = out_obs_map_.end();
			for (obs_map_iterator map_it = out_obs_map_.begin();
				 map_it != map_end_it;
				 ++map_it)
			{
				application_performance_category cat(map_it->first);

				obs_iterator end_it = map_it->second.end();
				for (obs_iterator it = map_it->second.begin();
					 it != end_it;
					 ++it)
				{
					real_type val(*it);

					if (::std::isnan(ewma_p_(0)))
					{
						ewma_p_(0) = val;
					}
					else
					{
						ewma_p_(0) = ewma_sf_*val+(1-ewma_sf_)*ewma_p_(0);
					}
				}
			}
#else // DCS_TESTBED_APP_MGR_APPLY_EWMA_TO_EACH_OBSERVATION
			obs_map_iterator map_end_it = out_obs_map_.end();
			for (obs_map_iterator map_it = out_obs_map_.begin();
				 map_it != map_end_it;
				 ++map_it)
			{
				application_performance_category cat(map_it->first);

				::boost::accumulators::accumulator_set< real_type, ::boost::accumulators::stats< ::boost::accumulators::tag::mean > > acc;
				obs_iterator end_it = map_it->second.end();
				for (obs_iterator it = map_it->second.begin();
					 it != end_it;
					 ++it)
				{
					acc(*it);
				}

				real_type aggr_obs = ::boost::accumulators::mean(acc);
				//if (ctl_count_ < 1)
				if (::std::isnan(ewma_p_(0)))
				{
					ewma_p_(0) = aggr_obs;
				}
				else
				{
					ewma_p_(0) = ewma_sf_*aggr_obs+(1-ewma_sf_)*ewma_p_(0);
				}
			}
#endif // DCS_TESTBED_APP_MGR_APPLY_EWMA_TO_EACH_OBSERVATION
		}
		else if (np > 0)
		{
			// No observation collected during the last control interval
			//TODO: what can we do?
			// - Skip control?
			// - Use the last EWMA value (if ctl_count_ > 1)?
			skip_ctl = true;
		}
		if (ns > 0)
		{
			::std::size_t v(0);

			vm_citerator vm_end_it = vms.end();
			for (vm_citerator vm_it = vms.begin();
				 vm_it != vm_end_it;
				 ++vm_it)
			{
				vm_pointer p_vm(*vm_it);

				// check: p_vm != null
				DCS_DEBUG_ASSERT( p_vm );

				real_type val(p_vm->cpu_share());

				if (::std::isnan(ewma_s_(v)))
				{
					ewma_s_(v) = val;
				}
				else
				{
					ewma_s_(v) = ewma_sf_*val+(1-ewma_sf_)*ewma_s_(v);
				}
			}
		}

		if (!skip_ctl)
		{
			// Rotate old with new inputs/outputs:
			//  x(k) = [p(k-n_a+1) ... p(k)]^T
			//       = [x_{n_p:n_x}(k-1) p(k)]^T
			//  u(k) = [s(k-n_b+1) ... s(k)]^T
			//       = [u_{n_s:n_u}(k-1) s(k)]^T
			// Check if a measure rotation is needed (always but the first time)
DCS_DEBUG_TRACE("Old x=" << x_);
DCS_DEBUG_TRACE("Old u=" << u_);
DCS_DEBUG_TRACE("Old y=" << y_);
			if (ctl_count_ > 1)
			{
				// throw away old observations from the state vector and make room for new ones
				if (nx_ > 0)
				{
#if defined(DCS_TESTBED_EXP_LQ_APP_MGR_USE_ALT_SS) && DCS_TESTBED_EXP_LQ_APP_MGR_USE_ALT_SS == 'X'
					if (nb > 1)
					{
						if (nb > 2)
						{
							ublas::subrange(x_, 0, (nb-2)*ns) = ublas::subrange(x_, ns, (nb-1)*ns);
						}
						ublas::subrange(x_, (nb-2)*ns, (nb-1)*ns) = u_;
					}
					ublas::subrange(x_, ns*(nb-1), nx_-np) = ublas::subrange(x_, (nb-1)*ns+np, nx_);
					ublas::subrange(x_, nx_-np, nx_) = ublas::scalar_vector<real_type>(np, ::std::numeric_limits<real_type>::quiet_NaN());
#elif defined(DCS_TESTBED_EXP_LQ_APP_MGR_USE_ALT_SS) && DCS_TESTBED_EXP_LQ_APP_MGR_USE_ALT_SS == 'C'
# error State-space representation in canonical controllable form has not fully implemented yet
#elif defined(DCS_TESTBED_EXP_LQ_APP_MGR_USE_ALT_SS) && DCS_TESTBED_EXP_LQ_APP_MGR_USE_ALT_SS == 'O'
					for (uint_type i = 0; i < na; ++i)
					{
						ublas::subrange(x_, np*i, np*(i+1)) = - ublas::prod(p_sysid_alg_->A(na-i), ublas::subrange(x_, nx_-np, nx_)) + ublas::prod(p_sysid_alg_->B(nb-i), u_);
						if (i > 0)
						{
							ublas::subrange(x_, np*i, np*(i+1)) = ublas::subrange(x_, np*i, (np+1)*i) + ublas::subrange(x_, np*(i-1), np*i);
						}
					}
#else // DCS_TESTBED_EXP_LQ_APP_MGR_USE_ALT_SS
					ublas::subrange(x_, 0, nx_-np) = ublas::subrange(x_, np, nx_);
					ublas::subrange(x_, nx_-np, nx_) = ublas::scalar_vector<real_type>(np, ::std::numeric_limits<real_type>::quiet_NaN());
#endif // DCS_TESTBED_EXP_LQ_APP_MGR_USE_ALT_SS
				}
				// throw away old observations from the input vector and make room for new ones
				if (nu_ > 0)
				{
#if defined(DCS_TESTBED_EXP_LQ_APP_MGR_USE_ALT_SS)
					u_ = ublas::scalar_vector<real_type>(ns, ::std::numeric_limits<real_type>::quiet_NaN());
#else // DCS_TESTBED_EXP_LQ_APP_MGR_USE_ALT_SS
					ublas::subrange(u_, 0, nu_-ns) = ublas::subrange(u_, ns, nu_);
					ublas::subrange(u_, nu_-ns, nu_) = ublas::scalar_vector<real_type>(ns, ::std::numeric_limits<real_type>::quiet_NaN());
#endif // DCS_TESTBED_EXP_LQ_APP_MGR_USE_ALT_SS
				}
			}
DCS_DEBUG_TRACE("Prep x=" << x_);
DCS_DEBUG_TRACE("Prep u=" << u_);
DCS_DEBUG_TRACE("Prep y=" << y_);

			// Update inputs/outputs
			if (nx_ > 0)
			{
				//FIXME: fix the assignment below
				//       Should we normalize/deviate/...?
//				for (::std::size_t v = 0; v < np; ++v)
//				{
//					x_(x_offset_+v) = p(v) = ewma_p_(v)/yr_(v);
//				}
				ublas::subrange(x_, x_offset_, nx_) = p
													= ublas::element_div(ewma_p_, yr_);
				//ublas::subrange(x_, x_offset_, nx_) = ublas::element_div(ewma_p_, yr_) - ublas::scalar_vector<real_type>(ny_, 1);
			}
			if (nu_ > 0)
			{
				//FIXME: actual share should be scaled according to the capacity of the "reference" machine

				::std::size_t v(0);

				vm_citerator vm_end_it = vms.end();
				for (vm_citerator vm_it = vms.begin();
					 vm_it != vm_end_it;
					 ++vm_it)
				{
					vm_pointer p_vm(*vm_it);

					// check: p_vm != null
					DCS_DEBUG_ASSERT( p_vm );

					u_(u_offset_+v) = s(v)
									= p_vm->cpu_share()/**p_vm->max_num_vcpus()*/;

					++v;
				}
			}
			if (ny_ > 0)
			{
				y_ = p;
			}
DCS_DEBUG_TRACE("New x=" << x_);
DCS_DEBUG_TRACE("New u=" << u_);
DCS_DEBUG_TRACE("New y=" << y_);

			// Estimate system params
			bool ok(true);
			numeric_vector_type ph;
			try
			{
				ph = p_sysid_alg_->estimate(p, s);
DCS_DEBUG_TRACE("RLS estimation:");//XXX
DCS_DEBUG_TRACE("p=" << p);//XXX
DCS_DEBUG_TRACE("s=" << s);//XXX
DCS_DEBUG_TRACE("p_hat=" << ph);//XXX
DCS_DEBUG_TRACE("Theta_hat=" << p_sysid_alg_->Theta_hat());//XXX
DCS_DEBUG_TRACE("P=" << p_sysid_alg_->P());//XXX
DCS_DEBUG_TRACE("phi=" << p_sysid_alg_->phi());//XXX

				if (!ublasx::all(ublasx::isfinite(p_sysid_alg_->Theta_hat())))
				{
					::std::ostringstream oss;
					oss << "Unable to estimate system parameters: infinite values in system parameters";
					::dcs::log_warn(DCS_LOGGING_AT, oss.str());

					ok = false;
				}
			}
			catch (::std::exception const& e)
			{
				DCS_DEBUG_TRACE( "Caught exception: " << e.what() );

				::std::ostringstream oss;
				oss << "Unable to estimate system parameters: " << e.what();
				::dcs::log_warn(DCS_LOGGING_AT, oss.str());

				ok = false;
			}

			if (ok && p_sysid_alg_->count() >= (na+nb+nk))
			{
				// Create the state-space representation of the system model:
				// x(k+1) = Ax(k)+Bu(k)
				// y(k)   = Cx(k)+Du(k)

				numeric_matrix_type A;
				numeric_matrix_type B;
				numeric_matrix_type C;
				numeric_matrix_type D;

				detail::make_ss(*p_sysid_alg_, A, B, C, D);
DCS_DEBUG_TRACE("State-space System - Matrix A: " << A);
DCS_DEBUG_TRACE("State-space System - Matrix B: " << B);
DCS_DEBUG_TRACE("State-space System - Matrix C: " << C);
DCS_DEBUG_TRACE("State-space System - Matrix D: " << D);

				numeric_vector_type opt_u;
				try
				{
                    // Check on B(1) suggested by Karlsson et al "Dynamic Black-Box Performance Model Estimation for Self-Tuning Regulators", 2005
                    // This essentially consider the model as a linear model where u(k) is the free variabile.
                    // They compute the first partial derivative wrt to u(k) which gives the matrix B(1).
                    // In order to preverse reverse proportionality ==> diag(B(1)) < 0
					if (ublasx::any(p_sysid_alg_->B(1), ::std::bind2nd(::std::greater_equal<real_type>(), 0)))
					{
#ifdef DCS_TESTBED_EXP_LQ_APP_MGR_USE_ARX_B0_SIGN_HEURISTIC
						DCS_EXCEPTION_THROW( ::std::runtime_error, "Cannot compute optimal control input: First partial derivative of input-output model has positive elements on the main diagonal" );
#else // DCS_TESTBED_EXP_LQ_APP_MGR_USE_ARX_B0_SIGN_HEURISTIC
						dcs::log_warn(DCS_LOGGING_AT, "First partial derivative of input-output model has positive elements on the main diagonal");
#endif // DCS_TESTBED_EXP_LQ_APP_MGR_USE_ARX_B0_SIGN_HEURISTIC
					}

					opt_u = this->lq_control(A, B, C, D);
				}
				catch (::std::exception const& e)
				{
					DCS_DEBUG_TRACE( "Caught exception: " << e.what() );

					::std::ostringstream oss;
					oss << "Unable to compute optimal control: " << e.what();
					::dcs::log_warn(DCS_LOGGING_AT, oss.str());

					ok = false;
				}

				if (ok)
				{
DCS_DEBUG_TRACE("Applying optimal control");
					//FIXME: new share should be scaled according to the capacity of the "real" machine
					//FIXME: implement the Physical Machine Manager

					if (ublasx::all(ublas::subrange(opt_u, u_offset_, vms.size()), ::std::bind2nd(::std::greater_equal<real_type>(), 0)))
					{
						::std::size_t v(0);
						vm_iterator vm_end_it = vms.end();
						for (vm_iterator vm_it = vms.begin();
							 vm_it != vm_end_it;
							 ++vm_it)
						{
							vm_pointer p_vm(*vm_it);

							// check: p_vm != null
							DCS_DEBUG_ASSERT( p_vm );

							real_type new_share = opt_u(u_offset_+v);

							if (::dcs::math::float_traits<real_type>::definitely_less(new_share, default_min_share))
							{
								::std::ostringstream oss;
								oss << "Optimal share (" << new_share << ") too small; adjusted to " << default_min_share;
								::dcs::log_warn(DCS_LOGGING_AT, oss.str());
							}
							if (::dcs::math::float_traits<real_type>::definitely_greater(new_share, default_max_share))
							{
								::std::ostringstream oss;
								oss << "Optimal share (" << new_share << ") too big; adjusted to " << default_max_share;
								::dcs::log_warn(DCS_LOGGING_AT, oss.str());
							}

							new_share = ::std::min(::std::max(new_share, default_min_share), default_max_share);
							const real_type old_share = p_vm->cpu_share();

DCS_DEBUG_TRACE("VM '" << p_vm->id() << "' - old-share: " << old_share << " - new-share: " << new_share);
							if (::std::isfinite(new_share) && !::dcs::math::float_traits<real_type>::essentially_equal(new_share, old_share))
							{
								p_vm->cpu_share(new_share);
							}

							++v;
						}
					}
					else
					{
						++ctl_fail_count_;

						::std::ostringstream oss;
						oss << "Control not applied: computed negative share for at least one VM";
						::dcs::log_warn(DCS_LOGGING_AT, oss.str());

						ok = false;
					}
DCS_DEBUG_TRACE("Optimal control applied");
				}
				else
				{
					++ctl_fail_count_;

					::std::ostringstream oss;
					oss << "Control not applied: failed to solve the control problem";
					::dcs::log_warn(DCS_LOGGING_AT, oss.str());
				}
			}
			else if (!ok)
			{
				p_sysid_alg_->reset();
				++sysid_fail_count_;

				::std::ostringstream oss;
				oss << "Control not applied: failed to solve the identification problem";
				::dcs::log_warn(DCS_LOGGING_AT, oss.str());
			}
		}
		else
		{
			++ctl_skip_count_;
		}

		// Reset measures
		out_obs_map_.clear();

		DCS_DEBUG_TRACE("(" << this << ") END Do CONTROL - Count: " << ctl_count_ << "/" << ctl_skip_count_ << "/" << sysid_fail_count_ << "/" << ctl_fail_count_);
	}

	private: virtual numeric_vector_type do_lq_control(numeric_matrix_type const& A,
													   numeric_matrix_type const& B,
													   numeric_matrix_type const& C,
													   numeric_matrix_type const& D) = 0;

	private: uint_type ts_; ///< Sampling time (in ms)
	private: uint_type tc_; ///< Control time (in ms)
	private: app_pointer p_app_; ///< Pointer to the managed application
	private: sensor_map out_sens_map_; ///< Sensor map for the application outputs
	private: sysid_strategy_pointer p_sysid_alg_;
	private: observation_map out_obs_map_; ///< Application output observations collected in the last control interval
	private: ::std::size_t nx_; ///< Number of states
	private: ::std::size_t nu_; ///< Number of inputs
	private: ::std::size_t ny_; ///< Number of outputs
	private: ::std::size_t x_offset_; ///< Offset used to rotate the state vector to make space for new observations
	private: ::std::size_t u_offset_; ///< Offset used to rotate the input vector to make space for new observations
	private: numeric_vector_type x_; ///< The state vector for the state-space representation
	private: numeric_vector_type u_; ///< The input vector for the state-space representation
	private: numeric_vector_type y_; ///< The output vector for the state-space representation
	private: numeric_vector_type yr_; ///< The output vector to be tracked
	private: ::std::size_t ctl_count_; ///< Number of times control function has been invoked
	private: ::std::size_t ctl_skip_count_; ///< Number of times control has been skipped
	private: ::std::size_t ctl_fail_count_; ///< Number of times control has failed
	private: ::std::size_t sysid_fail_count_; ///< Number of times system identification has failed
	private: real_type ewma_sf_; ///< EWMA smoothing factor
	private: numeric_vector_type ewma_s_; ///< Current EWMA values for inputs
	private: numeric_vector_type ewma_p_; ///< Current EWMA values for outputs
	private: target_map tgt_map_; ///< Mapping between application performance categories and target values
}; // lq_application_manager

template <typename T>
const typename lq_application_manager<T>::uint_type lq_application_manager<T>::default_sampling_time = 1;

template <typename T>
const typename lq_application_manager<T>::uint_type lq_application_manager<T>::default_control_time = 5;

template <typename T>
const typename lq_application_manager<T>::real_type lq_application_manager<T>::default_min_share = 0.20;

template <typename T>
const typename lq_application_manager<T>::real_type lq_application_manager<T>::default_max_share = 1.00;

template <typename T>
const typename lq_application_manager<T>::real_type lq_application_manager<T>::default_ewma_smoothing_factor = 0.70;

template <typename TraitsT>
class lqry_application_manager: public lq_application_manager<TraitsT>
{
	private: typedef lq_application_manager<TraitsT> base_type;
	public: typedef typename base_type::traits_type traits_type;
	private: typedef typename traits_type::real_type real_type;
	private: typedef typename traits_type::uint_type uint_type;
	private: typedef ::dcs::control::dlqry_controller<real_type> lq_controller_type;
	private: typedef typename base_type::numeric_vector_type numeric_vector_type;
	private: typedef typename base_type::numeric_matrix_type numeric_matrix_type;

	public: lqry_application_manager()
	{
	}

	public: template <typename QMatrixExprT, typename RMatrixExprT>
			lqry_application_manager(::boost::numeric::ublas::matrix_expression<QMatrixExprT> const& Q,
									 ::boost::numeric::ublas::matrix_expression<RMatrixExprT> const& R)
	: ctlr_(Q,R)
	{
	}

	private: numeric_vector_type do_lq_control(numeric_matrix_type const& A,
											   numeric_matrix_type const& B,
											   numeric_matrix_type const& C,
											   numeric_matrix_type const& D)
	{
		namespace ublas = ::boost::numeric::ublas;
		namespace ublasx = ::boost::numeric::ublasx;

		// Check: if (A,B) is stabilizable, then the assoicated DARE has a
		//        positive semidefinite solution.
		//        (sufficient and necessary condition)
		if (!::dcs::control::is_stabilizable(A, B, true))
		{
			::std::ostringstream oss;
			oss << "System (A,B) is not stabilizable (the associated DARE cannot have a positive semidefinite solution) [with A=" << A << " and B=" << B << "]";
			::dcs::log_warn(DCS_LOGGING_AT, oss.str());

			DCS_EXCEPTION_THROW(::std::runtime_error, oss.str());
		}
		// Check: if (A,B) stabilizable and (C'QC,A) detectable, then the
		//        associated DARE has a unique and stabilizing solution such
		//        that the closed-loop system:
		//          x(k+1) = Ax(k) + Bu(k) = (A + BK)x(k)
		//        is stable (K is the LQRY-optimal state feedback gain).
		//        (sufficient and necessary condition)
		numeric_matrix_type QQ(ublas::prod(ctlr_.Q(), C));
		QQ = ublas::prod(ublas::trans(C), QQ);
		if (!::dcs::control::is_detectable(A, QQ, true))
		{
			::std::ostringstream oss;
			oss << "System (C'QC,A) is not detectable (closed-loop system will not be stable) [with " << A << ", Q=" << ctlr_.Q() << " and C=" << C << "]";
			::dcs::log_warn(DCS_LOGGING_AT, oss.str());

			DCS_EXCEPTION_THROW(::std::runtime_error, oss.str());
		}

		uint_type nx(ublas::num_columns(A));
		uint_type nu(ublas::num_columns(B));
		uint_type ny(ublas::num_rows(C));
		numeric_vector_type r(ny, 1);//FIXME

		numeric_vector_type opt_u;

		ctlr_.solve(A, B, C, D);
		opt_u = ublas::real(ctlr_.control(this->state_vector()));

#ifdef DCS_TESTBED_EXP_LQ_APP_MGR_USE_COMPENSATION
		uint_type ncp(nx+nu);
		uint_type nrp(nx+ny);

		numeric_matrix_type P(nrp, ncp, 0);
		ublas::subrange(P, 0, nx, 0, nx) = ublas::identity_matrix<real_type>(nx, nx) - A;
		ublas::subrange(P, 0, nx, nx, ncp) = B;
		ublas::subrange(P, nx, nrp, 0, nx) = -C;
		ublas::subrange(P, nx, nrp, nx, ncp) = D;
		numeric_matrix_type Pt(ublas::trans(P));
		numeric_matrix_type PP(ublas::prod(P, Pt));
		bool inv = ublasx::inv_inplace(PP);
		if (inv)
		{
			PP = ublas::prod(Pt, PP);
			numeric_vector_type yd(nrp,0);
			ublas::subrange(yd, nx, nrp) = r;
			numeric_vector_type xdud(ublas::prod(PP, yd));
			DCS_DEBUG_TRACE("COMPENSATION: P=" << P << " ==> (xd,ud)=" << xdud << ", opt_u=" << opt_u);//XXX
			opt_u = opt_u + ublas::subrange(xdud, nx, ncp);
			DCS_DEBUG_TRACE("COMPENSATION: P=" << P << " ==> (xd,ud)=" << xdud << ", NEW opt_u=" << opt_u);//XXX
		}
		else
		{
			DCS_EXCEPTION_THROW( ::std::runtime_error, "Cannot compute equilibrium control input: Rosenbrock's system matrix is not invertible" );
		}
#endif // DCS_TESTBED_EXP_LQ_APP_MGR_USE_COMPENSATION

		return opt_u;
	}

	private: lq_controller_type ctlr_;
}; // lqry_application_manager

}} // Namespace dcs::testbed

#endif // DCS_TESTBED_LQ_SYSTEM_MANAGER_HPP


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 * Copyright 2012 The MITRE Corporation                                      *
 *                                                                           *
 * Licensed under the Apache License, Version 2.0 (the "License");           *
 * you may not use this file except in compliance with the License.          *
 * You may obtain a copy of the License at                                   *
 *                                                                           *
 *     http://www.apache.org/licenses/LICENSE-2.0                            *
 *                                                                           *
 * Unless required by applicable law or agreed to in writing, software       *
 * distributed under the License is distributed on an "AS IS" BASIS,         *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  *
 * See the License for the specific language governing permissions and       *
 * limitations under the License.                                            *
 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#include <Eigen/Dense>

#include "openbr_internal.h"

#include "openbr/core/common.h"
#include "openbr/core/eigenutils.h"
#include "openbr/core/opencvutils.h"

namespace br
{

/*!
 * \ingroup initializers
 * \brief Initialize Eigen
 * http://eigen.tuxfamily.org/dox/TopicMultiThreading.html
 * \author Scott Klum \cite sklum
 */
class EigenInitializer : public Initializer
{
    Q_OBJECT

    void initialize() const
    {
        Eigen::initParallel();
    }
};

BR_REGISTER(Initializer, EigenInitializer)

/*!
 * \ingroup transforms
 * \brief Projects input into learned Principal Component Analysis subspace.
 * \author Brendan Klare \cite bklare
 * \author Josh Klontz \cite jklontz
 */
class PCATransform : public Transform
{
    Q_OBJECT
    friend class DFFSTransform;
    friend class LDATransform;

protected:
    Q_PROPERTY(float keep READ get_keep WRITE set_keep RESET reset_keep STORED false)
    Q_PROPERTY(int drop READ get_drop WRITE set_drop RESET reset_drop STORED false)
    Q_PROPERTY(bool whiten READ get_whiten WRITE set_whiten RESET reset_whiten STORED false)

    /*!
     *     keep <  0: All eigenvalues are retained.
     *     keep =  0: No PCA performed, eigenvectors form an identity matrix.
     * 0 < keep <  1: Fraction of the variance to retain.
     *     keep >= 1: Number of leading eigenvectors to retain.
     */
    BR_PROPERTY(float, keep, 0.95)
    BR_PROPERTY(int, drop, 0)
    BR_PROPERTY(bool, whiten, false)

    Eigen::VectorXf mean, eVals;
    Eigen::MatrixXf eVecs;

    int originalRows;

public:
    PCATransform() : keep(0.95), drop(0), whiten(false) {}

private:
    double residualReconstructionError(const Template &src) const
    {
        Template proj;
        project(src, proj);

        Eigen::Map<const Eigen::VectorXf> srcMap(src.m().ptr<float>(), src.m().rows*src.m().cols);
        Eigen::Map<Eigen::VectorXf> projMap(proj.m().ptr<float>(), keep);

        return (srcMap - mean).squaredNorm() - projMap.squaredNorm();
    }

    void train(const TemplateList &trainingSet)
    {
        if (trainingSet.first().m().type() != CV_32FC1)
            qFatal("Requires single channel 32-bit floating point matrices.");

        originalRows = trainingSet.first().m().rows;
        int dimsIn = trainingSet.first().m().rows * trainingSet.first().m().cols;
        const int instances = trainingSet.size();

        // Map into 64-bit Eigen matrix
        Eigen::MatrixXd data(dimsIn, instances);
        for (int i=0; i<instances; i++)
            data.col(i) = Eigen::Map<const Eigen::MatrixXf>(trainingSet[i].m().ptr<float>(), dimsIn, 1).cast<double>();

        trainCore(data);
    }

    void project(const Template &src, Template &dst) const
    {
        dst = cv::Mat(1, keep, CV_32FC1);

        // Map Eigen into OpenCV
        Eigen::Map<const Eigen::MatrixXf> inMap(src.m().ptr<float>(), src.m().rows*src.m().cols, 1);
        Eigen::Map<Eigen::MatrixXf> outMap(dst.m().ptr<float>(), keep, 1);

        // Do projection
        outMap = eVecs.transpose() * (inMap - mean);
    }

    void store(QDataStream &stream) const
    {
        stream << keep << drop << whiten << originalRows << mean << eVals << eVecs;
    }

    void load(QDataStream &stream)
    {
        stream >> keep >> drop >> whiten >> originalRows >> mean >> eVals >> eVecs;
    }

protected:
    void trainCore(Eigen::MatrixXd data)
    {
        int dimsIn = data.rows();
        int instances = data.cols();
        const bool dominantEigenEstimation = (dimsIn > instances);

        Eigen::MatrixXd allEVals, allEVecs;
        if (keep != 0) {
            // Compute and remove mean
            mean = Eigen::VectorXf(dimsIn);
            for (int i=0; i<dimsIn; i++) mean(i) = data.row(i).sum() / (float)instances;
            for (int i=0; i<dimsIn; i++) data.row(i).array() -= mean(i);

            // Calculate covariance matrix
            Eigen::MatrixXd cov;
            if (dominantEigenEstimation) cov = data.transpose() * data / (instances-1.0);
            else                         cov = data * data.transpose() / (instances-1.0);

            // Compute eigendecomposition. Returns eigenvectors/eigenvalues in increasing order by eigenvalue.
            Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eSolver(cov);
            allEVals = eSolver.eigenvalues();
            allEVecs = eSolver.eigenvectors();
            if (dominantEigenEstimation) allEVecs = data * allEVecs;
        } else {
            // Null case
            mean = Eigen::VectorXf::Zero(dimsIn);
            allEVecs = Eigen::MatrixXd::Identity(dimsIn, dimsIn);
            allEVals = Eigen::VectorXd::Ones(dimsIn);
        }

        if (keep <= 0) {
            keep = dimsIn - drop;
        } else if (keep < 1) {
            // Keep eigenvectors that retain a certain energy percentage.
            const double totalEnergy = allEVals.sum();
            if (totalEnergy == 0) {
                keep = 0;
            } else {
                double currentEnergy = 0;
                int i=0;
                while ((currentEnergy / totalEnergy < keep) && (i < allEVals.rows())) {
                    currentEnergy += allEVals(allEVals.rows()-(i+1));
                    i++;
                }
                keep = i - drop;
            }
        } else {
            if (keep + drop > allEVals.rows())
                qFatal("Insufficient samples, needed at least %d but only got %d.", (int)keep + drop, (int)allEVals.rows());
        }

        // Keep highest energy vectors
        eVals = Eigen::VectorXf((int)keep, 1);
        eVecs = Eigen::MatrixXf(allEVecs.rows(), (int)keep);
        for (int i=0; i<keep; i++) {
            int index = allEVals.rows()-(i+drop+1);
            eVals(i) = allEVals(index);
            eVecs.col(i) = allEVecs.col(index).cast<float>() / allEVecs.col(index).norm();
            if (whiten) eVecs.col(i) /= sqrt(eVals(i));
        }

        // Debug output
        if (Globals->verbose) qDebug() << "PCA Training:\n\tDimsIn =" << dimsIn << "\n\tKeep =" << keep;
    }

    void writeEigenVectors(const Eigen::MatrixXd &allEVals, const Eigen::MatrixXd &allEVecs) const
    {
        const int originalCols = mean.rows() / originalRows;

        { // Write out mean image
            cv::Mat out(originalRows, originalCols, CV_32FC1);
            Eigen::Map<Eigen::MatrixXf> outMap(out.ptr<float>(), mean.rows(), 1);
            outMap = mean.col(0);
            // OpenCVUtils::saveImage(out, Globals->Debug+"/PCA/eigenVectors/mean.png");
        }

        // Write out sample eigen vectors (16 highest, 8 lowest), filename = eigenvalue.
        for (int k=0; k<(int)allEVals.size(); k++) {
            if ((k < 8) || (k >= (int)allEVals.size()-16)) {
                cv::Mat out(originalRows, originalCols, CV_64FC1);
                Eigen::Map<Eigen::MatrixXd> outMap(out.ptr<double>(), mean.rows(), 1);
                outMap = allEVecs.col(k);
                // OpenCVUtils::saveImage(out, Globals->Debug+"/PCA/eigenVectors/"+QString::number(allEVals(k),'f',0)+".png");
            }
        }
    }
};

BR_REGISTER(Transform, PCATransform)

/*!
 * \ingroup transforms
 * \brief PCA on each row.
 * \author Josh Klontz \cite jklontz
 */
class RowWisePCATransform : public PCATransform
{
    Q_OBJECT

    void train(const TemplateList &trainingSet)
    {
        if (trainingSet.first().m().type() != CV_32FC1)
            qFatal("Requires single channel 32-bit floating point matrices.");

        originalRows = trainingSet.first().m().rows;
        const int dimsIn = trainingSet.first().m().cols;
        int instances = 0;
        foreach (const Template &t, trainingSet)
            instances += t.m().rows;

        // Map into 64-bit Eigen matrix
        Eigen::MatrixXd data(dimsIn, instances);
        int index = 0;
        foreach (const Template &t, trainingSet)
            for (int i=0; i<t.m().rows; i++)
                data.col(index++) = Eigen::Map<const Eigen::MatrixXf>(t.m().ptr<float>(i), dimsIn, 1).cast<double>();

        PCATransform::trainCore(data);
    }

    void project(const Template &src, Template &dst) const
    {
        dst = cv::Mat(src.m().rows, keep, CV_32FC1);

        for (int i=0; i<src.m().rows; i++) {
            Eigen::Map<const Eigen::MatrixXf> inMap(src.m().ptr<float>(i), src.m().cols, 1);
            Eigen::Map<Eigen::MatrixXf> outMap(dst.m().ptr<float>(i), keep, 1);
            outMap = eVecs.transpose() * (inMap - mean);
        }
    }
};

BR_REGISTER(Transform, RowWisePCATransform)

/*!
 * \ingroup transforms
 * \brief Computes Distance From Feature Space (DFFS) \cite moghaddam97.
 * \author Josh Klontz \cite jklontz
 */
class DFFSTransform : public Transform
{
    Q_OBJECT
    Q_PROPERTY(float keep READ get_keep WRITE set_keep RESET reset_keep STORED false)
    BR_PROPERTY(float, keep, 0.95)

    PCATransform pca;
    Transform *cvtFloat;

    void init()
    {
        pca.keep = keep;
        cvtFloat = make("CvtFloat");
    }

    void train(const TemplateList &data)
    {
        pca.train((*cvtFloat)(data));
    }

    void project(const Template &src, Template &dst) const
    {
        dst = src;
        dst.file.set("DFFS", sqrt(pca.residualReconstructionError((*cvtFloat)(src))));
    }

    void store(QDataStream &stream) const
    {
        pca.store(stream);
    }

    void load(QDataStream &stream)
    {
        pca.load(stream);
    }
};

BR_REGISTER(Transform, DFFSTransform)

/*!
 * \ingroup transforms
 * \brief Projects input into learned Linear Discriminant Analysis subspace.
 * \author Brendan Klare \cite bklare
 * \author Josh Klontz \cite jklontz
 */
class LDATransform : public Transform
{
    friend class SparseLDATransform;

    Q_OBJECT
    Q_PROPERTY(float pcaKeep READ get_pcaKeep WRITE set_pcaKeep RESET reset_pcaKeep STORED false)
    Q_PROPERTY(bool pcaWhiten READ get_pcaWhiten WRITE set_pcaWhiten RESET reset_pcaWhiten STORED false)
    Q_PROPERTY(int directLDA READ get_directLDA WRITE set_directLDA RESET reset_directLDA STORED false)
    Q_PROPERTY(float directDrop READ get_directDrop WRITE set_directDrop RESET reset_directDrop STORED false)
    Q_PROPERTY(QString inputVariable READ get_inputVariable WRITE set_inputVariable RESET reset_inputVariable STORED false)
    Q_PROPERTY(bool isBinary READ get_isBinary WRITE set_isBinary RESET reset_isBinary STORED false)
    Q_PROPERTY(bool normalize READ get_normalize WRITE set_normalize RESET reset_normalize STORED false)
    BR_PROPERTY(float, pcaKeep, 0.98)
    BR_PROPERTY(bool, pcaWhiten, false)
    BR_PROPERTY(int, directLDA, 0)
    BR_PROPERTY(float, directDrop, 0.1)
    BR_PROPERTY(QString, inputVariable, "Label")
    BR_PROPERTY(bool, isBinary, false)
    BR_PROPERTY(bool, normalize, true)

    int dimsOut;
    Eigen::VectorXf mean;
    Eigen::MatrixXf projection;
    float stdDev;

    void train(const TemplateList &_trainingSet)
    {
        // creates "Label"
        TemplateList trainingSet = TemplateList::relabel(_trainingSet, inputVariable, isBinary);
        int instances = trainingSet.size();

        // Perform PCA dimensionality reduction
        PCATransform pca;
        pca.keep = pcaKeep;
        pca.whiten = pcaWhiten;
        pca.train(trainingSet);
        mean = pca.mean;

        TemplateList ldaTrainingSet;
        static_cast<Transform*>(&pca)->project(trainingSet, ldaTrainingSet);

        int dimsIn = ldaTrainingSet.first().m().rows * ldaTrainingSet.first().m().cols;

        // OpenBR ensures that class values range from 0 to numClasses-1.
        // Label exists because we created it earlier with relabel
        QList<int> classes = File::get<int>(trainingSet, "Label");
        QMap<int, int> classCounts = trainingSet.countValues<int>("Label");
        const int numClasses = classCounts.size();

        // Map Eigen into OpenCV
        Eigen::MatrixXd data = Eigen::MatrixXd(dimsIn, instances);
        for (int i=0; i<instances; i++)
            data.col(i) = Eigen::Map<const Eigen::MatrixXf>(ldaTrainingSet[i].m().ptr<float>(), dimsIn, 1).cast<double>();

        // Removing class means
        Eigen::MatrixXd classMeans = Eigen::MatrixXd::Zero(dimsIn, numClasses);
        for (int i=0; i<instances; i++)  classMeans.col(classes[i]) += data.col(i);
        for (int i=0; i<numClasses; i++) classMeans.col(i) /= classCounts[i];
        for (int i=0; i<instances; i++)  data.col(i) -= classMeans.col(classes[i]);

        PCATransform space1;

        if (!directLDA)
        {
            // The number of LDA dimensions is limited by the degrees
            // of freedom of scatter matrix computed from 'data'. Because
            // the mean of each class is removed (lowering degree of freedom
            // one per class), the total rank of the covariance/scatter
            // matrix that will be computed in PCA is bound by instances - numClasses.
            space1.keep = std::min(dimsIn, instances-numClasses);
            space1.trainCore(data);

            // Divide each eigenvector by sqrt of eigenvalue.
            // This has the effect of whitening the within-class scatter.
            // In effect, this minimizes the within-class variation energy.
            for (int i=0; i<space1.keep; i++) space1.eVecs.col(i) /= pow((double)space1.eVals(i),0.5);
        }
        else if (directLDA == 2)
        {
            space1.drop = instances - numClasses;
            space1.keep = std::min(dimsIn, instances) - space1.drop;
            space1.trainCore(data);
        }
        else
        {
            // Perform (modified version of) Direct LDA

            // Direct LDA uses to the Null space of the within-class scatter.
            // Thus, the lower rank, is used to our benefit. We are not discarding
            // these vectors now (in non-direct code we use the keep parameter
            // to discard Null space). We keep the Null space b/c this is where
            // the within-class scatter goes to zero, i.e. it is very useful.
            space1.keep = dimsIn;
            space1.trainCore(data);

            if (dimsIn > instances - numClasses) {
                // Here, we are replacing the eigenvalue of the  null space
                // eigenvectors with the eigenvalue (divided by 2) of the
                // smallest eigenvector from the row space eigenvector.
                // This allows us to scale these null-space vectors (otherwise
                // it is a divide by zero.
                double null_eig = space1.eVals(instances - numClasses - 1) / 2;
                for (int i = instances - numClasses; i < dimsIn; i++)
                    space1.eVals(i) = null_eig;
            }

            // Drop the first few leading eigenvectors in the within-class space
            QList<float> eVal_list; eVal_list.reserve(dimsIn);
            float fmax = -1;
            for (int i=0; i<dimsIn; i++) fmax = std::max(fmax, space1.eVals(i));
            for (int i=0; i<dimsIn; i++) eVal_list.append(space1.eVals(i)/fmax);

            QList<float> dSum = Common::CumSum(eVal_list);
            int drop_idx;
            for (drop_idx = 0; drop_idx<dimsIn; drop_idx++)
                if (dSum[drop_idx]/dSum[dimsIn-1] >= directDrop)
                    break;

            drop_idx++;
            space1.keep = dimsIn - drop_idx;

            Eigen::MatrixXf new_vecs = Eigen::MatrixXf(space1.eVecs.rows(), (int)space1.keep);
            Eigen::MatrixXf new_vals = Eigen::MatrixXf((int)space1.keep, 1);

            for (int i = 0; i < space1.keep; i++) {
                new_vecs.col(i) = space1.eVecs.col(i + drop_idx);
                new_vals(i) = space1.eVals(i + drop_idx);
            }

            space1.eVecs = new_vecs;
            space1.eVals = new_vals;

            // We will call this "agressive" whitening. Really, it is not whitening
            // anymore. Instead, we are further scaling the small eigenvalues and the
            // null space eigenvalues (to increase their impact).
            for (int i=0; i<space1.keep; i++) space1.eVecs.col(i) /= pow((double)space1.eVals(i),0.15);
        }

        // Now we project the mean class vectors into this second
        // subspace that minimizes the within-class scatter energy.
        // Inside this subspace we learn a subspace projection that
        // maximizes the between-class scatter energy.
        Eigen::MatrixXd mean2 = Eigen::MatrixXd::Zero(dimsIn, 1);

        // Remove means
        for (int i=0; i<dimsIn; i++)     mean2(i) = classMeans.row(i).sum() / numClasses;
        for (int i=0; i<numClasses; i++) classMeans.col(i) -= mean2;

        // Project into second subspace
        Eigen::MatrixXd data2 = space1.eVecs.transpose().cast<double>() * classMeans;

        // The rank of the between-class scatter matrix is bound by numClasses - 1
        // because each class is a vector used to compute the covariance,
        // but one degree of freedom is lost removing the global mean.
        int dim2 = std::min((int)space1.keep, numClasses-1);
        PCATransform space2;
        space2.keep = dim2;
        space2.trainCore(data2);

        // Compute final projection matrix
        projection = ((space2.eVecs.transpose() * space1.eVecs.transpose()) * pca.eVecs.transpose()).transpose();
        dimsOut = dim2;

        stdDev = 1; // default initialize
        if (isBinary) {
            assert(dimsOut == 1);
            float posVal = 0;
            float negVal = 0;
            Eigen::MatrixXf results(trainingSet.size(),1);
            for (int i = 0; i < trainingSet.size(); i++) {
                Template t;
                project(trainingSet[i],t);
                //Note: the positive class is assumed to be 0 b/c it will
                // typically be the first gallery template in the TemplateList structure
                if (classes[i] == 0)
                    posVal += t.m().at<float>(0,0);
                else if (classes[i] == 1)
                    negVal += t.m().at<float>(0,0);
                else
                    qFatal("Binary mode only supports two class problems.");
                results(i) = t.m().at<float>(0,0);  //used for normalization
            }
            posVal /= classCounts[0];
            negVal /= classCounts[1];

            if (posVal < negVal) {
                //Ensure positive value is supposed to be > 0 after projection
                Eigen::MatrixXf invert = Eigen::MatrixXf::Ones(dimsIn,1);
                invert *= -1;
                projection = invert.transpose() * projection;
            }

            if (normalize)
                stdDev = sqrt(results.array().square().sum() / trainingSet.size());
        }
    }

    void project(const Template &src, Template &dst) const
    {
        dst = cv::Mat(1, dimsOut, CV_32FC1);

        // Map Eigen into OpenCV
        Eigen::Map<Eigen::MatrixXf> inMap((float*)src.m().ptr<float>(), src.m().rows*src.m().cols, 1);
        Eigen::Map<Eigen::MatrixXf> outMap(dst.m().ptr<float>(), dimsOut, 1);

        // Do projection
        outMap = projection.transpose() * (inMap - mean);
        if (normalize && isBinary)
            dst.m().at<float>(0,0) = dst.m().at<float>(0,0) / stdDev;
    }

    void store(QDataStream &stream) const
    {
        stream << pcaKeep;
        stream << directLDA;
        stream << directDrop;
        stream << dimsOut;
        stream << mean;
        stream << projection;
        if (normalize && isBinary)
            stream << stdDev;
    }

    void load(QDataStream &stream)
    {
        stream >> pcaKeep;
        stream >> directLDA;
        stream >> directDrop;
        stream >> dimsOut;
        stream >> mean;
        stream >> projection;
        if (normalize && isBinary)
            stream >> stdDev;
    }
};

BR_REGISTER(Transform, LDATransform)

/*!
 * \ingroup transforms
 * \brief Projects input into learned Linear Discriminant Analysis subspace
 *          learned on a sparse subset of features with the highest weight
 *          in the original LDA algorithm.
 * \author Brendan Klare \cite bklare
 */
class SparseLDATransform : public Transform
{
    Q_OBJECT
    Q_PROPERTY(float varThreshold READ get_varThreshold WRITE set_varThreshold RESET reset_varThreshold STORED false)
    Q_PROPERTY(float pcaKeep READ get_pcaKeep WRITE set_pcaKeep RESET reset_pcaKeep STORED false)
    Q_PROPERTY(bool normalize READ get_normalize WRITE set_normalize RESET reset_normalize STORED false)
    BR_PROPERTY(float, varThreshold, 1.5)
    BR_PROPERTY(float, pcaKeep, 0.98)
    BR_PROPERTY(bool, normalize, true)

    LDATransform ldaSparse;
    int dimsOut;
    QList<int> selections;

    Eigen::VectorXf mean;

    void init()
    {
        ldaSparse.init();
        ldaSparse.pcaKeep = pcaKeep;
        ldaSparse.inputVariable = "Label";
        ldaSparse.isBinary = true;
        ldaSparse.normalize = true;
    }

    void train(const TemplateList &_trainingSet)
    {

        LDATransform ldaOrig;
        ldaOrig.init();
        ldaOrig.inputVariable = "Label";
        ldaOrig.pcaKeep = pcaKeep;
        ldaOrig.isBinary = true;
        ldaOrig.normalize = true;

        ldaOrig.train(_trainingSet);

        //Only works on binary class problems for now
        assert(ldaOrig.projection.cols() == 1);
        float ldaStd = eigStd(ldaOrig.projection);
        for (int i = 0; i < ldaOrig.projection.rows(); i++)
            if (abs(ldaOrig.projection(i)) > varThreshold * ldaStd)
                selections.append(i);

        TemplateList newSet;
        for (int i = 0; i < _trainingSet.size(); i++) {
            cv::Mat x(_trainingSet[i]);
            cv::Mat y = cv::Mat(selections.size(), 1, CV_32FC1);
            int idx = 0;
            int cnt = 0;
            for (int j = 0; j < x.rows; j++)
                for (int k = 0; k < x.cols; k++, cnt++)
                    if (selections.contains(cnt))
                        y.at<float>(idx++,0) = x.at<float>(j, k);
            newSet.append(Template(_trainingSet[i].file, y));
        }
        ldaSparse.train(newSet);
        dimsOut = ldaSparse.dimsOut;
    }

    void project(const Template &src, Template &dst) const
    {
        Eigen::Map<Eigen::MatrixXf> inMap((float*)src.m().ptr<float>(), src.m().rows*src.m().cols, 1);
        Eigen::Map<Eigen::MatrixXf> outMap(dst.m().ptr<float>(), dimsOut, 1);

        int d = selections.size();
        cv::Mat inSelect(d,1,CV_32F);
        for (int i = 0; i < d; i++)
            inSelect.at<float>(i) = src.m().at<float>(selections[i]);
        ldaSparse.project(Template(src.file, inSelect), dst);
    }

    void store(QDataStream &stream) const
    {
        stream << pcaKeep;
        stream << ldaSparse;
        stream << dimsOut;
        stream << selections;
    }

    void load(QDataStream &stream)
    {
        stream >> pcaKeep;
        stream >> ldaSparse;
        stream >> dimsOut;
        stream >> selections;
    }
};

BR_REGISTER(Transform, SparseLDATransform)

/*!
 * \ingroup distances
 * \brief L1 distance computed using eigen.
 * \author Josh Klontz \cite jklontz
 */
class L1Distance : public Distance
{
    Q_OBJECT

    float compare(const Template &a, const Template &b) const
    {
        const int size = a.m().rows * a.m().cols;
        Eigen::Map<Eigen::VectorXf> aMap((float*)a.m().data, size);
        Eigen::Map<Eigen::VectorXf> bMap((float*)b.m().data, size);
        return (aMap-bMap).cwiseAbs().sum();
    }
};

BR_REGISTER(Distance, L1Distance)

/*!
 * \ingroup distances
 * \brief L2 distance computed using eigen.
 * \author Josh Klontz \cite jklontz
 */
class L2Distance : public Distance
{
    Q_OBJECT

    float compare(const Template &a, const Template &b) const
    {
        const int size = a.m().rows * a.m().cols;
        Eigen::Map<Eigen::VectorXf> aMap((float*)a.m().data, size);
        Eigen::Map<Eigen::VectorXf> bMap((float*)b.m().data, size);
        return (aMap-bMap).squaredNorm();
    }
};

BR_REGISTER(Distance, L2Distance)

} // namespace br

#include "eigen3.moc"


#include "iri_geometry.h"
#include <math.h>
#include <iostream>
#include <Eigen/Dense>

// Spoint functions ------------------------------------------
Spoint::Spoint() : x(0) , y(0),  time_stamp(0) { }

Spoint::Spoint( double x_ , double y_ , double time_stamp_) :
		x(x_), y(y_), time_stamp(time_stamp_) { }
Spoint Spoint::operator+ (Spoint p2) const
{
	return Spoint(  x + p2.x , y + p2.y , time_stamp );
}
Spoint Spoint::operator- (Spoint p2) const
{
	return Spoint(  x - p2.x , y - p2.y , time_stamp);
}
Spoint Spoint::operator* (double k) const
{
	return Spoint(  x *k , y *k , time_stamp);
}
double Spoint::distance(Spoint p2) const
{
	return sqrt( distance2(p2)  );
}
double Spoint::distance2(Spoint p2) const
{
	return (x-p2.x)*(x-p2.x) + (y-p2.y)*(y-p2.y);
}
Spoint Spoint::propagate( double dt , Sforce f, double desired_velocity) const
{
	return *this;
}
void Spoint::print() const
{
	std::cout << "(x , y ,t) = (" << x << " , " << y  << " | " << time_stamp << " )" << std::endl;
}

//Spoint_cov methods -----------------------------------------------------------
Spoint_cov::Spoint_cov():
		Spoint()
{
	cov.reserve(4);
	cov.resize(4,0.0);
	cov[0] = 1.0;
	cov[3] = 1.0;
}
Spoint_cov::Spoint_cov( double x_ , double y_ , double time_stamp_,
		const std::vector<double>& cov_ ):
		Spoint( x_,y_,time_stamp_ )
{
	if ( cov_.empty() || cov_.size() < (std::size_t) 4 )
	{
		cov.reserve(4);
		cov.resize(4,0.0);
		cov[0] = 1.0;
		cov[3] = 1.0;
	}
	else
	{
		cov = cov_;
	}
}
double Spoint_cov::cov_dist( Spoint p2 ) const
{
	//analitical inversion of the 2x2 matrix
	//       det =   cov_xx * cov_yy - cov_xy * cov_xy
	double cov_det = cov[0] * cov[3] - cov[1] * cov[2];
	double dx = x - p2.x;
	double dy = y - p2.y;
	return ( dx*dx*cov[3] - 2*dx*dy*cov[1] + dy*dy*cov[0] )/cov_det;
}
Spoint_cov Spoint_cov::operator+ (Spoint_cov p2) const
{
    return Spoint_cov(  x + p2.x , y + p2.y , time_stamp , cov);
}
Spoint_cov Spoint_cov::operator- (Spoint_cov p2) const
{
    return Spoint_cov(  x - p2.x , y - p2.y , time_stamp , cov);
}
Spoint_cov Spoint_cov::operator* (double k) const
{
    return Spoint_cov(  x*k , y*k , time_stamp , cov);
}
double Spoint_cov::cov_xx() const
{
	return cov[0];
}
double Spoint_cov::cov_yy() const
{
	return cov[3];
}
double Spoint_cov::cov_xy() const
{
	return cov[1];
}
void Spoint_cov::print() const
{
    Spoint::print();
    std::cout << "cov = [" << cov[0] << " , " << cov[1] << std::endl;
    std::cout << "       " << cov[2] << " , " << cov[3] << " ]" << std::endl;
}

//SpointV methods -----------------------------------------------------------
SpointV::SpointV() : Spoint(), vx(0), vy(0) { }

SpointV::SpointV( double x_ , double y_, double time_stamp_, double vx_, double vy_ ) :
		Spoint(x_,y_,time_stamp_), vx(vx_),vy(vy_) { }
SpointV SpointV::operator+ (SpointV p2) const
{
	return SpointV(  x + p2.x , y + p2.y , time_stamp,  vx + p2.vx , vy + p2.vy );
}
SpointV SpointV::operator- (SpointV p2) const
{
	return SpointV(  x - p2.x , y - p2.y , time_stamp-p2.time_stamp,  vx - p2.vx , vy - p2.vy );
}
SpointV SpointV::operator* ( double k) const
{
	return SpointV(  x * k , y * k, time_stamp,  vx * k , vy * k );
}
void SpointV::print() const
{
	std::cout << "(x , y ,t) = (" << x << " , " << y  << " | " << time_stamp << " )" <<
			"     (vx,vy) = ( " << vx << " , " << vy << " )" << std::endl;
}
SpointV SpointV::propagate( double dt , Sforce f, double desired_velocity) const
{
	//linear propagation (uniformly accelerated system)
	double vxn = vx + f.fx*dt;
	double vyn = vy + f.fy*dt;
	double dx = vx*dt + f.fx*dt*dt*0.5;
	double dy = vy*dt + f.fy*dt*dt*0.5;

	//apply constrains:
	double v = sqrt( vxn*vxn + vyn*vyn );
	if ( v > desired_velocity)
	{
		vxn *= desired_velocity /v;
		vyn *= desired_velocity /v;
		dx = vxn*dt;
		dy = vyn*dt;
	}

	return SpointV( x+dx,y+dy, time_stamp+dt, vxn, vyn);
}
double SpointV::orientation() const
{
	return atan2( vy, vx );
}

double SpointV::v() const
{
	return sqrt( vy*vy + vx*vx );
}

void SpointV::norm_v(double v)
{
	double theta = this->orientation();
	vx = cos(theta)*v;
	vy = sin(theta)*v;
}

double SpointV::angle_heading_point( Spoint p2 ) const
{
	double dx = p2.x - x;
	double dy = p2.y - y;
	return diffangle( atan2( dy , dx ), orientation() );
}

//SpointV_cov methods -----------------------------------------------------------
SpointV_cov::SpointV_cov() :
		SpointV()
{
	cov.reserve(16);
	cov.resize(16,0.0);
	cov[0] = 0.4;
	cov[5] = 0.4;
	cov[10] = 0.1;
	cov[15] = 0.1;
}
SpointV_cov::SpointV_cov(double x_ , double y_, double time_stamp_, double vx_, double vy_,
		const std::vector<double>& cov_) :
		SpointV(x_,y_,time_stamp_,vx_,vy_)
{
	if ( cov_.empty() || cov_.size() < (std::size_t) 16 )
	{
		cov.reserve(16);
		cov.resize(16,0.0);
		cov[0] = 0.4;
		cov[5] = 0.4;
		cov[10] = 0.1;
		cov[15] = 0.1;
	}
	else
	{
		cov = cov_;
	}
}
SpointV_cov::SpointV_cov(SpointV p, const std::vector<double>& cov_) :
		SpointV( p.x , p.y, p.time_stamp, p.vx, p.vy)
{
	if ( cov_.empty() || cov_.size() < (std::size_t) 16 )
	{
		cov.reserve(16);
		cov.resize(16,0.0);
		cov[0] = 0.4;
		cov[5] = 0.4;
		cov[10] = 0.1;
		cov[15] = 0.1;
	}
	else
	{
		cov = cov_;
	}
}
SpointV_cov::SpointV_cov(Spoint_cov p ):
		SpointV( p.x , p.y, p.time_stamp, 0.0, 0.0)
{
	cov.reserve(16);
	cov.resize(16,0.0);
	cov[0] = p.cov[0];
	cov[1] = p.cov[1];
	cov[4] = p.cov[2];
	cov[5] = p.cov[3];
	cov[10] = 0.1;
	cov[15] = 0.1;
}

double SpointV_cov::cov_dist( Spoint p2 ) const
{
	// TODO: consider velocities, right now is a covariance distance of positions
	//       det =   cov_xx * cov_yy - cov_xy * cov_xy
	double cov_det = cov[0] * cov[5] - cov[1] * cov[4];
	double dx = x - p2.x;
	double dy = y - p2.y;
	//
	return ( dx*dx*cov[5] - 2*dx*dy*cov[1] + dy*dy*cov[0] )/cov_det;
}
double SpointV_cov::cov_dist( Spoint p2 , double &det) const
{
	det = cov[0] * cov[5] - cov[1] * cov[4];
	double dx = x - p2.x;
	double dy = y - p2.y;
	return ( dx*dx*cov[5] - 2*dx*dy*cov[1] + dy*dy*cov[0] )/det;
}

double SpointV_cov::cov_distV( SpointV_cov p2, double &distV, double &distxv ) const
{
	// TODO: consider velocities, right now is a covariance distance of positions
	//       det =   cov_xx * cov_yy - cov_xy * cov_xy
	// SpointV_cov external for actual track, SpointV_cov internal for the initial Spoint_cov for this track in the first time that the cross is made.
	double cov_det = cov[0] * cov[4] - cov[1] * cov[5];
	double dx = x - p2.x;
	double dy = y - p2.y;
	double distx=( dx*dx*cov[5] - 2*dx*dy*cov[1] + dy*dy*cov[0] )/cov_det;

	double cov_detV = cov[10] * cov[14] - cov[11] * cov[15];
	double dvx = vx - p2.vx;
	double dvy = vy - p2.vy;
	distV=( dvx*dvx*cov[15] - 2*dvx*dvy*cov[11] + dvy*dvy*cov[10] )/cov_detV;

	distxv=distx+distV;

	return distx;

}

SpointV_cov SpointV_cov::operator+ (SpointV_cov p2) const
{
    return SpointV_cov(  x + p2.x , y + p2.y , time_stamp , vx + p2.vx , vy + p2.vy ,cov);
}
SpointV_cov SpointV_cov::operator- (SpointV_cov p2) const
{
    return SpointV_cov(  x - p2.x , y - p2.y , time_stamp-p2.time_stamp,  vx - p2.vx , vy - p2.vy ,cov);
}
SpointV_cov SpointV_cov::operator* (double k) const
{
    return SpointV_cov(  x * k , y * k, time_stamp,  vx * k , vy * k , cov);
}
SpointV_cov SpointV_cov::propagate( double dt , Sforce f, double desired_velocity) const
{
	SpointV new_point = SpointV::propagate(dt,f,desired_velocity);
	Eigen::MatrixXd cov_prev(4 , 4 );
	for (unsigned int i = 0; i < 4; ++i)
	{
		cov_prev.row(i) << cov[i*4],cov[i*4+1],cov[i*4+2],cov[i*4+3];
	}
	//std::cout << "cov_ prev = \n" <<  cov_prev << std::endl;
	Eigen::MatrixXd Phi(4 , 4 );
	Phi = Eigen::MatrixXd::Identity(4,4);
	Phi(0,2) = dt;
	Phi(1,3) = dt;
	//std::cout << "Phi = " << Phi << std::endl;
	Eigen::MatrixXd G(4 , 2 );
	G = Eigen::MatrixXd::Zero(4,2);
	G(0,0) = dt*dt/2;
	G(1,1) = dt*dt/2;
	G(2,0) = dt;
	G(3,1) = dt;
	//std::cout << "G = " << G << std::endl;
	Eigen::MatrixXd new_cov(4,4);
	Eigen::MatrixXd cov_f(2,2);
	cov_f = Eigen::MatrixXd::Zero(2,2);
	cov_f(0,0) = 0.3;//TODO this covariance should be associated to the corresponding force and not generic
	cov_f(1,1) = 0.3;
	//std::cout << "f = " << cov_f << std::endl;
	new_cov = Phi*cov_prev*Phi.transpose() + G*cov_f*G.transpose();
	std::vector<double> new_cov_std(16,0.0);
	for(unsigned int i = 0; i<16; ++i)
	{
		new_cov_std[i] = new_cov( i/4 , i%4 );
	}
	//std::cout << "new cov = \n" <<  new_cov << std::endl;
	return SpointV_cov( new_point, new_cov_std );
}
double SpointV_cov::cov_xx() const
{
	return cov[0];
}
double SpointV_cov::cov_yy() const
{
	return cov[5];
}
double SpointV_cov::cov_xy() const
{
	return cov[1];
}
Spoint_cov SpointV_cov::toSpoint_cov() const
{
	std::vector<double> cov_point;
	cov_point.reserve(4);
	cov_point.resize(4,0.0);
	cov_point[0] = cov[0];
	cov_point[1] = cov[1];
	cov_point[2] = cov[4];
	cov_point[3] = cov[5];
	return Spoint_cov( x,y,time_stamp, cov_point );
}
void SpointV_cov::print() const
{
    SpointV::print();
    std::cout << "covariance = [ " << cov[0] << " , " << cov[1] << " , " <<cov[2]  << " , " << cov[3] << std::endl <<
    		     "               " << cov[4] << " , " << cov[5] << " , " <<cov[6]  << " , " << cov[7] << std::endl <<
    		     "               " << cov[8] << " , " << cov[9] << " , " <<cov[10]  << " , " << cov[11] << std::endl <<
    		     "               " << cov[12] << " , " << cov[13] << " , " <<cov[14]  << " , " << cov[15] << std::endl;
}

//Spose methods -----------------------------------------------------------
Spose::Spose() : Spoint(), theta(0)  , v(0) , w(0)  { }
Spose::Spose( double x_ , double y_ , double time_stamp_ , double theta_ , double v_, double w_) :
	Spoint(x_,y_, time_stamp_), theta(theta_), v(v_), w(w_) { }
Spose Spose::operator+ (Spose p2) const
{
	return Spose(  x + p2.x , y + p2.y , theta + p2.theta, p2.time_stamp , p2.v , p2.w );
}
Spose Spose::operator- (Spose p2) const
{
	return Spose(  x - p2.x , y - p2.y , theta - p2.theta, p2.time_stamp , p2.v , p2.w );
}
Spose Spose::operator* ( double k) const
{
	return Spose(  x * k , y *k , theta, time_stamp , v , w );
}
double Spose::distance(Spoint p2) const
{
	return sqrt(  (x-p2.x)*(x-p2.x) + (y-p2.y)*(y-p2.y) );
}
double Spose::angle_heading_pose( Spose p2 ) const
{
	double dx = p2.x - x;
	double dy = p2.y - y;
	return diffangle( atan2( dy , dx ), theta );
}
double Spose::social_distance( Spose p2 ) const
{
	double lambda = 0.0;
	double phi = this->angle_heading_pose( p2 );
	double anisotropy = (lambda + (1-lambda)*(1 + cos(phi))/2 );
	return this->distance(p2)/anisotropy;
}
void Spose::print() const
{
	std::cout << "(x , y | theta) = (" << x << " , " << y  << " | " << theta <<
			" )  --  at t = " << time_stamp << std::endl <<
			"(v , w) = (" << v  << " , " << w << " )" << std::endl;
}

//Spose_cov methods -----------------------------------------------------------
Spose_cov::Spose_cov():	 Spose()
{
	cov.reserve(25);
	cov.resize(25,0.0);
	cov[0] = 0.5;//xx
	cov[6] = 0.5;//yy
	cov[12] = 0.05;//theta_theta
	cov[18] = 0.1;//cov_vv
	cov[24] = 0.1;//ww
}
Spose_cov::Spose_cov(double x_ , double y_ ,
		double time_stamp_ , double theta_ ,  double v_, double w_,
		const std::vector<double>& cov_ ):
		Spose( x_,y_,time_stamp_, theta_,v_,w_ )
{
	if ( cov_.empty() || cov_.size() < (std::size_t) 25 )
	{
		cov.reserve(25);
		cov.resize(25,0.0);
		cov[0] = 0.5;//xx
		cov[6] = 0.5;//yy
		cov[12] = 0.05;//theta_theta
		cov[18] = 0.1;//cov_vv
		cov[24] = 0.1;//ww
	}
	else
	{
		cov = cov_;
	}
}
Spose_cov::Spose_cov( Spose pose_, const std::vector<double>& cov_ ):
		Spose(pose_)
{
	if ( cov_.empty() || cov_.size() < (std::size_t) 25 )
	{
		cov.reserve(25);
		cov.resize(25,0.0);
		cov[0] = 0.5;//xx
		cov[6] = 0.5;//yy
		cov[12] = 0.05;//theta_theta
		cov[18] = 0.1;//cov_vv
		cov[24] = 0.1;//ww
	}
	else
	{
		cov = cov_;
	}
}
Spose_cov::Spose_cov( SpointV_cov point ):
		Spose(point.x,point.y,point.time_stamp,point.orientation(),point.v())
{
	cov.reserve(25);
	cov.resize(25,0.0);
	cov[0] = point.cov[0];//xx
	cov[6] = point.cov[5];//yy
	cov[12] = 0.05;//theta_theta
	cov[18] = 0.1;//cov_vv
	cov[24] = 0.1;//ww
}
double Spose_cov::distance( Spose_cov p2) const
{
	return Spose::distance( Spose(p2.x,p2.y) );
}
double Spose_cov::cov_dist( Spose p2 ) const
{
	//analitical inversion of the 2x2 matrix
	double cov_xx = cov[0];
	double cov_yy = cov[6];
	double cov_xy = cov[5];
	double cov_det = cov_xx * cov_yy - cov_xy * cov_xy;
	double dx = x - p2.x;
	double dy = y - p2.y;
	return ( dx*dx*cov_yy - 2*dx*dy*cov_xy + dy*dy*cov_xx )/cov_det;
}
double Spose_cov::cov_dist( Spose_cov p2 ) const
{
	return this->cov_dist( Spose(p2.x,p2.y) );
}

//Sdestinations methods -----------------------------------------------------------
Sdestination::Sdestination( int id_, double x_ , double y_ , double prob_ ,
		Sdestination::destination_type type_, std::vector<int> neighbours_ids_ ) :
	Spoint(x_,y_), id(id_), prob(prob_) ,
	type(type_), neighbours_ids(neighbours_ids_)  { }
Sdestination::~Sdestination(){}
bool Sdestination::operator== (Sdestination d2) const
{
	if ( x == d2.x && y == d2.y )
		return true;
	else
		return false;
}

void Sdestination::print() const
{
	std::cout << "destination " << id << "at (" << x << " , " << y  <<  ")  of type " << type  <<
		"   and probability = " << prob  << "neighbours " << neighbours_ids.size() << std::endl;
}

SdetectionObservation::SdetectionObservation() :
				SpointV_cov(), id(0) { }
SdetectionObservation::SdetectionObservation( int id_ , double time_stamp_ ,
		double x_, double y_, double vx_, double vy_, const std::vector<double>& cov_) :
			SpointV_cov(x_,y_,time_stamp_,vx_,vy_,cov_) , id(id_) { }
SdetectionObservation::SdetectionObservation( int id_ , SpointV_cov pointV_cov_) :
	SpointV_cov( pointV_cov_ ), id(id_) {}
void SdetectionObservation::print() const
{
	std::cout <<  "detection Observation = " << id << " th "  << std::endl;
	SpointV_cov::print();
}

//Sforce methods
Sforce::Sforce() : fx(0.0) , fy(0.0) {}
Sforce::Sforce(double fx_ , double fy_) : fx(fx_) , fy(fy_) {}
double Sforce::module() { return sqrt(fx*fx + fy*fy);}
double Sforce::module(double r2) { return sqrt(fx*fx + r2*fy*fy);}
double Sforce::module2( ) { return fx*fx + fy*fy;}
double Sforce::module2( double r2) { return fx*fx + r2*fy*fy;}
void Sforce::print() const
{
	std::cout <<  "Force = (" << fx << " , " << fy  << " )" << std::endl;
}
Sforce Sforce::operator+ (Sforce f2) const
{
	return Sforce(  fx + f2.fx , fy + f2.fy );
}
void Sforce::sum(Sforce f2)
//a method similar to +=
{
	fx += f2.fx;
	fy += f2.fy;
}
Sforce& Sforce::operator +=(Sforce f2)
{
	fx += f2.fx;
	fy += f2.fy;
	return *this;
}

Sforce Sforce::operator* (double k) const
{
	return Sforce(  fx *k , fy *k );
}

double Sforce::operator* (Spoint dr) const
{
	return this->fx * dr.x + this->fy * dr.y;
}
Sforce Sforce::operator- (Sforce f2) const
{
    return Sforce(  fx - f2.fx , fy - f2.fy );
}

double diffangle(double alpha , double beta)
{
	  double delta = alpha - beta;
	  if ( alpha >= beta )
	  {
	    while (delta >  PI)
	      delta -= 2*PI;
	  }
	  else
	  {
		  while (delta < -PI )
			  delta += 2*PI;
	  }
	  return delta;
}