/**
 * \file dcs/testbed/system_identification_strategies.hpp
 *
 * \brief Class containing wrappers for different system identification
 *  strategies.
 *
 * \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_SYSTEM_IDENTIFICATION_STRATEGIES_HPP
#define DCS_TESTBED_SYSTEM_IDENTIFICATION_STRATEGIES_HPP

#include <algorithm>
#include <boost/numeric/ublas/expression_types.hpp>
#ifdef DCS_DEBUG
#	include <boost/numeric/ublas/io.hpp>
#endif // DCS_DEBUG
#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_proxy.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/operation/max.hpp>
#include <boost/numeric/ublasx/operation/num_columns.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>
#include <boost/numeric/ublasx/operation/rcond.hpp>
#include <boost/numeric/ublasx/operation/size.hpp>
#include <boost/smart_ptr.hpp>
#include <cmath>
#include <dcs/assert.hpp>
#include <dcs/debug.hpp>
#include <dcs/exception.hpp>
#include <dcs/macro.hpp>
#include <dcs/sysid/algorithm/rls.hpp>
#include <dcs/testbed/system_identification_strategy_params.hpp>
#include <stdexcept>
#include <vector>

namespace dcs { namespace testbed {

template <typename TraitsT>
class base_arx_system_identification_strategy
{
	public: typedef TraitsT traits_type;
	public: typedef typename traits_type::real_type real_type;
	public: typedef ::boost::numeric::ublas::matrix<real_type> matrix_type;
	public: typedef ::boost::numeric::ublas::vector<real_type> vector_type;
	public: typedef typename ::boost::numeric::ublas::promote_traits<
							typename ::boost::numeric::ublas::matrix_traits<matrix_type>::size_type,
							typename ::boost::numeric::ublas::vector_traits<vector_type>::size_type
					>::promote_type size_type;

	public: base_arx_system_identification_strategy()
	: n_a_(0),
	  n_b_(0),
	  d_(0),
	  n_y_(0),
	  n_u_(0),
	  count_(0)
	{
	}

	public: base_arx_system_identification_strategy(size_type n_a, size_type n_b, size_type d, size_type n_y, size_type n_u)
	: n_a_(n_a),
	  n_b_(n_b),
	  d_(d),
	  n_y_(n_y),
	  n_u_(n_u),
	  count_(0)
	{
	}

	public: base_arx_system_identification_strategy(base_system_identification_strategy_params<traits_type> const& params)
	: n_a_(params.output_order()),
	  n_b_(params.input_order()),
	  d_(params.input_delay()),
	  n_y_(params.num_outputs()),
	  n_u_(params.num_inputs()),
	  count_(0)
	{
	}

	public: virtual ~base_arx_system_identification_strategy()
	{
	}

	public: size_type input_order() const
	{
		return n_b_;
	}

	public: size_type output_order() const
	{
		return n_a_;
	}

	public: size_type input_delay() const
	{
		return d_;
	}

	public: size_type num_inputs() const
	{
		return n_u_;
	}

	public: size_type num_outputs() const
	{
		return n_y_;
	}

	public: matrix_type Theta_hat() const
	{
		return do_Theta_hat();
	}

	public: matrix_type P() const
	{
		return do_P();
	}

	public: vector_type phi() const
	{
		return do_phi();
	}

	public: void init()
	{
		count_ = 0;

		do_init();
	}

	public: template <typename YVectorExprT, typename UVectorExprT>
		vector_type estimate(::boost::numeric::ublas::vector_expression<YVectorExprT> const& y,
							 ::boost::numeric::ublas::vector_expression<UVectorExprT> const& u)
	{
		++count_;

		return do_estimate(y, u);
	}

	public: matrix_type A(size_type k) const
	{
		DCS_ASSERT(k >= 1 && k <= this->output_order(),
				   DCS_EXCEPTION_THROW(::std::invalid_argument,
									   "Argument k is out-of-range"));

		return do_A(k);
	}

	public: matrix_type B(size_type k) const
	{
		DCS_ASSERT(k >= 1 && k <= this->input_order(),
				   DCS_EXCEPTION_THROW(::std::invalid_argument,
									   "Argument k is out-of-range"));

		return do_B(k);
	}

	public: vector_type y(size_type k) const
	{
		DCS_ASSERT(k >= 1 && k <= this->output_order(),
				   DCS_EXCEPTION_THROW(::std::invalid_argument,
									   "Argument k is out-of-range"));

		return do_y(k);
	}

	public: vector_type u(size_type k) const
	{
		DCS_ASSERT(k >= 1 && k <= (this->input_order()+this->input_delay()),
				   DCS_EXCEPTION_THROW(::std::invalid_argument,
									   "Argument k is out-of-range"));

		return do_u(k);
	}

	public: vector_type y_hat() const
	{
		namespace ublas = ::boost::numeric::ublas;
		namespace ublasx = ::boost::numeric::ublasx;

		vector_type phi(this->phi());
		size_type np(ublasx::size(phi));
		size_type nyna(this->num_outputs()*this->output_order());
		size_type nd((this->input_delay()-1)*this->num_inputs());

		vector_type delayed_phi(np-nd, 0);
		ublas::subrange(delayed_phi, 0, nyna) = ublas::subrange(phi, 0, nyna);
		ublas::subrange(delayed_phi, nyna, np-nd) = ublas::subrange(phi, nyna+nd, np);

		return ublas::prod(delayed_phi, this->Theta_hat());
	}

	public: size_type count() const
	{
		return count_;
	}

	public: void reset()
	{
		init();

		do_reset();
	}

	protected: virtual void do_reset()
	{
	}

	private: virtual matrix_type do_Theta_hat() const = 0;

	private: virtual matrix_type do_P() const = 0;

	private: virtual vector_type do_phi() const = 0;

	private: virtual void do_init() = 0;

	private: virtual vector_type do_estimate(vector_type const& y, vector_type const& u) = 0;

	private: virtual matrix_type do_A(size_type k) const = 0;

	private: virtual matrix_type do_B(size_type k) const = 0;

	private: virtual vector_type do_y(size_type k) const = 0;

	private: virtual vector_type do_u(size_type k) const = 0;

	/// The memory for the control output.
	private: size_type n_a_;
	/// The memory for the control input.
	private: size_type n_b_;
	/// Input delay (dead time).
	private: size_type d_;
	/// The size of the control output vector.
	private: size_type n_y_;
	/// The size of the augmented control input vector.
	private: size_type n_u_;
	/// Count the number of times RLS has been applied
	private: size_type count_;
}; // base_arx_system_identification_strategy

template <typename TraitsT>
class rls_arx_system_identification_strategy: public base_arx_system_identification_strategy<TraitsT>
{
	private: typedef base_arx_system_identification_strategy<TraitsT> base_type;
	public: typedef TraitsT traits_type;
	public: typedef typename traits_type::real_type real_type;
	public: typedef typename traits_type::uint_type uint_type;
	public: typedef typename base_type::size_type size_type;

	public: rls_arx_system_identification_strategy()
	: base_type(),
	  max_cov_heuristic_(false),
	  max_cov_heuristic_max_val_(0),
	  cond_cov_heuristic_(false),
	  cond_cov_heuristic_trust_digits_(0)
	{
	}

	public: rls_arx_system_identification_strategy(size_type n_a, size_type n_b, size_type d, size_type n_y, size_type n_u)
	: base_type(n_a, n_b, d, n_y, n_u),
	  max_cov_heuristic_(false),
	  max_cov_heuristic_max_val_(0),
	  cond_cov_heuristic_(false),
	  cond_cov_heuristic_trust_digits_(0)
	{
	}

	public: rls_arx_system_identification_strategy(rls_system_identification_strategy_params<traits_type> const& params)
	: base_type(params),
	  max_cov_heuristic_(false),
	  max_cov_heuristic_max_val_(0),
	  cond_cov_heuristic_(false),
	  cond_cov_heuristic_trust_digits_(0)
	{
	}

	public: void max_covariance_heuristic(bool value)
	{
		max_cov_heuristic_ = value;
	}

	public: bool max_covariance_heuristic() const
	{
		return max_cov_heuristic_;
	}

	public: void max_covariance_heuristic_max_value(real_type value)
	{
		max_cov_heuristic_max_val_ = value;
	}

	public: real_type max_covariance_heuristic_max_value() const
	{
		return max_cov_heuristic_max_val_;
	}

	public: void condition_number_covariance_heuristic(bool value)
	{
		cond_cov_heuristic_ = value;
	}

	public: bool condition_number_covariance_heuristic() const
	{
		return cond_cov_heuristic_;
	}

	public: void condition_number_covariance_heuristic_max_value(uint_type value)
	{
		cond_cov_heuristic_trust_digits_ = value;
	}

	public: uint_type condition_number_covariance_heuristic_trusted_digits() const
	{
		return cond_cov_heuristic_trust_digits_;
	}

	private: bool max_cov_heuristic_;
	private: real_type max_cov_heuristic_max_val_;
	private: bool cond_cov_heuristic_;
	private: uint_type cond_cov_heuristic_trust_digits_;
}; // rls_arx_system_identification_strategy

/**
 * \brief Proxy for directly applying the Recursive Least Square with
 *  forgetting-factor algorithm to a MIMO system model.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template <typename TraitsT>
class rls_ff_arx_mimo_proxy: public rls_arx_system_identification_strategy<TraitsT>
{
	private: typedef rls_arx_system_identification_strategy<TraitsT> base_type;
	public: typedef TraitsT traits_type;
	public: typedef typename traits_type::real_type real_type;
	public: typedef typename base_type::matrix_type matrix_type;
	public: typedef typename base_type::vector_type vector_type;
	public: typedef typename base_type::size_type size_type;

	public: rls_ff_arx_mimo_proxy()
	: base_type(),
	  ff_(0),
	  Theta_hat_(),
	  P_(),
	  phi_()
	{
	}

	public: rls_ff_arx_mimo_proxy(size_type n_a, size_type n_b, size_type d, size_type n_y, size_type n_u, real_type ff)
	: base_type(n_a, n_b, d, n_y, n_u),
	  ff_(ff),
	  Theta_hat_(),
	  P_(),
	  phi_()
	{
	}

	public: rls_ff_arx_mimo_proxy(rls_ff_system_identification_strategy_params<traits_type> const& params)
	: base_type(params),
	  ff_(params.forgetting_factor()),
	  Theta_hat_(),
	  P_(),
	  phi_()
	{
	}

	public: real_type forgetting_factor() const
	{
		return ff_;
	}

	private: matrix_type do_Theta_hat() const
	{
		return Theta_hat_;
	}

	private: matrix_type do_P() const
	{
		return P_;
	}

	private: vector_type do_phi() const
	{
		return phi_;
	}

	private: void do_init()
	{
		// Prepare the data structures for the RLS algorithm
		::dcs::sysid::rls_arx_mimo_init(this->output_order(),
										this->input_order(),
										this->input_delay(),
										this->num_outputs(),
										this->num_inputs(),
										Theta_hat_,
										P_,
										phi_);

	}

	private: vector_type do_estimate(vector_type const& y, vector_type const& u)
	{
		// Apply enabled heuristics
		bool reset(false);
		// Apply the "max-covariance" heuristic (if enabled)
		if (!reset && this->max_covariance_heuristic())
		{
			if (::boost::numeric::ublasx::max(P_) > this->max_covariance_heuristic_max_value())
			{
				reset = true;
			}
		}
		// Apply the "condition-number-covariance" heuristic (if enabled)
		if (!reset && this->condition_number_covariance_heuristic())
		{
			// Use the following rule-of-thumb [1]:
			//   if cond(A)*eps >= 0.5*10^{-d} then d significant digits of the
			//   solution of the problem involving A are at least correct.
			// NOTE #1: we use reciprocal condition estimator since it's less
			//          time-consuming.
			// NOTE #2: we use log10 to avoid to compute 10^d which can
			//          potentially lead to overflow problems.
			//
			// References:
			// [1] Holistic Numerical Methods Institute,
			//     "Chapter 04.09 -- Adequacy of Solutions"
			//     Found online at http://numericalmethods.eng.usf.edu/mws/gen/04sle/mws_gen_sle_spe_adequacy.pdf

			real_type check_val = ::std::log10(static_cast<real_type>(2)*::std::numeric_limits<real_type>::epsilon())
								  + this->condition_number_covariance_heuristic_trusted_digits();

			if (::std::log10(::boost::numeric::ublasx::rcond(P_)) > check_val)
			{
				reset = true;
			}
		}
		if (reset)
		{
			this->reset();
		}

		// Estimate system parameters
//DCS_DEBUG_TRACE("BEGIN estimation");//XXX
//DCS_DEBUG_TRACE("y(k): " << y);//XXX
//DCS_DEBUG_TRACE("u(k): " << u);//XXX
//DCS_DEBUG_TRACE("Theta_hat(k): " << Theta_hat_);//XXX
//DCS_DEBUG_TRACE("P(k): " << P_);//XXX
//DCS_DEBUG_TRACE("phi(k): " << phi_);//XXX
		vector_type y_hat;
		y_hat = ::dcs::sysid::rls_ff_arx_mimo(y,
											  u,
											  ff_,
											  this->output_order(),
											  this->input_order(),
											  this->input_delay(),
											  Theta_hat_,
											  P_,
											  phi_);
//DCS_DEBUG_TRACE("New Theta_hat(k): " << Theta_hat_);//XXX
//DCS_DEBUG_TRACE("New P(k): " << P_);//XXX
//DCS_DEBUG_TRACE("New phi(k): " << phi_);//XXX
//DCS_DEBUG_TRACE("END estimation");//XXX
		return y_hat;
	}

	/// Return matrix A_k from \hat{\Theta}.
	private: matrix_type do_A(size_type k) const
	{
		namespace ublas = ::boost::numeric::ublas;

		// Remember:
		//   \hat{\Theta} = [a_{11}^{1},     a_{21}^{1},     ...,  a_{n_y1}^{1};
		//                   ...,            ...,            ...,  ...;
		//                   a_{11}^{n_a},   a_{21}^{n_a},   ...,  a_{n_y1}^{n_a};
		//                   ...,            ...,            ...,  ...;
		//                   a_{1n_y}^{1},   a_{2n_y}^{1},   ...,  a_{n_yn_y}^{1};
		//                   ...,            ...,            ...,  ...;
		//                   a_{1n_y}^{n_a}, a_{2n_y}^{n_a}, ...,  a_{n_yn_y}^{n_a};
		//                   b_{11}^{1},     b_{21}^{1},     ...,  b_{n_y1}^{1};
		//                   ...,            ...,            ...,  ...;
		//                   b_{11}^{n_b},   b_{21}^{n_b},   ...,  b_{n_y1}^{n_b};
		//                   ...,            ...,            ...,  ...;
		//                   b_{1n_u}^{1},   b_{2n_u}^{1},   ...,  b_{n_yn_u}^{1};
		//                   ...,            ...,            ...,  ...;
		//                   b_{1n_u}^{n_b}, b_{2n_u}^{n_b}, ...,  b_{n_yn_u}^{n_b}]
		// So in \hat{\Theta} the matrix A_k stays at:
		//   A_k <- (\hat{\Theta}(k:n_a:n_y,:))^T
		matrix_type A_k; // (this->num_outputs(), this->num_outputs());
		A_k = ublas::trans(ublas::subslice(Theta_hat_, k-1, this->output_order(), this->num_outputs(), 0, 1, this->num_outputs()));

		return A_k;
	}

	/// Return matrix B_k from \hat{\Theta}.
	private: matrix_type do_B(size_type k) const
	{
		namespace ublas = ::boost::numeric::ublas;

		// Remember:
		//   \hat{\Theta} = [a_{11}^{1},     a_{21}^{1},     ...,  a_{n_y1}^{1};
		//                   ...,            ...,            ...,  ...;
		//                   a_{11}^{n_a},   a_{21}^{n_a},   ...,  a_{n_y1}^{n_a};
		//                   ...,            ...,            ...,  ...;
		//                   a_{1n_y}^{1},   a_{2n_y}^{1},   ...,  a_{n_yn_y}^{1};
		//                   ...,            ...,            ...,  ...;
		//                   a_{1n_y}^{n_a}, a_{2n_y}^{n_a}, ...,  a_{n_yn_y}^{n_a};
		//                   b_{11}^{1},     b_{21}^{1},     ...,  b_{n_y1}^{1};
		//                   ...,            ...,            ...,  ...;
		//                   b_{11}^{n_b},   b_{21}^{n_b},   ...,  b_{n_y1}^{n_b};
		//                   ...,            ...,            ...,  ...;
		//                   b_{1n_u}^{1},   b_{2n_u}^{1},   ...,  b_{n_yn_u}^{1};
		//                   ...,            ...,            ...,  ...;
		//                   b_{1n_u}^{n_b}, b_{2n_u}^{n_b}, ...,  b_{n_yn_u}^{n_b}]
		// So in \hat{\Theta} the matrix B_k stays at:
		//   B_k <- (\hat{\Theta}(((n_a*n_y)+k):n_b:n_u,:))^T
		matrix_type B_k; // (n_y_, n_u_);
		B_k = ublas::trans(ublas::subslice(Theta_hat_, this->output_order()*this->num_outputs()+k-1, this->input_order(), this->num_inputs(), 0, 1, this->num_outputs()));

		return B_k;
	}

	private: vector_type do_y(size_type k) const
	{
		namespace ublas = ::boost::numeric::ublas;

		// Remeber:
		// \phi = [ -y_{1}(t-1);
		//          -y_{1}(t-2);
		//           ...
		//          -y_{1}(t-n_a);
		//          -y_{2}(t-1);
		//           ...
		//          -y_{2}(t-n_a);
		//           ...
		//          -y_{n_y}(t-1);
		//           ...
		//          -y_{n_y}(t-n_a);
		//           u_{1}(t-1);
		//          ...
		//           u_{1}(t-n_b-d);
		//           ...
		//           u_{n_u}(t-1);
		//           ...
		//           u_{n_u}(t-n_b-d)]
		// So in \phi the vector y_k=y(t-k) stays at:
		//   y_k <- -[\phi((k-1):n_a:n_y*n_a]
		vector_type y_k;
		y_k = -ublas::subslice(phi_, k-1, this->output_order(), this->num_outputs()*this->output_order());

		return y_k;
	}

	private: vector_type do_u(size_type k) const
	{
		namespace ublas = ::boost::numeric::ublas;

		// Remeber:
		// \phi = [ -y_{1}(t-1);
		//          -y_{1}(t-2);
		//           ...
		//          -y_{1}(t-n_a);
		//          -y_{2}(t-1);
		//           ...
		//          -y_{2}(t-n_a);
		//           ...
		//          -y_{n_y}(t-1);
		//           ...
		//          -y_{n_y}(t-n_a);
		//           u_{1}(t-1);
		//          ...
		//           u_{1}(t-n_b-d);
		//           ...
		//           u_{n_u}(t-1);
		//           ...
		//           u_{n_u}(t-n_b-d)]
		// So in \phi the vector u_k=u(t-k) stays at:
		//   u_k <- [\phi((n_y*n_a+k-1):(n_b+d):(n_y*n_a+n_u*(n_b+d))]
		vector_type u_k;
		size_type offs(this->num_outputs()*this->output_order());
		size_type nbd(this->input_order()+this->input_delay());
		u_k = ublas::subslice(phi_, offs+k-1, nbd, offs+nbd*this->input_order());

		return u_k;
	}

	/// Forgetting factor.
	private: real_type ff_;
	/// Matrix of system parameters estimated by RLS: [A_1 ... A_{n_a} B_1 ... B_{n_b}].
	private: matrix_type Theta_hat_;
	/// The covariance matrix.
	private: matrix_type P_;
	/// The regression vector.
	private: vector_type phi_;
}; // rls_ff_arx_mimo_proxy

/**
 * \brief Proxy to identify a MIMO system model by applying the Recursive Least
 *  Square with forgetting-factor algorithm to several MISO system models.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template <typename TraitsT>
class rls_ff_arx_miso_proxy: public rls_arx_system_identification_strategy<TraitsT>
{
	private: typedef rls_arx_system_identification_strategy<TraitsT> base_type;
	public: typedef TraitsT traits_type;
	public: typedef typename traits_type::real_type real_type;
	public: typedef typename base_type::matrix_type matrix_type;
	public: typedef typename base_type::vector_type vector_type;
	public: typedef typename base_type::size_type size_type;

	public: rls_ff_arx_miso_proxy()
	: base_type(),
	  ff_(0),
	  theta_hats_(),
	  Ps_(),
	  phis_()
	{
	}

	public: rls_ff_arx_miso_proxy(size_type n_a, size_type n_b, size_type d, size_type n_y, size_type n_u, real_type ff)
	: base_type(n_a,n_b,d,n_y,n_u),
	  ff_(ff),
	  theta_hats_(n_y),
	  Ps_(n_y),
	  phis_(n_y)
	{
	}

	public: rls_ff_arx_miso_proxy(rls_ff_system_identification_strategy_params<traits_type> const& params)
	: base_type(params),
	  ff_(params.forgetting_factor()),
	  theta_hats_(params.num_outputs()),
	  Ps_(params.num_outputs()),
	  phis_(params.num_outputs())
	{
	}

	public: real_type forgetting_factor() const
	{
		return ff_;
	}

	private: matrix_type do_Theta_hat() const
	{
		namespace ublas = ::boost::numeric::ublas;

		const size_type na(this->output_order());
		const size_type nb(this->input_order());
		const size_type ny(this->num_outputs());
		const size_type nu(this->num_inputs());
		const size_type nay(na*ny);
		const size_type nbu(nb*nu);
		const size_type n(nay+nbu);
		matrix_type X(n, ny, real_type/*zero()*/());

		for (size_type i = 0; i < ny; ++i)
		{
			// ith output => ith column of Theta_hat
			// ith column of Theta_hat = [0; ...; 0; a_{ii}^{1}
			const size_type k(i*na);
			ublas::matrix_column<matrix_type> mc(X,i);
			ublas::subrange(mc, k, k+na) = ublas::subrange(theta_hats_[i], 0, na);
			ublas::subrange(mc, nay, n) = ublas::subrange(theta_hats_[i], na, na+nbu);
		}

		return X;
	}

	private: matrix_type do_P() const
	{
		matrix_type aux_P;
//TODO: implement me!
		return aux_P;
	}

	private: vector_type do_phi() const
	{
		namespace ublas = ::boost::numeric::ublas;

		const size_type na(this->output_order());
		const size_type ny(this->num_outputs());
		const size_type nay(na*ny);
		const size_type nbu(this->input_order()*this->num_inputs());
		const size_type n(nay+nbu);
		vector_type x(n, 0);

		for (size_type i = 0; i < ny; ++i)
		{
			const size_type k(i*na);
			ublas::subrange(x, k, k+na) = ublas::subrange(phis_[i], 0, na);
		}

		ublas::subrange(x, nay, n) = ublas::subrange(phis_[0], na, na+nbu);

		return x;
	}

	private: void do_init()
	{
		const size_type na(this->output_order());
		const size_type nb(this->input_order());
		const size_type d(this->input_delay());
		const size_type ny(this->num_outputs());
		const size_type nu(this->num_inputs());

		// Prepare the data structures for the RLS algorithm
		for (size_type i = 0; i < ny; ++i)
		{
			::dcs::sysid::rls_arx_miso_init(na,
											nb,
											d,
											nu,
											theta_hats_[i],
											Ps_[i],
											phis_[i]);
		}
	}

	private: vector_type do_estimate(vector_type const& y, vector_type const& u)
	{
		const size_type na(this->output_order());
		const size_type nb(this->input_order());
		const size_type d(this->input_delay());
		const size_type ny(this->num_outputs());

		// Apply enabled heuristics
		bool reset(false);
		// Apply the "max-covariance" heuristic (if enabled)
		if (this->max_covariance_heuristic())
		{
			for (size_type i = 0; i < ny && !reset; ++i)
			{
				if (::boost::numeric::ublasx::max(Ps_[i]) > this->max_covariance_heuristic_max_value())
				{
					reset = true;
				}
			}
		}
		// Apply the "condition-number-covariance" heuristic (if enabled)
		if (this->condition_number_covariance_heuristic())
		{
			// Use the following rule-of-thumb:
			//   if cond(A)*eps >= 0.5*10^{-d} then d significant digits of the
			//   solution of the problem involving A are at least correct.
			// NOTE #1: we use reciprocal condition estimator since it's less
			//          time-consuming.
			// NOTE #2: we use log10 to avoid to compute 10^d which can
			//          potentially lead to overflow problems.
			//
			// References:
			// [1] Holistic Numerical Methods Institute,
			//     "Chapter 04.09 -- Adequacy of Solutions"
			//     Found online at http://numericalmethods.eng.usf.edu/mws/gen/04sle/mws_gen_sle_spe_adequacy.pdf

			real_type check_val = ::std::log10(static_cast<real_type>(2)*::std::numeric_limits<real_type>::epsilon())
								  + this->condition_number_covariance_heuristic_trusted_digits();

			for (size_type i = 0; i < ny && !reset; ++i)
			{
				if (::std::log10(::boost::numeric::ublasx::rcond(Ps_[i])) > check_val)
				{
					reset = true;
				}
			}
		}
		if (reset)
		{
			this->reset();
		}

		// Estimate system parameters
DCS_DEBUG_TRACE("BEGIN estimation");//XXX
DCS_DEBUG_TRACE("y(k): " << y);//XXX
DCS_DEBUG_TRACE("u(k): " << u);//XXX
//::std::cerr << "BEGIN estimation" << ::std::endl;//XXX
//::std::cerr << "y(k): " << y << ::std::endl;//XXX
//::std::cerr << "u(k): " << u << ::std::endl;//XXX
		vector_type y_hat(ny, 0);
		for (size_type i = 0; i < ny; ++i)
		{
DCS_DEBUG_TRACE("theta_hat["<< i << "](k): " << theta_hats_[i]);//XXX
DCS_DEBUG_TRACE("P["<< i << "](k): " << Ps_[i]);//XXX
DCS_DEBUG_TRACE("phi["<< i << "](k): " << phis_[i]);//XXX
//::std::cerr << "theta_hat["<< i << "](k): " << theta_hats_[i] << ::std::endl;//XXX
//::std::cerr << "P["<< i << "](k): " << Ps_[i] << ::std::endl;//XXX
//::std::cerr << "phi["<< i << "](k): " << phis_[i] << ::std::endl;//XXX
			y_hat(i) = ::dcs::sysid::rls_ff_arx_miso(y(i),
													 u,
													 ff_,
													 na,
													 nb,
													 d,
													 theta_hats_[i],
													 Ps_[i],
													 phis_[i]);
DCS_DEBUG_TRACE("New theta_hat["<< i << "](k): " << theta_hats_[i]);//XXX
DCS_DEBUG_TRACE("New P["<< i << "](k): " << Ps_[i]);//XXX
DCS_DEBUG_TRACE("New rcond(P["<< i << "](k)): " << ::boost::numeric::ublasx::rcond(Ps_[i]));//XXX
DCS_DEBUG_TRACE("New phi["<< i << "](k): " << phis_[i]);//XXX
DCS_DEBUG_TRACE("New e["<< i << "](k): " << (y(i)-y_hat(i)));//XXX
//::std::cerr << "New theta_hat["<< i << "](k): " << theta_hats_[i] << ::std::endl;//XXX
//::std::cerr << "New P["<< i << "](k): " << Ps_[i] << ::std::endl;//XXX
//::std::cerr << "New rcond(P["<< i << "](k)): " << ::boost::numeric::ublasx::rcond(Ps_[i]) << ::std::endl;//XXX
//::std::cerr << "New phi["<< i << "](k): " << phis_[i] << ::std::endl;//XXX
//::std::cerr << "New e["<< i << "](k): " << (y(i)-y_hat(i)) << ::std::endl;//XXX
		}
DCS_DEBUG_TRACE("New y_hat(k): " << y_hat);//XXX
DCS_DEBUG_TRACE("END estimation");//XXX
//::std::cerr << "New y_hat(k): " << y_hat << ::std::endl;//XXX
//::std::cerr << "END estimation" << ::std::endl;//XXX

		return y_hat;
	}

	/// Return matrix A_k from \hat{\Theta}.
	private: matrix_type do_A(size_type k) const
	{
		namespace ublas = ::boost::numeric::ublas;

		const size_type ny(this->num_outputs());

		// Remember, for each output i=1,...,n_y:
		//   \hat{\theta}_i = [a_{ii}^{1};
		//                     ...;
		//                     a_{ii}^{n_a};
		//                     b_{i1}^{1};
		//                     ...;
		//                     b_{i1}^{n_b};
		//                     ...;
		//                     b_{in_u}^{1};
		//                     ...;
		//                     b_{in_u}^{n_b}]
		// So in \hat{\theta}_i the ith diagonal element of matrix A_k stays at:
		//   A_k(i,i) <- \hat{\theta}_i(k)
		matrix_type A_k(ny, ny, real_type/*zero*/());
		for (size_type i = 0; i < ny; ++i)
		{
			A_k(i,i) = theta_hats_[i](k-1);
		}

		return A_k;
	}

	/// Return matrix B_k from \hat{\Theta}.
	private: matrix_type do_B(size_type k) const
	{
		namespace ublas = ::boost::numeric::ublas;

		const size_type na(this->output_order());
		const size_type nb(this->input_order());
		const size_type ny(this->num_outputs());
		const size_type nu(this->num_inputs());

		// Remember, for each output i=1,...,n_y:
		//   \hat{\theta}_i = [a_{ii}^{1};
		//                     ...;
		//                     a_{ii}^{n_a};
		//                     b_{i1}^{1};
		//                     ...;
		//                     b_{i1}^{n_b};
		//                     ...;
		//                     b_{in_u}^{1};
		//                     ...;
		//                     b_{in_u}^{n_b}]
		// So in \hat{\theta}_i the ith row of matrix B_k stays at:
		//   B_k(i,:) <- (\hat{\theta}_i(((n_a+k):n_b:n_u))^T
		matrix_type B_k(ny, nu);
		for (size_type i = 0; i < ny; ++i)
		{
			ublas::row(B_k, i) = ublas::subslice(theta_hats_[i], na+k-1, nb, nu);
		}

		return B_k;
	}

	private: vector_type do_y(size_type k) const
	{
		namespace ublas = ::boost::numeric::ublas;

		// Remeber:
		// \phi = [ -y(t-1);
		//          -y(t-2);
		//           ...
		//          -y(t-n_a);
		//           u_{1}(t-1);
		//          ...
		//           u_{1}(t-n_b-d);
		//           ...
		//           u_{n_u}(t-1);
		//           ...
		//           u_{n_u}(t-n_b-d)]
		// So in \phi the vector y_k=y(t-k) stays at:
		//   y_k <- -[\phi(k-1)]

		size_type ny(this->num_outputs());
		vector_type yk(ny, 0);

		for (size_type i = 0; i < ny; ++i)
		{
			yk(i) = -phis_[i](k-1);
		}

		return yk;
	}

	private: vector_type do_u(size_type k) const
	{
		namespace ublas = ::boost::numeric::ublas;

		// Remeber:
		// \phi = [ -y(t-1);
		//          -y(t-2);
		//           ...
		//          -y(t-n_a);
		//           u_{1}(t-1);
		//          ...
		//           u_{1}(t-n_b-d);
		//           ...
		//           u_{n_u}(t-1);
		//           ...
		//           u_{n_u}(t-n_b-d)]
		// So in \phi the vector u_k=u(t-k) stays at:
		//   u_k <- [\phi(n_a+k-1):(n_b+d):(n_a+n_u*(n_b+d))]
		vector_type u_k;
		u_k = ublas::subslice(phis_[0], this->output_order()+k-1, this->input_order()+this->input_delay(), this->num_inputs());

		return u_k;
	}

	/// Forgetting factor.
	private: real_type ff_;
	/// Matrix of system parameters estimated by RLS: [A_1 ... A_{n_a} B_1 ... B_{n_b}].
	private: ::std::vector<vector_type> theta_hats_;
	/// The covariance matrix.
	private: ::std::vector<matrix_type> Ps_;
	/// The regression vector.
	private: ::std::vector<vector_type> phis_;
}; // rls_ff_arx_miso_proxy

