/**
 * \file dcs/testbed/rao2013_dynaqos_application_manager.hpp
 *
 * \brief Application manager based on the work by (RAO et al., 2013)
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2014   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_RAO2013_DYNAQOS_APPLICATION_MANAGER_HPP
#define DCS_TESTBED_RAO2013_DYNAQOS_APPLICATION_MANAGER_HPP

#include <boost/smart_ptr.hpp>
#include <cmath>
#include <cstddef>
#include <dcs/assert.hpp>
#include <dcs/debug.hpp>
#include <dcs/exception.hpp>
#include <dcs/logging.hpp>
#include <dcs/macro.hpp>
#include <dcs/math/traits/float.hpp>
#include <dcs/testbed/application_performance_category.hpp>
#include <dcs/testbed/base_application_manager.hpp>
#include <dcs/testbed/data_smoothers.hpp>
#include <dcs/testbed/virtual_machine_performance_category.hpp>
#include <fl/Headers.h>
#include <fstream>
#include <limits>
#include <map>
#include <stdexcept>
#include <string>
#include <vector>

namespace dcs { namespace testbed {

/**
 * \brief Application manager based on the work by (Rao et al., 2013)
 *
 * \tparam Traits The traits type
 *
 * This class implements the DynaQoS framework proposed in [1,2].
 *
 * References:
 * -# J. Rao and Y. Wei and J. Gong and C.-Z. Xu,
 *    "QoS Guarantees and Service Differentiation for Dynamic Cloud Applications,"
 *    IEEE Transactions on Network and Service Management 10(1):43-55, 2013.
 * -# J. Wei and C.-Z. Xu,
 *    "eQoS: Provisioning of Client-Perceived End-to-End QoS Guarantees in Web Servers,"
 *    IEEE Transactions on Computers 55(12):1543-1556, 2006.
 * .
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template <typename TraitsT>
class rao2013_dynaqos_application_manager: public base_application_manager<TraitsT>
{
	private: typedef base_application_manager<TraitsT> base_type;
	public: typedef typename base_type::traits_type traits_type;
	public: typedef typename traits_type::real_type real_type;
	private: typedef typename base_type::app_type app_type;
	private: typedef typename base_type::app_pointer app_pointer;
	private: typedef typename base_type::vm_identifier_type vm_identifier_type;
	private: typedef typename app_type::sensor_type sensor_type;
	private: typedef typename app_type::sensor_pointer sensor_pointer;
	private: typedef ::std::vector<real_type> observation_container;
	private: typedef ::std::map<application_performance_category,observation_container> observation_map;
	private: typedef ::std::map<application_performance_category,sensor_pointer> app_sensor_map;
	private: typedef ::std::map<virtual_machine_performance_category,::std::map<vm_identifier_type,sensor_pointer> > vm_sensor_map;

	private: static const ::std::string alpha_fuzzy_var_name;
	private: static const ::std::string e_fuzzy_var_name;
	private: static const ::std::string de_fuzzy_var_name;
	private: static const ::std::string du_fuzzy_var_name;

	public: rao2013_dynaqos_application_manager()
	: gamma_(0.8),
	  Ke_(0),
	  Kde_(0),
	  p_rc_fuzzy_eng_(new ::fl::Engine()),
	  p_sfc_fuzzy_eng_(new ::fl::Engine())
	{
		init();
	}

	public: void discount_factor(real_type value)
	{
		gamma_ = value;
	}

	public: real_type discount_factor() const
	{
		return gamma_;
	}

	public: void export_data_to(::std::string const& fname)
	{
		dat_fname_ = fname;
	}

	private: void init()
	{
		DCS_DEBUG_ASSERT( p_rc_fuzzy_eng_ );
		DCS_DEBUG_ASSERT( p_sfc_fuzzy_eng_ );

		const real_type one_third = 1.0/3.0;
		const real_type two_third = 2.0/3.0;
		const real_type one_sixth = 1.0/6.0;
		const real_type five_sixth = 5.0/6.0;

		::fl::InputVariable* p_iv = 0;
		::fl::OutputVariable* p_ov = 0;
		::fl::RuleBlock* p_rules = 0;

		// Setup the Resource Controller
		//   Membership functions taken from (Wei et al.,2006) [2]

		for (::std::size_t i = 0; i < 2; ++i)
		{
			p_iv = new ::fl::InputVariable();
			p_iv->setEnabled(true);
			switch (i)
			{
				case 0: // \Delta e(k)
					p_iv->setName(de_fuzzy_var_name);
					break;
				case 1: // e(k)
					p_iv->setName(e_fuzzy_var_name);
					break;
			}
			p_iv->setRange(-1, 1);
			p_iv->addTerm(new ::fl::Ramp("NL", -two_third, -1));
			p_iv->addTerm(new ::fl::Triangle("NM", -1, -two_third, -one_third));
			p_iv->addTerm(new ::fl::Triangle("NS", -two_third, -one_third, 0));
			p_iv->addTerm(new ::fl::Triangle("ZE", -one_third, 0, one_third));
			p_iv->addTerm(new ::fl::Triangle("PS", 0, one_third, two_third));
			p_iv->addTerm(new ::fl::Triangle("PM", one_third, two_third, 1));
			p_iv->addTerm(new ::fl::Ramp("PL", two_third, 1));
			p_rc_fuzzy_eng_->addInputVariable(p_iv);
		}

		p_ov = new ::fl::OutputVariable();
		p_ov->setEnabled(true);
		p_ov->setName(du_fuzzy_var_name);
		p_ov->setRange(-1, 1);
#if defined(FL_VERSION) // Until fuzzylite v. 5.x (the FL_VERSION macro was removed since fuzzylite 6.x)
		p_ov->fuzzyOutput()->setAccumulation(new ::fl::Maximum());
#else // Since fuzzylite v. 6.x
		p_ov->fuzzyOutput()->setAggregation(new ::fl::Maximum());
#endif // FL_VERSION
		p_ov->setDefuzzifier(new ::fl::Centroid());
		p_ov->setDefaultValue(::fl::nan);
		p_ov->setLockPreviousValue(false);
		p_ov->addTerm(new ::fl::Ramp("NL", -two_third, -1));
		p_ov->addTerm(new ::fl::Triangle("NM", -1, -two_third, -one_third));
		p_ov->addTerm(new ::fl::Triangle("NS", -two_third, -one_third, 0));
		p_ov->addTerm(new ::fl::Triangle("ZE", -one_third, 0, one_third));
		p_ov->addTerm(new ::fl::Triangle("PS", 0, one_third, two_third));
		p_ov->addTerm(new ::fl::Triangle("PM", one_third, two_third, 1));
		p_ov->addTerm(new ::fl::Ramp("PL", two_third, 1));
		p_rc_fuzzy_eng_->addOutputVariable(p_ov);

		p_rules = new ::fl::RuleBlock();
		p_rules->setEnabled(true);
		p_rules->setConjunction(new ::fl::Minimum());
		p_rules->setDisjunction(new ::fl::Maximum());
		p_rules->setImplication(new ::fl::Minimum());
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NL and " + de_fuzzy_var_name + " is NL then " + du_fuzzy_var_name + " is PL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NL and " + de_fuzzy_var_name + " is NM then " + du_fuzzy_var_name + " is PL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NL and " + de_fuzzy_var_name + " is NS then " + du_fuzzy_var_name + " is PL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NL and " + de_fuzzy_var_name + " is ZE then " + du_fuzzy_var_name + " is PL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NL and " + de_fuzzy_var_name + " is PS then " + du_fuzzy_var_name + " is PM", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NL and " + de_fuzzy_var_name + " is PM then " + du_fuzzy_var_name + " is PS", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NL and " + de_fuzzy_var_name + " is PL then " + du_fuzzy_var_name + " is ZE", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NM and " + de_fuzzy_var_name + " is NL then " + du_fuzzy_var_name + " is PL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NM and " + de_fuzzy_var_name + " is NM then " + du_fuzzy_var_name + " is PL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NM and " + de_fuzzy_var_name + " is NS then " + du_fuzzy_var_name + " is PL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NM and " + de_fuzzy_var_name + " is ZE then " + du_fuzzy_var_name + " is PM", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NM and " + de_fuzzy_var_name + " is PS then " + du_fuzzy_var_name + " is PS", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NM and " + de_fuzzy_var_name + " is PM then " + du_fuzzy_var_name + " is ZE", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NM and " + de_fuzzy_var_name + " is PL then " + du_fuzzy_var_name + " is NS", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NS and " + de_fuzzy_var_name + " is NL then " + du_fuzzy_var_name + " is PL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NS and " + de_fuzzy_var_name + " is NM then " + du_fuzzy_var_name + " is PL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NS and " + de_fuzzy_var_name + " is NS then " + du_fuzzy_var_name + " is PM", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NS and " + de_fuzzy_var_name + " is ZE then " + du_fuzzy_var_name + " is PS", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NS and " + de_fuzzy_var_name + " is PS then " + du_fuzzy_var_name + " is ZE", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NS and " + de_fuzzy_var_name + " is PM then " + du_fuzzy_var_name + " is NS", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NS and " + de_fuzzy_var_name + " is PL then " + du_fuzzy_var_name + " is NM", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is ZE and " + de_fuzzy_var_name + " is NL then " + du_fuzzy_var_name + " is PL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is ZE and " + de_fuzzy_var_name + " is NM then " + du_fuzzy_var_name + " is PM", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is ZE and " + de_fuzzy_var_name + " is NS then " + du_fuzzy_var_name + " is PS", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is ZE and " + de_fuzzy_var_name + " is ZE then " + du_fuzzy_var_name + " is ZE", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is ZE and " + de_fuzzy_var_name + " is PS then " + du_fuzzy_var_name + " is NS", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is ZE and " + de_fuzzy_var_name + " is PM then " + du_fuzzy_var_name + " is NM", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is ZE and " + de_fuzzy_var_name + " is PL then " + du_fuzzy_var_name + " is NL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PS and " + de_fuzzy_var_name + " is NL then " + du_fuzzy_var_name + " is PM", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PS and " + de_fuzzy_var_name + " is NM then " + du_fuzzy_var_name + " is PS", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PS and " + de_fuzzy_var_name + " is NS then " + du_fuzzy_var_name + " is ZE", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PS and " + de_fuzzy_var_name + " is ZE then " + du_fuzzy_var_name + " is NS", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PS and " + de_fuzzy_var_name + " is PS then " + du_fuzzy_var_name + " is NM", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PS and " + de_fuzzy_var_name + " is PM then " + du_fuzzy_var_name + " is NL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PS and " + de_fuzzy_var_name + " is PL then " + du_fuzzy_var_name + " is NL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PM and " + de_fuzzy_var_name + " is NL then " + du_fuzzy_var_name + " is PS", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PM and " + de_fuzzy_var_name + " is NM then " + du_fuzzy_var_name + " is ZE", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PM and " + de_fuzzy_var_name + " is NS then " + du_fuzzy_var_name + " is NS", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PM and " + de_fuzzy_var_name + " is ZE then " + du_fuzzy_var_name + " is NM", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PM and " + de_fuzzy_var_name + " is PS then " + du_fuzzy_var_name + " is NL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PM and " + de_fuzzy_var_name + " is PM then " + du_fuzzy_var_name + " is NL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PM and " + de_fuzzy_var_name + " is PL then " + du_fuzzy_var_name + " is NL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PL and " + de_fuzzy_var_name + " is NL then " + du_fuzzy_var_name + " is ZE", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PL and " + de_fuzzy_var_name + " is NM then " + du_fuzzy_var_name + " is NS", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PL and " + de_fuzzy_var_name + " is NS then " + du_fuzzy_var_name + " is NM", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PL and " + de_fuzzy_var_name + " is ZE then " + du_fuzzy_var_name + " is NL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PL and " + de_fuzzy_var_name + " is PS then " + du_fuzzy_var_name + " is NL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PL and " + de_fuzzy_var_name + " is PM then " + du_fuzzy_var_name + " is NL", p_rc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PL and " + de_fuzzy_var_name + " is PL then " + du_fuzzy_var_name + " is NL", p_rc_fuzzy_eng_.get()));
		p_rc_fuzzy_eng_->addRuleBlock(p_rules);

		// Setup the Scaling Factor Controller
		//   Membership functions taken from (Wei et al.,2006) [2]

		for (::std::size_t i = 0; i < 2; ++i)
		{
			p_iv = new ::fl::InputVariable();
			p_iv->setEnabled(true);
			switch (i)
			{
				case 0: // \Delta e(k)
					p_iv->setName(de_fuzzy_var_name);
					break;
				case 1: // e(k)
					p_iv->setName(e_fuzzy_var_name);
					break;
			}
			p_iv->setRange(-1, 1);
			p_iv->addTerm(new ::fl::Ramp("NL", -two_third, -1));
			p_iv->addTerm(new ::fl::Triangle("NM", -1, -two_third, -one_third));
			p_iv->addTerm(new ::fl::Triangle("NS", -two_third, -one_third, 0));
			p_iv->addTerm(new ::fl::Triangle("ZE", -one_third, 0, one_third));
			p_iv->addTerm(new ::fl::Triangle("PS", 0, one_third, two_third));
			p_iv->addTerm(new ::fl::Triangle("PM", one_third, two_third, 1));
			p_iv->addTerm(new ::fl::Ramp("PL", two_third, 1));
			p_sfc_fuzzy_eng_->addInputVariable(p_iv);
		}

		p_ov = new ::fl::OutputVariable();
		p_ov->setEnabled(true);
		p_ov->setName(alpha_fuzzy_var_name);
		p_ov->setRange(0, 1);
#if defined(FL_VERSION) // Until fuzzylite v. 5.x (the FL_VERSION macro was removed since fuzzylite 6.x)
		p_ov->fuzzyOutput()->setAccumulation(new ::fl::Maximum());
#else // Since fuzzylite v. 6.x
		p_ov->fuzzyOutput()->setAggregation(new ::fl::Maximum());
#endif // FL_VERSION
		p_ov->setDefuzzifier(new ::fl::Centroid());
		p_ov->setDefaultValue(::fl::nan);
		p_ov->setLockPreviousValue(false);
		p_ov->addTerm(new ::fl::Ramp("ZE", one_sixth, 0));
		p_ov->addTerm(new ::fl::Triangle("VS", 0, one_sixth, one_third));
		p_ov->addTerm(new ::fl::Triangle("SM", one_sixth, one_third, 0.5));
		p_ov->addTerm(new ::fl::Triangle("SL", one_third, 0.5, two_third));
		p_ov->addTerm(new ::fl::Triangle("ML", 0.5, two_third, five_sixth));
		p_ov->addTerm(new ::fl::Triangle("LG", two_third, five_sixth, 1));
		p_ov->addTerm(new ::fl::Ramp("VL", five_sixth, 1));
		p_sfc_fuzzy_eng_->addOutputVariable(p_ov);

		p_rules = new ::fl::RuleBlock();
		p_rules->setEnabled(true);
		p_rules->setConjunction(new ::fl::Minimum());
		p_rules->setDisjunction(new ::fl::Maximum());
		p_rules->setImplication(new ::fl::Minimum());
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NL and " + de_fuzzy_var_name + " is NL then " + alpha_fuzzy_var_name + " is VL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NL and " + de_fuzzy_var_name + " is NM then " + alpha_fuzzy_var_name + " is VL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NL and " + de_fuzzy_var_name + " is NS then " + alpha_fuzzy_var_name + " is VL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NL and " + de_fuzzy_var_name + " is ZE then " + alpha_fuzzy_var_name + " is SM", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NL and " + de_fuzzy_var_name + " is PS then " + alpha_fuzzy_var_name + " is VS", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NL and " + de_fuzzy_var_name + " is PM then " + alpha_fuzzy_var_name + " is VS", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NL and " + de_fuzzy_var_name + " is PL then " + alpha_fuzzy_var_name + " is ZE", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NM and " + de_fuzzy_var_name + " is NL then " + alpha_fuzzy_var_name + " is VL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NM and " + de_fuzzy_var_name + " is NM then " + alpha_fuzzy_var_name + " is VL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NM and " + de_fuzzy_var_name + " is NS then " + alpha_fuzzy_var_name + " is LG", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NM and " + de_fuzzy_var_name + " is ZE then " + alpha_fuzzy_var_name + " is SL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NM and " + de_fuzzy_var_name + " is PS then " + alpha_fuzzy_var_name + " is SM", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NM and " + de_fuzzy_var_name + " is PM then " + alpha_fuzzy_var_name + " is SM", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NM and " + de_fuzzy_var_name + " is PL then " + alpha_fuzzy_var_name + " is SM", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NS and " + de_fuzzy_var_name + " is NL then " + alpha_fuzzy_var_name + " is VL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NS and " + de_fuzzy_var_name + " is NM then " + alpha_fuzzy_var_name + " is VL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NS and " + de_fuzzy_var_name + " is NS then " + alpha_fuzzy_var_name + " is LG", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NS and " + de_fuzzy_var_name + " is ZE then " + alpha_fuzzy_var_name + " is ML", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NS and " + de_fuzzy_var_name + " is PS then " + alpha_fuzzy_var_name + " is VS", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NS and " + de_fuzzy_var_name + " is PM then " + alpha_fuzzy_var_name + " is SM", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is NS and " + de_fuzzy_var_name + " is PL then " + alpha_fuzzy_var_name + " is SL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is ZE and " + de_fuzzy_var_name + " is NL then " + alpha_fuzzy_var_name + " is LG", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is ZE and " + de_fuzzy_var_name + " is NM then " + alpha_fuzzy_var_name + " is ML", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is ZE and " + de_fuzzy_var_name + " is NS then " + alpha_fuzzy_var_name + " is SL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is ZE and " + de_fuzzy_var_name + " is ZE then " + alpha_fuzzy_var_name + " is ZE", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is ZE and " + de_fuzzy_var_name + " is PS then " + alpha_fuzzy_var_name + " is SL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is ZE and " + de_fuzzy_var_name + " is PM then " + alpha_fuzzy_var_name + " is ML", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is ZE and " + de_fuzzy_var_name + " is PL then " + alpha_fuzzy_var_name + " is LG", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PS and " + de_fuzzy_var_name + " is NL then " + alpha_fuzzy_var_name + " is SL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PS and " + de_fuzzy_var_name + " is NM then " + alpha_fuzzy_var_name + " is SM", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PS and " + de_fuzzy_var_name + " is NS then " + alpha_fuzzy_var_name + " is VS", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PS and " + de_fuzzy_var_name + " is ZE then " + alpha_fuzzy_var_name + " is ML", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PS and " + de_fuzzy_var_name + " is PS then " + alpha_fuzzy_var_name + " is LG", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PS and " + de_fuzzy_var_name + " is PM then " + alpha_fuzzy_var_name + " is LG", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PS and " + de_fuzzy_var_name + " is PL then " + alpha_fuzzy_var_name + " is VL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PM and " + de_fuzzy_var_name + " is NL then " + alpha_fuzzy_var_name + " is SM", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PM and " + de_fuzzy_var_name + " is NM then " + alpha_fuzzy_var_name + " is SM", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PM and " + de_fuzzy_var_name + " is NS then " + alpha_fuzzy_var_name + " is SM", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PM and " + de_fuzzy_var_name + " is ZE then " + alpha_fuzzy_var_name + " is SL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PM and " + de_fuzzy_var_name + " is PS then " + alpha_fuzzy_var_name + " is LG", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PM and " + de_fuzzy_var_name + " is PM then " + alpha_fuzzy_var_name + " is VL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PM and " + de_fuzzy_var_name + " is PL then " + alpha_fuzzy_var_name + " is VL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PL and " + de_fuzzy_var_name + " is NL then " + alpha_fuzzy_var_name + " is ZE", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PL and " + de_fuzzy_var_name + " is NM then " + alpha_fuzzy_var_name + " is VS", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PL and " + de_fuzzy_var_name + " is NS then " + alpha_fuzzy_var_name + " is VS", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PL and " + de_fuzzy_var_name + " is ZE then " + alpha_fuzzy_var_name + " is SM", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PL and " + de_fuzzy_var_name + " is PS then " + alpha_fuzzy_var_name + " is VL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PL and " + de_fuzzy_var_name + " is PM then " + alpha_fuzzy_var_name + " is VL", p_sfc_fuzzy_eng_.get()));
		p_rules->addRule(::fl::Rule::parse("if " + e_fuzzy_var_name + " is PL and " + de_fuzzy_var_name + " is PL then " + alpha_fuzzy_var_name + " is VL", p_sfc_fuzzy_eng_.get()));
		p_sfc_fuzzy_eng_->addRuleBlock(p_rules);
	}

	private: void do_reset()
	{
		typedef typename base_type::target_value_map::const_iterator target_iterator;
		typedef typename app_type::vm_pointer vm_pointer;

		const ::std::vector<vm_pointer> vms = this->app().vms();
		const ::std::size_t nvms = this->app().num_vms();

		// Reset output sensors
		app_sensors_.clear();
		const target_iterator tgt_end_it = this->target_values().end();
		for (target_iterator tgt_it = this->target_values().begin();
			 tgt_it != tgt_end_it;
			 ++tgt_it)
		{
			const application_performance_category cat(tgt_it->first);

			app_sensors_[cat] = this->app().sensor(cat);
			es_[cat] = 0;
		}

		// Reset input sensors
		vm_sensors_.clear();
		for (::std::size_t i = 0; i < nvms; ++i)
		{
			const virtual_machine_performance_category cat = cpu_util_virtual_machine_performance;
			vm_pointer p_vm = vms[i];

			vm_sensors_[cat][p_vm->id()] = p_vm->sensor(cat);
		}

		// Reset counters
		ctl_count_ = ctl_skip_count_
				   = ctl_fail_count_
				   = 0;

		// Reset fuzzy controllers
		p_rc_fuzzy_eng_->restart();
		p_sfc_fuzzy_eng_->restart();

		// Reset input scaling factors
		Ke_ = Kde_ = 0;

		// Reset VCPU util estimator and smoother
		for (::std::size_t i = 0; i < nvms; ++i)
		{
			//this->data_estimator(cpu_util_virtual_machine_performance, vms[i]->id(), ::boost::make_shared< testbed::mean_estimator<real_type> >());
			this->data_smoother(cpu_util_virtual_machine_performance, vms[i]->id(), ::boost::make_shared< testbed::brown_single_exponential_smoother<real_type> >(0.9));
			//this->data_smoother(cpu_util_virtual_machine_performance, vms[i]->id(), ::boost::make_shared< testbed::holt_winters_double_exponential_smoother<real_type> >(beta_));
		}

		// Reset output data file and write header
		if (p_dat_ofs_ && p_dat_ofs_->is_open())
		{
			p_dat_ofs_->close();
		}
		p_dat_ofs_.reset();
		if (!dat_fname_.empty())
		{
			p_dat_ofs_ = ::boost::make_shared< ::std::ofstream >(dat_fname_.c_str());
			if (!p_dat_ofs_->good())
			{
				::std::ostringstream oss;
				oss << "Cannot open output data file '" << dat_fname_ << "'";

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

			*p_dat_ofs_ << "\"ts\"";

			const ::std::size_t nvms = this->app().num_vms();
			for (::std::size_t i = 0; i < nvms; ++i)
			{
				*p_dat_ofs_ << ",\"Cap_{" << vms[i]->id() << "}\",\"Share_{" << vms[i]->id() << "}\",\"Util_{" << vms[i]->id() << "}\"";
			}
			for (target_iterator tgt_it = this->target_values().begin();
				 tgt_it != tgt_end_it;
				 ++tgt_it)
			{
				const application_performance_category cat = tgt_it->first;

				*p_dat_ofs_ << ",\"y_{" << cat << "}\",\"r_{" << cat << "}\"";
			}
			*p_dat_ofs_ << ",\"alpha\",\"Delta u\",\"K_e\",\"K_{Delta e}\",\"# Controls\",\"# Skip Controls\",\"# Fail Controls\"";
			*p_dat_ofs_ << ::std::endl;
		}
	}

	private: void do_sample()
	{
		typedef typename vm_sensor_map::const_iterator vm_sensor_iterator;
		typedef typename app_sensor_map::const_iterator app_sensor_iterator;
		typedef ::std::vector<typename sensor_type::observation_type> obs_container;
		typedef typename obs_container::const_iterator obs_iterator;

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

		// Collect VM values
		const vm_sensor_iterator vm_sens_end_it = vm_sensors_.end();
		for (vm_sensor_iterator vm_sens_it = vm_sensors_.begin();
			 vm_sens_it != vm_sens_end_it;
			 ++vm_sens_it)
		{
			const virtual_machine_performance_category cat = vm_sens_it->first;

			const typename vm_sensor_map::mapped_type::const_iterator vm_end_it = vm_sens_it->second.end();
			for (typename vm_sensor_map::mapped_type::const_iterator vm_it = vm_sens_it->second.begin();
				 vm_it != vm_end_it;
				 ++vm_it)
			{
				const vm_identifier_type vm_id = vm_it->first;
				sensor_pointer p_sens = vm_it->second;

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

				p_sens->sense();
				if (p_sens->has_observations())
				{
					const obs_container obs = p_sens->observations();
					const obs_iterator end_it = obs.end();
					for (obs_iterator it = obs.begin();
						 it != end_it;
						 ++it)
					{
						//this->data_estimator(cat, vm_id).collect(it->value());
						this->data_smoother(cat, vm_id).smooth(it->value());
					}
				}
			}
		}

		// Collect App values
		const app_sensor_iterator app_sens_end_it = app_sensors_.end();
		for (app_sensor_iterator app_sens_it = app_sensors_.begin();
			 app_sens_it != app_sens_end_it;
			 ++app_sens_it)
		{
			const application_performance_category cat(app_sens_it->first);

			sensor_pointer p_sens(app_sens_it->second);

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

			p_sens->sense();
			if (p_sens->has_observations())
			{
				const obs_container obs = p_sens->observations();
				const obs_iterator end_it = obs.end();
				for (obs_iterator it = obs.begin();
					 it != end_it;
					 ++it)
				{
					this->data_estimator(cat).collect(it->value());
				}
			}
		}

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

	private: void do_control()
	{
		typedef typename base_type::target_value_map::const_iterator target_iterator;
		typedef typename app_type::vm_pointer vm_pointer;

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

		++ctl_count_;

		bool skip_ctl = false;

		::std::vector<vm_pointer> vms = this->app().vms();
		const ::std::size_t nvms = vms.size();

		//std::map<application_performance_category,real_type> es_;
		std::map<application_performance_category,real_type> des;

		const target_iterator tgt_end_it = this->target_values().end();
		for (target_iterator tgt_it = this->target_values().begin();
			 tgt_it != tgt_end_it;
			 ++tgt_it)
		{
			const application_performance_category cat(tgt_it->first);

			// Compute a summary statistics of collected observation
			if (this->data_estimator(cat).count() > 0)
			{
				const real_type y = this->data_estimator(cat).estimate();
				const real_type r = this->target_value(cat);

				real_type e = 0;
				switch (cat)
				{
					case response_time_application_performance:
						if (dcs::math::float_traits<real_type>::approximately_less_equal(y, 2.0*r))
						{
							e = (r-y)/r;
						}
						else
						{
							e = -1;
						}
						break;
					case throughput_application_performance:
						if (dcs::math::float_traits<real_type>::approximately_greater_equal(y, 0.5*r))
						{
							e = (y-r)/r;
						}
						else
						{
							e = -1;
						}
						break;
				}
				if (es_.count(cat) > 0)
				{
					des[cat] = e-es_.at(cat);
				}
				else
				{
					des[cat] = e;
				}
				es_[cat] = e;
DCS_DEBUG_TRACE("APP Performance Category: " << cat << " - Y(k): " << y << " - R: " << r << " -> E(k+1): " << es_.at(cat) << " - DeltaE(k+1): " << des.at(cat));//XXX
			}
			else
			{
				// No observation collected during the last control interval
				DCS_DEBUG_TRACE("No output observation collected during the last control interval -> Skip control");
				skip_ctl = true;
				break;
			}
#ifdef DCSXX_TESTBED_EXP_APP_MGR_RESET_ESTIMATION_EVERY_INTERVAL
			this->data_estimator(cat).reset();
#endif // DCSXX_TESTBED_EXP_APP_MGR_RESET_ESTIMATION_EVERY_INTERVAL
		}

        if (!skip_ctl)
        {
			//FIXME: actually we only handle SISO systems
			DCS_ASSERT(es_.size() == 1,
					   DCS_EXCEPTION_THROW(::std::runtime_error,
					   "Only SISO system are currently managed"));

			// Compute the input to the RC fuzzy controller
			const real_type Ke = (ctl_count_ > 1) ? ::std::abs(Ke_) : 1.0;
			const real_type Kde = (ctl_count_ > 1) ? ::std::abs(Kde_) : 1.0;
			const real_type e = Ke*es_.begin()->second;
			const real_type de = Kde*des.begin()->second;

			// Update input scaling factors
			Ke_ = (1-gamma_)*Ke_ + gamma_*es_.begin()->second;
			Kde_ = (1-gamma_)*Kde_ - gamma_*des.begin()->second;

			// Perform fuzzy control
			bool ok = false;
			real_type du = 0;
			real_type alpha = 0;

			try
			{
				// Compute \Delta u(k)
				p_rc_fuzzy_eng_->setInputValue(e_fuzzy_var_name, e);
				p_rc_fuzzy_eng_->setInputValue(de_fuzzy_var_name, de);
				p_rc_fuzzy_eng_->process();
				du = p_rc_fuzzy_eng_->getOutputValue(du_fuzzy_var_name);

				// Compute the \alpha(k) scaling factor
				p_sfc_fuzzy_eng_->setInputValue(e_fuzzy_var_name, e);
				p_sfc_fuzzy_eng_->setInputValue(de_fuzzy_var_name, de);
				p_sfc_fuzzy_eng_->process();
				alpha = p_sfc_fuzzy_eng_->getOutputValue(alpha_fuzzy_var_name);

				ok = true;
			}
			catch (::fl::Exception const& fe)
			{
				DCS_DEBUG_TRACE( "Caught exception: " << fe.what() );

				::std::ostringstream oss;
				oss << "Unable to compute optimal control: " << fe.what();
				::dcs::log_warn(DCS_LOGGING_AT, oss.str());
			}
			catch (::std::exception const& se)
			{
				DCS_DEBUG_TRACE( "Caught exception: " << se.what() );

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

			// Apply fuzzy control results
			if (ok)
			{
				for (::std::size_t i = 0; i < nvms; ++i)
				{
					vm_pointer p_vm = vms[i];

					const real_type c = p_vm->cpu_share();

					// Compute the output amplifier
					const real_type Kdu = c*0.5*::std::abs(e);

					const real_type u = ::std::max(::std::min(c+alpha*Kdu*du, 1.0), 0.0);

					DCS_DEBUG_TRACE("VM '" << p_vm->id() << "' - old-share: " << c << " - new-share: " << u);

					if (::std::isfinite(u) && !::dcs::math::float_traits<real_type>::essentially_equal(c, u))
					{
						p_vm->cpu_share(u);
DCS_DEBUG_TRACE("VM " << vms[i]->id() << ", Alpha: " << alpha << ", DeltaU: " << du << ", K_{DeltaU}: " << Kdu << " -> U(k+1): " << u);//XXX
					}
				}
DCS_DEBUG_TRACE("Optimal control applied");//XXX
			}
			else
			{
				++ctl_fail_count_;

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

		// Export to file
		if (p_dat_ofs_)
		{
			*p_dat_ofs_ << ::std::time(0) << ",";
			for (::std::size_t i = 0; i < nvms; ++i)
			{
				const vm_pointer p_vm = vms[i];

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

				if (i != 0)
				{
					*p_dat_ofs_ << ",";
				}
				*p_dat_ofs_ << p_vm->cpu_cap() << "," << p_vm->cpu_share() << "," << this->data_smoother(cpu_util_virtual_machine_performance, p_vm->id()).forecast(0);
			}
			*p_dat_ofs_ << ",";
			const target_iterator tgt_end_it = this->target_values().end();
			for (target_iterator tgt_it = this->target_values().begin();
				 tgt_it != tgt_end_it;
				 ++tgt_it)
			{
				const application_performance_category cat = tgt_it->first;

				if (tgt_it != this->target_values().begin())
				{
					*p_dat_ofs_ << ",";
				}
				const real_type y = this->data_estimator(cat).estimate();
				const real_type r = tgt_it->second;
				*p_dat_ofs_ << y << "," << r;
			}
			if (skip_ctl)
			{
				const real_type nan = ::std::numeric_limits<real_type>::quiet_NaN();

				*p_dat_ofs_ << "," << nan << "," << nan;
			}
			else
			{
				const real_type alpha = p_sfc_fuzzy_eng_->getOutputValue(alpha_fuzzy_var_name);
				const real_type du = p_rc_fuzzy_eng_->getOutputValue(du_fuzzy_var_name);

				*p_dat_ofs_ << "," << alpha << "," << du;
			}
			*p_dat_ofs_ << "," << Ke_ << "," << Kde_;
			*p_dat_ofs_ << "," << ctl_count_ << "," << ctl_skip_count_ << "," << ctl_fail_count_;
			*p_dat_ofs_ << ::std::endl;
		}

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

	private: real_type gamma_; ///< The EWMA smoothing factor for Cres
	private: real_type Ke_;
	private: real_type Kde_;
	private: ::boost::shared_ptr< ::fl::Engine > p_rc_fuzzy_eng_; ///< The fuzzy resource control engine
	private: ::boost::shared_ptr< ::fl::Engine > p_sfc_fuzzy_eng_; ///< The fuzzy scaling factor control engine
	private: ::std::map<application_performance_category,real_type> es_; ///< The e(k) variable, grouped by application performance category
	private: ::std::size_t ctl_count_; ///< Number of times control function has been invoked
	private: ::std::size_t ctl_skip_count_; ///< Number of times control has been skipped
	private: ::std::size_t ctl_fail_count_; ///< Number of times control has failed
	private: vm_sensor_map vm_sensors_;
	private: app_sensor_map app_sensors_;
	private: ::std::string dat_fname_;
	private: ::boost::shared_ptr< ::std::ofstream > p_dat_ofs_;
}; // rao2013_dynaqos_application_manager

template <typename T>
const ::std::string rao2013_dynaqos_application_manager<T>::alpha_fuzzy_var_name = "alpha";

template <typename T>
const ::std::string rao2013_dynaqos_application_manager<T>::de_fuzzy_var_name = "DeltaE";

template <typename T>
const ::std::string rao2013_dynaqos_application_manager<T>::du_fuzzy_var_name = "DeltaU";

template <typename T>
const ::std::string rao2013_dynaqos_application_manager<T>::e_fuzzy_var_name = "E";

}} // Namespace dcs::testbed

#endif // DCS_TESTBED_RAO2013_DYNAQOS_APPLICATION_MANAGER_HPP


#include <boost/geometry/strategies/geographic/intersection_elliptic.hpp>


//
// Copyright (c) 2015-2019 CNRS INRIA
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#ifndef __pinocchio_joint_free_flyer_hpp__
#define __pinocchio_joint_free_flyer_hpp__

#include "pinocchio/macros.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/spatial/explog.hpp"
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/constraint.hpp"
#include "pinocchio/math/fwd.hpp"
#include "pinocchio/math/quaternion.hpp"

namespace pinocchio
{

  template<typename Scalar, int Options> struct ConstraintIdentityTpl;

  template<typename _Scalar, int _Options>
  struct traits< ConstraintIdentityTpl<_Scalar,_Options> >
  {
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
    enum {
      LINEAR = 0,
      ANGULAR = 3
    };
    typedef MotionTpl<Scalar,Options> JointMotion;
    typedef Eigen::Matrix<Scalar,6,1,Options> JointForce;
    typedef Eigen::Matrix<Scalar,6,6,Options> DenseBase;
    typedef typename Matrix6::IdentityReturnType ConstMatrixReturnType;
    typedef typename Matrix6::IdentityReturnType MatrixReturnType;
  }; // traits ConstraintRevolute

  template<typename _Scalar, int _Options>
  struct ConstraintIdentityTpl
  : ConstraintBase< ConstraintIdentityTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintIdentityTpl)

    enum { NV = 6 };

    template<typename Vector6Like>
    JointMotion __mult__(const Eigen::MatrixBase<Vector6Like> & vj) const
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like,6);
      return JointMotion(vj);
    }

    template<typename S1, int O1>
    typename SE3Tpl<S1,O1>::ActionMatrixType
    se3Action(const SE3Tpl<S1,O1> & m) const
    {
      return m.toActionMatrix();

    }

    template<typename S1, int O1>
    typename SE3Tpl<S1,O1>::ActionMatrixType
    se3ActionInverse(const SE3Tpl<S1,O1> & m) const
    {
      return m.toActionMatrixInverse();
    }

    int nv_impl() const { return NV; }

    struct TransposeConst
    {
      template<typename Derived>
      typename ForceDense<Derived>::ToVectorConstReturnType
      operator*(const ForceDense<Derived> & phi)
      {  return phi.toVector();  }

      /* [CRBA]  MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
      template<typename MatrixDerived>
      typename PINOCCHIO_EIGEN_REF_CONST_TYPE(MatrixDerived)
      operator*(const Eigen::MatrixBase<MatrixDerived> & F)
      {
        return F.derived();
      }
    };

    TransposeConst transpose() const { return TransposeConst(); }
    MatrixReturnType matrix_impl() const { return DenseBase::Identity(); }

    template<typename MotionDerived>
    typename MotionDerived::ActionMatrixType
    motionAction(const MotionBase<MotionDerived> & v) const
    { return v.toActionMatrix(); }

    bool isEqual(const ConstraintIdentityTpl &) const { return true; }

  }; // struct ConstraintIdentityTpl

  template<typename Scalar, int Options, typename Vector6Like>
  MotionRef<const Vector6Like>
  operator*(const ConstraintIdentityTpl<Scalar,Options> &,
            const Eigen::MatrixBase<Vector6Like> & v)
  {
    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like,6);
//    typedef typename ConstraintIdentityTpl<Scalar,Options>::Motion Motion;
    typedef MotionRef<const Vector6Like> Motion;
    return Motion(v.derived());
  }

  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
  template<typename S1, int O1, typename S2, int O2>
  inline typename InertiaTpl<S1,O1>::Matrix6
  operator*(const InertiaTpl<S1,O1> & Y, const ConstraintIdentityTpl<S2,O2> &)
  {
    return Y.matrix();
  }

  /* [ABA] Y*S operator*/
  template<typename Matrix6Like, typename S2, int O2>
  inline typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like)
  operator*(const Eigen::MatrixBase<Matrix6Like> & Y, const ConstraintIdentityTpl<S2,O2> &)
  {
    return Y.derived();
  }

  template<typename S1, int O1>
  struct SE3GroupAction< ConstraintIdentityTpl<S1,O1> >
  { typedef typename SE3Tpl<S1,O1>::ActionMatrixType ReturnType; };

  template<typename S1, int O1, typename MotionDerived>
  struct MotionAlgebraAction< ConstraintIdentityTpl<S1,O1>,MotionDerived >
  { typedef typename SE3Tpl<S1,O1>::ActionMatrixType ReturnType; };

  template<typename Scalar, int Options> struct JointFreeFlyerTpl;

  template<typename _Scalar, int _Options>
  struct traits< JointFreeFlyerTpl<_Scalar,_Options> >
  {
    enum {
      NQ = 7,
      NV = 6
    };
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef JointDataFreeFlyerTpl<Scalar,Options> JointDataDerived;
    typedef JointModelFreeFlyerTpl<Scalar,Options> JointModelDerived;
    typedef ConstraintIdentityTpl<Scalar,Options> Constraint_t;
    typedef SE3Tpl<Scalar,Options> Transformation_t;
    typedef MotionTpl<Scalar,Options> Motion_t;
    typedef MotionZeroTpl<Scalar,Options> Bias_t;

    // [ABA]
    typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
    typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
    typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;

    PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE

    typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
    typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
  };

  template<typename Scalar, int Options>
  struct traits< JointDataFreeFlyerTpl<Scalar,Options> >
  { typedef JointFreeFlyerTpl<Scalar,Options> JointDerived; };

  template<typename Scalar, int Options>
  struct traits< JointModelFreeFlyerTpl<Scalar,Options> >
  { typedef JointFreeFlyerTpl<Scalar,Options> JointDerived; };

  template<typename _Scalar, int _Options>
  struct JointDataFreeFlyerTpl : public JointDataBase< JointDataFreeFlyerTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef JointFreeFlyerTpl<_Scalar,_Options> JointDerived;
    PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
    PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR

    Constraint_t S;
    Transformation_t M;
    Motion_t v;
    Bias_t c;

    // [ABA] specific data
    U_t U;
    D_t Dinv;
    UD_t UDinv;

    JointDataFreeFlyerTpl()
    : M(Transformation_t::Identity())
    , v(Motion_t::Zero())
    , U(U_t::Zero())
    , Dinv(D_t::Zero())
    , UDinv(UD_t::Identity())
    {}

    static std::string classname() { return std::string("JointDataFreeFlyer"); }
    std::string shortname() const { return classname(); }

  }; // struct JointDataFreeFlyerTpl

  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelFreeFlyerTpl);
  template<typename _Scalar, int _Options>
  struct JointModelFreeFlyerTpl
  : public JointModelBase< JointModelFreeFlyerTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef JointFreeFlyerTpl<_Scalar,_Options> JointDerived;
    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);

    typedef JointModelBase<JointModelFreeFlyerTpl> Base;
    using Base::id;
    using Base::idx_q;
    using Base::idx_v;
    using Base::setIndexes;

    JointDataDerived createData() const { return JointDataDerived(); }

    template<typename ConfigVectorLike>
    inline void forwardKinematics(Transformation_t & M, const Eigen::MatrixBase<ConfigVectorLike> & q_joint) const
    {
      EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,ConfigVectorLike);
      typedef typename Eigen::Quaternion<typename ConfigVectorLike::Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options> Quaternion;
      typedef Eigen::Map<const Quaternion> ConstQuaternionMap;

      ConstQuaternionMap quat(q_joint.template tail<4>().data());
      //assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon())); TODO: check validity of the rhs precision
      assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= 1e-4);

      M.rotation(quat.matrix());
      M.translation(q_joint.template head<3>());
    }

    template<typename Vector3Derived, typename QuaternionDerived>
    EIGEN_DONT_INLINE
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<Vector3Derived> & trans,
              const typename Eigen::QuaternionBase<QuaternionDerived> & quat) const
    {
      data.M.translation(trans);
      data.M.rotation(quat.matrix());
    }

    template<typename ConfigVector>
    EIGEN_DONT_INLINE
    void calc(JointDataDerived & data,
              const typename Eigen::PlainObjectBase<ConfigVector> & qs) const
    {
      typedef typename Eigen::Quaternion<typename ConfigVector::Scalar,ConfigVector::Options> Quaternion;
      typedef Eigen::Map<const Quaternion> ConstQuaternionMap;

      typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type q = qs.template segment<NQ>(idx_q());
      ConstQuaternionMap quat(q.template tail<4>().data());

      calc(data,q.template head<3>(),quat);
    }

    template<typename ConfigVector>
    EIGEN_DONT_INLINE
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs) const
    {
      typedef typename Eigen::Quaternion<Scalar,Options> Quaternion;

      typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type q = qs.template segment<NQ>(idx_q());
      const Quaternion quat(q.template tail<4>());

      calc(data,q.template head<3>(),quat);
    }

    template<typename ConfigVector, typename TangentVector>
    EIGEN_DONT_INLINE
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs,
              const typename Eigen::MatrixBase<TangentVector> & vs) const
    {
      calc(data,qs.derived());

      data.v = vs.template segment<NV>(idx_v());
    }

    template<typename Matrix6Like>
    void calc_aba(JointDataDerived & data,
                  const Eigen::MatrixBase<Matrix6Like> & I,
                  const bool update_I) const
    {
      data.U = I;

      // compute inverse
//      data.Dinv.setIdentity();
//      I.llt().solveInPlace(data.Dinv);
      internal::PerformStYSInversion<Scalar>::run(I,data.Dinv);

      if (update_I)
        PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I).setZero();
    }

    static std::string classname() { return std::string("JointModelFreeFlyer"); }
    std::string shortname() const { return classname(); }

    /// \returns An expression of *this with the Scalar type casted to NewScalar.
    template<typename NewScalar>
    JointModelFreeFlyerTpl<NewScalar,Options> cast() const
    {
      typedef JointModelFreeFlyerTpl<NewScalar,Options> ReturnType;
      ReturnType res;
      res.setIndexes(id(),idx_q(),idx_v());
      return res;
    }

  }; // struct JointModelFreeFlyerTpl

} // namespace pinocchio

