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

/* boosting_training_tool.cc                                       -*- C++ -*-
   Jeremy Barnes, 12 June 2003
   Copyright (c) 2003 Jeremy Barnes.  All rights reserved.
   $Source$

   Tool to use to train boosting with.  Very similar in spirit to the MLP
   training tool.
*/

#include "mldb/plugins/jml/jml/training_data.h"
#include "mldb/plugins/jml/jml/training_index.h"
#include "mldb/plugins/jml/jml/dense_features.h"
#include "mldb/plugins/jml/jml/sparse_features.h"
#include "mldb/plugins/jml/jml/classifier.h"
#include "mldb/plugins/jml/jml/boosted_stumps.h"
#include "mldb/plugins/jml/jml/boosted_stumps_impl.h"
#include "mldb/plugins/jml/jml/probabilizer.h"
#include "mldb/plugins/jml/jml/decoded_classifier.h"
#include "mldb/plugins/jml/jml/boosting_tool_common.h"
#include "mldb/plugins/jml/jml/null_decoder.h"
#include "mldb/jml/utils/command_line.h"
#include "mldb/utils/file_functions.h"
#include "mldb/vfs/filter_streams.h"
#include "mldb/base/parse_context.h"
#include <boost/multi_array.hpp>
#include "mldb/plugins/jml/sgi_numeric.h"
#include "mldb/utils/vector_utils.h"
#include "mldb/utils/distribution.h"
#include "mldb/utils/selection.h"
#include <boost/progress.hpp>
#include "mldb/jml/sections/sink_stream.h"
#include "mldb/jml/utils/metrics.h"
#include <regex>
#include "mldb/utils/distribution_ops.h"
#include "mldb/plugins/jml/jml/evaluation.h"

#include <iterator>
#include <iostream>
#include <set>

using namespace std;

using namespace ML;
using namespace Math;
using namespace Stats;
using namespace Metrics;

struct Tracer {
    Tracer(size_t label_count)
        : nl(label_count)
    {
    }

    /* Record a result against the correct feature. */
    void operator () (const distribution<float> & dist, float weight,
                      const Feature & feature) const
    {
        if (scores[feature].empty())
            scores[feature] = distribution<float>(nl, 0.0);
        scores[feature] += weight * dist;
    }

    /* Indexed by the feature number and then the label number.  Tells us
       how much effect that feature has had on that label. */
    typedef map<Feature, distribution<float> > scores_type;
    mutable scores_type scores;

    size_t nl;

    /* Return the actual result. */
    distribution<float> totals() const
    {
        distribution<float> result(nl);

        for (scores_type::const_iterator it = scores.begin();
             it != scores.end();  ++it) {
            result += it->second;
        }

        return result;
    }
};

/** Small structure to accumulate the effect of a variable. */
struct Effect1 {
    Effect1()
        : correct(0.0), incorrect(0.0),
          margin_correct(0.0), margin_incorrect(0.0)
    {
    }

    void add(const distribution<float> & vals, int label, int highest,
             int next_highest)
    {
        bool is_correct = (vals[highest] == vals[label]);
        if (is_correct) {
            correct += vals[label];
            abs_correct += abs(vals[label]);
            double margin = vals[label] - vals[next_highest];
            margin_correct += margin;
        }
        else {
            incorrect += vals[label];
            abs_incorrect += abs(vals[label]);
            double margin = vals[highest] - vals[label];
            margin_incorrect += margin;
        }
    }

    double correct;
    double abs_correct;
    double incorrect;
    double abs_incorrect;
    double margin_correct;
    double margin_incorrect;

    static string key()
    {
        return "   score    corr abs_corr      inc  abs_inc";//"   m_corr    m_inc";
    }

    string print() const
    {
        return format("%8.3f %8.3f %8.3f %8.3f %8.3f",//" %8.3f %8.3f",
                      score(), correct, abs_correct, incorrect, abs_incorrect);
        //margin_correct, margin_incorrect);
    }

    double score() const
    {
        return correct - incorrect;
    }

    bool operator < (const Effect1 & other) const
    {
        return score() < other.score();
    }
};

struct Effect2 {
    Effect2()
    {
    }

    void add(const distribution<float> & vals, int label, int highest,
             int next_highest)
    {
        if (correct.size() < vals.size()) correct.resize(vals.size());
        if (incorrect.size() < vals.size()) incorrect.resize(vals.size());
        if (abs_correct.size() < vals.size()) abs_correct.resize(vals.size());
        if (abs_incorrect.size() < vals.size()) abs_incorrect.resize(vals.size());
        for (unsigned l = 0;  l < vals.size();  ++l) {
            bool is_correct = (label == l);
            if (is_correct) {
                correct[l] += vals[l];
                abs_correct[l] += abs(vals[l]);
            }
            else {
                incorrect[l] += vals[l];
                abs_incorrect[l] += abs(vals[l]);
            }
        }
    }

    distribution<double> correct;
    distribution<double> abs_correct;
    distribution<double> incorrect;
    distribution<double> abs_incorrect;

    static string key()
    {
        return "   score    corr abs_corr      inc  abs_inc";//"   m_corr    m_inc";
    }

    string print() const
    {
        string result = format("%12.3f", score());
        //for (unsigned l = 0;  l < correct.size();  ++l)
        //    result += format(" %7.1f %7.3f", correct[l] - incorrect[l],
        //                     (correct[l] / abs_correct[l])
        //                     * (incorrect[l] / abs_incorrect[l]));
        return result;
        //return format("%8.3f %8.3f %8.3f %8.3f %8.3f",//" %8.3f %8.3f",
        //              score(), correct, abs_correct, incorrect, abs_incorrect);
        //margin_correct, margin_incorrect);
    }

    double score() const
    {
        //return (((correct / abs_correct) * (incorrect / abs_incorrect))
        //        * (correct - incorrect)).total();
        return (correct * incorrect).total();
    }

    bool operator < (const Effect2 & other) const
    {
        return score() < other.score();
    }
};

typedef Effect2 Effect;

void trace_output(const Classifier & current, const Training_Data & data,
                  int feature_tracing, int verbosity = 1,
                  float min_output = 0.0, float max_output = 1.0)
{
    const Decoded_Classifier & decoded
        = dynamic_cast<const Decoded_Classifier &>(*current.impl);
    const Boosted_Stumps & stumps
        = dynamic_cast<const Boosted_Stumps &>(*decoded.classifier().impl);
    const GLZ_Probabilizer & prob
        = dynamic_cast<const GLZ_Probabilizer &>(decoded.decoder().impl());

    Feature predicted = current.predicted();

    size_t nl = current.label_count();
    size_t nx = data.example_count();

    map<Feature, Effect> effects;

    for (unsigned x = 0;  x < nx;  ++x) {
        Tracer tracer(nl);

        stumps.predict_core(data[x], tracer);

        distribution<float> input = tracer.totals();

        if (stumps.output == 1 || stumps.output == 2) {
            for (unsigned i = 0;  i < input.size();  ++i) {
                double e = exp(input[i]);
                input[i] = e / (e + 1.0 / e);
            }
            if (stumps.output == 2) input.normalize();
        }

        distribution<float> result = prob.apply(input);

        if (result[1] < min_output || result[1] > max_output) continue;

        int highest, highest_2;
        std::tie(highest, highest_2) = idxmax2(result.begin(), result.end());

        int label    = (int)data[x][predicted];
        bool correct = highest == label;

        /* Merge the two maps in an efficient manner. */
        Tracer::scores_type::const_iterator it1 = tracer.scores.begin();
        Tracer::scores_type::const_iterator e1 = tracer.scores.end();
        map<Feature, Effect>::iterator it2 = effects.begin();
        map<Feature, Effect>::iterator e2 = effects.end();

        while (it1 != e1 && it2 != e2) {
            if (it1->first == it2->first) {
                it2->second.add(it1->second, label, highest, highest_2);
                ++it1;  ++it2;
            }
            else if (it1->first < it2->first) {
                it2->second.add(it1->second, label, highest, highest_2);
                ++it1;
            }
            else ++it2;
        }
        while (it1 != e1) { // new ones at end
            effects[it1->first].add(it1->second, label, highest, highest_2);
            ++it1;
        }

        if (verbosity > 1) {
            cout << "example " << x << ":  label = " << label
                 << (correct ? " correct" : " incorrect")
                 << " (" << result[label] << ")" << endl;
        }

        for (unsigned l = 0;  l < nl;  ++l) {
            if (nl == 2 && l == 0) continue;  // trace true for binary

            vector<pair<Feature, float> > features;
            double total = 0.0;
            for (Tracer::scores_type::const_iterator it
                     = tracer.scores.begin();
                 it != tracer.scores.end();  ++it) {
                features.push_back(make_pair(it->first, abs(it->second[l])));
                total += it->second[l];
            }

            if (verbosity > 1) {
                cout << "  label " << l
                     << format("  prob: %6.3f, raw: %6.3f, total: %6.3f",
                               result[l], input[l], total)
                     << endl;
            }

            sort_on_second_descending(features);

            if (verbosity > 2) {
                for (unsigned i = 0;
                     i < std::min(features.size(), (size_t)20);  ++i) {
                    Feature_Set::const_iterator first, last;
                    std::tie(first, last)
                        = data[x].find(features[i].first);
                    if (first == last)
                        throw Exception("Feature not found");

                    cout << format("    %8.4f [%8.3g] ",
                                   tracer.scores[features[i].first][l],
                                   (*first).second)
                         << stumps.feature_space()->print(features[i].first)
                         << endl;
                }
            }
        }
    }

    if (feature_tracing != 0) {
        vector<pair<Feature, Effect> > sorted(effects.begin(), effects.end());
        sort_on_second_ascending(sorted);

        cout << "features from worst to best:" << endl;
        cout << endl;
        cout << Effect::key() << " feature" << endl;

        for (unsigned i = 0;  i < sorted.size();  ++i)
            cout << sorted[i].second.print() << " "
                 << stumps.feature_space()->print(sorted[i].first) << endl;
        cout << endl;
    }
}