/**
 * \brief Proxy to identify a MIMO system model by applying the Recursive Least
 *  Square with forgetting-factor algorithm to several MISO system models.
 *
 * The forgetting-factor is varied according to the following law [1]:
 * \f[
 *   \lambda(t) = \lambda_{\text{min}}+(1-\lambda_{\text{min}})2^{-\text{NINT}[\rho \epsilon^2(t)]}
 * \f]
 * where
 * - \f$\rho\f$, the <em>sensitivity gain</em>, is a design parameter.
 * - \f$\epsilon\f$, is the estimation error (i.e., the difference between the
 *    value of the current observed output and the one of the current estimated output).
 * - \f$\text{NINT}[\cdot]\f$ is the nearest integer of \f$[\cdot\]\f$.
 * .
 *
 * References:
 * -# Park et al.
 *    "Fast Tracking RLS Algorithm Using Novel Variable Forgetting Factor with Unity Zone",
 *    Electronic Letters, Vol. 23, 1991.
 * .
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template <typename TraitsT>
class rls_park1991_arx_miso_proxy: public rls_arx_system_identification_strategy<TraitsT>
{
	private: typedef rls_arx_system_identification_strategy<TraitsT> base_type;
	public: typedef TraitsT traits_type;
	public: typedef typename traits_type::real_type real_type;
	public: typedef typename base_type::matrix_type matrix_type;
	public: typedef typename base_type::vector_type vector_type;
	public: typedef typename base_type::size_type size_type;

	public: rls_park1991_arx_miso_proxy()
	: base_type(),
	  ff_(0),
	  rho_(0),
	  theta_hats_(),
	  Ps_(),
	  phis_()
	{
	}

	public: rls_park1991_arx_miso_proxy(size_type n_a, size_type n_b, size_type d, size_type n_y, size_type n_u, real_type ff, real_type rho)
	: base_type(n_a,n_b,d,n_y,n_u),
	  ff_(ff),
	  rho_(rho),
	  theta_hats_(n_y),
	  Ps_(n_y),
	  phis_(n_y)
	{
	}

	public: rls_park1991_arx_miso_proxy(rls_park1991_system_identification_strategy_params<traits_type> const& params)
	: base_type(params),
	  ff_(params.forgetting_factor()),
	  rho_(params.sensitivity_gain()),
	  theta_hats_(params.num_outputs()),
	  Ps_(params.num_outputs()),
	  phis_(params.num_outputs())
	{
	}

	public: real_type forgetting_factor() const
	{
		return ff_;
	}

	public: real_type sensitivity_gain() const
	{
		return rho_;
	}

	private: matrix_type do_Theta_hat() const
	{
		namespace ublas = ::boost::numeric::ublas;

		const size_type na(this->output_order());
		const size_type nb(this->input_order());
		const size_type ny(this->num_outputs());
		const size_type nu(this->num_inputs());
		const size_type nay(na*ny);
		const size_type nbu(nb*nu);
		const size_type n(nay+nbu);
		matrix_type X(n, ny, 0);

		for (size_type i = 0; i < ny; ++i)
		{
			// ith output => ith column of Theta_hat
			// ith column of Theta_hat = [0; ...; 0; a_{ii}^{1}
			const size_type k(i*na);
			ublas::matrix_column<matrix_type> mc(X,i);
			ublas::subrange(mc, k, k+na) = ublas::subrange(theta_hats_[i], 0, na);
			ublas::subrange(mc, nay, n) = ublas::subrange(theta_hats_[i], na, na+nbu);
		}

		return X;
	}

	private: matrix_type do_P() const
	{
		matrix_type aux_P;
//TODO: implement me!!
		return aux_P;
	}

	private: vector_type do_phi() const
	{
		namespace ublas = ::boost::numeric::ublas;

		const size_type na(this->output_order());
		const size_type ny(this->num_outputs());
		const size_type nay(na*ny);
		const size_type nbu(this->input_order()*this->num_inputs());
		const size_type n(nay+nbu);
		vector_type x(n);

		for (size_type i = 0; i < ny; ++i)
		{
			const size_type k(i*na);
			ublas::subrange(x, k, k+na) = ublas::subrange(phis_[i], 0, na);
		}

		ublas::subrange(x, nay, n) = ublas::subrange(phis_[0], na, na+nbu);

		return x;
	}

	private: void do_init()
	{
		const size_type na(this->output_order());
		const size_type nb(this->input_order());
		const size_type d(this->input_delay());
		const size_type ny(this->num_outputs());
		const size_type nu(this->num_inputs());

		// Prepare the data structures for the RLS algorithm
		for (size_type i = 0; i < ny; ++i)
		{
			::dcs::sysid::rls_arx_miso_init(na,
											nb,
											d,
											nu,
											theta_hats_[i],
											Ps_[i],
											phis_[i]);
		}
	}

	private: vector_type do_estimate(vector_type const& y, vector_type const& u)
	{
		const size_type na(this->output_order());
		const size_type nb(this->input_order());
		const size_type d(this->input_delay());
		const size_type ny(this->num_outputs());

		// Apply enabled heuristics
		bool reset(false);
		// Apply the "max-covariance" heuristic (if enabled)
		if (this->max_covariance_heuristic())
		{
			for (size_type i = 0; i < ny && !reset; ++i)
			{
				if (::boost::numeric::ublasx::max(Ps_[i]) > this->max_covariance_heuristic_max_value())
				{
					reset = true;
				}
			}
		}
		// Apply the "condition-number-covariance" heuristic (if enabled)
		if (this->condition_number_covariance_heuristic())
		{
			// Use the following rule-of-thumb:
			//   if cond(A)*eps >= 0.5*10^{-d} then d significant digits of the
			//   solution of the problem involving A are at least correct.
			// NOTE #1: we use reciprocal condition estimator since it's less
			//          time-consuming.
			// NOTE #2: we use log10 to avoid to compute 10^d which can
			//          potentially lead to overflow problems.
			//
			// References:
			// [1] Holistic Numerical Methods Institute,
			//     "Chapter 04.09 -- Adequacy of Solutions"
			//     Found online at http://numericalmethods.eng.usf.edu/mws/gen/04sle/mws_gen_sle_spe_adequacy.pdf

			real_type check_val = ::std::log10(static_cast<real_type>(2)*::std::numeric_limits<real_type>::epsilon())
								  + this->condition_number_covariance_heuristic_trusted_digits();

			for (size_type i = 0; i < ny && !reset; ++i)
			{
				if (::std::log10(::boost::numeric::ublasx::rcond(Ps_[i])) > check_val)
				{
					reset = true;
				}
			}
		}
		if (reset)
		{
			this->reset();
		}

		// Estimate system parameters
DCS_DEBUG_TRACE("BEGIN estimation");//XXX
DCS_DEBUG_TRACE("y(k): " << y);//XXX
DCS_DEBUG_TRACE("u(k): " << u);//XXX
//::std::cerr << "BEGIN estimation" << ::std::endl;//XXX
//::std::cerr << "y(k): " << y << ::std::endl;//XXX
//::std::cerr << "u(k): " << u << ::std::endl;//XXX
		vector_type y_hat(ny);
		for (size_type i = 0; i < ny; ++i)
		{
DCS_DEBUG_TRACE("theta_hat["<< i << "](k): " << theta_hats_[i]);//XXX
DCS_DEBUG_TRACE("P["<< i << "](k): " << Ps_[i]);//XXX
DCS_DEBUG_TRACE("phi["<< i << "](k): " << phis_[i]);//XXX
//::std::cerr << "theta_hat["<< i << "](k): " << theta_hats_[i] << ::std::endl;//XXX
//::std::cerr << "P["<< i << "](k): " << Ps_[i] << ::std::endl;//XXX
//::std::cerr << "phi["<< i << "](k): " << phis_[i] << ::std::endl;//XXX
			y_hat(i) = ::dcs::sysid::rls_park1991_arx_miso(y(i),
													 u,
													 ff_,
													 rho_,
													 na,
													 nb,
													 d,
													 theta_hats_[i],
													 Ps_[i],
													 phis_[i]);
DCS_DEBUG_TRACE("New theta_hat["<< i << "](k): " << theta_hats_[i]);//XXX
DCS_DEBUG_TRACE("New P["<< i << "](k): " << Ps_[i]);//XXX
DCS_DEBUG_TRACE("New rcond(P["<< i << "](k)): " << ::boost::numeric::ublasx::rcond(Ps_[i]));//XXX
DCS_DEBUG_TRACE("New phi["<< i << "](k): " << phis_[i]);//XXX
DCS_DEBUG_TRACE("New e["<< i << "](k): " << (y(i)-y_hat(i)));//XXX
//::std::cerr << "New theta_hat["<< i << "](k): " << theta_hats_[i] << ::std::endl;//XXX
//::std::cerr << "New P["<< i << "](k): " << Ps_[i] << ::std::endl;//XXX
//::std::cerr << "New rcond(P["<< i << "](k)): " << ::boost::numeric::ublasx::rcond(Ps_[i]) << ::std::endl;//XXX
//::std::cerr << "New phi["<< i << "](k): " << phis_[i] << ::std::endl;//XXX
//::std::cerr << "New e["<< i << "](k): " << (y(i)-y_hat(i)) << ::std::endl;//XXX
		}
DCS_DEBUG_TRACE("New y_hat(k): " << y_hat);//XXX
DCS_DEBUG_TRACE("END estimation");//XXX
//::std::cerr << "New y_hat(k): " << y_hat << ::std::endl;//XXX
//::std::cerr << "END estimation" << ::std::endl;//XXX

		return y_hat;
	}

	/// Return matrix A_k from \hat{\Theta}.
	private: matrix_type do_A(size_type k) const
	{
		namespace ublas = ::boost::numeric::ublas;

		const size_type ny(this->num_outputs());

		// Remember, for each output i=1,...,n_y:
		//   \hat{\theta}_i = [a_{ii}^{1};
		//                     ...;
		//                     a_{ii}^{n_a};
		//                     b_{i1}^{1};
		//                     ...;
		//                     b_{i1}^{n_b};
		//                     ...;
		//                     b_{in_u}^{1};
		//                     ...;
		//                     b_{in_u}^{n_b}]
		// So in \hat{\theta}_i the ith diagonal element of matrix A_k stays at:
		//   A_k(i,i) <- \hat{\theta}_i(k)
		matrix_type A_k(ny, ny, 0);
		for (size_type i = 0; i < ny; ++i)
		{
			A_k(i,i) = theta_hats_[i](k-1);
		}

		return A_k;
	}

	/// Return matrix B_k from \hat{\Theta}.
	private: matrix_type do_B(size_type k) const
	{
		namespace ublas = ::boost::numeric::ublas;

		const size_type na(this->output_order());
		const size_type nb(this->input_order());
		const size_type ny(this->num_outputs());
		const size_type nu(this->num_inputs());

		// Remember, for each output i=1,...,n_y:
		//   \hat{\theta}_i = [a_{ii}^{1};
		//                     ...;
		//                     a_{ii}^{n_a};
		//                     b_{i1}^{1};
		//                     ...;
		//                     b_{i1}^{n_b};
		//                     ...;
		//                     b_{in_u}^{1};
		//                     ...;
		//                     b_{in_u}^{n_b}]
		// So in \hat{\theta}_i the ith row of matrix B_k stays at:
		//   B_k(i,:) <- (\hat{\theta}_i(((n_a+k):n_b:n_u))^T
		matrix_type B_k(ny, nu, 0);
		for (size_type i = 0; i < ny; ++i)
		{
			ublas::row(B_k, i) = ublas::subslice(theta_hats_[i], na+k-1, nb, nu);
		}

		return B_k;
	}

    private: vector_type do_y(size_type k) const
    {
        namespace ublas = ::boost::numeric::ublas;

        // Remeber:
        // \phi = [ -y(t-1);
        //          -y(t-2);
        //           ...
        //          -y(t-n_a);
        //           u_{1}(t-1);
        //          ...
        //           u_{1}(t-n_b-d);
        //           ...
        //           u_{n_u}(t-1);
        //           ...
        //           u_{n_u}(t-n_b-d)]
        // So in \phi the vector y_k=y(t-k) stays at:
        //   y_k <- -[\phi(k-1)]
        size_type ny(this->num_outputs());
        vector_type yk(ny);

        for (size_type i = 0; i < ny; ++i)
        {
            yk(i) = -phis_[i](k-1);
        }

        return yk;
    }

    private: vector_type do_u(size_type k) const
    {
        namespace ublas = ::boost::numeric::ublas;

        // Remeber:
        // \phi = [ -y(t-1);
        //          -y(t-2);
        //           ...
        //          -y(t-n_a);
        //           u_{1}(t-1);
        //          ...
        //           u_{1}(t-n_b-d);
        //           ...
        //           u_{n_u}(t-1);
        //           ...
        //           u_{n_u}(t-n_b-d)]
        // So in \phi the vector u_k=u(t-k) stays at:
        //   u_k <- [\phi(n_a+k-1):(n_b+d):(n_a+n_u*(n_b+d))]
        vector_type u_k;
        u_k = ublas::subslice(phis_[0], this->output_order()+k-1, this->input_order()+this->input_delay(), this->num_inputs());

        return u_k;
    }

	/// Forgetting factor.
	private: real_type ff_;
	/// Sensitivity gain.
	private: real_type rho_;
	/// Matrix of system parameters estimated by RLS: [A_1 ... A_{n_a} B_1 ... B_{n_b}].
	private: ::std::vector<vector_type> theta_hats_;
	/// The covariance matrix.
	private: ::std::vector<matrix_type> Ps_;
	/// The regression vector.
	private: ::std::vector<vector_type> phis_;
}; // rls_park1991_arx_miso_proxy

/**
 * \brief Proxy to identify a MIMO system model by applying the Recursive Least
 *  Square with forgetting-factor algorithm to several MISO system models.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template <typename TraitsT>
class rls_kulhavy1984_arx_miso_proxy: public rls_arx_system_identification_strategy<TraitsT>
{
	private: typedef rls_arx_system_identification_strategy<TraitsT> base_type;
	public: typedef TraitsT traits_type;
	public: typedef typename traits_type::real_type real_type;
	public: typedef typename base_type::matrix_type matrix_type;
	public: typedef typename base_type::vector_type vector_type;
	public: typedef typename base_type::size_type size_type;

	public: rls_kulhavy1984_arx_miso_proxy()
	: base_type(),
	  ff_(0),
	  theta_hats_(),
	  Ps_(),
	  phis_()
	{
	}

	public: rls_kulhavy1984_arx_miso_proxy(size_type n_a, size_type n_b, size_type d, size_type n_y, size_type n_u, real_type ff)
	: base_type(n_a,n_b,d,n_y,n_u),
	  ff_(ff),
	  theta_hats_(n_y),
	  Ps_(n_y),
	  phis_(n_y)
	{
	}

	public: rls_kulhavy1984_arx_miso_proxy(rls_kulhavy1984_system_identification_strategy_params<traits_type> const& params)
	: base_type(params),
	  ff_(params.forgetting_factor()),
	  theta_hats_(params.num_outputs()),
	  Ps_(params.num_outputs()),
	  phis_(params.num_outputs())
	{
	}

	public: real_type forgetting_factor() const
	{
		return ff_;
	}

	private: matrix_type do_Theta_hat() const
	{
		namespace ublas = ::boost::numeric::ublas;

		const size_type na(this->output_order());
		const size_type nb(this->input_order());
		const size_type ny(this->num_outputs());
		const size_type nu(this->num_inputs());
		const size_type nay(na*ny);
		const size_type nbu(nb*nu);
		const size_type n(nay+nbu);
		matrix_type X(n, ny, 0);

		for (size_type i = 0; i < ny; ++i)
		{
			// ith output => ith column of Theta_hat
			// ith column of Theta_hat = [0; ...; 0; a_{ii}^{1}
			const size_type k(i*na);
			ublas::matrix_column<matrix_type> mc(X,i);
			ublas::subrange(mc, k, k+na) = ublas::subrange(theta_hats_[i], 0, na);
			ublas::subrange(mc, nay, n) = ublas::subrange(theta_hats_[i], na, na+nbu);
		}

		return X;
	}

	private: matrix_type do_P() const
	{
		matrix_type aux_P;
//TODO: implement me!!
		return aux_P;
	}

	private: vector_type do_phi() const
	{
		namespace ublas = ::boost::numeric::ublas;

		const size_type na(this->output_order());
		const size_type ny(this->num_outputs());
		const size_type nay(na*ny);
		const size_type nbu(this->input_order()*this->num_inputs());
		const size_type n(nay+nbu);
		vector_type x(n);

		for (size_type i = 0; i < ny; ++i)
		{
			const size_type k(i*na);
			ublas::subrange(x, k, k+na) = ublas::subrange(phis_[i], 0, na);
		}

		ublas::subrange(x, nay, n) = ublas::subrange(phis_[0], na, na+nbu);

		return x;
	}

	private: void do_init()
	{
		const size_type na(this->output_order());
		const size_type nb(this->input_order());
		const size_type d(this->input_delay());
		const size_type ny(this->num_outputs());
		const size_type nu(this->num_inputs());

		// Prepare the data structures for the RLS algorithm
		for (size_type i = 0; i < ny; ++i)
		{
			::dcs::sysid::rls_arx_miso_init(na,
											nb,
											d,
											nu,
											theta_hats_[i],
											Ps_[i],
											phis_[i]);
		}
	}

	private: vector_type do_estimate(vector_type const& y, vector_type const& u)
	{
		const size_type na(this->output_order());
		const size_type nb(this->input_order());
		const size_type d(this->input_delay());
		const size_type ny(this->num_outputs());

		// Apply enabled heuristics
		bool reset(false);
		// Apply the "max-covariance" heuristic (if enabled)
		if (this->max_covariance_heuristic())
		{
			for (size_type i = 0; i < ny && !reset; ++i)
			{
				if (::boost::numeric::ublasx::max(Ps_[i]) > this->max_covariance_heuristic_max_value())
				{
					reset = true;
				}
			}
		}
		// Apply the "condition-number-covariance" heuristic (if enabled)
		if (this->condition_number_covariance_heuristic())
		{
			// Use the following rule-of-thumb:
			//   if cond(A)*eps >= 0.5*10^{-d} then d significant digits of the
			//   solution of the problem involving A are at least correct.
			// NOTE #1: we use reciprocal condition estimator since it's less
			//          time-consuming.
			// NOTE #2: we use log10 to avoid to compute 10^d which can
			//          potentially lead to overflow problems.
			//
			// References:
			// [1] Holistic Numerical Methods Institute,
			//     "Chapter 04.09 -- Adequacy of Solutions"
			//     Found online at http://numericalmethods.eng.usf.edu/mws/gen/04sle/mws_gen_sle_spe_adequacy.pdf

			real_type check_val = ::std::log10(static_cast<real_type>(2)*::std::numeric_limits<real_type>::epsilon())
								  + this->condition_number_covariance_heuristic_trusted_digits();

			for (size_type i = 0; i < ny && !reset; ++i)
			{
				if (::std::log10(::boost::numeric::ublasx::rcond(Ps_[i])) > check_val)
				{
					reset = true;
				}
			}
		}
		if (reset)
		{
			this->reset();
		}

		// Estimate system parameters
DCS_DEBUG_TRACE("BEGIN estimation");//XXX
DCS_DEBUG_TRACE("y(k): " << y);//XXX
DCS_DEBUG_TRACE("u(k): " << u);//XXX
		vector_type y_hat(ny);
		for (size_type i = 0; i < ny; ++i)
		{
DCS_DEBUG_TRACE("theta_hat["<< i << "](k): " << theta_hats_[i]);//XXX
DCS_DEBUG_TRACE("P["<< i << "](k): " << Ps_[i]);//XXX
DCS_DEBUG_TRACE("phi["<< i << "](k): " << phis_[i]);//XXX
			y_hat(i) = ::dcs::sysid::rls_kulhavy1984_arx_miso(y(i),
													 u,
													 ff_,
													 na,
													 nb,
													 d,
													 theta_hats_[i],
													 Ps_[i],
													 phis_[i]);
DCS_DEBUG_TRACE("New theta_hat["<< i << "](k): " << theta_hats_[i]);//XXX
DCS_DEBUG_TRACE("New P["<< i << "](k): " << Ps_[i]);//XXX
DCS_DEBUG_TRACE("New rcond(P["<< i << "](k)): " << ::boost::numeric::ublasx::rcond(Ps_[i]));//XXX
DCS_DEBUG_TRACE("New phi["<< i << "](k): " << phis_[i]);//XXX
DCS_DEBUG_TRACE("New e["<< i << "](k): " << (y(i)-y_hat(i)));//XXX
		}
DCS_DEBUG_TRACE("New y_hat(k): " << y_hat);//XXX
DCS_DEBUG_TRACE("END estimation");//XXX

		return y_hat;
	}

	/// Return matrix A_k from \hat{\Theta}.
	private: matrix_type do_A(size_type k) const
	{
		namespace ublas = ::boost::numeric::ublas;

		const size_type ny(this->num_outputs());

		// Remember, for each output i=1,...,n_y:
		//   \hat{\theta}_i = [a_{ii}^{1};
		//                     ...;
		//                     a_{ii}^{n_a};
		//                     b_{i1}^{1};
		//                     ...;
		//                     b_{i1}^{n_b};
		//                     ...;
		//                     b_{in_u}^{1};
		//                     ...;
		//                     b_{in_u}^{n_b}]
		// So in \hat{\theta}_i the ith diagonal element of matrix A_k stays at:
		//   A_k(i,i) <- \hat{\theta}_i(k)
		matrix_type A_k(ny, ny, 0);
		for (size_type i = 0; i < ny; ++i)
		{
			A_k(i,i) = theta_hats_[i](k-1);
		}

		return A_k;
	}

	/// Return matrix B_k from \hat{\Theta}.
	private: matrix_type do_B(size_type k) const
	{
		namespace ublas = ::boost::numeric::ublas;

		const size_type na(this->output_order());
		const size_type nb(this->input_order());
		const size_type ny(this->num_outputs());
		const size_type nu(this->num_inputs());

		// Remember, for each output i=1,...,n_y:
		//   \hat{\theta}_i = [a_{ii}^{1};
		//                     ...;
		//                     a_{ii}^{n_a};
		//                     b_{i1}^{1};
		//                     ...;
		//                     b_{i1}^{n_b};
		//                     ...;
		//                     b_{in_u}^{1};
		//                     ...;
		//                     b_{in_u}^{n_b}]
		// So in \hat{\theta}_i the ith row of matrix B_k stays at:
		//   B_k(i,:) <- (\hat{\theta}_i(((n_a+k):n_b:n_u))^T
		matrix_type B_k(ny, nu, 0);
		for (size_type i = 0; i < ny; ++i)
		{
			ublas::row(B_k, i) = ublas::subslice(theta_hats_[i], na+k-1, nb, nu);
		}

		return B_k;
	}

    private: vector_type do_y(size_type k) const
    {
        namespace ublas = ::boost::numeric::ublas;

        // Remeber:
        // \phi = [ -y(t-1);
        //          -y(t-2);
        //           ...
        //          -y(t-n_a);
        //           u_{1}(t-1);
        //          ...
        //           u_{1}(t-n_b-d);
        //           ...
        //           u_{n_u}(t-1);
        //           ...
        //           u_{n_u}(t-n_b-d)]
        // So in \phi the vector y_k=y(t-k) stays at:
        //   y_k <- -[\phi(k-1)]

        size_type ny(this->num_outputs());
        vector_type yk(ny);

        for (size_type i = 0; i < ny; ++i)
        {
            yk(i) = -phis_[i](k-1);
        }

        return yk;
    }

	private: vector_type do_u(size_type k) const
	{
		namespace ublas = ::boost::numeric::ublas;

		// Remeber:
		// \phi = [ -y(t-1);
		//          -y(t-2);
		//           ...
		//          -y(t-n_a);
		//           u_{1}(t-1);
		//          ...
		//           u_{1}(t-n_b-d);
		//           ...
		//           u_{n_u}(t-1);
		//           ...
		//           u_{n_u}(t-n_b-d)]
		// So in \phi the vector u_k=u(t-k) stays at:
		//   u_k <- [\phi(n_a+k-1):(n_b+d):(n_a+n_u*(n_b+d))]
		vector_type u_k;
		u_k = ublas::subslice(phis_[0], this->output_order()+k-1, this->input_order()+this->input_delay(), this->num_inputs());

		return u_k;
	}

	/// Forgetting factor.
	private: real_type ff_;
	/// Matrix of system parameters estimated by RLS: [A_1 ... A_{n_a} B_1 ... B_{n_b}].
	private: ::std::vector<vector_type> theta_hats_;
	/// The covariance matrix.
	private: ::std::vector<matrix_type> Ps_;
	/// The regression vector.
	private: ::std::vector<vector_type> phis_;
}; // rls_kulhavy1984_arx_miso_proxy

/**
 * \brief Proxy to identify a MIMO system model by applying the Recursive Least
 *  Square with forgetting-factor algorithm to several MISO system models.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template <typename TraitsT>
class rls_bittanti1990_arx_miso_proxy: public rls_arx_system_identification_strategy<TraitsT>
{
	private: typedef rls_arx_system_identification_strategy<TraitsT> base_type;
	public: typedef TraitsT traits_type;
	public: typedef typename traits_type::real_type real_type;
	public: typedef typename base_type::matrix_type matrix_type;
	public: typedef typename base_type::vector_type vector_type;
	public: typedef typename base_type::size_type size_type;

	public: rls_bittanti1990_arx_miso_proxy()
	: base_type(),
	  ff_(0),
	  delta_(0),
	  theta_hats_(),
	  Ps_(),
	  phis_()
	{
	}

	public: rls_bittanti1990_arx_miso_proxy(size_type n_a, size_type n_b, size_type d, size_type n_y, size_type n_u, real_type ff, real_type delta)
	: base_type(n_a,n_b,d,n_y,n_u),
	  ff_(ff),
	  delta_(delta),
	  theta_hats_(n_y),
	  Ps_(n_y),
	  phis_(n_y)
	{
	}

	public: rls_bittanti1990_arx_miso_proxy(rls_bittanti1990_system_identification_strategy_params<traits_type> const& params)
	: base_type(params),
	  ff_(params.forgetting_factor()),
	  delta_(params.correction_factor()),
	  theta_hats_(params.num_outputs()),
	  Ps_(params.num_outputs()),
	  phis_(params.num_outputs())
	{
	}

	public: real_type forgetting_factor() const
	{
		return ff_;
	}

	public: real_type correction_factor() const
	{
		return delta_;
	}

	private: matrix_type do_Theta_hat() const
	{
		namespace ublas = ::boost::numeric::ublas;

		const size_type na(this->output_order());
		const size_type nb(this->input_order());
		const size_type ny(this->num_outputs());
		const size_type nu(this->num_inputs());
		const size_type nay(na*ny);
		const size_type nbu(nb*nu);
		const size_type n(nay+nbu);
		matrix_type X(n, ny, 0);

		for (size_type i = 0; i < ny; ++i)
		{
			// ith output => ith column of Theta_hat
			// ith column of Theta_hat = [0; ...; 0; a_{ii}^{1}
			const size_type k(i*na);
			ublas::matrix_column<matrix_type> mc(X,i);
			ublas::subrange(mc, k, k+na) = ublas::subrange(theta_hats_[i], 0, na);
			ublas::subrange(mc, nay, n) = ublas::subrange(theta_hats_[i], na, na+nbu);
		}

		return X;
	}

	private: matrix_type do_P() const
	{
		matrix_type aux_P;
//TODO: implement me!!
		return aux_P;
	}

	private: vector_type do_phi() const
	{
		namespace ublas = ::boost::numeric::ublas;

		const size_type na(this->output_order());
		const size_type ny(this->num_outputs());
		const size_type nay(na*ny);
		const size_type nbu(this->input_order()*this->num_inputs());
		const size_type n(nay+nbu);
		vector_type x(n);

		for (size_type i = 0; i < ny; ++i)
		{
			const size_type k(i*na);
			ublas::subrange(x, k, k+na) = ublas::subrange(phis_[i], 0, na);
		}

		ublas::subrange(x, nay, n) = ublas::subrange(phis_[0], na, na+nbu);

		return x;
	}

	private: void do_init()
	{
		const size_type na(this->output_order());
		const size_type nb(this->input_order());
		const size_type d(this->input_delay());
		const size_type ny(this->num_outputs());
		const size_type nu(this->num_inputs());

		// Prepare the data structures for the RLS algorithm
		for (size_type i = 0; i < ny; ++i)
		{
			::dcs::sysid::rls_arx_miso_init(na,
											nb,
											d,
											nu,
											theta_hats_[i],
											Ps_[i],
											phis_[i]);
		}
	}

	private: vector_type do_estimate(vector_type const& y, vector_type const& u)
	{
		const size_type na(this->output_order());
		const size_type nb(this->input_order());
		const size_type d(this->input_delay());
		const size_type ny(this->num_outputs());

		// Apply enabled heuristics
		bool reset(false);
		// Apply the "max-covariance" heuristic (if enabled)
		if (this->max_covariance_heuristic())
		{
			for (size_type i = 0; i < ny && !reset; ++i)
			{
				if (::boost::numeric::ublasx::max(Ps_[i]) > this->max_covariance_heuristic_max_value())
				{
					reset = true;
				}
			}
		}
		// Apply the "condition-number-covariance" heuristic (if enabled)
		if (this->condition_number_covariance_heuristic())
		{
			// Use the following rule-of-thumb:
			//   if cond(A)*eps >= 0.5*10^{-d} then d significant digits of the
			//   solution of the problem involving A are at least correct.
			// NOTE #1: we use reciprocal condition estimator since it's less
			//          time-consuming.
			// NOTE #2: we use log10 to avoid to compute 10^d which can
			//          potentially lead to overflow problems.
			//
			// References:
			// [1] Holistic Numerical Methods Institute,
			//     "Chapter 04.09 -- Adequacy of Solutions"
			//     Found online at http://numericalmethods.eng.usf.edu/mws/gen/04sle/mws_gen_sle_spe_adequacy.pdf

			real_type check_val = ::std::log10(static_cast<real_type>(2)*::std::numeric_limits<real_type>::epsilon())
								  + this->condition_number_covariance_heuristic_trusted_digits();

			for (size_type i = 0; i < ny && !reset; ++i)
			{
				if (::std::log10(::boost::numeric::ublasx::rcond(Ps_[i])) > check_val)
				{
					reset = true;
				}
			}
		}
		if (reset)
		{
			this->reset();
		}

		// Estimate system parameters
DCS_DEBUG_TRACE("BEGIN estimation");//XXX
DCS_DEBUG_TRACE("y(k): " << y);//XXX
DCS_DEBUG_TRACE("u(k): " << u);//XXX
		vector_type y_hat(ny);
		for (size_type i = 0; i < ny; ++i)
		{
DCS_DEBUG_TRACE("theta_hat["<< i << "](k): " << theta_hats_[i]);//XXX
DCS_DEBUG_TRACE("P["<< i << "](k): " << Ps_[i]);//XXX
DCS_DEBUG_TRACE("phi["<< i << "](k): " << phis_[i]);//XXX
			y_hat(i) = ::dcs::sysid::rls_bittanti1990_arx_miso(y(i),
															   u,
															   ff_,
															   na,
															   nb,
															   d,
															   theta_hats_[i],
															   Ps_[i],
															   phis_[i],
															   delta_);
DCS_DEBUG_TRACE("New theta_hat["<< i << "](k): " << theta_hats_[i]);//XXX
DCS_DEBUG_TRACE("New P["<< i << "](k): " << Ps_[i]);//XXX
DCS_DEBUG_TRACE("New rcond(P["<< i << "](k)): " << ::boost::numeric::ublasx::rcond(Ps_[i]));//XXX
DCS_DEBUG_TRACE("New phi["<< i << "](k): " << phis_[i]);//XXX
DCS_DEBUG_TRACE("New e["<< i << "](k): " << (y(i)-y_hat(i)));//XXX
		}
DCS_DEBUG_TRACE("New y_hat(k): " << y_hat);//XXX
DCS_DEBUG_TRACE("END estimation");//XXX

		return y_hat;
	}

	/// Return matrix A_k from \hat{\Theta}.
	private: matrix_type do_A(size_type k) const
	{
		namespace ublas = ::boost::numeric::ublas;

		const size_type ny(this->num_outputs());

		// Remember, for each output i=1,...,n_y:
		//   \hat{\theta}_i = [a_{ii}^{1};
		//                     ...;
		//                     a_{ii}^{n_a};
		//                     b_{i1}^{1};
		//                     ...;
		//                     b_{i1}^{n_b};
		//                     ...;
		//                     b_{in_u}^{1};
		//                     ...;
		//                     b_{in_u}^{n_b}]
		// So in \hat{\theta}_i the ith diagonal element of matrix A_k stays at:
		//   A_k(i,i) <- \hat{\theta}_i(k)
		matrix_type A_k(ny, ny, 0);
		for (size_type i = 0; i < ny; ++i)
		{
			A_k(i,i) = theta_hats_[i](k-1);
		}

		return A_k;
	}

	/// Return matrix B_k from \hat{\Theta}.
	private: matrix_type do_B(size_type k) const
	{
		namespace ublas = ::boost::numeric::ublas;

		const size_type na(this->output_order());
		const size_type nb(this->input_order());
		const size_type ny(this->num_outputs());
		const size_type nu(this->num_inputs());

		// Remember, for each output i=1,...,n_y:
		//   \hat{\theta}_i = [a_{ii}^{1};
		//                     ...;
		//                     a_{ii}^{n_a};
		//                     b_{i1}^{1};
		//                     ...;
		//                     b_{i1}^{n_b};
		//                     ...;
		//                     b_{in_u}^{1};
		//                     ...;
		//                     b_{in_u}^{n_b}]
		// So in \hat{\theta}_i the ith row of matrix B_k stays at:
		//   B_k(i,:) <- (\hat{\theta}_i(((n_a+k):n_b:n_u))^T
		matrix_type B_k(ny, nu, 0);
		for (size_type i = 0; i < ny; ++i)
		{
			ublas::row(B_k, i) = ublas::subslice(theta_hats_[i], na+k-1, nb, nu);
		}

		return B_k;
	}

	private: vector_type do_y(size_type k) const
	{
		namespace ublas = ::boost::numeric::ublas;

		// Remeber:
		// \phi = [ -y(t-1);
		//          -y(t-2);
		//           ...
		//          -y(t-n_a);
		//           u_{1}(t-1);
		//          ...
		//           u_{1}(t-n_b-d);
		//           ...
		//           u_{n_u}(t-1);
		//           ...
		//           u_{n_u}(t-n_b-d)]
		// So in \phi the vector y_k=y(t-k) stays at:
		//   y_k <- -[\phi(k-1)]

		size_type ny(this->num_outputs());
		vector_type yk(ny);

		for (size_type i = 0; i < ny; ++i)
		{
			yk(i) = -phis_[i](k-1);
		}

		return yk;
	}

	private: vector_type do_u(size_type k) const
	{
		namespace ublas = ::boost::numeric::ublas;

		// Remeber:
		// \phi = [ -y(t-1);
		//          -y(t-2);
		//           ...
		//          -y(t-n_a);
		//           u_{1}(t-1);
		//          ...
		//           u_{1}(t-n_b-d);
		//           ...
		//           u_{n_u}(t-1);
		//           ...
		//           u_{n_u}(t-n_b-d)]
		// So in \phi the vector u_k=u(t-k) stays at:
		//   u_k <- [\phi(n_a+k-1):(n_b+d):(n_a+n_u*(n_b+d))]
		vector_type u_k;
		u_k = ublas::subslice(phis_[0], this->output_order()+k-1, this->input_order()+this->input_delay(), this->num_inputs());

		return u_k;
	}

	/// Forgetting factor.
	private: real_type ff_;
	/// Bittanti's correction factor.
	private: real_type delta_;
	/// Matrix of system parameters estimated by RLS: [A_1 ... A_{n_a} B_1 ... B_{n_b}].
	private: ::std::vector<vector_type> theta_hats_;
	/// The covariance matrix.
	private: ::std::vector<matrix_type> Ps_;
	/// The regression vector.
	private: ::std::vector<vector_type> phis_;
}; // rls_bittanti1990_arx_miso_proxy

template <typename TraitsT>
::boost::shared_ptr< base_arx_system_identification_strategy<TraitsT> > make_system_identification_strategy(base_system_identification_strategy_params<TraitsT> const& params)
{
	typedef TraitsT traits_type;
	typedef base_arx_system_identification_strategy<traits_type> strategy_type;
	typedef ::boost::shared_ptr<strategy_type> strategy_pointer;

	strategy_pointer ptr_strategy;

	switch (params.category())
	{
		case rls_bittanti1990_system_identification_strategy:
			{
				typedef rls_bittanti1990_system_identification_strategy_params<traits_type> const* strategy_params_impl_pointer;

				strategy_params_impl_pointer ptr_params_impl = dynamic_cast<strategy_params_impl_pointer>(&params);
				if (!ptr_params_impl)
				{
					DCS_EXCEPTION_THROW(::std::runtime_error, "Failed to retrieve RLS FF strategy parameters.");
				}
				if (ptr_params_impl->mimo_as_miso())
				{
					typedef rls_bittanti1990_arx_miso_proxy<traits_type> strategy_impl_type;

					ptr_strategy = ::boost::make_shared<strategy_impl_type>(*ptr_params_impl);
				}
				else
				{
					DCS_EXCEPTION_THROW(::std::runtime_error, "MIMO RLS (Bittanti, 1990) has not been implemented yet.");
//					typedef rls_bittanti1990_arx_mimo_proxy<traits_type> strategy_impl_type;
//
//					ptr_strategy = ::boost::make_shared<strategy_impl_type>(*ptr_params_impl);
				}
			}
			break;
		case rls_ff_system_identification_strategy:
			{
				typedef rls_ff_system_identification_strategy_params<traits_type> const* strategy_params_impl_pointer;

				strategy_params_impl_pointer ptr_params_impl = dynamic_cast<strategy_params_impl_pointer>(&params);
				if (!ptr_params_impl)
				{
					DCS_EXCEPTION_THROW(::std::runtime_error, "Failed to retrieve RLS FF strategy parameters.");
				}
				if (ptr_params_impl->mimo_as_miso())
				{
					typedef rls_ff_arx_miso_proxy<traits_type> strategy_impl_type;

					ptr_strategy = ::boost::make_shared<strategy_impl_type>(*ptr_params_impl);
				}
				else
				{
					typedef rls_ff_arx_mimo_proxy<traits_type> strategy_impl_type;

					ptr_strategy = ::boost::make_shared<strategy_impl_type>(*ptr_params_impl);
				}
			}
			break;
		case rls_kulhavy1984_system_identification_strategy:
			{
				typedef rls_kulhavy1984_system_identification_strategy_params<traits_type> const* strategy_params_impl_pointer;

				strategy_params_impl_pointer ptr_params_impl = dynamic_cast<strategy_params_impl_pointer>(&params);
				if (!ptr_params_impl)
				{
					DCS_EXCEPTION_THROW(::std::runtime_error, "Failed to retrieve RLS FF strategy parameters.");
				}
				if (ptr_params_impl->mimo_as_miso())
				{
					typedef rls_kulhavy1984_arx_miso_proxy<traits_type> strategy_impl_type;

					ptr_strategy = ::boost::make_shared<strategy_impl_type>(*ptr_params_impl);
				}
				else
				{
					DCS_EXCEPTION_THROW(::std::runtime_error, "MIMO RLS (Kulhavy, 1984) has not been implemented yet.");
//					typedef rls_kulhavy1984_arx_mimo_proxy<traits_type> strategy_impl_type;
//
//					ptr_strategy = ::boost::make_shared<strategy_impl_type>(*ptr_params_impl);
				}
			}
			break;
		case rls_park1991_system_identification_strategy:
			{
				typedef rls_park1991_system_identification_strategy_params<traits_type> const* strategy_params_impl_pointer;

				strategy_params_impl_pointer ptr_params_impl = dynamic_cast<strategy_params_impl_pointer>(&params);
				if (!ptr_params_impl)
				{
					DCS_EXCEPTION_THROW(::std::runtime_error, "Failed to retrieve RLS FF strategy parameters.");
				}
				if (ptr_params_impl->mimo_as_miso())
				{
					typedef rls_park1991_arx_miso_proxy<traits_type> strategy_impl_type;

					ptr_strategy = ::boost::make_shared<strategy_impl_type>(*ptr_params_impl);
				}
				else
				{
					DCS_EXCEPTION_THROW(::std::runtime_error, "MIMO RLS (Park, 1991) has not been implemented yet.");
//					typedef rls_park1991_arx_mimo_proxy<traits_type> strategy_impl_type;
//
//					ptr_strategy = ::boost::make_shared<strategy_impl_type>(*ptr_params_impl);
				}
			}
			break;
	}

	return ptr_strategy;
}

}} // Namespace dcs::testbed

#endif // DCS_TESTBED_SYSTEM_IDENTIFICATION_STRATEGIES_HPP


#include <boost/preprocessor/list/for_each_product.hpp>
#include <boost/preprocessor/cat.hpp>
#include <boost/preprocessor/stringize.hpp>
#define SI_UNITS_R (meter_t, (ampere_t, (candela_t, (kelvin_t, (mol_t, (second_t, BOOST_PP_NIL))))))
#define SI_PREFIXES (pico, (nano, (micro, (milli, (centi, (deci, (deca, (hecto, (kilo, (mega, (giga, (tera, BOOST_PP_NIL))))))))))))

#define TEST_GENERATE(r, product) \
		TEST_CASE(BOOST_PP_STRINGIZE(BOOST_PP_CAT product), "format") \
		{ \
			TEST_BODY product \
		}
#define TEST_BODY(prefix, unit_type) \
    using namespace std::literals; \
    REQUIRE(fmt(1 | prefix * unit_type{}) == "1 ["s + prefix_<std::prefix>::str + symbol_<typename basis_<unit_type>::template type<0>, 0>::str + "]"s);
#define CATCH_CONFIG_MAIN
#include <test_util.hpp>
#include <catch2/catch.hpp>
#include <boost/format.hpp>
#include <catch2/catch.hpp>
#include <mitama/dimensional/arithmetic.hpp>
#include <mitama/dimensional/io.hpp>
#include <mitama/dimensional/systems/si/prefix.hpp>
#include <mitama/dimensional/quantity.hpp>
#include <mitama/dimensional/systems/si/all.hpp>
#include <string>
using namespace mitama;
using namespace mitama::systems::si;
using namespace mitama::systems::nonsi;

inline auto fmt = [](auto const &a) {
  return (boost::format("%1%") % a).str();
};

// First, we test metric prefixes format.
TEST_CASE("prefix format tests", "[prefix]") {
  using namespace std::literals;
  REQUIRE(prefix_<std::pico>::str == "p"s);
  REQUIRE(prefix_<std::nano>::str == "n"s);
  REQUIRE(prefix_<std::micro>::str == "u"s);
  REQUIRE(prefix_<std::milli>::str == "m"s);
  REQUIRE(prefix_<std::centi>::str == "c"s);
  REQUIRE(prefix_<std::deci>::str == "d"s);
  REQUIRE(prefix_<std::deca>::str == "da"s);
  REQUIRE(prefix_<std::hecto>::str == "h"s);
  REQUIRE(prefix_<std::kilo>::str == "k"s);
  REQUIRE(prefix_<std::mega>::str == "M"s);
  REQUIRE(prefix_<std::giga>::str == "G"s);
  REQUIRE(prefix_<std::tera>::str == "T"s);
}

// Second, we test SI unit symbols format.
TEST_CASE("symbol format tests", "[symbol]") {
  using namespace std::literals;
  REQUIRE(symbol_<::mitama::systems::si::length, 0>::str == "m"s);
  REQUIRE(symbol_<electric_current, 0>::str == "A"s);
  REQUIRE(symbol_<luminous_intensity, 0>::str == "cd"s);
  REQUIRE(symbol_<thermodynamic_temperature, 0>::str == "K"s);
  REQUIRE(symbol_<::mitama::systems::si::mass, 0>::str == "g"s);
  REQUIRE(symbol_<amount_of_substance, 0>::str == "mol"s);
  REQUIRE(symbol_<::mitama::systems::si::time, 0>::str == "s"s);
}

// Third, we test symbol with metric prefix.
// For each test with cartecian product of metric prefixes and SI units
// will be automatically generated by BOOST_PP_LIST_FOR_EACH_PRODUCT.
BOOST_PP_LIST_FOR_EACH_PRODUCT(TEST_GENERATE, 2, (SI_PREFIXES, SI_UNITS_R))


// Copyright András Vukics 2006–2020. Distributed under the Boost Software License, Version 1.0. (See accompanying file LICENSE.txt)
#include "Archive.h"
#include "BlitzArray.h"
#include "Random.h"

#define BOOST_TEST_MODULE RandomEngineSerialization test
#include <boost/test/unit_test.hpp>

#include <fstream>

using namespace std;

const size_t seed=1001;

template<typename RandomEngine>
auto testcase()
{
  DArray<1>
    array(10),
    arrayInterrupted(10),
    firstHalf(arrayInterrupted(blitz::Range(0,4))),
    secondHalf(arrayInterrupted(blitz::Range(5,9)));

  randomutils::fill<uniform_real_distribution<double>,RandomEngine>(array,seed);

  cerr<<array<<endl;

  RandomEngine reFirst(seed), reSecond(seed);

  randomutils::fill<uniform_real_distribution<double>>(firstHalf,reFirst);

  const string filename{"CPPQEDutils_testing_"+randomutils::EngineID_v<RandomEngine>+".d"};

  BOOST_TEST_CHECKPOINT( "PRE_OUTPUT" );

  {
    ofstream ofs(filename);
    cpputils::oarchive ar(ofs, ios_base::trunc | ios_base::binary);
    ar & reFirst;
  }

  BOOST_TEST_CHECKPOINT( "POST_OUTPUT" );

  // ... some time later restore the other RandomEngine instance to its state
  {
    ifstream ifs(filename);
    cpputils::iarchive ar(ifs, ios_base::binary);
    ar & reSecond;
  }

  BOOST_TEST_CHECKPOINT( "POST_INPUT" );

  randomutils::fill<uniform_real_distribution<double>>(secondHalf,reSecond);

  cerr<<arrayInterrupted<<endl;

  return all(array==arrayInterrupted) ;
}

BOOST_AUTO_TEST_CASE( GSL_RandomEngineTest ) { BOOST_CHECK(testcase<randomutils::GSL_Engine>()); }

BOOST_AUTO_TEST_CASE( STD_MersenneTwisterTest ) { BOOST_CHECK(testcase<mt19937_64>()); }

BOOST_AUTO_TEST_CASE( PCG64_Test ) { BOOST_CHECK(testcase<pcg64>()); }

BOOST_AUTO_TEST_CASE( Xoshiro256pp_Test ) { BOOST_CHECK(testcase<XoshiroCpp::Xoshiro256PlusPlus>()); }


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test
//
// Copyright (c) 2009-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_TEST_OVERLAY_P_Q_HPP
#define BOOST_GEOMETRY_TEST_OVERLAY_P_Q_HPP

#include <fstream>
#include <sstream>
#include <iomanip>

//#define BOOST_GEOMETRY_ROBUSTNESS_USE_DIFFERENCE

#include <geometry_test_common.hpp>

// For mixing int/float
#if defined(_MSC_VER)
#pragma warning( disable : 4244 )
#pragma warning( disable : 4267 )
#endif

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/io/svg/svg_mapper.hpp>

#include <boost/geometry/algorithms/detail/overlay/debug_turn_info.hpp>
#include <boost/geometry/algorithms/intersects.hpp>
#include <boost/geometry/algorithms/touches.hpp>

struct p_q_settings
{
    bool svg;
    bool also_difference;
    bool wkt;
    double tolerance;

    p_q_settings()
        : svg(false)
        , also_difference(false)
        , wkt(false)
        , tolerance(1.0e-3) // since rescaling to integer the tolerance should be less. Was originally 1.0e-6
    {}
};

template <typename Geometry>
inline typename bg::default_area_result<Geometry>::type p_q_area(Geometry const& g)
{
    try
    {
        return bg::area(g);
    }
    catch(bg::empty_input_exception const&)
    {
        return 0;
    }
}

template <typename OutputType, typename CalculationType, typename G1, typename G2>
static bool test_overlay_p_q(std::string const& caseid,
            G1 const& p, G2 const& q,
            p_q_settings const& settings)
{
    bool result = true;

    typedef typename bg::coordinate_type<G1>::type coordinate_type;
    typedef typename bg::point_type<G1>::type point_type;

    bg::model::multi_polygon<OutputType> out_i, out_u, out_d, out_d2;

    CalculationType area_p = p_q_area(p);
    CalculationType area_q = p_q_area(q);
    CalculationType area_d1 = 0, area_d2 = 0;

    bg::intersection(p, q, out_i);
    CalculationType area_i = p_q_area(out_i);

    bg::union_(p, q, out_u);
    CalculationType area_u = p_q_area(out_u);

    double sum = (area_p + area_q) - area_u - area_i;

    bool wrong = std::abs(sum) > settings.tolerance;

    if (settings.also_difference)
    {
        bg::difference(p, q, out_d);
        bg::difference(q, p, out_d2);
        area_d1 = p_q_area(out_d);
        area_d2 = p_q_area(out_d2);
        double sum_d1 = (area_u - area_q) - area_d1;
        double sum_d2 = (area_u - area_p) - area_d2;
        bool wrong_d1 = std::abs(sum_d1) > settings.tolerance;
        bool wrong_d2 = std::abs(sum_d2) > settings.tolerance;

        if (wrong_d1 || wrong_d2)
        {
            wrong = true;
        }
    }

    if (true)
    {
        if ((area_i > 0 && bg::touches(p, q))
            || (area_i <= 0 && bg::intersects(p, q) && ! bg::touches(p, q)))
        {
            std::cout << "Wrong 'touch'! "
                << " Intersection area: " << area_i
                << " Touch gives: " << std::boolalpha << bg::touches(p, q)
                << std::endl;
            wrong = true;
        }
    }

    bool svg = settings.svg;

    if (wrong || settings.wkt)
    {
        if (wrong)
        {
            result = false;
            svg = true;
        }
        bg::unique(out_i);
        bg::unique(out_u);

        std::cout
            << "type: " << string_from_type<CalculationType>::name()
            << " id: " << caseid
            << " area i: " << area_i
            << " area u: " << area_u
            << " area p: " << area_p
            << " area q: " << area_q
            << " sum: " << sum;

        if (settings.also_difference)
        {
            std::cout
                << " area d1: " << area_d1
                << " area d2: " << area_d2;
        }
        std::cout
            << std::endl
            << std::setprecision(9)
            << " p: " << bg::wkt(p) << std::endl
            << " q: " << bg::wkt(q) << std::endl
            << " i: " << bg::wkt(out_i) << std::endl
            << " u: " << bg::wkt(out_u) << std::endl
            ;

    }

    if(svg)
    {
        std::ostringstream filename;
        filename << "overlay_" << caseid << "_"
            << string_from_type<coordinate_type>::name()
            << string_from_type<CalculationType>::name()
            << ".svg";

        std::ofstream svg(filename.str().c_str());

        bg::svg_mapper<point_type> mapper(svg, 500, 500);

        mapper.add(p);
        mapper.add(q);

        // Input shapes in green/blue
        mapper.map(p, "fill-opacity:0.5;fill:rgb(153,204,0);"
                "stroke:rgb(153,204,0);stroke-width:3");
        mapper.map(q, "fill-opacity:0.3;fill:rgb(51,51,153);"
                "stroke:rgb(51,51,153);stroke-width:3");

        if (settings.also_difference)
        {
            for (BOOST_AUTO(it, out_d.begin()); it != out_d.end(); ++it)
            {
                mapper.map(*it,
                    "opacity:0.8;fill:none;stroke:rgb(255,128,0);stroke-width:4;stroke-dasharray:1,7;stroke-linecap:round");
            }
            for (BOOST_AUTO(it, out_d2.begin()); it != out_d2.end(); ++it)
            {
                mapper.map(*it,
                    "opacity:0.8;fill:none;stroke:rgb(255,0,255);stroke-width:4;stroke-dasharray:1,7;stroke-linecap:round");
            }
        }
        else
        {
            for (BOOST_AUTO(it, out_i.begin()); it != out_i.end(); ++it)
            {
                mapper.map(*it, "fill-opacity:0.1;stroke-opacity:0.4;fill:rgb(255,0,0);"
                        "stroke:rgb(255,0,0);stroke-width:4");
            }
            for (BOOST_AUTO(it, out_u.begin()); it != out_u.end(); ++it)
            {
                mapper.map(*it, "fill-opacity:0.1;stroke-opacity:0.4;fill:rgb(255,0,0);"
                        "stroke:rgb(255,0,255);stroke-width:4");
            }
        }
    }
    return result;
}

#endif // BOOST_GEOMETRY_TEST_OVERLAY_P_Q_HPP


#include <random>
#include <string>
#include <iostream>
#include <fstream>
#include <chrono>

#include <Eigen/Dense>

#include <cannon/logic/cnf.hpp>
#include <cannon/logic/dpll.hpp>
#include <cannon/logic/read_dimacs_cnf.hpp>

using namespace Eigen;

using namespace cannon::logic;

double calculate_moms(const CNFFormula& form, const Assignment& a, const
    Simplification& s, unsigned int prop) {
  unsigned int num_smallest_pos = 0;
  unsigned int num_smallest_neg = 0;
  unsigned int smallest_clause_size = form.get_smallest_clause_size(a, s);

  for (unsigned int i = 0; i < form.clauses_.size(); i++) {
    if (s[i])
      continue;

    if (form.clauses_[i].contains_prop(a, prop)) {
      if (form.clauses_[i].size(a) == smallest_clause_size) {
        if (form.clauses_[i].has_pos_literal(a, prop)) {
          num_smallest_pos += 1;
        } else {
          num_smallest_neg += 1;
        }
      }
    }
  }

  double first_part = (double)(num_smallest_pos + num_smallest_neg) * std::pow(2.0, 3.0);
  double second_part = num_smallest_pos * num_smallest_neg;

  return first_part + second_part;
}

unsigned int uniform_random_prop(const DPLLState& ds,
    const Assignment& a, const Simplification& s, std::vector<unsigned int>& props) {
  RandomComparator comp(ds);
  std::sort(props.begin(), props.end(), comp);

  return props[props.size() - 1];
}

unsigned int two_clause_prop(const DPLLState& ds,
    const Assignment& a, const Simplification& s, std::vector<unsigned int>& props) {
  RandomComparator rc(ds);
  TwoClauseComparator tc(ds, a, s, props, rc);

  std::sort(props.begin(), props.end(), tc);
  return props[props.size() - 1];
}

unsigned int vsids_prop(const DPLLState& ds, const Assignment& a, const Simplification& s,
    std::vector<unsigned int>& props) {
  RandomComparator rc(ds);
  VsidsComparator vc(ds, rc);
  TwoClauseComparator tc(ds, a, s, props, vc);

  std::sort(props.begin(), props.end(), tc);
  return props[props.size() - 1];
}

bool uniform_random_assign(const CNFFormula& form, const Assignment& a,
    const Simplification& s, unsigned int prop, const std::vector<std::vector<unsigned int>>& watched) {
  std::random_device rd;
  static std::default_random_engine gen(rd());
  std::uniform_real_distribution<double> d;

  double t = d(gen);
  return t < 0.5;
}

bool max_vote_assign(const CNFFormula& form, const Assignment& a, const
    Simplification& s, unsigned int prop, const std::vector<std::vector<unsigned int>>& watched) {
  std::random_device rd;
  static std::default_random_engine gen(rd());
  std::uniform_real_distribution<double> d;

  int num_pos = 0;
  int num_neg = 0;
  for (unsigned int c_num : watched[prop]) {
    if (s[c_num])
      continue;

    for (auto &l : form.clauses_[c_num].literals_) {
      if (l.prop_ == prop && l.negated_)
        num_neg += 1;

      if (l.prop_ == prop && !l.negated_)
        num_pos += 1;
    }
  }

  if (num_pos == num_neg) {
    double t = d(gen);
    return t < 0.5;
  } else {
    return num_pos > num_neg;
  }
}

bool min_vote_assign(const CNFFormula& form, const Assignment& a, const
    Simplification& s, unsigned int prop, const std::vector<std::vector<unsigned int>>& watched) {
  std::random_device rd;
  static std::default_random_engine gen(rd());
  std::uniform_real_distribution<double> d;

  int num_pos = 0;
  int num_neg = 0;
  for (unsigned int c_num : watched[prop]) {
    if (s[c_num])
      continue;

    for (auto &l : form.clauses_[c_num].literals_) {
      if (l.prop_ == prop && l.negated_)
        num_neg += 1;

      if (l.prop_ == prop && !l.negated_)
        num_pos += 1;
    }
  }

  if (num_pos == num_neg) {
    double t = d(gen);
    return t < 0.5;
  } else {
    return num_pos < num_neg;
  }
}

int main(int argc, char **argv) {
  DPLLResult r;
  Assignment a;
  int c;

  assert(argc >= 2);
  std::string filename(argv[1]);
  std::string filename_copy(filename);

  std::string report_filename =
    filename_copy.erase(filename_copy.find_first_of("."),
        filename_copy.size()-1);

  std::string yaml_report_filename(report_filename);
  report_filename += "_report.txt";
  yaml_report_filename += "_report.yaml";

  std::ofstream os(report_filename);
  std::ofstream y_os(yaml_report_filename); // Write to YAML as well

  y_os << "stats:" << std::endl;

  CNFFormula f = load_cnf(filename);
  std::cout << "Parsed passed CNF file as " << f << std::endl;
  os << "Parsed passed CNF file " << filename << " as " << std::endl << f << std::endl << std::endl;

  auto start = std::chrono::steady_clock::now();
  //std::tie(r, a, c) = dpll(f, uniform_random_prop, uniform_random_assign, std::chrono::seconds(1200)); // Random heuristic
  //std::tie(r, a, c) = dpll(f, two_clause_prop, uniform_random_assign); // 2-clause heuristic
  std::tie(r, a, c) = dpll(f, vsids_prop, min_vote_assign, std::chrono::seconds(1200)); // custom heuristic
  auto end = std::chrono::steady_clock::now();
  auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
  y_os << "  time_micros: " << std::to_string(duration.count()) << std::endl;

  std::cout << "DPLL recursively called " << c << " times" << std::endl;
  os << "DPLL recursively called " << c << " times" << std::endl << std::endl;
  y_os << "  calls: " << std::to_string(c) << std::endl;

  std::cout << "DPLL on " << filename << " returned result " << r << " with assignment " << a << std::endl;
  os << "DPLL returned result " << r << " with assignment " << a << std::endl << std::endl;

  y_os << "  sat: ";
  if (r == DPLLResult::Satisfiable)
    y_os << "true" << std::endl;
  else
    y_os << "false" << std::endl;

  y_os << "  cutoff: ";
  if (r == DPLLResult::Unknown)
    y_os << "true" << std::endl;
  else
    y_os << "false" << std::endl;

  Simplification s = std::valarray<bool>(false, f.get_num_clauses());
  if (r == DPLLResult::Satisfiable) {
    std::cout << "Evaluation of formula on returned assignment is" << std::endl;
    std::cout << f.eval(a, s) << std::endl;
    assert(f.eval(a, s) == PropAssignment::True);

    os << "Evaluation of formula on returned assignment is " << f.eval(a, s);
    os.close();
  }
}


#ifndef HEADERS_BANDITPAM_HPP_
#define HEADERS_BANDITPAM_HPP_

#include "kmedoids_algorithm.hpp"
#include "log_helper.hpp"

#include <carma>
#include <armadillo>
#include <vector>
#include <fstream>
#include <iostream>
#include <chrono>
#include <omp.h>

/**
 *  \brief Class implementation for BanditPAM algorithm.
 *
 *  BanditPAM class. Consists of all necessary functions to implement
 *  BanditPAM algorithm.
 *
 */