#include <boost/type_traits.hpp>

namespace boost
{
  template<typename Scalar, int Options>
  struct has_nothrow_constructor< ::pinocchio::JointModelFreeFlyerTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options>
  struct has_nothrow_copy< ::pinocchio::JointModelFreeFlyerTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options>
  struct has_nothrow_constructor< ::pinocchio::JointDataFreeFlyerTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options>
  struct has_nothrow_copy< ::pinocchio::JointDataFreeFlyerTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};
}

#endif // ifndef __pinocchio_joint_free_flyer_hpp__


#include "AnimationObject.h"
#include "AnimationKeyframe.h"

#include "SceneRigidBodyConfig.h"

#include "ThreadingSafety.h"

#include "XmlReader.h"

#include <boost\property_tree\ptree_fwd.hpp>
#include <boost\property_tree\xml_parser.hpp>

namespace
{
	void lerp(const Storm::Vector3 &vectBefore, const Storm::Vector3 &vectAfter, const float coeff, Storm::Vector3 &result)
	{
		result = vectBefore + (vectAfter - vectBefore) * coeff;
	}

	void lerp(const Storm::Rotation &vectBefore, const Storm::Rotation &vectAfter, const float coeff, Storm::Rotation &result)
	{
		result.axis() = vectBefore.axis();  // TODO : Maybe a slerp. But I don't need it for now since axises are constants
		result.angle() = std::lerp(vectBefore.angle(), vectAfter.angle(), coeff);
	}