void do_features(const Training_Data & data,
                 std::shared_ptr<const Feature_Space> feature_space,
                 std::vector<Feature> & features,
                 std::map<std::string, Feature> & feature_index)
{
    features.clear();
    vector<string> feature_names;

    features = data.all_features();
    std::sort(features.begin(), features.end());

    for (unsigned i = 0;  i < features.size();  ++i)
        feature_names.push_back(feature_space->print(features[i]));

    /* Get an index of all of our variables. */
    feature_index.clear();
    for (unsigned i = 0;  i < features.size();  ++i)
        feature_index[feature_space->print(features[i])] = features[i];

    //guess_all_info(data, *feature_space, true);
}

int main(int argc, char ** argv)
try
{
    ios::sync_with_stdio(false);

    bool sparse_data        = false;
    int draw_graphs         = 0;
    int print_confusion     = 0;
    bool trace_features     = false;
    int verbosity           = 1;
    int feature_tracing     = 1;
    string classifier_file;
    string group_feature_name = "";
    bool eval_by_group      = false;  // Evaluate a group at a time?
    bool eval_by_group_set  = false;  // Value of eval_by_group has been set?

    vector<string> extra;
    {
        using namespace CmdLine;

        static const Option classifier_options[] = {
            { "classifier-file", 'C', classifier_file, classifier_file,
              false, "load classifier from FILE", "FILE" },
            Last_Option
        };

        static const Option data_options[] = {
            { "sparse-data", 'S', NO_ARG, flag(sparse_data),
              false, "dataset is in sparse format" },
            Last_Option
        };

        static const Option output_options[] = {
            { "quiet", 'q', NO_ARG, assign(verbosity, 0),
              false, "don't write any non-fatal output" },
            { "verbosity", 'v', optional(2), verbosity,
              false, "set verbosity to LEVEL (0-3)", "LEVEL" },
            { "draw-graphs", 'G', NO_ARG, increment(draw_graphs),
              false, "draw graphs for two-class predictor" },
            { "print-confusion", 'C', NO_ARG, increment(print_confusion),
              false, "print confusion matrix" },
            { "trace-features", 'T', NO_ARG, flag(trace_features),
              false, "trace operation of boosting over its features" },
            { "feature-tracing", 't', feature_tracing, feature_tracing,
              true, "feature tracing: 1=correct, 2=margins", "1-2" },
            { "group-feature", 'g', group_feature_name, group_feature_name,
              false, "use FEATURE to group examples in dataset", "FEATURE" },
            { "eval-by-group", 0, NO_ARG, flag(eval_by_group, eval_by_group_set),
              false, "evaluate by group rather than by example" },
            Last_Option
        };

        static const Option options[] = {
            group("Classifier options", classifier_options),
            group("Data options", data_options),
            group("Output options",   output_options),
            Help_Options,
            Last_Option };

        Command_Line_Parser parser("boosting_testing_tool", argc, argv,
                                   options);

        bool res = parser.parse();
        if (res == false) exit(1);

        extra.insert(extra.end(), parser.extra_begin(), parser.extra_end());
    }

    if (extra.empty()) {
        cerr << "error: need to specify testing data" << endl;
        exit(1);
    }

    vector<std::shared_ptr<Training_Data> > data(extra.size());
    ssize_t var_count = -1;

    std::shared_ptr<Dense_Feature_Space> dense_feature_space;
    std::shared_ptr<Sparse_Feature_Space> sparse_feature_space;
    std::shared_ptr<Mutable_Feature_Space> feature_space;

    if (sparse_data) {
        sparse_feature_space.reset(new Sparse_Feature_Space());
        feature_space = sparse_feature_space;

        for (unsigned i = 0;  i < extra.size();  ++i) {
            std::shared_ptr<Sparse_Training_Data> dataset
                (new Sparse_Training_Data());
            dataset->init(extra[i], sparse_feature_space);
            data[i] = dataset;

            if (verbosity > 0)
                cerr << "dataset \'" << extra[i] << "\': "
                     << data[i]->all_features().size() << " vars, "
                     << data[i]->example_count() << " rows." << endl;
        }
    }
    else {
        dense_feature_space.reset(new Dense_Feature_Space());
        feature_space = dense_feature_space;

        for (unsigned i = 0;  i < extra.size();  ++i) {
            std::shared_ptr<Dense_Training_Data> dataset
                (new Dense_Training_Data());
            dataset->init(extra[i], dense_feature_space);
            data[i] = dataset;

            /* Make sure that there are enough variables. */
            if (var_count == -1)
                var_count = dataset->variable_count();
            else if (var_count != dataset->variable_count())
                throw Exception(format("error: file \'%s\' has"
                                       " %zd variables; expected %zd",
                                       extra[i].c_str(),
                                       dataset->variable_count(), var_count));

            if (verbosity > 0)
                cerr << "dataset \'" << extra[i] << "\': "
                     << var_count << " vars, "
                     << dataset->example_count() << " rows." << endl;
        }
    }

    if (verbosity > 0) {
        unsigned rows = 0;
        for (unsigned i = 0;  i < data.size();  ++i)
            rows += data[i]->example_count();

        cerr << "data size: " << rows << " rows." << endl;
    }

    /* Now fix up the feature space. */
    bool use_existing = true;
    guess_all_info(*data[0], *feature_space, use_existing);

    /* Load the classifier. */
    if (classifier_file == "")
        throw Exception("Need to specify a classifier to test.");
    Classifier classifier;
    classifier.load(classifier_file, feature_space);

    vector<Feature> features;
    map<string, Feature> feature_index;

    cerr << "doing features..." << endl;

    do_features(*data[0], classifier.feature_space(), features, feature_index);

    cerr << "done." << endl;

    cerr << "predicted by classifier = " << classifier.predicted() << endl;
    cerr << "classifier predicted name = "
         << classifier.feature_space()->print(classifier.predicted())
         << endl;
    cerr << "dataset predicted name = "
         << feature_space->print(classifier.predicted()) << endl;

    /* Test all of the testing datasets. */
    for (unsigned i = 0;  i < data.size();  ++i) {
        cerr << "Stats over dataset " << i << ":" << endl;

        Null_Decoder decoder;

        calc_stats(*classifier.impl, decoder, *data[i], draw_graphs,
                   false, 1, false, MISSING_FEATURE);

        if (trace_features)
            trace_output(classifier, *data[i], feature_tracing, verbosity);
    }
}
catch (const std::exception & exc) {
    cerr << "error: " << exc.what() << endl;
    exit(1);
}


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

#ifndef __SCARPHASE_INTERNAL_UTIL_RANDOM_PROJECTION_HPP
#define __SCARPHASE_INTERNAL_UTIL_RANDOM_PROJECTION_HPP

#include <stdint.h>
#include <boost/array.hpp>

namespace scarphase {
namespace internal {
namespace util {

/**
 * @brief Random projection to reduce the dimentioality of a large vector
 *        space.
 *
 *  V[1,n] * M[n,m] = V[1,m]
 *
 */
template<typename value_type,
         int      OUTPUT_SIZE>
class RandomProjection //-----------------------------------------------------//
{

public: //--------------------------------------------------------------------//

    /**
     * @brief C-tor.
     */
    RandomProjection();

    /**
     * @brief Hash a value using random projection.
     *        y = x * M, where y << x
     *
     * @param[in]  x Value to hash.
     * @returns A hash of x.
     *
     */
    value_type Hash(value_type x);

private: //-------------------------------------------------------------------//

    /**
     * @brief The size of the input variable.
     */
    #define INPUT_SIZE int(sizeof(value_type) * 8)

    /**
     * @brief Random projection matrix.
     */
    boost::array< boost::array< uint8_t, INPUT_SIZE >, OUTPUT_SIZE > m;

};

} /* namespace util */
} /* namespace internal */
} /* namespace scarphase */

//----------------------------------------------------------------------------//
// Inline Definitions                                                         //
//----------------------------------------------------------------------------//

#include <cstdlib>
#include <boost/random.hpp>
#include <boost/static_assert.hpp>

namespace scarphase {
namespace internal {
namespace util {

template<typename value_type,
         int      OUTPUT_SIZE>
RandomProjection<value_type, OUTPUT_SIZE>::RandomProjection()
{
    BOOST_STATIC_ASSERT(OUTPUT_SIZE > 0);

    boost::mt19937 rng;
    boost::uniform_int<> bin(0,1);
    boost::variate_generator< boost::mt19937&,
                              boost::uniform_int<> > die(rng, bin);

    for (int i = 0; i < OUTPUT_SIZE; i++)
    {
        for (int j = 0; j < INPUT_SIZE; j++)
        {
            m[i][j] = die(); //std::rand() % 2;
        }
    }

}

template<typename value_type,
         int      OUTPUT_SIZE>
inline value_type
RandomProjection<value_type, OUTPUT_SIZE>::Hash(value_type x)
{
    value_type result = 0;
    for (int i = 0; i < OUTPUT_SIZE; i++)
    {
        int t = 0;
        for (int j = 0; j < INPUT_SIZE; j++)
        {
            t += m[i][j] & ((x >> j) & 0x01);
        }
        result |= (t % 2) << i;
    }
    return result;
}

} /* namespace util */
} /* namespace internal */
} /* namespace scarphase */

#endif /* __SCARPHASE_INTERNAL_UTIL_RANDOM_PROJECTION_HPP */


#ifndef ASLAM_CAMERA_GEOMETRY_HPP
#define ASLAM_CAMERA_GEOMETRY_HPP

#include <aslam/cameras/CameraGeometryBase.hpp>
#include <sm/eigen/NumericalDiff.hpp>
#include <sm/kinematics/UncertainHomogeneousPoint.hpp>
#include <boost/serialization/nvp.hpp>
#include "FiniteDifferences.hpp"
#include <sm/PropertyTree.hpp>
#include <boost/serialization/split_member.hpp>
#include <boost/serialization/version.hpp>
#include <sm/boost/serialization.hpp>
#include <aslam/Frame.hpp>