class BanditPAM : public km::KMedoids {
 public:
    /*! \brief Runs BanditPAM algorithm.
    *
    *  Run the BanditPAM algorithm to identify a dataset's medoids.
    *
    *  @param input_data Input data to cluster
    */
    void fit_bpam(const arma::mat& inputData);

    /*! \brief Build step for BanditPAM
    *
    *  Runs build step for the BanditPAM algorithm. Draws batch sizes with replacement
    *  from reference set, and uses the estimated reward of the potential medoid
    *  solutions on the reference set to update the reward confidence intervals and
    *  accordingly narrow the solution set.
    *
    *  @param data Transposed input data to find the medoids of
    *  @param medoid_indices Uninitialized array of medoids that is modified in place
    *  as medoids are identified
    *  @param medoids Matrix of possible medoids that is updated as the bandit
    *  learns which datapoints will be unlikely to be good candidates
    */
    void build(const arma::mat& data, arma::rowvec& medoidIndices, arma::mat& medoids);

    /*! \brief Estimates the mean reward for each arm in build step
    *
    *  Estimates the mean reward (or loss) for each arm in the identified targets
    *  in the build step and returns a list of the estimated reward.
    *
    *  @param data Transposed input data to find the medoids of
    *  @param target Set of target datapoints to be estimated
    *  @param batch_size Number of datapoints sampled for updating confidence
    *  intervals
    *  @param best_distances Array of best distances from each point to previous set
    *  of medoids
    *  @param use_absolute Determines whether the absolute cost is added to the total
    */
    arma::rowvec build_target(
        const arma::mat& data,
        arma::uvec& target,
        size_t batch_size,
        arma::rowvec& best_distances,
        bool use_absolute
    );

