/**
 * \file dcs/testbed/anglano2014_fc2q_application_manager.hpp
 *
 * \brief Application manager based on the work by (Anglano et al., 2014)
 *
 * This class implements the FC2Q fuzzy controller proposed in [1].
 *
 * References:
 * -# C. Anglano, M. Canonico and M. Guazzone,
 *    "FC2Q: Exploiting Fuzzy Control in Server Consolidation for Cloud Applications with SLA Constraints,"
 *    Concurrency and Computation: Practice and Experience, 27:4491–4514, 2015.
 *    doi:10.1002/cpe.3410
 * .
 *
 * \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_ANGLANO2014_FC2Q_APPLICATION_MANAGER_HPP
#define DCS_TESTBED_ANGLANO2014_FC2Q_APPLICATION_MANAGER_HPP

#include <boost/smart_ptr.hpp>
#include <boost/timer/timer.hpp>
#include <cmath>
#include <cstddef>
#include <dcs/assert.hpp>
#include <dcs/debug.hpp>
#include <dcs/exception.hpp>
#include <dcs/logging.hpp>
#include <dcs/math/function/clamp.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_estimators.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 <sstream>
#include <stdexcept>
#include <string>
#include <vector>

namespace dcs { namespace testbed {

/**
 * \brief Application manager based on the work by (Anglano et al., 2014)
 *
 * This class implements the FC2Q fuzzy controller proposed in [1].
 *
 * References:
 * -# C. Anglano, M. Canonico and M. Guazzone,
 *    "FC2Q: Exploiting Fuzzy Control in Server Consolidation for Cloud Applications with SLA Constraints,"
 *    Concurrency and Computation: Practice and Experience, 27:4491–4514, 2015.
 *    doi:10.1002/cpe.3410
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template <typename TraitsT>
class anglano2014_fc2q_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::map<application_performance_category,sensor_pointer> out_sensor_map;
	private: typedef ::std::map<virtual_machine_performance_category,::std::map<vm_identifier_type,sensor_pointer> > in_sensor_map;

	private: static const std::size_t control_warmup_size;
	private: static const float resource_share_lb_scale_factor;

	private: static const ::std::string rgain_fuzzy_var_name;
	private: static const ::std::string cres_fuzzy_var_name;
	private: static const ::std::string deltac_fuzzy_var_name;

	public: anglano2014_fc2q_application_manager()
	: beta_(0.9),
	  p_fuzzy_eng_(new fl::Engine()),
	  ctl_count_(0),
	  ctl_skip_count_(0),
	  ctl_fail_count_(0)
	{
		init();
	}

	public: void smoothing_factor(real_type value)
	{
		beta_ = value;
	}

	public: real_type smoothing_factor() const
	{
		return beta_;
	}

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

	private: void init()
	{
		DCS_DEBUG_ASSERT( p_fuzzy_eng_ );

		fl::InputVariable* p_iv = 0;

		p_iv = new fl::InputVariable();
		p_iv->setEnabled(true);
		p_iv->setName(cres_fuzzy_var_name);
		p_iv->setRange(0.0, 1.0);
		p_iv->addTerm(new fl::Ramp("LOW", 0.30, 0.00));
		p_iv->addTerm(new fl::Triangle("FINE", 0.10, 0.25, 0.40));
		p_iv->addTerm(new fl::Ramp("HIGH", 0.30, 1.00));
		p_fuzzy_eng_->addInputVariable(p_iv);

		p_iv = new fl::InputVariable();
		p_iv->setEnabled(true);
		p_iv->setName(rgain_fuzzy_var_name);
		p_iv->setRange(-1, 1);
		p_iv->addTerm(new fl::Ramp("LOW", 0.20, -0.40));
		p_iv->addTerm(new fl::Triangle("FINE", 0.10, 0.20, 0.30));
		p_iv->addTerm(new fl::Ramp("HIGH", 0.30, 1.00));
		p_fuzzy_eng_->addInputVariable(p_iv);

		fl::OutputVariable* p_ov = 0;
		p_ov = new fl::OutputVariable();
		p_ov->setEnabled(true);
		p_ov->setName(deltac_fuzzy_var_name);
		p_ov->setRange(-1, 1);
		//p_ov->setLockValueInRange(true);
		p_ov->fuzzyOutput()->setAccumulation(new fl::AlgebraicSum());
		p_ov->setDefuzzifier(new fl::Centroid());
		p_ov->setDefaultValue(fl::nan);
		p_ov->setPreviousValue(false);
		p_ov->addTerm(new fl::Triangle("BDW", -1.00, -0.55, -0.10));
		p_ov->addTerm(new fl::Triangle("DWN", -0.20, -0.125, -0.05));
		p_ov->addTerm(new fl::Triangle("STY", -0.10, 0.0, 0.10));
		p_ov->addTerm(new fl::Triangle("UP", 0.05, 0.125, 0.20));
		p_ov->addTerm(new fl::Triangle("BUP", 0.10, 0.55, 1.00));
		p_fuzzy_eng_->addOutputVariable(p_ov);

		fl::RuleBlock* 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::AlgebraicProduct());
		p_rules->addRule(fl::Rule::parse("if " + cres_fuzzy_var_name + " is LOW and " + rgain_fuzzy_var_name + " is LOW then " + deltac_fuzzy_var_name + " is BUP", p_fuzzy_eng_.get()));
		p_rules->addRule(fl::Rule::parse("if " + cres_fuzzy_var_name + " is LOW and " + rgain_fuzzy_var_name + " is FINE then " + deltac_fuzzy_var_name + " is UP", p_fuzzy_eng_.get()));
		p_rules->addRule(fl::Rule::parse("if " + cres_fuzzy_var_name + " is LOW and " + rgain_fuzzy_var_name + " is HIGH then " + deltac_fuzzy_var_name + " is UP", p_fuzzy_eng_.get()));
		p_rules->addRule(fl::Rule::parse("if " + cres_fuzzy_var_name + " is FINE and " + rgain_fuzzy_var_name + " is LOW then " + deltac_fuzzy_var_name + " is UP", p_fuzzy_eng_.get()));
		p_rules->addRule(fl::Rule::parse("if " + cres_fuzzy_var_name + " is FINE and " + rgain_fuzzy_var_name + " is FINE then " + deltac_fuzzy_var_name + " is STY", p_fuzzy_eng_.get()));
		p_rules->addRule(fl::Rule::parse("if " + cres_fuzzy_var_name + " is FINE and " + rgain_fuzzy_var_name + " is HIGH then " + deltac_fuzzy_var_name + " is DWN", p_fuzzy_eng_.get()));
		p_rules->addRule(fl::Rule::parse("if " + cres_fuzzy_var_name + " is HIGH and " + rgain_fuzzy_var_name + " is LOW then " + deltac_fuzzy_var_name + " is STY", p_fuzzy_eng_.get()));
		p_rules->addRule(fl::Rule::parse("if " + cres_fuzzy_var_name + " is HIGH and " + rgain_fuzzy_var_name + " is FINE then " + deltac_fuzzy_var_name + " is DWN", p_fuzzy_eng_.get()));
		p_rules->addRule(fl::Rule::parse("if " + cres_fuzzy_var_name + " is HIGH and " + rgain_fuzzy_var_name + " is HIGH then " + deltac_fuzzy_var_name + " is BDW", p_fuzzy_eng_.get()));
		p_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
		out_sensors_.clear();
		for (target_iterator tgt_it = this->target_values().begin(),
							 tgt_end_it = this->target_values().end();
			 tgt_it != tgt_end_it;
			 ++tgt_it)
		{
			const application_performance_category cat = tgt_it->first;

			out_sensors_[cat] = this->app().sensor(cat);
		}

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

			in_sensors_[cpu_util_virtual_machine_performance][p_vm->id()] = p_vm->sensor(cpu_util_virtual_machine_performance);
			in_sensors_[memory_util_virtual_machine_performance][p_vm->id()] = p_vm->sensor(memory_util_virtual_machine_performance);
		}

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

		// Reset fuzzy controller
		p_fuzzy_eng_->restart();

		// Reset resource utilization smoothers
		for (::std::size_t i = 0; i < nvms; ++i)
		{
			this->data_smoother(cpu_util_virtual_machine_performance, vms[i]->id(), ::boost::make_shared< testbed::brown_single_exponential_smoother<real_type> >(beta_));
			this->data_smoother(memory_util_virtual_machine_performance, vms[i]->id(), ::boost::make_shared< testbed::brown_single_exponential_smoother<real_type> >(beta_));
		}

		// Reset output data file
		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\"";

			for (::std::size_t i = 0; i < nvms; ++i)
			{
				*p_dat_ofs_ << ",\"CPUCap_{" << vms[i]->id() << "}(k)\",\"CPUShare_{" << vms[i]->id() << "}(k)\"";
			}
			for (::std::size_t i = 0; i < nvms; ++i)
			{
				*p_dat_ofs_ << ",\"CPUShare_{" << vms[i]->id() << "}(k-1)\"";
			}
			for (::std::size_t i = 0; i < nvms; ++i)
			{
				*p_dat_ofs_ << ",\"CPUUtil_{" << vms[i]->id() << "}(k-1)\",\"MemUtil_{" << vms[i]->id() << "}(k-1)\"";
			}
			for (target_iterator tgt_it = this->target_values().begin(),
							 	 tgt_end_it = this->target_values().end();
				 tgt_it != tgt_end_it;
				 ++tgt_it)
			{
				const application_performance_category cat = tgt_it->first;

				*p_dat_ofs_ << ",\"ReferenceOutput_{" << cat << "}(k-1)\",\"MeasuredOutput_{" << cat << "}(k-1)\",\"RelativeOutputError_{" << cat << "}(k-1)\"";
			}
			for (::std::size_t i = 0; i < nvms; ++i)
			{
				*p_dat_ofs_ << ",\"Cres_{" << vms[i]->id() << "}(k-1)\"";
			}
			for (::std::size_t i = 0; i < nvms; ++i)
			{
				*p_dat_ofs_ << ",\"DeltaC_{" << vms[i]->id() << "}(k)\"";
			}
			//NOTE: C(k) may differ from CPUShare(k) for several reasons:
			// - There is a latency in setting the new share (e.g., this is usually not the case of CPU but of other resources like the memory, whereby the new share is not immediately set but the memory is (de)allocated incrementally)
			// - There is another component between this controller and physical resources that may change the wanted share (e.g., if a physical resource is shared among different VMs, there can be a component that try to allocate the contented physical resource fairly).
			for (std::size_t i = 0; i < nvms; ++i)
			{
				*p_dat_ofs_ << ",\"C_{" << vms[i]->id() << "}(k)\"";
			}
			*p_dat_ofs_ << ",\"# Controls\",\"# Skip Controls\",\"# Fail Controls\"";
            *p_dat_ofs_ << ",\"Elapsed Time\"";
			*p_dat_ofs_ << ::std::endl;
		}
	}

	private: void do_sample()
	{
		typedef typename in_sensor_map::const_iterator in_sensor_iterator;
		typedef typename out_sensor_map::const_iterator out_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 input values
		for (in_sensor_iterator in_sens_it = in_sensors_.begin(),
								in_sens_end_it = in_sensors_.end();
			 in_sens_it != in_sens_end_it;
			 ++in_sens_it)
		{
			const virtual_machine_performance_category cat = in_sens_it->first;

			for (typename in_sensor_map::mapped_type::const_iterator vm_it = in_sens_it->second.begin(),
																	 vm_end_it = in_sens_it->second.end();
				 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 output values
		for (out_sensor_iterator out_sens_it = out_sensors_.begin(),
								 out_sens_end_it = out_sensors_.end();
			 out_sens_it != out_sens_end_it;
			 ++out_sens_it)
		{
			const application_performance_category cat = out_sens_it->first;

			sensor_pointer p_sens = out_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_);

        boost::timer::cpu_timer cpu_timer;

		++ctl_count_;

		bool skip_ctl = false;

		std::vector<real_type> old_shares;
		std::vector<real_type> new_shares;
		std::vector<real_type> deltacs;
		std::vector<real_type> cress;
		std::vector<real_type> cutils;
		std::map<application_performance_category,real_type> rgains;

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

		for (::std::size_t i = 0; i < nvms; ++i)
		{
			const virtual_machine_performance_category cat = cpu_util_virtual_machine_performance;
			const vm_pointer p_vm = vms[i];

//			if (this->data_estimator(cat, vms[i]->id()).count() > 0)
//			{
//				//const real_type uh = this->data_estimator(cat, p_vm->id()).estimate();
//				const real_type uh = this->data_smoother(cat, p_vm->id()).forecast(0);
//				const real_type c = p_vm->cpu_share();
//
//				cress[cat].push_back(c-uh);
//			}
//			else
//			{
//				// No observation collected during the last control interval
//				DCS_DEBUG_TRACE("No input observation collected during the last control interval -> Skip control");
//				skip_ctl = true;
//				break;
//			}
			const real_type uh = this->data_smoother(cat, p_vm->id()).forecast(0);
			const real_type c = p_vm->cpu_share();
			const real_type cres = c-uh;

			cress.push_back(cres);
			old_shares.push_back(c);
			cutils.push_back(uh);
DCS_DEBUG_TRACE("VM " << p_vm->id() << " - Performance Category: " << cat << " - Uhat(k): " << uh << " - C(k): " << c << " -> Cres(k+1): " << cres << " (Relative Cres(k+1): " << cres/c << ")");//XXX
		}

		if (!skip_ctl)
		{
			for (target_iterator tgt_it = this->target_values().begin(),
							 	 tgt_end_it = this->target_values().end();
				 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 yh = this->data_estimator(cat).estimate();
					const real_type yr = this->target_value(cat);

					switch (cat)
					{
						case response_time_application_performance:
							rgains[cat] = (yr-yh)/yr;
							break;
						case throughput_application_performance:
							rgains[cat] = (yh-yr)/yr;
							break;
					}
DCS_DEBUG_TRACE("APP Performance Category: " << cat << " - Yhat(k): " << yh << " - R: " << yr << " -> Rgain(k+1): " << rgains.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
			}
		}

		// Skip control until we see enough observations.
		// This should give enough time to let the estimated performance metric
		// (e.g., 95th percentile of response time) stabilize
		if (ctl_count_ <= control_warmup_size)
		{
			skip_ctl = true;
		}

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

			// Perform fuzzy control
			bool ok = false;
			//::std::vector<real_type> deltacs(nvms);
			try
			{
				for (::std::size_t i = 0; i < nvms; ++i)
				{
					const real_type cres = cress[i];
					const real_type cutil = cutils[i];
					const real_type rgain = rgains.begin()->second;
					const real_type old_share = old_shares[i];
					const real_type deltac_lb = std::min(1.0, cutil*resource_share_lb_scale_factor)-old_share;
					const real_type deltac_ub = std::max(0.0, 1-old_share);

					p_fuzzy_eng_->setInputValue(cres_fuzzy_var_name, cres/old_share);
					p_fuzzy_eng_->setInputValue(rgain_fuzzy_var_name, rgain);
					//p_fuzzy_eng_->getOutputVariable(deltac_fuzzy_var_name)->setMinimum(-cres);
					//p_fuzzy_eng_->getOutputVariable(deltac_fuzzy_var_name)->setMaximum(1-old_shares[i]);

					p_fuzzy_eng_->process();

					const real_type fuzzy_deltac = p_fuzzy_eng_->getOutputValue(deltac_fuzzy_var_name);

					real_type deltac = fuzzy_deltac;
					deltac = dcs::math::clamp(deltac, deltac_lb, deltac_ub);

					deltacs.push_back(deltac);
DCS_DEBUG_TRACE("VM " << vms[i]->id() << " -> DeltaC(k+1): " << deltacs.at(i) << " (computed: " << fuzzy_deltac << ", lb: " << deltac_lb << ", ub: " << deltac_ub << ")");//XXX
				}

				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& e)
			{
				DCS_DEBUG_TRACE( "Caught exception: " << e.what() );

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

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

					const real_type old_share = p_vm->cpu_share();
					const real_type new_share = ::std::max(::std::min(old_share+deltacs[i], 1.0), 0.0);

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

					if (::std::isfinite(new_share) && !::dcs::math::float_traits<real_type>::essentially_equal(old_share, new_share))
					{
						p_vm->cpu_share(new_share);
DCS_DEBUG_TRACE("VM " << vms[i]->id() << " -> C(k+1): " << new_share);//XXX

						new_shares.push_back(new_share);
					}
					else
					{
DCS_DEBUG_TRACE("VM " << vms[i]->id() << " -> C(k+1) not set!");//XXX

						new_shares.push_back(old_share);
					}
				}
DCS_DEBUG_TRACE("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_;
		}

        cpu_timer.stop();

		// Export to file
		if (p_dat_ofs_)
		{
			if (new_shares.size() == 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 );

					new_shares.push_back(p_vm->cpu_share());
				}
			}
			if (old_shares.size() == 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 );

					old_shares.push_back(p_vm->cpu_share());
				}
			}
			if (deltacs.size() == 0)
			{
				deltacs.assign(nvms, std::numeric_limits<real_type>::quiet_NaN());
			}
			if (cress.size() == 0)
			{
				cress.assign(nvms, std::numeric_limits<real_type>::quiet_NaN());
			}
			if (rgains.size() == 0)
			{
				for (target_iterator tgt_it = this->target_values().begin(),
									 tgt_end_it = this->target_values().end();
				tgt_it != tgt_end_it;
				++tgt_it)
				{
					const application_performance_category cat = tgt_it->first;

					rgains[cat] = std::numeric_limits<real_type>::quiet_NaN();
				}
			}

			*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();
			}
			*p_dat_ofs_ << ",";
            for (std::size_t i = 0; i < nvms; ++i)
            {
                if (i != 0)
                {
                    *p_dat_ofs_ << ",";
                }
                *p_dat_ofs_ << old_shares[i];
            }
			*p_dat_ofs_ << ",";
			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_ << this->data_smoother(cpu_util_virtual_machine_performance, p_vm->id()).forecast(0)
				//			<< "," << this->data_smoother(memory_util_virtual_machine_performance, p_vm->id()).forecast(0);
				*p_dat_ofs_ << cutils[i]
							<< "," << this->data_smoother(memory_util_virtual_machine_performance, p_vm->id()).forecast(0);
			}
			*p_dat_ofs_ << ",";
			for (target_iterator tgt_it = this->target_values().begin(),
								 tgt_end_it = this->target_values().end();
				 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 yh = this->data_estimator(cat).estimate();
				const real_type yr = tgt_it->second;
				const real_type rgain = rgains.at(cat);
				*p_dat_ofs_ << yr << "," << yh << "," << rgain;
			}
			for (::std::size_t i = 0; i < nvms; ++i)
			{
				*p_dat_ofs_ << "," << cress[i];
			}
			for (std::size_t i = 0; i < nvms; ++i)
			{
				*p_dat_ofs_ << "," << deltacs[i];
			}
			for (std::size_t i = 0; i < nvms; ++i)
			{
				*p_dat_ofs_ << "," << new_shares[i];
			}
			*p_dat_ofs_ << "," << ctl_count_ << "," << ctl_skip_count_ << "," << ctl_fail_count_;
            *p_dat_ofs_ << "," << (cpu_timer.elapsed().user+cpu_timer.elapsed().system);
			*p_dat_ofs_ << ::std::endl;
		}

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

	private: real_type beta_; ///< The EWMA smoothing factor for resource utilizations
	private: ::boost::shared_ptr<fl::Engine> p_fuzzy_eng_; ///< The fuzzy control engine
	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: in_sensor_map in_sensors_;
	private: out_sensor_map out_sensors_;
	private: ::std::string dat_fname_;
	private: ::boost::shared_ptr< ::std::ofstream > p_dat_ofs_;
}; // anglano2014_fc2q_application_manager

template <typename T>
const std::size_t anglano2014_fc2q_application_manager<T>::control_warmup_size = 5;

template <typename T>
const float anglano2014_fc2q_application_manager<T>::resource_share_lb_scale_factor = 1.1;

template <typename T>
const ::std::string anglano2014_fc2q_application_manager<T>::rgain_fuzzy_var_name = "Rgain";

template <typename T>
const ::std::string anglano2014_fc2q_application_manager<T>::cres_fuzzy_var_name = "Cres";

template <typename T>
const ::std::string anglano2014_fc2q_application_manager<T>::deltac_fuzzy_var_name = "DeltaC";

}} // Namespace dcs::testbed

#endif // DCS_TESTBED_ANGLANO2014_FC2Q_APPLICATION_MANAGER_HPP


/*
 *
 * Copyright (c) Toon Knapen & Kresimir Fresl 2003
 *
 * Distributed under 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)
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_HESV_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_HESV_HPP

#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/lapack/ilaenv.hpp>
#include <boost/numeric/bindings/traits/detail/array.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits/is_same.hpp>
#endif

#include <cassert>

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    /////////////////////////////////////////////////////////////////////
    //
    // system of linear equations A * X = B
    // with A Hermitian indefinite matrix
    //
    /////////////////////////////////////////////////////////////////////

    namespace detail {

      inline
      integer_t hetrf_block (traits::complex_f,
                       integer_t const ispec, char const ul, integer_t const n)
      {
        char ul2[2] = "x"; ul2[0] = ul;
        return ilaenv (ispec, "CHETRF", ul2, n);
      }
      inline
      integer_t hetrf_block (traits::complex_d,
                       integer_t const ispec, char const ul, integer_t const n)
      {
        char ul2[2] = "x"; ul2[0] = ul;
        return ilaenv (ispec, "ZHETRF", ul2, n);
      }

    }

    template <typename HermA>
    integer_t hetrf_block (char const q, char const ul, HermA const& a) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::general_t
      >::value));
#endif
      assert (q == 'O' || q == 'M');
      assert (ul == 'U' || ul == 'L');

      integer_t n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<HermA>::value_type val_t;
#else
      typedef typename HermA::value_type val_t;
#endif
      integer_t ispec = (q == 'O' ? 1 : 2);
      return detail::hetrf_block (val_t(), ispec, ul, n);
    }

    template <typename HermA>
    integer_t hetrf_block (char const q, HermA const& a) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::hermitian_t
      >::value));
#endif
      assert (q == 'O' || q == 'M');

      char ul = traits::matrix_uplo_tag (a);
      integer_t n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<HermA>::value_type val_t;
#else
      typedef typename HermA::value_type val_t;
#endif
      integer_t ispec = (q == 'O' ? 1 : 2);
      return detail::hetrf_block (val_t(), ispec, ul, n);
    }

    template <typename HermA>
    integer_t hetrf_work (char const q, char const ul, HermA const& a) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::general_t
      >::value));
#endif
      assert (q == 'O' || q == 'M');
      assert (ul == 'U' || ul == 'L');

      integer_t n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<HermA>::value_type val_t;
#else
      typedef typename HermA::value_type val_t;
#endif
      integer_t lw = -13;
      if (q == 'M')
        lw = 1;
      if (q == 'O')
        lw = n * detail::hetrf_block (val_t(), 1, ul, n);
      return lw;
    }

    template <typename HermA>
    integer_t hetrf_work (char const q, HermA const& a) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::hermitian_t
      >::value));
#endif
      assert (q == 'O' || q == 'M');

      char ul = traits::matrix_uplo_tag (a);
      integer_t n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<HermA>::value_type val_t;
#else
      typedef typename HermA::value_type val_t;
#endif
      integer_t lw = -13;
      if (q == 'M')
        lw = 1;
      if (q == 'O')
        lw = n * detail::hetrf_block (val_t(), 1, ul, n);
      return lw;
    }

    template <typename HermA>
    inline
    integer_t hesv_work (char const q, char const ul, HermA const& a) {
      return hetrf_work (q, ul, a);
    }

    template <typename HermA>
    inline
    integer_t hesv_work (char const q, HermA const& a) { return hetrf_work (q, a); }

    /*
     * hesv() computes the solution to a system of linear equations
     * A * X = B, where A is an N-by-N Hermitian matrix and X and B
     * are N-by-NRHS matrices.
     *
     * The diagonal pivoting method is used to factor A as
     *   A = U * D * U^H,  if UPLO = 'U',
     *   A = L * D * L^H,  if UPLO = 'L',
     * where  U (or L) is a product of permutation and unit upper
     * (lower) triangular matrices, and D is Hermitian and block
     * diagonal with 1-by-1 and 2-by-2 diagonal blocks. The factored
     * form of A is then used to solve the system of equations A * X = B.
     */

    namespace detail {

      inline
      void hesv (char const uplo, integer_t const n, integer_t const nrhs,
                 traits::complex_f* a, integer_t const lda, integer_t* ipiv,
                 traits::complex_f* b, integer_t const ldb,
                 traits::complex_f* w, integer_t const lw, integer_t* info)
      {
        LAPACK_CHESV (&uplo, &n, &nrhs,
                      traits::complex_ptr (a), &lda, ipiv,
                      traits::complex_ptr (b), &ldb,
                      traits::complex_ptr (w), &lw, info);
      }

      inline
      void hesv (char const uplo, integer_t const n, integer_t const nrhs,
                 traits::complex_d* a, integer_t const lda, integer_t* ipiv,
                 traits::complex_d* b, integer_t const ldb,
                 traits::complex_d* w, integer_t const lw, integer_t* info)
      {
        LAPACK_ZHESV (&uplo, &n, &nrhs,
                      traits::complex_ptr (a), &lda, ipiv,
                      traits::complex_ptr (b), &ldb,
                      traits::complex_ptr (w), &lw, info);
      }

      template <typename HermA, typename MatrB, typename IVec, typename Work>
      int hesv (char const ul, HermA& a, IVec& i, MatrB& b, Work& w) {

        integer_t const n = traits::matrix_size1 (a);
        assert (n == traits::matrix_size2 (a));
        assert (n == traits::matrix_size1 (b));
        assert (n == traits::vector_size (i));

        integer_t info;
        hesv (ul, n, traits::matrix_size2 (b),
              traits::matrix_storage (a),
              traits::leading_dimension (a),
              traits::vector_storage (i),
              traits::matrix_storage (b),
              traits::leading_dimension (b),
              traits::vector_storage (w),
              traits::vector_size (w),
              &info);
        return info;
      }

    }

    template <typename HermA, typename MatrB, typename IVec, typename Work>
    int hesv (char const ul, HermA& a, IVec& i, MatrB& b, Work& w) {

      assert (ul == 'U' || ul == 'L');

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      integer_t const lw = traits::vector_size (w);
      assert (lw >= 1);
      return detail::hesv (ul, a, i, b, w);
    }

    template <typename HermA, typename MatrB, typename IVec, typename Work>
    int hesv (HermA& a, IVec& i, MatrB& b, Work& w) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::hermitian_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      integer_t const lw = traits::vector_size (w);
      assert (lw >= 1);
      char uplo = traits::matrix_uplo_tag (a);
      return detail::hesv (uplo, a, i, b, w);
    }

    template <typename HermA, typename MatrB>
    int hesv (char const ul, HermA& a, MatrB& b) {
      // with 'internal' pivot and work vectors

      assert (ul == 'U' || ul == 'L');

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      integer_t const n = traits::matrix_size1 (a);
      integer_t info = -101;
      traits::detail::array<integer_t> i (n);

      if (i.valid()) {
        info = -102;
        integer_t lw = hetrf_work ('O', ul, a);
        assert (lw >= 1); // paranoia ?
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
        typedef typename traits::matrix_traits<HermA>::value_type val_t;
#else
        typedef typename HermA::value_type val_t;
#endif
        traits::detail::array<val_t> w (lw);
        if (w.valid())
          info = detail::hesv (ul, a, i, b, w);
      }
      return info;
    }

    template <typename HermA, typename MatrB>
    int hesv (HermA& a, MatrB& b) {
      // with 'internal' pivot and work vectors

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::hermitian_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      integer_t const n = traits::matrix_size1 (a);
      char uplo = traits::matrix_uplo_tag (a);
      integer_t info = -101;
      traits::detail::array<integer_t> i (n);

      if (i.valid()) {
        info = -102;
        integer_t lw = hetrf_work ('O', a);
        assert (lw >= 1); // paranoia ?
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
        typedef typename traits::matrix_traits<HermA>::value_type val_t;
#else
        typedef typename HermA::value_type val_t;
#endif
        traits::detail::array<val_t> w (lw);
        w.resize (lw);
        if (w.valid())
          info = detail::hesv (uplo, a, i, b, w);
      }
      return info;
    }

    /*
     * hetrf() computes the factorization of a Hermitian matrix A using
     * the  Bunch-Kaufman diagonal pivoting method. The form of the
     * factorization is
     *    A = U * D * U^H  or  A = L * D * L^H
     * where U (or L) is a product of permutation and unit upper (lower)
     * triangular matrices, and D is Hermitian and block diagonal with
     * 1-by-1 and 2-by-2 diagonal blocks.
     */

    namespace detail {

      inline
      void hetrf (char const uplo, integer_t const n,
                  traits::complex_f* a, integer_t const lda, integer_t* ipiv,
                  traits::complex_f* w, integer_t const lw, integer_t* info)
      {
        LAPACK_CHETRF (&uplo, &n,
                       traits::complex_ptr (a), &lda, ipiv,
                       traits::complex_ptr (w), &lw, info);
      }

      inline
      void hetrf (char const uplo, integer_t const n,
                  traits::complex_d* a, integer_t const lda, integer_t* ipiv,
                  traits::complex_d* w, integer_t const lw, integer_t* info)
      {
        LAPACK_ZHETRF (&uplo, &n,
                       traits::complex_ptr (a), &lda, ipiv,
                       traits::complex_ptr (w), &lw, info);
      }

      template <typename HermA, typename IVec, typename Work>
      int hetrf (char const ul, HermA& a, IVec& i, Work& w) {

        integer_t const n = traits::matrix_size1 (a);
        assert (n == traits::matrix_size2 (a));
        assert (n == traits::vector_size (i));

        integer_t info;
        hetrf (ul, n, traits::matrix_storage (a),
               traits::leading_dimension (a),
               traits::vector_storage (i),
               traits::vector_storage (w),
               traits::vector_size (w),
               &info);
        return info;
      }

    }

    template <typename HermA, typename IVec, typename Work>
    int hetrf (char const ul, HermA& a, IVec& i, Work& w) {

      assert (ul == 'U' || ul == 'L');

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::general_t
      >::value));