namespace aslam {
// forward declaration
template<typename C>
class Frame;

namespace cameras {

/**
 * \class CameraGeometry
 *
 * A camera geometry class based on three configurable policies:
 * 1. a lens type that determines the geometry of the camera
 * 2. a shutter type that determines the timing of each pixel.
 * 3. a mask type.
 *
 */
template<typename PROJECTION_T, typename SHUTTER_T, typename MASK_T>
class CameraGeometry : public CameraGeometryBase {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  typedef PROJECTION_T projection_t;
  // Not all projections will have a distortion...is this necessary? Maybe.
  //typedef typename projection_t::distortion_t distortion_t;
  typedef typename projection_t::keypoint_t keypoint_t;
  typedef SHUTTER_T shutter_t;
  typedef MASK_T mask_t;
  typedef aslam::Frame<CameraGeometry<projection_t, shutter_t, mask_t> > frame_t;

  enum {
    KeypointDimension = projection_t::KeypointDimension
  };

  typedef Eigen::Matrix<double, KeypointDimension, 4> jacobian_homogeneous_t;
  typedef Eigen::Matrix<double, KeypointDimension, 3> jacobian_t;
  typedef Eigen::Matrix<double, 3, KeypointDimension> inverse_jacobian_t;
  typedef Eigen::Matrix<double, 4, KeypointDimension> inverse_jacobian_homogeneous_t;
  typedef Eigen::Matrix<double, KeypointDimension, KeypointDimension> covariance_t;

  /// \brief default constructor
  CameraGeometry();

  /// \brief Construct from projection
  CameraGeometry(const projection_t & projection);

  /// \brief Construct from projection and shutter
  CameraGeometry(const projection_t & projection, const shutter_t & shutter);

  /// \brief Construct from projection, shutter, and mask
  CameraGeometry(const projection_t & projection, const shutter_t & shutter,
                 const mask_t & mask);

  /// \brief Construct from a property tree
  CameraGeometry(const sm::PropertyTree & config);

  /// \brief simple destructor
  virtual ~CameraGeometry();

  //////////////////////////////////////////////////////////////
  // PROJECTION FUNCTIONS
  //////////////////////////////////////////////////////////////
  template<typename DERIVED_P, typename DERIVED_K>
  bool euclideanToKeypoint(
      const Eigen::MatrixBase<DERIVED_P> & p,
      const Eigen::MatrixBase<DERIVED_K> & outKeypoint) const;

  template<typename DERIVED_P, typename DERIVED_K, typename DERIVED_JP>
  bool euclideanToKeypoint(const Eigen::MatrixBase<DERIVED_P> & p,
                           const Eigen::MatrixBase<DERIVED_K> & outKeypoint,
                           const Eigen::MatrixBase<DERIVED_JP> & outJp) const;

  template<typename DERIVED_P, typename DERIVED_K, typename DERIVED_JP>
  bool euclideanToKeypointFiniteDifference(
      const Eigen::MatrixBase<DERIVED_P> & p,
      const Eigen::MatrixBase<DERIVED_K> & outKeypoint,
      const Eigen::MatrixBase<DERIVED_JP> & outJp) const;

  template<typename DERIVED_P, typename DERIVED_K>
  bool homogeneousToKeypoint(
      const Eigen::MatrixBase<DERIVED_P> & p,
      const Eigen::MatrixBase<DERIVED_K> & outKeypoint) const;

  template<typename DERIVED_P, typename DERIVED_K, typename DERIVED_JP>
  bool homogeneousToKeypoint(const Eigen::MatrixBase<DERIVED_P> & p,
                             const Eigen::MatrixBase<DERIVED_K> & outKeypoint,
                             const Eigen::MatrixBase<DERIVED_JP> & outJp) const;

  template<typename DERIVED_P, typename DERIVED_K, typename DERIVED_JP>
  bool homogeneousToKeypointFiniteDifference(
      const Eigen::MatrixBase<DERIVED_P> & p,
      const Eigen::MatrixBase<DERIVED_K> & outKeypoint,
      const Eigen::MatrixBase<DERIVED_JP> & outJp) const;

  // Project the point and get the associated uncertainty of the projection.
  template<typename DERIVED_K>
  bool homogeneousToKeypoint(
      const sm::kinematics::UncertainHomogeneousPoint & p,
      const Eigen::MatrixBase<DERIVED_K> & outKeypoint,
      covariance_t & outProjectionUncertainty) const;

  // Project the point
  template<typename DERIVED_K>
  bool homogeneousToKeypoint(
      const sm::kinematics::HomogeneousPoint & p,
      const Eigen::MatrixBase<DERIVED_K> & outKeypoint) const;

  template<typename DERIVED_K, typename DERIVED_P>
  bool keypointToEuclidean(const Eigen::MatrixBase<DERIVED_K> & keypoint,
                           const Eigen::MatrixBase<DERIVED_P> & outPoint) const;

  template<typename DERIVED_K, typename DERIVED_P, typename DERIVED_JK>
  bool keypointToEuclidean(const Eigen::MatrixBase<DERIVED_K> & keypoint,
                           const Eigen::MatrixBase<DERIVED_P> & outPoint,
                           const Eigen::MatrixBase<DERIVED_JK> & outJk) const;

  template<typename DERIVED_K, typename DERIVED_P, typename DERIVED_JK>
  bool keypointToEuclideanFiniteDifference(
      const Eigen::MatrixBase<DERIVED_K> & keypoint,
      const Eigen::MatrixBase<DERIVED_P> & outPoint,
      const Eigen::MatrixBase<DERIVED_JK> & outJk) const;

  template<typename DERIVED_K, typename DERIVED_P>
  bool keypointToHomogeneous(
      const Eigen::MatrixBase<DERIVED_K> & keypoint,
      const Eigen::MatrixBase<DERIVED_P> & outPoint) const;

  template<typename DERIVED_K, typename DERIVED_P, typename DERIVED_JK>
  bool keypointToHomogeneous(const Eigen::MatrixBase<DERIVED_K> & keypoint,
                             const Eigen::MatrixBase<DERIVED_P> & outPoint,
                             const Eigen::MatrixBase<DERIVED_JK> & outJk) const;

  template<typename DERIVED_K, typename DERIVED_P, typename DERIVED_JK>
  bool keypointToHomogeneousFiniteDifference(
      const Eigen::MatrixBase<DERIVED_K> & keypoint,
      const Eigen::MatrixBase<DERIVED_P> & outPoint,
      const Eigen::MatrixBase<DERIVED_JK> & outJk) const;

  template<typename DERIVED_P, typename DERIVED_JI>
  void euclideanToKeypointIntrinsicsJacobian(
      const Eigen::MatrixBase<DERIVED_P> & p,
      const Eigen::MatrixBase<DERIVED_JI> & outJi) const;

  template<typename DERIVED_P, typename DERIVED_JD>
  void euclideanToKeypointDistortionJacobian(
      const Eigen::MatrixBase<DERIVED_P> & p,
      const Eigen::MatrixBase<DERIVED_JD> & outJd) const;

  template<typename DERIVED_P, typename DERIVED_JI>
  void euclideanToKeypointIntrinsicsJacobianFiniteDifference(
      const Eigen::MatrixBase<DERIVED_P> & p,
      const Eigen::MatrixBase<DERIVED_JI> & outJi) const;

  template<typename DERIVED_P, typename DERIVED_JD>
  void euclideanToKeypointDistortionJacobianFiniteDifference(
      const Eigen::MatrixBase<DERIVED_P> & p,
      const Eigen::MatrixBase<DERIVED_JD> & outJd) const;

  template<typename DERIVED_P, typename DERIVED_JI>
  void homogeneousToKeypointIntrinsicsJacobian(
      const Eigen::MatrixBase<DERIVED_P> & p,
      const Eigen::MatrixBase<DERIVED_JI> & outJi) const;

  template<typename DERIVED_P, typename DERIVED_JI>
  void homogeneousToKeypointIntrinsicsJacobianFiniteDifference(
      const Eigen::MatrixBase<DERIVED_P> & p,
      const Eigen::MatrixBase<DERIVED_JI> & outJi) const;

  template<typename DERIVED_P, typename DERIVED_JS>
  void homogeneousToKeypointDistortionJacobian(
      const Eigen::MatrixBase<DERIVED_P> & p,
      const Eigen::MatrixBase<DERIVED_JS> & outJs) const;

  template<typename DERIVED_P, typename DERIVED_JD>
  void homogeneousToKeypointDistortionJacobianFiniteDifference(
      const Eigen::MatrixBase<DERIVED_P> & p,
      const Eigen::MatrixBase<DERIVED_JD> & outJd) const;

  //////////////////////////////////////////////////////////////
  // SHUTTER SUPPORT
  //////////////////////////////////////////////////////////////

  // The amount of time elapsed between the start of the image and the
  // keypoint. For a global shutter camera, this can return Duration(0).
  template<typename DERIVED_K>
  Duration temporalOffset(const Eigen::MatrixBase<DERIVED_K> & keypoint) const;

  virtual Duration temporalOffset(const Eigen::VectorXd & keypoint) const;

  //////////////////////////////////////////////////////////////
  // VALIDITY TESTING
  //////////////////////////////////////////////////////////////

  template<typename DERIVED_K>
  bool isValid(const Eigen::MatrixBase<DERIVED_K> & keypoint) const;

  template<typename DERIVED_P>
  bool isEuclideanVisible(const Eigen::MatrixBase<DERIVED_P> & p) const;

  template<typename DERIVED_P>
  bool isHomogeneousVisible(const Eigen::MatrixBase<DERIVED_P> & ph) const;