	// Called keyframe a and b on purpose. a goes to b, no telling in what chronological direction it is
	// (if a time is less than b time, we goes to the future, otherwise we'll go to the past).
	void lerpKeyframe(const Storm::AnimationKeyframe &a, const Storm::AnimationKeyframe &b, const float time, Storm::Vector3 &outPos, Storm::Rotation &outRot)
	{
		if (a._timeInSecond == b._timeInSecond)
		{
			outPos = a._position;
			outRot = a._rotation;
		}

		const float coeff = (time - a._timeInSecond) / (b._timeInSecond - a._timeInSecond);

		lerp(a._position, b._position, coeff, outPos);
		lerp(a._rotation, b._rotation, coeff, outRot);
	}
}

Storm::AnimationObject::AnimationObject(const Storm::SceneRigidBodyConfig &rbSceneConfig) :
	_isLooping{ false },
	_lastKeyIndex{ 0 }
{
	boost::property_tree::ptree rootXmlTree;
	{
		std::stringstream str;
		str << rbSceneConfig._animationXmlContent;
		boost::property_tree::read_xml(str, rootXmlTree);
	}

	const boost::property_tree::ptree &animationXmlTree = rootXmlTree.get_child("Animation");
	_keyframes.reserve(animationXmlTree.size());

	for (const auto &animationKeys : animationXmlTree)
	{
		if (_isLooping)
		{
			Storm::throwException<Storm::Exception>("We mustn't find any keys after a loop key.");
		}

		if (animationKeys.first == "Keyframe")
		{
			const boost::property_tree::ptree &keyframeElementXmlTree = animationKeys.second;

			Storm::AnimationKeyframe &keyframe = _keyframes.emplace_back();
			const std::size_t lastIndex = _keyframes.size() - 1;

			if (lastIndex > 0)
			{
				const Storm::AnimationKeyframe &keyframeBefore = _keyframes[lastIndex - 1];
				keyframe._position = keyframeBefore._position;
				keyframe._rotation = keyframeBefore._rotation;
			}
			else
			{
				keyframe._position = rbSceneConfig._translation;
				keyframe._rotation = rbSceneConfig._rotation;
			}

			Storm::XmlReader::sureReadXmlAttribute(keyframeElementXmlTree, keyframe._timeInSecond, "time");

			if (keyframe._timeInSecond < 0.f)
			{
				Storm::throwException<Storm::Exception>("Keyframe time shouldn't be negative! (" + std::to_string(keyframe._timeInSecond) + ")");
			}

			if (lastIndex > 0 && _keyframes[lastIndex - 1]._timeInSecond >= keyframe._timeInSecond)
			{
				Storm::throwException<Storm::Exception>("Disorder detected in the keyframe declaration order! Please, declare them in chronological order.");
			}

			Storm::XmlReader::readXmlAttribute(keyframeElementXmlTree, keyframe._position.x(), "posX");
			Storm::XmlReader::readXmlAttribute(keyframeElementXmlTree, keyframe._position.y(), "posY");
			Storm::XmlReader::readXmlAttribute(keyframeElementXmlTree, keyframe._position.z(), "posZ");

			Storm::XmlReader::readXmlAttribute(keyframeElementXmlTree, keyframe._rotation.angle(), "angle");
			Storm::XmlReader::readXmlAttribute(keyframeElementXmlTree, keyframe._rotation.axis().x(), "rotX");
			Storm::XmlReader::readXmlAttribute(keyframeElementXmlTree, keyframe._rotation.axis().y(), "rotY");
			Storm::XmlReader::readXmlAttribute(keyframeElementXmlTree, keyframe._rotation.axis().z(), "rotZ");
		}
		else if (animationKeys.first == "Motor")
		{
			const boost::property_tree::ptree &motorElementXmlTree = animationKeys.second;

			Storm::Vector3 initialRbPosition = _keyframes.empty() ? rbSceneConfig._translation : (_keyframes.back()._position);

			Storm::Vector3 pivotPoint = initialRbPosition;
			Storm::XmlReader::readXmlAttribute(motorElementXmlTree, pivotPoint.x(), "pivotX");
			Storm::XmlReader::readXmlAttribute(motorElementXmlTree, pivotPoint.y(), "pivotY");
			Storm::XmlReader::readXmlAttribute(motorElementXmlTree, pivotPoint.z(), "pivotZ");

			Storm::Rotation rotationAxis = Storm::Rotation::Identity();
			Storm::XmlReader::readXmlAttribute(motorElementXmlTree, rotationAxis.axis().x(), "rotX");
			Storm::XmlReader::readXmlAttribute(motorElementXmlTree, rotationAxis.axis().y(), "rotY");
			Storm::XmlReader::readXmlAttribute(motorElementXmlTree, rotationAxis.axis().z(), "rotZ");

			const float rotationAxisNorm = rotationAxis.axis().norm();
			if (rotationAxisNorm < 0.00001f)
			{
				Storm::throwException<Storm::Exception>("Rotation axis shouldn't be a null vector!");
			}

			rotationAxis.axis() /= rotationAxisNorm;
			rotationAxis.angle() = std::fmodf(rotationAxis.angle(), static_cast<float>(2.0 * M_PI));

			float rotateSpeed;
			Storm::XmlReader::sureReadXmlAttribute(motorElementXmlTree, rotateSpeed, "speed");
			if (rotateSpeed == 0.f)
			{
				Storm::throwException<Storm::Exception>("Motor should move!");
			}

			constexpr unsigned int angularTurnDivision = 180;
			const float angularTimeDivision = 1.f / (static_cast<float>(angularTurnDivision) * std::fabs(rotateSpeed));

			constexpr float angularStep = static_cast<float>(2.0 * M_PI / static_cast<double>(angularTurnDivision));
			constexpr float radToDegrees = static_cast<float>(180.0 / M_PI);

			Storm::Rotation rotationAxisDescription = rotationAxis;
			rotationAxisDescription.angle() = angularStep;

			const bool rotateClockwise = rotateSpeed > 0.f;

			const Storm::Quaternion rotationQuaternion = rotateClockwise ? Storm::Quaternion{ rotationAxisDescription } : Storm::Quaternion{ rotationAxisDescription }.inverse();
			const Storm::Quaternion conjugateRotationQuaternion = rotationQuaternion.conjugate();

			_keyframes.reserve(_keyframes.size() + angularTurnDivision + 1);

			{
				bool noKeyframe = _keyframes.empty();
				Storm::Rotation currentRotation = noKeyframe ? rotationAxis : (_keyframes.back()._rotation);

				Storm::AnimationKeyframe &keyframe = _keyframes.emplace_back();
				keyframe._timeInSecond = noKeyframe ? 0.f : (_keyframes.back()._timeInSecond + angularTimeDivision);
				keyframe._position = initialRbPosition;
				keyframe._rotation = currentRotation;
			}

			Storm::Vector3 relativePos = initialRbPosition - pivotPoint;

			for (unsigned int iter = 1; iter < angularTurnDivision; ++iter)
			{
				const Storm::AnimationKeyframe &lastKeyframe = _keyframes.back();

				const Storm::Quaternion lastPosAsPureQuat{ 0.f, relativePos.x(), relativePos.y(), relativePos.z() };

				const Storm::Quaternion newPos = rotationQuaternion * lastPosAsPureQuat * conjugateRotationQuaternion;
				relativePos = newPos.vec();

				Storm::AnimationKeyframe &keyframe = _keyframes.emplace_back();
				keyframe._timeInSecond = lastKeyframe._timeInSecond + angularTimeDivision;
				keyframe._position = relativePos + pivotPoint;
				keyframe._rotation.axis() = lastKeyframe._rotation.axis(); // TODO : Maybe a slerp. But I don't need it for now since axises are constants
				keyframe._rotation.angle() = lastKeyframe._rotation.angle() + rotationAxisDescription.angle();
			}

			_isLooping = true;
		}
		else if (animationKeys.first == "Loop")
		{
			_isLooping = true;
		}
	}

	if (_keyframes.empty())
	{
		Storm::throwException<Storm::Exception>("We must have at least one keyframe to animate!");
	}

	// The first keyframe should always be the one at t=0s
	if (_keyframes[0]._timeInSecond != 0.f)
	{
		Storm::AnimationKeyframe &keyframe = *_keyframes.emplace(std::begin(_keyframes));
		keyframe._timeInSecond = 0.f;
		keyframe._position = rbSceneConfig._translation;
		keyframe._rotation = rbSceneConfig._rotation;
	}

	_endAnimTime = _keyframes.back()._timeInSecond;
}