    /*! \brief Swap step for BanditPAM
    *
    *  Runs Swap step for the BanditPAM algorithm. Draws batch sizes with replacement
    *  from reference set, and uses the estimated reward of the potential medoid
    *  solutions on the reference set to update the reward confidence intervals and
    *  accordingly narrow the solution set.
    *
    *  @param data Transposed input data to find the medoids of
    *  @param medoid_indices Array of medoid indices created from the build step
    *  that is modified in place as better medoids are identified
    *  @param medoids Matrix of possible medoids that is updated as the bandit
    *  learns which datapoints will be unlikely to be good candidates
    *  @param assignments Uninitialized array of indices corresponding to each
    *  datapoint assigned the index of the medoid it is closest to
    */
    void swap(
        const arma::mat& data,
        arma::rowvec& medoidIndices,
        arma::mat& medoids,
        arma::rowvec& assignments
    );

    /*! \brief Estimates the mean reward for each arm in swap step
    *
    *  Estimates the mean reward (or loss) for each arm in the identified targets
    *  in the swap step and returns a list of the estimated reward.
    *
    *  @param data Transposed input data to find the medoids of
    *  @param sigma Dispersion paramater for each datapoint
    *  @param targets Set of target datapoints to be estimated
    *  @param batch_size Number of datapoints sampled for updating confidence
    *  intervals
    *  @param best_distances Array of best distances from each point to previous set
    *  of medoids
    *  @param second_best_distances Array of second smallest distances from each
    *  point to previous set of medoids
    *  @param assignments Assignments of datapoints to their closest medoid
    */
    arma::vec swap_target(
        const arma::mat& data,
        arma::rowvec& medoidIndices,
        arma::uvec& targets,
        size_t batch_size,
        arma::rowvec& best_distances,
        arma::rowvec& second_best_distances,
        arma::rowvec& assignments
    );
};
#endif // HEADERS_BANDITPAM_HPP_


