// This file is part of MLDB. Copyright 2015 mldb.ai inc. All rights reserved.

/* boosting_generator.cc
   Jeremy Barnes, 15 March 2006
   Copyright (c) 2006 Jeremy Barnes  All rights reserved.
   $Source$

   Generator for boosted stumps.
*/

#include "boosting_generator.h"
#include "mldb/plugins/jml/jml/registry.h"
#include <boost/timer.hpp>
#include <boost/progress.hpp>
#include "training_index.h"
#include "weighted_training.h"
#include "mldb/plugins/jml/jml/committee.h"
#include "mldb/plugins/jml/sgi_numeric.h"
#include "boosting_training.h"
#include "mldb/arch/simd_vector.h"
#include "boosting_core.h"
#include "boosting_core_parallel.h"

#include "binary_symmetric.h"
#include "mldb/base/parallel.h"
#include "mldb/base/scope.h"
#include <boost/scoped_ptr.hpp>

using namespace std;

namespace ML {

/*****************************************************************************/
/* BOOSTING_GENERATOR                                                        */
/*****************************************************************************/

Boosting_Generator::
Boosting_Generator()
{
    defaults();
}

Boosting_Generator::~Boosting_Generator()
{
}

void
Boosting_Generator::
configure(const Configuration & config, vector<string> & unparsedKeys)
{
    Early_Stopping_Generator::configure(config, unparsedKeys);

    config.findAndRemove(max_iter, "max_iter", unparsedKeys);
    config.findAndRemove(min_iter, "min_iter", unparsedKeys);
    config.findAndRemove(cost_function, "cost_function", unparsedKeys);
    config.findAndRemove(short_circuit_window, "short_circuit_window", unparsedKeys);
    config.findAndRemove(trace_training_acc, "trace_training_acc", unparsedKeys);

    weak_learner = get_trainer("weak_learner", config);
}

void
Boosting_Generator::
defaults()
{
    Early_Stopping_Generator::defaults();
    max_iter = 500;
    min_iter = 10;
    cost_function = CF_EXPONENTIAL;
    short_circuit_window = 0;
    weak_learner.reset();
    trace_training_acc = false;
}

Config_Options
Boosting_Generator::
options() const
{
    Config_Options result = Early_Stopping_Generator::options();
    result
        .add("min_iter", min_iter, "1-max_iter",
             "minimum number of training iterations to run")
        .add("max_iter", max_iter, ">=min_iter",
             "maximum number of training iterations to run")
        .add("cost_function", cost_function,
             "select cost function for boosting weight update")
        .add("short_circuit_window", short_circuit_window, "0-",
             "short circuit (stop) training if no improvement for N iter "
             "(0 off)")
        .add("trace_training_acc", trace_training_acc,
             "trace the accuracy of the training set as well as validation")
        .subconfig("weak_leaner", weak_learner,
                   "weak learner that produces each bag");

    if (weak_learner) result.add(weak_learner->options());

    return result;
}

void
Boosting_Generator::
init(std::shared_ptr<const Feature_Space> fs, Feature predicted)
{
    Classifier_Generator::init(fs, predicted);
    weak_learner->init(fs, predicted);
}

std::shared_ptr<Classifier_Impl>
Boosting_Generator::
generate(Thread_Context & context,
         const Training_Data & training_set,
         const Training_Data & validation_set,
         const distribution<float> & training_ex_weights,
         const distribution<float> & validate_ex_weights,
         const std::vector<Feature> & features_, int) const
{
    vector<Feature> features = features_;

    boost::timer timer;

    unsigned nl = training_set.label_count(predicted);

    float best_acc = 0.0;
    int best_iter = 0;

    bool validate_is_train = false;
    if (validation_set.example_count() == 0
        || ((&validation_set == &training_set)
            && (training_ex_weights.size() == validate_ex_weights.size())
            && !(training_ex_weights != validate_ex_weights).any()))
        validate_is_train = true;

    boost::multi_array<float, 2> training_output
        (boost::extents[training_set.example_count()][nl]);

    boost::multi_array<float, 2> validation_output
        (boost::extents[validation_set.example_count()][nl]);

    boost::multi_array<float, 2> weights
        = expand_weights(training_set, training_ex_weights, predicted);

    boost::multi_array<float, 2> last_weights = weights;

    std::unique_ptr<boost::progress_display> progress;

    if (verbosity == 1 || verbosity == 2) {
        cerr << "training " << max_iter << " iterations..." << endl;
        progress.reset(new boost::progress_display(max_iter, cerr));
    }

    if (min_iter > max_iter)
        throw Exception("min_iter is greater than max_iter");

    if (verbosity > 2) {
        cerr << "  it   ";
        if (trace_training_acc) cerr << "train     ";
        cerr << "val   Z    Classifier" << endl;
    }

    vector<std::shared_ptr<Classifier_Impl> > classifiers;

    double validate_acc = 0.0;
    double train_acc = 0.0;

    for (unsigned i = 0;  i < max_iter;  ++i) {

        if (progress) ++(*progress);

        float Z;

        std::shared_ptr<Classifier_Impl> weak_classifier;
        Optimization_Info opt_info;

        if (validate_is_train || trace_training_acc)
            weak_classifier
                = train_iteration(context, training_set, weights, features,
                                  training_output, training_ex_weights,
                                  train_acc, Z, opt_info);
        else
            weak_classifier
                = train_iteration(context, training_set, weights, features,
                                  Z, opt_info);

        if (validate_is_train) validate_acc = train_acc;
        else
            validate_acc
                = update_accuracy(context, *weak_classifier, opt_info,
                                  validation_set, features,
                                  validation_output, validate_ex_weights);

#if 0
        float min_weight = 1.0, max_weight = 0.0, max_diff = 0.0;
        int where_max_diff = -1, where_max_weight = -1;

        int nx = training_set.example_count();

        double total = 0.0;

        for (unsigned x = 0;  x < nx;  ++x) {
            float old_w = last_weights[x][0];
            float new_w = weights[x][0];

            min_weight = std::min(min_weight, new_w);

            if (new_w > max_weight) {
                max_weight = new_w;
                where_max_weight = x;
            }

            float diff = fabs(old_w - new_w);
            if (diff > max_diff) {
                where_max_diff = x;
                max_diff = diff;
            }

            total += new_w;
        }

        cerr << "total = " << total << endl;

        cerr << "min_weight " << min_weight << " max_weight "
             << max_weight << " where " << where_max_weight
             << " old " << last_weights[where_max_weight][0]
             << " new " << weights[where_max_weight][0]
             << " pred " << weak_classifier->predict(0,
                                                     training_set[where_max_weight])
             << endl;

        cerr << "max_diff " << max_diff << " where " << where_max_diff
             << " old " << last_weights[where_max_diff][0]
             << " new " << weights[where_max_diff][0]
             << " pred " << weak_classifier->predict(0,
                                                     training_set[where_max_diff])
             << endl;

        cerr << "ratio = " << max_weight / min_weight << endl;

        last_weights.resize(boost::extents[weights.shape()[0]][weights.shape()[1]]);
        last_weights = weights;
#endif

        if (verbosity > 2) {
            cerr << format("%4d", i);
            if (trace_training_acc)
                cerr << format(" %6.2f%%", train_acc * 100.0);
            cerr << format(" %6.2f%% %5.3f ", validate_acc * 100.0, Z);
            cerr << weak_classifier->summary();
            cerr << endl;
        }

        classifiers.push_back(weak_classifier);

        if (validate_acc > best_acc && i >= min_iter) {
            best_iter = i;
            best_acc = validate_acc;
        }

        if (Z == 1.0f) {
            cerr << "Z = " << format("%12f", Z) << endl;
            cerr << "stopping due to perfect learning" << endl;
            break;  // too much capacity; we've learned non-zero weights
                    // perfectly
        }
    }

    if (verbosity > 0) {
        cerr << format("best was %6.2f%% on iteration %d", best_acc * 100.0,
                       best_iter)
             << endl;
    }

    if (profile)
        cerr << "training time: " << timer.elapsed() << "s" << endl;

    std::shared_ptr<Committee>
        result(new Committee(feature_space, predicted));

    for (int i = 0;  i <= best_iter;  ++i)
        result->add(classifiers[i]);

    return result;
}

std::shared_ptr<Classifier_Impl>
Boosting_Generator::
generate_and_update(Thread_Context & context,
                    const Training_Data & training_set,
                    boost::multi_array<float, 2> & weights,
                    const std::vector<Feature> & features_) const
{
    vector<Feature> features = features_;

    boost::timer timer;

    unsigned nl = training_set.label_count(predicted);

    float best_acc = 0.0;
    int best_iter = 0;

    boost::multi_array<float, 2> training_output
        (boost::extents[training_set.example_count()][nl]);

    if (weights.shape()[0] != training_set.example_count()
        || weights.shape()[1] != nl)
        throw Exception("Boosting_Generator::generate_and_update(): "
                        "weights have the wrong shape");

    distribution<float> training_ex_weights(training_set.example_count(), 1.0);

    std::unique_ptr<boost::progress_display> progress;

    if (verbosity == 1 || verbosity == 2) {
        cerr << "training " << max_iter << " iterations..." << endl;
        progress.reset(new boost::progress_display(max_iter, cerr));
    }

    if (verbosity > 2)
        cerr << "  it   train     Z    Classifier" << endl;

    vector<std::shared_ptr<Classifier_Impl> > classifiers;

    double train_acc = 0.0;

    for (unsigned i = 0;  i < max_iter;  ++i) {

        if (progress) ++(*progress);

        float Z;

        Optimization_Info opt_info;

        std::shared_ptr<Classifier_Impl> weak_classifier
            = train_iteration(context, training_set, weights, features,
                              training_output, training_ex_weights,
                              train_acc, Z, opt_info);

        if (verbosity > 2) {
            cerr << format("%4d", i);
            cerr << format(" %6.2f%% %5.3f ", train_acc * 100.0, Z);
            cerr << weak_classifier->summary();
            cerr << endl;
        }

        classifiers.push_back(weak_classifier);

        if (train_acc > best_acc && i >= min_iter) {
            best_iter = i;
            best_acc = train_acc;
        }

        if (1.0 - Z <= 1e-5)
            break;  // too much capacity; we've learned non-zero weights
                    // perfectly
    }

    if (verbosity > 0) {
        cerr << format("best was %6.2f%% on iteration %d", best_acc * 100.0,
                       best_iter)
             << endl;
    }

    if (profile)
        cerr << "training time: " << timer.elapsed() << "s" << endl;

    std::shared_ptr<Committee>
        result(new Committee(feature_space, predicted));

    for (int i = 0;  i <= best_iter;  ++i)
        result->add(classifiers[i]);

    return result;
}

std::shared_ptr<Classifier_Impl>
Boosting_Generator::
train_iteration(Thread_Context & context,
                const Training_Data & data,
                boost::multi_array<float, 2> & weights,
                vector<Feature> & features,
                float & Z,
                Optimization_Info & opt_info) const
{
    bool bin_sym = convert_bin_sym(weights, data, predicted, features);

    /* Make sure we have some features. */
    if (features.empty()) features = data.all_features();

    Z = 0.0;

    /* Find the best stump */
    std::shared_ptr<Classifier_Impl> weak_classifier
        = weak_learner->generate(context, data, weights, features, Z);

    const Feature_Space & fs = *data.feature_space();

    if (fs.type() == DENSE)
        opt_info = weak_classifier->optimize(fs.dense_features());

    /* Update the d distribution. */
    double total = 0.0;

    size_t nl = weights.shape()[1];

    if (cost_function == CF_EXPONENTIAL) {
        typedef Boosting_Loss Loss;
        if (bin_sym) {
            if (true /* use parallel */) {
                typedef Binsym_Updater<Loss> Updater;
                typedef Update_Weights_Parallel<Updater> Update;
                Update update;

                update(*weak_classifier, opt_info, 1.0, weights, data, total);
            }
            else {
                typedef Binsym_Updater<Loss> Updater;
                typedef Update_Weights<Updater> Update;
                Update update;

                total = update(*weak_classifier, opt_info, 1.0, weights, data);
            }
        }
        else {
            typedef Normal_Updater<Loss> Updater;
            typedef Update_Weights_Parallel<Updater> Update;
            Updater updater(nl);
            Update update(updater);

            update(*weak_classifier, opt_info, 1.0, weights, data, total);
        }
    }
    else if (cost_function == CF_LOGISTIC) {
        throw Exception("Boosting_Generator::train_iteration(): "
                        "what is Z for logistic loss?");
#if 0
        typedef Logistic_Loss Loss;
        Loss loss(Z);

        if (bin_sym) {
            typedef Binsym_Updater<Loss> Updater;
            typedef Update_Weights_Parallel<Updater> Update;
            Updater updater(loss);
            Update update(updater);

            update(*weak_classifier, opt_info, 1.0, weights, data, total);
        }
        else {
            //PROFILE_FUNCTION(t_update);
            typedef Normal_Updater<Loss> Updater;
            typedef Update_Weights_Parallel<Updater> Update;
            Updater updater(nl, loss);
            Update update(updater);

            update(*weak_classifier, opt_info, 1.0, weights, data, total);
        }
#endif
    }
    else throw Exception("Boosting_Generator::train_iteration: "
                         "unknown cost function");

    //cerr << "total = " << total << " Z = " << Z << endl;

    if (Z == 0.0) Z = total;

    if (Z > 1.0) {
        //cerr << "weak learner returned Z of " << Z << endl;
        //cerr << weak_classifier->print() << endl;
    }

    float * start = weights.data();
    float * finish = start + weights.num_elements();
    SIMD::vec_scale(start, 1.0 / total, start, finish - start);

    return weak_classifier;
}

/* This does the following:
   - Trains a stump
   - Updates the weights
   - Updates the output weights
   - Calculates the accuracy over the training set

   all in one pass (which saves on execution time as less memory is used).
*/

std::shared_ptr<Classifier_Impl>
Boosting_Generator::
train_iteration(Thread_Context & context,
                const Training_Data & data,
                boost::multi_array<float, 2> & weights,
                std::vector<Feature> & features,
                boost::multi_array<float, 2> & output,
                const distribution<float> & ex_weights,
                double & training_accuracy, float & Z,
                Optimization_Info & opt_info) const
{
    bool bin_sym
        = convert_bin_sym(weights, data, predicted, features);

    //cerr << "bin_sym = " << bin_sym << endl;

    convert_bin_sym(output, data, predicted, features);

    Z = 0.0;

    /* Find the best weak learner */
    std::shared_ptr<Classifier_Impl> weak_classifier
        = weak_learner->generate(context, data, weights, features, Z);

    const Feature_Space & fs = *data.feature_space();

    if (fs.type() == DENSE)
        opt_info = weak_classifier->optimize(fs.dense_features());

    /* Update the d distribution. */
    double total = 0.0;
    double correct = 0.0;

    size_t nx = data.example_count();
    if (nx != output.shape()[0])
        throw Exception("update_scores: example counts don't match");

    typedef Normal_Updater<Boosting_Predict> Output_Updater;
    Output_Updater output_updater(nl);

    if (cost_function == CF_EXPONENTIAL) {
        typedef Boosting_Loss Loss;
        if (bin_sym) {
            typedef Binsym_Updater<Loss> Weights_Updater;
            typedef Update_Weights_And_Scores_Parallel
                <Weights_Updater, Output_Updater, Binsym_Scorer>
                Update;
            Weights_Updater weights_updater;
            Update update(weights_updater, output_updater);

            update(*weak_classifier, opt_info,
                   1.0, weights, output, data, ex_weights,
                   correct, total);
        }
        else {
            typedef Normal_Updater<Loss> Weights_Updater;
            typedef Update_Weights_And_Scores_Parallel
                <Weights_Updater, Output_Updater, Normal_Scorer>
                Update;
            Weights_Updater weights_updater(nl);
            Update update(weights_updater, output_updater);

            update(*weak_classifier, opt_info,
                   1.0, weights, output, data, ex_weights,
                   correct, total);
        }
    }
    else if (cost_function == CF_LOGISTIC) {
        throw Exception("Boosting_Generator::train_iteration(): "
                        "what is Z for logistic loss?");
#if 0
        typedef Logistic_Loss Loss;
        Loss loss(stump.Z);

        if (bin_sym) {
            typedef Binsym_Updater<Loss> Weights_Updater;
            typedef Update_Weights_And_Scores_Parallel
                <Weights_Updater, Output_Updater, Binsym_Scorer>
                Update;
            Weights_Updater weights_updater(loss);
            Update update(weights_updater, output_updater);

            update(stump, opt_info,
                   1.0, weights, output, data, ex_weights,
                   correct, total);
        }
        else {
            typedef Normal_Updater<Loss> Weights_Updater;
            typedef Update_Weights_And_Scores_Parallel
                <Weights_Updater, Output_Updater, Normal_Scorer>
                Update;
            Weights_Updater weights_updater(nl, loss);
            Update update(weights_updater, output_updater);

            update(stump, opt_info,
                   1.0, weights, output, data, ex_weights,
                   correct, total);
        }
#endif
    }
    else throw Exception("Boosted_Stumps_Generator::train_iteration: "
                         "unknown cost function");

    if (Z == 0.0) Z = total;

    //cerr << "total = " << total << " Z = " << Z << endl;

    training_accuracy = correct / ex_weights.total();

    float * start = weights.data();
    float * finish = start + weights.num_elements();
    SIMD::vec_scale(start, 1.0 / total, start, finish - start);

    return weak_classifier;
}

/* This does the following:
   - Trains a stump
   - Updates the weights
   - Updates the output weights
   - Calculates the accuracy over the training set

   all in one pass (which saves on execution time as less memory is used).
*/

double
Boosting_Generator::
update_accuracy(Thread_Context & context,
                const Classifier_Impl & weak_classifier,
                const Optimization_Info & opt_info,
                const Training_Data & data,
                const vector<Feature> & features,
                boost::multi_array<float, 2> & output,
                const distribution<float> & ex_weights) const
{
    bool bin_sym
        = convert_bin_sym(output, data, predicted, features);

    double correct = 0.0;

    if (bin_sym) {
        typedef Binsym_Updater<Boosting_Predict> Output_Updater;
        Output_Updater output_updater;

        typedef Update_Scores_Parallel<Output_Updater, Binsym_Scorer> Update;
        Update update(output_updater);

        update(weak_classifier, opt_info,
               1.0, output, data, ex_weights, correct);
    }
    else {
        typedef Normal_Updater<Boosting_Predict> Output_Updater;
        Output_Updater output_updater(nl);

        typedef Update_Scores_Parallel<Output_Updater, Normal_Scorer> Update;
        Update update(output_updater);

        update(weak_classifier, opt_info,
               1.0, output, data, ex_weights, correct);
    }

    return correct / ex_weights.total();
}

/*****************************************************************************/
/* REGISTRATION                                                              */
/*****************************************************************************/

namespace {

Register_Factory<Classifier_Generator, Boosting_Generator>
    BOOSTING_REGISTER("boosting");

} // file scope

} // namespace ML