  //////////////////////////////////////////////////////////////
  // SUPERCLASS SUPPORT
  //////////////////////////////////////////////////////////////
  virtual bool vsEuclideanToKeypoint(const Eigen::Vector3d & p,
                                     Eigen::VectorXd & outKeypoint) const;
  virtual bool vsEuclideanToKeypoint(const Eigen::Vector3d & p,
                                     Eigen::VectorXd & outKeypoint,
                                     Eigen::MatrixXd & outJacobian) const;

  virtual bool vsHomogeneousToKeypoint(const Eigen::Vector4d & ph,
                                       Eigen::VectorXd & outKeypoint) const;

  virtual bool vsHomogeneousToKeypoint(const Eigen::Vector4d & ph,
                                       Eigen::VectorXd & outKeypoint,
                                       Eigen::MatrixXd & outJacobian) const;

  virtual bool vsKeypointToEuclidean(const Eigen::VectorXd & keypoint,
                                     Eigen::Vector3d & outPoint) const;

  virtual bool vsKeypointToEuclidean(const Eigen::VectorXd & keypoint,
                                     Eigen::VectorXd & outPoint,
                                     Eigen::MatrixXd & outJacobian) const;

  virtual bool vsKeypointToHomogeneous(Eigen::VectorXd const & keypoint,
                                       Eigen::VectorXd & outPoint) const;

  virtual bool vsKeypointToHomogeneous(Eigen::VectorXd const & keypoint,
                                       Eigen::VectorXd & outPoint,
                                       Eigen::MatrixXd & outJacobian) const;

  virtual bool vsIsValid(const Eigen::VectorXd & keypoint) const;

  virtual bool vsIsEuclideanVisible(const Eigen::Vector3d & p) const;

  virtual bool vsIsHomogeneousVisible(const Eigen::Vector4d & ph) const;

  // The amount of time elapsed between the start of the image and the
  // keypoint. For a global shutter camera, this can return Duration(0).
  virtual Duration vsTemporalOffset(const Eigen::VectorXd & keypoint) const;

  /// \brief get the camera id.
  virtual CameraId id() const;

  /// \brief set the camera id.
  virtual void setId(CameraId id);

  /// \brief the length of a keypoint
  virtual size_t keypointDimension() const;

  /// \brief is this an invertible camera model
  virtual bool isProjectionInvertible() const;

  //////////////////////////////////////////////////////////////
  // CAMERA CALIBRATION
  //////////////////////////////////////////////////////////////
  /// \brief initialize the intrinsics based on a list of views of a gridded calibration target
  /// \return true on success
  bool initializeIntrinsics(const std::vector<GridCalibrationTargetObservation> &observations);

  /// \brief estimate the transformation of the camera with respect to the calibration target
  ///        On success out_T_t_c is filled in with the transformation that takes points from
  ///        the camera frame to the target frame
  /// \return true on success
  virtual bool estimateTransformation(
      const ObservationInterface & obs,
      sm::kinematics::Transformation & out_T_t_c) const;

  ////////////////////////////////////////////////////////////////
  // OPTIMIZATION SUPPORT
  ////////////////////////////////////////////////////////////////
  /// \brief the minimal dimensions of the projection parameters
  virtual int minimalDimensionsProjection() const;

  /// \brief the minimal dimensions of the distortion parameters
  virtual int minimalDimensionsDistortion() const;

  /// \brief the minimal dimensions of the shutter parameters
  virtual int minimalDimensionsShutter() const;

  // aslam::backend compatibility

  /// \brief update the intrinsics
  virtual void update(const double * v, bool projection, bool distortion,
                      bool shutter);

  /// \brief Get the total number of dimensions of the intrinsic parameters
  virtual int minimalDimensions(bool projection, bool distortion,
                                bool shutter) const;

  /// \brief get the intrinsic parameters.
  virtual void getParameters(Eigen::MatrixXd & P, bool projection,
                             bool distortion, bool shutter) const;

  /// \brief set the intrinsic parameters.
  virtual void setParameters(const Eigen::MatrixXd & P, bool projection,
                             bool distortion, bool shutter);

  /// \brief return the Jacobian of the projection with respect to the intrinsics.
  virtual void euclideanToKeypointIntrinsicsJacobian(
      const Eigen::Vector3d & p, Eigen::MatrixXd & outJi,
      bool estimateProjection, bool estimateDistortion,
      bool estimateShutter) const;

  /// \brief return the Jacobian of the projection with respect to the intrinsics.
  virtual void homogeneousToKeypointIntrinsicsJacobian(
      const Eigen::Vector4d & p, Eigen::MatrixXd & outJi,
      bool estimateProjection, bool estimateDistortion,
      bool estimateShutter) const;

  /// \brief return the temporal offset with respect to the intrinsics.
  virtual void temporalOffsetIntrinsicsJacobian(
      const Eigen::VectorXd & keypoint, Eigen::MatrixXd & outJi,
      bool estimateProjection, bool estimateDistortion,
      bool estimateShutter) const;

  //////////////////////////////////////////////////////////////
  // UNIT TEST SUPPORT
  //////////////////////////////////////////////////////////////
  // \brief creates a random valid keypoint.
  virtual Eigen::VectorXd createRandomKeypoint() const;

  // \brief creates a random visible point. Negative depth means random between 0 and 100 meters.
  virtual Eigen::Vector3d createRandomVisiblePoint(double depth = -1.0) const;

  projection_t & projection() {
    return _projection;
  }
  const projection_t & projection() const {
    return _projection;
  }

  shutter_t & shutter() {
    return _shutter;
  }
  const shutter_t & shutter() const {
    return _shutter;
  }

  void setMask(const mask_t & mask) {
    _mask = mask;
  }
  mask_t & mask() {
    return _mask;
  }
  const mask_t & mask() const {
    return _mask;
  }

  virtual bool hasMask() const
  {
	  return _mask.isSet();
  }

  virtual void print(std::ostream & out);

  bool isBinaryEqual(
      const CameraGeometry<PROJECTION_T, SHUTTER_T, MASK_T> & rhs) const;

  /// \brief This will create a frame with the right type. WARNING: it will not
  ///        fill in the camera geometry in the frame. This must be done manually
  ///        outside the call.
  virtual boost::shared_ptr<FrameBase> createUninitializedFrameBase() const;

  /// \brief This will create a frame with the right type. WARNING: it will not
  ///        fill in the camera geometry in the frame. This must be done manually
  ///        outside the call.
  boost::shared_ptr<frame_t> createUninitializedFrame() const;

  enum {
    CLASS_SERIALIZATION_VERSION = 3
  };BOOST_SERIALIZATION_SPLIT_MEMBER();

  template<class Archive>
  void load(Archive & ar, const unsigned int version) {
    SM_ASSERT_LE(std::runtime_error, version,
                 (unsigned int) CLASS_SERIALIZATION_VERSION,
                 "Unsupported serialization version");
    if (version >= 2) {
      ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(CameraGeometryBase);
    }

    ar >> BOOST_SERIALIZATION_NVP(_id);
    ar >> BOOST_SERIALIZATION_NVP(_projection);
    ar >> BOOST_SERIALIZATION_NVP(_shutter);
    ar >> BOOST_SERIALIZATION_NVP(_mask);

  }

  template<class Archive>
  void save(Archive & ar, const unsigned int /* version */) const {
    ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(CameraGeometryBase);
    ar << BOOST_SERIALIZATION_NVP(_id);
    ar << BOOST_SERIALIZATION_NVP(_projection);
    ar << BOOST_SERIALIZATION_NVP(_shutter);
    ar << BOOST_SERIALIZATION_NVP(_mask);
  }

  static CameraGeometry<PROJECTION_T, SHUTTER_T, MASK_T> getTestGeometry();

  /// \todo redo this. Somehow Stefan's stuff needs it.
  virtual int width() const {
    return _projection.ru();
  }
  virtual int height() const {
    return _projection.rv();
  }
 private:
  CameraId _id;
  projection_t _projection;
  shutter_t _shutter;
  mask_t _mask;
};

}  // namespace cameras
}  // namespace aslam

#include "implementation/CameraGeometry.hpp"

SM_BOOST_CLASS_VERSION_T1T2T3 (aslam::cameras::CameraGeometry);

#endif /* ASLAM_CAMERA_GEOMETRY_BASE_HPP */


#include <franka_gazebo/model_kdl.h>

#include <eigen_conversions/eigen_kdl.h>
#include <ros/ros.h>
#include <Eigen/Dense>
#include <algorithm>
#include <array>
#include <kdl/chain.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames.hpp>
#include <kdl/solveri.hpp>
#include <kdl/tree.hpp>
#include <kdl_parser/kdl_parser.hpp>

namespace franka_gazebo {

int ModelKDL::segment(franka::Frame frame) {
  // clang-format off
  switch (frame) {
    case franka::Frame::kJoint1:      return 1;
    case franka::Frame::kJoint2:      return 2;
    case franka::Frame::kJoint3:      return 3;
    case franka::Frame::kJoint4:      return 4;
    case franka::Frame::kJoint5:      return 5;
    case franka::Frame::kJoint6:      return 6;
    case franka::Frame::kJoint7:      return 7;
    case franka::Frame::kFlange:      return 8;
    case franka::Frame::kEndEffector: return 9;
    case franka::Frame::kStiffness:   return 10;
    default: return -1;
  }
  // clang-format on
}

bool ModelKDL::isCloseToSingularity(const KDL::Jacobian& jacobian) const {
  if (this->singularity_threshold_ < 0) {
    return false;
  }
  Eigen::Matrix<double, 6, 6> mat = jacobian.data * jacobian.data.transpose();
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(mat, Eigen::ComputeFullU | Eigen::ComputeFullV);
  double critical = svd.singularValues().tail(1)(0);
  return critical < this->singularity_threshold_;
}

// Implementation copied from <kdl/isolveri.hpp> because
// KDL::ChainDynSolver inherits *privately* from SolverI ... -.-'
std::string ModelKDL::strError(const int error) {
  // clang-format off
  switch(error) {
  case KDL::SolverI::E_NOERROR:                 return "No error"; break;
  case KDL::SolverI::E_NO_CONVERGE:             return "Failed to converge"; break;
  case KDL::SolverI::E_UNDEFINED:               return "Undefined value"; break;
  case KDL::SolverI::E_DEGRADED:                return "Converged but degraded solution"; break;
#if ROS_VERSION_MINIMUM(1, 15, 0)
  // These were introduced in melodic
  case KDL::SolverI::E_NOT_UP_TO_DATE:          return "Internal data structures not up to date with Chain"; break;
  case KDL::SolverI::E_SIZE_MISMATCH:           return "The size of the input does not match the internal state"; break;
  case KDL::SolverI::E_MAX_ITERATIONS_EXCEEDED: return "The maximum number of iterations is exceeded"; break;
  case KDL::SolverI::E_OUT_OF_RANGE:            return "The requested index is out of range"; break;
  case KDL::SolverI::E_NOT_IMPLEMENTED:         return "The requested function is not yet implemented"; break;
  case KDL::SolverI::E_SVD_FAILED:              return "SVD failed"; break;
#endif
  default: return "UNKNOWN ERROR";
  }
  // clang-format on
}
ModelKDL::ModelKDL(const urdf::Model& model,
                   const std::string& root,
                   const std::string& tip,
                   double singularity_threshold)
    : singularity_threshold_(singularity_threshold) {
  KDL::Tree tree;
  if (not kdl_parser::treeFromUrdfModel(model, tree)) {
    throw std::invalid_argument("Cannot construct KDL tree from URDF");
  }

  if (not tree.getChain(root, tip, this->chain_)) {
    throw std::invalid_argument("Cannot find chain within URDF tree from root '" + root +
                                "' to tip '" + tip + "'. Do these links exist?");
  }

  ROS_INFO_STREAM("KDL Model initialized for chain from '" << root << "' -> '" << tip << "'");
}

void ModelKDL::augmentFrame(const std::string& name,
                            const std::array<double, 16>& transform,
                            KDL::Chain& chain) {
  Eigen::Affine3d t;
  KDL::Frame f;
  t.matrix() = Eigen::Matrix4d(transform.data());
  tf::transformEigenToKDL(t, f);
  chain.addSegment(KDL::Segment(name, KDL::Joint(KDL::Joint::None), f));
}

void ModelKDL::augmentFrame(const std::string& name,
                            const std::array<double, 3>& center_of_mass,
                            double mass,
                            const std::array<double, 9>& inertia,
                            KDL::Chain& chain) {
  KDL::Vector kc;
  KDL::RotationalInertia ki;
  std::copy(center_of_mass.begin(), center_of_mass.end(), std::begin(kc.data));
  std::copy(inertia.begin(), inertia.end(), std::begin(ki.data));

  chain.addSegment(KDL::Segment(name, KDL::Joint(KDL::Joint::None), KDL::Frame(KDL::Vector::Zero()),
                                KDL::RigidBodyInertia(mass, kc, ki)));
}

std::array<double, 16> ModelKDL::pose(
    franka::Frame frame,
    const std::array<double, 7>& q,
    const std::array<double, 16>& F_T_EE,  // NOLINT(readability-identifier-naming)
    const std::array<double, 16>& EE_T_K)  // NOLINT(readability-identifier-naming)
    const {
  KDL::JntArray kq;
  KDL::Frame kp;

  // Agument the chain with the two new Frames 'EE' and 'K'
  KDL::Chain chain = this->chain_;  // copy
  augmentFrame("EE", F_T_EE, chain);
  augmentFrame("K", EE_T_K, chain);

  KDL::ChainFkSolverPos_recursive solver(chain);

  kq.data = Eigen::Matrix<double, 7, 1>(q.data());

  int error = solver.JntToCart(kq, kp, segment(frame));
  if (error != KDL::SolverI::E_NOERROR) {
    throw std::logic_error("KDL forward kinematics pose calculation failed with error: " +
                           strError(error));
  }
  Eigen::Affine3d p;
  tf::transformKDLToEigen(kp, p);

  std::array<double, 16> result;
  Eigen::MatrixXd::Map(&result[0], 4, 4) = p.matrix();

  return result;
}

std::array<double, 42> ModelKDL::bodyJacobian(
    franka::Frame frame,
    const std::array<double, 7>& q,
    const std::array<double, 16>& F_T_EE,  // NOLINT(readability-identifier-naming)
    const std::array<double, 16>& EE_T_K)  // NOLINT(readability-identifier-naming)
    const {
  KDL::JntArray kq;
  KDL::Jacobian J(7);  // NOLINT(readability-identifier-naming)
  kq.data = Eigen::Matrix<double, 7, 1>(q.data());

  // Augment the chain with the two virtual frames 'EE' and 'K'
  KDL::Chain chain = this->chain_;  // copy
  augmentFrame("EE", F_T_EE, chain);
  augmentFrame("K", EE_T_K, chain);

  KDL::ChainJntToJacSolver solver(chain);

  int seg = segment(frame);
  int error = solver.JntToJac(kq, J, seg);
  if (error != KDL::SolverI::E_NOERROR) {
    throw std::logic_error("KDL zero jacobian calculation failed with error: " + strError(error));
  }

  // Shift the zero jacobian to the "body" frame by using the rotation of the FK of that frame
  KDL::Frame f = KDL::Frame::Identity();
  auto transform = pose(frame, q, F_T_EE, EE_T_K);
  Eigen::Affine3d t;
  t.matrix() = Eigen::Matrix4d(transform.data());
  tf::transformEigenToKDL(t, f);

  J.changeBase(f.M.Inverse());

  // Singularity check
  if (isCloseToSingularity(J)) {
    ROS_WARN_STREAM_THROTTLE(1, "Body Jacobian close to singularity");
  }

  std::array<double, 42> result;
  Eigen::MatrixXd::Map(&result[0], 6, 7) = J.data;

  return result;
}

std::array<double, 42> ModelKDL::zeroJacobian(
    franka::Frame frame,
    const std::array<double, 7>& q,
    const std::array<double, 16>& F_T_EE,  // NOLINT(readability-identifier-naming)
    const std::array<double, 16>& EE_T_K)  // NOLINT(readability-identifier-naming)
    const {
  KDL::JntArray kq;
  KDL::Jacobian J(7);  // NOLINT(readability-identifier-naming)
  kq.data = Eigen::Matrix<double, 7, 1>(q.data());

  // Augment the chain with the two virtual frames 'EE' and 'K'
  KDL::Chain chain = this->chain_;  // copy
  augmentFrame("EE", F_T_EE, chain);
  augmentFrame("K", EE_T_K, chain);

  KDL::ChainJntToJacSolver solver(chain);

  int error = solver.JntToJac(kq, J, segment(frame));
  if (error != KDL::SolverI::E_NOERROR) {
    throw std::logic_error("KDL zero jacobian calculation failed with error: " + strError(error));
  }

  // Singularity Check
  if (isCloseToSingularity(J)) {
    ROS_WARN_STREAM_THROTTLE(1, "Zero Jacobian close to singularity");
  }

  std::array<double, 42> result;
  Eigen::MatrixXd::Map(&result[0], 6, 7) = J.data;

  return result;
}

std::array<double, 49> ModelKDL::mass(
    const std::array<double, 7>& q,
    const std::array<double, 9>& I_total,  // NOLINT(readability-identifier-naming)
    double m_total,
    const std::array<double, 3>& F_x_Ctotal)  // NOLINT(readability-identifier-naming)
    const {
  KDL::JntArray kq;
  KDL::JntSpaceInertiaMatrix M(7);  // NOLINT(readability-identifier-naming)
  kq.data = Eigen::Matrix<double, 7, 1>(q.data());

  KDL::Chain chain = this->chain_;  // copy
  augmentFrame("load", F_x_Ctotal, m_total, I_total, chain);
  KDL::ChainDynParam solver(chain, KDL::Vector(0, 0, -9.81));

  int error = solver.JntToMass(kq, M);
  if (error != KDL::SolverI::E_NOERROR) {
    throw std::logic_error("KDL mass calculation failed with error: " + strError(error));
  }

  std::array<double, 49> result;
  Eigen::MatrixXd::Map(&result[0], 7, 7) = M.data;

  return result;
}

std::array<double, 7> ModelKDL::coriolis(
    const std::array<double, 7>& q,
    const std::array<double, 7>& dq,
    const std::array<double, 9>& I_total,  // NOLINT(readability-identifier-naming)
    double m_total,
    const std::array<double, 3>& F_x_Ctotal)  // NOLINT(readability-identifier-naming)
    const {
  KDL::JntArray kq, kdq, kc(7);
  kq.data = Eigen::Matrix<double, 7, 1>(q.data());
  kdq.data = Eigen::Matrix<double, 7, 1>(dq.data());

  KDL::Chain chain = this->chain_;  // copy
  augmentFrame("load", F_x_Ctotal, m_total, I_total, chain);
  KDL::ChainDynParam solver(chain, KDL::Vector(0, 0, -9.81));

  int error = solver.JntToCoriolis(kq, kdq, kc);
  if (error != KDL::SolverI::E_NOERROR) {
    throw std::logic_error("KDL coriolis calculation failed with error: " + strError(error));
  }

  std::array<double, 7> result;
  Eigen::VectorXd::Map(&result[0], kc.data.size()) = kc.data;

  return result;
}

std::array<double, 7> ModelKDL::gravity(
    const std::array<double, 7>& q,
    double m_total,
    const std::array<double, 3>& F_x_Ctotal,  // NOLINT(readability-identifier-naming)
    const std::array<double, 3>& gravity_earth) const {
  KDL::JntArray kq, kg(7);
  KDL::Vector grav(gravity_earth[0], gravity_earth[1], gravity_earth[2]);
  kq.data = Eigen::Matrix<double, 7, 1>(q.data());

  KDL::Chain chain = this->chain_;  // copy
  augmentFrame("load", F_x_Ctotal, m_total, {1, 0, 0, 0, 1, 0, 0, 0, 1}, chain);
  KDL::ChainDynParam solver(chain, grav);

  int error = solver.JntToGravity(kq, kg);
  if (error != KDL::SolverI::E_NOERROR) {
    throw std::logic_error("KDL gravity calculation failed with error: " + strError(error));
  }

  std::array<double, 7> result;
  Eigen::VectorXd::Map(&result[0], kg.data.size()) = kg.data;

  return result;
}
}  // namespace franka_gazebo


#define BOOST_TEST_MODULE pcrblock de_haan_compactor
#include <boost/test/unit_test.hpp>
#include "dal_MathUtils.h"
#include "block_dehaancompactor.h"

BOOST_AUTO_TEST_CASE(test)
{
  using namespace block;

  // {
  //   DeHaanCompactor compactor;
  //   BOOST_CHECK(dal::comparable(compactor( 4.0,   5.0, 6.0), REAL4(0.0)));
  // }

  {
    DeHaanCompactor compactor(0.2, 0.02, 6.0);
    BOOST_CHECK(dal::comparable(compactor( 0.0, 5.0, 6.0), REAL4(0.0)));
    BOOST_CHECK(dal::comparable(compactor( 0.0, 5.0, 0.0), REAL4(0.0)));
    BOOST_CHECK(dal::comparable(compactor( 0.0, 0.0, 0.0), REAL4(0.0)));
    BOOST_CHECK(dal::comparable(compactor( 0.0, 0.0, 6.0), REAL4(0.0)));
    BOOST_CHECK(dal::comparable(compactor( 4.0, 5.0, 0.0), REAL4(4.0)));
    BOOST_CHECK(dal::comparable(compactor( 4.0, 0.0, 0.0), REAL4(4.0)));
  }
}


#ifndef EPIPOLAR_GEOMETRY_HPP
#define EPIPOLAR_GEOMETRY_HPP

#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>

#include <image_geometry/pinhole_camera_model.h>
#include <boost/math/special_functions/binomial.hpp>
#include <cmath>

#include <random>

class EpipolarGeometry {
 private:
  const double PI;

  /*Random Values */
  std::random_device _randomDevice;
  std::mt19937 _randomGenerator;

  cv::Vec3d calculateBaseLine(const std::vector<cv::Vec3d> &mhi, const std::vector<cv::Vec3d> &mt);

  unsigned int estimateNumberOfIteration(unsigned int N, double inlierProbability, unsigned int s, double ps);

  unsigned int reEstimateNumberOfIteration(unsigned int N, unsigned int nInlier, unsigned int s, double ps);

 public:
  EpipolarGeometry();

  ~EpipolarGeometry() = default;
/**
 *
 *
 * @param mhi
 * @param mt
 * @param inlierIndexes
 * @param ps
 * @param threshold
 * @return
 */
  cv::Vec3d estimateBaseLine(const std::vector<cv::Vec3d> &mhi, const std::vector<cv::Vec3d> &mt,
                             std::vector<unsigned int> &inlierIndexes, double ps, double threshold);
};

#endif //EPIPOLAR_GEOMETRY_HPP


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://github.com/mCRL2org/mCRL2/blob/master/COPYING
//
// 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)
//
/// \file abstract_test.cpp
/// \brief Test the pbes abstract algorithm.

#define BOOST_TEST_MODULE abstract_test
#include <boost/test/included/unit_test_framework.hpp>
#include "mcrl2/pbes/abstract.h"
#include "mcrl2/pbes/txt2pbes.h"

using namespace mcrl2;
using namespace mcrl2::pbes_system;

void test_pbesabstract(const std::string& pbes_spec, const std::string& variable_spec, bool value_true)
{
  pbes p = txt2pbes(pbes_spec);
  pbes_system::detail::pbes_parameter_map parameter_map = pbes_system::detail::parse_pbes_parameter_map(p, variable_spec);
  pbes_abstract_algorithm algorithm;
  algorithm.run(p, parameter_map, value_true);
  std::cout << "\n-------------------------------\n" << pbes_system::pp(p) << std::endl;
}

BOOST_AUTO_TEST_CASE(pbesabstract)
{
  test_pbesabstract(
    "pbes nu X(a: Bool, b: Nat) =  \n"
    "       val(a) || X(a, b + 1); \n"
    "                              \n"
    "init X(true, 0);              \n"
    ,
    "X(b:Nat)"
    ,
    true
  );

  test_pbesabstract(
    "pbes nu X1(b:Bool) = exists b:Bool.(X2 || val(b)); \n"
    "     mu X2 = X2;                                   \n"
    "                                                   \n"
    "init X1(true);                                     \n"
    ,
    "X1(b:Bool)"
    ,
    true
  );

  test_pbesabstract(
    "pbes nu X1(b:Bool) = X2 || val(b); \n"
    "     mu X2 = X2;                   \n"
    "                                   \n"
    "init X1(true);                     \n"
    ,
    "X1(b:Bool)"
    ,
    true
  );
}


#ifndef QUBUS_AFFINE_CONSTRAINTS_HPP
#define QUBUS_AFFINE_CONSTRAINTS_HPP

#include <qubus/IR/expression.hpp>
#include <qubus/IR/variable_declaration.hpp>

#include <qubus/isl/affine_expr.hpp>
#include <qubus/isl/ast.hpp>
#include <qubus/isl/context.hpp>
#include <qubus/isl/set.hpp>
#include <qubus/isl/space.hpp>

#include <boost/optional.hpp>

#include <functional>
#include <map>
#include <memory>
#include <string>
#include <tuple>
#include <vector>

namespace qubus
{

class affine_expr_context;

class affine_constraint;

class affine_expr
{
public:
    affine_expr() = default;

    affine_expr(const affine_expr& other);
    affine_expr(affine_expr&& other) noexcept;

    affine_expr& operator=(const affine_expr& other);
    affine_expr& operator=(affine_expr&& other) noexcept;

    const expression& underlying_expression() const;

    isl::affine_expr convert(isl::context& isl_ctx) const;

    friend affine_expr operator+(affine_expr lhs, affine_expr rhs);
    friend affine_expr operator-(affine_expr lhs, affine_expr rhs);
    friend affine_expr operator*(affine_expr lhs, affine_expr rhs);
    friend affine_expr operator/(affine_expr lhs, affine_expr rhs);
    friend affine_expr operator%(affine_expr lhs, affine_expr rhs);
    friend affine_expr operator-(affine_expr arg);

    friend bool is_const(const affine_expr& expr);
    friend bool is_variable(const affine_expr& expr);

    friend std::string get_variable_name(const affine_expr& expr);

    friend affine_constraint equal_to(affine_expr lhs, affine_expr rhs);
    friend affine_constraint not_equal_to(affine_expr lhs, affine_expr rhs);
    friend affine_constraint less(affine_expr lhs, affine_expr rhs);
    friend affine_constraint greater(affine_expr lhs, affine_expr rhs);
    friend affine_constraint less_equal(affine_expr lhs, affine_expr rhs);
    friend affine_constraint greater_equal(affine_expr lhs, affine_expr rhs);

private:
    friend class affine_expr_context;

    affine_expr(std::unique_ptr<expression> expr_, affine_expr_context& ctx_);

    std::unique_ptr<expression> expr_;
    affine_expr_context* ctx_;
};

class affine_expr_context
{
public:
    explicit affine_expr_context(std::function<bool(const expression&)> invariant_checker_);

    affine_expr declare_variable(const variable_declaration& var);

    affine_expr define_constant(const expression& value);
    affine_expr create_literal(util::index_t value);

    boost::optional<variable_declaration> lookup_variable(const std::string& name) const;
    std::unique_ptr<expression> get_constant(const std::string& name) const;

    isl::space construct_corresponding_space(isl::context& isl_ctx) const;

    const std::function<bool(const expression&)>& get_invariant_checker() const;

private:
    std::function<bool(const expression&)> invariant_checker_;

    std::size_t free_variable_id_ = 0;
    std::vector<std::tuple<variable_declaration, std::string>> variable_table_;

    std::size_t free_constant_id_ = 0;
    std::vector<std::tuple<const expression*, std::string>> constant_table_;
};

class affine_constraint
{
public:
    affine_constraint(std::unique_ptr<expression> expr_, affine_expr_context& ctx_);

    affine_constraint(const affine_constraint& other);
    affine_constraint(affine_constraint&& other) noexcept;

    affine_constraint& operator=(const affine_constraint& other);
    affine_constraint& operator=(affine_constraint&& other) noexcept;

    isl::set convert(isl::context& isl_ctx) const;

    friend affine_constraint operator&&(affine_constraint lhs, affine_constraint rhs);
    friend affine_constraint operator||(affine_constraint lhs, affine_constraint rhs);
    friend affine_constraint operator!(affine_constraint arg);

private:
    std::unique_ptr<expression> expr_;
    affine_expr_context* ctx_;
};

boost::optional<affine_expr> try_construct_affine_expr(const expression& expr,
                                                       affine_expr_context& ctx);

boost::optional<affine_constraint> try_extract_affine_constraint(const expression& expr,
                                                                 affine_expr_context& ctx);

std::unique_ptr<expression> convert_isl_ast_expr_to_qir(const isl::ast_expr &expr,
                                                        const affine_expr_context &ctx);
}

#endif


///////////////////////////////////////////////////////////////////////////////
/// \file vector.hpp
///
//  Copyright 2005 Eric Niebler. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_NUMERIC_FUNCTIONAL_VECTOR_HPP_EAN_12_12_2005
#define BOOST_NUMERIC_FUNCTIONAL_VECTOR_HPP_EAN_12_12_2005

#ifdef BOOST_NUMERIC_FUNCTIONAL_HPP_INCLUDED
# error Include this file before boost/accumulators/numeric/functional.hpp
#endif

#include <vector>
#include <functional>
#include <boost/assert.hpp>
#include <boost/mpl/and.hpp>
#include <boost/mpl/not.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/is_scalar.hpp>
#include <boost/type_traits/remove_const.hpp>
#include <boost/typeof/std/vector.hpp>
#include <boost/accumulators/numeric/functional_fwd.hpp>

namespace boost { namespace numeric
{
    namespace operators
    {
        namespace acc_detail
        {
            template<typename Fun>
            struct make_vector
            {
                typedef std::vector<typename Fun::result_type> type;
            };
        }

        ///////////////////////////////////////////////////////////////////////////////
        // Handle vector<Left> / Right where Right is a scalar.
        template<typename Left, typename Right>
        typename lazy_enable_if<
            is_scalar<Right>
          , acc_detail::make_vector<functional::divides<Left, Right> >
        >::type
        operator /(std::vector<Left> const &left, Right const &right)
        {
            typedef typename functional::divides<Left, Right>::result_type value_type;
            std::vector<value_type> result(left.size());
            for(std::size_t i = 0, size = result.size(); i != size; ++i)
            {
                result[i] = numeric::divides(left[i], right);
            }
            return result;
        }

        ///////////////////////////////////////////////////////////////////////////////
        // Handle vector<Left> / vector<Right>.
        template<typename Left, typename Right>
        std::vector<typename functional::divides<Left, Right>::result_type>
        operator /(std::vector<Left> const &left, std::vector<Right> const &right)
        {
            typedef typename functional::divides<Left, Right>::result_type value_type;
            std::vector<value_type> result(left.size());
            for(std::size_t i = 0, size = result.size(); i != size; ++i)
            {
                result[i] = numeric::divides(left[i], right[i]);
            }
            return result;
        }

        ///////////////////////////////////////////////////////////////////////////////
        // Handle vector<Left> * Right where Right is a scalar.
        template<typename Left, typename Right>
        typename lazy_enable_if<
            is_scalar<Right>
          , acc_detail::make_vector<functional::multiplies<Left, Right> >
        >::type
        operator *(std::vector<Left> const &left, Right const &right)
        {
            typedef typename functional::multiplies<Left, Right>::result_type value_type;
            std::vector<value_type> result(left.size());
            for(std::size_t i = 0, size = result.size(); i != size; ++i)
            {
                result[i] = numeric::multiplies(left[i], right);
            }
            return result;
        }

        ///////////////////////////////////////////////////////////////////////////////
        // Handle Left * vector<Right> where Left is a scalar.
        template<typename Left, typename Right>
        typename lazy_enable_if<
            is_scalar<Left>
          , acc_detail::make_vector<functional::multiplies<Left, Right> >
        >::type
        operator *(Left const &left, std::vector<Right> const &right)
        {
            typedef typename functional::multiplies<Left, Right>::result_type value_type;
            std::vector<value_type> result(right.size());
            for(std::size_t i = 0, size = result.size(); i != size; ++i)
            {
                result[i] = numeric::multiplies(left, right[i]);
            }
            return result;
        }

        ///////////////////////////////////////////////////////////////////////////////
        // Handle vector<Left> * vector<Right>
        template<typename Left, typename Right>
        std::vector<typename functional::multiplies<Left, Right>::result_type>
        operator *(std::vector<Left> const &left, std::vector<Right> const &right)
        {
            typedef typename functional::multiplies<Left, Right>::result_type value_type;
            std::vector<value_type> result(left.size());
            for(std::size_t i = 0, size = result.size(); i != size; ++i)
            {
                result[i] = numeric::multiplies(left[i], right[i]);
            }
            return result;
        }

        ///////////////////////////////////////////////////////////////////////////////
        // Handle vector<Left> + vector<Right>
        template<typename Left, typename Right>
        std::vector<typename functional::plus<Left, Right>::result_type>
        operator +(std::vector<Left> const &left, std::vector<Right> const &right)
        {
            typedef typename functional::plus<Left, Right>::result_type value_type;
            std::vector<value_type> result(left.size());
            for(std::size_t i = 0, size = result.size(); i != size; ++i)
            {
                result[i] = numeric::plus(left[i], right[i]);
            }
            return result;
        }

        ///////////////////////////////////////////////////////////////////////////////
        // Handle vector<Left> - vector<Right>
        template<typename Left, typename Right>
        std::vector<typename functional::minus<Left, Right>::result_type>
        operator -(std::vector<Left> const &left, std::vector<Right> const &right)
        {
            typedef typename functional::minus<Left, Right>::result_type value_type;
            std::vector<value_type> result(left.size());
            for(std::size_t i = 0, size = result.size(); i != size; ++i)
            {
                result[i] = numeric::minus(left[i], right[i]);
            }
            return result;
        }

        ///////////////////////////////////////////////////////////////////////////////
        // Handle vector<Left> += vector<Left>
        template<typename Left>
        std::vector<Left> &
        operator +=(std::vector<Left> &left, std::vector<Left> const &right)
        {
            BOOST_ASSERT(left.size() == right.size());
            for(std::size_t i = 0, size = left.size(); i != size; ++i)
            {
                numeric::plus_assign(left[i], right[i]);
            }
            return left;
        }

        ///////////////////////////////////////////////////////////////////////////////
        // Handle -vector<Arg>
        template<typename Arg>
        std::vector<typename functional::unary_minus<Arg>::result_type>
        operator -(std::vector<Arg> const &arg)
        {
            typedef typename functional::unary_minus<Arg>::result_type value_type;
            std::vector<value_type> result(arg.size());
            for(std::size_t i = 0, size = result.size(); i != size; ++i)
            {
                result[i] = numeric::unary_minus(arg[i]);
            }
            return result;
        }
    }

    namespace functional
    {
        struct std_vector_tag;

        template<typename T, typename Al>
        struct tag<std::vector<T, Al> >
        {
            typedef std_vector_tag type;
        };

        ///////////////////////////////////////////////////////////////////////////////
        // element-wise min of std::vector
        template<typename Left, typename Right>
        struct min_assign<Left, Right, std_vector_tag, std_vector_tag>
          : std::binary_function<Left, Right, void>
        {
            void operator ()(Left &left, Right &right) const
            {
                BOOST_ASSERT(left.size() == right.size());
                for(std::size_t i = 0, size = left.size(); i != size; ++i)
                {
                    if(numeric::less(right[i], left[i]))
                    {
                        left[i] = right[i];
                    }
                }
            }
        };

        ///////////////////////////////////////////////////////////////////////////////
        // element-wise max of std::vector
        template<typename Left, typename Right>
        struct max_assign<Left, Right, std_vector_tag, std_vector_tag>
          : std::binary_function<Left, Right, void>
        {
            void operator ()(Left &left, Right &right) const
            {
                BOOST_ASSERT(left.size() == right.size());
                for(std::size_t i = 0, size = left.size(); i != size; ++i)
                {
                    if(numeric::greater(right[i], left[i]))
                    {
                        left[i] = right[i];
                    }
                }
            }
        };

        // partial specialization for std::vector.
        template<typename Left, typename Right>
        struct fdiv<Left, Right, std_vector_tag, void>
          : mpl::if_<
                are_integral<typename Left::value_type, Right>
              , divides<Left, double const>
              , divides<Left, Right>
            >::type
        {};

        // promote
        template<typename To, typename From>
        struct promote<To, From, std_vector_tag, std_vector_tag>
          : std::unary_function<From, To>
        {
            To operator ()(From &arr) const
            {
                typename remove_const<To>::type res(arr.size());
                for(std::size_t i = 0, size = arr.size(); i != size; ++i)
                {
                    res[i] = numeric::promote<typename To::value_type>(arr[i]);
                }
                return res;
            }
        };

        template<typename ToFrom>
        struct promote<ToFrom, ToFrom, std_vector_tag, std_vector_tag>
          : std::unary_function<ToFrom, ToFrom>
        {
            ToFrom &operator ()(ToFrom &tofrom) const
            {
                return tofrom;
            }
        };

        ///////////////////////////////////////////////////////////////////////////////
        // functional::as_min
        template<typename T>
        struct as_min<T, std_vector_tag>
          : std::unary_function<T, typename remove_const<T>::type>
        {
            typename remove_const<T>::type operator ()(T &arr) const
            {
                return 0 == arr.size()
                  ? T()
                  : T(arr.size(), numeric::as_min(arr[0]));
            }
        };

        ///////////////////////////////////////////////////////////////////////////////
        // functional::as_max
        template<typename T>
        struct as_max<T, std_vector_tag>
          : std::unary_function<T, typename remove_const<T>::type>
        {
            typename remove_const<T>::type operator ()(T &arr) const
            {
                return 0 == arr.size()
                  ? T()
                  : T(arr.size(), numeric::as_max(arr[0]));
            }
        };

        ///////////////////////////////////////////////////////////////////////////////
        // functional::as_zero
        template<typename T>
        struct as_zero<T, std_vector_tag>
          : std::unary_function<T, typename remove_const<T>::type>
        {
            typename remove_const<T>::type operator ()(T &arr) const
            {
                return 0 == arr.size()
                  ? T()
                  : T(arr.size(), numeric::as_zero(arr[0]));
            }
        };

        ///////////////////////////////////////////////////////////////////////////////
        // functional::as_one
        template<typename T>
        struct as_one<T, std_vector_tag>
          : std::unary_function<T, typename remove_const<T>::type>
        {
            typename remove_const<T>::type operator ()(T &arr) const
            {
                return 0 == arr.size()
                  ? T()
                  : T(arr.size(), numeric::as_one(arr[0]));
            }
        };

    } // namespace functional

}} // namespace boost::numeric

#endif


//
//=======================================================================
// Copyright 2014
// Author: Alex Hagen-Zanker
//
// 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)
//=======================================================================
//
// An algorithm object version of the dijkstra shortest path algorithm, It
// uses the Boost ASIO stackless coroutine as suggested by Dave Abrahams on
// Boost mailing list. The interface roughly follows the suggestion of
// Jeremiah Willcock.
//
// The algorithm itself is modified from Boost Graph Library and combines
// lines from breadth_first_search and bfs_dijkstra_visitor
//=======================================================================
//