Storm::AnimationObject::~AnimationObject() = default;

bool Storm::AnimationObject::getCurrentFrameOrShouldBeDestroyed(float currentTime, Storm::Vector3 &outPos, Storm::Rotation &outRot) const
{
	assert(Storm::isAnimationThread() && "This method should only be called in animation thread!");

	const std::size_t lastKeyframeIndex = _keyframes.size() - 1;

	if (_isLooping)
	{
		while (currentTime > _endAnimTime)
		{
			currentTime -= _endAnimTime;
		}
		while (currentTime < 0)
		{
			currentTime += _endAnimTime;
		}
	}

	const Storm::AnimationKeyframe* lastAnimKeyframe = &_keyframes[_lastKeyIndex];
	if (lastAnimKeyframe->_timeInSecond < currentTime) // We go forward in time.
	{
		if (_lastKeyIndex == lastKeyframeIndex)
		{
			if (_isLooping)
			{
				_lastKeyIndex = 0;
			}
			else
			{
				// We're freed from the animation object.
				return false;
			}
		}

		do
		{
			const Storm::AnimationKeyframe &animKeyframeAfter = _keyframes[_lastKeyIndex + 1];
			if (animKeyframeAfter._timeInSecond < currentTime)
			{
				++_lastKeyIndex;

				if (_lastKeyIndex == lastKeyframeIndex)
				{
					if (_isLooping)
					{
						_lastKeyIndex = 0;
					}
					else
					{
						// We're freed from the animation object.
						return false;
					}
				}

				lastAnimKeyframe = &_keyframes[_lastKeyIndex];
			}
			else
			{
				if (animKeyframeAfter._timeInSecond == currentTime)
				{
					++_lastKeyIndex;
					outPos = animKeyframeAfter._position;
					outRot = animKeyframeAfter._rotation;
				}
				else
				{
					lerpKeyframe(*lastAnimKeyframe, animKeyframeAfter, currentTime, outPos, outRot);
				}

				break;
			}

		} while (true);
	}
	else if (lastAnimKeyframe->_timeInSecond == currentTime) // We are already at the current frame.
	{
		outPos = lastAnimKeyframe->_position;
		outRot = lastAnimKeyframe->_rotation;
	}
	else  // We go backward in time.
	{
		do
		{
			if (_lastKeyIndex == 0)
			{
				if (_isLooping)
				{
					_lastKeyIndex = lastKeyframeIndex;
				}
				else
				{
					Storm::throwException<Storm::Exception>(
						"We're going backward in time and since we don't loop, we have requested a negative time to come here.\n"
						"This is not allowed!\n"
						"Time value was " + std::to_string(currentTime)
					);
				}
			}

			const Storm::AnimationKeyframe &animKeyframeBefore = _keyframes[_lastKeyIndex - 1];

			if (animKeyframeBefore._timeInSecond > currentTime)
			{
				--_lastKeyIndex;

				if (_lastKeyIndex == 0)
				{
					if (_isLooping)
					{
						_lastKeyIndex = lastKeyframeIndex;
					}
					else
					{
						// We're freed from the animation object.
						return false;
					}
				}

				lastAnimKeyframe = &_keyframes[_lastKeyIndex];
			}
			else
			{
				if (animKeyframeBefore._timeInSecond == currentTime)
				{
					--_lastKeyIndex;
					outPos = animKeyframeBefore._position;
					outRot = animKeyframeBefore._rotation;
				}
				else
				{
					lerpKeyframe(*lastAnimKeyframe, animKeyframeBefore, currentTime, outPos, outRot);
				}

				break;
			}

		} while (true);
	}

	return _isLooping || _lastKeyIndex != lastKeyframeIndex;
}