//
//=======================================================================
// Copyright 2007 Stanford University
// Authors: David Gleich
//
// 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)
//=======================================================================
//
#ifndef BOOST_GRAPH_CORE_NUMBERS_HPP
#define BOOST_GRAPH_CORE_NUMBERS_HPP

#include <boost/graph/detail/d_ary_heap.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/iterator/reverse_iterator.hpp>
#include <boost/concept/assert.hpp>

/*
 * core_numbers
 *
 * Requirement: IncidenceGraph
 */

// History
//
// 30 July 2007
// Added visitors to the implementation
//
// 8 February 2008
// Fixed headers and missing typename

namespace boost
{

// A linear time O(m) algorithm to compute the indegree core number
// of a graph for unweighted graphs.
//
// and a O((n+m) log n) algorithm to compute the in-edge-weight core
// numbers of a weighted graph.
//
// The linear algorithm comes from:
// Vladimir Batagelj and Matjaz Zaversnik, "An O(m) Algorithm for Cores
// Decomposition of Networks."  Sept. 1 2002.

template < typename Visitor, typename Graph > struct CoreNumbersVisitorConcept
{
    void constraints()
    {
        BOOST_CONCEPT_ASSERT((CopyConstructibleConcept< Visitor >));
        vis.examine_vertex(u, g);
        vis.finish_vertex(u, g);
        vis.examine_edge(e, g);
    }
    Visitor vis;
    Graph g;
    typename graph_traits< Graph >::vertex_descriptor u;
    typename graph_traits< Graph >::edge_descriptor e;
};

template < class Visitors = null_visitor >
class core_numbers_visitor : public bfs_visitor< Visitors >
{
public:
    core_numbers_visitor() {}
    core_numbers_visitor(Visitors vis) : bfs_visitor< Visitors >(vis) {}

private:
    template < class Vertex, class Graph >
    void initialize_vertex(Vertex, Graph&)
    {
    }