#include "rosplan_knowledge_base/KnowledgeComparitor.h"

#include <boost/algorithm/string.hpp>

/* implementation of KnowledgeComparitor.h */
namespace KCL_rosplan {

	/**
	 * returns true iff inequality is true
	 */
	bool KnowledgeComparitor::inequalityTrue(const rosplan_knowledge_msgs::KnowledgeItem &a, const std::vector<rosplan_knowledge_msgs::KnowledgeItem> &modelFunctions) {

		double lhs = KnowledgeComparitor::evaluateExpression(a.ineq.LHS, modelFunctions);
		double rhs = KnowledgeComparitor::evaluateExpression(a.ineq.RHS, modelFunctions);

		switch(a.ineq.comparison_type) {
			case rosplan_knowledge_msgs::DomainInequality::GREATER:   return lhs>rhs;
			case rosplan_knowledge_msgs::DomainInequality::GREATEREQ: return lhs>=rhs;
			case rosplan_knowledge_msgs::DomainInequality::LESS:      return lhs<rhs;
			case rosplan_knowledge_msgs::DomainInequality::LESSEQ:    return lhs<=rhs;
			case rosplan_knowledge_msgs::DomainInequality::EQUALS:    return lhs==rhs;
		}
		ROS_ERROR("KCL: (%s) Knowledge Comapritor does not recognise comparison type.", ros::this_node::getName().c_str());
		return false;
	}