#endif

      assert (traits::vector_size (w) >= 1);
      return detail::hetrf (ul, a, i, w);
    }

    template <typename HermA, typename IVec, typename Work>
    int hetrf (HermA& a, IVec& i, Work& w) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::hermitian_t
      >::value));
#endif

      std::ptrdiff_t const lw = traits::vector_size (w);
      assert (lw >= 1);
      char uplo = traits::matrix_uplo_tag (a);
      return detail::hetrf (uplo, a, i, w);
    }

    template <typename HermA, typename Ivec>
    int hetrf (char const ul, HermA& a, Ivec& i) {
      // with 'internal' work vector

      assert (ul == 'U' || ul == 'L');

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::general_t
      >::value));
#endif

      integer_t info = -101;
      integer_t lw = hetrf_work ('O', ul, a);
      assert (lw >= 1); // paranoia ?
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<HermA>::value_type val_t;
#else
      typedef typename HermA::value_type val_t;
#endif
      traits::detail::array<val_t> w (lw);
      if (w.valid())
        info = detail::hetrf (ul, a, i, w);
      return info;
    }

    template <typename HermA, typename Ivec>
    int hetrf (HermA& a, Ivec& i) {
      // with 'internal' work vector

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::hermitian_t
      >::value));
#endif

      char uplo = traits::matrix_uplo_tag (a);
      integer_t info = -101;
      integer_t lw = hetrf_work ('O', a);
      assert (lw >= 1); // paranoia ?
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<HermA>::value_type val_t;
#else
      typedef typename HermA::value_type val_t;