std::size_t Storm::AnimationObject::getKeyframeCount() const
{
	return _keyframes.size();
}


/*************************************************************************
    > File Name: LayoutDB.cpp
    > Author: Yibo Lin
    > Mail: yibolin@utexas.edu
    > Created Time: Sat 29 Aug 2015 11:45:41 AM CDT
 ************************************************************************/

#include "LayoutDB.h"
#include <boost/dynamic_bitset.hpp>

SIMPLEMPL_BEGIN_NAMESPACE

namespace gtl = boost::polygon;
namespace bg = boost::geometry;
namespace bgi = bg::index;

LayoutDB::LayoutDB()
    : LayoutDB::base_type()
{
    initialize();
}
LayoutDB::LayoutDB(LayoutDB::coordinate_type xl, LayoutDB::coordinate_type yl, LayoutDB::coordinate_type xh, LayoutDB::coordinate_type yh)
    : LayoutDB::base_type(xl, yl, xh, yh)
{
    initialize();
}
LayoutDB::LayoutDB(LayoutDB const& rhs)
    : LayoutDB::base_type(rhs)
{
    copy(rhs);
}
LayoutDB::~LayoutDB()
{
    // recycle
    for (std::vector<rectangle_pointer_type>::iterator it = vPatternBbox.begin(), ite = vPatternBbox.end(); it != ite; ++it)
        delete *it;
}
LayoutDB& LayoutDB::operator=(LayoutDB const& rhs)
{
    if (this != &rhs)
    {
        this->base_type::operator=(rhs);
        copy(rhs);
    }
    return *this;
}
void LayoutDB::initialize()
{
    strname                  = "TOPCELL";
    unit                     = 0.001;
    coloring_distance        = 0;
}
void LayoutDB::copy(LayoutDB const& rhs)
{
    hPath    = rhs.hPath;
    strname  = rhs.strname;
    unit     = rhs.unit;
    coloring_distance = rhs.coloring_distance;
    // options
    parms    = rhs.parms;
    // layout information
    tPatternBbox = rhs.tPatternBbox;
    vPatternBbox = rhs.vPatternBbox;
}
void LayoutDB::add(int32_t layer, std::vector<point_type> const& vPoint)
{
    // classify features
    // sometimes path may be defined as boundary
    if (parms.sPathLayer.count(layer))
        this->add_path(layer, vPoint);
    else
        this->add_pattern(layer, vPoint);
}
void LayoutDB::add_path(int32_t layer, std::vector<point_type> const& vPoint)
{
    if (vPoint.size() < 2) return;

    path_type p;
    if (vPoint.size() == 2) // initialized by path
        p = gtl::construct<path_type>(vPoint.front(), vPoint.back());
    else if (vPoint.size() == 4) // probably it is initialized from boundary
    {
        gtl::coordinate_traits<coordinate_type>::coordinate_distance dist[] = {
            gtl::euclidean_distance(vPoint[0], vPoint[1]),
            gtl::euclidean_distance(vPoint[1], vPoint[2])
        };
        // simply check if one edge is much longer than the other and choose the longer one
        int32_t offset = -1;
        if (dist[0] > 10*dist[1])
            offset = 0;
        else if (10*dist[0] < dist[1])
            offset = 1;
        if (offset >= 0)
            p = gtl::construct<path_type>(vPoint[offset], vPoint[offset+1]);
        else return;
    }
    if (hPath.count(layer))
        hPath[layer].push_back(p);
    else mplAssert(hPath.insert(make_pair(layer, std::vector<path_type>(1, p))).second);
}
void LayoutDB::initialize_data()
{
    // remember to set coloring_distance from coloring_distance_nm and unit
    coloring_distance = (coordinate_difference)round(coloring_distance_nm()/(unit*1e+9));
}
void LayoutDB::report_data_kernel() const
{
    mplPrint(kINFO, "Total patterns # = %lu\n", vPatternBbox.size());
    mplPrint(kINFO, "Shape mode = %s\n", std::string(shape_mode()).c_str());
    mplPrint(kINFO, "Coloring distance = %lld db ( %g nm )\n", coloring_distance, coloring_distance_nm());
    mplPrint(kINFO, "Color num = %d\n", color_num());
    mplPrint(kINFO, "Simplification level = %u\n", simplify_level());
    mplPrint(kINFO, "Thread num = %u\n", thread_num());
    mplPrint(kINFO, "Uncolored layer # = %lu", parms.sUncolorLayer.size());
    if (!parms.sUncolorLayer.empty())
    {
        mplPrint(kNONE, " ( ");
        for (std::set<int32_t>::const_iterator it = parms.sUncolorLayer.begin(), ite = parms.sUncolorLayer.end(); it != ite; ++it)
            mplPrint(kNONE, "%d ", *it);
        mplPrint(kNONE, ")");
    }
    mplPrint(kNONE, "\n");
    mplPrint(kINFO, "Precolored layer # = %lu", parms.sPrecolorLayer.size());
    if (!parms.sPrecolorLayer.empty())
    {
        mplPrint(kNONE, " ( ");
        for (std::set<int32_t>::const_iterator it = parms.sPrecolorLayer.begin(), ite = parms.sPrecolorLayer.end(); it != ite; ++it)
            mplPrint(kNONE, "%d ", *it);
        mplPrint(kNONE, ")");
    }
    mplPrint(kNONE, "\n");
    mplPrint(kINFO, "Path layer # = %lu", parms.sPathLayer.size());
    if (!parms.sPathLayer.empty())
    {
        mplPrint(kNONE, " ( ");
        for (std::set<int32_t>::const_iterator it = parms.sPathLayer.begin(), ite = parms.sPathLayer.end(); it != ite; ++it)
            mplPrint(kNONE, "%d ", *it);
        mplPrint(kNONE, ")");
    }
    mplPrint(kNONE, "\n");
    mplPrint(kINFO, "Algorithm = %s\n", std::string(algo()).c_str());

    // if defined sPathLayer, then hPath must not be empty
    mplAssertMsg(parms.sPathLayer.empty() == hPath.empty(), "defined %lu path layers, but no path read from input\n", parms.sPathLayer.size());

}
void LayoutDB::update_bbox(base_type const& bbox)
{
    // LayoutDB is an abstract class
    // under g++ 4.8, it complains construction of abstract class in gtl::construct<T>()
    // actually we only want to call gtl::assign<base_type, base_type>() and gtl::encompass<base_type, base_type>()
    // so simply specialize it
    if (vPatternBbox.empty())
        gtl::assign<base_type>(*this, bbox);
    else
        gtl::encompass<base_type>(*this, bbox);
}
void LayoutDB::check_layer_and_color(int32_t layer, bool& pattern_layer_flag, int8_t& color) const
{
    pattern_layer_flag = true;
    color = -1;
    if (parms.sPrecolorLayer.count(layer))
    {
        // for precolored patterns
        // set colors
        color = layer-*parms.sPrecolorLayer.begin();
    }
    else if (parms.sUncolorLayer.count(layer))
    {
        // for uncolored patterns
    }
    else pattern_layer_flag = false;
}
/// remove overlapping patterns
/// based on scanline approach, horizontal sort and then vertical sort
/// O(nlogn)
void LayoutDB::remove_overlap(std::vector<LayoutDB::rectangle_pointer_type>& vTargetPattern)
{
    // do nothing if empty
    if (vTargetPattern.empty()) return;
    std::sort(vTargetPattern.begin(), vTargetPattern.end(), compare_rectangle_type());
    // only duplicate is removed so far
    // TO DO: remove overlapping patterns
    boost::dynamic_bitset<uint32_t, std::allocator<uint32_t> > vValid (vTargetPattern.size()); // use a bit set to record validity
    vValid.set(); // set all to 1
#ifdef DEBUG
    mplAssert(vValid[0] && vValid[vTargetPattern.size()-1]);
#endif
    uint32_t duplicate_cnt = 0;
    for (std::vector<rectangle_pointer_type>::iterator it1 = vTargetPattern.begin(), it2 = vTargetPattern.begin();
            it2 != vTargetPattern.end(); ++it2)
    {
        if (it2 != vTargetPattern.begin())
        {
            rectangle_pointer_type& pPattern1 = *it1;
            rectangle_pointer_type& pPattern2 = *it2;

            if (gtl::equivalence(*pPattern1, *pPattern2))
            {
#ifdef DEBUG
                mplPrint(kWARN, "%s duplicates with %s ignored %u\n", std::string(*pPattern1).c_str(), std::string(*pPattern2).c_str(), duplicate_cnt);
#endif
                vValid[pPattern2->pattern_id()] = false;
                duplicate_cnt += 1;
                continue;
            }
        }
        it1 = it2;
    }
    mplPrint(kINFO, "Ignored %u duplicate patterns\n", duplicate_cnt);

    // erase invalid patterns
    uint32_t invalid_cnt = 0;
    for (uint32_t i = 0; i < vTargetPattern.size(); )
    {
        if (!vValid[vTargetPattern[i]->pattern_id()])
        {
            std::swap(vTargetPattern[i], vTargetPattern.back());
            delete vTargetPattern.back(); // recycle
            vTargetPattern.pop_back();
            invalid_cnt += 1;
        }
        else ++i;
    }
    mplAssert(duplicate_cnt == invalid_cnt);
#ifdef DEBUG
    for (uint32_t i = 0; i != vTargetPattern.size(); ++i)
        mplAssert(vValid[vTargetPattern[i]->pattern_id()]);
#endif
    // update pattern_id
    for (uint32_t i = 0; i != vTargetPattern.size(); ++i)
        vTargetPattern[i]->pattern_id(i);
}