#ifndef BLINK_GRAPH_DIJKSTRA_OBJECT_HPP
#define BLINK_GRAPH_DIJKSTRA_OBJECT_HPP

#include <blink/graph/dijkstra_state.hpp>
#include <blink/graph/dijkstra_control.hpp>
#include <blink/graph/relax.hpp>

#include <boost/asio/coroutine.hpp>

#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/properties.hpp> //color_traits
#include <boost/property_map/property_map.hpp>
#include <boost/range.hpp>

#include <boost/asio/yield.hpp> // defines macros, undefined at bottom of this file

namespace blink {

template<typename DijkstraState, typename SourceRange, typename ControlMap>
struct dijkstra_object : dijkstra_state_mixin<DijkstraState>
{
  dijkstra_object(const DijkstraState& state, const SourceRange& source_range)
    : dijkstra_state_mixin(boost::shared_ptr<DijkstraState>(new DijkstraState(state) ) )
    , m_source_range(source_range)
  {}

  bool operator()() // jump to next control point
  {
    func();
    return !m_coro.is_complete();
  }

  vertex_descriptor get_u() { return m_u; }          // latest (source) vertex
  vertex_descriptor get_v() { return m_v;}           // latest (target) vertex
  edge_descriptor get_e() { return m_e;}             // latest edge
  control_point get_control_point() { return m_cp; } // latest control point

  // TODO (?): do we need two functions to get the latest vertex?
  vertex_descriptor get_vertex()
  {
    switch (m_cp)
    {
    case  cp_initialize_vertex:
    case  cp_examine_vertex:
    case  cp_finish_vertex:
      return m_u;
    case  cp_gray_target:
    case  cp_black_target:
    case  cp_discover_vertex:
      return m_v;
    case  cp_examine_edge:
    case  cp_edge_relaxed:
    case  cp_edge_not_relaxed:
    case  cp_tree_edge:
    case  cp_non_tree_edge:
    case  cp_invalid:
    default:
      // should not be gettting vertex
      assert(false);
      return m_u;
    }
  }

  edge_descriptor get_edge()
  {
    return m_e;
  }

 private:
  typedef typename typename boost::range_iterator<const SourceRange>::type source_range_iterator;

  template<control_point CP> struct yield_here : is_cp_included<ControlMap, CP> {};

  // TODO (?): maybe it is useful return the control_point?
  void func()
  {
      reenter(m_coro) {
        // Init vertices
        for (boost::tie(ui, ui_end) = vertices(m_graph); ui != ui_end; ++ui) {
          m_u = *ui;
          m_v = *ui;

          m_graph_visitor.initialize_vertex(m_u, m_graph);
          if(yield_here<cp_initialize_vertex>::value) yield m_cp = cp_initialize_vertex;

          put(m_vertex_color, m_u, color_traits::white() );
          put(m_vertex_predecessor, m_u, m_u);
          put(m_vertex_distance, m_u, m_distance_inf);
        }

        // Init sources
        clear(m_max_priority_queue);
        ri = boost::begin(m_source_range);
        ri_end = boost::end(m_source_range);
        for(; ri != ri_end; ++ri) {
          put(m_vertex_color, *ri, color_traits::gray() );
          put(m_vertex_distance, *ri, m_distance_zero);
          m_max_priority_queue.push(*ri);
          m_u = *ri;
          m_v = *ri;

          m_graph_visitor.discover_vertex(*ri, m_graph);
          if(yield_here<cp_discover_vertex>::value) yield m_cp = cp_discover_vertex;
        }

        // Main loop
        while (! m_max_priority_queue.empty()) {
          m_u = m_max_priority_queue.top();
          m_max_priority_queue.pop();

          m_graph_visitor.examine_vertex(m_u, m_graph);
          if(yield_here<cp_examine_vertex>::value) yield m_cp = cp_examine_vertex;

          for (boost::tie(ei, ei_end) = out_edges(m_u, m_graph); ei != ei_end; ++ei) {
            m_e = *ei;
            m_v = target(m_e, m_graph);

            if (m_distance_compare( boost::get(m_edge_weight, m_e), m_distance_zero)) {
              boost::throw_exception(boost::negative_edge());
            }

            m_graph_visitor.examine_edge(m_e, m_graph);
            if(yield_here<cp_examine_edge>::value) yield m_cp = cp_examine_edge;

            v_color = boost::get(m_vertex_color, m_v);
            if (v_color == color_traits::white()) {

              if(yield_here<cp_tree_edge>::value) yield m_cp = cp_tree_edge;

              relax_target_confident(m_e, m_graph, m_edge_weight, m_vertex_predecessor,
                m_vertex_distance, m_distance_combine);

              m_graph_visitor.edge_relaxed(m_e, m_graph);
              if(yield_here<cp_edge_relaxed>::value) yield  m_cp = cp_edge_relaxed;

              m_graph_visitor.discover_vertex(m_v, m_graph);
              if(yield_here<cp_discover_vertex>::value) yield m_cp = cp_discover_vertex;

              put(m_vertex_color, m_v, color_traits::gray());
              m_max_priority_queue.push(m_v);

            } else {

              if(yield_here<cp_non_tree_edge>::value) yield m_cp = cp_non_tree_edge;

              if (v_color == color_traits::gray()){

                if(yield_here<cp_gray_target>::value) yield m_cp = cp_gray_target;

                decreased = relax_target(m_e, m_graph, m_edge_weight, m_vertex_predecessor,
                  m_vertex_distance, m_distance_combine, m_distance_compare);

                if (decreased) {
                  m_max_priority_queue.update(m_v);

                  m_graph_visitor.edge_relaxed(m_e, m_graph);
                  if(yield_here<cp_edge_relaxed>::value) yield m_cp = cp_edge_relaxed;

                } else {

                  m_graph_visitor.edge_not_relaxed(m_e, m_graph);
                  if(yield_here<cp_edge_not_relaxed>::value) yield  m_cp = cp_edge_not_relaxed;

                }
              } else { // v_color = black

                  if(yield_here<cp_black_target>::value) yield m_cp = cp_black_target;

              }
            }
          } // end for
          put(m_vertex_color, m_u, color_traits::black());
          m_graph_visitor.finish_vertex(m_u, m_graph);

          if(yield_here<cp_finish_vertex>::value) yield m_cp = cp_finish_vertex;

        } // end while
        m_cp = cp_invalid;
    } // reenter
  }  // func

  // All variables in the algorithm must be member variables, hence declared here:
  source_range_iterator ri, ri_end;
  vertex_iterator       ui, ui_end;
  out_edge_iterator     ei, ei_end;
  color_type v_color;
  bool decreased;

  // These are the variables that are returned to the user
  vertex_descriptor m_u;
  vertex_descriptor m_v;
  edge_descriptor m_e;
  control_point m_cp;

  // This is input
  const SourceRange& m_source_range;

  // The stackless coroutine that allows resuming the algorithm
  boost::asio::coroutine m_coro;
}; // class

  // extend dijkstra_state_helper with dijkstra_object type
template <typename Graph, typename SourceRange, typename ControlMap, typename Params>
struct dijkstra_object_helper : protected dijkstra_state_helper<Graph, Params>
{
  typedef typename boost::bgl_named_params
      < typename Params::value_type
      , typename Params::tag_type
      , typename Params::next_type> params_bgl;

  BOOST_STATIC_ASSERT(boost::is_same<Params, params_bgl >::value);

  typedef typename dijkstra_state_helper<Graph, params_bgl> parent;

  template<typename Tag>
  struct param : parent::template param<Tag>
  { };

  // The dijkstra_state with all template parameters resolved
  typedef typename  parent::type dijkstra_state_type;

  typedef typename dijkstra_object<dijkstra_state_type, SourceRange, ControlMap> type;

  static type make(
    const Graph& g, SourceRange& source_range, const params_bgl& params)
  {
    return type(parent::make(g, params), source_range);
  }
}; // struct dijkstra_object_helper

// Make a dijkstra_state object
template <typename Graph, typename SourceRange, typename ControlMap, typename Params>
typename dijkstra_object_helper<Graph, SourceRange, ControlMap, Params>::type
make_dijkstra_object(const Graph& g, SourceRange& source_range, ControlMap, const Params& params)
{
  return dijkstra_object_helper<Graph, SourceRange, ControlMap, Params>::make(g, source_range, params);
}

// Make a dijkstra_state object, using only default parameters
template <typename Graph, typename SourceRange, typename ControlMap>
typename dijkstra_object_helper<Graph, SourceRange, ControlMap, boost::no_named_parameters>::type
make_dijkstra_object(const Graph& g, SourceRange& source_range, ControlMap)
{
  return make_dijkstra_object(g, source_range, ControlMap(), boost::no_named_parameters() );
}

// Make a dijkstra_state object, using only default parameters also for control map
// TODO (?): add the control_map to the named parameters
template <typename Graph, typename SourceRange>
typename dijkstra_object_helper<Graph, SourceRange, only_finish_vertex_type, boost::no_named_parameters>::type
make_dijkstra_object(const Graph& g, SourceRange& source_range)
{
  return make_dijkstra_object(g, source_range, only_finish_vertex_type(), boost::no_named_parameters());
}

template <typename Graph, typename SourceRange, typename Type, typename Tag, typename Rest>
typename dijkstra_object_helper<Graph, SourceRange, only_finish_vertex_type, boost::bgl_named_params<Type, Tag, Rest> >::type
  make_dijkstra_object(const Graph& g, SourceRange& source_range, boost::bgl_named_params<Type, Tag, Rest>& params)
{
    return make_dijkstra_object(g, source_range, only_finish_vertex_type(), params);
}

}// namespace blink

#include <boost/asio/unyield.hpp> // undefines macros

#endif //BLINK_GRAPH_DIJKSTRA_OBJECT_HPP