#endif
      traits::detail::array<val_t> w (lw);
      if (w.valid())
        info = detail::hetrf (uplo, a, i, w);
      return info;
    }

    /*
     * hetrs() solves a system of linear equations A*X = B with
     * a Hermitian matrix A using the factorization
     *    A = U * D * U^H   or  A = L * D * L^H
     * computed by hetrf().
     */

    namespace detail {

      inline
      void hetrs (char const uplo, integer_t const n, integer_t const nrhs,
                  traits::complex_f const* a, integer_t const lda,
                  integer_t const* ipiv,
                  traits::complex_f* b, integer_t const ldb, integer_t* info)
      {
        LAPACK_CHETRS (&uplo, &n, &nrhs,
                       traits::complex_ptr (a), &lda, ipiv,
                       traits::complex_ptr (b), &ldb, info);
      }

      inline
      void hetrs (char const uplo, integer_t const n, integer_t const nrhs,
                  traits::complex_d const* a, integer_t const lda,
                  integer_t const* ipiv,
                  traits::complex_d* b, integer_t const ldb, integer_t* info)
      {
        LAPACK_ZHETRS (&uplo, &n, &nrhs,
                       traits::complex_ptr (a), &lda, ipiv,
                       traits::complex_ptr (b), &ldb, info);
      }

      template <typename HermA, typename MatrB, typename IVec>
      int hetrs (char const ul, HermA const& a, IVec const& i, MatrB& b) {

        integer_t const n = traits::matrix_size1 (a);
        assert (n == traits::matrix_size2 (a));
        assert (n == traits::matrix_size1 (b));
        assert (n == traits::vector_size (i));

        integer_t info;
        hetrs (ul, n, traits::matrix_size2 (b),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
               traits::matrix_storage (a),
#else
               traits::matrix_storage_const (a),
#endif
               traits::leading_dimension (a),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
               traits::vector_storage (i),
#else
               traits::vector_storage_const (i),
#endif
               traits::matrix_storage (b),
               traits::leading_dimension (b), &info);
        return info;
      }

    }

    template <typename HermA, typename MatrB, typename IVec>
    inline
    int hetrs (char const ul, HermA const& a, IVec const& i, MatrB& b) {

      assert (ul == 'U' || ul == 'L');

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      return detail::hetrs (ul, a, i, b);
    }

    template <typename HermA, typename MatrB, typename IVec>
    inline
    int hetrs (HermA const& a, IVec const& i, MatrB& b) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::hermitian_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      char uplo = traits::matrix_uplo_tag (a);
      return detail::hetrs (uplo, a, i, b);
    }

    // TO DO: hetri

  }

}}}

#endif


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2013, PAL Robotics, S.L.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the PAL Robotics nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/*
 * Author: Luca Marchionni
 * Author: Bence Magyar
 * Author: Enrique Fernández
 * Author: Paul Mathieu
 */

#include "odometry.h"

#include <cmath>
#include <boost/bind.hpp>

#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/command-direct-getter.h>
#include <dynamic-graph/command-direct-setter.h>
#include <dynamic-graph/factory.h>

namespace dynamicgraph
{
namespace details
{
  namespace bacc = boost::accumulators;

  Odometry::Odometry(size_t velocity_rolling_window_size)
  : x_(0.0)
  , y_(0.0)
  , heading_(0.0)
  , linear_(0.0)
  , angular_(0.0)
  , wheelSeparation_(0.0)
  , left_wheelRadius_(0.0)
  , right_wheelRadius_(0.0)
  , left_wheel_old_pos_(0.0)
  , right_wheel_old_pos_(0.0)
  , velocity_rolling_window_size_(velocity_rolling_window_size)
  , linear_acc_(RollingWindow::window_size = velocity_rolling_window_size)
  , angular_acc_(RollingWindow::window_size = velocity_rolling_window_size)
  , integrate_fun_(boost::bind(&Odometry::integrateExact, this, _1, _2))
  {
  }

  void Odometry::init()
  {
    // Reset accumulators:
    resetAccumulators();
  }

  bool Odometry::update(double left_pos, double right_pos, double dt)
  {
    /// Get current wheel joint positions:
    const double left_wheel_cur_pos  = left_pos  * left_wheelRadius_;
    const double right_wheel_cur_pos = right_pos * right_wheelRadius_;

    /// Estimate velocity of wheels using old and current position:
    const double left_wheel_est_vel  = left_wheel_cur_pos  - left_wheel_old_pos_;
    const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_;

    /// Update old position with current:
    left_wheel_old_pos_  = left_wheel_cur_pos;
    right_wheel_old_pos_ = right_wheel_cur_pos;

    /// Compute linear and angular diff:
    const double linear  = (right_wheel_est_vel + left_wheel_est_vel) * 0.5 ;
    const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheelSeparation_;

    /// Integrate odometry:
    integrate_fun_(linear, angular);

    /// We cannot estimate the speed with very small time intervals:
    if (dt < 0.0001)
      return false; // Interval too small to integrate with

    /// Estimate speeds using a rolling mean to filter them out:
    linear_acc_(linear/dt);
    angular_acc_(angular/dt);

    linear_ = bacc::rolling_mean(linear_acc_);
    angular_ = bacc::rolling_mean(angular_acc_);

    return true;
  }

  void Odometry::updateOpenLoop(double linear, double angular, double dt)
  {
    /// Save last linear and angular velocity:
    linear_ = linear;
    angular_ = angular;

    /// Integrate odometry:
    integrate_fun_(linear * dt, angular * dt);
  }

  void Odometry::setWheelParams(double wheelSeparation, double left_wheelRadius, double right_wheelRadius)
  {
    wheelSeparation_   = wheelSeparation;
    left_wheelRadius_  = left_wheelRadius;
    right_wheelRadius_ = right_wheelRadius;
  }

  void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size)
  {
    velocity_rolling_window_size_ = velocity_rolling_window_size;

    resetAccumulators();
  }

  void Odometry::integrateRungeKutta2(double linear, double angular)
  {
    const double direction = heading_ + angular * 0.5;

    /// Runge-Kutta 2nd order integration:
    x_       += linear * cos(direction);
    y_       += linear * sin(direction);
    heading_ += angular;
  }

  /**
   * \brief Other possible integration method provided by the class
   * \param linear
   * \param angular
   */
  void Odometry::integrateExact(double linear, double angular)
  {
    if (fabs(angular) < 1e-6)
      integrateRungeKutta2(linear, angular);
    else
    {
      /// Exact integration (should solve problems when angular is zero):
      const double heading_old = heading_;
      const double r = linear/angular;
      heading_ += angular;
      x_       +=  r * (sin(heading_) - sin(heading_old));
      y_       += -r * (cos(heading_) - cos(heading_old));
    }
  }

  void Odometry::resetAccumulators()
  {
    linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
    angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
  }

} // namespace details

  namespace bacc = boost::accumulators;

  Odometry::Odometry(const std::string& name)
  : Entity (name)
  , wheelsPositionSIN (NULL, "Odometry("+name+")::input(vector)::wheelsPosition")
  , baseVelocityEstimationSOUT (boost::bind (&Odometry::computeVelocityEstimation, this, _1, _2),
      wheelsPositionSIN, "Odometry("+name+")::output(vector)::baseVelocityEstimation")
  , baseVelocityFilteredEstimationSOUT (boost::bind (&Odometry::computeVelocityFilteredEstimation, this, _1, _2),
      baseVelocityEstimationSOUT, "Odometry("+name+")::output(vector)::baseVelocityFilteredEstimation")
  , baseVelocitySIN (NULL, "Odometry("+name+")::input(vector)::baseVelocity")
  , baseConfigSOUT (boost::bind (&Odometry::computeBaseConfig, this, _1, _2),
      baseVelocitySIN, "Odometry("+name+")::output(vector)::baseConfig")
  , basePoseSOUT (boost::bind (&Odometry::computeBasePose, this, _1, _2),
      baseConfigSOUT, "Odometry("+name+")::output(matrixHomogeneous)::basePose")

  , dt_ (1.)
  , x_(0.0)
  , y_(0.0)
  , heading_(0.0)
  , wheelSeparationInv_(0.0)
  , wheelRadii_(0.0, 0.0)
  , wheelOldPos_ (0.0, 0.0)
  , rollingWindowSize_ (10)
  , linear_acc_(RollingWindow::window_size = rollingWindowSize_)
  , angular_acc_(RollingWindow::window_size = rollingWindowSize_)
  {
    signalRegistration (
        wheelsPositionSIN <<
        baseVelocityEstimationSOUT <<
        baseVelocityFilteredEstimationSOUT <<
        baseVelocitySIN <<
        baseConfigSOUT <<
        basePoseSOUT);

    // By default, use the velocity estimation
    // (i.e. loop closed with wheel position sensors).
    baseVelocitySIN.plug (&baseVelocityEstimationSOUT);

    addCommand ("setWheelParams",
        command::makeCommandVoid3 (*this, &Odometry::setWheelParams,
          command::docCommandVoid3 ("Sets the wheel parameters: radius and separation",
            "double: Separation between left and right wheels [m]",
            "double: Left wheel radius [m]",
            "double: Right wheel radius [m]"))
        );
    addCommand ("setBasePose",
        command::makeCommandVoid3 (*this, &Odometry::setBasePose,
          command::docCommandVoid3 ("Set the base pose to integrate from.",
            "double: x [m]",
            "double: y [m]",
            "double: heading [rad]"))
        );
    addCommand ("setPeriod", command::makeDirectSetter (*this, &dt_,
          "Set period (only affects the velocity estimation and nothing else)."));
    addCommand ("getPeriod", command::makeDirectGetter (*this, &dt_,
          "the period"));
  }

  Vector& Odometry::computeVelocityEstimation (Vector& vel, int time)
  {
    vel.resize(2);
    const Vector& wheelCurAng = wheelsPositionSIN (time);

    /// Get current wheel joint positions:
    Eigen::Array2d wheelCurPos = wheelCurAng.array() * wheelRadii_;

    /// Estimate velocity of wheels using old and current position:
    Eigen::Array2d wheelEstVel = ( wheelCurPos - wheelOldPos_ ) / dt_;

    /// Update old position with current:
    wheelOldPos_ = wheelCurPos;

    /// Compute linear and angular diff:
    vel[0] = (wheelEstVel[1] + wheelEstVel[0]) * 0.5                ;
    vel[1] = (wheelEstVel[1] - wheelEstVel[0]) * wheelSeparationInv_;

    /// Estimate speeds using a rolling mean to filter them out:
    linear_acc_ (vel[0]);
    angular_acc_(vel[1]);

    return vel;
  }

  Vector& Odometry::computeVelocityFilteredEstimation (Vector& vel, int time)
  {
    baseVelocityEstimationSOUT.recompute (time);

    vel.resize(2);
    vel[0] = bacc::rolling_mean(linear_acc_);
    vel[1] = bacc::rolling_mean(angular_acc_);

    return vel;
  }

  Vector& Odometry::computeBaseConfig (Vector& base, int time)
  {
    base.resize(3);
    const Vector& vel = baseVelocitySIN (time);
    integrateExact (vel[0] * dt_, vel[1] * dt_);

    base << x_, y_, heading_;
    return base;
  }

  sot::MatrixHomogeneous& Odometry::computeBasePose (sot::MatrixHomogeneous& M, int time)
  {
    const Vector& base = baseConfigSOUT (time);
    M.setIdentity();
    M.translation().head<2>() = base.head<2>();
    double c = cos(base(2)), s = sin(base(2));
    M.linear().topLeftCorner<2,2>() <<
      c, -s,
      s,  c;
    return M;
  }

  void Odometry::setBasePose (const double& x, const double& y, const double& heading)
  {
    x_ = x;
    y_ = y;
    heading_ = heading;

    resetAccumulators ();
  }

  void Odometry::setWheelParams(const double& wheelSeparation, const double& leftWheelRadius, const double& rightWheelRadius)
  {
    wheelSeparationInv_   = 1/wheelSeparation;
    wheelRadii_  << leftWheelRadius, rightWheelRadius;
  }

  void Odometry::setVelocityRollingWindowSize(const size_t& rollingWindowSize)
  {
    rollingWindowSize_ = rollingWindowSize;
    resetAccumulators ();
  }

  void Odometry::integrateRungeKutta2(double linear, double angular)
  {
    const double direction = heading_ + angular * 0.5;

    /// Runge-Kutta 2nd order integration:
    x_       += linear * cos(direction);
    y_       += linear * sin(direction);
    heading_ += angular;
  }

  /**
   * \brief Other possible integration method provided by the class
   * \param linear
   * \param angular
   */
  void Odometry::integrateExact(double linear, double angular)
  {
    if (fabs(angular) < 1e-6)
      integrateRungeKutta2(linear, angular);
    else
    {
      /// Exact integration (should solve problems when angular is zero):
      const double heading_old = heading_;
      const double r = linear/angular;
      heading_ += angular;
      x_       +=  r * (sin(heading_) - sin(heading_old));
      y_       += -r * (cos(heading_) - cos(heading_old));
    }
  }

  void Odometry::resetAccumulators()
  {
    linear_acc_  = RollingMeanAcc(RollingWindow::window_size = rollingWindowSize_);
    angular_acc_ = RollingMeanAcc(RollingWindow::window_size = rollingWindowSize_);
  }

  DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN (Odometry, "Odometry");

} // namespace diff_drive_controller


/* The Image Registration Toolkit (IRTK)
 *
 * Copyright 2008-2015 Imperial College London
 *
 * 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. */

// MUST be included first due to nasty IRTK "round" macro
#include <boost/random.hpp>

#include <irtkPointSamples.h>

typedef boost::mt19937 RandomNumberGeneratorType;

// =============================================================================
// Construction/Destruction
// =============================================================================

// -----------------------------------------------------------------------------
irtkPointSamples::irtkPointSamples(int n, int seed)
:
  irtkPointSet(n), _RandomNumberGenerator(new RandomNumberGeneratorType())
{
  RandomNumberGeneratorType &rng = *(RandomNumberGeneratorType*)_RandomNumberGenerator;
  if (seed < 0) rng.seed(static_cast<unsigned int>(std::time(0)));
  else          rng.seed(seed);
}

// -----------------------------------------------------------------------------
irtkPointSamples::~irtkPointSamples()
{
  delete (RandomNumberGeneratorType*)_RandomNumberGenerator;
}

// =============================================================================
// Uniform grid sampling
// =============================================================================

// -----------------------------------------------------------------------------
void irtkPointSamples::SampleGrid(const irtkPoint &p1, const irtkPoint &p2,
                                  int nx, int ny, int nz)
{
  SampleGrid(p1._x, p1._y, p1._z, p2._x, p2._y, p2._z, nx, ny, nz);
}

// -----------------------------------------------------------------------------
void irtkPointSamples::SampleGrid(const irtkPoint &p1, const irtkPoint &p2,
                                  double dx, double dy, double dz)
{
  SampleGrid(p1._x, p1._y, p1._z, p2._x, p2._y, p2._z, dx, dy, dz);
}

// -----------------------------------------------------------------------------
void irtkPointSamples::SampleGrid(double x1, double y1, double z1,
                                  double x2, double y2, double z2,
                                  int    nx, int    ny, int    nz)
{
  if (x2 < x1) swap(x1, x2);
  if (y2 < y1) swap(y1, y2);
  if (z2 < z1) swap(z1, z2);
  if (nx <= 0) nx = 1;
  if (ny <= 0) ny = 1;
  if (nz <= 0) nz = 1;
  const double sx = x2 - x1;
  const double sy = x2 - x1;
  const double sz = x2 - x1;
  const double dx = sx / nx;
  const double dy = sy / ny;
  const double dz = sz / nz;
  Size(nx * ny * nz);
  int n = 0;
  for (int k = 0; k < nz; ++k)
  for (int j = 0; j < ny; ++j)
  for (int i = 0; i < nx; ++i, ++n) {
    _data[n] = irtkPoint(x1 + i * dx, y1 + j * dy, z1 + k * dz);
  }
}

// -----------------------------------------------------------------------------
void irtkPointSamples::SampleGrid(double x1, double y1, double z1,
                                  double x2, double y2, double z2,
                                  double dx, double dy, double dz)
{
  if (x2 < x1) swap(x1, x2);
  if (y2 < y1) swap(y1, y2);
  if (z2 < z1) swap(z1, z2);
  if (dx < 0) dx = .0;
  if (dy < 0) dy = .0;
  if (dz < 0) dz = .0;
  const double sx = x2 - x1;
  const double sy = x2 - x1;
  const double sz = x2 - x1;
  const int    nx = (dx > .0 ? round(sx / dx) : 1);
  const int    ny = (dy > .0 ? round(sy / dy) : 1);
  const int    nz = (dz > .0 ? round(sz / dz) : 1);
  Size(nx * ny * nz);
  int n = 0;
  for (int k = 0; k < nz; ++k)
  for (int j = 0; j < ny; ++j)
  for (int i = 0; i < nx; ++i, ++n) {
    _data[n] = irtkPoint(x1 + i * dx, y1 + j * dy, z1 + k * dz);
  }
}

// =============================================================================
// Uniform spherical distribution
// =============================================================================

// -----------------------------------------------------------------------------
void irtkPointSamples::SampleSphere(double r)
{
  SampleSphere(.0, r);
}

// -----------------------------------------------------------------------------
void irtkPointSamples::SampleSphere(double c, double r)
{
  SampleSphere(c, c, c, r, r, r);
}

// -----------------------------------------------------------------------------
void irtkPointSamples::SampleSphere(const irtkPoint & c, double r)
{
  SampleSphere(c._x, c._y, c._z, r, r, r);
}

// -----------------------------------------------------------------------------
void irtkPointSamples::SampleSphere(const irtkPoint & c, double rx, double ry, double rz)
{
  SampleSphere(c._x, c._y, c._z, rx, ry, rz);
}

// -----------------------------------------------------------------------------
void irtkPointSamples::SampleSphere(double cx, double cy, double cz, double r)
{
  SampleSphere(cx, cy, cz, r, r, r);
}

// -----------------------------------------------------------------------------
void irtkPointSamples::SampleSphere(double cx, double cy, double cz,
                                    double rx, double ry, double rz)
{
  typedef boost::uniform_on_sphere<double>                                   Distribution;
  typedef boost::variate_generator<RandomNumberGeneratorType&, Distribution> Generator;

  RandomNumberGeneratorType &rng = *(RandomNumberGeneratorType*)_RandomNumberGenerator;

  Distribution dist(3);
  Generator next(rng, dist);

  vector<double> p(3);
  for (int i = 0; i < _n; ++i) {
    p = next();
    _data[i]._x = cx + rx * p[0];
    _data[i]._y = cy + ry * p[1];
    _data[i]._z = cz + rz * p[2];
  }
}

// =============================================================================
// Normal distribution
// =============================================================================

// -----------------------------------------------------------------------------
void irtkPointSamples::SampleGaussian(double s)
{
  SampleGaussian(.0, s);
}

// -----------------------------------------------------------------------------
void irtkPointSamples::SampleGaussian(double m, double s)
{
  SampleGaussian(m, m, m, s, s, s);
}

// -----------------------------------------------------------------------------
void irtkPointSamples::SampleGaussian(const irtkPoint & m, double s)
{
  SampleGaussian(m._x, m._y, m._z, s, s, s);
}

// -----------------------------------------------------------------------------
void irtkPointSamples::SampleGaussian(const irtkPoint & m, double sx, double sy, double sz)
{
  SampleGaussian(m._x, m._y, m._z, sx, sy, sz);
}

// -----------------------------------------------------------------------------
void irtkPointSamples::SampleGaussian(double mx, double my, double mz, double s)
{
  SampleGaussian(mx, my, mz, s, s, s);
}

// -----------------------------------------------------------------------------
void irtkPointSamples::SampleGaussian(double mx, double my, double mz,
                                      double sx, double sy, double sz)
{
  typedef boost::normal_distribution<double>                                 Distribution;
  typedef boost::variate_generator<RandomNumberGeneratorType&, Distribution> Generator;

  RandomNumberGeneratorType &rng = *(RandomNumberGeneratorType*)_RandomNumberGenerator;

  Distribution distx(mx, sx);
  Distribution disty(my, sy);
  Distribution distz(mz, sz);

  Generator x(rng, distx);
  Generator y(rng, disty);
  Generator z(rng, distz);

  for (int i = 0; i < _n; ++i) {
    _data[i]._x = x();
    _data[i]._y = y();
    _data[i]._z = z();
  }
}


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published
// by the Free Software Foundation, either version 3 of the License,
// or (at your option) any later version.
//
// FlexibleSUSY is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

#ifndef NMSSM_TWOLOOPHIGGS_H
#define NMSSM_TWOLOOPHIGGS_H

#include <Eigen/Core>

/**
 * @file nmssm_twoloophiggs.hpp
 * @brief function declarations for 2-loop NMSSM Higgs self-energies
 *        and tadpoles
 *
 * Notation:
 *
 * mt2    : squared DR-bar top mass in the NMSSM
 * mb2    : squared DR-bar bottom mass in the NMSSM
 * mtau2  : squared DR-bar tau mass in the NMSSM
 * mg     : DR-bar gluino mass in the NMSSM
 * mA2    : squared DR-bar CP-odd Higgs mass in the NMSSM
 * mst12  : squared DR-bar lightest stop mass
 * mst22  : squared DR-bar heaviest stop mass
 * msb12  : squared DR-bar lightest sbottom mass
 * msb22  : squared DR-bar heaviest sbottom mass
 * mstau12: squared DR-bar lightest stau mass
 * mstau22: squared DR-bar heaviest stau mass
 *
 * sxt    : sine of DR-bar stop mixing angle in the NMSSM
 * cxt    : cosine of DR-bar stop mixing angle in the NMSSM
 * sxb    : sine of DR-bar sbottom mixing angle in the NMSSM
 * cxb    : cosine of DR-bar sbottom mixing angle in the NMSSM
 * sintau : sine of DR-bar stau mixing angle in the NMSSM
 * costau : cosine of DR-bar stau mixing angle in the NMSSM
 *
 * gs     : DR-bar strong gauge coupling g3 in the NMSSM
 * mu     : DR-bar mu-parameter in the NMSSM (arXiv:0907.4682)
 * tanb   : DR-bar tan(beta) = vu/vd in the NMSSM
 * cotb   : DR-bar 1/tan(beta) in the NMSSM
 * vev2   : squared DR-bar vev^2 = (vu^2 + vd^2) in the NMSSM
 * lam    : DR-bar lambda in the NNMSSM
 * svev   : DR-bar singlet VEV = mu_eff / lam
 *
 * scheme : DR-bar scheme (0) or on-shell scheme (1)
 */

namespace flexiblesusy {
namespace nmssm_twoloophiggs {

Eigen::Matrix<double, 3, 1> tadpole_higgs_2loop_at_as_nmssm(
   double mt2, double mg, double mst12, double mst22,
   double sxt, double cxt, double scale2,
   double mu, double tanb, double vev2, double gs, double svev);

Eigen::Matrix<double, 3, 1> tadpole_higgs_2loop_ab_as_nmssm(
   double mb2, double mg, double msb12, double msb22,
   double sxb, double cxb, double scale2,
   double mu, double cotb, double vev2, double gs, double svev);

Eigen::Matrix<double, 3, 3> self_energy_higgs_2loop_at_as_nmssm(
   double rmt, double mg, double mst12, double mst22,
   double sxt, double cxt, double scale2, double tanb, double vev2,
   double lam, double svev, double as, double mu);

Eigen::Matrix<double, 3, 3> self_energy_higgs_2loop_ab_as_nmssm(
   double rmb, double mg, double msb12, double msb22,
   double sxb, double cxb, double scale2, double cotb, double vev2,
   double lam, double svev, double as, double mu);

Eigen::Matrix<double, 3, 3> self_energy_pseudoscalar_2loop_at_as_nmssm(
   double rmt, double mg, double mst12, double mst22,
   double sxt, double cxt, double scale2, double tanb, double vev2,
   double lam, double svev, double as, double mu);

Eigen::Matrix<double, 3, 3> self_energy_pseudoscalar_2loop_ab_as_nmssm(
   double rmb, double mg, double msb12, double msb22,
   double sxb, double cxb, double scale2, double cotb, double vev2,
   double lam, double svev, double as, double mu);

Eigen::Matrix<double, 3, 3> self_energy_higgs_2loop_at_as_nmssm_with_tadpoles(
   double rmt, double mg, double mst12, double mst22,
   double sxt, double cxt, double scale2, double tanb, double vev2,
   double lam, double svev, double as);

Eigen::Matrix<double, 3, 3> self_energy_higgs_2loop_ab_as_nmssm_with_tadpoles(
   double rmb, double mg, double msb12, double msb22,
   double sxb, double cxb, double scale2, double cotb, double vev2,
   double lam, double svev, double as);

Eigen::Matrix<double, 3, 3> self_energy_pseudoscalar_2loop_at_as_nmssm_with_tadpoles(
   double rmt, double mg, double mst12, double mst22,
   double sxt, double cxt, double scale2, double tanb, double vev2,
   double lam, double svev, double as);

Eigen::Matrix<double, 3, 3> self_energy_pseudoscalar_2loop_ab_as_nmssm_with_tadpoles(
   double rmb, double mg, double msb12, double msb22,
   double sxb, double cxb, double scale2, double cotb, double vev2,
   double lam, double svev, double as);

} // namespace nmssm_twoloophiggs
} // namespace flexiblesusy

#endif


/**
 * @file BayesianNetwork.hpp
 * @brief Implementation of the BayesianNetwork functions.
 * @author Ankit Srivastava <asrivast@gatech.edu>
 *
 * Copyright 2020 Georgia Institute of Technology
 *
 * 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 DETAIL_BAYESIANNETWORK_HPP_
#define DETAIL_BAYESIANNETWORK_HPP_

#include "utils/Logging.hpp"

#include <boost/graph/copy.hpp>
#include <boost/graph/tiernan_all_cycles.hpp>

template <typename Var>
/**
  * @brief Helper class that implements the anti-parallel edge filter functionality.
 */
class BayesianNetwork<Var>::AntiParallelEdgeFilter {
public:
  AntiParallelEdgeFilter(
  ) : m_graph(nullptr)
  {
  }

  AntiParallelEdgeFilter(
    const GraphImpl& g
  ) : m_graph(&g)
  {
  }

  template <typename EdgeDescriptor>
  bool
  operator()(
    const EdgeDescriptor& e
  ) const
  {
    auto source = boost::source(e, *m_graph);
    auto target = boost::target(e, *m_graph);
    return !boost::edge(target, source, *m_graph).second;
  }

private:
  const GraphImpl* m_graph;
}; // class AntiParallelEdgeFilter

template <typename Var>
/**
 * @brief Helper class for counting all the simple cycles that
 *        an edge is part of.
 */
class BayesianNetwork<Var>::EdgeCycleCounter {
public:
  EdgeCycleCounter(
    const Graph<BidirectionalAdjacencyList, VertexLabel, Var>& graph,
    std::unordered_map<Edge, size_t, typename Edge::Hash>& counts
  ) : m_graph(graph),
      m_counts(counts)
  {
  }