SIMPLEMPL_END_NAMESPACE


/**
 * @file   CamPose.cpp
 * @brief  Implementation of CamPose class for camera pose representation.
 * @author Charlie Li
 * @date   2019.08.23
 */

#include "CamPose.hpp"

#include <iostream>

#include <opencv2/calib3d.hpp> // cv::Rodrigues()
#include <opencv2/core.hpp>
#include <opencv2/core/eigen.hpp> // cv::cv2eigen()
#include <Eigen/Core>
#include <Eigen/Geometry> // for Matrix3f::eulerAngles(), Quaternion

namespace SLAM_demo {

using std::endl;
using cv::Mat;

CamPose::CamPose() : mTcw(Mat(3, 4, CV_32FC1)), mTwc(Mat(3, 4, CV_32FC1))
{
    Mat I = Mat::eye(3, 3, CV_32FC1);
    Mat zero = Mat::zeros(3, 1, CV_32FC1);
    I.copyTo(mTcw.rowRange(0, 3).colRange(0, 3));
    zero.copyTo(mTcw.rowRange(0, 3).col(3));
    setPoseInv();
}

CamPose::CamPose(const cv::Mat& Tcw) :
    mTcw(Tcw.clone()), mTwc(Mat(3, 4, CV_32FC1))
{
    setPoseInv();
}

CamPose::CamPose(const cv::Mat& Rcw, const cv::Mat& tcw) :
    mTcw(Mat(3, 4, CV_32FC1)), mTwc(Mat(3, 4, CV_32FC1))
{
    Rcw.copyTo(mTcw.rowRange(0, 3).colRange(0, 3));
    tcw.copyTo(mTcw.rowRange(0, 3).col(3));
    setPoseInv();
}

CamPose::CamPose(const CamPose& pose)
{
    mTcw = pose.mTcw.clone(); // clone the cv::Mat!
    mTwc = pose.mTwc.clone();
}

CamPose& CamPose::operator=(const CamPose& pose)
{
    mTcw = pose.mTcw.clone(); // clone the cv::Mat!
    mTwc = pose.mTwc.clone();
    return *this;
}

void CamPose::setPose(const cv::Mat& Tcw)
{
    mTcw = Tcw.clone();
    setPoseInv();
}

void CamPose::setPose(const cv::Mat& Rcw, const cv::Mat& tcw)
{
    setRotation(Rcw);
    setTranslation(tcw);
    setPoseInv();
}

cv::Mat CamPose::getRotationAngleAxis() const
{
    Mat RcwAA; // rotation in angle-axis representation
    Mat Rcw = getRotation();
    cv::Rodrigues(Rcw, RcwAA, cv::noArray());
    return RcwAA;
}

//cv::Mat CamPose::getRotationInvAngleAxis() const
//{
//    Mat Rwc = getRotationInv();
//    Mat RwcAA = cv::Rodrigues(Rcw, RcwAA. cv::noArray());
//    return RwcAA;
//}

cv::Mat CamPose::getTranslationSS() const
{
    Mat tx = Mat::zeros(3, 3, CV_32FC1);
    Mat t = getTranslation();
    tx.at<float>(0, 1) = -t.at<float>(2);
    tx.at<float>(0, 2) =  t.at<float>(1);
    tx.at<float>(1, 2) = -t.at<float>(0);
    tx.at<float>(1, 0) = -tx.at<float>(0, 1);
    tx.at<float>(2, 0) = -tx.at<float>(0, 2);
    tx.at<float>(2, 1) = -tx.at<float>(1, 2);
    return tx;
}

Eigen::Matrix<float, 3, 1> CamPose::getREulerAngleEigen() const
{
    Eigen::Matrix<float, 3, 1> ea;
    Eigen::Matrix<float, 3, 3> R;
    cv::cv2eigen(getRotation(), R);
    // construct corresponding Euler angles from rotation matrix representation
    // (yaw, pitch, roll)^T
    ea = R.eulerAngles(2, 1, 0);
    // convert unit from radian to degree
    ea *= 180.f / M_PI;
    return ea;
}

Eigen::Quaternion<float> CamPose::getRQuatEigen() const
{
    Eigen::Matrix<float, 3, 3> R;
    cv::cv2eigen(getRotation(), R);
    Eigen::Quaternionf q(R);
    return q;
}

Eigen::Quaternion<float> CamPose::getRInvQuatEigen() const
{
    Eigen::Matrix<float, 3, 3> RInv;
    cv::cv2eigen(getRotation().t(), RInv);
    Eigen::Quaternionf q(RInv);
    return q;
}

CamPose& CamPose::operator*=(const CamPose& rhs)
{
    Mat RcwL = getRotation();
    Mat tcwL = getTranslation();
    Mat RcwR = rhs.getRotation();
    Mat tcwR = rhs.getTranslation();
    // (4*4 matrix) T_L * T_R = [R_L*R_R, R_L*t_R + t_L; 0^T, 1]
    this->setPose(RcwL*RcwR, RcwL*tcwR + tcwL);
    return *this;
}

const CamPose CamPose::operator*(const CamPose& rhs) const
{
    CamPose ret = *this;
    ret *= rhs;
    return ret;
}

void CamPose::setRotation(const cv::Mat& Rcw)
{
    Rcw.copyTo(mTcw.rowRange(0, 3).colRange(0, 3));
}

void CamPose::setTranslation(const cv::Mat& tcw)
{
    tcw.copyTo(mTcw.rowRange(0, 3).col(3));
}

void CamPose::setPoseInv()
{
    Mat Rcw = getRotation();
    Mat tcw = getTranslation();
    Mat Rwc = Rcw.t();
    Mat twc = -Rwc*tcw;
    Rwc.copyTo(mTwc.rowRange(0, 3).colRange(0, 3));
    twc.copyTo(mTwc.rowRange(0, 3).col(3));
}

std::ostream& operator<<(std::ostream& os, const CamPose& pose)
{
    //os << "Pose Tcw = [Rcw | tcw] = " << endl << pose.getPose() << endl;
    Eigen::Vector3f ea = pose.getREulerAngleEigen();
    os << "Camera origin = " << pose.getCamOrigin().t() << "; ";
    os << "Rotation {yaw, pitch, roll} = {"
       << ea(0) << ", " << ea(1) << ", " << ea(2) << "} (deg); ";
    //os << "Translation tcw = " << pose.getTranslation().t();
    return os;
}

} // namespace SLAM_demo