	/**
	 * evaluates the prefix expression
	 */
	double KnowledgeComparitor::evaluateExpression(const rosplan_knowledge_msgs::ExprComposite &expr, const std::vector<rosplan_knowledge_msgs::KnowledgeItem> &modelFunctions) {

		std::vector<rosplan_knowledge_msgs::ExprBase> tokens = expr.tokens;

		bool pending_operand = false;
		std::vector<rosplan_knowledge_msgs::ExprBase> operator_stack;
		std::vector<rosplan_knowledge_msgs::ExprBase> operand_stack;
		for(int i=0;i<tokens.size();i++) {
			rosplan_knowledge_msgs::ExprBase token = tokens[i];
			switch(token.expr_type) {
				case rosplan_knowledge_msgs::ExprBase::OPERATOR:
					operator_stack.push_back(token);
					pending_operand = false;
					break;
				case rosplan_knowledge_msgs::ExprBase::CONSTANT:
				case rosplan_knowledge_msgs::ExprBase::FUNCTION:
				case rosplan_knowledge_msgs::ExprBase::SPECIAL:

					if(pending_operand) {
						while (operand_stack.size()>0) {
							rosplan_knowledge_msgs::ExprBase lhs = token;
							rosplan_knowledge_msgs::ExprBase rhs = operand_stack.back();
							operand_stack.pop_back();
							rosplan_knowledge_msgs::ExprBase op = operator_stack.back();
							operator_stack.back();
							double value = KnowledgeComparitor::evaluateOperation(
									KnowledgeComparitor::getValue(lhs, modelFunctions),
									KnowledgeComparitor::getValue(rhs, modelFunctions),
									op);
							token.expr_type = rosplan_knowledge_msgs::ExprBase::CONSTANT;
							token.constant = value;
						}
					}
					pending_operand = true;
					operand_stack.push_back(token);
					break;
			}
		}

		rosplan_knowledge_msgs::ExprBase result = operand_stack.back();
		operand_stack.pop_back();
		return getValue(result, modelFunctions);
	}

	/*
	 * evaluate numeric expression between two ExprBase messages.
	 */
	double KnowledgeComparitor::evaluateOperation(const double &lhs, const double &rhs, const rosplan_knowledge_msgs::ExprBase &op) {

		switch(op.op) {
			case rosplan_knowledge_msgs::ExprBase::ADD: return lhs+rhs;
			case rosplan_knowledge_msgs::ExprBase::SUB: return lhs-rhs;
			case rosplan_knowledge_msgs::ExprBase::MUL: return lhs*rhs;
			case rosplan_knowledge_msgs::ExprBase::DIV: return lhs/rhs;
			case rosplan_knowledge_msgs::ExprBase::UMINUS: return -lhs;
		}
		ROS_ERROR("KCL: (%s) Knowledge Comapritor does not recognise operator type.", ros::this_node::getName().c_str());
		return 0;
	}

	/*
	 * Find numeric value of an ExprBase message, i.e. evaluate the value of PDDL function
	 */
	double KnowledgeComparitor::getValue(const rosplan_knowledge_msgs::ExprBase &expr, const std::vector<rosplan_knowledge_msgs::KnowledgeItem> &modelFunctions) {
		switch(expr.expr_type) {
			case rosplan_knowledge_msgs::ExprBase::CONSTANT:
				return expr.constant;
			case rosplan_knowledge_msgs::ExprBase::FUNCTION:
				std::vector<rosplan_knowledge_msgs::KnowledgeItem>::const_iterator fit = modelFunctions.begin();
				for(; fit!=modelFunctions.end(); fit++) {
					if(expr.function.name == fit->attribute_name && expr.function.typed_parameters.size() == fit->values.size()) {
						// parameters
						bool match = true;
						for(size_t i=0;i<fit->values.size();i++) {
							if("" != expr.function.typed_parameters[i].value && !(boost::iequals(expr.function.typed_parameters[i].value, fit->values[i].value))) {
								match = false;
							}
						}
						if(match) {
							return fit->function_value;
						}
					}
				}
				return 0;
		}
		ROS_ERROR("KCL: (%s) Knowledge Comapritor cannot get value for base expression.", ros::this_node::getName().c_str());
		return 0;
	}

	/**
	 * returns true iff a matches the knowledge in b.
	 */
	bool KnowledgeComparitor::containsKnowledge(const rosplan_knowledge_msgs::KnowledgeItem &a, const rosplan_knowledge_msgs::KnowledgeItem &b) {

		int matches = 0;
		if(a.knowledge_type != b.knowledge_type) return false;

		switch(a.knowledge_type) {

		case rosplan_knowledge_msgs::KnowledgeItem::INSTANCE:
			{
				// check instance knowledge
				if(0!=a.instance_type.compare(b.instance_type)) return false;
				if(a.instance_name!="" && !boost::iequals(a.instance_name, b.instance_name)) return false;
				return true;
			}
			break;

		case rosplan_knowledge_msgs::KnowledgeItem::FUNCTION:

			// check function value
			if(a.function_value != b.function_value) return false;

		// check function or fact label and parameters all match
		case rosplan_knowledge_msgs::KnowledgeItem::FACT:
			{
				// label
				if(a.attribute_name=="") return true;
				if(!boost::iequals(a.attribute_name, b.attribute_name)) return false;

				// negative fact
				if(a.is_negative != b.is_negative) return false;

				// parameters
				if(a.values.size() != b.values.size()) return false;
				for(size_t i=0;i<a.values.size();i++) {
					if("" == a.values[i].value) {
						++matches;
					} else if(boost::iequals(a.values[i].value, b.values[i].value)) {
						++matches;
					}
				}
				return (matches == a.values.size());
			}
			break;
		}
	}

	/**
	 * returns true is the knowledge item contains the instance, as instance or attribute parameter.
	 */
	bool KnowledgeComparitor::containsInstance(const rosplan_knowledge_msgs::KnowledgeItem &a, std::string &name) {

		if(0==a.instance_name.compare(name))
			return true;

		for(size_t i=0;i<a.values.size();i++) {
			if(boost::iequals(a.values[i].value, name))
				return true;
		}

		return false;
	}

} // close namespace


/* Copyright John Reid 2007
*/

#include "bio-pch.h"

#include "bio/defs.h"

#include "bio/random.h"

#include <boost/date_time/posix_time/posix_time_types.hpp>
#include <boost/date_time/posix_time/time_parsers.hpp>
using namespace boost;
using namespace boost::posix_time;

#include <limits>
#include <iostream>
using namespace std;

#undef min
#undef max

BIO_NS_START

default_rng_t default_rng;

namespace impl
{
	typedef boost::uniform_int<size_t> dist_t;
	typedef boost::variate_generator<default_rng_t, dist_t> var_gen_t;

	dist_t dist(numeric_limits<size_t>::min(), numeric_limits<size_t>::max());
	var_gen_t var_gen(default_rng, dist);

	bool default_rng_already_seeded = false;
};

double
get_uniform_01()
{
	ensure_default_rng_seeded();

	typedef boost::uniform_real<> distribution_type;
	typedef boost::variate_generator<default_rng_t &, distribution_type> gen_type;
	return gen_type(default_rng, distribution_type(0,1))();
}

size_t
get_uniform_index(size_t max)
{
	if (0 == max)
	{
		throw std::logic_error( "max must be >= 1" );
	}

	// Define a uniform random number distribution of integer values between
	// 0 and max-1 inclusive.
	typedef boost::uniform_int<size_t> distribution_type;
	typedef boost::variate_generator<default_rng_t &, distribution_type> gen_type;

	return gen_type(default_rng, distribution_type(0, max - 1))();
}

size_t get_random_seed()
{
#ifdef _DEBUG
	return 1; //guarantee reproducibility
#else
	const ptime now(posix_time::second_clock::universal_time());
	const ptime then(posix_time::from_iso_string("19720112T000000"));
	const time_duration diff = now - then;
	return (size_t) diff.total_seconds();
#endif
}

void
seed_default_rng(size_t seed)
{
	cout << "Using random seed: " << seed << endl;

	//seed the random number generator
	default_rng.seed((default_rng_t::result_type) seed);

	impl::default_rng_already_seeded = true;
}

void
ensure_default_rng_seeded()
{
	if (! impl::default_rng_already_seeded)
	{
		seed_default_rng(get_random_seed());
	}
}

BIO_NS_END


// Copyright (c) 2015-2019 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#include "quality_by_depth.hpp"

#include <boost/variant.hpp>
#include <boost/optional.hpp>

#include "io/variant/vcf_record.hpp"

namespace octopus { namespace csr {

const std::string QualityByDepth::name_ = "QD";

QualityByDepth::QualityByDepth(bool recalculate) : depth_ {recalculate, true} {}

std::unique_ptr<Measure> QualityByDepth::do_clone() const
{
    return std::make_unique<QualityByDepth>(*this);
}

Measure::ResultType QualityByDepth::get_default_result() const
{
    return boost::optional<double> {};
}

Measure::ResultType QualityByDepth::do_evaluate(const VcfRecord& call, const FacetMap& facets) const
{
    auto depth = boost::get<std::size_t>(depth_.evaluate(call, facets));
    boost::optional<double> result {};
    if (depth > 0) {
        result = static_cast<double>(*call.qual()) / depth;
    }
    return result;
}

Measure::ResultCardinality QualityByDepth::do_cardinality() const noexcept
{
    return depth_.cardinality();
}

const std::string& QualityByDepth::do_name() const
{
    return name_;
}

std::string QualityByDepth::do_describe() const
{
    return "QUAL divided by DP";
}

std::vector<std::string> QualityByDepth::do_requirements() const
{
    return depth_.requirements();
}

bool QualityByDepth::is_equal(const Measure& other) const noexcept
{
    return depth_ == static_cast<const QualityByDepth&>(other).depth_;
}

} // namespace csr
} // namespace octopus