  template <typename Path, typename DirectedGraph>
  void
  cycle(
    const Path& p,
    const DirectedGraph& dg
  )
  {
    using IndexMap = typename boost::property_map<DirectedGraph, boost::vertex_index_t>::const_type;
    IndexMap indices = boost::get(boost::vertex_index, dg);
    auto u = p.begin();
    auto v = u+1;
    while (v != p.end()) {
      auto e = m_graph.getEdge(boost::get(indices, *u), boost::get(indices, *v));
      if (m_counts.find(e) == m_counts.end()) {
        m_counts[e] = 0;
      }
      m_counts[e] += 1;
      ++u;
      ++v;
    }
    v = p.begin();
    auto e = m_graph.getEdge(boost::get(indices, *u), boost::get(indices, *v));
    if (m_counts.find(e) == m_counts.end()) {
      m_counts[e] = 0;
    }
    m_counts[e] += 1;
  }

private:
  const Graph<BidirectionalAdjacencyList, VertexLabel, Var>& m_graph;
  std::unordered_map<Edge, size_t, typename Edge::Hash>& m_counts;
}; // class EdgeCycleCounter

template <typename Var>
/**
 * @brief Constructs empty network with given labels as vertices.
 */
BayesianNetwork<Var>::BayesianNetwork(
  const std::vector<std::string>& varLabels
) : Graph<BidirectionalAdjacencyList, VertexLabel, Var>(varLabels),
    m_directed(this->filterAntiParallelEdges())
{
}

template <typename Var>
/**
 * @brief Adds a directed or undirected edge between the vertices.
 */
void
BayesianNetwork<Var>::addEdge(
  const Var source,
  const Var target,
  const bool undirected
)
{
  this->addEdge(source, target);
  if (undirected) {
    this->addEdge(target, source);
  }
}

template <typename Var>
/**
 * @brief Returns a filtered view of the current graph with the anti-parallel edges removed.
 */
typename BayesianNetwork<Var>::FilteredGraph
BayesianNetwork<Var>::filterAntiParallelEdges(
) const
{
  AntiParallelEdgeFilter bef(this->m_graph);
  boost::filtered_graph<decltype(this->m_graph), AntiParallelEdgeFilter> fg(this->m_graph, bef);
  return FilteredGraph(std::move(fg), this->m_idVertexMap);
}

template <typename Var>
/**
 * @brief Orients the edges of the network in accordance with v-structures.
 *
 * @param vStructures A tuple with all the discovered v-structures and the corresponding p-values.
 */
void
BayesianNetwork<Var>::applyVStructures(
  std::vector<std::tuple<double, Var, Var, Var>>&& vStructures
)
{
  // First sort the v-structures in the ascending order of the p-values
  std::sort(vStructures.begin(), vStructures.end());
  for (const auto& vs : vStructures) {
    auto y = this->wrap(this->m_idVertexMap.at(std::get<1>(vs)));
    auto x = this->wrap(this->m_idVertexMap.at(std::get<2>(vs)));
    auto z = this->wrap(this->m_idVertexMap.at(std::get<3>(vs)));
    // First check if the reverse edges still exist
    if (!this->edgeExists(y, x) || !this->edgeExists(z, x)) {
      LOG_MESSAGE(warning, "* Could not apply v-structure %s -> %s <- %s (p-value = % g)",
                           y.property().label, x.property().label, z.property().label, std::get<0>(vs));
      LOG_MESSAGE_IF(!this->edgeExists(y, x), debug, "* %s - %s has already been oriented in the opposite direction", y.property().label, x.property().label);
      LOG_MESSAGE_IF(!this->edgeExists(z, x), debug, "* %s - %s has already been oriented in the opposite direction", x.property().label, z.property().label);
      continue;
    }
    LOG_MESSAGE(info, "+ Applying the v-structure %s -> %s <- %s (p-value = %g)",
                      y.property().label, x.property().label, z.property().label, std::get<0>(vs));
    this->removeEdge(x, y);
    this->removeEdge(x, z);
  }
}

template <typename Var>
/**
 * @brief Function which checks if the network has directed cycles.
 */
bool
BayesianNetwork<Var>::hasDirectedCycles(
) const
{
  return m_directed.hasCycles();
}

template <typename Var>
/**
 * @brief Counts the number of simple cycles that each edge is part of.
 */
std::unordered_map<typename BayesianNetwork<Var>::Edge, size_t, typename BayesianNetwork<Var>::Edge::Hash>
BayesianNetwork<Var>::countEdgeCycles(
) const
{
  // Copy the directed view of the graph to a directed graph
  typename DirectedGraph<VertexLabel, Var>::Impl dg;
  boost::copy_graph(*m_directed, dg);
  // Record all the counts
  std::unordered_map<Edge, size_t, typename Edge::Hash> counts;
  EdgeCycleCounter ecc(*this, counts);
  boost::tiernan_all_cycles(dg, ecc);
  return counts;
}

template <typename Var>
/**
 * @brief Function which breaks directed cycles in the network by reversing
 *        the direction of the edge which is part of most cycles.
 */
void
BayesianNetwork<Var>::breakDirectedCycles(
)
{
  Edge e;
  auto maxCount = 0u;
  for (const auto& cc : this->countEdgeCycles()) {
    if (cc.second > maxCount) {
      e = cc.first;
      maxCount = cc.second;
    }
  }
  if (maxCount > 0) {
    auto source = *e.source();
    auto target = *e.target();
    // Reverse the edge
    LOG_MESSAGE(info, "* Reversing the direction of edge %s -> %s", e.source().property().label, e.target().property().label);
    this->removeEdge(source, target);
    this->addEdge(target, source);
  }
}

template <typename Var>
/**
 * @brief Function which orients an edge, if it doesn't create directed cycles.
 *
 * @param e The edge to be removed.
 *
 * @returns true if any changes were made, otherwise returns false.
 */
bool
BayesianNetwork<Var>::removeEdgeAcyclic(
  Edge&& e
)
{
  auto source = e.source();
  auto target = e.target();
  this->removeEdge(std::move(e));
  if (m_directed.hasCycles(*source)) {
    this->addEdge(source, target);
    return false;
  }
  return true;
}

template <typename Var>
/**
 * @brief Function which checks if the undirected edge Y - Z can be
 *        oriented as Y -> Z to prevent new unshielded colliders.
 */
bool
BayesianNetwork<Var>::unshieldedColliderRule(
  const Vertex& y,
  const Vertex& z
) const
{
  if (m_directed.wrap(*y).inDegree() == 0) {
    return false;
  }
  bool potential = false;
  // Examine all the edges incoming into Y for a potential X
  for (const auto inY : m_directed.wrap(*y).inEdges()) {
    auto x = inY.source();
    // Check if an X exists such that no edge exists between X and Z
    if (!(this->edgeExists(*z, *x) || this->edgeExists(*x, *z))) {
      potential = true;
      break;
    }
  }
  return potential;
}

template <typename Var>
/**
 * @brief Function which checks if the undirected edge X - Z can be
 *        oriented as X -> Z to prevent directed cycles.
 */
bool
BayesianNetwork<Var>::acyclicityRule(
  const Vertex& x,
  const Vertex& z
) const
{
  // Therefore, try to apply the rule by setting the source as Z and the target as X
  bool orientEdge = false;
  // Iterate over all the outgoing neighbors of X for a potential Y
  for (const auto y : m_directed.wrap(*x).outNeighbors()) {
    if (m_directed.edgeExists(*y, *z)) {
      // Orient this edge as X -> Z
      // ...as long as it does not create an immorality
      orientEdge = true;
      break;
    }
  }
  bool immorality = false;
  if (orientEdge) {
    for (const auto inZ : m_directed.wrap(*z).inEdges()) {
      auto w = inZ.source();
      // Check if there is an edge between the source of another incoming edge into Z and X
      if (!(this->edgeExists(*x, *w) || this->edgeExists(*w, *x))) {
        // If no such edge exists, then the rule can not be applied because an immorality will be created
        immorality = true;
        break;
      }
    }
  }
  return (orientEdge && !immorality);
}

template <typename Var>
/**
 * @brief Function which checks if the undirected edge X - Z can be
 *        oriented as X -> Z by applying the hybrid rule.
 */
bool
BayesianNetwork<Var>::hybridRule(
  const Vertex& x,
  const Vertex& z
) const
{
  auto countY = 0u;
  // Iterate over all the incoming neighbors of Z for potential Y
  for (const auto inZ : m_directed.wrap(*z).inEdges()) {
    auto y = inZ.source();
    // Check if an undirected edge exists between X and Y
    if (this->edgeExists(*x, *y) && this->edgeExists(*y, *x)) {
      ++countY;
    }
  }
  // The rule can be applied only if at least two Ys were found
  return (countY >= 2);
}

template <typename Var>
/**
 * @brief Top level function for orienting edges using Meek's rules.
 *
 * @returns true if any changes were made, otherwise returns false.
 */
bool
BayesianNetwork<Var>::applyMeekRules(
)
{
  bool changed = false;
  auto isCollider = [] (const Vertex& v) { return (v.inDegree() > v.outDegree()) &&
                                                  (v.inDegree() - v.outDegree() > 1); };
  // Iterate over all the undirected edges
  for (auto e : this->antiParallelEdges()) {
    // Check if the anti-parallel edge still exists
    if (!e.hasAntiParallel()) {
      continue;
    }
    // See if this direction of the anti-parallel edge can be removed
    // Therefore, check if the rules can be applied to the reverse direction
    auto source = e.source();
    auto target = e.target();
    if (isCollider(source) && isCollider(target)) {
      LOG_MESSAGE_IF(*source < *target, info, "* Fixing edge %s - %s because of conflicting v-structures", source.property().label, target.property().label);
      continue;
    }
    if (this->unshieldedColliderRule(target, source)) { // Apply Meek's Rule 1
      if (this->removeEdgeAcyclic(std::move(e))) {
        LOG_MESSAGE(info, "* Directing edge %s -> %s (R1: unshielded colliders)", target.property().label, source.property().label);
        changed = true;
      }
    }
    else if (this->acyclicityRule(target, source)) { // Apply Meek's Rule 2
      if (this->removeEdgeAcyclic(std::move(e))) {
        LOG_MESSAGE(info, "* Directing edge %s -> %s (R2: acyclicity)", target.property().label, source.property().label);
        changed = true;
      }
    }
    else if (this->hybridRule(target, source)) { // Apply Meek's Rule 3
      if (this->removeEdgeAcyclic(std::move(e))) {
        LOG_MESSAGE(info, "* Directing edge %s -> %s (R3: hybrid)", target.property().label, source.property().label);
        changed = true;
      }
    }
  }
  return changed;
}

template <typename Var>
/**
 * @brief Top level function for writing the network in graphviz format.
 *
 * @param fileName Name of the file to which the network should be written.
 */
void
BayesianNetwork<Var>::writeGraphviz(
  const std::string& fileName
) const
{
  std::ofstream out(fileName);
  // Check if there are any directed edges in the graph
  // If not, write the graph as an undirected graph
  auto directed = false;
  for (const auto e : m_directed.edges()) {
    std::ignore = e;
    directed = true;
    break;
  }
  out << (directed ? "digraph" : "graph") << " {" << std::endl;
  for (const auto v : this->vertices()) {
    out << "  ";
    out << boost::escape_dot_string(v.property().label);
    out << " ;" << std::endl;
  }
  auto delimiter = directed ? " -> " : " -- ";
  for (const auto e : this->edges()) {
    bool write = false;
    if (directed && !this->edgeExists(e.target(), e.source())) {
      out << "  edge [dir=forward] ";
      write = true;
    }
    else if (e.source() < e.target()) {
      out << "  edge [dir=none] ";
      write = true;
    }
    if (write) {
      out << boost::escape_dot_string(e.source().property().label);
      out << delimiter;
      out << boost::escape_dot_string(e.target().property().label);
      out << " ;" << std::endl;
    }
  }
  out << "}" << std::endl;
}

#endif // DETAIL_BAYESIANNETWORK_HPP


//---------------------------------------------------------------------------//
//!
//! \file   Utility_AtomicMassUnit.hpp
//! \author Alex Robinson
//! \brief  The atomic mass unit
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_ATOMIC_MASS_UNIT_HPP
#define UTILITY_ATOMIC_MASS_UNIT_HPP

// Boost Includes
#include <boost/units/systems/si/mass.hpp>
#include <boost/units/base_unit.hpp>
#include <boost/units/conversion.hpp>

namespace Utility{

namespace Units{

//! The atomic mass base unit
struct AtomicMassBaseUnit : public boost::units::base_unit<AtomicMassBaseUnit,boost::units::mass_dimension,4>
{
  static const char* name() { return "atomic mass unit"; }
  static const char* symbol() { return "amu"; }
};

//! The atomic mass unit
typedef AtomicMassBaseUnit::unit_type AtomicMass;

BOOST_UNITS_STATIC_CONSTANT( amu, AtomicMass );

} // end Units namespace

} // end Utility namespace

// From codata 2014
BOOST_UNITS_DEFINE_CONVERSION_FACTOR( Utility::Units::AtomicMassBaseUnit, boost::units::si::mass, double, 1.660539040e-27 );
BOOST_UNITS_DEFINE_CONVERSION_FACTOR( boost::units::si::mass, Utility::Units::AtomicMassBaseUnit, double, 1.0/1.660539040e-27 );

BOOST_UNITS_DEFAULT_CONVERSION( Utility::Units::AtomicMassBaseUnit, boost::units::si::mass );

#endif // end UTILITY_ATOMIC_MASS_UNIT_HPP

//---------------------------------------------------------------------------//
// end Utility_AtomicMassUnit.hpp
//---------------------------------------------------------------------------//


/**
 * \file
 *
 * \author Nicholas J. Curtis
 * \date 04/29/2016
 *
 * \brief Defines an interface for boost's runge_kutta_fehlberg78 solver
 *
*/

//wrapper code
#include "rk78_typedefs.hpp"

//boost includes
#include <boost/numeric/odeint/integrate/integrate_adaptive.hpp>
extern "C" {
#include "solver.h"
}

#ifdef GENERATE_DOCS
namespace rk78 {
#endif

extern std::vector<state_type*> state_vectors;
extern std::vector<rhs_eval*> evaluators;
extern std::vector<controller> controllers;

extern "C" void intDriver(const int, const double, const double, const double*, double*);

#ifdef STIFFNESS_MEASURE
    extern std::vector<double> max_stepsize;
    controlled_step_result test_step(int index, const state_type& y, const double t, state_type& y_out, const double dt)
    {
        try
        {
            double t_copy = t;
            double dt_copy = dt;
            return controllers[index]->try_step(*evaluators[index], y, t_copy, y_out, dt_copy);
        }
        catch(...)
        {
            return fail;
        }
    }
#endif

/**
 * \brief Integration driver for the CPU integrators
 * \param[in]       NUM         the number of IVPs to solve
 * \param[in]       t           the current IVP time
 * \param[in]       t_end       the time to integrate the IVP to
 * \param[in]       pr_global   the pressure value for the IVPs
 * \param[in, out]  y_global    the state vectors
 *
 * The integration driver for the RK78 solver
 */
void intDriver (const int NUM, const double t, const double t_end,
                const double *pr_global, double *y_global)
{
    #ifdef STIFFNESS_MEASURE
    max_stepsize.clear();
    max_stepsize.resize(NUM, 0.0);
    #endif

	int tid = 0;
#ifdef STIFFNESS_MEASURE
    #pragma omp parallel for shared(state_vectors, evaluators, controllers, max_stepsize) private(tid)
#else
	#pragma omp parallel for shared(state_vectors, evaluators, controllers) private(tid)
#endif
    for (tid = 0; tid < NUM; ++tid) {
    	int index = omp_get_thread_num();

        // local array with initial values
        state_type& vec = *state_vectors[index];
        evaluators[index]->set_state_var(pr_global[tid]);

        // load local array with initial values from global array
        for (int i = 0; i < NSP; i++)
        {
            vec[i] = y_global[tid + i * NUM];
        }

#ifndef STIFFNESS_MEASURE
        integrate_adaptive(controllers[index],
            *evaluators[index], vec, t, t_end, t_end - t);
#else
        double tol = 1e-15;
        state_type y_copy(vec);
        //do a binary search to find the maximum stepsize
        double left_size = 1.0;
        while (test_step(index, vec, t, y_copy, left_size) == success)
        {
            left_size *= 10.0;
        }
        double right_size = 1e-20;
        while (test_step(index, vec, t, y_copy, right_size) == fail)
        {
            right_size /= 10.0;
        }
        double delta = 1.0;
        double mid = 0;
        while (delta > tol) {
            mid = (left_size + right_size) / 2.0;
            controlled_step_result result = test_step(index, vec, t, y_copy, mid);
            if (result == fail) {
                //mid becomes the new left
                delta = fabs(left_size - mid) / left_size;
                left_size = mid;
            }
            else{
                delta = fabs(right_size - mid) / right_size;
                right_size = mid;
            }
        }
        max_stepsize[tid] = mid;
#endif

        // update global array with integrated values
        for (int i = 0; i < NSP; i++)
        {
            y_global[tid + i * NUM] = vec[i];
        }

    }
}

#ifdef GENERATE_DOCS
}
#endif


#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <string>
#include <sstream>
#include <iostream>
#include <stdexcept>
#include <functional>
#include <mutex>
#include <Eigen/Geometry>

#ifdef ENABLE_PARALLEL_HAUSDORFF_DISTANCE
    #include <omp.h>
#endif

#ifndef SIMPLE_HAUSDORFF_DISTANCE_HPP
#define SIMPLE_HAUSDORFF_DISTANCE_HPP

namespace simple_hausdorff_distance
{
    class SimpleHausdorffDistance
    {
    private:

        SimpleHausdorffDistance() {}

        static inline size_t GetNumOMPThreads(void)
        {
#ifdef ENABLE_PARALLEL_HAUSDORFF_DISTANCE
        #if defined(_OPENMP)
            size_t num_threads = 0;
            #pragma omp parallel
            {
                num_threads = (size_t)omp_get_num_threads();
            }
            return num_threads;
        #else
            return 1;
        #endif
#else
            return 1;
#endif
        }

    public:

        template<typename FirstDatatype, typename SecondDatatype, typename FirstAllocator=std::allocator<FirstDatatype>, typename SecondAllocator=std::allocator<SecondDatatype>>
        static double ComputeDistance(const std::vector<FirstDatatype, FirstAllocator>& first_distribution, const std::vector<SecondDatatype, SecondAllocator>& second_distribution, const std::function<double(const FirstDatatype&, const SecondDatatype&)>& distance_fn)
        {
            // Compute the Hausdorff distance - the "maximum minimum" distance
            std::vector<double> per_thread_storage(GetNumOMPThreads(), 0.0);
#ifdef ENABLE_PARALLEL_HAUSDORFF_DISTANCE
            #pragma omp parallel for
#endif
            for (size_t idx = 0; idx < first_distribution.size(); idx++)
            {
                const FirstDatatype& first = first_distribution[idx];
                double minimum_distance = INFINITY;
                for (size_t jdx = 0; jdx < second_distribution.size(); jdx++)
                {
                    const SecondDatatype& second = second_distribution[jdx];
                    const double current_distance = distance_fn(first, second);
                    if (current_distance < minimum_distance)
                    {
                        minimum_distance = current_distance;
                    }
                }
#ifdef ENABLE_PARALLEL_HAUSDORFF_DISTANCE
                #if defined(_OPENMP)
                const size_t current_thread_id = (size_t)omp_get_thread_num();
                #else
                const size_t current_thread_id = 0;
                #endif
#else
                const size_t current_thread_id = 0;
#endif
                if (minimum_distance > per_thread_storage[current_thread_id])
                {
                    per_thread_storage[current_thread_id] = minimum_distance;
                }
            }
            double maximum_minimum_distance = 0.0;
            for (size_t idx = 0; idx < per_thread_storage.size(); idx++)
            {
                const double temp_minimum_distance = per_thread_storage[idx];
                if (temp_minimum_distance > maximum_minimum_distance)
                {
                    maximum_minimum_distance = temp_minimum_distance;
                }
            }
            return maximum_minimum_distance;
        }
    };
}
#endif // SIMPLE_HAUSDORFF_DISTANCE_HPP