// Licensed to the Apache Software Foundation (ASF) under one
// or more contributor license agreements.  See the NOTICE file
// distributed with this work for additional information
// regarding copyright ownership.  The ASF licenses this file
// to you under the Apache License, Version 2.0 (the
// "License"); you may not use this file except in compliance
// with the License.  You may obtain a copy of the License at
//
//   http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing,
// software distributed under the License is distributed on an
// "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
// KIND, either express or implied.  See the License for the
// specific language governing permissions and limitations
// under the License.

#include <string>

#include <boost/math/constants/constants.hpp>
#include "runtime/multi-precision.h"
#include "testutil/gtest-util.h"

#include "common/names.h"

namespace mp = boost::multiprecision;
using std::max;
using std::min;
using std::numeric_limits;

namespace impala {

TEST(MultiPrecisionIntTest, Conversion) {
  int128_t x = 0;
  int256_t y = 0;
  EXPECT_TRUE(ConvertToInt256(x) == 0);

  x = -1;
  EXPECT_TRUE(ConvertToInt256(x) == -1);

  x = 1;
  EXPECT_TRUE(ConvertToInt256(x) == 1);

  x = numeric_limits<int32_t>::max();
  EXPECT_TRUE(ConvertToInt256(x) == numeric_limits<int32_t>::max());

  x = numeric_limits<int32_t>::min();
  EXPECT_TRUE(ConvertToInt256(x) == numeric_limits<int32_t>::min());

  x = numeric_limits<int64_t>::max();
  EXPECT_TRUE(ConvertToInt256(x) == numeric_limits<int64_t>::max());

  x = numeric_limits<int64_t>::min();
  EXPECT_TRUE(ConvertToInt256(x) == numeric_limits<int64_t>::min());

  x = numeric_limits<int64_t>::max();
  x *= 1000;
  y = numeric_limits<int64_t>::max();
  y *= 1000;
  EXPECT_TRUE(ConvertToInt256(x) == y);

  x = -numeric_limits<int64_t>::max();
  x *= 1000;
  y = -numeric_limits<int64_t>::max();
  y *= 1000;
  EXPECT_TRUE(ConvertToInt256(x) == y);

  // Note: numer_limits<> doesn't work for int128_t.
  static int128_t MAX_VALUE;
  memset(&MAX_VALUE, 255, sizeof(MAX_VALUE));
  uint8_t* buf = reinterpret_cast<uint8_t*>(&MAX_VALUE);
  buf[15] = 127;

  bool overflow = false;
  EXPECT_TRUE(ConvertToInt128(ConvertToInt256(x), MAX_VALUE, &overflow) == x);
  EXPECT_FALSE(overflow);
}

TEST(MultiPrecisionIntTest, HighLowBits) {
  // x = 0x0f0e0d0c0b0a09080706050403020100
  int128_t x = 0;
  for (int i = 0; i < sizeof(x); ++i) {
    *(reinterpret_cast<uint8_t*>(&x) + i) = i;
  }
  EXPECT_EQ(LowBits(x), 0x0706050403020100);
  EXPECT_EQ(HighBits(x), 0x0f0e0d0c0b0a0908);
}

// Simple example of adding and subtracting numbers that use more than
// 64 bits.
TEST(MultiPrecisionIntTest, Example) {
  int128_t v128 = 0;
  v128 += int128_t(numeric_limits<uint64_t>::max());
  v128 += int128_t(numeric_limits<uint64_t>::max());

  v128 -= int128_t(numeric_limits<uint64_t>::max());
  EXPECT_EQ(v128, numeric_limits<uint64_t>::max());

  v128 -= int128_t(numeric_limits<uint64_t>::max());
  EXPECT_EQ(v128, 0);
}

// Example taken from:
// http://www.boost.org/doc/libs/1_55_0/libs/multiprecision/doc/html/boost_multiprecision/tut/floats/fp_eg/aos.html

template<typename T> inline T area_of_a_circle(T r) {
   using boost::math::constants::pi;
   return pi<T>() * r * r;
}

TEST(MultiPrecisionFloatTest, Example) {
  const float r_f(float(123) / 100);
  const float a_f = area_of_a_circle(r_f);

  const double r_d(double(123) / 100);
  const double a_d = area_of_a_circle(r_d);

  const mp::cpp_dec_float_50 r_mp(mp::cpp_dec_float_50(123) / 100);
  const mp::cpp_dec_float_50 a_mp = area_of_a_circle(r_mp);

  stringstream ss;

  // Verify the results at different precisions.
  ss.str("");
  ss << setprecision(numeric_limits<float>::digits10)
     << a_f;
  EXPECT_EQ(ss.str(), "4.75292");

  ss.str("");
  ss << std::setprecision(std::numeric_limits<double>::digits10)
     << a_d;
  EXPECT_EQ(ss.str(), "4.752915525616");

  ss.str("");
  ss << std::setprecision(std::numeric_limits<mp::cpp_dec_float_50>::digits10)
     << a_mp;
  EXPECT_EQ(ss.str(), "4.7529155256159981904701331745635599135018975843146");
}

}

