/**
 * \file dcs/testbed/wang2015_qoscloud_application_manager.hpp
 *
 * \brief Application manager based on the QoSCloud solution proposed in
 * (Wang et al.,2012) and (Wang et al.,2015).
 *
 * References
 * -# Lixi Wang, Jing Xu, and Ming Zhao,
 *    "Modeling VM Performance Interference with Fuzzy MIMO Model,"
 *    Proc. of the 7th International Workshop on Feedback Computing (FeedbackComputing, co-held with ICAC2012), 2012.
 * -# Lixi Wang, Jing Xu, and Ming Zhao,
 *    "QoS-driven Cloud Resource Management through Fuzzy Model Predictive Control,"
 *    Proc. of the 12th International Conference on Autonomic Computing (ICAC), 2015.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2016   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_WANG2015_QOSCLOUD_APPLICATION_MANAGER_HPP
#define DCS_TESTBED_WANG2015_QOSCLOUD_APPLICATION_MANAGER_HPP

#include <boost/algorithm/string/replace.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_expression.hpp>
#include <boost/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/round.hpp>
#include <dcs/math/traits/float.hpp>
#include <dcs/testbed/application_performance_category.hpp>
#include <dcs/testbed/base_application_manager.hpp>
#include <dcs/testbed/detail/matlab_utility.hpp>
//#include <dcs/testbed/data_estimators.hpp>
#include <dcs/testbed/virtual_machine_performance_category.hpp>
#include <deque>
#include <fl/anfis.h>
#include <fl/dataset.h>
#include <fl/Headers.h>
#include <fstream>
#include <limits>
#include <map>
#include <sstream>
#include <stdexcept>
#include <string>
#include <vector>

namespace dcs { namespace testbed {

namespace wang2015_qoscloud_detail {

template <typename RealT>
class matlab_output_consumer
{
    public: matlab_output_consumer()
    : ok_(true)
    {
    }

    public: void operator()(dcs::system::posix_process& matlab_process)
    {
        ok_ = true;

        if (!matlab_process.alive())
        {
            ok_ = false;
            errmsg_ = "MATLAB is not running";
            return;
        }

        std::istream& is = matlab_process.output_stream();

//DCS_DEBUG_TRACE("BEGIN parsing MATLAB output");//XXX

        bool parse_line = false;
        while (matlab_process.alive() && is.good())
        {
//DCS_DEBUG_TRACE("Going to read another line: GOOD: " << is.good() << " - EOF: " << is.eof() << " - FAIL: " << is.fail() << " - BAD: " << is.bad() << " - !(): " << !static_cast<bool>(is) << "");//XXX
            std::string line;

            std::getline(is, line);

//DCS_DEBUG_TRACE("Read from MATLAB --> " << line);//XXX

            if (line.find("???") != std::string::npos
                || line.find("Error:") != std::string::npos)
            {
                DCS_DEBUG_TRACE("An error is occurred while executing MATLAB: " << line);//XXX
                ok_ = false;
                errmsg_ = line;
                break;
            }

            if (parse_line)
            {
                if (line.find("[/dcs::testbed::wang2015_qoscloud_application_manager]") != std::string::npos)
                {
                    // The end of parsable lines
                    parse_line = false;
                }
                else
                {
                    typename std::string::size_type pos;

                    if ((pos = line.find("x=")) != std::string::npos)
                    {
                        detail::parse_matlab_str(line.substr(pos+2), x_);
//DCS_DEBUG_TRACE("Parsed as uopt=" << x_);//XXX
                    }
                    else if ((pos = line.find("fval=")) != std::string::npos)
                    {
                        detail::parse_matlab_str(line.substr(pos+5), fval_);
//DCS_DEBUG_TRACE("Parsed as fxopt=" << fval_);//XXX
                    }
                    else if ((pos = line.find("exitflag=")) != std::string::npos)
                    {
                        detail::parse_matlab_str(line.substr(pos+9), exitflag_);
//DCS_DEBUG_TRACE("Parsed as Uopt=" << exitflag_);//XXX
                    }
                }
            }
            else
            {
                if (line.find("[dcs::testbed::wang2015_qoscloud_application_manager]") != std::string::npos)
                {
                    // The beginning of parsable lines
                    parse_line = true;
                }
            }
//DCS_DEBUG_TRACE("Looping: GOOD: " << is.good() << " - EOF: " << is.eof() << " - FAIL: " << is.fail() << " - BAD: " << is.bad() << " - !(): " << !static_cast<bool>(is) << "");//XXX
        }

//DCS_DEBUG_TRACE("DONE WITH LOOPING");//XXX
//DCS_DEBUG_TRACE("END parsing MATLAB output");//XXX
//DCS_DEBUG_TRACE("IS state: " << is.good() << " - " << is.eof() << " - " << is.fail() << " - " << is.bad());//XXX
    }

    public: bool ok_;
    public: std::string errmsg_;
    public: boost::numeric::ublas::vector<RealT> x_;
    public: RealT fval_;
    public: int exitflag_;
}; // matlab_output_consumer

} // Namespace wang2015_qoscloud_detail

/**
 * \brief Application manager based on the QoS Cloud solution proposed in
 *  (Wang et al.,2012) and (Wang et al.,2015).
 *
 * Parameters:
 *  --- TODO ---
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template <typename TraitsT>
class wang2015_qoscloud_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;
    public: typedef typename traits_type::uint_type uint_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_tol;

    public: wang2015_qoscloud_application_manager()
    : output_order_(1),
//      prediction_horizon_(1),
//      control_horizon_(1),
      forget_factor_(0.9),
      mpc_tracking_weight_(1),
      mpc_control_weight_(1),
      num_inputs_(0),
      num_outputs_(0),
      use_prebuilt_fis_(false),
      p_anfis_eng_(new fl::anfis::Engine()),
      p_anfis_builder_(new fl::SubtractiveClusteringFisBuilder<fl::anfis::Engine>()),
      p_anfis_trainer_(new fl::anfis::LeastSquaresLearningAlgorithm()),
	  beta_(0.9),
      ctrl_count_(0),
      ctrl_skip_count_(0),
      ctrl_fail_count_(0),
      anfis_initialized_(false)
    {
        init();
    }

    public: void output_order(std::size_t value)
    {
        output_order_ = value;
    }

    public: std::size_t output_order() const
    {
        return output_order_;
    }

//    public: void prediction_horizon(std::size_t value)
//    {
//        prediction_horizon_ = value;
//    }

//    public: std::size_t prediction_horizon() const
//    {
//        return prediction_horizon_;
//    }

//    public: void control_horizon(std::size_t value)
//    {
//        control_horizon_ = value;
//    }

//    public: std::size_t control_horizon() const
//    {
//        return control_horizon_;
//    }

    public: void forgetting_factor(real_type value)
    {
        forget_factor_ = value;
    }

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

    public: void mpc_tracking_weight(real_type value)
    {
        mpc_tracking_weight_ = value;
    }

    public: real_type mpc_tracking_weight() const
    {
        return mpc_tracking_weight_;
    }

    public: void mpc_control_weight(real_type value)
    {
        mpc_control_weight_ = value;
    }

    public: real_type mpc_control_weight() const
    {
        return mpc_control_weight_;
    }

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

    public: void use_prebuilt_anfis(bool value)
    {
        use_prebuilt_fis_ = value;
    }

    public: void prebuilt_anfis_file(std::string const& fname)
    {
        prebuilt_fis_fname_ = fname;
    }

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

        vm_perf_cats_.clear();
        vm_perf_cats_.push_back(cpu_util_virtual_machine_performance);
        vm_perf_cats_.push_back(memory_util_virtual_machine_performance);

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

        // 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 history
        in_shares_.clear();
        in_shares_.resize(nvms);
        in_utils_.clear();
        in_utils_.resize(nvms);
        out_perf_history_.clear();

        // Reset counters
        ctrl_count_ = ctrl_skip_count_
                    = ctrl_fail_count_
                    = 0;

        // Computes number of system inputs/outputs
        num_inputs_ = nvms*num_vm_perf_cats;
        num_outputs_ = std::distance(this->target_values().begin(), this->target_values().end());

        // Reset fuzzy and MPC controller
        this->init_anfis();
        this->init_mpc();

        // 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)\""
							<< ",\"MemCap_{" << vms[i]->id() << "}(k)\",\"MemShare_{" << vms[i]->id() << "}(k)\"";
            }
			for (std::size_t i = 0; i < nvms; ++i)
			{
				*p_dat_ofs_ << ",\"CPUShare_{" << vms[i]->id() << "}(k-1)\""
							<< ",\"MemShare_{" << 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,
                             ni = num_outputs_*output_order_ + num_inputs_;
                 i < ni;
                 ++i)
            {
                *p_dat_ofs_ << ",\"ANFIS Input_{" << i << "}\"";
            }
            for (std::size_t i = 0; i < num_outputs_; ++i)
            {
                *p_dat_ofs_ << ",\"ANFIS Output_{" << i << "}\"";
            }
            for (std::size_t i = 0; i < num_inputs_; ++i)
            {
                *p_dat_ofs_ << ",\"MPC Output_{" << i << "}\"";
            }
            *p_dat_ofs_ << ",\"# Controls\",\"# Skip Controls\",\"# Fail Controls\"";
            *p_dat_ofs_ << ",\"Elapsed Time\"";
            *p_dat_ofs_ << std::endl;
        }

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

                for (std::size_t j = 0; j < num_vm_perf_cats; ++j)
                {
                    const virtual_machine_performance_category cat = vm_perf_cats_[j];

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

            // Reset VM smoother
            for (::std::size_t i = 0; i < nvms; ++i)
            {
                const vm_pointer p_vm = vms[i];

                for (std::size_t j = 0; j < num_vm_perf_cats; ++j)
                {
                    const virtual_machine_performance_category cat = vm_perf_cats_[j];

                    this->data_smoother(cat, p_vm->id(), ::boost::make_shared< testbed::brown_single_exponential_smoother<real_type> >(beta_));
                }
            }
        }
    }

    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: " << ctrl_count_ << "/" << ctrl_skip_count_ << "/" << ctrl_fail_count_);

        if (p_dat_ofs_)
        {
            // Collect input values
            const in_sensor_iterator in_sens_end_it = in_sensors_.end();
            for (in_sensor_iterator in_sens_it = in_sensors_.begin();
                 in_sens_it != in_sens_end_it;
                 ++in_sens_it)
            {
                const virtual_machine_performance_category cat = in_sens_it->first;

                const typename in_sensor_map::mapped_type::const_iterator vm_end_it = in_sens_it->second.end();
                for (typename in_sensor_map::mapped_type::const_iterator vm_it = in_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_smoother(cat, vm_id).smooth(it->value());
                        }
                    }
                }
            }
        }

        // Collect output values
        const out_sensor_iterator out_sens_end_it = out_sensors_.end();
        for (out_sensor_iterator out_sens_it = out_sensors_.begin();
             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: " << ctrl_count_ << "/" << ctrl_skip_count_ << "/" << ctrl_fail_count_);
    }

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

        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: " << ctrl_count_ << "/" << ctrl_skip_count_ << "/" << ctrl_fail_count_);

        boost::timer::cpu_timer cpu_timer;

        const std::size_t num_vm_perf_cats = vm_perf_cats_.size();

        ++ctrl_count_;

        bool skip_ctrl = false;
        bool skip_collect = false;

        std::vector<real_type> new_xshares;
		std::map<virtual_machine_performance_category,std::vector<real_type> > old_xshares;

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

        // Makes sure there is some data to collect for control
        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)
            {
                    skip_ctrl = true;
                    skip_collect = true;
                    DCS_DEBUG_TRACE("No output observation collected during the last control interval -> Skip control");
                    break;
            }
        }

        // Collects data for control
        if (!skip_collect)
        {
            for (std::size_t i = 0; i < nvms; ++i)
            {
                for (std::size_t j = 0; j < num_vm_perf_cats; ++j)
                {
                    const virtual_machine_performance_category cat = vm_perf_cats_[j];
                    const vm_pointer p_vm = vms[i];

                    real_type c = 0;
                    switch (cat)
                    {
                        case cpu_util_virtual_machine_performance:
                            c = p_vm->cpu_share();
                            break;
                        case memory_util_virtual_machine_performance:
                            c = p_vm->memory_share();
                            break;
                    }
                    in_shares_[i][cat] = c;
                    in_utils_[i][cat] = this->data_smoother(cat, p_vm->id()).forecast(0);
DCS_DEBUG_TRACE("VM " << p_vm->id() << " - Performance Category: " << cat << " - Uhat(k): " << in_utils_[i].at(cat) << " - C(k): " << c);//XXX
                }
            }

            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
                const real_type yh = this->data_estimator(cat).estimate();
                const real_type yr = this->target_value(cat);

#ifdef DCS_DEBUG
                real_type err = 0;
                switch (cat)
                {
                    case response_time_application_performance:
                        err = (yr-yh)/yr;
                        break;
                    case throughput_application_performance:
                        err = (yh-yr)/yr;
                        break;
                }
DCS_DEBUG_TRACE("APP Performance Category: " << cat << " - Yhat(k): " << yh << " - R: " << yr << " -> E(k+1): " << err);//XXX
#endif // DCS_DEBUG

                // NOTE: we need to collect (output_order_+1) number of output
                //       observations in order to form input/output data needed
                //       to train the ANFIS model.

//[XXX]
{
std::cerr << "OUT_PERF_HISTORY - BEFORE: [";
for (std::size_t k = 0; k < out_perf_history_[cat].size(); ++k)
{
    std::cerr << ", " << out_perf_history_[cat].at(k);
}
std::cerr << "]" << std::endl;
}
//[/XXX]

                if (out_perf_history_[cat].size() >= (output_order_+1))
                {
                    out_perf_history_[cat].pop_front();
                }
                out_perf_history_[cat].push_back(yh);

//[XXX]
{
std::cerr << "OUT_PERF_HISTORY - AFTER: [";
for (std::size_t k = 0; k < out_perf_history_[cat].size(); ++k)
{
    std::cerr << ", " << out_perf_history_[cat].at(k);
}
std::cerr << "]" << std::endl;
}
//[/XXX]

                if (out_perf_history_[cat].size() < (output_order_+1))
                {
                    // Not enough outputs have been collected -> not ready for control yet
                    skip_ctrl = true;
                }

#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 (ctrl_count_ <= control_warmup_size)
		{
			skip_ctrl = true;
		}

        if (!skip_ctrl)
        {
            // Update ANFIS model

            this->update_anfis_model();

            if (!anfis_initialized_)
            {
                skip_ctrl = true;
            }
        }

        if (!skip_ctrl)
        {
            // Perform MPC control

            bool ok = false;

            try
            {
                new_xshares = this->perform_mpc_control();

                ok = true;
            }
            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 control results
            if (ok)
            {
                std::size_t k = 0;
                for (std::size_t i = 0; i < nvms; ++i)
                {
                    vm_pointer p_vm = vms[i];

                    for (std::size_t j = 0; j < num_vm_perf_cats; ++j)
                    {
                        const virtual_machine_performance_category cat = vm_perf_cats_[j];

                        real_type old_share = 0;
                        switch (cat)
                        {
                            case cpu_util_virtual_machine_performance:
                                old_share = p_vm->cpu_share();
                                break;
                            case memory_util_virtual_machine_performance:
                                old_share = p_vm->memory_share();
                                break;
                        }
                        old_xshares[cat].push_back(old_share);

                        real_type new_share = std::max(std::min(new_xshares[k], 1.0), 0.0);
					    new_share = dcs::math::round(new_share/resource_share_tol)*resource_share_tol;

                        DCS_DEBUG_TRACE("VM '" << p_vm->id() << "' - Performance Category: " << cat << " - 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, resource_share_tol))
                        {
                            switch (cat)
                            {
                                case cpu_util_virtual_machine_performance:
                                    p_vm->cpu_share(new_share);
                                    break;
                                case memory_util_virtual_machine_performance:
                                    p_vm->memory_share(new_share);
                                    break;
                            }
                            new_xshares[k] = new_share;
DCS_DEBUG_TRACE("VM " << vms[i]->id() << ", Performance Category: " << cat << " -> C(k+1): " << new_share);//XXX
                        }
                        else
                        {
                            new_xshares[k] = old_share;
DCS_DEBUG_TRACE("VM " << vms[i]->id() << ", Performance Category: " << cat << " -> C(k+1): not set!");//XXX
                        }

                        ++k;
                    }
                }
DCS_DEBUG_TRACE("Control applied");//XXX
            }
            else
            {
                ++ctrl_fail_count_;

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

        cpu_timer.stop();

        // Export to file
        if (p_dat_ofs_)
        {
			// Initialize data structures if needed

			if (new_xshares.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_xshares.push_back(p_vm->cpu_share());
					new_xshares.push_back(p_vm->memory_share());
				}
			}
			if (old_xshares.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_xshares[cpu_util_virtual_machine_performance].push_back(p_vm->cpu_share());
					old_xshares[memory_util_virtual_machine_performance].push_back(p_vm->memory_share());
				}
			}

            // Write to data file

            *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_vm->memory_cap() << "," << p_vm->memory_share();
            }
            *p_dat_ofs_ << ",";
            for (std::size_t i = 0; i < nvms; ++i)
            {
                if (i != 0)
                {
                    *p_dat_ofs_ << ",";
                }
                *p_dat_ofs_ << old_xshares.at(cpu_util_virtual_machine_performance)[i]
                            << "," << old_xshares.at(memory_util_virtual_machine_performance)[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_ << ",";
                }
				for (std::size_t j = 0; j < num_vm_perf_cats; ++j)
				{
					const virtual_machine_performance_category vm_cat = vm_perf_cats_[j];
					const real_type uh = (in_utils_.size() > 0 && in_utils_[i].count(vm_cat) > 0)
                                         ? in_utils_[i].at(vm_cat)
                                         : std::numeric_limits<real_type>::quiet_NaN();

                    if (j != 0)
                    {
                        *p_dat_ofs_ << ",";
                    }
					//*p_dat_ofs_ << this->data_smoother(vm_cat, p_vm->id()).forecast(0);
					*p_dat_ofs_ << uh;
				}
                //*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_ << ",";
            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;
                real_type err = 0;
                switch (cat)
                {
                    case response_time_application_performance:
                        err = (yr-yh)/yr;
                        break;
                    case throughput_application_performance:
                        err = (yh-yr)/yr;
                        break;
                }
                *p_dat_ofs_ << yr << "," << yh << "," << err;
            }
            for (std::size_t i = 0,
                             ni = p_anfis_eng_->numberOfInputVariables();
                 i < ni;
                 ++i)
            {
                *p_dat_ofs_ << "," << p_anfis_eng_->getInputVariable(i)->getValue();
            }
            for (std::size_t i = 0,
                             ni = p_anfis_eng_->numberOfOutputVariables();
                 i < ni;
                 ++i)
            {
                *p_dat_ofs_ << "," << p_anfis_eng_->getOutputVariable(i)->getValue();
            }
            if (new_xshares.size() > 0)
            {
                for (std::size_t i = 0; i < num_inputs_; ++i)
                {
                    *p_dat_ofs_ << "," << new_xshares[i];
                }
            }
            else
            {
                for (std::size_t i = 0; i < num_inputs_; ++i)
                {
                    *p_dat_ofs_ << ","; // << std::numeric_limits<real_type>::quiet_NaN();
                }
            }
            *p_dat_ofs_ << "," << ctrl_count_ << "," << ctrl_skip_count_ << "," << ctrl_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: " << ctrl_count_ << "/" << ctrl_skip_count_ << "/" << ctrl_fail_count_);
    }

    private: void init()
    {
        //vm_perf_cats_.clear();
        //vm_perf_cats_.push_back(cpu_util_virtual_machine_performance);
        //vm_perf_cats_.push_back(memory_util_virtual_machine_performance);

        // Reset fuzzy and MPC controller
        this->init_anfis();
        this->init_mpc();
    }

    private: void init_anfis()
    {
        DCS_DEBUG_ASSERT( p_anfis_builder_ );
        DCS_DEBUG_ASSERT( p_anfis_eng_ );
        DCS_DEBUG_ASSERT( p_anfis_trainer_ );

        fl::fuzzylite::setDecimals(std::numeric_limits<fl::scalar>::digits10+1);
        fl::fuzzylite::setMachEps(std::numeric_limits<fl::scalar>::epsilon());

        p_anfis_eng_->clear();

        if (use_prebuilt_fis_)
        {
            //std::ostringstream oss;
            //oss << "experiments/data/rubis-wang2015_qoscloud-order"
            //    << "_out_" << output_order_
            //    //<< "-users_30_45_15-sig_mesh_a05_f12_p3_b05_lb02_ub1"
            //    //<< "-users_45-sig_mesh_a05_f12_p3_b05_lb02_ub1"
            //    << "-users_45-sig_constant_v1_lb02_ub1"
            //    << "-anfis_trained.fll";
            //const std::string prebuilt_fis_fname(oss.str());

            fl::FllImporter fllImp;
            fl::Engine* p_eng = fllImp.fromFile(prebuilt_fis_fname_);

            DCS_ASSERT(p_eng,
                       DCS_EXCEPTION_THROW(std::runtime_error, "Unable to initialize ANFIS fuzzy controller"));

            *p_anfis_eng_ = fl::anfis::Engine(*p_eng);

            delete p_eng;

            anfis_initialized_ = true;
        }
        else
        {
            anfis_initialized_ = false;
        }

        p_anfis_trainer_->setIsOnline(true);
        p_anfis_trainer_->setForgettingFactor(forget_factor_);
        p_anfis_trainer_->setEngine(p_anfis_eng_.get());
        p_anfis_trainer_->reset();

        const std::size_t nxi = output_order_*num_outputs_;
        anfis_trainset_ = fl::DataSet<real_type>(nxi+num_inputs_, num_outputs_);
    }

    private: void init_mpc()
    {
        // Empty
    }

    private: void update_anfis_model()
    {
        //
        // Builds the input vector for the ANFIS model
        // The input vector is formed by the concatenation of two vectors $\xi(k)$ and $u(k)$ where:
        // - $\xi(k)$ is the regressor vector at time $k$, with
        //   \[
        //    \xi(k) = [y_1(k), ..., y_1(k-n_a+1), ..., y_{n_y}(k), ...,  y_{n_y}(k-n_a+1)]^T
        //   \]
        //   where:
        //   * $U_{i,j}(k)$ and $C_{i,j}(k)$ are the utilization and capacity of resource $j$ of tier $i$ at time $k$, respectively,
        //   * $n_a$ and $n_b$ are the input and output order of the model, respectively.
        // - $u(k)$ is the system input vector at time $k$, with:
        //   \[
        //    u(k) = [C_{1,1}(k), ..., C_{1,R}(k), ..., C_{T,1}(k), ..., C_{T,R}(k)]^T
        //   \]
        // This is equivalent to see the ANFIS model as a NARX(n_a,n_b) model
        // \[
        //  y(k+1) = R(\xi(k),u(k))
        // \]
        // where $y(k)$ is the output of the ANFIS model at time $k$.
        //

        namespace ublas = boost::numeric::ublas;

        typedef typename base_type::target_value_map::const_iterator target_iterator;

        // Forms the $\xi(k-1)$, $u(k-1)$, and $y(k)$ vectors
        const std::size_t num_vm_perf_cats = vm_perf_cats_.size();
        const std::size_t nvms = this->app().num_vms();
        const std::size_t nxi = output_order_*num_outputs_;
        ublas::vector<real_type> u(num_inputs_, 0);
        ublas::vector<real_type> xi(nxi, 0);
        //ublas::vector<real_type> y(num_outputs_, 0);
        std::size_t u_ix = 0;
        std::size_t xi_ix = 0;
        //std::size_t y_ix = 0;
        ublas::vector<real_type> u_train(num_inputs_, 0);
        ublas::vector<real_type> xi_train(nxi, 0);
        ublas::vector<real_type> y_train(num_outputs_, 0);
        std::size_t u_ix_train = 0;
        std::size_t xi_ix_train = 0;
        std::size_t y_ix_train = 0;
DCS_DEBUG_TRACE("BUILDING XIs...");//XXX
        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;

            for (typename std::deque<real_type>::const_reverse_iterator val_it = out_perf_history_.at(cat).rbegin(),
                                                                        val_end_it = out_perf_history_.at(cat).rend();
                 val_it != val_end_it;
                 ++val_it)
            {
                if (val_it == out_perf_history_.at(cat).rbegin())
                {
                    y_train(y_ix_train++) = *val_it;
DCS_DEBUG_TRACE("Y_TRAIN[" << (y_ix_train-1) << "]: " << y_train(y_ix_train-1));//XXX
                }
                else
                {
                    xi_train(xi_ix_train++) = *val_it;
DCS_DEBUG_TRACE("XI_TRAIN[" << (xi_ix_train-1) << "]: " << xi_train(xi_ix_train-1));//XXX
                }
                if (xi_ix < nxi)
                {
                    xi(xi_ix++) = *val_it;
DCS_DEBUG_TRACE("XI[" << (xi_ix-1) << "]: " << xi(xi_ix-1));//XXX
                }
            }
        }
DCS_DEBUG_TRACE("BUILDING Us...");//XXX
        for (std::size_t i = 0; i < nvms; ++i)
        {
            for (std::size_t j = 0; j < num_vm_perf_cats; ++j)
            {
                const virtual_machine_performance_category cat = vm_perf_cats_[j];

#if 1 // Use shares as resource usage. This seems the solution chosen by Lama
                u_train(u_ix_train++) = in_shares_[i].at(cat);
                u(u_ix++) = in_shares_[i].at(cat);
#else // Use utilizations as resource usage
                //u_train(u_ix_train++) = in_utils_[i].at(cat);
                //u(u_ix++) = in_utils_[i].at(cat);
#endif
DCS_DEBUG_TRACE("U_TRAIN[" << (u_ix_train-1) << "]: " << u_train(u_ix_train-1));//XXX
DCS_DEBUG_TRACE("U[" << (u_ix-1) << "]: " << u(u_ix-1));//XXX
            }
        }

        // post conditions
        DCS_DEBUG_ASSERT( xi_ix_train == nxi );
        DCS_DEBUG_ASSERT( u_ix_train == num_inputs_ );
        DCS_DEBUG_ASSERT( y_ix_train == num_outputs_ );

        {
            ublas::vector<real_type> inputs(nxi+num_inputs_, 0);
            ublas::subrange(inputs, 0, nxi) = xi_train;
            ublas::subrange(inputs, nxi, inputs.size()) = u_train;
            fl::DataSetEntry<real_type> entry(inputs.begin(), inputs.end(), y_train.begin(), y_train.end());
            anfis_trainset_.add(entry);
//[XXX]
{
std::cerr << "ANFIS - TRAINING INSTANCE: <";
std::cerr << "IN: [";
for (std::size_t i = 0; i < inputs.size(); ++i)
{
    std::cerr << ", " << inputs[i];
}
std::cerr << "], OUT: [";
for (std::size_t i = 0; i < num_outputs_; ++i)
{
    std::cerr << ", " << y_train[i];
}
std::cerr << "]>" << std::endl;
}
//[/XXX]
        }

        if (anfis_initialized_)
        {
            // Train the ANFIS model
            real_type rmse = 0;
//[FIXME]
#if 1
            const std::size_t min_trainset_size_online = 1;
            const std::size_t min_trainset_size_offline = 10;
#else
            const std::size_t min_trainset_size_online = 10;
            const std::size_t min_trainset_size_offline = 10;
#endif
//[/FIXME]
            if ((p_anfis_trainer_->isOnline() && anfis_trainset_.size() >= min_trainset_size_online)
                || anfis_trainset_.size() >= min_trainset_size_offline)
            {
                //rmse = p_anfis_trainer_->train(anfis_trainset_);
                rmse = p_anfis_trainer_->trainSingleEpoch(anfis_trainset_);
//[XXX]
{
std::ostringstream oss;
oss << "wang2015_qoscloud_trainset_n" << ctrl_count_ << ".dat";
std::ofstream ofs(oss.str().c_str());
fl::detail::MatrixOutput(ofs, anfis_trainset_.data());
ofs.close();
}
//[/XXX]
                anfis_trainset_.clear();
DCS_DEBUG_TRACE("ANFIS TRAINED -> RMSE: " << rmse);//XXX
            }

            DCS_DEBUG_ASSERT( p_anfis_eng_->numberOfInputVariables() == (nxi+num_inputs_) );
            DCS_DEBUG_ASSERT( p_anfis_eng_->numberOfOutputVariables() == num_outputs_ );

            // Load the $\xi$ and $u$ vector into the ANFIS model
            for (std::size_t i = 0; i < nxi; ++i)
            {
                //p_anfis_eng_->getInputVariable(i)->setValue(xi_train(i));
                p_anfis_eng_->getInputVariable(i)->setValue(xi(i));
            }
            for (std::size_t i = 0; i < num_inputs_; ++i)
            {
                //p_anfis_eng_->getInputVariable(i+nxi)->setValue(u_train(i));
                p_anfis_eng_->getInputVariable(i+nxi)->setValue(u(i));
            }

            // Apply the inputs to the ANFIS model
            p_anfis_eng_->process();
//[XXX]
{
std::cerr << "ANFIS - PROCESS: <";
std::cerr << "IN: [";
for (std::size_t i = 0; i < p_anfis_eng_->numberOfInputVariables(); ++i)
{
    std::cerr << ", " << p_anfis_eng_->getInputVariable(i)->getValue();
}
std::cerr << "], OUT: [";
for (std::size_t i = 0; i < p_anfis_eng_->numberOfOutputVariables(); ++i)
{
    std::cerr << ", " << p_anfis_eng_->getOutputVariable(i)->getValue();
}
std::cerr << "]>" << std::endl;
}
//[/XXX]
        }
        else
        {
            // Build the ANFIS model
            if (anfis_trainset_.size() >= 200)
            {
                FL_unique_ptr<fl::anfis::Engine> p_eng = p_anfis_builder_->build(anfis_trainset_);
                *p_anfis_eng_ = *p_eng;
                p_anfis_eng_->build();
                anfis_initialized_ = true;
                real_type rmse = 0;
                rmse = p_anfis_trainer_->train(anfis_trainset_, 20);
DCS_DEBUG_TRACE("ANFIS TRAINED FIRST TIME -> RMSE: " << rmse);//XXX
                anfis_trainset_.clear();
            }
        }
    }

    private: std::vector<real_type> perform_mpc_control()
    {
        namespace ublas = boost::numeric::ublas;

        typedef typename base_type::target_value_map::const_iterator target_iterator;

        const std::size_t nxi = output_order_*num_outputs_;
        const std::size_t nu = num_inputs_;
        const std::size_t ny = p_anfis_eng_->numberOfOutputVariables();

        // Retrieves the $\xi$ and $u$ vector from the ANFIS model
        ublas::vector<real_type> xi(nxi, 0);
        ublas::vector<real_type> u(nu, 0);
        for (std::size_t i = 0,
                         ni = nxi+nu;
             i < ni;
             ++i)
        {
            if (i < nxi)
            {
                xi(i) = p_anfis_eng_->getInputVariable(i)->getValue();
            }
            else
            {
                u(i-nxi) = p_anfis_eng_->getInputVariable(i)->getValue();
            }
        }

        // Builds the vector of output reference values
        ublas::vector<real_type> yref(num_outputs_, 0);
        {
            std::size_t i = 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;

                // Compute a summary statistics of collected observation
                yref(i++) = this->target_value(cat);
            }
        }

        fl::FisExporter fisExp;
        ublas::vector<real_type> ydefaults(ny);
        // Cancel default output values since MATLAB don't use them (and raise an error if it finds them in the FIS file)
        for (std::size_t i = 0; i < ny; ++i)
        {
            ydefaults[i] = p_anfis_eng_->getOutputVariable(i)->getDefaultValue();
            p_anfis_eng_->getOutputVariable(i)->setDefaultValue(std::numeric_limits<real_type>::quiet_NaN());
        }
        std::string fis_str = fisExp.toString(p_anfis_eng_.get());
        // Restore default output values
        for (std::size_t i = 0; i < ny; ++i)
        {
            p_anfis_eng_->getOutputVariable(i)->setDefaultValue(ydefaults[i]);
        }
        boost::algorithm::replace_all(fis_str, "\n", "\\n");
        boost::algorithm::replace_all(fis_str, "'", "''");

        std::vector<std::string> matlab_args;

        matlab_args.push_back("-nodisplay");
        //matlab_args.push_back("-nojvm");
        matlab_args.push_back("-nodesktop");

        std::ostringstream oss;

        oss << "-r \""
            << " try"
            << "   xi = " << detail::to_matlab_str(xi) << ";"
            << "   u = " << detail::to_matlab_str(u) << ";"
            << "   yref = " << detail::to_matlab_str(yref) << ";"
            << "   ny = " << ny << ";"
            << "   nu = " << nu << ";"
            << "   nxi = " << nxi << ";"
            << "   Q = " << mpc_tracking_weight_ << "*eye(ny);"
            << "   R = " << mpc_control_weight_ << "*eye(nu);"
            << "   fisstr = sprintf('" << fis_str << "');"
            << "   fisfile = [tempname, '.fis'];"
            //<< "   save(fisfile, 'fisstr', '-ascii');"
            << "   fd = fopen(fisfile, 'w');"
            << "   fwrite(fd, fisstr);"
            << "   fclose(fd);"
            << "   fis = readfis(fisfile);"
            << "   delete(fisfile);"
            << "   nvar = nu+nxi;"
            << "   wsqr = @(x,w) x'*w*x;"
            << "   objfun = @(x) (wsqr((evalfis(x',fis)-yref),Q) + wsqr((x(nxi+1:end)'-u),R));" // We need to transpose x since it is represented as a row vector
            << "   x0 = [xi; u];"
            << "   gaopts = gaoptimset('InitialPopulation',[x0'], 'TimeLimit', " << (this->control_time()*0.5) << ");" // We need to transpose x0 since it is represented as a column vector
            << "   LB = zeros(size(x0));"
            << "   UB = ones(size(x0));"
            << "   rng(1, 'twister');" // For reproducibility
            << "   [x,fval,exitflag] = ga(objfun, nvar, [], [], [], [], LB, UB, [], gaopts);"
            << "   format long;"
            << "   disp('--- [dcs::testbed::wang2015_qoscloud_application_manager] ---');"
            << "   disp(['x=', mat2str(x)]);"
            << "   disp(['fval=', num2str(fval)]);"
            << "   disp(['exitflag=', num2str(exitflag)]);"
            << "   disp('--- [/dcs::testbed::wang2015_qoscloud_application_manager] ---');"
            << " catch me,"
            << "  disp(['??? Error: ', me.message]);"
            << " end;"
            << " quit force;"
            << "\"";

        matlab_args.push_back(oss.str());

        wang2015_qoscloud_detail::matlab_output_consumer<real_type> matlab_consumer;
        detail::run_matlab("matlab", matlab_args.begin(), matlab_args.end(), matlab_consumer);

        if (!matlab_consumer.ok_)
        {
            DCS_EXCEPTION_THROW(std::runtime_error, matlab_consumer.errmsg_);
        }

        ublas::vector<real_type> u_opt;
        int exit_status;
        u_opt = matlab_consumer.x_;
        exit_status = matlab_consumer.exitflag_;
DCS_DEBUG_TRACE("Optimal control from GA: " << u_opt);///XXX
DCS_DEBUG_TRACE("Exit status from GA: " << exit_status);///XXX

        // Check exit status
        // From MATLAB documentation:
        //  1   Without nonlinear constraints — Average cumulative change in value of the fitness function over StallGenLimit generations is less than TolFun, and the constraint violation is less than TolCon.
        //      With nonlinear constraints — Magnitude of the complementarity measure (see Complementarity Measure) is less than sqrt(TolCon), the subproblem is solved using a tolerance less than TolFun, and the constraint violation is less than TolCon.
        //  2   Fitness limit reached and the constraint violation is less than TolCon.
        //  3   Value of the fitness function did not change in StallGenLimit generations and the constraint violation is less than TolCon.
        //  4   Magnitude of step smaller than machine precision and the constraint violation is less than TolCon.
        //  5   Minimum fitness limit FitnessLimit reached and the constraint violation is less than TolCon.
        //  0   Maximum number of generations Generations exceeded.
        // -1   Optimization terminated by an output function or plot function.
        // -2   No feasible point found.
        // -4   Stall time limit StallTimeLimit exceeded.
        // -5   Time limit TimeLimit exceeded.
        if (exit_status == -2)
        {
            DCS_EXCEPTION_THROW(std::runtime_error, "(GA optimization) No feasible point found");
        }

        return std::vector<real_type>(u_opt.begin(), u_opt.end());
    }

    private: std::size_t output_order_; ///< The number of past outputs to consider that can influence the current output
    //private: std::size_t prediction_horizon_; ///< The prediction horizon used by MPC
    //private: std::size_t control_horizon_; ///< The prediction horizon used by MPC
    private: real_type forget_factor_; ///< The forgetting factor used by the wRLS algorithm
    private: real_type mpc_tracking_weight_; ///< The weight to apply to the tracking part of the MPC objective function
    private: real_type mpc_control_weight_; ///< The weight to apply to the tracking part of the MPC objective function
    private: std::size_t num_inputs_; ///< The number of system inputs
    private: std::size_t num_outputs_; ///< The number of system outputs
    private: bool use_prebuilt_fis_; ///< `true` if ANFIS is initialized from a previously built FIS
    private: ::boost::shared_ptr<fl::anfis::Engine> p_anfis_eng_; ///< The fuzzy modeling engine based on ANFIS
    private: boost::shared_ptr<fl::SubtractiveClusteringFisBuilder<fl::anfis::Engine> > p_anfis_builder_; ///< Builder for the ANFIS model
    private: boost::shared_ptr<fl::anfis::LeastSquaresLearningAlgorithm> p_anfis_trainer_; ///< Training algorithm for the ANFIS model
    private: real_type beta_; ///< Smoothing factor for VM CPU and Memory utilization
    private: std::size_t ctrl_count_; ///< Number of times control function has been invoked
    private: std::size_t ctrl_skip_count_; ///< Number of times control has been skipped
    private: std::size_t ctrl_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: std::string prebuilt_fis_fname_;
    private: ::boost::shared_ptr< std::ofstream > p_dat_ofs_;
    private: std::vector<virtual_machine_performance_category> vm_perf_cats_;
    //private: std::vector<application_performance_category> app_perf_cats_;
    private: std::vector< std::map<virtual_machine_performance_category, real_type> > in_shares_;
    private: std::vector< std::map<virtual_machine_performance_category, real_type> > in_utils_;
    private: std::map< application_performance_category, std::deque<real_type> > out_perf_history_;
    private: bool anfis_initialized_;
    private: fl::DataSet<real_type> anfis_trainset_;
}; // wang2015_qoscloud_application_manager

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

template <typename T>
const float wang2015_qoscloud_application_manager<T>::resource_share_tol = 1e-2;

}} // Namespace dcs::testbed

#endif // DCS_TESTBED_WANG2015_QOSCLOUD_APPLICATION_MANAGER_HPP

/* vim: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */


#ifndef __HMM_HPP__
#define __HMM_HPP__

#include "utility.hpp"
#include <iostream>
#include <string>
#include <unordered_map>
#include <vector>
#include <fstream>
#include <boost/functional/hash.hpp>
#include <boost/bimap.hpp>
#include <math.h>

	auto total = std::chrono::duration<double, std::milli>{0};

template <typename viterbi_T>
class HMM {
public:
	using Viterbi_type = viterbi_T;
	using Model_type = typename Viterbi_type::Model;

public:
	HMM() = delete;
	HMM(const std::string& filename) : m_viterbi(mlTrain(filename)) {}

	HMM(const HMM& other) = default;
	HMM& operator=(const HMM& rhs) = default;
	std::vector<std::string> infer(const std::vector<std::string>& ws) {
		auto is =
			std::vector<typename Viterbi_type::Emission_type>(ws.size(), 0);

		std::transform(ws.cbegin(), ws.cend(), is.begin(),
					   [this](const std::string& w) {
						   auto i = emissionBijection.left.find(w);
						   if (i == emissionBijection.left.end()) {
							   std::cerr << w << " not covered.\n";
							   return 0;
						   }
						   return i->second;
					   });

		auto before = std::chrono::high_resolution_clock::now();

		auto iouts = m_viterbi.infer(is);

		total += std::chrono::high_resolution_clock::now() - before;

		auto outs = std::vector<std::string>(ws.size(), "");

		std::transform(iouts.cbegin(), iouts.cend(), outs.begin(),
					   [this](const typename Viterbi_type::Label_type& l) {
						   return labelBijection.right.find(l)->second;
					   });

		return outs;
	}  // end infer

private:
	using stringmap = std::unordered_map<std::string, int>;
	using stringpair = std::pair<std::string, std::string>;
	using pairmap =
		std::unordered_map<stringpair, int, boost::hash<stringpair>>;
	using bijection = boost::bimap<std::string, int>;
	using bijectionPair = bijection::value_type;

private:
	Model_type mlTrain(const std::string& filename) {
		auto file = std::ifstream{filename};

		bool isNewSent = true;
		stringmap initialLabelCount;
		stringmap labelCount;
		pairmap transitionCount;
		pairmap emissionCount;
		std::string prevLabel;
		int nextLabel = 0;
		int nextEmission = 0;
		int numSents = 0;

		for (std::string label, emission = "";
			 std::getline(file, emission) && std::getline(file, label);) {
			// if its two new lines in the file emission will be empty
			// note: we require two newlines at end of file (or numSents is
			// wrong)
			if (emission == "") {
				isNewSent = true;
				numSents += 1;
				emission = label;
				if (!std::getline(file, label)) break;
//			} else {
			}

				// bijections
				if (labelBijection.left.find(label) ==
					labelBijection.left.end()) {
					labelBijection.insert(bijectionPair(label, nextLabel++));
				}

				if (emissionBijection.left.find(emission) ==
					emissionBijection.left.end()) {
					emissionBijection.insert(
						bijectionPair(emission, nextEmission++));
				}

				if (isNewSent) {
					isNewSent = false;
					initialLabelCount[label] += 1;
					labelCount[label] += 1;
					prevLabel = label;
				} else {
					// it is not a new sentence, so prevLabel is valid
					transitionCount[std::make_pair(prevLabel, label)] += 1;
					labelCount[label] += 1;
				}

				// we can always count emissions
				emissionCount[std::make_pair(label, emission)] += 1;
				prevLabel = label;
		}  // end for

		// now comes the ml-estimate step
		auto maxLabel = nextLabel;
		auto maxEmission = nextEmission;
/*		std::cout << "Labels: " << maxLabel << "\nEmissions: " << maxEmission
				  << "\nSentences: " << numSents << std::endl;
*/
		auto m = Model_type{maxLabel, maxEmission, std::log(0.0f)};

/*		for (auto i = 0; i < maxLabel; ++i) {
			m.start[i] =
				log(initialLabelCount[labelBijection.right.find(i)->second]) -
				log(numSents);
		}
*/

		for(const auto& l : initialLabelCount) {
			m.setStart(labelBijection.left.find(l.first)->second, log(l.second) - log(numSents));
		}

		// transitions
		for(const auto& t : transitionCount) {
			auto i = labelBijection.left.find(t.first.first)->second;
			auto j = labelBijection.left.find(t.first.second)->second;
			m.setTransition(i, j, log(t.second) - log(labelCount[t.first.first]));
		}

/*
		for (auto i = 0; i < maxLabel; ++i) {
			for (auto j = 0; j < maxLabel; ++j) {
				m.setTransition(i, j,
					log(transitionCount[std::make_pair(
						labelBijection.right.find(i)->second,
						labelBijection.right.find(j)->second)]) -
					log(labelCount[labelBijection.right.find(i)->second]));
			}
		}
*/
		// emissions
/*
		for(auto i = 0; i < maxLabel; ++i) {
		for(auto j = 0; j < nextEmission-1; ++j) {
			m.emissions(i, j) = log(emissionCount[std::make_pair(labelBijection.right.find(i)->second, emissionBijection.right.find(j)->second)]) - log(labelCount[labelBijection.right.find(i)->second]);
		}
		}
*/
		for ( auto e = emissionCount.begin(); e != emissionCount.end(); ++e) {
			m.setEmission(labelBijection.left.find(e->first.first)->second,
						emissionBijection.left.find(e->first.second)->second,
						log(e->second) - log(labelCount[e->first.first]));
		}

		return std::move(m);
	}

private:
	bijection labelBijection;
	bijection emissionBijection;
	Viterbi_type m_viterbi;
};  // end class HMM

#endif


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

#include "../util/types_and_headers.h"
#include "../linalg/lapack_wrapper.h"
#include "../dtensor/dtensor_all.h"
#include "../qtensor/qtensor_all.h"
#include "../mps/mps_all.h"

#include "../models/sites/spinhalf.h"
#include "../models/sites/electron.h"
#include "../models/hams/Heisenberg.h"

#include "../algos/dmrg/dmrg.h"

#include "../util/timer.h"

#include "../models/hams/AutoMPO.h"

#include "../models/lattice/square.h"
#include "../models/lattice/triangular.h"

#include <ctf.hpp>
#include <sys/stat.h>
using namespace std;
inline bool check_existence(const string& name);

//#include <Eigen/Dense>
#include "hdf5.h"

filebuf fbuf;
ostream perr(&fbuf);
filebuf fbufo;
ostream pout(&fbufo);

template <typename T, template <typename,unsigned> class TensorType>
void fixTensors(TensorType<T,1>& psi,TensorType<T,2>& H){
  uint_vec perm;
  string Link_name_pref = "ID"+to_string(psi._id)+"Link";
  string Site_name_pref = "Site";
  for(unsigned l=0;l<psi.length;l++){
    string left_link_name  = Link_name_pref+to_string(l);
    string right_link_name = Link_name_pref+to_string(l+1);
    string site_name       = Site_name_pref+to_string(l);
    vector<qtensor_index> inds(3);
    for(auto& ix : psi.A[l].idx_set){
      if(ix.name()==left_link_name) inds[0] = ix;
      else if(ix.name()==site_name) inds[1] = ix;
      else if(ix.name()==right_link_name) inds[2] = ix;
    }
    find_index_permutation(psi.A[l].idx_set,inds, perm);
    psi.A[l].permute(perm);
  }
  //check if we need to dagger H
  string site_name = Site_name_pref+to_string(0);
  auto& psiSet = psi.A[0].idx_set;
  auto& HSet   = H.A[0].idx_set;
  auto it  = find_if(psiSet.begin(), psiSet.end(), [&](qtensor_index& qi){ return qi.name()==site_name;});
  auto itH = find_if(HSet.begin(),   HSet.end(),   [&](qtensor_index& qi){ return (qi.name()==site_name) && (qi.level()==0);});
  assert(it!=psiSet.end() && itH != HSet.end());
  if( it->arrow() == itH->arrow()){
    for(auto& Ai: H.A) Ai.dag();
  }

}

int main(int argc, char **argv)
{
  MPI_Init(&argc, &argv);
  {
    CTF::World world(argc,argv);
    if (world.rank == 0){
      perr.rdbuf(cerr.rdbuf());
      pout.rdbuf(cout.rdbuf());
      perr.precision(12);
      pout.precision(12);
    }
    if(argc==1) {
      perr<<"Error: Need filename!"<<endl;
      assert(1==2);
    }
    string inputName = string(argv[1]);
    ifstream in;
    in.open(inputName);
    assert(in);
    string line; stringstream linest;

    /*
     *              input file format
     * type
     * Nx Ny
     * J1 J2
     * fname
     * prefix
     * nsweeps
     * maxm maxm maxm
     * cutoff cutoff cutoff
     * restart restart restart
     *
     */

    string type; getline(in,type);
    getline(in,line); linest.clear(); linest.str(line);
    int Nx,Ny;
    linest >> Nx >> Ny;
    auto N = Nx*Ny;
    double J1,J2;
    getline(in,line); linest.clear(); linest.str(line);
    perr << "J1 line=" << line << endl;
    linest >> J1 >> J2;
    string fname="";
    getline(in,line); if(!line.empty()) fname=line;
    string pref="";
    getline(in,line); if(!line.empty()) pref=line;
    int nsweeps = 0;
    getline(in,line); linest.clear(); linest.str(line);
    linest >> nsweeps;

    vector<int> maxm;
    vector<double> cutoff;
    vector<int> max_restart;
    getline(in,line); linest.clear(); linest.str(line);
    while(getline(linest,line,' ')){
      maxm.push_back(std::stoi(line));
    }
    getline(in,line); linest.clear(); linest.str(line);
    while(getline(linest,line,' ')){
      cutoff.push_back(std::stof(line));
    }
    getline(in,line); linest.clear(); linest.str(line);
    while(getline(linest,line,' ')){
      max_restart.push_back(std::stoi(line));
    }
    in.close();

    if(world.rank==0)
      printf("type=%s N=(%i,%i) J1=%f J2=%f\nfile=%s prefix=%s\nnsweeps=%i \n",
          type.c_str(),Nx,Ny,J1,J2,fname.c_str(),pref.c_str(),nsweeps);
    perr<<"maxm:";        for(auto m: maxm)        perr<<m<<" "; perr<<"\n";
    perr<<"cutoff:";      for(auto c: cutoff)      perr<<c<<" "; perr<<"\n";
    perr<<"max_restart:"; for(auto r: max_restart) perr<<r<<" "; perr<<"\n";
    //spinhalf sites(N);
    electron sites(N);
    str_vec ps;
    for (size_t i = 0; i < sites.N(); i++) {
      if(i%2==0)
        ps.push_back("Dn");
      else
        ps.push_back("Up");
    }

    AutoMPO ampo(sites);
    bool yperiodic = true;
    auto lattice = triangularLattice(Nx,Ny,yperiodic);
    auto t = 1.0;
    auto U = J2;

    for(auto bnd : lattice){
      //hopping terms in the 1DEG
      ampo+=-t,"Cdagup",bnd.s1-1,"Cup",bnd.s2-1;
      ampo+=-t,"Cdagdn",bnd.s1-1,"Cdn",bnd.s2-1;
      ampo+=-t,"Cdagup",bnd.s2-1,"Cup",bnd.s1-1;
      ampo+=-t,"Cdagdn",bnd.s2-1,"Cdn",bnd.s1-1;
    }
    //-------------------------
    for(int i=0;i<N;i++)
      ampo+=U,"Nupdn",i;

    if(type=="d"){
      pout << "\n" << "Dense MPS DMRG (Linear Heisenberg)" << '\n';

      MPS<double> psi(&sites,ps);
      //psi.load(fname); //TODO: fix pref
      psi.print();
      MPO< double > H;
      Heisenberg< double > HB(&sites);
      HB.buildHam(H);
      dmrg(psi, H, nsweeps, maxm, cutoff, max_restart);
      psi.print();

    }
    if(type=="q"){
      pout << "\n" << "qMPS Fermionic DMRG" << '\n';

      qMPS< double > psi(&sites,ps);
      bool yperiodic=true;
      auto lattice = triangularLattice(Nx,Ny,yperiodic);

      auto t = 1.0;
      auto U = J2;

      AutoMPO ampo(sites);
      for(auto bnd : lattice){
        //hopping terms in the 1DEG
        ampo+=-t,"Cdagup",bnd.s1-1,"Cup",bnd.s2-1;
        ampo+=-t,"Cdagdn",bnd.s1-1,"Cdn",bnd.s2-1;
        ampo+=-t,"Cdagup",bnd.s2-1,"Cup",bnd.s1-1;
        ampo+=-t,"Cdagdn",bnd.s2-1,"Cdn",bnd.s1-1;
      }
      //-------------------------
      for(int i=0;i<N;i++)
        ampo+=U,"Nupdn",i;

      string postfix = to_string(Nx)+"x"+to_string(Ny)+"_U"+to_string(U);
      auto sp = "psi_"+postfix;
      qMPO< double > H;
      Heisenberg< double > HB(&sites);
      perr<<"making H"<<endl;
      HB.buildHam(ampo,H);

      psi.load(fname,pref);
      psi.print();

      assert(H._id != psi._id);
      fixTensors(psi,H);

      dmrg(psi, H, nsweeps, maxm, cutoff, max_restart);

    }
    if(type=="qs"){
      pout << "\n" << "qsMPS Fermionic DMRG" << '\n';

      qsMPS<double> psi(&sites,ps);
      psi.load(fname,pref);
      psi.print();
      qMPO< double > H;
      Heisenberg< double > HB(&sites);
      HB.buildHam(ampo,H);
      qsMPO< double > Hq = H;

      assert(H._id != psi._id);
      fixTensors(psi,Hq);

      dmrg(psi, Hq, nsweeps, maxm, cutoff, max_restart);

      psi.print();
      for(int l=0;l<N;l++){
        auto& Al = psi.A[l];
        perr<<Al._T.nnz_tot<<","<<(double)Al._T.nnz_tot/(Al._T.get_tot_size(false))<<endl;
        for(size_t i=0;i<Al.block_index_qd.size();i++)
          perr<<"   ("<<Al.block_index_qd[i][0]<<","
              << Al.block_index_qd[i][1]<<","<<Al.block_index_qd[i][2]<<")\n";
      }
    }
    if(type=="qToqs"){
      pout << "\n" << "q to qsMPS Fermionic DMRG" << '\n';

      qMPO< double > H;
      Heisenberg< double > HB(&sites);
      HB.buildHam(ampo,H);
      H.load("H_36_1.h5");
      H.print();
      qsMPO< double > Hq = H;

      qMPS<double> psi(&sites);
      psi.load(fname,pref);
      psi.print();
      qsMPS<double> psiq = psi;
      psiq.print();

      assert(H._id != psi._id);
      fixTensors(psiq,Hq);

      dmrg(psiq, Hq, nsweeps, maxm, cutoff, max_restart);

      psiq.print();
      /*for(int l=0;l<N;l++){
        auto& Al = psiq.A[l];
        perr<<Al._T.nnz_tot<<","<<(double)Al._T.nnz_tot/(Al._T.get_tot_size(false))<<endl;
        perr<<"   ";
        for(size_t i=0;i<Al.block_index_qd.size();i++)
          perr<<"("<<Al.block_index_qd[i][0]<<","
              << Al.block_index_qd[i][1]<<","<<Al.block_index_qd[i][2]<<") ";
        perr<<'\n';
      }*/
    }
  }
  MPI_Finalize();
  return 0;
  exit(1);
  //------------------------------------
  return 0;
}

inline bool check_existence(const std::string& name){
  struct stat buffer;
  return (stat (name.c_str(),&buffer) == 0);
}


// This example shows how to sort structs using complex multiple part keys using
// string_sort.
//
//  Copyright Steven Ross 2009-2014.
//
// 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)

//  See http://www.boost.org/libs/sort for library home page.

#include <boost/sort/spreadsort/string_sort.hpp>
#include <boost/sort/spreadsort/float_sort.hpp>
#include <time.h>
#include <stdio.h>
#include <stdlib.h>
#include <algorithm>
#include <vector>
#include <iostream>
#include <fstream>
#include <string>
using std::string;
using namespace boost::sort::spreadsort;

//[generalized_functors
struct DATA_TYPE {
  time_t birth;
  float net_worth;
  string first_name;
  string last_name;
};

static const int birth_size = sizeof(time_t);
static const int first_name_offset = birth_size + sizeof(float);
static const boost::uint64_t base_mask = 0xff;

struct lessthan {
  inline bool operator()(const DATA_TYPE &x, const DATA_TYPE &y) const {
    if (x.birth != y.birth) {
      return x.birth < y.birth;
    }
    if (x.net_worth != y.net_worth) {
      return x.net_worth < y.net_worth;
    }
    if (x.first_name != y.first_name) {
      return x.first_name < y.first_name;
    }
    return x.last_name < y.last_name;
  }
};

struct bracket {
  inline unsigned char operator()(const DATA_TYPE &x, size_t offset) const {
    // Sort date as a signed int, returning the appropriate byte.
    if (offset < birth_size) {
      const int bit_shift = 8 * (birth_size - offset - 1);
      unsigned char result = (x.birth & (base_mask << bit_shift)) >> bit_shift;
      // Handling the sign bit.  Unnecessary if the data is always positive.
      if (offset == 0) {
        return result ^ 128;
      }

      return result;
    }

    // Sort a signed float.  This requires reversing the order of negatives
    // because of the way floats are represented in bits.
    if (offset < first_name_offset) {
      const int bit_shift = 8 * (first_name_offset - offset - 1);
      unsigned key = float_mem_cast<float, unsigned>(x.net_worth);
      unsigned char result = (key & (base_mask << bit_shift)) >> bit_shift;
      // Handling the sign.
      if (x.net_worth < 0) {
        return 255 - result;
      }
      // Increasing positives so they are higher than negatives.
      if (offset == birth_size) {
        return 128 + result;
      }

      return result;
    }

    // Sort a string that is before the end.  This approach supports embedded
    // nulls.  If embedded nulls are not required, then just delete the "* 2"
    // and the inside of the following if just becomes:
    // return x.first_name[offset - first_name_offset];
    const unsigned first_name_end_offset =
      first_name_offset + x.first_name.size() * 2;
    if (offset < first_name_end_offset) {
      int char_offset = offset - first_name_offset;
      // This signals that the string continues.
      if (!(char_offset & 1)) {
        return 1;
      }
      return x.first_name[char_offset >> 1];
    }

    // This signals that the string has ended, so that shorter strings come
    // before longer ones.
    if (offset == first_name_end_offset) {
      return 0;
    }

    // The final string needs no special consideration.
    return x.last_name[offset - first_name_end_offset - 1];
  }
};

struct getsize {
  inline size_t operator()(const DATA_TYPE &x) const {
    return first_name_offset + x.first_name.size() * 2 + 1 +
      x.last_name.size();
  }
};
//] [/generalized_functors]

//Pass in an argument to test std::sort
int main(int argc, const char ** argv) {
  std::ifstream indata;
  std::ofstream outfile;
  bool stdSort = false;
  unsigned loopCount = 1;
  for (int u = 1; u < argc; ++u) {
    if (std::string(argv[u]) == "-std")
      stdSort = true;
    else
      loopCount = atoi(argv[u]);
  }
  double total = 0.0;
  //Run multiple loops, if requested
  std::vector<DATA_TYPE> array;
  for (unsigned u = 0; u < loopCount; ++u) {
    indata.open("input.txt", std::ios_base::in | std::ios_base::binary);
    if (indata.bad()) {
      printf("input.txt could not be opened\n");
      return 1;
    }

    // Read in the data.
    DATA_TYPE inval;
    while (!indata.eof() ) {
      indata >> inval.first_name;
      indata >> inval.last_name;
      indata.read(reinterpret_cast<char *>(&(inval.birth)), birth_size);
      indata.read(reinterpret_cast<char *>(&(inval.net_worth)), sizeof(float));
      // Handling nan.
      if (inval.net_worth != inval.net_worth) {
        inval.net_worth = 0;
      }
      if (indata.eof())
        break;
      array.push_back(inval);
    }
    indata.close();

    // Sort the data.
    clock_t start, end;
    double elapsed;
    start = clock();
    if (stdSort) {
      std::sort(array.begin(), array.end(), lessthan());
    } else {
//[generalized_functors_call
      string_sort(array.begin(), array.end(), bracket(), getsize(), lessthan());
//] [/generalized_functors_call]
    }
    end = clock();
    elapsed = static_cast<double>(end - start);
    if (stdSort) {
      outfile.open("standard_sort_out.txt", std::ios_base::out |
                   std::ios_base::binary | std::ios_base::trunc);
    } else {
      outfile.open("boost_sort_out.txt", std::ios_base::out |
                   std::ios_base::binary | std::ios_base::trunc);
    }
    if (outfile.good()) {
      for (unsigned u = 0; u < array.size(); ++u)
        outfile << array[u].birth << " " << array[u].net_worth << " "
                << array[u].first_name << " " << array[u].last_name << "\n";
      outfile.close();
    }
    total += elapsed;
    array.clear();
  }
  if (stdSort) {
    printf("std::sort elapsed time %f\n", total / CLOCKS_PER_SEC);
  } else {
    printf("spreadsort elapsed time %f\n", total / CLOCKS_PER_SEC);
  }
  return 0;
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/QR>

template<typename BoxType> void alignedbox(const BoxType& _box)
{
  /* this test covers the following files:
     AlignedBox.h
  */

  const int dim = _box.dim();
  typedef typename BoxType::Scalar Scalar;
  typedef typename NumTraits<Scalar>::Real RealScalar;
  typedef Matrix<Scalar, BoxType::AmbientDimAtCompileTime, 1> VectorType;

  VectorType p0 = VectorType::Random(dim);
  VectorType p1 = VectorType::Random(dim);
  RealScalar s1 = ei_random<RealScalar>(0,1);

  BoxType b0(dim);
  BoxType b1(VectorType::Random(dim),VectorType::Random(dim));
  BoxType b2;

  b0.extend(p0);
  b0.extend(p1);
  VERIFY(b0.contains(p0*s1+(Scalar(1)-s1)*p1));
  VERIFY(!b0.contains(p0 + (1+s1)*(p1-p0)));

  (b2 = b0).extend(b1);
  VERIFY(b2.contains(b0));
  VERIFY(b2.contains(b1));
  VERIFY_IS_APPROX(b2.clamp(b0), b0);

  // casting
  const int Dim = BoxType::AmbientDimAtCompileTime;
  typedef typename GetDifferentType<Scalar>::type OtherScalar;
  AlignedBox<OtherScalar,Dim> hp1f = b0.template cast<OtherScalar>();
  VERIFY_IS_APPROX(hp1f.template cast<Scalar>(),b0);
  AlignedBox<Scalar,Dim> hp1d = b0.template cast<Scalar>();
  VERIFY_IS_APPROX(hp1d.template cast<Scalar>(),b0);
}

void test_eigen2_alignedbox()
{
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1( alignedbox(AlignedBox<float,2>()) );
    CALL_SUBTEST_2( alignedbox(AlignedBox<float,3>()) );
    CALL_SUBTEST_3( alignedbox(AlignedBox<double,4>()) );
  }
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/ext/boost/mpl/integral_c.hpp>

#include <boost/mpl/int.hpp>
#include <boost/mpl/integral_c.hpp>
#include <boost/mpl/long.hpp>
using namespace boost::hana;
namespace mpl = boost::mpl;

static_assert(value(mpl::integral_c<int, 3>{}) == 3, "");

BOOST_HANA_CONSTANT_ASSERT(equal(mpl::integral_c<int, 3>{}, mpl::int_<3>{}));
BOOST_HANA_CONSTANT_ASSERT(equal(mpl::integral_c<int, 3>{}, mpl::long_<3>{}));
BOOST_HANA_CONSTANT_ASSERT(not_equal(mpl::integral_c<int, 3>{}, mpl::int_<0>{}));

int main() { }


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_SDK_COMPLEX_DETAILS_COMPLEX_META_HPP_INCLUDED
#define NT2_SDK_COMPLEX_DETAILS_COMPLEX_META_HPP_INCLUDED

#include <nt2/sdk/complex/details/complex/fusion_adapt.hpp>
#include <nt2/sdk/complex/meta/real_of.hpp>
#include <nt2/sdk/complex/hierarchy.hpp>
#include <boost/dispatch/meta/property_of.hpp>
#include <boost/dispatch/meta/hierarchy_of.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>
#include <boost/type_traits/has_trivial_copy.hpp>
#include <boost/type_traits/has_trivial_constructor.hpp>
#include <boost/type_traits/has_trivial_destructor.hpp>
#include <boost/mpl/bool.hpp>

namespace nt2 { namespace meta
{
  template<class T>
  struct real_of< std::complex<T> >
  {
    typedef T type;
  };
} }

namespace boost { namespace dispatch { namespace meta
{
  template<class T, class Origin>
  struct property_of< std::complex<T>, Origin >
  {
    typedef complex_< typename property_of<T, Origin>::type > type;
  };

  template<class T, class Origin>
  struct hierarchy_of< std::complex<T>, Origin >
  {
    typedef typename remove_const<Origin>::type stripped;
    typedef typename mpl::if_< is_same< std::complex<T>, stripped >, stripped, Origin >::type origin_;
    typedef scalar_< complex_< typename property_of<T, origin_>::type > > type;
  };

  template<class T>
  struct scalar_of< std::complex<T> >
  {
    typedef std::complex<T> type;
  };

  template<class T>
  struct value_of< std::complex<T> >
  {
    typedef T type;
  };

} } }

namespace boost { namespace simd
{
  template<class T>
  struct is_value;

  template<class T>
  struct is_value< std::complex<T> >
       : boost::mpl::true_
  {
  };
} }

namespace boost
{
  template<class T>
  struct has_trivial_copy< std::complex<T> >
       : boost::mpl::true_
  {
  };

  template<class T>
  struct has_trivial_default_constructor< std::complex<T> >
       : boost::mpl::true_
  {
  };

  template<class T>
  struct has_trivial_destructor< std::complex<T> >
       : boost::mpl::true_
  {
  };
}

#endif


// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>

#include <iostream>

#define BOOST_TEST_MODULE TrajectoriesTests
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

#include "multicontact-api/trajectories/cubic-hermite-spline.hpp"

using namespace multicontact_api::trajectories;

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(HermiteSpline)
{
  typedef CubicHermiteSplineTpl<double,3> CubicHermiteSpline3;
  typedef CubicHermiteSpline3::VectorX VectorX;
  typedef CubicHermiteSpline3::MatrixDx MatrixDx;
  typedef CubicHermiteSpline3::Index Index;

  // Test default constructor
  {
    CubicHermiteSpline3 spline;
    BOOST_CHECK(spline.size() == 0);
    CubicHermiteSpline3 spline2(0);
    BOOST_CHECK(spline.size() == 0);
  }

  const int size = 20;

  const MatrixDx points(MatrixDx::Random(3,size));
  const MatrixDx derivatives(MatrixDx::Random(3,size));
  const VectorX absicca(VectorX::LinSpaced(size,0.,1.));

  CubicHermiteSpline3 spline(absicca, points, derivatives);

  VectorX p0, m0;
  spline.eval(0.,p0,m0);
  VectorX p1, m1;
  spline.eval(1.,p1,m1);

  BOOST_CHECK(p0.isApprox(points.col(0)));
  BOOST_CHECK(m0.isApprox(derivatives.col(0)));
  BOOST_CHECK(p1.isApprox(points.rightCols<1>()));
  BOOST_CHECK(m1.isApprox(derivatives.rightCols<1>()));

  for(Index k = 0; k < size; ++k)
  {
    VectorX p,m;
    spline.eval(absicca[k],p,m);
    BOOST_CHECK(p.isApprox(points.col(k)));
    BOOST_CHECK(m.isApprox(derivatives.col(k)));
  }

  spline.derivatives().setZero();
  for(Index k = 0; k < spline.numIntervals(); ++k)
  {
    double t0 = absicca[k];
    double t1 = absicca[k+1];

    VectorX p0 = points.col(k);
    VectorX p1 = points.col(k+1);
    VectorX p_ref(0.5*(p0+p1));

    double t = 0.5*(t0+t1);
    VectorX p,m;
    spline.eval(t,p,m);
    BOOST_CHECK(p.isApprox(p_ref));
    BOOST_CHECK(m.isZero());
  }

  // Test copy constructor
  CubicHermiteSpline3 spline2(spline);
  BOOST_CHECK(spline2 == spline);

  // Test spline with only a single trajectorie defined by two end points
  {
    CubicHermiteSpline3 spline(2);
    spline.points().setZero();
    spline.derivatives().setZero();

    CubicHermiteSpline3::VectorX absicca = spline.absicca();
    absicca[0] = 0.; absicca[1] = 1.;
    spline.setAbsicca(absicca);

    CubicHermiteSpline3::VectorD value, dvalue;
    spline.eval(0.,value,dvalue);
    BOOST_CHECK(value.isApprox(CubicHermiteSpline3::VectorD::Zero()));
    BOOST_CHECK(dvalue.isApprox(CubicHermiteSpline3::VectorD::Zero()));
  }

  // Test the addition and subtraction operators
  {
    const int size = 20;

    const MatrixDx points1(MatrixDx::Random(3,size));
    const MatrixDx derivatives1(MatrixDx::Random(3,size));
    const MatrixDx points2(MatrixDx::Random(3,size));
    const MatrixDx derivatives2(MatrixDx::Random(3,size));

    const VectorX absicca(VectorX::LinSpaced(size,0.,1.));

    CubicHermiteSpline3 spline1(absicca, points1, derivatives1);
    CubicHermiteSpline3 spline2(absicca, points2, derivatives2);

    const CubicHermiteSpline3 spline_res_a = spline1+spline2;
    const CubicHermiteSpline3 spline_res_s = spline1-spline2;

    VectorX p1,m1, p2, m2, p_res_a, m_res_a, p_res_s, m_res_s;
    VectorX evalPoints(VectorX::Random(size));  //Random Evaluation points in range[-1., 1.]
    evalPoints.array() += 1.0;     evalPoints /=2.0;   // Move evaluation points to [0.,1.]
    for(int i=0;i<size;i++)
    {
      double t = evalPoints[i];
      spline1.eval(t,p1,m1);
      spline2.eval(t,p2,m2);
      spline_res_a.eval(t,p_res_a,m_res_a);
      spline_res_s.eval(t,p_res_s,m_res_s);

      BOOST_CHECK(p_res_a.isApprox(p1+p2));
      BOOST_CHECK(m_res_a.isApprox(m1+m2));
      BOOST_CHECK(p_res_s.isApprox(p1-p2));
      BOOST_CHECK(m_res_s.isApprox(m1-m2));
    }
  }
    // Test spline recomputation at a different abscissa
  {
    const int size = 20;
    const int size2 = 33;
    const int neval = 100;
    const MatrixDx points(MatrixDx::Random(3,size));
    const MatrixDx derivatives(MatrixDx::Random(3,size));

    const VectorX absicca(VectorX::LinSpaced(size,0.,1.));
    VectorX absicca2(VectorX::LinSpaced(size2,0.,1.));

    CubicHermiteSpline3 spline1(absicca, points, derivatives);

    CubicHermiteSpline3 spline2 = createHermiteSplineAtAbsicca(spline1, absicca2);
    VectorX p1,m1, p2, m2;
    VectorX evalPoints(VectorX::Random(neval));  //Random Evaluation points in range[-1., 1.]
    evalPoints.array() += 1.0;     evalPoints /=2.0;   // Move evaluation points to [0.,1.]
    for(int i=0;i<size2;i++)
    {
      double t = absicca2[i];
      spline1.eval(t,p1,m1);
      spline2.eval(t,p2,m2);
      BOOST_CHECK(p1.isApprox(p2));
      BOOST_CHECK(m1.isApprox(m2));
    }
  }
}

BOOST_AUTO_TEST_SUITE_END()


/**
 * \file boost/numeric/ublasx/operation/tril.hpp
 *
 * \brief Lower triangular view of a matrix.
 *
 * Copyright (c) 2011, Marco Guazzone
 *
 * 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)
 *
 * \author Marco Guazzone, <marco.guazzone@gmail.com>
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_TRIL_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_TRIL_HPP

#include <algorithm>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublasx/operation/num_columns.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>
#include <boost/numeric/ublasx/traits/layout_type.hpp>

//TODO:
// - Actually the tril function does not create a view; instead it copies the
//   elements of the input matrix according to the requested triangular view.
//   We need to create a 'generalized_triangular_matrix' and
//   'generalized_triangular_adaptor' class containers.
//

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

template <typename MatrixT>
struct tril_traits
{
//	typedef generalized_triangular_adaptor<MatrixT,lower> triangular_matrix_type;
	typedef matrix<typename matrix_traits<MatrixT>::value_type,
				   typename layout_type<MatrixT>::type> triangular_matrix_type;
};

template <typename MatrixExprT>
typename tril_traits<MatrixExprT>::triangular_matrix_type tril(matrix_expression<MatrixExprT> const& A,
															   typename matrix_traits<MatrixExprT>::difference_type k=0)
{
	typedef typename tril_traits<MatrixExprT>::triangular_matrix_type triangular_matrix_type;
	typedef typename matrix_traits<MatrixExprT>::value_type value_type;
	typedef typename matrix_traits<MatrixExprT>::size_type size_type;
	typedef typename matrix_traits<MatrixExprT>::difference_type difference_type;

//	return triangular_matrix_type(A());

	const difference_type zero(0);
	size_type nr(num_rows(A));
	size_type nc(num_columns(A));

	triangular_matrix_type X(nr, nc, value_type/*zero*/());

	size_type row_start;
	size_type row_stop(nr);

	if (k > zero)
	{
		//row_stop = ::std::min(nr,nc);
		row_start = zero;
	}
	else if (k < zero)
	{
		row_start = -k;
	}
	else
	{
		row_start = zero;
	}

	for (size_type r = row_start; r < row_stop; ++r)
	{
		size_type col_start(zero);
		size_type col_stop(nc);

		if (k > zero)
		{
			col_start = zero;
			col_stop = ::std::min(r+k+1,nc);
		}
		else if (k < zero)
		{
			col_start = zero;
			col_stop = ::std::min(nc,static_cast<size_type>(::std::max(zero,static_cast<difference_type>(r+k+1))));
		}
		else
		{
			col_start = zero;
			col_stop = ::std::min(r+k+1,nc);
		}

		for (size_type c = col_start; c < col_stop; ++c)
		{
			X(r,c) = A()(r,c);
		}
	}

	return X;
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_TRIL_HPP


/**
 * @file : test_cartesian_product.hpp
 * @author : Vikas Dhiman
 * @date: Wed July 11 2013
 */

#include "OccupancyGrid/cartesian_product.hpp"

#include <boost/bind.hpp>
#include <cstdio>
#include <vector>
#include <iostream>
#include <gtest/gtest.h>

#include <boost/property_map/property_map.hpp>
#include <boost/unordered_map.hpp>
#include <boost/type_traits.hpp>

using namespace occgrid;

typedef size_t nodeId_t;
typedef size_t Value;
typedef boost::unordered_map<size_t, Value> Assignment;
/****************************************************************
 * Testing
 ****************************************************************/
template<int Radix>
class RadixNode {
  typedef typename std::vector< Value >::const_iterator PossibleValueIter;
  typedef typename std::pair<PossibleValueIter, PossibleValueIter> iter_pair;
  private:
    const static std::vector<Value> poss_values_;
    nodeId_t node_id_;
    nodeId_t next_node_id() {
      static nodeId_t inc_node_id(0);
      nodeId_t tmpnode = inc_node_id;
      inc_node_id ++;
      return tmpnode;
    }
  public:
    RadixNode() : node_id_(next_node_id()) {
    }

    RadixNode(nodeId_t nid) : node_id_(nid) {}
    const iter_pair possible_values() const {
      return std::make_pair(poss_values_.begin(), poss_values_.end());
    }
    nodeId_t nodeId() const { return node_id_; }
    virtual ~RadixNode() {};
};

Value pv10[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
template<>
const std::vector<Value> RadixNode<10>::poss_values_(pv10, pv10 + sizeof(pv10) / sizeof(Value));

Value pv2[] = {0, 1};
template<>
const std::vector<Value> RadixNode<2>::poss_values_(pv2, pv2 + sizeof(pv2) / sizeof(Value));

typedef RadixNode<2> BinaryNode;
typedef RadixNode<10> DecimalNode;

typedef std::size_t codomain_type;
typedef typename std::vector<codomain_type>::const_iterator codomain_iterator;
typedef std::pair<codomain_iterator, codomain_iterator> codomain_iter_pair;
codomain_iter_pair codomain_static_function() {
  const static std::size_t codomain_array[] = {0, 1, 2, 3, 4, 5, 6, 7,8 ,9};
  const static std::vector<codomain_type> codomain_vector(
      codomain_array, codomain_array + 10);
  return std::make_pair(codomain_vector.begin(), codomain_vector.end());
}
/// Codomain Property Map for variable nodes
class DecimalCodomainMap {
  public:
  typedef nodeId_t key_type;
  typedef codomain_iter_pair value_type;
  typedef value_type& reference;
  typedef boost::lvalue_property_map_tag category;
};
DecimalCodomainMap::value_type get(DecimalCodomainMap c, nodeId_t n) {
  return codomain_static_function();
}

Assignment::mapped_type& get(Assignment& map, const typename Assignment::key_type& k) {
  return map[k];
}
void put(Assignment& map, const typename Assignment::key_type& k, typename Assignment::mapped_type& value) {
  map[k] = value;
}

TEST(CartesianProduct, test1) {
// int main(int argc, const char *argv[])
// {
  std::vector<nodeId_t> bivars;
  for (int i = 0; i < 5; i ++)
    bivars.push_back(i);

  codomain_iter_pair cd( codomain_static_function());
  DecimalCodomainMap cdmap;
  CartesianProduct< std::vector<nodeId_t>::const_iterator, DecimalCodomainMap >
    poss_assign(bivars.begin(), bivars.end(), cdmap);

  Assignment assign;
  int NDIGIT = 5;
  int num = 0;
  char num_as_str[NDIGIT];
  while (poss_assign.next(assign)) {
    typedef typename std::vector<nodeId_t>::const_iterator iter_t;
    sprintf(num_as_str, "%05d", num++);
    for (iter_t it(bivars.begin()); it != bivars.end(); it++) {
      nodeId_t nid = *it;
      char digit = num_as_str[NDIGIT - nid - 1];
      size_t fulldigit = (size_t) (digit - '0');
      (void) fulldigit;
      ASSERT_EQ(fulldigit, assign[nid]) << "Error at " << nid << ":" << fulldigit << "<>" << assign[nid] << "num:" << std::string(num_as_str);
    }
  }
}

/// Codomain Property Map for variable nodes
class BinaryCodomainMap {
  public:
  typedef nodeId_t key_type;
  typedef codomain_iter_pair value_type;
  typedef value_type& reference;
  typedef boost::lvalue_property_map_tag category;
};
BinaryCodomainMap::value_type get(BinaryCodomainMap c, nodeId_t n) {
  const static std::size_t codomain_array[] = {0, 1};
  const static std::vector<codomain_type> codomain_vector(
      codomain_array, codomain_array + 2);
  return std::make_pair(codomain_vector.begin(), codomain_vector.end());
}

template<typename InputIterator>
struct averageOf
: public std::unary_function<const Assignment&, double> {
  averageOf(InputIterator begin, InputIterator end) : begin_(begin), end_(end) {}
  double operator() (const Assignment& assign) const {
    double sum = 0;
    for (InputIterator it(begin_); it != end_; ++it)
      sum += const_cast<Assignment&>(assign)[*it];
    double avg =  sum / 5;
    return avg;
  }
  private:
  InputIterator begin_, end_;
};

TEST(summaryOf, test1) {
// int main(int argc, const char *argv[])
// {
  std::vector<nodeId_t> dependent_nodes;
  for  (int i=0;i<5;i++) {
    dependent_nodes.push_back(i);
  }
  //typedef typename UnaryFunction::argument_type PropertyMap;
  //typedef boost::remove_reference<typename UnaryFunction::argument_type>::type PropertyMap;
  typedef std::vector<nodeId_t>::const_iterator InputIterator;
  //typename PropertyMap::mapped_type one(1);
  Value one(1);
  averageOf<InputIterator> func(dependent_nodes.begin(), dependent_nodes.end());
    //boost::bind(averageOf<std::vector<nodeId_t>::const_iterator>, dependent_nodes.begin(), dependent_nodes.end(), _1);
  BinaryCodomainMap cdmap;
  InputIterator dnb = dependent_nodes.begin(), dne = dependent_nodes.end();
  typename InputIterator::value_type x(dependent_nodes[2]);
  double res = summaryOf<double,
         InputIterator, BinaryCodomainMap, Assignment>(func , dnb, dne, cdmap, x, one);
  (void)res;
  ASSERT_EQ(9.6, res);
}

int main(int argc, char *argv[])
{
   ::testing::InitGoogleTest(&argc, argv);
   return RUN_ALL_TESTS();
}


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    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 Delft University of Technology 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 HOLDER 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.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      100902    K. Kumar          File header and footer added.
 *      100916    D. Dirkx          Added minor comments during checking.
 *      100928    K. Kumar          Small comment modifications.
 *      100929    K. Kumar          Small comment modifications.
 *      110202    K. Kumar          Added overload for map with State* for
 *                                  computeNearestLeftNeighborUsingBinarySearch( ).
 *      110803    J. Leloux         Added convertStringToTemplate.
 *      110805    J. Leloux         Added outputCurrentRunningTime( ).
 *      110807    K. Kumar          Minor comment modifications.
 *      110810    J. Leloux         Minor comment modifications.
 *      110913    K. Kumar          Implemented automatic root-path functions based on
 *                                  suggestions by M. Persson.
 *      111117    K. Kumar          Added listAllFilesInDirectory( ) function.
 *
 *    References
 *      Press W.H., et al. Numerical Recipes in C++: The Art of Scientific Computing. Cambridge
 *          University Press, February 2002.
 *
 *    Notes
 *      Need to add bounds checking with respect to targetValue for
 *      computeNearestLeftNeighborUsingBinarySearch( ).
 */

#include <map>
#include <iterator>
#include <Eigen/Core>

namespace tudat
{
namespace basic_mathematics
{

//! Nearest left neighbor binary search.
int computeNearestLeftNeighborUsingBinarySearch(
        const Eigen::VectorXd& vectorOfSortedData,
        const double targetValueInVectorOfSortedData )
{
    // Declare local variables.
    // Declare bounds of vector of sorted data and current position.
    int leftLimitOfVectorOfSortedData = 0;
    int rightLimitOfVectorOfSortedData = vectorOfSortedData.rows( ) - 1;
    int currentPositionInVectorOfSortedData;

    // Check if data is sorted in ascending order.
    // ( true if ascending, else false ).
    bool isVectorOfSortedDataAscending
            = ( vectorOfSortedData[ rightLimitOfVectorOfSortedData ]
                >= vectorOfSortedData[ leftLimitOfVectorOfSortedData ] );

    // Loop through vector of sorted data until left and right limits
    // are neighbours.
    while ( rightLimitOfVectorOfSortedData
            - leftLimitOfVectorOfSortedData > 1 )
    {
        // Compute midpoint ( bitshift is same as division by 2.0 ).
        currentPositionInVectorOfSortedData
                = ( rightLimitOfVectorOfSortedData
                    + leftLimitOfVectorOfSortedData ) >> 1;

        // Check which limit to replace ( if ascending and target datum
        // is in right half, replace left limit ).
        if ( targetValueInVectorOfSortedData
             >= vectorOfSortedData[ currentPositionInVectorOfSortedData ]
             && isVectorOfSortedDataAscending )
        {
            // Set left limit to current position in vector of sorted data.
            leftLimitOfVectorOfSortedData = currentPositionInVectorOfSortedData;
        }

        else
        {
            // Set right limit to current position in vector of sorted data.
            rightLimitOfVectorOfSortedData = currentPositionInVectorOfSortedData;
        }
    }

    // Set current position to left limit.
    currentPositionInVectorOfSortedData = leftLimitOfVectorOfSortedData;

    // Return current position in vector.
    return currentPositionInVectorOfSortedData;
}

//! Nearest left neighbor binary search.
int computeNearestLeftNeighborUsingBinarySearch(
        const std::map < double, Eigen::VectorXd >& sortedIndepedentAndDependentVariables,
        const double targetValueInMapOfData )
{
    // Declare local variables.
    // Declare bounds of key of map of data and current position.
    int leftLimitOfKeyOfMapOfData = 0;
    int rightLimitOfKeyOfMapOfData = sortedIndepedentAndDependentVariables
                                     .size( ) - 1;
    int currentPositionInKeyOfMapOfData;

    // Declare map iterator
    std::map < double, Eigen::VectorXd >::const_iterator mapIterator;

    // Loop through vector of sorted data until left and right limits
    // are neighbours
    while ( rightLimitOfKeyOfMapOfData - leftLimitOfKeyOfMapOfData > 1 )
    {
        // Compute midpoint ( bitshift is same as division by 2.0 ).
        currentPositionInKeyOfMapOfData
                = ( rightLimitOfKeyOfMapOfData
                   + leftLimitOfKeyOfMapOfData ) >> 1;

        // Set map iterator to begin begin of map of sorted independent and
        // dependent variables.
        mapIterator = sortedIndepedentAndDependentVariables.begin( );

        // Advance iterator to location of current position in key of map of
        // data.
        advance( mapIterator, currentPositionInKeyOfMapOfData );

        // Check that target value lies to the right of lower bound.
        if ( targetValueInMapOfData
             >= mapIterator->first )
        {
            // Set left limit to current position in map of data.
            leftLimitOfKeyOfMapOfData = currentPositionInKeyOfMapOfData;
        }

        else
        {
            // Set right limit to current position in map of data.
            rightLimitOfKeyOfMapOfData = currentPositionInKeyOfMapOfData;
        }
    }

    // Set current position to left limit.
    currentPositionInKeyOfMapOfData = leftLimitOfKeyOfMapOfData;

    // Return current position in map of data.
    return currentPositionInKeyOfMapOfData;
}

} // namespace basic_mathematics
} // namespace tudat