    template < class Vertex, class Graph > void discover_vertex(Vertex, Graph&)
    {
    }

    template < class Vertex, class Graph > void gray_target(Vertex, Graph&) {}

    template < class Vertex, class Graph > void black_target(Vertex, Graph&) {}

    template < class Edge, class Graph > void tree_edge(Edge, Graph&) {}

    template < class Edge, class Graph > void non_tree_edge(Edge, Graph&) {}
};

template < class Visitors >
core_numbers_visitor< Visitors > make_core_numbers_visitor(Visitors vis)
{
    return core_numbers_visitor< Visitors >(vis);
}

typedef core_numbers_visitor<> default_core_numbers_visitor;

namespace detail
{

    // implement a constant_property_map to simplify compute_in_degree
    // for the weighted and unweighted case
    // this is based on dummy property map
    template < typename ValueType >
    class constant_value_property_map
    : public boost::put_get_helper< ValueType,
          constant_value_property_map< ValueType > >
    {
    public:
        typedef void key_type;
        typedef ValueType value_type;
        typedef const ValueType& reference;
        typedef boost::readable_property_map_tag category;
        inline constant_value_property_map(ValueType cc) : c(cc) {}
        inline constant_value_property_map(
            const constant_value_property_map< ValueType >& x)
        : c(x.c)
        {
        }
        template < class Vertex > inline reference operator[](Vertex) const
        {
            return c;
        }

    protected:
        ValueType c;
    };

    // the core numbers start as the indegree or inweight.  This function
    // will initialize these values
    template < typename Graph, typename CoreMap, typename EdgeWeightMap >
    void compute_in_degree_map(Graph& g, CoreMap d, EdgeWeightMap wm)
    {
        typename graph_traits< Graph >::vertex_iterator vi, vi_end;
        typename graph_traits< Graph >::out_edge_iterator ei, ei_end;
        for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
        {
            put(d, *vi, 0);
        }
        for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
        {
            for (boost::tie(ei, ei_end) = out_edges(*vi, g); ei != ei_end; ++ei)
            {
                put(d, target(*ei, g), get(d, target(*ei, g)) + get(wm, *ei));
            }
        }
    }

    // the version for weighted graphs is a little different
    template < typename Graph, typename CoreMap, typename EdgeWeightMap,
        typename MutableQueue, typename Visitor >
    typename property_traits< CoreMap >::value_type core_numbers_impl(
        Graph& g, CoreMap c, EdgeWeightMap wm, MutableQueue& Q, Visitor vis)
    {
        typename property_traits< CoreMap >::value_type v_cn = 0;
        typedef typename graph_traits< Graph >::vertex_descriptor vertex;
        while (!Q.empty())
        {
            // remove v from the Q, and then decrease the core numbers
            // of its successors
            vertex v = Q.top();
            vis.examine_vertex(v, g);
            Q.pop();
            v_cn = get(c, v);
            typename graph_traits< Graph >::out_edge_iterator oi, oi_end;
            for (boost::tie(oi, oi_end) = out_edges(v, g); oi != oi_end; ++oi)
            {
                vis.examine_edge(*oi, g);
                vertex u = target(*oi, g);
                // if c[u] > c[v], then u is still in the graph,
                if (get(c, u) > v_cn)
                {
                    // remove the edge
                    put(c, u, get(c, u) - get(wm, *oi));
                    if (Q.contains(u))
                        Q.update(u);
                }
            }
            vis.finish_vertex(v, g);
        }
        return (v_cn);
    }

    template < typename Graph, typename CoreMap, typename EdgeWeightMap,
        typename IndexMap, typename CoreNumVisitor >
    typename property_traits< CoreMap >::value_type core_numbers_dispatch(
        Graph& g, CoreMap c, EdgeWeightMap wm, IndexMap im, CoreNumVisitor vis)
    {
        typedef typename property_traits< CoreMap >::value_type D;
        typedef std::less< D > Cmp;
        // build the mutable queue
        typedef typename graph_traits< Graph >::vertex_descriptor vertex;
        std::vector< std::size_t > index_in_heap_data(num_vertices(g));
        typedef iterator_property_map< std::vector< std::size_t >::iterator,
            IndexMap >
            index_in_heap_map_type;
        index_in_heap_map_type index_in_heap_map(
            index_in_heap_data.begin(), im);
        typedef d_ary_heap_indirect< vertex, 4, index_in_heap_map_type, CoreMap,
            Cmp >
            MutableQueue;
        MutableQueue Q(c, index_in_heap_map, Cmp());
        typename graph_traits< Graph >::vertex_iterator vi, vi_end;
        for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
        {
            Q.push(*vi);
        }
        return core_numbers_impl(g, c, wm, Q, vis);
    }

    // the version for the unweighted case
    // for this functions CoreMap must be initialized
    // with the in degree of each vertex
    template < typename Graph, typename CoreMap, typename PositionMap,
        typename Visitor >
    typename property_traits< CoreMap >::value_type core_numbers_impl(
        Graph& g, CoreMap c, PositionMap pos, Visitor vis)
    {
        typedef typename graph_traits< Graph >::vertices_size_type size_type;
        typedef typename graph_traits< Graph >::degree_size_type degree_type;
        typedef typename graph_traits< Graph >::vertex_descriptor vertex;
        typename graph_traits< Graph >::vertex_iterator vi, vi_end;

        // store the vertex core numbers
        typename property_traits< CoreMap >::value_type v_cn = 0;

        // compute the maximum degree (degrees are in the coremap)
        typename graph_traits< Graph >::degree_size_type max_deg = 0;
        for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
        {
            max_deg = (std::max<
                typename graph_traits< Graph >::degree_size_type >)(max_deg,
                get(c, *vi));
        }

        // store the vertices in bins by their degree
        // allocate two extra locations to ease boundary cases
        std::vector< size_type > bin(max_deg + 2);
        for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
        {
            ++bin[get(c, *vi)];
        }

        // this loop sets bin[d] to the starting position of vertices
        // with degree d in the vert array for the bucket sort
        size_type cur_pos = 0;
        for (degree_type cur_deg = 0; cur_deg < max_deg + 2; ++cur_deg)
        {
            degree_type tmp = bin[cur_deg];
            bin[cur_deg] = cur_pos;
            cur_pos += tmp;
        }

        // perform the bucket sort with pos and vert so that
        // pos[0] is the vertex of smallest degree
        std::vector< vertex > vert(num_vertices(g));
        for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
        {
            vertex v = *vi;
            size_type p = bin[get(c, v)];
            put(pos, v, p);
            vert[p] = v;
            ++bin[get(c, v)];
        }
        // we ``abused'' bin while placing the vertices, now,
        // we need to restore it
        std::copy(boost::make_reverse_iterator(bin.end() - 2),
            boost::make_reverse_iterator(bin.begin()),
            boost::make_reverse_iterator(bin.end() - 1));
        // now simulate removing the vertices
        for (size_type i = 0; i < num_vertices(g); ++i)
        {
            vertex v = vert[i];
            vis.examine_vertex(v, g);
            v_cn = get(c, v);
            typename graph_traits< Graph >::out_edge_iterator oi, oi_end;
            for (boost::tie(oi, oi_end) = out_edges(v, g); oi != oi_end; ++oi)
            {
                vis.examine_edge(*oi, g);
                vertex u = target(*oi, g);
                // if c[u] > c[v], then u is still in the graph,
                if (get(c, u) > v_cn)
                {
                    degree_type deg_u = get(c, u);
                    degree_type pos_u = get(pos, u);
                    // w is the first vertex with the same degree as u
                    // (this is the resort operation!)
                    degree_type pos_w = bin[deg_u];
                    vertex w = vert[pos_w];
                    if (u != v)
                    {
                        // swap u and w
                        put(pos, u, pos_w);
                        put(pos, w, pos_u);
                        vert[pos_w] = u;
                        vert[pos_u] = w;
                    }
                    // now, the vertices array is sorted assuming
                    // we perform the following step
                    // start the set of vertices with degree of u
                    // one into the future (this now points at vertex
                    // w which we swapped with u).
                    ++bin[deg_u];
                    // we are removing v from the graph, so u's degree
                    // decreases
                    put(c, u, get(c, u) - 1);
                }
            }
            vis.finish_vertex(v, g);
        }
        return v_cn;
    }

} // namespace detail

// non-named parameter version for the unweighted case
template < typename Graph, typename CoreMap, typename CoreNumVisitor >
typename property_traits< CoreMap >::value_type core_numbers(
    Graph& g, CoreMap c, CoreNumVisitor vis)
{
    typedef typename graph_traits< Graph >::vertices_size_type size_type;
    detail::compute_in_degree_map(g, c,
        detail::constant_value_property_map<
            typename property_traits< CoreMap >::value_type >(1));
    return detail::core_numbers_impl(g, c,
        make_iterator_property_map(
            std::vector< size_type >(num_vertices(g)).begin(),
            get(vertex_index, g)),
        vis);
}

// non-named paramter version for the unweighted case
template < typename Graph, typename CoreMap >
typename property_traits< CoreMap >::value_type core_numbers(
    Graph& g, CoreMap c)
{
    return core_numbers(g, c, make_core_numbers_visitor(null_visitor()));
}

// non-named parameter version for the weighted case
template < typename Graph, typename CoreMap, typename EdgeWeightMap,
    typename VertexIndexMap, typename CoreNumVisitor >
typename property_traits< CoreMap >::value_type core_numbers(Graph& g,
    CoreMap c, EdgeWeightMap wm, VertexIndexMap vim, CoreNumVisitor vis)
{
    detail::compute_in_degree_map(g, c, wm);
    return detail::core_numbers_dispatch(g, c, wm, vim, vis);
}

// non-named parameter version for the weighted case
//    template <typename Graph, typename CoreMap, typename EdgeWeightMap>
//    typename property_traits<CoreMap>::value_type
//    core_numbers(Graph& g, CoreMap c, EdgeWeightMap wm)
//    {
//        typedef typename graph_traits<Graph>::vertices_size_type size_type;
//        detail::compute_in_degree_map(g,c,wm);
//        return detail::core_numbers_dispatch(g,c,wm,get(vertex_index,g),
//            make_core_numbers_visitor(null_visitor()));
//    }

template < typename Graph, typename CoreMap >
typename property_traits< CoreMap >::value_type weighted_core_numbers(
    Graph& g, CoreMap c)
{
    return weighted_core_numbers(
        g, c, make_core_numbers_visitor(null_visitor()));
}

template < typename Graph, typename CoreMap, typename CoreNumVisitor >
typename property_traits< CoreMap >::value_type weighted_core_numbers(
    Graph& g, CoreMap c, CoreNumVisitor vis)
{
    return core_numbers(g, c, get(edge_weight, g), get(vertex_index, g), vis);
}

} // namespace boost

#endif // BOOST_GRAPH_CORE_NUMBERS_HPP


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

#ifndef CRYPTO3_DETAIL_IMPLODER_HPP
#define CRYPTO3_DETAIL_IMPLODER_HPP

#include <nil/crypto3/detail/stream_endian.hpp>
#include <nil/crypto3/detail/unbounded_shift.hpp>
#include <nil/crypto3/detail/reverser.hpp>

#include <boost/static_assert.hpp>

namespace nil {
    namespace crypto3 {
        namespace detail {

            // By definition, for all imploders, InputValueBits < OutputValueBits,
            // so we're taking many smaller values and combining them into one value

            /*!
             * @defgroup imploder Imploder functions
             */

            /*!
             * @brief imploder_shift trait is used to determine whether the input elements are packed into
             * an output element in reverse order. Since the input and output types are integral now, this
             * trait contains the shift indicating the position of input element in the output element when
             * k input bits have already been processed.
             *
             * @ingroup imploder
             *
             * @tparam OutputEndianness
             * @tparam UnitBits
             * @tparam InputBits
             * @tparam OutputBits
             * @tparam k
             * @tparam IsLittleUnit
             */
            template<typename OutputEndianness, int UnitBits, int InputBits, int OutputBits, int k,
                     bool IsLittleUnit = is_little_unit<OutputEndianness, UnitBits>::value>
            struct imploder_shift;

            template<typename OutputEndianness, int UnitBits, int InputBits, int OutputBits, int k>
            struct imploder_shift<OutputEndianness, UnitBits, InputBits, OutputBits, k, false> {
                constexpr static int const value = OutputBits - (InputBits + k);
            };

            template<typename OutputEndianness, int UnitBits, int InputBits, int OutputBits, int k>
            struct imploder_shift<OutputEndianness, UnitBits, InputBits, OutputBits, k, true> {
                constexpr static int const value = k;
            };

            /*!
             * @brief imploder_step packs an input value represented in InputEndianness endianness
             * into an output value represented in OutputEndianness endianness when k input bits
             * have already been processed. It uses unit_reverser and bit_reverser to deal with the
             * order of units and bits in the input value, respectively. Shift constant is determined
             * by the imploder_shift trait.
             *
             * @ingroup imploder
             *
             * @tparam InputEndianness
             * @tparam OutputEndianness
             * @tparam UnitBits
             * @tparam InputBits
             * @tparam OutputBits
             * @tparam k
             */
            template<typename InputEndianness, typename OutputEndianness, int UnitBits, int InputBits, int OutputBits,
                     int k>
            struct imploder_step {
                constexpr static int const shift =
                    imploder_shift<OutputEndianness, UnitBits, InputBits, OutputBits, k>::value;

                template<typename InputValue, typename OutputValue>
                inline static void step(InputValue &in, OutputValue &out) {
                    InputValue tmp = in;
                    unit_reverser<InputEndianness, OutputEndianness, UnitBits>::reverse(tmp);
                    bit_reverser<InputEndianness, OutputEndianness, UnitBits>::reverse(tmp);
                    out |= unbounded_shl<shift>(low_bits<InputBits>(OutputValue(tmp)));
                }
            };

            /*!
             * @brief imploder processes a sequence of input values represented in InputEndianness endianness
             * into an output value represented in OutputEndianness endianness. The function implode is
             * invoked recursively, and the parameter k is used to track the number of already processed
             * input values packed into the output value. The recursion ends when all elements the output
             * value can hold have already been processed, i.e. when k == OutputBits.
             *
             * @ingroup imploder
             *
             * @tparam InputEndianness
             * @tparam OutputEndianness
             * @tparam InputBits
             * @tparam OutputBits
             * @tparam k
             */
            template<typename InputEndianness, typename OutputEndianness, int InputBits, int OutputBits, int k = 0>
            struct imploder;

            template<template<int> class InputEndian, template<int> class OutputEndian, int UnitBits, int InputBits,
                     int OutputBits, int k>
            struct imploder<InputEndian<UnitBits>, OutputEndian<UnitBits>, InputBits, OutputBits, k> {

                // To keep the implementation managable, input and output sizes must
                // be multiples or factors of the unit size.
                // If one of these is firing, you may want a bit-only stream_endian
                // rather than one that mentions bytes or octets.
                BOOST_STATIC_ASSERT(!(InputBits % UnitBits && UnitBits % InputBits));
                BOOST_STATIC_ASSERT(!(OutputBits % UnitBits && UnitBits % OutputBits));

                typedef InputEndian<UnitBits> InputEndianness;
                typedef OutputEndian<UnitBits> OutputEndianness;
                typedef imploder_step<InputEndianness, OutputEndianness, UnitBits, InputBits, OutputBits, k> step_type;
                typedef imploder<InputEndianness, OutputEndianness, InputBits, OutputBits, k + InputBits> next_type;

                template<typename InIter, typename OutputValue>
                inline static void implode(InIter &in, OutputValue &x) {
                    step_type::step(*in++, x);
                    next_type::implode(in, x);
                }
            };

            template<template<int> class InputEndian, template<int> class OutputEndian, int UnitBits, int InputBits,
                     int OutputBits>
            struct imploder<InputEndian<UnitBits>, OutputEndian<UnitBits>, InputBits, OutputBits, OutputBits> {
                template<typename InIter, typename OutputValue>
                inline static void implode(InIter &, OutputValue &) {
                }
            };

        }    // namespace detail
    }        // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_DETAIL_IMPLODER_HPP


// FILE: src/misc.cpp  Date Modified: March 17, 2015
// PacketGenie
// Copyright 2014 The Regents of the University of Michigan
// Dong-Hyeon Park, Ritesh Parikh, Valeria Bertacco
//
// PacketGenie is 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 _MISC_CPP_
#define _MISC_CPP_

#include <list>
#include <vector>
#include <string>
#include <iostream>
#include <ostream>
#include <sstream>
#include <fstream>
#include <cstdlib>
#include "globals.hpp"
#include <boost/lexical_cast.hpp>
#include <boost/regex.hpp>

static const boost::regex
    range_specifier( "^(\\d+)-(\\d+)$" );
static const boost::regex
    valid_idx("^\\d+$");
static const boost::regex
    keyword_syntax("^[A-Z]+$");

static int factorial(int n)
{
  return (n == 1 || n == 0) ? 1 : factorial(n - 1) * n;
}

// Allow printing of vector data.
template<typename T>
ostream& operator<< (ostream& out, const vector<T> v) {
    int last = v.size() - 1;
    out << "[";
    for(int i = 0; i < last; i++)
        out << v[i] << ", ";
    out << v[last] << "]";
    return out;
}

static bool lessThan(const int& num1, const int& num2)
{
      return num1 < num2;
}

static int strvec_to_intlist(vector<string> *strvec, list<int> *intlist)
{
    try
    {
        for (vector<string>::iterator it = strvec->begin();
             it != strvec->end(); ++it)
        {
            intlist->push_back(boost::lexical_cast<int>(*it));
        }
        return SUCCESS;
    }
    catch (const boost::bad_lexical_cast &)
    {
        cerr << "ERROR: BAD CASTING" << endl;
        return ERROR;
    }
}

static int strvec_to_doubvec(vector<string> *strvec, vector<double> *doubvec)
{
    try
    {
        for (vector<string>::iterator it = strvec->begin();
             it != strvec->end(); ++it)
        {
            doubvec->push_back(boost::lexical_cast<double>(*it));
        }
        return SUCCESS;
    }
    catch (const boost::bad_lexical_cast &)
    {
        cerr << "ERROR: BAD CASTING" << endl;
        return ERROR;
    }
}

static int write_output(string &filename, string &write_data)
{
      ofstream outfile;
      outfile.open(filename.c_str());
      outfile << write_data;
      outfile.close();

      return 0;

}

static int process_keyword(string token, list<int> *intlist, int ntwk_size)
{

      int init, term, step;
      if (token == "ALL")
      {
          init = 0;
          term = ntwk_size;
          step = 1;
      }
      else if (token == "EVEN")
      {
          init = 0;
          term =  ntwk_size;
          step = 2;
      }
      else if (token == "ODD")
      {
          init = 1;
          term = ntwk_size;
          step = 2;
      }
      else
          return ERROR;

      for (int i = init; i < term; i += step)
          intlist->push_back(i);

      return SUCCESS;
}

static int interpret_range(string token, list<int> *intlist)
{
      int split_pos = token.find_first_of('-');
      int init = boost::lexical_cast<int>(token.substr(0, split_pos));
      int last = boost::lexical_cast<int>(token.substr(split_pos+1));

      for (int i = init; i <= last ; ++i)
           intlist->push_back(i);

      return SUCCESS;

}

//Extract string that are between { }
static	int extract_idxstr(vector<string> *strvec,  vector<string> &node_idxs)
{
      vector<string>::iterator first, last;
      bool brk_found = false;
      bool done = false;

      for ( vector<string>::iterator it = strvec->begin(); it != strvec->end();
            ++it)
      {
            if (*it == "{")
            {
                if (brk_found)
                    return ERROR;
                first = it;
                ++first; // Exclude the bracket
                brk_found = true;
            }

            if (*it == "}")
            {
                if (!brk_found)
                    return ERROR;
                last = it;
                done = true;
                break;
            }
      }

      if (!done)
          return ERROR;

      node_idxs = vector<string>(first, last);
      return SUCCESS;
}

// Extract strings that are between < >
static	int extract_str(vector<string> *strvec,  vector<string> &extracted)
{
      vector<string>::iterator first, last;
      bool brk_found = false;
      bool done = false;

      for ( vector<string>::iterator it = strvec->begin(); it != strvec->end();
            ++it)
      {
            if (*it == "<")
            {
                if (brk_found)
                    return ERROR;
                first = it;
                ++first; // Exclude the bracket
                brk_found = true;
            }

            if (*it == ">")
            {
                if (!brk_found)
                    return ERROR;
                last = it;
                done = true;
                break;
            }
      }

      if (!done)
          return ERROR;

      extracted = vector<string>(first, last);
      return SUCCESS;

}

// Looks at a string vector and parse out the node index values inside
// { } into an integer list
static int process_strvec(vector<string> *raw_strvec,
                          list<int> *intlist, int ntwk_size)
{

      vector<string> str_vec;

      boost::smatch str_matches;
      size_t size_max = ntwk_size;
      int error;

      error = extract_idxstr(raw_strvec, str_vec);
      if (error)
          return error;

      // Look for any special keyword
      vector<string>::iterator it = str_vec.begin();
      if ( boost::regex_match(*it, str_matches, keyword_syntax) )
      {
            // Process Keyword
            process_keyword(*it, intlist, ntwk_size);
            ++it;
      }

      unsigned int idx;
      string idx_str;

      // NEED TO LOOP THROUGH AND CHECK FOR RANGE.
      while(it != str_vec.end())
      {
          idx_str = *it;

          if ( boost::regex_match(idx_str, str_matches, range_specifier) )
          {
                // Process range syntax
                interpret_range(idx_str, intlist);

          }
          else if (boost::regex_match(idx_str, str_matches, valid_idx) )
          {
              // Process a regular single integer index
              idx = boost::lexical_cast<int>(idx_str);

              if ( idx <  size_max )
              {
                    intlist->push_back(idx);
              }
              else
              {
                  cerr << "ERROR node index out of bounds " << endl
                       << "Index parsed: " << idx
                       << "Max allowed: " << size_max <<endl;
                  return ERROR;
              }
          }
          else
          {
              cerr << "ERROR: Invalid node index - " << idx_str << endl;
              return ERROR;
          }
          ++it;
    }

    // Sort the list from smallest to largest.
    intlist->sort(lessThan);
    // Remove duplicates
    intlist->unique();
    return SUCCESS;

}

static vector<string>& get_strvec(multimap< string,vector<string> > &mmap,
                                  const string &key)
{
      return mmap.find(key)->second;

}

/*
static int generate_node_prob(string name, size_t m, list<double> *prob_dist)
{
    double temp;

    if (prob_dist->size() != 0)
        return ERROR;

    if ( name == "uniform")
    {
        temp = 1.0;
        for (size_t i = 0; i < m; ++i)
            prob_dist->push_back(temp);

        return SUCCESS;
    }

    return ERROR;
}
*/

// Give random number between 0 and 1. (1 not included)
static double get_rand_decimal()
{
    return rand() / (RAND_MAX+1.0);
}

static double factorial(size_t val)
{
    size_t ret = 1;
    for (size_t i = 1; i <= val; ++i)
        ret *= i;
    return ret;
}

// For s=1
static double erlang_delay_constant(double arrival_rate,
                                    double service_time)
{
    double a, util_fac;
//          size_t num_server, s_fac;

//          num_server = 1;
//          s_fac = factorial(num_server);

    a = arrival_rate * service_time;
    if (a >= 1)
        return 1; // Utilization factor becomes 1.

    util_fac = a;

    return (a/(1-util_fac)) / ( a + a/(1-util_fac));
}

static double erlang_delay_constant(size_t num,
                                    double arrival_rate,
                                    double service_time)
{
    double numerator, denominator;
    double a, a_pow_s, util_fac;
    int num_factorial = factorial(num);

    a = arrival_rate * service_time;
    a_pow_s = pow(a,num);
    if (a >= 1)
        return 1; // Utilization factor becomes 1.

    util_fac = a;

    numerator = a_pow_s/(num_factorial*(1-util_fac));
    denominator = numerator;
    for (size_t k=0; k<num; k++)
        denominator += pow(a,k)/factorial(k);

    return (numerator /denominator);
}

static double erlang_prob_wait(size_t wait_time,
                               double arrival_rate,
                               double service_time,
                               double erlangC)
{
    double a, util_fac;
    a = arrival_rate * service_time;
    if (a >= 1)
        return erlangC;

    util_fac = a;
    return erlangC*exp(-1*(1-util_fac)*(1/service_time)*wait_time);
}

static double erlang_prob_wait(size_t wait_time,
                               size_t num,
                               double arrival_rate,
                               double service_time,
                               double erlangC)
{
    double a, util_fac;
    a = arrival_rate * service_time;
    if (a >= 1)
        return erlangC;

    util_fac = a;
    return erlangC*exp(-1*(1-util_fac)*(num/service_time)*wait_time);
}

#endif


#ifndef BOOST_GEOMETRY_PROJECTIONS_OB_TRAN_HPP
#define BOOST_GEOMETRY_PROJECTIONS_OB_TRAN_HPP

// Boost.Geometry - extensions-gis-projections (based on PROJ4)
// This file is automatically generated. DO NOT EDIT.

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 4.9.1

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#include <boost/geometry/util/math.hpp>
#include <boost/shared_ptr.hpp>

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>
#include <boost/geometry/srs/projections/impl/aasincos.hpp>

namespace boost { namespace geometry
{

namespace srs { namespace par4
{
    //struct ob_tran_oblique {};
    //struct ob_tran_transverse {};
    struct ob_tran {};

}} //namespace srs::par4

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail {

        // fwd declaration needed below
        template <typename CalculationType>
        inline detail::base_v<CalculationType, parameters<CalculationType> >*
            create_new(parameters<CalculationType> const& parameters);

    } // namespace detail

    namespace detail { namespace ob_tran
    {

            static const double TOL = 1e-10;

            template <typename Parameters>
            inline Parameters o_proj_parameters(Parameters const& par)
            {
                Parameters pj;

                /* get name of projection to be translated */
                pj.name = pj_param(par.params, "so_proj").s;
                /* copy existing header into new */
                pj.params = par.params;
                pj.over = par.over;
                pj.geoc = par.geoc;
                pj.a = par.a;
                pj.es = par.es;
                pj.ra = par.ra;
                pj.lam0 = par.lam0;
                pj.phi0 = par.phi0;
                pj.x0 = par.x0;
                pj.y0 = par.y0;
                pj.k0 = par.k0;
                /* force spherical earth */
                pj.one_es = pj.rone_es = 1.;
                pj.es = pj.e = 0.;

                return pj;
            }

            template <typename CalculationType, typename Parameters>
            struct par_ob_tran
            {
                par_ob_tran(Parameters const& par)
                    : link(projections::detail::create_new(o_proj_parameters(par)))
                {
                    if (! link.get())
                        BOOST_THROW_EXCEPTION( projection_exception(-26) );
                }

                template <typename T>
                inline void fwd(T& lp_lon, T& lp_lat, T& xy_x, T& xy_y) const
                {
                    link->fwd(lp_lon, lp_lat, xy_x, xy_y);
                }

                template <typename T>
                inline void inv(T& xy_x, T& xy_y, T& lp_lon, T& lp_lat) const
                {
                    link->inv(xy_x, xy_y, lp_lon, lp_lat);
                }

                boost::shared_ptr<base_v<CalculationType, Parameters> > link;
                CalculationType lamp;
                CalculationType cphip, sphip;
            };

            template <typename StaticParameters, typename CalculationType, typename Parameters>
            struct par_ob_tran_static
            {
                typedef typename srs::par4::detail::pick_o_proj_tag
                    <
                        StaticParameters
                    >::type o_proj_tag;

                typedef typename projections::detail::static_projection_type
                    <
                        o_proj_tag,
                        srs_sphere_tag, // force spherical
                        StaticParameters,
                        CalculationType,
                        Parameters
                    >::type projection_type;

                par_ob_tran_static(Parameters const& par)
                    : link(o_proj_parameters(par))
                {}

                template <typename T>
                inline void fwd(T& lp_lon, T& lp_lat, T& xy_x, T& xy_y) const
                {
                    link.fwd(lp_lon, lp_lat, xy_x, xy_y);
                }

                template <typename T>
                inline void inv(T& xy_x, T& xy_y, T& lp_lon, T& lp_lat) const
                {
                    link.inv(xy_x, xy_y, lp_lon, lp_lat);
                }

                projection_type link;
                CalculationType lamp;
                CalculationType cphip, sphip;
            };

            template <typename T, typename Par>
            inline void o_forward(T& lp_lon, T& lp_lat, T& xy_x, T& xy_y, Par const& proj_parm)
            {
                T coslam, sinphi, cosphi;

                coslam = cos(lp_lon);
                sinphi = sin(lp_lat);
                cosphi = cos(lp_lat);
                lp_lon = adjlon(aatan2(cosphi * sin(lp_lon), proj_parm.sphip * cosphi * coslam +
                    proj_parm.cphip * sinphi) + proj_parm.lamp);
                lp_lat = aasin(proj_parm.sphip * sinphi - proj_parm.cphip * cosphi * coslam);

                proj_parm.fwd(lp_lon, lp_lat, xy_x, xy_y);
            }

            template <typename T, typename Par>
            inline void o_inverse(T& xy_x, T& xy_y, T& lp_lon, T& lp_lat, Par const& proj_parm)
            {
                T coslam, sinphi, cosphi;

                proj_parm.inv(xy_x, xy_y, lp_lon, lp_lat);
                if (lp_lon != HUGE_VAL) {
                    coslam = cos(lp_lon -= proj_parm.lamp);
                    sinphi = sin(lp_lat);
                    cosphi = cos(lp_lat);
                    lp_lat = aasin(proj_parm.sphip * sinphi + proj_parm.cphip * cosphi * coslam);
                    lp_lon = aatan2(cosphi * sin(lp_lon), proj_parm.sphip * cosphi * coslam -
                        proj_parm.cphip * sinphi);
                }
            }

            template <typename T, typename Par>
            inline void t_forward(T& lp_lon, T& lp_lat, T& xy_x, T& xy_y, Par const& proj_parm)
            {
                T cosphi, coslam;

                cosphi = cos(lp_lat);
                coslam = cos(lp_lon);
                lp_lon = adjlon(aatan2(cosphi * sin(lp_lon), sin(lp_lat)) + proj_parm.lamp);
                lp_lat = aasin(- cosphi * coslam);
                proj_parm.fwd(lp_lon, lp_lat, xy_x, xy_y);
            }

            template <typename T, typename Par>
            inline void t_inverse(T& xy_x, T& xy_y, T& lp_lon, T& lp_lat, Par const& proj_parm)
            {
                T cosphi, t;

                proj_parm.inv(xy_x, xy_y, lp_lon, lp_lat);
                if (lp_lon != HUGE_VAL) {
                    cosphi = cos(lp_lat);
                    t = lp_lon - proj_parm.lamp;
                    lp_lon = aatan2(cosphi * sin(t), - sin(lp_lat));
                    lp_lat = aasin(cosphi * cos(t));
                }
            }

            // General Oblique Transformation
            template <typename CalculationType, typename Parameters, typename ProjParameters>
            inline CalculationType setup_ob_tran(Parameters & par, ProjParameters& proj_parm)
            {
                static const CalculationType HALFPI = detail::HALFPI<CalculationType>();

                CalculationType phip;

                par.es = 0.; /* force to spherical */

                // proj_parm.link should be created at this point

                if (pj_param(par.params, "to_alpha").i) {
                    CalculationType lamc, phic, alpha;

                    lamc    = pj_param(par.params, "ro_lon_c").f;
                    phic    = pj_param(par.params, "ro_lat_c").f;
                    alpha    = pj_param(par.params, "ro_alpha").f;
            /*
                    if (fabs(phic) <= TOL ||
                        fabs(fabs(phic) - HALFPI) <= TOL ||
                        fabs(fabs(alpha) - HALFPI) <= TOL)
            */
                    if (fabs(fabs(phic) - HALFPI) <= TOL)
                        BOOST_THROW_EXCEPTION( projection_exception(-32) );
                    proj_parm.lamp = lamc + aatan2(-cos(alpha), -sin(alpha) * sin(phic));
                    phip = aasin(cos(phic) * sin(alpha));
                } else if (pj_param(par.params, "to_lat_p").i) { /* specified new pole */
                    proj_parm.lamp = pj_param(par.params, "ro_lon_p").f;
                    phip = pj_param(par.params, "ro_lat_p").f;
                } else { /* specified new "equator" points */
                    CalculationType lam1, lam2, phi1, phi2, con;

                    lam1 = pj_param(par.params, "ro_lon_1").f;
                    phi1 = pj_param(par.params, "ro_lat_1").f;
                    lam2 = pj_param(par.params, "ro_lon_2").f;
                    phi2 = pj_param(par.params, "ro_lat_2").f;
                    if (fabs(phi1 - phi2) <= TOL ||
                        (con = fabs(phi1)) <= TOL ||
                        fabs(con - HALFPI) <= TOL ||
                        fabs(fabs(phi2) - HALFPI) <= TOL)
                        BOOST_THROW_EXCEPTION( projection_exception(-33) );
                    proj_parm.lamp = atan2(cos(phi1) * sin(phi2) * cos(lam1) -
                        sin(phi1) * cos(phi2) * cos(lam2),
                        sin(phi1) * cos(phi2) * sin(lam2) -
                        cos(phi1) * sin(phi2) * sin(lam1));
                    phip = atan(-cos(proj_parm.lamp - lam1) / tan(phi1));
                }
                if (fabs(phip) > TOL) { /* oblique */
                    proj_parm.cphip = cos(phip);
                    proj_parm.sphip = sin(phip);
                } else { /* transverse */
                }
                // return phip to choose model
                return phip;
            }

            // template class, using CRTP to implement forward/inverse
            template <typename CalculationType, typename Parameters>
            struct base_ob_tran_oblique : public base_t_fi<base_ob_tran_oblique<CalculationType, Parameters>,
                     CalculationType, Parameters>
            {

                typedef CalculationType geographic_type;
                typedef CalculationType cartesian_type;

                par_ob_tran<CalculationType, Parameters> m_proj_parm;

                inline base_ob_tran_oblique(Parameters const& par,
                                            par_ob_tran<CalculationType, Parameters> const& proj_parm)
                    : base_t_fi
                        <
                            base_ob_tran_oblique<CalculationType, Parameters>, CalculationType, Parameters
                        >(*this, par)
                    , m_proj_parm(proj_parm)
                {}

                // FORWARD(o_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    o_forward(lp_lon, lp_lat, xy_x, xy_y, this->m_proj_parm);
                }

                // INVERSE(o_inverse)  spheroid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    o_inverse(xy_x, xy_y, lp_lon, lp_lat, this->m_proj_parm);
                }

                static inline std::string get_name()
                {
                    return "ob_tran_oblique";
                }

            };

            // template class, using CRTP to implement forward/inverse
            template <typename CalculationType, typename Parameters>
            struct base_ob_tran_transverse : public base_t_fi<base_ob_tran_transverse<CalculationType, Parameters>,
                     CalculationType, Parameters>
            {

                typedef CalculationType geographic_type;
                typedef CalculationType cartesian_type;

                par_ob_tran<CalculationType, Parameters> m_proj_parm;

                inline base_ob_tran_transverse(Parameters const& par,
                                               par_ob_tran<CalculationType, Parameters> const& proj_parm)
                    : base_t_fi
                        <
                            base_ob_tran_transverse<CalculationType, Parameters>, CalculationType, Parameters
                        >(*this, par)
                    , m_proj_parm(proj_parm)
                {}

                // FORWARD(t_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    t_forward(lp_lon, lp_lat, xy_x, xy_y, this->m_proj_parm);
                }

                // INVERSE(t_inverse)  spheroid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    t_inverse(xy_x, xy_y, lp_lon, lp_lat, this->m_proj_parm);
                }

                static inline std::string get_name()
                {
                    return "ob_tran_transverse";
                }

            };

            // template class, using CRTP to implement forward/inverse
            template <typename StaticParameters, typename CalculationType, typename Parameters>
            struct base_ob_tran_static : public base_t_fi<base_ob_tran_static<StaticParameters, CalculationType, Parameters>,
                     CalculationType, Parameters>
            {

                typedef CalculationType geographic_type;
                typedef CalculationType cartesian_type;

                par_ob_tran_static<StaticParameters, CalculationType, Parameters> m_proj_parm;
                bool m_is_oblique;

                inline base_ob_tran_static(Parameters const& par)
                    : base_t_fi<base_ob_tran_static<StaticParameters, CalculationType, Parameters>, CalculationType, Parameters>(*this, par)
                    , m_proj_parm(par)
                {}

                // FORWARD(o_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    if (m_is_oblique) {
                        o_forward(lp_lon, lp_lat, xy_x, xy_y, this->m_proj_parm);
                    } else {
                        t_forward(lp_lon, lp_lat, xy_x, xy_y, this->m_proj_parm);
                    }
                }

                // INVERSE(o_inverse)  spheroid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    if (m_is_oblique) {
                        o_inverse(xy_x, xy_y, lp_lon, lp_lat, this->m_proj_parm);
                    } else {
                        t_inverse(xy_x, xy_y, lp_lon, lp_lat, this->m_proj_parm);
                    }
                }

                static inline std::string get_name()
                {
                    return "ob_tran";
                }

            };

    }} // namespace detail::ob_tran
    #endif // doxygen

    /*!
        \brief General Oblique Transformation projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Miscellaneous
         - Spheroid
        \par Projection parameters
         - o_proj (string)
         - Plus projection parameters
         - o_lat_p (degrees)
         - o_lon_p (degrees)
         - New pole
         - o_alpha: Alpha (degrees)
         - o_lon_c (degrees)
         - o_lat_c (degrees)
         - o_lon_1 (degrees)
         - o_lat_1: Latitude of first standard parallel (degrees)
         - o_lon_2 (degrees)
         - o_lat_2: Latitude of second standard parallel (degrees)
        \par Example
        \image html ex_ob_tran.gif
    */
    template <typename CalculationType, typename Parameters>
    struct ob_tran_oblique : public detail::ob_tran::base_ob_tran_oblique<CalculationType, Parameters>
    {
        inline ob_tran_oblique(Parameters const& par,
                               detail::ob_tran::par_ob_tran<CalculationType, Parameters> const& proj_parm)
            : detail::ob_tran::base_ob_tran_oblique<CalculationType, Parameters>(par, proj_parm)
        {
            // already done
            //detail::ob_tran::setup_ob_tran(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief General Oblique Transformation projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Miscellaneous
         - Spheroid
        \par Projection parameters
         - o_proj (string)
         - Plus projection parameters
         - o_lat_p (degrees)
         - o_lon_p (degrees)
         - New pole
         - o_alpha: Alpha (degrees)
         - o_lon_c (degrees)
         - o_lat_c (degrees)
         - o_lon_1 (degrees)
         - o_lat_1: Latitude of first standard parallel (degrees)
         - o_lon_2 (degrees)
         - o_lat_2: Latitude of second standard parallel (degrees)
        \par Example
        \image html ex_ob_tran.gif
    */
    template <typename CalculationType, typename Parameters>
    struct ob_tran_transverse : public detail::ob_tran::base_ob_tran_transverse<CalculationType, Parameters>
    {
        inline ob_tran_transverse(Parameters const& par,
                                  detail::ob_tran::par_ob_tran<CalculationType, Parameters> const& proj_parm)
            : detail::ob_tran::base_ob_tran_transverse<CalculationType, Parameters>(par, proj_parm)
        {
            // already done
            //detail::ob_tran::setup_ob_tran(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief General Oblique Transformation projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Miscellaneous
         - Spheroid
        \par Projection parameters
         - o_proj (string)
         - Plus projection parameters
         - o_lat_p (degrees)
         - o_lon_p (degrees)
         - New pole
         - o_alpha: Alpha (degrees)
         - o_lon_c (degrees)
         - o_lat_c (degrees)
         - o_lon_1 (degrees)
         - o_lat_1: Latitude of first standard parallel (degrees)
         - o_lon_2 (degrees)
         - o_lat_2: Latitude of second standard parallel (degrees)
        \par Example
        \image html ex_ob_tran.gif
    */
    template <typename StaticParameters, typename CalculationType, typename Parameters>
    struct ob_tran_static : public detail::ob_tran::base_ob_tran_static<StaticParameters, CalculationType, Parameters>
    {
        inline ob_tran_static(const Parameters& par)
            : detail::ob_tran::base_ob_tran_static<StaticParameters, CalculationType, Parameters>(par)
        {
            CalculationType phip = detail::ob_tran::setup_ob_tran<CalculationType>(this->m_par, this->m_proj_parm);
            this->m_is_oblique = fabs(phip) > detail::ob_tran::TOL;
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        template <typename BGP, typename CT, typename P>
        struct static_projection_type<srs::par4::ob_tran, srs_sphere_tag, BGP, CT, P>
        {
            typedef ob_tran_static<BGP, CT, P> type;
        };
        template <typename BGP, typename CT, typename P>
        struct static_projection_type<srs::par4::ob_tran, srs_spheroid_tag, BGP, CT, P>
        {
            typedef ob_tran_static<BGP, CT, P> type;
        };

        // Factory entry(s)
        template <typename CalculationType, typename Parameters>
        class ob_tran_entry : public detail::factory_entry<CalculationType, Parameters>
        {
            public :
                virtual base_v<CalculationType, Parameters>* create_new(const Parameters& par) const
                {
                    Parameters params = par;
                    detail::ob_tran::par_ob_tran<CalculationType, Parameters> proj_parm(params);
                    CalculationType phip = detail::ob_tran::setup_ob_tran<CalculationType>(params, proj_parm);

                    if (fabs(phip) > detail::ob_tran::TOL)
                        return new base_v_fi<ob_tran_oblique<CalculationType, Parameters>, CalculationType, Parameters>(params, proj_parm);
                    else
                        return new base_v_fi<ob_tran_transverse<CalculationType, Parameters>, CalculationType, Parameters>(params, proj_parm);
                }
        };

        template <typename CalculationType, typename Parameters>
        inline void ob_tran_init(detail::base_factory<CalculationType, Parameters>& factory)
        {
            factory.add_to_factory("ob_tran", new ob_tran_entry<CalculationType, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_OB_TRAN_HPP


//headers in this package
#include <driving_force_controller.h>
#include <ros_ship_visualization/PlotCharacteristicCurve.h>

//headers for controller_interface
#include <controller_interface/controller.h>

//headers for pluginlib
#include <pluginlib/class_list_macros.h>

//headers for realtime_tools
#include <realtime_tools/realtime_publisher.h>
#include <realtime_tools/realtime_buffer.h>

//headers for boost
#include <boost/shared_ptr.hpp>
#include <boost/scoped_ptr.hpp>
#include <boost/thread/condition.hpp>
#include <boost/thread.hpp>
#include <boost/algorithm/clamp.hpp>

//headers for ROS
#include <ros/package.h>

//headers for STL
#include <vector>

namespace driving_force_controller
{
  DrivingForceController::DrivingForceController()
  {

  }

  DrivingForceController::~DrivingForceController()
  {

  }

  void DrivingForceController::drivingForceCallback(const std_msgs::Float64& msg)
  {
    if (isRunning())
    {
      driving_force_struct.value = msg.data;
      driving_force.writeFromNonRT(driving_force_struct);
    }
    else
    {
      ROS_WARN_STREAM("Can't accept new commands. Controller is not running.");
    }
  }

  void DrivingForceController::twistCallback(const geometry_msgs::Twist& msg)
  {
    if (isRunning())
    {
      twist_struct.ang_x = msg.angular.x;
      twist_struct.ang_y = msg.angular.y;
      twist_struct.ang_z = msg.angular.z;
      twist_struct.lin_x = msg.linear.x;
      twist_struct.lin_y = msg.linear.y;
      twist_struct.lin_z = msg.linear.z;
      twist_struct.stamp = ros::Time::now();
      twist.writeFromNonRT(twist_struct);
    }
    else
    {
      ROS_WARN_STREAM("Can't accept new commands. Controller is not running.");
    }
  }

  double DrivingForceController::get_thrust(double rotational_speed,double inflow_rate)
  {
    if(rotational_speed == 0)
    {
      return 0;
    }
    if(rotational_speed > 0)
    {
      double Js = inflow_rate/rotational_speed*turning_radius;
      double Kt = k2*Js*Js + k1*Js + k0;
      double thrust = fluid_density*std::pow(rotational_speed,2)*std::pow(turning_radius,4)*Kt;
      return thrust;
    }
    if(rotational_speed < 0)
    {
      double Js = inflow_rate/rotational_speed*turning_radius;
      double Kt = k2*Js*Js + k1*Js + k0;
      double thrust = -fluid_density*std::pow(rotational_speed,2)*std::pow(turning_radius,4)*Kt;
      return thrust;
    }
  }

  bool DrivingForceController::init(hardware_interface::VelocityJointInterface* hw,ros::NodeHandle& controller_nh)
  {
    controller_nh.getParam("twist_topic",twist_topic);
    controller_nh.getParam("motor_command_topic",motor_command_topic);
    controller_nh.getParam("driving_force_command_topic",driving_force_command_topic);
    controller_nh.getParam("propeller/turning_radius",turning_radius);
    controller_nh.getParam("propeller/k2",k2);
    controller_nh.getParam("propeller/k1",k1);
    controller_nh.getParam("propeller/k0",k0);
    controller_nh.getParam("propeller/fluid_density",  fluid_density);
    controller_nh.getParam("characteristic_curve_file_name",characteristic_curve_file_name);
    controller_nh.getParam("gradient_descent/tolerance",tolerance);
    controller_nh.getParam("gradient_descent/alpha",alpha);
    controller_nh.getParam("max_rotational_speed",max_rotational_speed);
    controller_nh.getParam("min_rotational_speed",min_rotational_speed);
    //plot_characteristic_curve(0,30,1,0,30,5);
    //boost::thread plot_thread(boost::bind(&DrivingForceController::plot_characteristic_curve,this,0,30,1,0,30,5));
    plot_client = controller_nh.serviceClient<ros_ship_visualization::PlotCharacteristicCurve>("/plot_characteristic_curve");
    ros_ship_visualization::PlotCharacteristicCurve plot_service_message;
    plot_service_message.request.fluid_density = fluid_density;
    plot_service_message.request.turning_radius = turning_radius;
    plot_service_message.request.k0 = k0;
    plot_service_message.request.k1 = k1;
    plot_service_message.request.k2 = k2;
    plot_service_message.request.file_name = characteristic_curve_file_name;
    plot_service_message.request.min_inflow_rate = 0;
    plot_service_message.request.max_inflow_rate = 3;
    plot_service_message.request.resolution_inflow_rate = 1;
    plot_service_message.request.min_rotational_speed = min_rotational_speed;
    plot_service_message.request.max_rotational_speed = max_rotational_speed;
    plot_service_message.request.resolution_rotational_speed = 0.01;
    plot_client.call(plot_service_message);
    motor_command_publisher.reset(new realtime_tools::RealtimePublisher<std_msgs::Float64>(controller_nh, motor_command_topic, 1));
    twist_sub = controller_nh.subscribe(twist_topic, 1, &DrivingForceController::twistCallback, this);
    driving_force_sub = controller_nh.subscribe(driving_force_command_topic, 1, &DrivingForceController::drivingForceCallback, this);
  }

  double DrivingForceController::get_rotational_speed(double thrust,double inflow_rate)
  {
    double rotational_speed = 0;
    double error = 0;
    error = thrust-get_thrust(rotational_speed,inflow_rate);
    while(std::fabs(error) > tolerance)
    {
      rotational_speed = rotational_speed+error*alpha;
      error = thrust-get_thrust(rotational_speed,inflow_rate);
    }
    return rotational_speed;
  }

  void DrivingForceController::starting(const ros::Time& time)
  {
    twist_struct.ang_x = 0;
    twist_struct.ang_y = 0;
    twist_struct.ang_z = 0;
    twist_struct.lin_x = 0;
    twist_struct.lin_y = 0;
    twist_struct.lin_z = 0;
    twist.initRT(twist_struct);
    driving_force_struct.value = 0;
    driving_force.initRT(driving_force_struct);
  }

  void DrivingForceController::update(const ros::Time& time, const ros::Duration& period)
  {
    driving_force_struct = *(driving_force.readFromRT());
    twist_struct = *(twist.readFromRT());
    if(motor_command_publisher && motor_command_publisher->trylock())
    {
      double target_rotational_speed = get_rotational_speed(driving_force_struct.value,twist_struct.lin_x);
      target_rotational_speed = boost::algorithm::clamp(target_rotational_speed,min_rotational_speed,max_rotational_speed);
      motor_command_publisher->msg_.data = target_rotational_speed;
      motor_command_publisher->unlockAndPublish();
    }
  }
  void DrivingForceController::stopping(const ros::Time& time)
  {

  }
}

PLUGINLIB_EXPORT_CLASS(driving_force_controller::DrivingForceController, controller_interface::ControllerBase);