IMPALA_TEST_MAIN();


/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use,
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following
 * conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 */

#include "dectest.hpp"

#include <iostream>

#include <boost/program_options.hpp>

#include <alice/rules.hpp>

#include <range/v3/range_for.hpp>
#include <range/v3/algorithm/for_each.hpp>
#include <range/v3/view/split.hpp>
#include <range/v3/view/transform.hpp>

#include <cli/stores.hpp>
#include <classical/utils/truth_table_utils.hpp>

using namespace ranges::v3;
using boost::program_options::value;

namespace cirkit
{

unsigned tt_is_decomposable( const tt& func, const tt& mask, unsigned var )
{
  tt fcopy = func & mask;
  tt fvarp = tt_nth_var( var ) & mask;
  tt fvarn = ~tt_nth_var( var ) & mask;
  tt_align( fcopy, fvarp );
  tt_align( fcopy, fvarn );

  if ( fcopy.is_subset_of( fvarp ) ) return 1;
  if ( fcopy.is_subset_of( fvarn ) ) return 2;
  if ( fvarp.is_subset_of( fcopy ) ) return 3;
  if ( fvarn.is_subset_of( fcopy ) ) return 4;

  return 0;
}

dectest_command::dectest_command( const environment::ptr& env )
  : cirkit_command( env, "Checks truth tables for decomposability" )
{
  opts.add_options()
    ( "staircase,s", value( &staircase ), "comma-separated list of ids: i,j,k,...\nchecks for decompositions f = g1(xi, g2(xj, g3(xk, others)))" )
    ;
}

command::rules_t dectest_command::validity_rules() const
{
  return {has_store_element<tt>( env )};
}

bool dectest_command::execute()
{
  /* truth table in store */
  const auto& func = env->store<tt>().current();

  if ( is_set( "staircase" ) )
  {
    std::cout << "f  = " << tt_to_hex( func ) << std::endl;

    auto mask = ~tt( func.size() );

    /* split ids into list of ints */
    RANGES_FOR( auto i, staircase
                | view::split( ',' )
                | view::transform( []( const std::string& s ) { return boost::lexical_cast<unsigned>( s ); } ) )
    {
      std::cout << " f  = " << func << std::endl;
      std::cout << " m  = " << mask << std::endl;

      const auto fvar = tt_nth_var( i );
      std::cout << " x" << i << " = " <<  fvar << std::endl;
      std::cout << "!x" << i << " = " << ~fvar << std::endl;

      const auto d = tt_is_decomposable( func, mask, i );
      if ( !d )
      {
        std::cout << "decomposition failed at variable " << i << std::endl;
        break;
      }
      else
      {
        std::cout << d << std::endl;
        switch ( d )
        {
        case 1: /* AND(x, g) */
        case 4: /* OR(!x, g) */
          mask &= fvar;
          break;
        case 2: /* AND(!x, g) */
        case 3: /* OR(x, g) */
          mask &= ~fvar;
          break;
        default: /* impossible */
          assert( 0 );
          break;
        }
      }
    }
  }

  return true;
}

}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End: