// Copyright (C) 2013  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <dlib/python.h>
#include <dlib/matrix.h>
#include <dlib/svm_threaded.h>

using namespace dlib;
using namespace std;
namespace py = pybind11;

typedef matrix<double,0,1> dense_vect;
typedef std::vector<std::pair<unsigned long,double> > sparse_vect;
typedef std::vector<std::pair<unsigned long, unsigned long> > ranges;

// ----------------------------------------------------------------------------------------

template <typename samp_type, bool BIO, bool high_order, bool nonnegative>
class segmenter_feature_extractor
{

public:
    typedef std::vector<samp_type> sequence_type;
    const static bool use_BIO_model = BIO;
    const static bool use_high_order_features = high_order;
    const static bool allow_negative_weights = nonnegative;

    unsigned long _num_features;
    unsigned long _window_size;

    segmenter_feature_extractor(
    ) : _num_features(1), _window_size(1) {}

    segmenter_feature_extractor(
        unsigned long _num_features_,
        unsigned long _window_size_
    ) : _num_features(_num_features_), _window_size(_window_size_) {}

    unsigned long num_features(
    ) const { return _num_features; }

    unsigned long window_size(
    ) const {return _window_size; }

    template <typename feature_setter>
    void get_features (
        feature_setter& set_feature,
        const std::vector<dense_vect>& x,
        unsigned long position
    ) const
    {
        for (long i = 0; i < x[position].size(); ++i)
        {
            set_feature(i, x[position](i));
        }
    }

    template <typename feature_setter>
    void get_features (
        feature_setter& set_feature,
        const std::vector<sparse_vect>& x,
        unsigned long position
    ) const
    {
        for (unsigned long i = 0; i < x[position].size(); ++i)
        {
            set_feature(x[position][i].first, x[position][i].second);
        }
    }

    friend void serialize(const segmenter_feature_extractor& item, std::ostream& out)
    {
        dlib::serialize(item._num_features, out);
        dlib::serialize(item._window_size, out);
    }
    friend void deserialize(segmenter_feature_extractor& item, std::istream& in)
    {
        dlib::deserialize(item._num_features, in);
        dlib::deserialize(item._window_size, in);
    }
};

// ----------------------------------------------------------------------------------------

struct segmenter_type
{
    /*!
        WHAT THIS OBJECT REPRESENTS
            This the object that python will use directly to represent a
            sequence_segmenter.  All it does is contain all the possible template
            instantiations of a sequence_segmenter and invoke the right one depending on
            the mode variable.
    !*/

    segmenter_type() : mode(-1)
    { }

    ranges segment_sequence_dense (
        const std::vector<dense_vect>& x
    ) const
    {
        switch (mode)
        {
            case 0: return segmenter0(x);
            case 1: return segmenter1(x);
            case 2: return segmenter2(x);
            case 3: return segmenter3(x);
            case 4: return segmenter4(x);
            case 5: return segmenter5(x);
            case 6: return segmenter6(x);
            case 7: return segmenter7(x);
            default: throw dlib::error("Invalid mode");
        }
    }

    ranges segment_sequence_sparse (
        const std::vector<sparse_vect>& x
    ) const
    {
        switch (mode)
        {
            case 8: return segmenter8(x);
            case 9: return segmenter9(x);
            case 10: return segmenter10(x);
            case 11: return segmenter11(x);
            case 12: return segmenter12(x);
            case 13: return segmenter13(x);
            case 14: return segmenter14(x);
            case 15: return segmenter15(x);
            default: throw dlib::error("Invalid mode");
        }
    }

    const matrix<double,0,1> get_weights()
    {
        switch(mode)
        {
            case 0: return segmenter0.get_weights();
            case 1: return segmenter1.get_weights();
            case 2: return segmenter2.get_weights();
            case 3: return segmenter3.get_weights();
            case 4: return segmenter4.get_weights();
            case 5: return segmenter5.get_weights();
            case 6: return segmenter6.get_weights();
            case 7: return segmenter7.get_weights();

            case 8: return segmenter8.get_weights();
            case 9: return segmenter9.get_weights();
            case 10: return segmenter10.get_weights();
            case 11: return segmenter11.get_weights();
            case 12: return segmenter12.get_weights();
            case 13: return segmenter13.get_weights();
            case 14: return segmenter14.get_weights();
            case 15: return segmenter15.get_weights();

            default: throw dlib::error("Invalid mode");
        }
    }

    friend void serialize (const segmenter_type& item, std::ostream& out)
    {
        serialize(item.mode, out);
        switch(item.mode)
        {
            case 0: serialize(item.segmenter0, out); break;
            case 1: serialize(item.segmenter1, out); break;
            case 2: serialize(item.segmenter2, out); break;
            case 3: serialize(item.segmenter3, out); break;
            case 4: serialize(item.segmenter4, out); break;
            case 5: serialize(item.segmenter5, out); break;
            case 6: serialize(item.segmenter6, out); break;
            case 7: serialize(item.segmenter7, out); break;

            case 8: serialize(item.segmenter8, out); break;
            case 9: serialize(item.segmenter9, out); break;
            case 10: serialize(item.segmenter10, out); break;
            case 11: serialize(item.segmenter11, out); break;
            case 12: serialize(item.segmenter12, out); break;
            case 13: serialize(item.segmenter13, out); break;
            case 14: serialize(item.segmenter14, out); break;
            case 15: serialize(item.segmenter15, out); break;
            default: throw dlib::error("Invalid mode");
        }
    }
    friend void deserialize (segmenter_type& item, std::istream& in)
    {
        deserialize(item.mode, in);
        switch(item.mode)
        {
            case 0: deserialize(item.segmenter0, in); break;
            case 1: deserialize(item.segmenter1, in); break;
            case 2: deserialize(item.segmenter2, in); break;
            case 3: deserialize(item.segmenter3, in); break;
            case 4: deserialize(item.segmenter4, in); break;
            case 5: deserialize(item.segmenter5, in); break;
            case 6: deserialize(item.segmenter6, in); break;
            case 7: deserialize(item.segmenter7, in); break;

            case 8: deserialize(item.segmenter8, in); break;
            case 9: deserialize(item.segmenter9, in); break;
            case 10: deserialize(item.segmenter10, in); break;
            case 11: deserialize(item.segmenter11, in); break;
            case 12: deserialize(item.segmenter12, in); break;
            case 13: deserialize(item.segmenter13, in); break;
            case 14: deserialize(item.segmenter14, in); break;
            case 15: deserialize(item.segmenter15, in); break;
            default: throw dlib::error("Invalid mode");
        }
    }

    int mode;

    typedef segmenter_feature_extractor<dense_vect, false,false,false> fe0;
    typedef segmenter_feature_extractor<dense_vect, false,false,true>  fe1;
    typedef segmenter_feature_extractor<dense_vect, false,true, false> fe2;
    typedef segmenter_feature_extractor<dense_vect, false,true, true>  fe3;
    typedef segmenter_feature_extractor<dense_vect, true, false,false> fe4;
    typedef segmenter_feature_extractor<dense_vect, true, false,true>  fe5;
    typedef segmenter_feature_extractor<dense_vect, true, true, false> fe6;
    typedef segmenter_feature_extractor<dense_vect, true, true, true>  fe7;
    sequence_segmenter<fe0> segmenter0;
    sequence_segmenter<fe1> segmenter1;
    sequence_segmenter<fe2> segmenter2;
    sequence_segmenter<fe3> segmenter3;
    sequence_segmenter<fe4> segmenter4;
    sequence_segmenter<fe5> segmenter5;
    sequence_segmenter<fe6> segmenter6;
    sequence_segmenter<fe7> segmenter7;

    typedef segmenter_feature_extractor<sparse_vect, false,false,false> fe8;
    typedef segmenter_feature_extractor<sparse_vect, false,false,true>  fe9;
    typedef segmenter_feature_extractor<sparse_vect, false,true, false> fe10;
    typedef segmenter_feature_extractor<sparse_vect, false,true, true>  fe11;
    typedef segmenter_feature_extractor<sparse_vect, true, false,false> fe12;
    typedef segmenter_feature_extractor<sparse_vect, true, false,true>  fe13;
    typedef segmenter_feature_extractor<sparse_vect, true, true, false> fe14;
    typedef segmenter_feature_extractor<sparse_vect, true, true, true>  fe15;
    sequence_segmenter<fe8> segmenter8;
    sequence_segmenter<fe9> segmenter9;
    sequence_segmenter<fe10> segmenter10;
    sequence_segmenter<fe11> segmenter11;
    sequence_segmenter<fe12> segmenter12;
    sequence_segmenter<fe13> segmenter13;
    sequence_segmenter<fe14> segmenter14;
    sequence_segmenter<fe15> segmenter15;
};

// ----------------------------------------------------------------------------------------

struct segmenter_params
{
    segmenter_params()
    {
        use_BIO_model = true;
        use_high_order_features = true;
        allow_negative_weights = true;
        window_size = 5;
        num_threads = 4;
        epsilon = 0.1;
        max_cache_size = 40;
        be_verbose = false;
        C = 100;
    }

    bool use_BIO_model;
    bool use_high_order_features;
    bool allow_negative_weights;
    unsigned long window_size;
    unsigned long num_threads;
    double epsilon;
    unsigned long max_cache_size;
    bool be_verbose;
    double C;
};

string segmenter_params__str__(const segmenter_params& p)
{
    ostringstream sout;
    if (p.use_BIO_model)
        sout << "BIO,";
    else
        sout << "BILOU,";

    if (p.use_high_order_features)
        sout << "highFeats,";
    else
        sout << "lowFeats,";

    if (p.allow_negative_weights)
        sout << "signed,";
    else
        sout << "non-negative,";

    sout << "win="<<p.window_size << ",";
    sout << "threads="<<p.num_threads << ",";
    sout << "eps="<<p.epsilon << ",";
    sout << "cache="<<p.max_cache_size << ",";
    if (p.be_verbose)
        sout << "verbose,";
    else
        sout << "non-verbose,";
    sout << "C="<<p.C;
    return trim(sout.str());
}

string segmenter_params__repr__(const segmenter_params& p)
{
    ostringstream sout;
    sout << "<";
    sout << segmenter_params__str__(p);
    sout << ">";
    return sout.str();
}

void serialize ( const segmenter_params& item, std::ostream& out)
{
    serialize(item.use_BIO_model, out);
    serialize(item.use_high_order_features, out);
    serialize(item.allow_negative_weights, out);
    serialize(item.window_size, out);
    serialize(item.num_threads, out);
    serialize(item.epsilon, out);
    serialize(item.max_cache_size, out);
    serialize(item.be_verbose, out);
    serialize(item.C, out);
}

void deserialize (segmenter_params& item, std::istream& in)
{
    deserialize(item.use_BIO_model, in);
    deserialize(item.use_high_order_features, in);
    deserialize(item.allow_negative_weights, in);
    deserialize(item.window_size, in);
    deserialize(item.num_threads, in);
    deserialize(item.epsilon, in);
    deserialize(item.max_cache_size, in);
    deserialize(item.be_verbose, in);
    deserialize(item.C, in);
}

// ----------------------------------------------------------------------------------------

template <typename T>
void configure_trainer (
    const std::vector<std::vector<dense_vect> >& samples,
    structural_sequence_segmentation_trainer<T>& trainer,
    const segmenter_params& params
)
{
    pyassert(samples.size() != 0, "Invalid arguments.  You must give some training sequences.");
    pyassert(samples[0].size() != 0, "Invalid arguments. You can't have zero length training sequences.");
    pyassert(params.window_size != 0, "Invalid window_size parameter, it must be > 0.");
    pyassert(params.epsilon > 0, "Invalid epsilon parameter, it must be > 0.");
    pyassert(params.C > 0, "Invalid C parameter, it must be > 0.");
    const long dims = samples[0][0].size();

    trainer = structural_sequence_segmentation_trainer<T>(T(dims, params.window_size));
    trainer.set_num_threads(params.num_threads);
    trainer.set_epsilon(params.epsilon);
    trainer.set_max_cache_size(params.max_cache_size);
    trainer.set_c(params.C);
    if (params.be_verbose)
        trainer.be_verbose();
}

// ----------------------------------------------------------------------------------------

template <typename T>
void configure_trainer (
    const std::vector<std::vector<sparse_vect> >& samples,
    structural_sequence_segmentation_trainer<T>& trainer,
    const segmenter_params& params
)
{
    pyassert(samples.size() != 0, "Invalid arguments.  You must give some training sequences.");
    pyassert(samples[0].size() != 0, "Invalid arguments. You can't have zero length training sequences.");

    unsigned long dims = 0;
    for (unsigned long i = 0; i < samples.size(); ++i)
    {
        dims = std::max(dims, max_index_plus_one(samples[i]));
    }

    trainer = structural_sequence_segmentation_trainer<T>(T(dims, params.window_size));
    trainer.set_num_threads(params.num_threads);
    trainer.set_epsilon(params.epsilon);
    trainer.set_max_cache_size(params.max_cache_size);
    trainer.set_c(params.C);
    if (params.be_verbose)
        trainer.be_verbose();
}

// ----------------------------------------------------------------------------------------

segmenter_type train_dense (
    const std::vector<std::vector<dense_vect> >& samples,
    const std::vector<ranges>& segments,
    segmenter_params params
)
{
    pyassert(is_sequence_segmentation_problem(samples, segments), "Invalid inputs");

    int mode = 0;
    if (params.use_BIO_model)
        mode = mode*2 + 1;
    else
        mode = mode*2;
    if (params.use_high_order_features)
        mode = mode*2 + 1;
    else
        mode = mode*2;
    if (params.allow_negative_weights)
        mode = mode*2 + 1;
    else
        mode = mode*2;

    segmenter_type res;
    res.mode = mode;
    switch(mode)
    {
        case 0: { structural_sequence_segmentation_trainer<segmenter_type::fe0> trainer;
                  configure_trainer(samples, trainer, params);
                  res.segmenter0 = trainer.train(samples, segments);
                } break;
        case 1: { structural_sequence_segmentation_trainer<segmenter_type::fe1> trainer;
                  configure_trainer(samples, trainer, params);
                  res.segmenter1 = trainer.train(samples, segments);
                } break;
        case 2: { structural_sequence_segmentation_trainer<segmenter_type::fe2> trainer;
                  configure_trainer(samples, trainer, params);
                  res.segmenter2 = trainer.train(samples, segments);
                } break;
        case 3: { structural_sequence_segmentation_trainer<segmenter_type::fe3> trainer;
                  configure_trainer(samples, trainer, params);
                  res.segmenter3 = trainer.train(samples, segments);
                } break;
        case 4: { structural_sequence_segmentation_trainer<segmenter_type::fe4> trainer;
                  configure_trainer(samples, trainer, params);
                  res.segmenter4 = trainer.train(samples, segments);
                } break;
        case 5: { structural_sequence_segmentation_trainer<segmenter_type::fe5> trainer;
                  configure_trainer(samples, trainer, params);
                  res.segmenter5 = trainer.train(samples, segments);
                } break;
        case 6: { structural_sequence_segmentation_trainer<segmenter_type::fe6> trainer;
                  configure_trainer(samples, trainer, params);
                  res.segmenter6 = trainer.train(samples, segments);
                } break;
        case 7: { structural_sequence_segmentation_trainer<segmenter_type::fe7> trainer;
                  configure_trainer(samples, trainer, params);
                  res.segmenter7 = trainer.train(samples, segments);
                } break;
        default: throw dlib::error("Invalid mode");
    }

    return res;
}

// ----------------------------------------------------------------------------------------

segmenter_type train_sparse (
    const std::vector<std::vector<sparse_vect> >& samples,
    const std::vector<ranges>& segments,
    segmenter_params params
)
{
    pyassert(is_sequence_segmentation_problem(samples, segments), "Invalid inputs");

    int mode = 0;
    if (params.use_BIO_model)
        mode = mode*2 + 1;
    else
        mode = mode*2;
    if (params.use_high_order_features)
        mode = mode*2 + 1;
    else
        mode = mode*2;
    if (params.allow_negative_weights)
        mode = mode*2 + 1;
    else
        mode = mode*2;

    mode += 8;

    segmenter_type res;
    res.mode = mode;
    switch(mode)
    {
        case 8: { structural_sequence_segmentation_trainer<segmenter_type::fe8> trainer;
                  configure_trainer(samples, trainer, params);
                  res.segmenter8 = trainer.train(samples, segments);
                } break;
        case 9: { structural_sequence_segmentation_trainer<segmenter_type::fe9> trainer;
                  configure_trainer(samples, trainer, params);
                  res.segmenter9 = trainer.train(samples, segments);
                } break;
        case 10: { structural_sequence_segmentation_trainer<segmenter_type::fe10> trainer;
                  configure_trainer(samples, trainer, params);
                  res.segmenter10 = trainer.train(samples, segments);
                } break;
        case 11: { structural_sequence_segmentation_trainer<segmenter_type::fe11> trainer;
                  configure_trainer(samples, trainer, params);
                  res.segmenter11 = trainer.train(samples, segments);
                } break;
        case 12: { structural_sequence_segmentation_trainer<segmenter_type::fe12> trainer;
                  configure_trainer(samples, trainer, params);
                  res.segmenter12 = trainer.train(samples, segments);
                } break;
        case 13: { structural_sequence_segmentation_trainer<segmenter_type::fe13> trainer;
                  configure_trainer(samples, trainer, params);
                  res.segmenter13 = trainer.train(samples, segments);
                } break;
        case 14: { structural_sequence_segmentation_trainer<segmenter_type::fe14> trainer;
                  configure_trainer(samples, trainer, params);
                  res.segmenter14 = trainer.train(samples, segments);
                } break;
        case 15: { structural_sequence_segmentation_trainer<segmenter_type::fe15> trainer;
                  configure_trainer(samples, trainer, params);
                  res.segmenter15 = trainer.train(samples, segments);
                } break;
        default: throw dlib::error("Invalid mode");
    }

    return res;
}

// ----------------------------------------------------------------------------------------

struct segmenter_test
{
    double precision;
    double recall;
    double f1;
};

void serialize(const segmenter_test& item, std::ostream& out)
{
    serialize(item.precision, out);
    serialize(item.recall, out);
    serialize(item.f1, out);
}

void deserialize(segmenter_test& item, std::istream& in)
{
    deserialize(item.precision, in);
    deserialize(item.recall, in);
    deserialize(item.f1, in);
}

std::string segmenter_test__str__(const segmenter_test& item)
{
    std::ostringstream sout;
    sout << "precision: "<< item.precision << "  recall: "<< item.recall << "  f1-score: " << item.f1;
    return sout.str();
}
std::string segmenter_test__repr__(const segmenter_test& item) { return "< " + segmenter_test__str__(item) + " >";}

// ----------------------------------------------------------------------------------------

const segmenter_test test_sequence_segmenter1 (
    const segmenter_type& segmenter,
    const std::vector<std::vector<dense_vect> >& samples,
    const std::vector<ranges>& segments
)
{
    pyassert(is_sequence_segmentation_problem(samples, segments), "Invalid inputs");
    matrix<double,1,3> res;

    switch(segmenter.mode)
    {
        case 0: res = test_sequence_segmenter(segmenter.segmenter0, samples, segments); break;
        case 1: res = test_sequence_segmenter(segmenter.segmenter1, samples, segments); break;
        case 2: res = test_sequence_segmenter(segmenter.segmenter2, samples, segments); break;
        case 3: res = test_sequence_segmenter(segmenter.segmenter3, samples, segments); break;
        case 4: res = test_sequence_segmenter(segmenter.segmenter4, samples, segments); break;
        case 5: res = test_sequence_segmenter(segmenter.segmenter5, samples, segments); break;
        case 6: res = test_sequence_segmenter(segmenter.segmenter6, samples, segments); break;
        case 7: res = test_sequence_segmenter(segmenter.segmenter7, samples, segments); break;
        default: throw dlib::error("Invalid mode");
    }

    segmenter_test temp;
    temp.precision = res(0);
    temp.recall = res(1);
    temp.f1 = res(2);
    return temp;
}

const segmenter_test test_sequence_segmenter2 (
    const segmenter_type& segmenter,
    const std::vector<std::vector<sparse_vect> >& samples,
    const std::vector<ranges>& segments
)
{
    pyassert(is_sequence_segmentation_problem(samples, segments), "Invalid inputs");
    matrix<double,1,3> res;

    switch(segmenter.mode)
    {
        case 8: res = test_sequence_segmenter(segmenter.segmenter8, samples, segments); break;
        case 9: res = test_sequence_segmenter(segmenter.segmenter9, samples, segments); break;
        case 10: res = test_sequence_segmenter(segmenter.segmenter10, samples, segments); break;
        case 11: res = test_sequence_segmenter(segmenter.segmenter11, samples, segments); break;
        case 12: res = test_sequence_segmenter(segmenter.segmenter12, samples, segments); break;
        case 13: res = test_sequence_segmenter(segmenter.segmenter13, samples, segments); break;
        case 14: res = test_sequence_segmenter(segmenter.segmenter14, samples, segments); break;
        case 15: res = test_sequence_segmenter(segmenter.segmenter15, samples, segments); break;
        default: throw dlib::error("Invalid mode");
    }

    segmenter_test temp;
    temp.precision = res(0);
    temp.recall = res(1);
    temp.f1 = res(2);
    return temp;
}

// ----------------------------------------------------------------------------------------

const segmenter_test cross_validate_sequence_segmenter1 (
    const std::vector<std::vector<dense_vect> >& samples,
    const std::vector<ranges>& segments,
    long folds,
    segmenter_params params
)
{
    pyassert(is_sequence_segmentation_problem(samples, segments), "Invalid inputs");
    pyassert(1 < folds && folds <= static_cast<long>(samples.size()), "folds argument is outside the valid range.");

    matrix<double,1,3> res;

    int mode = 0;
    if (params.use_BIO_model)
        mode = mode*2 + 1;
    else
        mode = mode*2;
    if (params.use_high_order_features)
        mode = mode*2 + 1;
    else
        mode = mode*2;
    if (params.allow_negative_weights)
        mode = mode*2 + 1;
    else
        mode = mode*2;

    switch(mode)
    {
        case 0: { structural_sequence_segmentation_trainer<segmenter_type::fe0> trainer;
                  configure_trainer(samples, trainer, params);
                  res = cross_validate_sequence_segmenter(trainer, samples, segments, folds);
                } break;
        case 1: { structural_sequence_segmentation_trainer<segmenter_type::fe1> trainer;
                  configure_trainer(samples, trainer, params);
                  res = cross_validate_sequence_segmenter(trainer, samples, segments, folds);
                } break;
        case 2: { structural_sequence_segmentation_trainer<segmenter_type::fe2> trainer;
                  configure_trainer(samples, trainer, params);
                  res = cross_validate_sequence_segmenter(trainer, samples, segments, folds);
                } break;
        case 3: { structural_sequence_segmentation_trainer<segmenter_type::fe3> trainer;
                  configure_trainer(samples, trainer, params);
                  res = cross_validate_sequence_segmenter(trainer, samples, segments, folds);
                } break;
        case 4: { structural_sequence_segmentation_trainer<segmenter_type::fe4> trainer;
                  configure_trainer(samples, trainer, params);
                  res = cross_validate_sequence_segmenter(trainer, samples, segments, folds);
                } break;
        case 5: { structural_sequence_segmentation_trainer<segmenter_type::fe5> trainer;
                  configure_trainer(samples, trainer, params);
                  res = cross_validate_sequence_segmenter(trainer, samples, segments, folds);
                } break;
        case 6: { structural_sequence_segmentation_trainer<segmenter_type::fe6> trainer;
                  configure_trainer(samples, trainer, params);
                  res = cross_validate_sequence_segmenter(trainer, samples, segments, folds);
                } break;
        case 7: { structural_sequence_segmentation_trainer<segmenter_type::fe7> trainer;
                  configure_trainer(samples, trainer, params);
                  res = cross_validate_sequence_segmenter(trainer, samples, segments, folds);
                } break;
        default: throw dlib::error("Invalid mode");
    }

    segmenter_test temp;
    temp.precision = res(0);
    temp.recall = res(1);
    temp.f1 = res(2);
    return temp;
}

const segmenter_test cross_validate_sequence_segmenter2 (
    const std::vector<std::vector<sparse_vect> >& samples,
    const std::vector<ranges>& segments,
    long folds,
    segmenter_params params
)
{
    pyassert(is_sequence_segmentation_problem(samples, segments), "Invalid inputs");
    pyassert(1 < folds && folds <= static_cast<long>(samples.size()), "folds argument is outside the valid range.");

    matrix<double,1,3> res;

    int mode = 0;
    if (params.use_BIO_model)
        mode = mode*2 + 1;
    else
        mode = mode*2;
    if (params.use_high_order_features)
        mode = mode*2 + 1;
    else
        mode = mode*2;
    if (params.allow_negative_weights)
        mode = mode*2 + 1;
    else
        mode = mode*2;

    mode += 8;

    switch(mode)
    {
        case 8: { structural_sequence_segmentation_trainer<segmenter_type::fe8> trainer;
                  configure_trainer(samples, trainer, params);
                  res = cross_validate_sequence_segmenter(trainer, samples, segments, folds);
                } break;
        case 9: { structural_sequence_segmentation_trainer<segmenter_type::fe9> trainer;
                  configure_trainer(samples, trainer, params);
                  res = cross_validate_sequence_segmenter(trainer, samples, segments, folds);
                } break;
        case 10: { structural_sequence_segmentation_trainer<segmenter_type::fe10> trainer;
                  configure_trainer(samples, trainer, params);
                  res = cross_validate_sequence_segmenter(trainer, samples, segments, folds);
                } break;
        case 11: { structural_sequence_segmentation_trainer<segmenter_type::fe11> trainer;
                  configure_trainer(samples, trainer, params);
                  res = cross_validate_sequence_segmenter(trainer, samples, segments, folds);
                } break;
        case 12: { structural_sequence_segmentation_trainer<segmenter_type::fe12> trainer;
                  configure_trainer(samples, trainer, params);
                  res = cross_validate_sequence_segmenter(trainer, samples, segments, folds);
                } break;
        case 13: { structural_sequence_segmentation_trainer<segmenter_type::fe13> trainer;
                  configure_trainer(samples, trainer, params);
                  res = cross_validate_sequence_segmenter(trainer, samples, segments, folds);
                } break;
        case 14: { structural_sequence_segmentation_trainer<segmenter_type::fe14> trainer;
                  configure_trainer(samples, trainer, params);
                  res = cross_validate_sequence_segmenter(trainer, samples, segments, folds);
                } break;
        case 15: { structural_sequence_segmentation_trainer<segmenter_type::fe15> trainer;
                  configure_trainer(samples, trainer, params);
                  res = cross_validate_sequence_segmenter(trainer, samples, segments, folds);
                } break;
        default: throw dlib::error("Invalid mode");
    }

    segmenter_test temp;
    temp.precision = res(0);
    temp.recall = res(1);
    temp.f1 = res(2);
    return temp;
}

// ----------------------------------------------------------------------------------------

void bind_sequence_segmenter(py::module& m)
{
    py::class_<segmenter_params>(m, "segmenter_params",
"This class is used to define all the optional parameters to the    \n\
train_sequence_segmenter() and cross_validate_sequence_segmenter() routines.   ")
        .def(py::init<>())
        .def_readwrite("use_BIO_model", &segmenter_params::use_BIO_model)
        .def_readwrite("use_high_order_features", &segmenter_params::use_high_order_features)
        .def_readwrite("allow_negative_weights", &segmenter_params::allow_negative_weights)
        .def_readwrite("window_size", &segmenter_params::window_size)
        .def_readwrite("num_threads", &segmenter_params::num_threads)
        .def_readwrite("epsilon", &segmenter_params::epsilon)
        .def_readwrite("max_cache_size", &segmenter_params::max_cache_size)
        .def_readwrite("C", &segmenter_params::C, "SVM C parameter")
        .def_readwrite("be_verbose", &segmenter_params::be_verbose)
        .def("__repr__",&segmenter_params__repr__)
        .def("__str__",&segmenter_params__str__)
        .def(py::pickle(&getstate<segmenter_params>, &setstate<segmenter_params>));

    py::class_<segmenter_type> (m, "segmenter_type", "This object represents a sequence segmenter and is the type of object "
        "returned by the dlib.train_sequence_segmenter() routine.")
        .def("__call__", &segmenter_type::segment_sequence_dense)
        .def("__call__", &segmenter_type::segment_sequence_sparse)
        .def_property_readonly("weights", &segmenter_type::get_weights)
        .def(py::pickle(&getstate<segmenter_type>, &setstate<segmenter_type>));

    py::class_<segmenter_test> (m, "segmenter_test", "This object is the output of the dlib.test_sequence_segmenter() and "
        "dlib.cross_validate_sequence_segmenter() routines.")
        .def_readwrite("precision", &segmenter_test::precision)
        .def_readwrite("recall", &segmenter_test::recall)
        .def_readwrite("f1", &segmenter_test::f1)
        .def("__repr__",&segmenter_test__repr__)
        .def("__str__",&segmenter_test__str__)
        .def(py::pickle(&getstate<segmenter_test>, &setstate<segmenter_test>));

    m.def("train_sequence_segmenter", train_dense, py::arg("samples"), py::arg("segments"), py::arg("params")=segmenter_params());
    m.def("train_sequence_segmenter", train_sparse, py::arg("samples"), py::arg("segments"), py::arg("params")=segmenter_params());

    m.def("test_sequence_segmenter", test_sequence_segmenter1);
    m.def("test_sequence_segmenter", test_sequence_segmenter2);

    m.def("cross_validate_sequence_segmenter", cross_validate_sequence_segmenter1,
        py::arg("samples"), py::arg("segments"), py::arg("folds"), py::arg("params")=segmenter_params());
    m.def("cross_validate_sequence_segmenter", cross_validate_sequence_segmenter2,
        py::arg("samples"), py::arg("segments"), py::arg("folds"), py::arg("params")=segmenter_params());
}


// Copyright Aleksey Gurtovoy 2001-2004
//
// 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/mpl for documentation.

// $Id: comparison.cpp 49268 2008-10-11 06:26:17Z agurtovoy $
// $Date: 2008-10-10 23:26:17 -0700 (Fri, 10 Oct 2008) $
// $Revision: 49268 $

#include <boost/mpl/comparison.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/aux_/test.hpp>

// make sure MSVC behaves nicely in presence of the following template
template< typename T > struct value {};

typedef int_<0> _0;
typedef int_<10> _10;

MPL_TEST_CASE()
{
    MPL_ASSERT_NOT(( equal_to<_0, _10> ));
    MPL_ASSERT_NOT(( equal_to<_10, _0> ));
    MPL_ASSERT(( equal_to<_10, _10> ));
}

MPL_TEST_CASE()
{
    MPL_ASSERT(( not_equal_to<_0, _10> ));
    MPL_ASSERT(( not_equal_to<_10, _0> ));
    MPL_ASSERT_NOT(( not_equal_to<_10, _10> ));
}

MPL_TEST_CASE()
{
    MPL_ASSERT(( less<_0, _10>  ));
    MPL_ASSERT_NOT(( less<_10, _0> ));
    MPL_ASSERT_NOT(( less<_10, _10> ));
}

MPL_TEST_CASE()
{
    MPL_ASSERT(( less_equal<_0, _10> ));
    MPL_ASSERT_NOT(( less_equal<_10, _0> ));
    MPL_ASSERT(( less_equal<_10, _10> ));
}

MPL_TEST_CASE()
{
    MPL_ASSERT(( greater<_10, _0> ));
    MPL_ASSERT_NOT(( greater<_0, _10> ));
    MPL_ASSERT_NOT(( greater<_10, _10> ));
}

MPL_TEST_CASE()
{
    MPL_ASSERT_NOT(( greater_equal<_0, _10> ));
    MPL_ASSERT(( greater_equal<_10, _0> ));
    MPL_ASSERT(( greater_equal<_10, _10> ));
}


#include <stdlib.h>
#include <iostream>
#include <functional>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>
#include <Eigen/Cholesky>
#include <type_traits>
#include <random>
#include <array>
#include <map>

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

#ifndef ARC_HELPERS_HPP
#define ARC_HELPERS_HPP

// Branch prediction hints
// Figure out which compiler we have
#if defined(__clang__)
    /* Clang/LLVM */
    #define likely(x) __builtin_expect(!!(x), 1)
    #define unlikely(x) __builtin_expect(!!(x), 0)
#elif defined(__ICC) || defined(__INTEL_COMPILER)
    /* Intel ICC/ICPC */
    #define likely(x) __builtin_expect(!!(x), 1)
    #define unlikely(x) __builtin_expect(!!(x), 0)
#elif defined(__GNUC__) || defined(__GNUG__)
    /* GNU GCC/G++ */
    #define likely(x) __builtin_expect(!!(x), 1)
    #define unlikely(x) __builtin_expect(!!(x), 0)
#elif defined(_MSC_VER)
    /* Microsoft Visual Studio */
    /* MSVC doesn't support branch prediction hints. Use PGO instead. */
    #define likely(x) (x)
    #define unlikely(x) (x)
#endif

// Macro to disable unused parameter compiler warnings
#define UNUSED(x) (void)(x)

namespace arc_helpers
{
    template <typename T>
    inline T SetBit(const T current, const uint32_t bit_position, const bool bit_value)
    {
        // Safety check on the type we've been called with
        static_assert((std::is_same<T, uint8_t>::value
                       || std::is_same<T, uint16_t>::value
                       || std::is_same<T, uint32_t>::value
                       || std::is_same<T, uint64_t>::value),
                      "Type must be a fixed-size unsigned integral type");
        // Do it
        T update_mask = 1;
        update_mask = update_mask << bit_position;
        if (bit_value)
        {
            return (current | update_mask);
        }
        else
        {
            update_mask = (~update_mask);
            return (current & update_mask);
        }
    }

    template <typename T>
    inline bool GetBit(const T current, const uint32_t bit_position)
    {
        const uint32_t mask = arc_helpers::SetBit((T)0, bit_position, true);
        if ((mask & current) > 0)
        {
            return true;
        }
        else
        {
            return false;
        }
    }

    template <class T>
    inline T ClampValue(const T& val, const T& min, const T& max)
    {
        return std::min(max, std::max(min, val));
    }

    template<typename Datatype, typename Allocator=std::allocator<Datatype>>
    static Eigen::MatrixXd BuildDistanceMatrix(const std::vector<Datatype, Allocator>& data, const std::function<double(const Datatype&, const Datatype&)>& distance_fn)
    {
        Eigen::MatrixXd distance_matrix(data.size(), data.size());
#ifdef ENABLE_PARALLEL
        #pragma omp parallel for schedule(guided)
#endif
        for (size_t idx = 0; idx < data.size(); idx++)
        {
            for (size_t jdx = idx; jdx < data.size(); jdx++)
            {
                const double distance = distance_fn(data[idx], data[jdx]);
                distance_matrix((ssize_t)idx, (ssize_t)jdx) = distance;
                distance_matrix((ssize_t)jdx, (ssize_t)idx) = distance;
            }
        }
        return distance_matrix;
    }

    class SplitMix64PRNG
    {
    private:

        uint64_t state_; /* The state can be seeded with any value. */

        inline uint64_t next(void)
        {
            uint64_t z = (state_ += UINT64_C(0x9E3779B97F4A7C15));
            z = (z ^ (z >> 30)) * UINT64_C(0xBF58476D1CE4E5B9);
            z = (z ^ (z >> 27)) * UINT64_C(0x94D049BB133111EB);
            return z ^ (z >> 31);
        }

    public:

        inline SplitMix64PRNG(const uint64_t seed_val)
        {
            seed(seed_val);
        }

        static constexpr uint64_t min(void)
        {
            return 0u;
        }

        static constexpr uint64_t max(void)
        {
            return std::numeric_limits<uint64_t>::max();
        }

        inline void seed(const uint64_t seed_val)
        {
            state_ = seed_val;
        }

        inline void discard(const unsigned long long z)
        {
            uint64_t temp __attribute__((unused)); // This suppresses "set but not used" warnings
            temp = 0u;
            for (unsigned long long i = 0; i < z; i++)
            {
                temp = next();
                __asm__ __volatile__(""); // This should prevent the compiler from optimizing out the loop
            }
        }

        inline uint64_t operator() (void)
        {
            return next();
        }
    };

    class XorShift128PlusPRNG
    {
    private:

        uint64_t state_1_;
        uint64_t state_2_;

        inline uint64_t next(void)
        {
            uint64_t s1 = state_1_;
            const uint64_t s0 = state_2_;
            state_1_ = s0;
            s1 ^= s1 << 23; // a
            state_2_ = s1 ^ s0 ^ (s1 >> 18) ^ (s0 >> 5); // b, c
            return state_2_ + s0;
        }

    public:

        inline XorShift128PlusPRNG(const uint64_t seed_val)
        {
            seed(seed_val);
        }

        static constexpr uint64_t min(void)
        {
            return 0u;
        }

        static constexpr uint64_t max(void)
        {
            return std::numeric_limits<uint64_t>::max();
        }

        inline void seed(const uint64_t seed_val)
        {
            SplitMix64PRNG temp_seed_gen(seed_val);
            state_1_ = temp_seed_gen();
            state_2_ = temp_seed_gen();
        }

        inline void discard(const unsigned long long z)
        {
            uint64_t temp __attribute__((unused)); // This suppresses "set but not used" warnings
            temp = 0u;
            for (unsigned long long i = 0; i < z; i++)
            {
                temp = next();
                __asm__ __volatile__(""); // This should prevent the compiler from optimizing out the loop
            }
        }

        inline uint64_t operator() (void)
        {
            return next();
        }
    };

    class XorShift1024StarPRNG
    {
    private:

        std::array<uint64_t, 16> state_;
        int32_t p;

        inline uint64_t next(void)
        {
            const uint64_t s0 = state_[(size_t)p];
            p = (p + 1) & 15;
            uint64_t s1 = state_[(size_t)p];
            s1 ^= s1 << 31; // a
            state_[(size_t)p] = s1 ^ s0 ^ (s1 >> 11) ^ (s0 >> 30); // b,c
            return state_[(size_t)p] * UINT64_C(1181783497276652981);
        }

    public:

        inline XorShift1024StarPRNG(const uint64_t seed_val)
        {
            seed(seed_val);
            p = 0;
        }

        static constexpr uint64_t min(void)
        {
            return 0u;
        }

        static constexpr uint64_t max(void)
        {
            return std::numeric_limits<uint64_t>::max();
        }

        inline void seed(const uint64_t seed_val)
        {
            SplitMix64PRNG temp_seed_gen(seed_val);
            for (size_t idx = 0u; idx < state_.size(); idx++)
            {
                state_[idx] = temp_seed_gen();
            }
        }

        inline void discard(const unsigned long long z)
        {
            uint64_t temp __attribute__((unused)); // This suppresses "set but not used" warnings
            temp = 0u;
            for (unsigned long long i = 0; i < z; i++)
            {
                temp = next();
                __asm__ __volatile__(""); // This should prevent the compiler from optimizing out the loop
            }
        }

        inline uint64_t operator() (void)
        {
            return next();
        }
    };

    class TruncatedNormalDistribution
    {
    protected:

        double mean_;
        double stddev_;
        double std_lower_bound_;
        double std_upper_bound_;

        enum CASES {TYPE_1, TYPE_2, TYPE_3, TYPE_4, NONE};
        CASES case_;
        std::uniform_real_distribution<double> uniform_unit_dist_;
        std::uniform_real_distribution<double> uniform_range_dist_;
        std::exponential_distribution<double> exponential_dist_;
        std::normal_distribution<double> normal_dist_;

        inline bool CheckSimple(const double lower_bound, const double upper_bound) const
        {
            // Init Values Used in Inequality of Interest
            const double val1 = (2 * sqrt(exp(1))) / (lower_bound + sqrt(pow(lower_bound, 2) + 4));
            const double val2 = exp((pow(lower_bound, 2) - lower_bound * sqrt(pow(lower_bound, 2) + 4)) / (4));
            if (upper_bound > lower_bound + val1 * val2)
            {
                return true;
            }
            else
            {
                return false;
            }
        }

        // Naive Accept-Reject algorithm
        template<typename Generator>
        inline double NaiveAcceptReject(const double lower_bound, const double upper_bound, Generator& prng)
        {
            while (true)
            {
                const double draw = normal_dist_(prng); // Rf_rnorm(0.0, 1.0) ; // Normal distribution (i.e. std::normal_distribution<double>)
                if ((draw <= upper_bound) && (draw >= lower_bound))
                {
                    return draw;
                }
            }
        }

        // Accept-Reject Algorithm
        template<typename Generator>
        inline double SimpleAcceptReject(const double lower_bound, Generator& prng)
        {
            // Init Values
            const double alpha = (lower_bound + sqrt(pow(lower_bound, 2) + 4.0)) / (2.0) ;
            while (true)
            {
                const double e = exponential_dist_(prng); // Rf_rexp(1.0) ; // Exponential distribution (i.e. std::exponential_distribution<double>)
                const double z = lower_bound + e / alpha;
                const double rho = exp(-pow(alpha - z, 2) / 2);
                const double u = uniform_unit_dist_(prng); //  Rf_runif(0, 1) ; // Uniform distribution (i.e. std::uniform_real_distribution<double>)
                if (u <= rho)
                {
                    return z;
                }
            }
        }

        // Accept-Reject Algorithm
        template<typename Generator>
        inline double ComplexAcceptReject(const double lower_bound, const double upper_bound, Generator& prng)
        {
            while (true)
            {
                const double z = uniform_range_dist_(prng); // Rf_runif(lower_bound, upper_bound) ; // Uniform distribution (i.e. std::uniform_real_distribution<double>)
                double rho = 0.0;
                if (0 < lower_bound)
                {
                    rho = exp((pow(lower_bound, 2) - pow(z, 2)) / 2);
                }
                else if (upper_bound < 0)
                {
                    rho = exp((pow(upper_bound, 2) - pow(z, 2)) / 2);
                }
                else if (0 < upper_bound && lower_bound < 0)
                {
                    rho = exp(- pow(z, 2) / 2);
                }
                const double u = uniform_unit_dist_(prng); // Rf_runif(0, 1) ; // Uniform distribution (i.e. std::uniform_real_distribution<double>)
                if (u <= rho)
                {
                    return z;
                }
            }
        }

        template<typename Generator>
        inline double Sample(Generator& prng)
        {
            if (case_ == TYPE_1)
            {
                const double draw = NaiveAcceptReject(std_lower_bound_, std_upper_bound_, prng);
                return mean_ + stddev_ * draw;
            }
            else if (case_ == TYPE_2)
            {
                const double draw = SimpleAcceptReject(std_lower_bound_, prng);
                return mean_ + stddev_ * draw;
            }
            else if (case_ == TYPE_3)
            {
                while (true)
                {
                    const double draw = SimpleAcceptReject(std_lower_bound_, prng); // use the simple algorithm if it is more efficient
                    if (draw <= std_upper_bound_)
                    {
                        return mean_ + stddev_ * draw;
                    }
                }
            }
            else if (case_ == TYPE_4)
            {
                const double draw = ComplexAcceptReject(std_lower_bound_, std_upper_bound_, prng);
                return mean_ + stddev_ * draw;
            }
            else
            {
                assert(case_ == NONE);
                return mean_;
            }
        }

    public:

        inline TruncatedNormalDistribution(const double mean, const double stddev, const double lower_bound, const double upper_bound) : uniform_unit_dist_(0.0, 1.0), uniform_range_dist_(lower_bound, upper_bound), exponential_dist_(1.0), normal_dist_(0.0, 1.0)
        {
            // Set operating parameters
            mean_ = mean;
            stddev_ = stddev;
            if (fabs(stddev_) == 0.0)
            {
                case_ = NONE;
            }
            else
            {
                // Standardize the lower and upper bounds
                std_lower_bound_ = (lower_bound - mean_) / stddev_;
                std_upper_bound_ = (upper_bound - mean_) / stddev_;
                // Set the operating case - i.e. which sampling method we will use
                case_ = NONE;
                if (0.0 <= std_upper_bound_ && 0.0 >= std_lower_bound_)
                {
                    case_ = TYPE_1;
                }
                if (0.0 < std_lower_bound_ && std_upper_bound_ == INFINITY)
                {
                    case_ = TYPE_2;
                }
                if (0.0 > std_upper_bound_ && std_lower_bound_ == -INFINITY)
                {
                    std_lower_bound_ = -1 * std_upper_bound_;
                    std_upper_bound_ = INFINITY;
                    stddev_ = -1 * stddev_;
                    case_ = TYPE_2;
                }
                if ((0.0 > std_upper_bound_ || 0.0 < std_lower_bound_) && !(std_upper_bound_ == INFINITY || std_lower_bound_ == -INFINITY))
                {
                    if (CheckSimple(std_lower_bound_, std_upper_bound_))
                    {
                        case_ = TYPE_3;
                    }
                    else
                    {
                        case_ = TYPE_4;
                    }
                }
                assert((case_ == TYPE_1) || (case_ == TYPE_2) || (case_ == TYPE_3) || (case_ == TYPE_4));
            }
        }

        template<typename Generator>
        inline double operator()(Generator& prng)
        {
            return Sample(prng);
        }
    };

    class MultivariteGaussianDistribution
    {
    protected:
        const Eigen::VectorXd mean_;
        const Eigen::MatrixXd norm_transform_;

        std::normal_distribution<double> unit_gaussian_dist_;

        template<typename Generator>
        inline Eigen::VectorXd Sample(Generator& prng)
        {
            Eigen::VectorXd draw;
            draw.resize(mean_.rows());

            for (ssize_t idx = 0; idx < draw.rows(); idx++)
            {
                draw(idx) = unit_gaussian_dist_(prng);
            }

            return norm_transform_ * draw + mean_;
        }

        static Eigen::MatrixXd CalculateNormTransform(const Eigen::MatrixXd& covariance)
        {
            Eigen::MatrixXd norm_transform;

            Eigen::LLT<Eigen::MatrixXd> chol_solver(covariance);

            if (chol_solver.info() == Eigen::Success)
            {
                // Use cholesky solver
                norm_transform = chol_solver.matrixL();
            }
            else
            {
                // Use eigen solver
                Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigen_solver(covariance);
                norm_transform = eigen_solver.eigenvectors() * eigen_solver.eigenvalues().cwiseMax(0.0).cwiseSqrt().asDiagonal();
            }

            return norm_transform;
        }

    public:
        inline MultivariteGaussianDistribution(const Eigen::VectorXd& mean, const Eigen::MatrixXd& covariance) : mean_(mean), norm_transform_(CalculateNormTransform(covariance)), unit_gaussian_dist_(0.0, 1.0)
        {
            assert(mean.rows() == covariance.rows());
            assert(covariance.cols() == covariance.rows());

            assert(!(norm_transform_.unaryExpr([] (const double &val) { return std::isnan(val); })).any() && "NaN Found in norm_transform in MultivariateGaussianDistribution");
            assert(!(norm_transform_.unaryExpr([] (const double &val) { return std::isinf(val); })).any() && "Inf Found in norm_transform in MultivariateGaussianDistribution");
        }

        template<typename Generator>
        inline Eigen::VectorXd operator()(Generator& prng)
        {
            return Sample(prng);
        }
    };

    class RandomRotationGenerator
    {
    protected:

        std::uniform_real_distribution<double> uniform_unit_dist_;

        // From: "Uniform Random Rotations", Ken Shoemake, Graphics Gems III, pg. 124-132
        template<typename Generator>
        inline Eigen::Quaterniond GenerateUniformRandomQuaternion(Generator& prng)
        {
            const double x0 = uniform_unit_dist_(prng);
            const double r1 = sqrt(1.0 - x0);
            const double r2 = sqrt(x0);
            const double t1 = 2.0 * M_PI * uniform_unit_dist_(prng);
            const double t2 = 2.0 * M_PI * uniform_unit_dist_(prng);
            const double c1 = cos(t1);
            const double s1 = sin(t1);
            const double c2 = cos(t2);
            const double s2 = sin(t2);
            const double x = s1 * r1;
            const double y = c1 * r1;
            const double z = s2 * r2;
            const double w = c2 * r2;
            return Eigen::Quaterniond(w, x, y, z);
        }

        // From Effective Sampling and Distance Metrics for 3D Rigid Body Path Planning, by James Kuffner, ICRA 2004
        template<typename Generator>
        Eigen::Vector3d GenerateUniformRandomEulerAngles(Generator& prng)
        {
            const double roll = M_PI * (-2.0 * uniform_unit_dist_(prng) + 1.0);
            const double pitch = acos(1.0 - 2.0 * uniform_unit_dist_(prng)) - M_PI_2;
            const double yaw = M_PI * (-2.0 * uniform_unit_dist_(prng) + 1.0);
            return Eigen::Vector3d(roll, pitch, yaw);
        }

    public:

        inline RandomRotationGenerator() : uniform_unit_dist_(0.0, 1.0) {}

        template<typename Generator>
        inline Eigen::Quaterniond GetQuaternion(Generator& prng)
        {
            return GenerateUniformRandomQuaternion(prng);
        }

        template<typename Generator>
        inline std::vector<double> GetRawQuaternion(Generator& prng)
        {
            const Eigen::Quaterniond quat = GenerateUniformRandomQuaternion(prng);
            return std::vector<double>{quat.x(), quat.y(), quat.z(), quat.w()};
        }

        template<typename Generator>
        inline Eigen::Vector3d GetEulerAngles(Generator& prng)
        {
            return GenerateUniformRandomEulerAngles(prng);
        }

        template<typename Generator>
        inline std::vector<double> GetRawEulerAngles(Generator& prng)
        {
            const Eigen::Vector3d angles = GenerateUniformRandomEulerAngles(prng);
            return std::vector<double>{angles.x(), angles.y(), angles.z()};
        }
    };

    /////////////////////////////////////////////////////////////////////////////////////////////////////////
    /////                                       PROTOTYPES ONLY                                         /////
    ///// Specializations for specific types - if you want a specialization for a new type, add it here /////
    /////////////////////////////////////////////////////////////////////////////////////////////////////////

    template<typename T>
    inline uint64_t SerializeFixedSizePOD(const T& item_to_serialize, std::vector<uint8_t>& buffer);

    template<typename T>
    inline std::pair<T, uint64_t> DeserializeFixedSizePOD(const std::vector<uint8_t>& buffer, const uint64_t current);

    template<typename T, typename Allocator=std::allocator<T>>
    inline uint64_t SerializeVector(const std::vector<T, Allocator>& vec_to_serialize, std::vector<uint8_t>& buffer, const std::function<uint64_t(const T&, std::vector<uint8_t>&)>& item_serializer);

    template<typename T, typename Allocator=std::allocator<T>>
    inline std::pair<std::vector<T, Allocator>, uint64_t> DeserializeVector(const std::vector<uint8_t>& buffer, const uint64_t current, const std::function<std::pair<T, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& item_deserializer);

    template<typename Key, typename T, typename Compare = std::less<Key>, typename Allocator = std::allocator<std::pair<const Key, T>>>
    inline uint64_t SerializeMap(const std::map<Key, T, Compare, Allocator>& map_to_serialize, std::vector<uint8_t>& buffer, const std::function<uint64_t(const Key&, std::vector<uint8_t>&)>& key_serializer, const std::function<uint64_t(const T&, std::vector<uint8_t>&)>& value_serializer);

    template<typename Key, typename T, typename Compare = std::less<Key>, typename Allocator = std::allocator<std::pair<const Key, T>>>
    inline std::pair<std::map<Key, T, Compare, Allocator>, uint64_t> DeserializeMap(const std::vector<uint8_t>& buffer, const uint64_t current, const std::function<std::pair<Key, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& key_deserializer, const std::function<std::pair<T, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& value_deserializer);

    template<typename First, typename Second>
    inline uint64_t SerializePair(const std::pair<First, Second>& pair_to_serialize, std::vector<uint8_t>& buffer, const std::function<uint64_t(const First&, std::vector<uint8_t>&)>& first_serializer, const std::function<uint64_t(const Second&, std::vector<uint8_t>&)>& second_serializer);

    template<typename First, typename Second>
    inline const std::pair<std::pair<First, Second>, uint64_t> DeserializePair(const std::vector<uint8_t>& buffer, const uint64_t current, const std::function<std::pair<First, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& first_deserializer, const std::function<std::pair<Second, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& second_deserializer);

    /////////////////////////////////////////////////////////////////////////////////////////////////////////
    /////                                   IMPLEMENTATIONS ONLY                                        /////
    ///// Specializations for specific types - if you want a specialization for a new type, add it here /////
    /////////////////////////////////////////////////////////////////////////////////////////////////////////

    template<typename T>
    inline uint64_t SerializeFixedSizePOD(const T& item_to_serialize, std::vector<uint8_t>& buffer)
    {
        const uint64_t start_buffer_size = buffer.size();
        // Fixed-size serialization via memcpy
        std::vector<uint8_t> temp_buffer(sizeof(item_to_serialize), 0x00);
        memcpy(&temp_buffer[0], &item_to_serialize, sizeof(item_to_serialize));
        // Move to buffer
        buffer.insert(buffer.end(), temp_buffer.begin(), temp_buffer.end());
        // Figure out how many bytes were written
        const uint64_t end_buffer_size = buffer.size();
        const uint64_t bytes_written = end_buffer_size - start_buffer_size;
        return bytes_written;
    }

    template<typename T>
    inline std::pair<T, uint64_t> DeserializeFixedSizePOD(const std::vector<uint8_t>& buffer, const uint64_t current)
    {
        T temp_item;
        assert(current <= buffer.size());
        assert((current + sizeof(temp_item)) <= buffer.size());
        memcpy(&temp_item, &buffer[current], sizeof(temp_item));
        return std::make_pair(temp_item, sizeof(temp_item));
    }

    template<typename T, typename Allocator>
    inline uint64_t SerializeVector(const std::vector<T, Allocator>& vec_to_serialize, std::vector<uint8_t>& buffer, const std::function<uint64_t(const T&, std::vector<uint8_t>&)>& item_serializer)
    {
        const uint64_t start_buffer_size = buffer.size();
        // First, write a uint64_t size header
        const uint64_t size = (uint64_t)vec_to_serialize.size();
        SerializeFixedSizePOD<uint64_t>(size, buffer);
        // Serialize the contained items
        for (size_t idx = 0; idx < size; idx++)
        {
            const T& current = vec_to_serialize[idx];
            item_serializer(current, buffer);
        }
        // Figure out how many bytes were written
        const uint64_t end_buffer_size = buffer.size();
        const uint64_t bytes_written = end_buffer_size - start_buffer_size;
        return bytes_written;
    }

    template<typename T, typename Allocator>
    inline std::pair<std::vector<T, Allocator>, uint64_t> DeserializeVector(const std::vector<uint8_t>& buffer, const uint64_t current, const std::function<std::pair<T, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& item_deserializer)
    {
        // First, try to load the header
        assert(current < buffer.size());
        uint64_t current_position = current;
        // Load the header
        const std::pair<uint64_t, uint64_t> deserialized_size = DeserializeFixedSizePOD<uint64_t>(buffer, current_position);
        const uint64_t size = deserialized_size.first;
        current_position += deserialized_size.second;
        // Deserialize the items
        std::vector<T, Allocator> deserialized;
        deserialized.reserve(size);
        for (uint64_t idx = 0; idx < size; idx++)
        {
            const std::pair<T, uint64_t> deserialized_item = item_deserializer(buffer, current_position);
            deserialized.push_back(deserialized_item.first);
            current_position += deserialized_item.second;
        }
        // Figure out how many bytes were read
        const uint64_t bytes_read = current_position - current;
        return std::make_pair(deserialized, bytes_read);
    }

    template<typename Key, typename T, typename Compare, typename Allocator>
    inline uint64_t SerializeMap(const std::map<Key, T, Compare, Allocator>& map_to_serialize, std::vector<uint8_t>& buffer, const std::function<uint64_t(const Key&, std::vector<uint8_t>&)>& key_serializer, const std::function<uint64_t(const T&, std::vector<uint8_t>&)>& value_serializer)
    {
        const uint64_t start_buffer_size = buffer.size();
        // First, write a uint64_t size header
        const uint64_t size = (uint64_t)map_to_serialize.size();
        SerializeFixedSizePOD<uint64_t>(size, buffer);
        // Serialize the contained items
        typename std::map<Key, T, Compare, Allocator>::const_iterator itr;
        for (itr = map_to_serialize.begin(); itr != map_to_serialize.end(); ++itr)
        {
            SerializePair<Key, T>(*itr, buffer, key_serializer, value_serializer);
        }
        // Figure out how many bytes were written
        const uint64_t end_buffer_size = buffer.size();
        const uint64_t bytes_written = end_buffer_size - start_buffer_size;
        return bytes_written;
    }

    template<typename Key, typename T, typename Compare, typename Allocator>
    inline std::pair<std::map<Key, T, Compare, Allocator>, uint64_t> DeserializeMap(const std::vector<uint8_t>& buffer, const uint64_t current, const std::function<std::pair<Key, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& key_deserializer, const std::function<std::pair<T, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& value_deserializer)
    {
        // First, try to load the header
        assert(current < buffer.size());
        uint64_t current_position = current;
        // Load the header
        const std::pair<uint64_t, uint64_t> deserialized_size = DeserializeFixedSizePOD<uint64_t>(buffer, current_position);
        const uint64_t size = deserialized_size.first;
        current_position += deserialized_size.second;
        // Deserialize the items
        std::map<Key, T, Compare, Allocator> deserialized;
        for (uint64_t idx = 0; idx < size; idx++)
        {
            std::pair<std::pair<Key, T>, uint64_t> deserialized_pair = DeserializePair(buffer, current_position, key_deserializer, value_deserializer);
            deserialized.insert(deserialized_pair.first);
            current_position += deserialized_pair.second;
        }
        // Figure out how many bytes were read
        const uint64_t bytes_read = current_position - current;
        return std::make_pair(deserialized, bytes_read);
    }

    template<typename First, typename Second>
    inline uint64_t SerializePair(const std::pair<First, Second>& pair_to_serialize, std::vector<uint8_t>& buffer, const std::function<uint64_t(const First&, std::vector<uint8_t>&)>& first_serializer, const std::function<uint64_t(const Second&, std::vector<uint8_t>&)>& second_serializer)
    {
        const uint64_t start_buffer_size = buffer.size();
        uint64_t running_total = 0u;
        // Write each element of the pair into the buffer
        running_total += first_serializer(pair_to_serialize.first, buffer);
        running_total += second_serializer(pair_to_serialize.second, buffer);
        // Figure out how many bytes were written
        const uint64_t end_buffer_size = buffer.size();
        const uint64_t bytes_written = end_buffer_size - start_buffer_size;
        assert(bytes_written == running_total);
        return bytes_written;
    }

    template<typename First, typename Second>
    inline const std::pair<std::pair<First, Second>, uint64_t> DeserializePair(const std::vector<uint8_t>& buffer, const uint64_t current, const std::function<std::pair<First, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& first_deserializer, const std::function<std::pair<Second, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& second_deserializer)
    {
        assert(current < buffer.size());
        // Deserialize each item in the pair individually
        uint64_t current_position = current;
        const std::pair<First, uint64_t> deserialized_first = first_deserializer(buffer, current_position);
        current_position += deserialized_first.second;
        const std::pair<Second, uint64_t> deserialized_second = second_deserializer(buffer, current_position);
        current_position += deserialized_second.second;
        // Build the resulting pair
        // TODO: Why can't I used make_pair here?
        const std::pair<First, Second> deserialized(deserialized_first.first, deserialized_second.first);
        // Figure out how many bytes were read
        const uint64_t bytes_read = current_position - current;
        return std::make_pair(deserialized, bytes_read);
    }

    inline void ConditionalPrint(const std::string& msg, const int32_t msg_level, const int32_t print_level)
    {
        if (unlikely(msg_level <= print_level))
        {
            std::cout << msg << std::endl;
        }
    }

    inline bool CheckAllStringsForSubstring(const std::vector<std::string>& strings, const std::string& substring)
    {
        for (size_t idx = 0; idx < strings.size(); idx++)
        {
            const std::string& candidate_string = strings[idx];
            const size_t found = candidate_string.find(substring);
            if (found == std::string::npos)
            {
                return false;
            }
        }
        return true;
    }
}

#endif // ARC_HELPERS_HPP


#pragma once
#include "GNAObject.hh"
#include "TransformationBind.hh"
#include <boost/optional.hpp>

class PartialSum: public GNAObject,
                 public TransformationBind<PartialSum> {
    public:
        using TransformationBind<PartialSum>::transformation_;
        PartialSum();
        PartialSum(double initial_value);
        PartialSum(double initial_value, double threshold);
        PartialSum(double threshold, bool);

    private:
        void init();
        void calc(FunctionArgs fargs);
        void findIdx(TypesFunctionArgs targs);
        void make_Points(TypesFunctionArgs targs);

        template<typename InputIterator>
        void findStartingPoint(InputIterator start, InputIterator end);

        boost::optional<double> m_threshold;
        boost::optional<double> m_initial_value;
        size_t m_idx = 0;
};


/*
  Copyright (c) 2020, Robin Zimmeck <robin@robin-zimmeck.de>

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions are met:

  1. Redistributions of source code must retain the above copyright notice, this
     list of conditions and the following disclaimer.

  2. 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.

  3. Neither the name of the copyright holder 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.
*/

#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

#include <sstream>

#include "minisi/math.h"
#include "minisi/io.h"
#include "minisi/literals.h"

using namespace minisi;
using namespace minisi::literals;

namespace
{
  template<typename T>
  std::string
  to_string(const T& value)
  {
    std::stringstream ss;
    ss << value;
    return ss.str();
  }
}

BOOST_AUTO_TEST_CASE(io_test)
{
  BOOST_CHECK_EQUAL(to_string(1.2_m), "1.2 m");
  BOOST_CHECK_EQUAL(to_string(1.2_s), "1.2 s");
  BOOST_CHECK_EQUAL(to_string(1.2_kg), "1.2 kg");

  BOOST_CHECK_EQUAL(to_string(1.2 * second
                                  * metre * metre
                                  * kilogram * kilogram * kilogram
                                  * ampere * ampere * ampere * ampere
                                  /
                                  (  kelvin
                                   * mole * mole
                                   * candela * candela * candela)),
                      "1.2 s·m²·kg³·A⁴·K⁻¹·mol⁻²·cd⁻³");

  BOOST_CHECK_EQUAL(to_string(sqrt(4.0 * metre)), "2 m¹⸍²");
}


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

#include <sot/torque_control/joint-torque-controller.hh>
#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>

#include <sot/torque_control/commands-helper.hh>
#include <Eigen/Dense>

namespace dynamicgraph
{
  namespace sot
  {
    namespace torque_control
    {

#define MODEL_INPUT_SIGNALS    m_motorParameterKt_pSIN << m_motorParameterKt_nSIN \
                            << m_motorParameterKf_pSIN << m_motorParameterKf_nSIN \
                            << m_motorParameterKv_pSIN << m_motorParameterKv_nSIN \
                            << m_motorParameterKa_pSIN << m_motorParameterKa_nSIN <<  m_polySignDqSIN

#define ESTIMATOR_INPUT_SIGNALS m_jointsPositionsSIN << m_jointsVelocitiesSIN << m_jointsAccelerationsSIN << \
                                m_jointsTorquesSIN << m_jointsTorquesDerivativeSIN

#define TORQUE_INTEGRAL_INPUT_SIGNALS m_KiTorqueSIN << m_torque_integral_saturationSIN
#define TORQUE_CONTROL_INPUT_SIGNALS  m_jointsTorquesDesiredSIN << m_KpTorqueSIN  << m_KdTorqueSIN \
                                      << m_coulomb_friction_compensation_percentageSIN
#define VEL_CONTROL_INPUT_SIGNALS  m_dq_desSIN << m_KdVelSIN << m_KiVelSIN \

#define ALL_INPUT_SIGNALS       ESTIMATOR_INPUT_SIGNALS << TORQUE_INTEGRAL_INPUT_SIGNALS << \
                                TORQUE_CONTROL_INPUT_SIGNALS << VEL_CONTROL_INPUT_SIGNALS << \
                                MODEL_INPUT_SIGNALS
#define ALL_OUTPUT_SIGNALS      m_uSOUT << m_torque_error_integralSOUT << m_smoothSignDqSOUT

      namespace dynamicgraph = ::dynamicgraph;
      using namespace dynamicgraph;
      using namespace dynamicgraph::command;
      using namespace std;
      using namespace Eigen;

      /// Define EntityClassName here rather than in the header file
      /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
      typedef JointTorqueController EntityClassName;

      /* --- DG FACTORY ------------------------------------------------------- */
      DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(JointTorqueController,"JointTorqueController");

      /* --- CONSTRUCTION ----------------------------------------------------- */
      /* --- CONSTRUCTION ----------------------------------------------------- */
      /* --- CONSTRUCTION ----------------------------------------------------- */
      JointTorqueController::
      JointTorqueController( const std::string & name )
        : Entity(name)
        ,CONSTRUCT_SIGNAL_IN(jointsPositions,         dynamicgraph::Vector)
        ,CONSTRUCT_SIGNAL_IN(jointsVelocities,        dynamicgraph::Vector)
        ,CONSTRUCT_SIGNAL_IN(jointsAccelerations,     dynamicgraph::Vector)
        ,CONSTRUCT_SIGNAL_IN(jointsTorques,           dynamicgraph::Vector)
        ,CONSTRUCT_SIGNAL_IN(jointsTorquesDesired,    dynamicgraph::Vector)
        ,CONSTRUCT_SIGNAL_IN(jointsTorquesDerivative, dynamicgraph::Vector)
        ,CONSTRUCT_SIGNAL_IN(dq_des,                  dynamicgraph::Vector)
        ,CONSTRUCT_SIGNAL_IN(KpTorque,                     dynamicgraph::Vector)   // proportional gain for torque feedback controller
        ,CONSTRUCT_SIGNAL_IN(KiTorque,                     dynamicgraph::Vector)   // integral gain for torque feedback controller
        ,CONSTRUCT_SIGNAL_IN(KdTorque,                     dynamicgraph::Vector)   // derivative gain for torque feedback controller
        ,CONSTRUCT_SIGNAL_IN(KdVel,                        dynamicgraph::Vector)   // derivative gain for velocity feedback
        ,CONSTRUCT_SIGNAL_IN(KiVel,                        dynamicgraph::Vector)   // integral gain for velocity feedback
        ,CONSTRUCT_SIGNAL_IN(coulomb_friction_compensation_percentage, dynamicgraph::Vector)
        ,CONSTRUCT_SIGNAL_IN(motorParameterKt_p, dynamicgraph::Vector)
        ,CONSTRUCT_SIGNAL_IN(motorParameterKt_n, dynamicgraph::Vector)
        ,CONSTRUCT_SIGNAL_IN(motorParameterKf_p, dynamicgraph::Vector)
        ,CONSTRUCT_SIGNAL_IN(motorParameterKf_n, dynamicgraph::Vector)
        ,CONSTRUCT_SIGNAL_IN(motorParameterKv_p, dynamicgraph::Vector)
        ,CONSTRUCT_SIGNAL_IN(motorParameterKv_n, dynamicgraph::Vector)
        ,CONSTRUCT_SIGNAL_IN(motorParameterKa_p, dynamicgraph::Vector)
        ,CONSTRUCT_SIGNAL_IN(motorParameterKa_n, dynamicgraph::Vector)
        ,CONSTRUCT_SIGNAL_IN(polySignDq        , dynamicgraph::Vector)
        ,CONSTRUCT_SIGNAL_IN(torque_integral_saturation, dynamicgraph::Vector)
        ,CONSTRUCT_SIGNAL_OUT(u,                     dynamicgraph::Vector, ESTIMATOR_INPUT_SIGNALS <<
                                                                           TORQUE_CONTROL_INPUT_SIGNALS <<
                                                                           VEL_CONTROL_INPUT_SIGNALS <<
                                                                           MODEL_INPUT_SIGNALS <<
                                                                           m_torque_error_integralSOUT)
        ,CONSTRUCT_SIGNAL_OUT(torque_error_integral, dynamicgraph::Vector, m_jointsTorquesSIN <<
                                                                           m_jointsTorquesDesiredSIN <<
                                                                           TORQUE_INTEGRAL_INPUT_SIGNALS )
        ,CONSTRUCT_SIGNAL_OUT(smoothSignDq,          dynamicgraph::Vector, m_jointsVelocitiesSIN )
      {
        Entity::signalRegistration( ALL_INPUT_SIGNALS << ALL_OUTPUT_SIGNALS);

        /* Commands. */
        addCommand("getTimestep", makeDirectGetter(*this,&m_dt,
                                  docDirectGetter("Control timestep","double")));

        addCommand("init", makeCommandVoid2(*this, &JointTorqueController::init,
                              docCommandVoid2("Initialize the controller.",
                                              "Control timestep [s].",
					      "Robot reference (string)")));
        addCommand("reset_integral", makeCommandVoid0(*this, &JointTorqueController::reset_integral,
                                     docCommandVoid0("Reset the integral error.")));
      }

      /* --- COMMANDS ---------------------------------------------------------- */
      /* --- COMMANDS ---------------------------------------------------------- */
      /* --- COMMANDS ---------------------------------------------------------- */
      void JointTorqueController::init(const double &timestep,
				       const std::string &robot_ref)
      {
        assert(timestep>0.0 && "Timestep should be > 0");
        if(!m_jointsVelocitiesSIN.isPlugged())
          return SEND_MSG("Init failed: signal jointsVelocities is not plugged", MSG_TYPE_ERROR);
        if(!m_jointsTorquesSIN.isPlugged())
          return SEND_MSG("Init failed: signal m_jointsTorquesSIN is not plugged", MSG_TYPE_ERROR);
        if(!m_jointsTorquesDesiredSIN.isPlugged())
          return SEND_MSG("Init failed: signal m_jointsTorquesDesiredSIN is not plugged", MSG_TYPE_ERROR);
        if(!m_KpTorqueSIN.isPlugged())
          return SEND_MSG("Init failed: signal m_KpTorqueSIN is not plugged", MSG_TYPE_ERROR);
        if(!m_KiTorqueSIN.isPlugged())
          return SEND_MSG("Init failed: signal m_KiTorqueSIN is not plugged", MSG_TYPE_ERROR);

        /* Retrieve m_robot_util  informations */
        std::string localName(robot_ref);
        if (isNameInRobotUtil(localName))
        {
          m_robot_util = getRobotUtil(localName);
        }
        else
        {
          SEND_MSG("You should have an entity controller manager initialized before",MSG_TYPE_ERROR);
          return;
        }

        m_dt = timestep;
        m_tau_star.setZero(m_robot_util->m_nbJoints);
        m_current_des.setZero(m_robot_util->m_nbJoints);
        m_tauErrIntegral.setZero(m_robot_util->m_nbJoints);
//        m_dqDesIntegral.setZero(m_robot_util->m_nbJoints);
        m_dqErrIntegral.setZero(m_robot_util->m_nbJoints);
      }

      void JointTorqueController::reset_integral()
      {
        m_tauErrIntegral.setZero();
        m_dqErrIntegral.setZero();
      }

      /* --- SIGNALS ---------------------------------------------------------- */
      /* --- SIGNALS ---------------------------------------------------------- */
      /* --- SIGNALS ---------------------------------------------------------- */

      DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
      {
        const Eigen::VectorXd& q                  = m_jointsPositionsSIN(iter);
        const Eigen::VectorXd& dq                 = m_jointsVelocitiesSIN(iter);
        const Eigen::VectorXd& ddq                = m_jointsAccelerationsSIN(iter);
        const Eigen::VectorXd& tau                = m_jointsTorquesSIN(iter);
        const Eigen::VectorXd& dtau               = m_jointsTorquesDerivativeSIN(iter);
        const Eigen::VectorXd& tau_d              = m_jointsTorquesDesiredSIN(iter);
//        const Eigen::VectorXd& dtau_d             = m_jointsTorquesDesiredDerivativeSIN(iter);
        const Eigen::VectorXd& dq_des             = m_dq_desSIN(iter);
        const Eigen::VectorXd& kp                 = m_KpTorqueSIN(iter);
        const Eigen::VectorXd& kd                 = m_KdTorqueSIN(iter);
        const Eigen::VectorXd& kd_vel             = m_KdVelSIN(iter);
        const Eigen::VectorXd& ki_vel             = m_KiVelSIN(iter);
        const Eigen::VectorXd& tauErrInt          = m_torque_error_integralSOUT(iter);
        const Eigen::VectorXd& colFricCompPerc    = m_coulomb_friction_compensation_percentageSIN(iter);
        const Eigen::VectorXd& motorParameterKt_p = m_motorParameterKt_pSIN(iter);
        const Eigen::VectorXd& motorParameterKt_n = m_motorParameterKt_nSIN(iter);
        const Eigen::VectorXd& motorParameterKf_p = m_motorParameterKf_pSIN(iter);
        const Eigen::VectorXd& motorParameterKf_n = m_motorParameterKf_nSIN(iter);
        const Eigen::VectorXd& motorParameterKv_p = m_motorParameterKv_pSIN(iter);
        const Eigen::VectorXd& motorParameterKv_n = m_motorParameterKv_nSIN(iter);
        const Eigen::VectorXd& motorParameterKa_p = m_motorParameterKa_pSIN(iter);
        const Eigen::VectorXd& motorParameterKa_n = m_motorParameterKa_nSIN(iter);
        const Eigen::VectorXd& polySignDq         = m_polySignDqSIN(iter);
//        const Eigen::VectorXd& dq_thr =        m_dq_thresholdSIN(iter);

        m_tau_star = tau_d + kp.cwiseProduct(tau_d - tau) + tauErrInt - kd.cwiseProduct(dtau);

        int offset = 0;
        if(dq.size()==(int)(m_robot_util->m_nbJoints+6))
          offset = 6;

        m_dqErrIntegral += m_dt * ki_vel.cwiseProduct(dq_des-dq);
        const Eigen::VectorXd& err_int_sat =   m_torque_integral_saturationSIN(iter);
        // saturate
        bool saturating = false;
        for(int i=0; i<(int)m_robot_util->m_nbJoints; i++)
        {
          if(m_dqErrIntegral(i) > err_int_sat(i))
          {
            saturating = true;
            m_dqErrIntegral(i) = err_int_sat(i);
          }
          else if(m_dqErrIntegral(i) < -err_int_sat(i))
          {
            saturating = true;
            m_dqErrIntegral(i) = -err_int_sat(i);
          }
        }
        if(saturating)
          SEND_INFO_STREAM_MSG("Saturate dqErr integral: "+toString(m_dqErrIntegral.head<12>()));

        for(int i=0; i<(int)m_robot_util->m_nbJoints; i++)
        {
          m_current_des(i) = motorModel.getCurrent(m_tau_star(i),
                                                   dq(i+offset)+kd_vel(i)*(dq_des(i)-dq(i+offset)) + m_dqErrIntegral(i), //ki_vel(i)*(m_dqDesIntegral(i)-q(i)),
                                                   ddq(i+offset),
                                                   motorParameterKt_p(i),
                                                   motorParameterKt_n(i),
                                                   motorParameterKf_p(i)*colFricCompPerc(i),
                                                   motorParameterKf_n(i)*colFricCompPerc(i),
                                                   motorParameterKv_p(i),
                                                   motorParameterKv_n(i),
                                                   motorParameterKa_p(i),
                                                   motorParameterKa_n(i),
                                                   polySignDq(i));
        }

        s = m_current_des;
        return s;
      }

      DEFINE_SIGNAL_OUT_FUNCTION(torque_error_integral, dynamicgraph::Vector)
      {
        const Eigen::VectorXd& tau =           m_jointsTorquesSIN(iter);
        const Eigen::VectorXd& tau_d =         m_jointsTorquesDesiredSIN(iter);
        const Eigen::VectorXd& err_int_sat =   m_torque_integral_saturationSIN(iter);
        const Eigen::VectorXd& ki =            m_KiTorqueSIN(iter);

        // compute torque error integral and saturate
        m_tauErrIntegral += m_dt * ki.cwiseProduct(tau_d-tau);
        for(int i=0; i<(int)m_robot_util->m_nbJoints; i++)
        {
          if(m_tauErrIntegral(i) > err_int_sat(i))
            m_tauErrIntegral(i) = err_int_sat(i);
          else if(m_tauErrIntegral(i) < -err_int_sat(i))
            m_tauErrIntegral(i) = -err_int_sat(i);
        }

        s = m_tauErrIntegral;
        return s;
      }

      DEFINE_SIGNAL_OUT_FUNCTION(smoothSignDq, dynamicgraph::Vector)
      {
        const Eigen::VectorXd& dq =            m_jointsVelocitiesSIN(iter);
        const Eigen::VectorXd& polySignDq =    m_polySignDqSIN(iter);
        if(s.size()!=(int)m_robot_util->m_nbJoints)
          s.resize(m_robot_util->m_nbJoints);

        for(int i=0; i<(int)m_robot_util->m_nbJoints; i++)
          s(i) = motorModel.smoothSign(dq[i], 0.1, polySignDq[i]); //TODO Use Eigen binaryexpr
        return s;
      }

      void JointTorqueController::display( std::ostream& os ) const
      {
        os << "JointTorqueController "<<getName()<<":\n";
        try
        {
          getProfiler().report_all(3, os);
        }
        catch (ExceptionSignal e) {}
      }

    } // namespace torque_control
  } // namespace sot
} // namespace dynamicgraph


#include <stan/math/prim/arr.hpp>
#include <gtest/gtest.h>
#include <iostream>
#include <sstream>
#include <vector>
#include <boost/numeric/odeint.hpp>
#include <test/unit/math/prim/arr/functor/harmonic_oscillator.hpp>
#include <test/unit/math/prim/arr/functor/lorenz.hpp>
#include <test/unit/util.hpp>

template <typename F>
void sho_death_test(F harm_osc,
                    std::vector<double>& y0,
                    double t0,
                    std::vector<double>& ts,
                    std::vector<double>& theta,
                    std::vector<double>& x,
                    std::vector<int>& x_int) {
  EXPECT_DEATH(stan::math::integrate_ode_rk45(harm_osc, y0, t0,
                                              ts, theta, x, x_int),
               "");
}

template <typename F>
void sho_value_test(F harm_osc,
                    std::vector<double>& y0,
                    double t0,
                    std::vector<double>& ts,
                    std::vector<double>& theta,
                    std::vector<double>& x,
                    std::vector<int>& x_int) {

  std::vector<std::vector<double> >  ode_res_vd
    = stan::math::integrate_ode_rk45(harm_osc, y0, t0,
                                     ts, theta, x, x_int);
  EXPECT_NEAR(0.995029, ode_res_vd[0][0], 1e-5);
  EXPECT_NEAR(-0.0990884, ode_res_vd[0][1], 1e-5);

  EXPECT_NEAR(-0.421907, ode_res_vd[99][0], 1e-5);
  EXPECT_NEAR(0.246407, ode_res_vd[99][1], 1e-5);
}

void sho_test(double t0) {
  harm_osc_ode_fun harm_osc;

  std::vector<double> theta;
  theta.push_back(0.15);

  std::vector<double> y0;
  y0.push_back(1.0);
  y0.push_back(0.0);

  std::vector<double> ts;
  for (int i = 0; i < 100; i++)
    ts.push_back(t0 + 0.1 * (i + 1));

  std::vector<double> x;
  std::vector<int> x_int;

  sho_value_test(harm_osc, y0, t0, ts, theta, x, x_int);
}

void sho_data_test(double t0) {
  harm_osc_ode_data_fun harm_osc;

  std::vector<double> theta;
  theta.push_back(0.15);

  std::vector<double> y0;
  y0.push_back(1.0);
  y0.push_back(0.0);

  std::vector<double> ts;
  for (int i = 0; i < 100; i++)
    ts.push_back(t0 + 0.1 * (i + 1));

  std::vector<double> x(3,1);
  std::vector<int> x_int(2,0);

  sho_value_test(harm_osc, y0, t0, ts, theta, x, x_int);
}

TEST(StanMathOde_integrate_ode_rk45, harmonic_oscillator) {
  sho_test(0.0);
  sho_test(1.0);
  sho_test(-1.0);

  sho_data_test(0.0);
  sho_data_test(1.0);
  sho_data_test(-1.0);
}

TEST(StanMathOde_integrate_ode_rk45, error_conditions) {
  using stan::math::integrate_ode_rk45;
  harm_osc_ode_data_fun harm_osc;

  std::vector<double> theta;
  theta.push_back(0.15);

  std::vector<double> y0;
  y0.push_back(1.0);
  y0.push_back(0.0);

  double t0 = 0;

  std::vector<double> ts;
  for (int i = 0; i < 100; i++)
    ts.push_back(t0 + 0.1 * (i + 1));

  std::vector<double> x(3,1);
  std::vector<int> x_int(2,0);

  ASSERT_NO_THROW(integrate_ode_rk45(harm_osc, y0, t0, ts, theta, x, x_int));

  std::vector<double> y0_bad;
  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0_bad, t0, ts, theta, x, x_int),
                   std::invalid_argument,
                   "initial state has size 0");

  double t0_bad = 2.0;
  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0_bad, ts, theta, x, x_int),
                   std::domain_error,
                   "initial time is 2, but must be less than 0.1");

  std::vector<double> ts_bad;
  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0, ts_bad, theta, x, x_int),
                   std::invalid_argument,
                   "times has size 0");

  ts_bad.push_back(3);
  ts_bad.push_back(1);
  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0, ts_bad, theta, x, x_int),
                   std::domain_error,
                   "times is not a valid ordered vector");

  std::vector<double> theta_bad;
  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0, ts, theta_bad, x, x_int),
                   std::out_of_range,
                   "vector");

  std::vector<double> x_bad;
  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0, ts, theta, x_bad, x_int),
                   std::out_of_range,
                   "vector");

  std::vector<int> x_int_bad;
  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0, ts, theta, x, x_int_bad),
                   std::out_of_range,
                   "vector");

  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0, ts, theta, x, x_int,
                                      0, -1, 1e-6, 10),
                   std::invalid_argument,
                   "relative_tolerance");

  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0, ts, theta, x, x_int,
                                      0, 1e-6, -1, 10),
                   std::invalid_argument,
                   "absolute_tolerance");

  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0, ts, theta, x, x_int,
                                      0, 1e-6, 1e-6, -1),
                   std::invalid_argument,
                   "max_num_steps");
}

TEST(StanMathOde_integrate_ode_rk45, error_conditions_nan) {
  using stan::math::integrate_ode_rk45;
  harm_osc_ode_data_fun harm_osc;

  std::vector<double> theta;
  theta.push_back(0.15);

  std::vector<double> y0;
  y0.push_back(1.0);
  y0.push_back(0.0);

  double t0 = 0;

  std::vector<double> ts;
  for (int i = 0; i < 100; i++)
    ts.push_back(t0 + 0.1 * (i + 1));

  std::vector<double> x(3,1);
  std::vector<int> x_int(2,0);

  ASSERT_NO_THROW(integrate_ode_rk45(harm_osc, y0, t0, ts, theta, x, x_int));

  double nan = std::numeric_limits<double>::quiet_NaN();
  std::stringstream expected_is_nan;
  expected_is_nan << "is " << nan;

  std::vector<double> y0_bad = y0;
  y0_bad[0] = nan;
  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0_bad, t0, ts, theta, x, x_int),
                   std::domain_error,
                   "initial state");
  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0_bad, t0, ts, theta, x, x_int),
                   std::domain_error,
                   expected_is_nan.str());

  double t0_bad = nan;
  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0_bad, ts, theta, x, x_int),
                   std::domain_error,
                   "initial time");
  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0_bad, ts, theta, x, x_int),
                   std::domain_error,
                   expected_is_nan.str());

  std::vector<double> ts_bad = ts;
  ts_bad[0] = nan;
  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0, ts_bad, theta, x, x_int),
                   std::domain_error,
                   "times");
  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0, ts_bad, theta, x, x_int),
                   std::domain_error,
                   expected_is_nan.str());

  std::vector<double> theta_bad = theta;
  theta_bad[0] = nan;
  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0, ts, theta_bad, x, x_int),
                   std::domain_error,
                   "parameter vector");
  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0, ts, theta_bad, x, x_int),
                   std::domain_error,
                   expected_is_nan.str());

  if (x.size() > 0) {
    std::vector<double> x_bad = x;
    x_bad[0] = nan;
    EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0, ts, theta, x_bad, x_int),
                     std::domain_error,
                     "continuous data");
    EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0, ts, theta, x_bad, x_int),
                     std::domain_error,
                     expected_is_nan.str());
  }
}

TEST(StanMathOde_integrate_ode_rk45, error_conditions_inf) {
  using stan::math::integrate_ode_rk45;
  harm_osc_ode_data_fun harm_osc;
  std::stringstream expected_is_inf;
  expected_is_inf << "is " << std::numeric_limits<double>::infinity();
  std::stringstream expected_is_neg_inf;
  expected_is_neg_inf << "is " << -std::numeric_limits<double>::infinity();

  std::vector<double> theta;
  theta.push_back(0.15);

  std::vector<double> y0;
  y0.push_back(1.0);
  y0.push_back(0.0);

  double t0 = 0;

  std::vector<double> ts;
  for (int i = 0; i < 100; i++)
    ts.push_back(t0 + 0.1 * (i + 1));

  std::vector<double> x(3,1);
  std::vector<int> x_int(2,0);

  ASSERT_NO_THROW(integrate_ode_rk45(harm_osc, y0, t0, ts, theta, x, x_int));

  double inf = std::numeric_limits<double>::infinity();
  std::vector<double> y0_bad = y0;
  y0_bad[0] = inf;
  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0_bad, t0, ts, theta, x, x_int),
                   std::domain_error,
                   "initial state");
  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0_bad, t0, ts, theta, x, x_int),
                   std::domain_error,
                   expected_is_inf.str());

  y0_bad[0] = -inf;
  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0_bad, t0, ts, theta, x, x_int),
                   std::domain_error,
                   "initial state");
  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0_bad, t0, ts, theta, x, x_int),
                   std::domain_error,
                   expected_is_neg_inf.str());

  double t0_bad = inf;
  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0_bad, ts, theta, x, x_int),
                   std::domain_error,
                   "initial time");
  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0_bad, ts, theta, x, x_int),
                   std::domain_error,
                   expected_is_inf.str());
  t0_bad = -inf;
  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0_bad, ts, theta, x, x_int),
                   std::domain_error,
                   "initial time");
  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0_bad, ts, theta, x, x_int),
                   std::domain_error,
                   expected_is_neg_inf.str());

  std::vector<double> ts_bad = ts;
  ts_bad.push_back(inf);
  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0, ts_bad, theta, x, x_int),
                   std::domain_error,
                   "times");
  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0, ts_bad, theta, x, x_int),
                   std::domain_error,
                   expected_is_inf.str());
  ts_bad[0] = -inf;
  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0, ts_bad, theta, x, x_int),
                   std::domain_error,
                   "times");
  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0, ts_bad, theta, x, x_int),
                   std::domain_error,
                   expected_is_neg_inf.str());

  std::vector<double> theta_bad = theta;
  theta_bad[0] = inf;
  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0, ts, theta_bad, x, x_int),
                   std::domain_error,
                   "parameter vector");
  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0, ts, theta_bad, x, x_int),
                   std::domain_error,
                   expected_is_inf.str());
  theta_bad[0] = -inf;
  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0, ts, theta_bad, x, x_int),
                   std::domain_error,
                   "parameter vector");
  EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0, ts, theta_bad, x, x_int),
                   std::domain_error,
                   expected_is_neg_inf.str());

  if (x.size() > 0) {
    std::vector<double> x_bad = x;
    x_bad[0] = inf;
    EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0, ts, theta, x_bad, x_int),
                     std::domain_error,
                     "continuous data");
    EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0, ts, theta, x_bad, x_int),
                     std::domain_error,
                     expected_is_inf.str());
    x_bad[0] = -inf;
    EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0, ts, theta, x_bad, x_int),
                     std::domain_error,
                     "continuous data");
    EXPECT_THROW_MSG(integrate_ode_rk45(harm_osc, y0, t0, ts, theta, x_bad, x_int),
                     std::domain_error,
                     expected_is_neg_inf.str());
  }
}


/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2016. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

#include "gyro.h"

#include <boost/algorithm/string/replace.hpp>

GZ_REGISTER_MODEL_PLUGIN(Gyro)

void Gyro::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
  this->model = model;

  // Parse SDF properties
  link = model->GetLink(sdf->Get<std::string>("link"));
  if (sdf->HasElement("topic")) {
    topic = sdf->Get<std::string>("topic");
  } else {
    topic = "~/" + sdf->GetAttribute("name")->GetAsString();
  }

  std::string axisString = sdf->Get<std::string>("axis");
  if (axisString == "roll") axis = Roll;
  if (axisString == "pitch") axis = Pitch;
  if (axisString == "yaw") axis = Yaw;

  if (sdf->HasElement("units")) {
    radians = sdf->Get<std::string>("units") != "degrees";
  } else {
    radians = true;
  }
  zero = GetAngle();

  gzmsg << "Initializing gyro: " << topic << " link=" << link->GetName()
        << " axis=" << axis << " radians=" << radians << std::endl;

  // Connect to Gazebo transport for messaging
  std::string scoped_name =
      model->GetWorld()->GetName() + "::" + model->GetScopedName();
  boost::replace_all(scoped_name, "::", "/");
  node = gazebo::transport::NodePtr(new gazebo::transport::Node());
  node->Init(scoped_name);
  command_sub = node->Subscribe(topic + "/control", &Gyro::Callback, this);
  pos_pub = node->Advertise<gazebo::msgs::Float64>(topic + "/position");
  vel_pub = node->Advertise<gazebo::msgs::Float64>(topic + "/velocity");

  // Connect to the world update event.
  // This will trigger the Update function every Gazebo iteration
  updateConn = gazebo::event::Events::ConnectWorldUpdateBegin(
      boost::bind(&Gyro::Update, this, _1));
}

void Gyro::Update(const gazebo::common::UpdateInfo& info) {
  gazebo::msgs::Float64 pos_msg, vel_msg;
  pos_msg.set_data(Limit(GetAngle() - zero));
  pos_pub->Publish(pos_msg);
  vel_msg.set_data(GetVelocity());
  vel_pub->Publish(vel_msg);
}

void Gyro::Callback(const gazebo::msgs::ConstStringPtr& msg) {
  std::string command = msg->data();
  if (command == "reset") {
    zero = GetAngle();
  } else {
    gzerr << "WARNING: Gyro got unknown command '" << command << "'."
          << std::endl;
  }
}

double Gyro::GetAngle() {
  if (radians) {
    return link->GetWorldCoGPose().rot.GetAsEuler()[axis];
  } else {
    return link->GetWorldCoGPose().rot.GetAsEuler()[axis] * (180.0 / M_PI);
  }
}

double Gyro::GetVelocity() {
  if (radians) {
    return link->GetRelativeAngularVel()[axis];
  } else {
    return link->GetRelativeAngularVel()[axis] * (180.0 / M_PI);
  }
}

double Gyro::Limit(double value) {
  if (radians) {
    while (true) {
      if (value < -M_PI)
        value += 2 * M_PI;
      else if (value > M_PI)
        value -= 2 * M_PI;
      else
        break;
    }
  } else {
    while (true) {
      if (value < -180)
        value += 360;
      else if (value > 180)
        value -= 360;
      else
        break;
    }
  }
  return value;
}