//
// Created by denn nevera on 2019-08-15.
//

#pragma once

#include <vector>
#include <armadillo>
#include <iostream>
#include <memory>
#include <functional>

#include "dehancer/vectors.hpp"
#include "dehancer/details/optional.hpp"
#include "dehancer/scope_guard.hpp"
#include "dehancer/details/observable_array.hpp"

namespace dehancer{

    namespace math {

        using namespace tl;

        struct Bounds {
            float2 left;
            float2 right;
        };

        namespace protocol {

            class Interpolator {

            public:

                /***
                 * Control points
                 */
                observable::Array<dehancer::math::float2> controls;

                /***
                 * Interpolation resolution
                 */
                READONLY_PROPERTY(resolution,
                                  size_t, Interpolator,
                                  { return this->resolution_; } // getter
                );

                /**
                 * Interpolation precision step
                 */
                READONLY_PROPERTY(step,
                                  float, Interpolator,
                                  { return 1.0f/this->resolution_; } // getter
                );

                /***
                 * Lerp value
                 * @param x - current "time"
                 * @return interpolated value
                 */
                virtual float value(float x) const = 0;

                /***
                 * Get linear interpolation for the x "time"
                 * @param x - "time"
                 * @return interpolated value
                 */
                virtual float linear(float x) const;

                /***
                 * Test bounds for the x "time"
                 * @return return nullopt if x is within bounds or closest bound
                 */
                virtual optional<float> test_bounds(float x) const;

                /**
                 * Interpolation bounds
                 * @return by default 0,0:1,1
                 */
                virtual Bounds get_bounds() const;

                /***
                 * Minimal control points that must be set for the intrapolation method
                 * @return size
                 */
                virtual size_t minimum_controls() const = 0;

                /***
                 * Get indices for the current interpolation control points
                 * @param x - "time"
                 * @return
                 */
                virtual std::tuple<std::size_t,std::size_t> indices(float x) const ;

                /***
                 * Return indices of piece on linear cut from any controls sequence
                 * @param controls - control points
                 * @param x - "time"
                 * @return start/end indices
                 */
                static std::tuple<std::size_t,std::size_t> indices(const std::vector<float2>& controls, float x);

                /***
                 * Get linear interpolation for the x "time" over curve
                 * @param curve
                 * @param x
                 * @return
                 */
                static float linear(const std::vector<float2>& curve, float x);

                /***
                 * Create interpolation object
                 * @param resolution - interpolation resolution
                 */
                explicit Interpolator(size_t resolution = 256);

                explicit Interpolator(const std::vector<float2>& controls, size_t resolution = 256);

                virtual ~Interpolator() = default;

            private:
                size_t resolution_;
            };
        }
    }
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    NonlinearFactor.cpp
 * @brief   Nonlinear Factor base classes
 * @author  Frank Dellaert
 * @author  Richard Roberts
 */

#include <gtsam/nonlinear/NonlinearFactor.h>
#include <boost/make_shared.hpp>
#include <boost/format.hpp>

namespace gtsam {

/* ************************************************************************* */
void NonlinearFactor::print(const std::string& s,
    const KeyFormatter& keyFormatter) const {
  std::cout << s << "  keys = { ";
  for(Key key: keys()) {
    std::cout << keyFormatter(key) << " ";
  }
  std::cout << "}" << std::endl;
}

/* ************************************************************************* */
bool NonlinearFactor::equals(const NonlinearFactor& f, double tol) const {
  return Base::equals(f);
}

/* ************************************************************************* */
NonlinearFactor::shared_ptr NonlinearFactor::rekey(
    const std::map<Key, Key>& rekey_mapping) const {
  shared_ptr new_factor = clone();
  for (size_t i = 0; i < new_factor->size(); ++i) {
    Key& cur_key = new_factor->keys()[i];
    std::map<Key, Key>::const_iterator mapping = rekey_mapping.find(cur_key);
    if (mapping != rekey_mapping.end())
      cur_key = mapping->second;
  }
  return new_factor;
}

/* ************************************************************************* */
NonlinearFactor::shared_ptr NonlinearFactor::rekey(
    const KeyVector& new_keys) const {
  assert(new_keys.size() == keys().size());
  shared_ptr new_factor = clone();
  new_factor->keys() = new_keys;
  return new_factor;
}

/* ************************************************************************* */
void NoiseModelFactor::print(const std::string& s,
    const KeyFormatter& keyFormatter) const {
  Base::print(s, keyFormatter);
  if (noiseModel_)
    noiseModel_->print("  noise model: ");
}

/* ************************************************************************* */
bool NoiseModelFactor::equals(const NonlinearFactor& f, double tol) const {
  const NoiseModelFactor* e = dynamic_cast<const NoiseModelFactor*>(&f);
  return e && Base::equals(f, tol)
      && ((!noiseModel_ && !e->noiseModel_)
          || (noiseModel_ && e->noiseModel_
              && noiseModel_->equals(*e->noiseModel_, tol)));
}

/* ************************************************************************* */
static void check(const SharedNoiseModel& noiseModel, size_t m) {
  if (noiseModel && m != noiseModel->dim())
    throw std::invalid_argument(
        boost::str(
            boost::format(
                "NoiseModelFactor: NoiseModel has dimension %1% instead of %2%.")
                % noiseModel->dim() % m));
}

/* ************************************************************************* */
Vector NoiseModelFactor::whitenedError(const Values& c) const {
  const Vector b = unwhitenedError(c);
  check(noiseModel_, b.size());
  return noiseModel_ ? noiseModel_->whiten(b) : b;
}

/* ************************************************************************* */
double NoiseModelFactor::error(const Values& c) const {
  if (active(c)) {
    const Vector b = unwhitenedError(c);
    check(noiseModel_, b.size());
    if (noiseModel_)
      return 0.5 * noiseModel_->distance(b);
    else
      return 0.5 * b.squaredNorm();
  } else {
    return 0.0;
  }
}

/* ************************************************************************* */
std::shared_ptr<GaussianFactor> NoiseModelFactor::linearize(
    const Values& x) const {

  // Only linearize if the factor is active
  if (!active(x))
    return std::shared_ptr<JacobianFactor>();

  // Call evaluate error to get Jacobians and RHS vector b
  std::vector<Matrix> A(size());
  Vector b = -unwhitenedError(x, A);
  check(noiseModel_, b.size());

  // Whiten the corresponding system now
  if (noiseModel_)
    noiseModel_->WhitenSystem(A, b);

  // Fill in terms, needed to create JacobianFactor below
  std::vector<std::pair<Key, Matrix> > terms(size());
  for (size_t j = 0; j < size(); ++j) {
    terms[j].first = keys()[j];
    terms[j].second.swap(A[j]);
  }

  // TODO pass unwhitened + noise model to Gaussian factor
  using noiseModel::Constrained;
  if (noiseModel_ && noiseModel_->isConstrained())
    return GaussianFactor::shared_ptr(
        new JacobianFactor(terms, b,
            std::static_pointer_cast<Constrained>(noiseModel_)->unit()));
  else
    return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
}

/* ************************************************************************* */

} // \namespace gtsam


#include <algorithm>
#include <boost/program_options.hpp>
#include <boost/algorithm/string/predicate.hpp>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <limits>
#include <random>
#include <vector>

enum returnID {
	success_help = -1,
	success = 0,

	overd_err = 1,
	underd_err = 2,
	zero_err = 3,
	gen_err = 4,
	conflict_err = 5,
	vect_nan = 6,
	known_err = 7,
	other_err = 8
};

struct program_args {
	// general
	int precision;
	bool quiet, list, numbers_force, flags;
	std::string delim = "\n";
	// intern
	long long number;
	long double lbound, ubound;
	std::string generator;
	// rounding
	bool ceil, floor, round, trunc;
	// matcher
	std::vector<long double> excluded, included;
	bool norepeat;
	std::vector<std::string> prefix, suffix, contains;
	// stats
	bool stat_all, stat_min, stat_max, stat_median,
		stat_avg, stat_var, stat_std, stat_coef;
};

returnID parse_args(program_args & args, int argc, char const * const * argv) {
	static auto const ld_prec = std::numeric_limits<long double>::max_digits10;

	namespace po = boost::program_options;
	po::options_description general("General options");
	general.add_options()
		("help,h", "produce this help message")
		("precision,p", po::value<int>(&args.precision)->default_value(ld_prec),
			"output precision (not internal precision)")
		("quiet,q", po::bool_switch(&args.quiet)->default_value(false),
			"disable number output, useful with stats")
		("list", po::bool_switch(&args.list)->default_value(false),
			"print numbers in a list")
		("delim", po::value<std::string>(&args.delim),
			"change the delimiter")
		("numbers-force", po::bool_switch(&args.numbers_force)->default_value(false),
			"force the count of numbers printed to be equal to --number")
		("flags", po::bool_switch(&args.flags)->default_value(false),
			"print the flags");

	po::options_description intern("Internal RNG options");
	intern.add_options()
		("number,n", po::value<long long>(&args.number)->default_value(1),
			"count of numbers to be generated")
		("lbound,l", po::value<long double>(&args.lbound)->default_value(0.0),
			"minimum number (ldouble)")
		("ubound,u", po::value<long double>(&args.ubound)->default_value(1.0),
			"maximum number (ldouble)")
		("generator,g", po::value<std::string>(&args.generator)->default_value("mt19937"),
			"change the RNG algorithm:\nminstd_rand0, minstd_rand"
			"\nmt19937, mt19937_64\nranlux24_base, ranlux48_base"
			"\nranlux24, ranlux48\nknuth_b, default_random_engine"
			"\nbadrandom (std::rand)");

	po::options_description rounding("Rounding options");
	rounding.add_options()
		("ceil,c", po::bool_switch(&args.ceil)->default_value(false),
			"apply ceiling function")
		("floor,f", po::bool_switch(&args.floor)->default_value(false),
			"apply floor function")
		("round,r", po::bool_switch(&args.round)->default_value(false),
			"apply round function")
		("trunc,t", po::bool_switch(&args.trunc)->default_value(false),
			"apply truncation");

	po::options_description matcher("Matcher options");
	matcher.add_options()
		("exclude", po::value<std::vector<long double> >(&args.excluded)->multitoken(),
			"print only the numbers not exactly specified, best with rounding")
		("include", po::value<std::vector<long double> >(&args.included)->multitoken(),
			"print only the numbers exactly specified, best with rounding ")
		("norepeat", po::bool_switch(&args.norepeat)->default_value(false),
			"exclude repeated numbers from being printed, best with rounding")
		("prefix", po::value<std::vector<std::string> >(&args.prefix)->multitoken(),
			"only print if the number begins with string(s)")
		("suffix", po::value<std::vector<std::string> >(&args.suffix)->multitoken(),
			"only print if the number ends with string(s)")
		("contains", po::value<std::vector<std::string> >(&args.contains)->multitoken(),
			"only print if the number contains string(s)");

	po::options_description stats("Statistics options");
	stats.add_options()
		("stat-all", po::bool_switch(&args.stat_all)->default_value(false),
			"print all statistics")
		("stat-min", po::bool_switch(&args.stat_min)->default_value(false),
			"print the minimum")
		("stat-max", po::bool_switch(&args.stat_max)->default_value(false),
			"print the maximum")
		("stat-median", po::bool_switch(&args.stat_median)->default_value(false),
			"print the median")
		("stat-avg", po::bool_switch(&args.stat_avg)->default_value(false),
			"print the average")
		("stat-var", po::bool_switch(&args.stat_var)->default_value(false),
			"print the variance")
		("stat-std", po::bool_switch(&args.stat_std)->default_value(false),
			"print the standard deviation")
		("stat-coef", po::bool_switch(&args.stat_coef)->default_value(false),
			"print the coefficient of variation");

	po::options_description all("Allowed options");
	all.add(general).add(intern).add(rounding).add(matcher).add(stats);

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, all), vm);
	po::notify(vm);

	if(vm.count("help")) {
		std::cout << all << '\n';
		return returnID::success_help;
	}

	if(args.precision > ld_prec) {
		std::cerr << "error: --precision cannot be greater than the precision"
			" for <long double> (" << ld_prec << ")\n";
		return returnID::overd_err;
	}

	if(args.precision <= -1) {
		std::cerr << "error: --precision cannot be < 0\n";
		return returnID::underd_err;
	}

	if(args.number <= 0) {
		std::cerr << "error: the argument for option '--number' is invalid"
			" (must be >= 1)\n";
		return returnID::zero_err;
	}

	const std::vector<std::string> gen_opts {{"minstd_rand0", "minstd_rand",
		"mt19937", "mt19937_64", "ranlux24_base", "ranlux48_base", "ranlux24",
		"ranlux48", "knuth_b", "default_random_engine", "badrandom"}};
	if(std::find(gen_opts.begin(), gen_opts.end(), args.generator) == gen_opts.end()) {
		std::cerr << "error: --generator must be: minstd_rand0, minstd_rand, "
			"mt19937, mt19937_64, ranlux24_base, ranlux48_base, "
			"ranlux24, ranlux48, knuth_b, default_random_engine, badrandom\n";
		return returnID::gen_err;
	}

	if(args.ceil + args.floor + args.round + args.trunc > 1) {
		std::cerr << "error: --ceil, --floor, --round, and --trunc"
			" are mutually exclusive\n";
		return returnID::conflict_err;
	}

	if(args.ceil || args.floor || args.round || args.trunc) {
		args.precision = 0;
	}

	std::vector<std::vector<std::string> > filters = {{args.prefix, args.suffix, args.contains}};
	for(auto i : filters) {
		for(auto j : i) {
			if(j.find_first_not_of("0123456789.") != std::string::npos || std::count(std::begin(j), std::end(j), '.') > 1) {
				std::cerr << "error: --prefix, --suffix, and --contains can only be numbers\n";
				return returnID::vect_nan;
			}
		}
	}

	return returnID::success;
}

template<typename GEN = std::mt19937, typename DIST = std::uniform_real_distribution<long double> >
std::function<long double()> r_gen(const program_args & args) {
	GEN generator{std::random_device{}()};
	DIST dis{args.lbound, args.ubound};
	return [dis, generator]() mutable -> auto { return dis(generator); };
}

long double random(const program_args & args) {
	if(args.generator == "minstd_rand0") return r_gen<std::minstd_rand0>(args)();
	else if(args.generator == "minstd_rand") return r_gen<std::minstd_rand>(args)();
	else if(args.generator == "mt19937_64") return r_gen<std::mt19937_64>(args)();
	else if(args.generator == "ranlux24_base") return r_gen<std::ranlux24_base>(args)();
	else if(args.generator == "ranlux48_base") return r_gen<std::ranlux48_base>(args)();
	else if(args.generator == "ranlux24") return r_gen<std::ranlux24>(args)();
	else if(args.generator == "ranlux48") return r_gen<std::ranlux48>(args)();
	else if(args.generator == "knuth_b") return r_gen<std::knuth_b>(args)();
	else if(args.generator == "default_random_engine") return r_gen<std::default_random_engine>(args)();
	else if(args.generator == "badrandom")
		return args.lbound + (std::rand() / (RAND_MAX / (args.ubound - args.lbound)));
	else return r_gen<std::mt19937>(args)();
}

bool filter(const long double rand, const int precision,
		const std::vector<std::string> & fx,
		bool(*predicate)(const std::string&, const std::string&)) {
	std::ostringstream oss;
	oss << std::fixed << std::setprecision(precision) << rand;
	const auto str_rand = oss.str();
	return std::none_of(fx.begin(), fx.end(), [&](auto const & s) { return predicate(str_rand, s); });
}

int main(int argc, char* argv[]) {
	try {
		program_args args;

		switch(auto result = parse_args(args, argc, argv)) {
			case returnID::success: break;
			case returnID::success_help: return 0;
			default: return result;
		}

		std::vector<long double> generated;

		std::cout.precision(args.precision);

		if(args.generator == "badrandom") std::srand(std::time(nullptr));

		long long list_cnt = 0;

		for(long long i = 1; i <= args.number;) {
			if(!args.numbers_force) ++i;
			if(args.list) ++list_cnt;

			long double rand = random(args);

			if(args.ceil) rand = std::ceil(rand);
			else if(args.floor) rand = std::floor(rand);
			else if(args.round) rand = std::round(rand);
			else if(args.trunc) rand = std::trunc(rand);

			if(!args.excluded.empty() && std::find(args.excluded.begin(), args.excluded.end(), rand) != args.excluded.end())
				continue;
			else if(!args.included.empty() && std::find(args.included.begin(), args.included.end(), rand) == args.included.end())
				continue;
			else if(args.norepeat && std::find(generated.begin(), generated.end(), rand) != generated.end())
				continue;
			else if(!args.prefix.empty() && filter(rand, args.precision, args.prefix, boost::starts_with))
				continue;
			else if(!args.suffix.empty() && filter(rand, args.precision, args.suffix, boost::ends_with))
				continue;
			else if(!args.contains.empty() && filter(rand, args.precision, args.contains, boost::contains))
				continue;

			generated.push_back(rand);

			if(!args.quiet) {
				if(args.list && args.numbers_force) std::cout << i << ". ";
				if(args.list) std::cout << list_cnt << ". ";
				std::cout << std::fixed << rand << args.delim;
				if(args.numbers_force) ++i;
			}
		}

		if(args.delim != "\n" && !args.quiet) std::cout << '\n';

		if((args.stat_all || args.stat_min || args.stat_max || args.stat_median || args.stat_avg
			|| args.stat_var || args.stat_std || args.stat_coef) && !args.quiet)
			std::cout << '\n';

		if(args.stat_all || args.stat_min || args.stat_max) {
			auto minmax = std::minmax_element(generated.begin(), generated.end());
			if(args.stat_all || args.stat_min)
				std::cout << std::fixed << "min: " << *minmax.first << '\n';
			if(args.stat_all || args.stat_max)
				std::cout << std::fixed << "max: " << *minmax.second << '\n';
		}

		if(args.stat_all || args.stat_median) {
			auto midpoint = generated.begin() + generated.size() / 2;
			std::nth_element(generated.begin(), midpoint, generated.end());
			auto median = *midpoint;
			if(generated.size() % 2 == 0)
				median = (median + *std::max_element(generated.begin(), midpoint)) / 2;
			std::cout << std::fixed << "median: " << median << '\n';
		}

		if(args.stat_all || args.stat_avg) {
			long double avg = std::accumulate(generated.begin(), generated.end(), 0.0) / generated.size();
			std::cout << std::fixed << "avg: " << avg << '\n';
		}

		if(args.stat_all || args.stat_var) {
			long double avg = std::accumulate(generated.begin(), generated.end(), 0.0) / generated.size();
			long double var = 0.0;
			for(const auto & i : generated) var += std::pow(i - avg, 2);
			std::cout << std::fixed << "variance: " << var / generated.size() << '\n';
		}

		if(args.stat_all || args.stat_std) {
			long double avg = std::accumulate(generated.begin(), generated.end(), 0.0) / generated.size();
			long double std = 0.0;
			for(const auto & i : generated) std += std::pow(i - avg, 2);
			std::cout << std::fixed << "standard deviation: " << std::sqrt(std / generated.size()) << '\n';
		}

		if(args.stat_all || args.stat_coef) {
			long double avg = std::accumulate(generated.begin(), generated.end(), 0.0) / generated.size();
			long double std = 0.0;
			for(const auto & i : generated) std += std::pow(i - avg, 2);
			std::cout << std::fixed << "coefficient of variation: " << std::sqrt(std / generated.size()) / avg << '\n';
		}

		if(args.flags) {
			std::cout << "\nFlags:\n - General options:\n\thelp: 0"
				<< "\n\tprecision: " << args.precision
				<< "\n\tquiet: " << args.quiet
				<< "\n\tlist: " << args.list
				<< "\n\tnumbers-force: " << args.numbers_force
				<< "\n\tflags: 1"
				<< "\n\tdelim: " << args.delim
				<< "\n - Internal RNG options:"
				<< "\n\tnumber: " << args.number
				<< "\n\tlbound: " << args.lbound
				<< "\n\tubound: " << args.ubound
				<< "\n\tgenerator: " << args.generator
				<< "\n - Rounding options:"
				<< "\n\tceil: " << args.ceil
				<< "\n\tfloor: " << args.floor
				<< "\n\tround: " << args.round
				<< "\n\ttrunc: " << args.trunc
				<< "\n - Matcher options:"
				<< "\n\texclude: ";
			for(const auto & i : args.excluded) std::cout << i << ' ';
			std::cout << "\n\tinclude: ";
			for(const auto & i : args.included) std::cout << i << ' ';
			std::cout << "\n\tnorepeat: " << args.norepeat
				<< "\n\tprefix: ";
			for(const auto & i : args.prefix) std::cout << i << ' ';
			std::cout << "\n\tsuffix: ";
			for(const auto & i : args.suffix) std::cout << i << ' ';
			std::cout << "\n\tcontains: ";
			for(const auto & i : args.contains) std::cout << i << ' ';
			std::cout << "\n - Statistics options:"
				<< "\n\tstat-all: " << args.stat_all
				<< "\n\tstat-min: " << args.stat_min
				<< "\n\tstat-max: " << args.stat_max
				<< "\n\tstat-median: " << args.stat_median
				<< "\n\tstat-avg: " << args.stat_avg
				<< "\n\tstat-var: " << args.stat_var
				<< "\n\tstat-std: " << args.stat_std
				<< "\n\tstat-coef: " << args.stat_coef << '\n';
		}

		return returnID::success;

	} catch(std::exception & e) {
		std::cerr << "error: " << e.what() << '\n';
		return returnID::known_err;

	} catch(...) {
		std::cerr << "error: exception of unknown type!\n";
		return returnID::other_err;
	}
}


// Copyright 2018 Hans Dembinski
//
// 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)

#include <algorithm>
#include <boost/core/lightweight_test.hpp>
#include <boost/histogram/axis/integer.hpp>
#include <boost/histogram/axis/ostream.hpp>
#include <boost/histogram/axis/variable.hpp>
#include <boost/histogram/histogram.hpp>
#include <boost/histogram/indexed.hpp>
#include <boost/histogram/literals.hpp>
#include <boost/histogram/ostream.hpp>
#include <boost/mp11/algorithm.hpp>
#include <boost/mp11/list.hpp>
#include <iterator>
#include <ostream>
#include <type_traits>
#include <vector>
#include "throw_exception.hpp"
#include "utility_histogram.hpp"

using namespace boost::histogram;
using namespace boost::histogram::literals;
using namespace boost::mp11;

template <class Tag, class Coverage>
void run_1d_tests(mp_list<Tag, Coverage>) {
  auto h = make(Tag(), axis::integer<>(0, 3));
  h(-1, weight(1));
  h(0, weight(2));
  h(1, weight(3));
  h(2, weight(4));
  h(3, weight(5));

  auto ind = indexed(h, Coverage());
  auto it = ind.begin();
  BOOST_TEST_EQ(it->indices().size(), 1);
  BOOST_TEST_EQ(it->indices()[0], Coverage() == coverage::all ? -1 : 0);

  if (Coverage() == coverage::all) {
    BOOST_TEST_EQ(it->index(0), -1);
    BOOST_TEST_EQ(**it, 1);
    BOOST_TEST_EQ(it->bin(0), h.axis().bin(-1));
    ++it;
  }
  BOOST_TEST_EQ(it->index(0), 0);
  BOOST_TEST_EQ(**it, 2);
  BOOST_TEST_EQ(it->bin(0), h.axis().bin(0));
  ++it;
  BOOST_TEST_EQ(it->index(0), 1);
  BOOST_TEST_EQ(**it, 3);
  BOOST_TEST_EQ(it->bin(0), h.axis().bin(1));
  ++it;
  // check post-increment
  auto prev = it++;
  BOOST_TEST_EQ(prev->index(0), 2);
  BOOST_TEST_EQ(**prev, 4);
  BOOST_TEST_EQ(prev->bin(0), h.axis().bin(2));
  if (Coverage() == coverage::all) {
    BOOST_TEST_EQ(it->index(0), 3);
    BOOST_TEST_EQ(**it, 5);
    BOOST_TEST_EQ(it->bin(0), h.axis().bin(3));
    ++it;
  }
  BOOST_TEST(it == ind.end());

  for (auto&& x : indexed(h, Coverage())) *x = 0;

  for (auto&& x : indexed(static_cast<const decltype(h)&>(h), Coverage()))
    BOOST_TEST_EQ(*x, 0);
}

template <class Tag, class Coverage>
void run_3d_tests(mp_list<Tag, Coverage>) {
  auto h = make_s(Tag(), std::vector<int>(), axis::integer<>(0, 2),
                  axis::integer<int, axis::null_type, axis::option::none_t>(0, 3),
                  axis::integer<int, axis::null_type, axis::option::overflow_t>(0, 4));

  for (int i = -1; i < 3; ++i)
    for (int j = -1; j < 4; ++j)
      for (int k = -1; k < 5; ++k) h(i, j, k, weight(i * 100 + j * 10 + k));

  auto ind = indexed(h, Coverage());
  auto it = ind.begin();
  BOOST_TEST_EQ(it->indices().size(), 3);

  const int d = Coverage() == coverage::all;

  // imitate iteration order of indexed loop
  for (int k = 0; k < 4 + d; ++k)
    for (int j = 0; j < 3; ++j)
      for (int i = -d; i < 2 + d; ++i) {
        BOOST_TEST_EQ(it->index(0), i);
        BOOST_TEST_EQ(it->index(1), j);
        BOOST_TEST_EQ(it->index(2), k);
        BOOST_TEST_EQ(it->bin(0_c), h.axis(0_c).bin(i));
        BOOST_TEST_EQ(it->bin(1_c), h.axis(1_c).bin(j));
        BOOST_TEST_EQ(it->bin(2_c), h.axis(2_c).bin(k));
        BOOST_TEST_EQ(**it, i * 100 + j * 10 + k);
        ++it;
      }
  BOOST_TEST(it == ind.end());
}

template <class Tag, class Coverage>
void run_density_tests(mp_list<Tag, Coverage>) {
  auto ax = axis::variable<>({0.0, 0.1, 0.3, 0.6});
  auto ay = axis::integer<int>(0, 2);
  auto az = ax;
  auto h = make_s(Tag(), std::vector<int>(), ax, ay, az);

  // fill uniformly
  for (auto&& x : h) x = 1;

  for (auto&& x : indexed(h, Coverage())) {
    BOOST_TEST_EQ(x.density(), *x / (x.bin(0).width() * x.bin(2).width()));
  }
}

template <class Tag, class Coverage>
void run_stdlib_tests(mp_list<Tag, Coverage>) {
  auto ax = axis::regular<>(3, 0, 1);
  auto ay = axis::integer<>(0, 2);
  auto h = make_s(Tag(), std::array<int, 20>(), ax, ay);

  struct generator {
    int i = 0;
    int operator()() { return ++i; }
  };

  auto ind = indexed(h, Coverage());
  std::generate(ind.begin(), ind.end(), generator{});

  {
    int i = 0;
    for (auto&& x : ind) BOOST_TEST_EQ(*x, ++i);
  }

  {
    auto it = std::min_element(ind.begin(), ind.end());
    BOOST_TEST(it == ind.begin());
    BOOST_TEST(it != ind.end());
  }

  {
    auto it = std::max_element(ind.begin(), ind.end());
    // get last before end()
    auto it2 = ind.begin();
    auto it3 = it2;
    while (it2 != ind.end()) it3 = it2++;
    BOOST_TEST(it == it3);
    BOOST_TEST(it != ind.begin());
  }
}

template <class Tag>
void run_indexed_with_range_tests(Tag) {
  {
    auto ax = axis::integer<>(0, 4);
    auto ay = axis::integer<>(0, 3);

    auto h = make(Tag(), ax, ay);
    for (auto&& x : indexed(h, coverage::all)) x = x.index(0) + x.index(1);
    BOOST_TEST_EQ(h.at(0, 0), 0);
    BOOST_TEST_EQ(h.at(0, 1), 1);
    BOOST_TEST_EQ(h.at(0, 2), 2);
    BOOST_TEST_EQ(h.at(1, 0), 1);
    BOOST_TEST_EQ(h.at(1, 1), 2);
    BOOST_TEST_EQ(h.at(1, 2), 3);
    BOOST_TEST_EQ(h.at(2, 0), 2);
    BOOST_TEST_EQ(h.at(2, 1), 3);
    BOOST_TEST_EQ(h.at(2, 2), 4);
    BOOST_TEST_EQ(h.at(3, 0), 3);
    BOOST_TEST_EQ(h.at(3, 1), 4);
    BOOST_TEST_EQ(h.at(3, 2), 5);

    /* x->
    y  0 1 2 3
    |  1 2 3 4
    v  2 3 4 5
    */

    int range[2][2] = {{1, 3}, {0, 2}};
    auto ir = indexed(h, range);
    auto sum = std::accumulate(ir.begin(), ir.end(), 0.0);
    BOOST_TEST_EQ(sum, 1 + 2 + 2 + 3);
  }
}

int main() {
  mp_for_each<mp_product<mp_list, mp_list<static_tag, dynamic_tag>,
                         mp_list<std::integral_constant<coverage, coverage::inner>,
                                 std::integral_constant<coverage, coverage::all>>>>(
      [](auto&& x) {
        run_1d_tests(x);
        run_3d_tests(x);
        run_density_tests(x);
        run_stdlib_tests(x);
      });

  run_indexed_with_range_tests(static_tag{});
  run_indexed_with_range_tests(dynamic_tag{});
  return boost::report_errors();
}


/* -*-C++-*- */
/*
   (c) Copyright 1993-2005, Hewlett-Packard Development Company, LP

   See the file named COPYING for license details
*/

/** @file
    \brief Header file for a histogram class.
*/

///////////////////////////////////////////////////////////////////////////////
//
// This package provides histogram gathering classes on variables.
// StatsHistograms can be used to approximate the mode and percentiles.
//
// The histograms collect samples into bins, where the number and size of
// bins is determined when the histogram object is created.  Two
// variations are provided: one version rescales bins when values falling
// outside any bin are sampled, while the other lumps extreme values into
// the end bins.  The former are called `scaling' histograms while the
// latter are `fixed'.
//
// When specifying the domain of a histogram, you specify the low and
// high values.  The domain is then all values low <= x < high.  For an
// integer histogram, where you are (say) interested in collecting values
// 0 through 11, specify 0 as the low and _12_ as the high point.
//
// Note also that the histograms will round the number of bins up to an
// even number.  This may have unfortunate side-effects when working with
// integer histograms.  To be safe, you should always specify an even
// number of bins.  You can usually put an extra bin on the high or low
// end.
//
// Note also that the `width' of bins is always a double, even in the
// integer histograms.  This is because width is calculated from the high
// and low points and the number of bins.  If you specify a funny
// combination of these, you might get (say) bins alternating with two
// and three bins.
//
// There are those who believe that integer histograms probably aren't
// terribly useful.  They are probably right.
//
//////////////////////////////////////////////////////////////////////////////

#ifndef LINTEL_STATSHISTOGRAM_HPP
#define LINTEL_STATSHISTOGRAM_HPP

#include <vector>
#include <math.h>
#include <boost/config.hpp>
#include <iostream>
#include <fstream>
#include <Lintel/AssertBoost.hpp>
#include <Lintel/Stats.hpp>

/// \brief Histogram statistics class, interface
class StatsHistogram : public Stats {

public:
    enum HistMode { Uniform, Log, UniformAccum, LogAccum };
    StatsHistogram();
    virtual ~StatsHistogram();
    virtual void reset();	// Put everything back to initial values

    //----Record a new value
    virtual void add(const double value) = 0;
    virtual void add(const Stats &stat);
    virtual void add(const double index_value, const double data_value) = 0;

    //----Histogram-wide access functions
    virtual double binWidth(unsigned index) const = 0;
    virtual double binWidth() const { return binWidth(0); }
    virtual double low() const = 0;
    virtual double binlow(unsigned index) const = 0;
    virtual double bincenter(unsigned index) const = 0;
    virtual double high() const = 0;
    virtual double binhigh(unsigned index) const = 0;
    virtual unsigned numBins() const = 0;
    virtual unsigned numRescales() const = 0;
    virtual unsigned numGrows() const = 0;
    virtual bool isScalable() const = 0;
    virtual bool isGrowable() const = 0;
    virtual double mode() const = 0;
    virtual const std::string getType() const = 0;
    virtual void printTabular(int depth, std::ostream &out) const;
    virtual void printHistogramRandomInput(std::ostream& out) const;
    virtual double percentile(double p) const = 0;   // 0.0 <=p<= 1.0
    virtual unsigned long operator[](unsigned index) const = 0;
				// returns the count in a particular bin

    //----Value-access functions
    virtual std::string debugString() const = 0;

    //----Sometimes we want another one of these, just like this one, but fresh.
    virtual Stats *another_new() const = 0;
};

/// \brief Histogram statistics class, uniformly-sized bins
class StatsHistogramUniform : public StatsHistogram {
protected:
    unsigned       num_bins;	// number of bins
    double        bin_width;	// width of each bin
    double        bin_low;	// lower bound of lowest bin
    double        bin_high;	// upper bound of highest bin
    unsigned long *bins;	// array of bin-counters
    const bool    is_scalable;	// true if rescale on outlier, false if fixed
    const bool    is_growable;	// true if grow on outlier, false if fixed
    unsigned       num_rescales;// number of times rescaled
    unsigned       num_grows;	// number of times rescaled

    unsigned sampleBin(const double x) const;

public:
    StatsHistogramUniform(const unsigned bins_in,
		   const double   low_in,
		   const double   high_in,
		   const bool     scalable = false,
		   const bool     growable = false);
    virtual ~StatsHistogramUniform();
    virtual void reset();	// Put everything back to initial values

    //----Record a new value
    virtual void add(const double value);
    virtual void add(const double index_value, const double data_value);
    using StatsHistogram::add;

    //----Histogram-wide access functions
    double binWidth() const { return bin_width; }
    double binWidth(unsigned /*index*/) const { return bin_width; }
    double   low()          const { return bin_low; }
    double binlow(unsigned index) const { return bin_low + index*bin_width; };
    double bincenter(unsigned index) const { return bin_low + (index+0.5)*bin_width; };
    double   high()         const { return bin_high; }
    double binhigh(unsigned index) const { return bin_low + (index+1)*bin_width; };
    unsigned  numBins() const { return num_bins; }
    unsigned  numRescales()  const { return num_rescales; }
    unsigned  numGrows()  const { return num_grows; }
    bool     isScalable()   const { return is_scalable; }
    bool     isGrowable()   const { return is_growable; }
    double   mode() const;
    double   percentile(double p) const;   // 0.0 <= p <= 1.0
    unsigned long  operator[](unsigned index) const;
				// Count in a particular bin

    //----Value-access functions
    virtual std::string debugString() const;

    virtual const std::string getType() const { return std::string("uniform");}
    virtual void printRome(int depth, std::ostream &out) const;

    //----Sometimes we want another one of these, just like this one, but fresh.
    virtual Stats *another_new() const;
};

/// \brief Histogram statistics class, exponentially-sized bins
class StatsHistogramLog : public StatsHistogram {
protected:
    unsigned	  num_bins;	// number of bins
    double        smallest_bin;	// width of bin zero
    double        bin_high;	// highest bound of highest bin
    double        bin_scaling;	// scales log(smallest-bin count) to bin#
    const bool    is_scalable;	// true if rescale on outlier, false if fixed
    unsigned	  num_rescales; // number of times rescaled
    unsigned long *bins;	// array of bin-counters

    unsigned sampleBin(double x) const;
    double binOffset(double index) const;

public:
    StatsHistogramLog(const unsigned bins_in,
		   const double   low_in,
		   const double   high_in,
                   const bool     scalable_in = false);
    virtual ~StatsHistogramLog();
    virtual void reset();	// Put everything back to initial values

    //----Record a new value
    virtual void add(const double value);
    virtual void add(const double index_value, const double data_value);
    using StatsHistogram::add;

    //----Histogram-wide access functions
    double binWidth() const { return binOffset(1.0); }
    double binWidth(unsigned index) const { return binOffset(index+1)-binOffset(index); }
    double low() const { return smallest_bin; }
    double binlow(unsigned index) const { return binOffset(index);};
    double bincenter(unsigned index) const {return binOffset(index+0.5);};
    double high() const { return bin_high; };
    double binhigh(unsigned index) const { return binOffset(index+1); };
    unsigned  numBins() const { return num_bins; }
    unsigned  numRescales()  const { return 0; }
    bool     isScalable()   const { return false; }
    unsigned  numGrows()  const { return 0; }
    bool     isGrowable()   const { return false; }
    double   mode() const;
    double   percentile(double p) const;   // 0.0 <= p <= 1.0
    unsigned long  operator[](unsigned index) const;  // Count in a particular bin

    //----Value-access functions
    virtual std::string debugString() const;

    virtual const std::string getType() const { return std::string("log");}
    virtual void printRome(int depth, std::ostream &out) const;
    //----Sometimes we want another one of these, just like this one, but fresh.
    virtual Stats *another_new() const;
};

/// \brief Histogram statistics class, exponentially-sized bins, sum as well as count
///
/// a kind of histogram that has exponentially-sized bins, and that
/// accumulates a value in the bin as well as the hit count.
///
/// The histogram can accumulate either the index value, using
/// the add(index_value) method, or some other data value associated
/// with the index value, using the add(index_value, data_value) method.
/// For example, the latter form makes it possible to accumulate response
/// times (data values) as a function of jump distance (index values).

class StatsHistogramLogAccum : public StatsHistogramLog {
private:
    double       *val_bins;

public:
    StatsHistogramLogAccum(const unsigned bins_in,
		   const double   low_in,
		   const double   high_in);
    virtual ~StatsHistogramLogAccum();
    virtual void reset();	// Put everything back to initial values

    //----Record a new value
    virtual void add(const double value);
    virtual void add(const double index_value, const double data_value);
    using StatsHistogram::add;

    //----Histogram-wide access functions
    double value(unsigned index) const;	// value in a particular bin

    //----Value-access functions
    virtual void printRome(int depth, std::ostream &out) const;

    //----Sometimes we want another one of these, just like this one, but fresh.
    virtual Stats *another_new() const;
};

/// \brief Histogram statistics class, exponentially-sized bins, sum as well as count
///
/// a kind of histogram that has uniformly-sized bins, and that
/// accumulates a value in the bin as well as the hit count.
///
/// The histogram can accumulate either the index value, using
/// the add(index_value) method, or some other data value associated
/// with the index value, using the add(index_value, data_value) method.
/// For example, the latter form makes it possible to accumulate response
/// times (data values) as a function of jump distance (index values).
class StatsHistogramUniformAccum : public StatsHistogramUniform {
private:
    double       *val_bins;

public:
    StatsHistogramUniformAccum(const unsigned bins_in,
		   const double   low_in,
		   const double   high_in,
   	           const bool     scalable_in = false,
   	           const bool     growable_in = false);
    virtual ~StatsHistogramUniformAccum();
    virtual void reset();	// Put everything back to initial values

    //----Record a new value
    virtual void add(const double value);
    virtual void add(const double index_value, const double data_value);
    using StatsHistogramUniform::add;

    //----Histogram-wide access functions
    double value(unsigned index) const;	// value in a particular bin

    //----Value-access functions
    virtual void printRome(int depth, std::ostream &out) const;

    //----Sometimes we want another one of these, just like this one, but fresh.
    virtual Stats *another_new() const;
};

/// \brief Histogram with specified subranges and different histogram types for each range.
///
/// StatsHistogramGroup: a collection of histograms, allowing multiple
/// granularities of histograms over different ranges plus a low, high,
/// and overall stats.
///
/// The histograms may be all the same type, or of different types,
/// depending on the constructor method chosen. For example, a uniform histogram
/// around 0 and exponential above 100.
class StatsHistogramGroup : public Stats {
public:
    // ranges=[0,1,10,100] => low, hist[0..1[, hist[1..10[, hist[10,100[, high
    StatsHistogramGroup(const StatsHistogram::HistMode mode,
                        const int sub_hist_buckets,
                        const std::vector<double> &ranges);
    StatsHistogramGroup(const std::vector<StatsHistogram::HistMode> &_modes,
                        const std::vector<int> &sub_hist_buckets,
                        const std::vector<double> &_ranges);
    virtual ~StatsHistogramGroup();
    virtual void reset();	// Put everything back to initial values

    //----Record a new value
    virtual void add(const double value);
    virtual void add(const double index_value, const double data_value);
    virtual void add(const Stats &stats);

    const StatsHistogram &getHistogram(unsigned int n) const {
        INVARIANT(n < histograms.size(),
                  boost::format("Requested out of bounds histogram %d valid range [%d .. %d]")
                  % n % 0 % histograms.size());
        return *histograms[n];
    }

    const Stats &getLow() const { return low; }
    const Stats &getHigh() const { return high; }
    virtual void printRome(int depth, std::ostream &out) const;

private:
    std::vector<StatsHistogram *> histograms;
    const std::vector<StatsHistogram::HistMode> modes;
    const std::vector<double> ranges;
    Stats low,high;
};

#endif /* _LINTEL_HISTOGRAM_H_INCLUDED */


//
// Variable - Permutation integration tests
//
#include <boost/test/unit_test.hpp>

#include "Permutation.hpp"
#include "Variable.hpp"

using namespace mist;

BOOST_AUTO_TEST_CASE(Permute_Variable_direct)
{
  auto* data_0{ new Variable::data_t[3]{ 1, 2, 3 } };
  ;
  auto* data_1{ new Variable::data_t[3]{ 1, 3, 2 } };
  auto* data_2{ new Variable::data_t[3]{ 2, 1, 3 } };
  Variable::data_ptr p_0(data_0);
  Variable::data_ptr p_1(data_1);
  Variable::data_ptr p_2(data_2);

  Variable v0(p_0, 3, 0, 3);
  Variable v1(p_1, 3, 0, 3);
  Variable v2(p_2, 3, 0, 3);

  Variable v = v0.deepCopy();

  BOOST_ASSERT(v == v0);
  BOOST_TEST(permutation::next(v));
  BOOST_ASSERT(v == v1);
  BOOST_TEST(permutation::next(v));
  BOOST_ASSERT(v == v2);
}

BOOST_AUTO_TEST_CASE(Permute_Variable_copy)
{
  Permutation<int> perm = { 2, 1, 0 };
  auto* data_3{ new Variable::data_t[3]{ 1, 2, 3 } };
  ;
  auto* data_3_exp{ new Variable::data_t[3]{ 3, 2, 1 } };

  Variable::data_ptr p3(data_3);
  Variable::data_ptr p3_exp(data_3_exp);
  Variable original(p3, 3, 0, 3);
  Variable expected(p3_exp, 3, 0, 3);
  Variable permuted = original.deepCopy();

  perm.apply(original, permuted);
  BOOST_ASSERT(permuted == expected);
}


/******************************************************************************
 * Copyright (C) 2013 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 *                                                                            *
 * This program is free software; you can redistribute it and/or modify       *
 * it under the terms of the Lesser GNU General Public License as published by*
 * the Free Software Foundation; either version 3 of the License, or          *
 * (at your option) any later version.                                        *
 *                                                                            *
 * This program 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              *
 * Lesser GNU General Public License for more details.                        *
 *                                                                            *
 * You should have received a copy of the Lesser GNU General Public License   *
 * along with this program. If not, see <http://www.gnu.org/licenses/>.       *
 ******************************************************************************/

/** \file ErrorTermWheelTest.cpp
    \brief This file tests the ErrorTermWheel class.
  */

#include <cmath>

#include <boost/make_shared.hpp>

#include <gtest/gtest.h>

#include <aslam/backend/test/ErrorTermTestHarness.hpp>
#include <aslam/backend/EuclideanPoint.hpp>
#include <aslam/backend/EuclideanExpression.hpp>
#include <aslam/backend/Scalar.hpp>
#include <aslam/backend/ScalarExpression.hpp>

#include "aslam/calibration/error-terms/ErrorTermWheel.h"
#include "aslam/calibration/error-terms/ErrorTermPose.h"
#include "aslam/calibration/error-terms/ConditionalErrorTerm.h"

using namespace aslam::backend;
using namespace aslam::calibration;

TEST(AslamCalibrationTestSuite, testConditionalErrorTerm) {

  const Eigen::Matrix<double, 3, 1> v(1.5, 1.7, 2.8);
  auto v_dv = boost::make_shared<EuclideanPoint>(v);
  EuclideanExpression v_exp(v_dv);

  // radius
  auto r_dv = boost::make_shared<Scalar>(1.6);
  ScalarExpression r_exp(r_dv);

  // error term wheel
  ErrorTermWheel ew(v_exp, r_exp, 1.5, 1);

  auto ewS = boost::make_shared<ErrorTermWheel>(v_exp, r_exp, 1.5, 1);

  bool active = true;
  auto cew = addCondition(ew, [&](){ return active; });
  auto cewS = addConditionShared(*ewS, [&](){ return active; });

  ewS.reset();

  ew.updateRawSquaredError();
  cew.updateRawSquaredError();
  cewS->updateRawSquaredError();

  EXPECT_NE(0, ew.getSquaredError());
  EXPECT_TRUE(cew.isActive());
  EXPECT_TRUE(boost::dynamic_pointer_cast<ConditionalErrorTermBase>(cewS)->isActive());
  EXPECT_EQ(ew.getSquaredError(), cew.getSquaredError());
  EXPECT_EQ(ew.getSquaredError(), cewS->getSquaredError());

  // test the error terms
  try {
    ErrorTermTestHarness<2> harness(&cew);
    ErrorTermTestHarness<2> harness2(&(*cewS));
    harness.testAll(1e-5);
    harness2.testAll(1e-5);
  }
  catch (const std::exception& e) {
    FAIL() << e.what();
  }

  active = false;
  cew.updateRawSquaredError();
  EXPECT_FALSE(cew.isActive());
  EXPECT_EQ(0, cew.getSquaredError());
}

TEST(AslamCalibrationTestSuite, testConditionalErrorTermWithPose) {
  TransformationExpression T(Eigen::Matrix4d::Identity());

  ErrorTermGroupReference etgr("test");
  PoseMeasurement p;
  p.q = sm::kinematics::quatIdentity();
  p.t.setZero();

  auto ewS = boost::make_shared<ErrorTermPose>(T, p, Eigen::Matrix3d::Identity(), Eigen::Matrix3d::Identity(), etgr);

  bool active = true;
  auto cewS = addConditionShared(*ewS, [&](){ return active; });

  auto v = ewS->getSquaredError();
  ewS.reset();

  cewS->updateRawSquaredError();

  EXPECT_TRUE(boost::dynamic_pointer_cast<ConditionalErrorTermBase>(cewS)->isActive());
  EXPECT_EQ(v, cewS->getSquaredError());
}


#include <catch2/catch.hpp>

#include <Eigen/Core>

#include <igl/IO>
#include <igl/edges.h>
#include <igl/Timer.h>

#include <ipc/ipc.hpp>
#include <ipc/broad_phase/hash_grid.hpp>
#include <ipc/broad_phase/spatial_hash.hpp>
#include <ipc/ccd/ccd.hpp>
#include <ipc/utils/faces_to_edges.hpp>

#include <broad_phase/brute_force_comparison.hpp>

using namespace ipc;

TEST_CASE("Vertex-Vertex Broad Phase", "[ccd][broad_phase]")
{
    Eigen::MatrixXd V_t0(4, 2);
    V_t0.row(0) << 1.11111, 0.5;  // edge 0 vertex 0
    V_t0.row(1) << 1.11111, 0.75; // edge 0 vertex 1
    V_t0.row(2) << 1, 0.5;        // edge 1 vertex 0
    V_t0.row(3) << 1, 0.75;       // edge 1 vertex 1

    Eigen::MatrixXd V_t1 = V_t0;
    V_t1.row(0) << 0.888889, 0.5;  // edge 0 vertex 0
    V_t1.row(1) << 0.888889, 0.75; // edge 0 vertex 1

    Eigen::MatrixXi E(2, 2);
    E.row(0) << 1, 0;
    E.row(1) << 2, 3;

    bool ignore_internal_vertices = GENERATE(false, true);

    BroadPhaseMethod method = GENERATE(
        BroadPhaseMethod::BRUTE_FORCE, BroadPhaseMethod::HASH_GRID,
        BroadPhaseMethod::SPATIAL_HASH);

    if (method == BroadPhaseMethod::SPATIAL_HASH && ignore_internal_vertices) {
        return; // Not implemented
    }

    double tolerance = 1e-6;
    int max_iterations = 1e7;

    bool is_valid_step;
    if (ignore_internal_vertices) {
        is_valid_step = ipc::is_step_collision_free(
            V_t0, V_t1, Eigen::VectorXi(), E, /*F=*/Eigen::MatrixXi(), method,
            tolerance, max_iterations);
    } else {
        is_valid_step = ipc::is_step_collision_free(
            V_t0, V_t1, E, /*F=*/Eigen::MatrixXi(), method, tolerance,
            max_iterations);
    }

    CAPTURE(ignore_internal_vertices);
    CHECK(!is_valid_step);
}

TEST_CASE("Entire 2D Mesh", "[ccd][broad_phase]")
{
    Eigen::MatrixXd V_t0;
    igl::readCSV(std::string(TEST_DATA_DIR) + "V_t0.csv", V_t0);

    Eigen::MatrixXd V_t1;
    igl::readCSV(std::string(TEST_DATA_DIR) + "V_t1.csv", V_t1);

    Eigen::MatrixXi E;
    igl::readCSV(std::string(TEST_DATA_DIR) + "E.csv", E);
    E.array() -= 1; // NOTE: Convert from OBJ format to index

    Eigen::VectorXi codim_V;
    Eigen::MatrixXi F;

    BroadPhaseMethod method = GENERATE(
        BroadPhaseMethod::BRUTE_FORCE, BroadPhaseMethod::HASH_GRID
        // , BroadPhaseMethod::SPATIAL_HASH
    );

    double tolerance = 1e-6;
    int max_iterations = 1e7;

    bool is_valid_step;
    SECTION("2D")
    {
        is_valid_step = ipc::is_step_collision_free(
            V_t0.leftCols(2), V_t1.leftCols(2), codim_V, E, F, method,
            tolerance, max_iterations);
    }
    SECTION("3D")
    {
        is_valid_step = ipc::is_step_collision_free(
            V_t0, V_t1, codim_V, E, F, method, tolerance, max_iterations);
    }

    CAPTURE(method);
    CHECK(!is_valid_step);
}

TEST_CASE(
    "Test construct_constraint_set() with codimensional points",
    "[construct_constraint_set][broad_phase]")
{
    const double dhat = 1e-3;
    Eigen::MatrixXd V_rest, V;
    igl::readDMAT(
        std::string(TEST_DATA_DIR) + "codim-points/V_rest.dmat", V_rest);
    igl::readDMAT(std::string(TEST_DATA_DIR) + "codim-points/V.dmat", V);
    Eigen::MatrixXi E, F;
    igl::readDMAT(std::string(TEST_DATA_DIR) + "codim-points/E.dmat", E);
    igl::readDMAT(std::string(TEST_DATA_DIR) + "codim-points/F.dmat", F);
    const Eigen::MatrixXi F2E = faces_to_edges(F, E);

#ifdef NDEBUG
    BroadPhaseMethod method = GENERATE(
        BroadPhaseMethod::BRUTE_FORCE, BroadPhaseMethod::HASH_GRID //,
        // BroadPhaseMethod::SPATIAL_HASH
    );
#else
    BroadPhaseMethod method =
        GENERATE(BroadPhaseMethod::HASH_GRID //,
                                             // BroadPhaseMethod::SPATIAL_HASH
        );
#endif

    Constraints constraint_set;
    construct_constraint_set(
        V_rest, V, E, F, dhat, constraint_set, F2E, /*dmin=*/0, method);

    CHECK(constraint_set.size() != 0);
}

TEST_CASE(
    "Compare HashGrid/SpatialHash against brute force",
    "[hash_grid][spatial_hash][brute_force]")
{
    using namespace ipc;

    Eigen::MatrixXd V0, U;
    Eigen::MatrixXi E, F;
    Eigen::VectorXi group_ids;

    SECTION("Simple")
    {
        V0.resize(4, 3);
        V0.row(0) << -1, -1, 0;
        V0.row(1) << 1, -1, 0;
        V0.row(2) << 0, 1, 1;
        V0.row(3) << 0, 1, -1;

        E.resize(2, 2);
        E.row(0) << 0, 1;
        E.row(1) << 2, 3;

        SECTION("Without group ids") { }
        SECTION("With group ids")
        {
            group_ids.resize(4);
            group_ids << 0, 0, 1, 1;
        }

        F.resize(0, 3);

        U = Eigen::MatrixXd::Zero(V0.rows(), V0.cols());
        U.col(1).head(2).setConstant(2);
        U.col(1).tail(2).setConstant(-2);
    }
    SECTION("Complex")
    {
        // #ifdef NDEBUG
        //         std::string filename =
        //             GENERATE(std::string("cube.obj"),
        //             std::string("bunny.obj"));
        // #else
        std::string filename = "cube.obj";
        // #endif
        std::string mesh_path = std::string(TEST_DATA_DIR) + filename;
        bool success = igl::read_triangle_mesh(mesh_path, V0, F);
        REQUIRE(success);
        igl::edges(F, E);

        U = Eigen::MatrixXd::Zero(V0.rows(), V0.cols());
        U.col(1).setOnes();
    }

    Candidates candidates;

    // int check_hash_grid = GENERATE(0, 1);
    int check_hash_grid = 1;

    double inflation_radius = 1e-2; // GENERATE(0.0, 1e-4, 1e-3, 1e-2, 1e-1);

    auto can_collide = [&group_ids](size_t vi, size_t vj) {
        return group_ids.size() == 0 || group_ids(vi) != group_ids(vj);
    };

    std::function<void(const Eigen::MatrixXd&)> build_candidates;
    if (check_hash_grid) {
        build_candidates = [&](const Eigen::MatrixXd& V1) {
            candidates.clear();

            HashGrid hashgrid;
            hashgrid.resize(V0, V1, E, inflation_radius);
            hashgrid.addVertices(V0, V1, inflation_radius);
            hashgrid.addEdges(V0, V1, E, inflation_radius);
            hashgrid.addFaces(V0, V1, F, inflation_radius);

            hashgrid.getVertexEdgePairs(
                E, candidates.ev_candidates, can_collide);
            hashgrid.getEdgeEdgePairs(E, candidates.ee_candidates, can_collide);
            hashgrid.getFaceVertexPairs(
                F, candidates.fv_candidates, can_collide);
        };
    } else {
        build_candidates = [&](const Eigen::MatrixXd& V1) {
            candidates.clear();

            SpatialHash sh;
            sh.build(V0, V1, E, F, inflation_radius);
            // TODO: use can_collide
            group_ids.resize(0);
            sh.queryMeshForCandidates(
                V0, V1, E, F, candidates,
                /*queryEV=*/true, /*queryEE=*/true, /*queryFV=*/true);
        };
    }

    for (int i = 0; i < 2; i++) {
        Eigen::MatrixXd V1 = V0 + U;

        build_candidates(V1);

        brute_force_comparison(
            V0, V1, E, F, group_ids, candidates, inflation_radius);

        U.setRandom();
        U *= 3;
    }
}


#include "popcount_boost.h"
#include <boost/type_traits.hpp>
#include <limits>
#include <stdexcept>
#include <type_traits>

namespace py_cpp_sample {
/**
 * @tparam SourceType The type of xs elements
 * @param[in] xs An integer array
 * @return The number of 1's of each element in xs
 */
template <typename SourceType>
boost::python::numpy::ndarray
popcount_cpp_impl_boost(const boost::python::numpy::ndarray &xs) {
    // Assuming NumPy arrays have C-like dense memory layout
    if (xs.strides(0) != sizeof(SourceType)) {
        throw std::runtime_error("Unexpected array layout");
    }

    auto size = xs.shape(0);
    const boost::python::tuple shape = boost::python::make_tuple(size);
    const boost::python::numpy::dtype data_type =
        boost::python::numpy::dtype::get_builtin<Count>();
    auto counts = boost::python::numpy::zeros(shape, data_type);
    if (counts.strides(0) != sizeof(Count)) {
        throw std::runtime_error("Unexpected array layout");
    }

    const SourceType *src = reinterpret_cast<const SourceType *>(xs.get_data());
    Count *dst = reinterpret_cast<Count *>(counts.get_data());
    for (decltype(size) i{0}; i < size; ++i) {
        const auto value = src[i];
#ifdef __GNUC__
        using PopcountArg = unsigned long long;
        static_assert(std::is_unsigned<SourceType>::value, "Must be unsigned");
        static_assert(std::is_unsigned<PopcountArg>::value, "Must be unsigned");
        static_assert(std::numeric_limits<SourceType>::max() <=
                          std::numeric_limits<PopcountArg>::max(),
                      "Avoid narrowing casting");
#ifndef __clang__
        static_assert(
            std::is_same<PopcountArg,
                         boost::function_traits<decltype(
                             __builtin_popcountll)>::arg1_type>::value,
            "Pass the correct argument to __builtin_popcountll");
#endif
        const auto count = static_cast<Count>(__builtin_popcountll(value));
#else
#error Use an alternative of __builtin_popcountll
#endif
        dst[i] = count;
    }
    return counts;
}

boost::python::numpy::ndarray
popcount_cpp_boost(const boost::python::numpy::ndarray &xs) {
    if (xs.get_nd() != 1) {
        throw std::runtime_error("xs must be a 1-D uint array");
    }

    if (xs.get_dtype() == boost::python::numpy::dtype::get_builtin<uint8_t>()) {
        return popcount_cpp_impl_boost<uint8_t>(xs);
    } else if (xs.get_dtype() ==
               boost::python::numpy::dtype::get_builtin<uint64_t>()) {
        return popcount_cpp_impl_boost<uint64_t>(xs);
    }

    throw std::runtime_error("Unsupported array element types");
}
} // namespace py_cpp_sample


#include <algorithm>
#include <queue>
#include <vector>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/property_map/property_map.hpp>
#include <CGAL/boost/graph/iterator.h>
#include <CGAL/boost/graph/properties.h>

namespace Euclid
{

namespace _impl
{

template<typename Mesh>
std::vector<vertex_t<Mesh>> mark_shortest_path(
    const Mesh& mesh,
    vertex_t<Mesh> src,
    std::vector<bool>& on_shortest_path_tree)
{
    auto vimap = get(boost::vertex_index, mesh);
    auto eimap = get(boost::edge_index, mesh);
    std::vector<vertex_t<Mesh>> predecessors(num_vertices(mesh));
    boost::dijkstra_shortest_paths(
        mesh,
        src,
        boost::predecessor_map(
            boost::make_iterator_property_map(predecessors.begin(), vimap)));

    for (auto v : vertices(mesh)) {
        if (v != src) {
            auto [h, exist] = halfedge(v, predecessors[get(vimap, v)], mesh);
            assert(exist == true);
            auto e = edge(h, mesh);
            on_shortest_path_tree[get(eimap, e)] = true;
        }
    }

    return predecessors;
}

template<typename Mesh>
void spanning_tree_on_dual_graph_not_cross_shortest_path_tree(
    const Mesh& mesh,
    const std::vector<bool>& on_shortest_path_tree,
    std::vector<bool>& cross_spanning_tree)
{
    using FD = face_t<Mesh>;
    auto fimap = get(CGAL::face_index, mesh);
    auto eimap = get(CGAL::edge_index, mesh);

    auto frange = faces(mesh);
    auto froot = *frange.first;
    std::queue<FD> q;
    q.push(froot);
    std::vector<bool> touched(num_faces(mesh), false);
    touched[get(fimap, froot)] = true;

    while (!q.empty()) {
        auto f = q.front();
        q.pop();
        for (auto h : CGAL::halfedges_around_face(halfedge(f, mesh), mesh)) {
            auto e = edge(h, mesh);
            auto eidx = get(eimap, e);
            if (!on_shortest_path_tree[eidx]) {
                auto ho = opposite(h, mesh);
                auto fo = face(ho, mesh);
                auto fidx = get(fimap, fo);
                if (!touched[fidx]) {
                    touched[fidx] = true;
                    q.push(fo);
                    cross_spanning_tree[eidx] = true;
                }
            }
        }
    }
}

template<typename Mesh>
VertexChain<Mesh> find_generator(
    const Mesh& mesh,
    edge_t<Mesh> e,
    vertex_t<Mesh> src,
    const std::vector<vertex_t<Mesh>>& predecessors)
{
    auto vimap = get(boost::vertex_index, mesh);
    auto h = halfedge(e, mesh);
    auto v1 = source(h, mesh);
    auto v2 = target(h, mesh);

    VertexChain<Mesh> generator;
    while (v1 != src) {
        generator.push_back(v1);
        v1 = predecessors[get(vimap, v1)];
    }
    generator.push_back(src);
    std::reverse(generator.begin(), generator.end());
    while (v2 != src) {
        generator.push_back(v2);
        v2 = predecessors[get(vimap, v2)];
    }
    return generator;
}

template<typename Mesh>
VertexChains<Mesh> find_generators(
    const Mesh& mesh,
    vertex_t<Mesh> src,
    const std::vector<bool>& on_shortest_path_tree,
    const std ::vector<bool>& cross_spanning_tree,
    const std ::vector<vertex_t<Mesh>>& predecessors)
{
    auto eimap = get(boost::edge_index, mesh);

    // find edges not on shortest path tree and crossing spanning tree
    std::vector<edge_t<Mesh>> left_edges;
    for (auto e : edges(mesh)) {
        auto eidx = get(eimap, e);
        if (!on_shortest_path_tree[eidx] && !cross_spanning_tree[eidx]) {
            left_edges.push_back(e);
        }
    }

    // find generators from left edges, should be 2G generators
    VertexChains<Mesh> generators;
    for (auto e : left_edges) {
        generators.push_back(find_generator(mesh, e, src, predecessors));
    }

    return generators;
}

} // namespace _impl

template<typename Mesh>
VertexChains<Mesh> greedy_homotopy_generators(const Mesh& mesh,
                                              vertex_t<Mesh> v)
{
    std::vector<bool> on_shortest_path_tree(num_edges(mesh), false);
    auto predecessors =
        _impl::mark_shortest_path(mesh, v, on_shortest_path_tree);

    std::vector<bool> cross_spanning_tree(num_edges(mesh), false);
    _impl::spanning_tree_on_dual_graph_not_cross_shortest_path_tree(
        mesh, on_shortest_path_tree, cross_spanning_tree);

    auto generators = _impl::find_generators(
        mesh, v, on_shortest_path_tree, cross_spanning_tree, predecessors);

    return generators;
}

} // namespace Euclid


//TODO: copy&paste code! write a base class!
#include "gp_deg.h"

#include "mex.h"
#include <math.h>
#include <string.h>
#include <Eigen/Dense>
#include <iostream>

//extern int dpotrs_(char *, long *, long *, double *, long *, double *, long *, long *);

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
	size_t M;
	size_t D;
	size_t n;
	size_t p;
	if (nrhs != 4 || nlhs < 2) /* check the input */
		mexErrMsgTxt("Usage: [alpha, L, nlZ, dnlZ] = infSolinmex(M, unwrap(hyp), x, y)");
	M = (size_t) mxGetScalar(prhs[0]);
	n = mxGetM(prhs[2]);
	D = mxGetN(prhs[2]);
	std::cout << "input dimensionality: " << D << std::endl;
	std::cout << "number of basis function: " << M << std::endl;
	std::cout << "number of points: " << n << std::endl;
	p = mxGetM(prhs[1]);
	libgp::DegGaussianProcess gp(D, "CovSum ( CovSEard, CovNoise)", M,
			"Solin");
	Eigen::VectorXd params = Eigen::Map<const Eigen::VectorXd>(mxGetPr(prhs[1]),
			p);
//	std::cout << "bf_multi_scale: params" << std::endl << params << std::endl;

	gp.covf().set_loghyper(params);

	Eigen::VectorXd y = Eigen::Map<const Eigen::VectorXd>(mxGetPr(prhs[3]), n);
	Eigen::MatrixXd X = Eigen::Map<const Eigen::MatrixXd>(mxGetPr(prhs[2]), n,
			D);

	for (size_t i=0; i < n; i++) {
		gp.add_pattern(X.row(i), y(i));
	}

	plhs[0] = mxCreateDoubleMatrix(M, 1, mxREAL); /* allocate space for output */
	Eigen::Map<Eigen::VectorXd>(mxGetPr(plhs[0]), M) = gp.getAlpha();
	plhs[1] = mxCreateDoubleMatrix(M, M, mxREAL);
	Eigen::Map<Eigen::MatrixXd>(mxGetPr(plhs[1]), M, M) = gp.getL();
	if(nlhs >= 3){
		double nlZ = gp.log_likelihood();
		plhs[2] = mxCreateDoubleScalar(nlZ);
		if(nlhs >= 4){
			Eigen::VectorXd grads = gp.log_likelihood_gradient();
			size_t params = grads.size();
			plhs[3] = mxCreateDoubleMatrix(params, 1, mxREAL); /* allocate space for output */
			Eigen::Map<Eigen::VectorXd>(mxGetPr(plhs[3]), params) = grads;
		}
	}
}


#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include "opencv2/imgproc.hpp"
#include "opencv2/imgcodecs.hpp"
#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include<nav_msgs/Odometry.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <stdio.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#define ture 1
#define false 0
// #include "extra.h" // use this if in OpenCV2
using namespace std;
using namespace cv;
Mat frame0,frame1;
/****************************************************
 * 本程序演示了如何使用2D-2D的特征匹配估计相机运动
 * **************************************************/

void find_feature_matches (
    const Mat& img_1, const Mat& img_2,
    std::vector<KeyPoint>& keypoints_1,
    std::vector<KeyPoint>& keypoints_2,
    std::vector< DMatch >& matches );

void pose_estimation_2d2d (
    std::vector<KeyPoint> keypoints_1,
    std::vector<KeyPoint> keypoints_2,
    std::vector< DMatch > matches,
    Mat& R, Mat& t );

// 像素坐标转相机归一化坐标
Point2d pixel2cam ( const Point2d& p, const Mat& K );

void imageCallback0(const sensor_msgs::ImageConstPtr& msg)
{

  cv_bridge::CvImagePtr cv_ptr;
  try
  {
     cv_ptr = cv_bridge::toCvCopy(msg, "mono8");
  }
  catch (cv_bridge::Exception& e)
  {
     ROS_ERROR("cv_bridge exception: %s", e.what());
     return;
  }

  // cv::Mat caml;
  frame0 = cv_ptr->image;
//imshow("1",caml);
  // fail if don't have waitKey(3).
 // cv::waitKey(3);
}
void imageCallback1(const sensor_msgs::ImageConstPtr& msg)
{

  cv_bridge::CvImagePtr cv_ptr;
  try
  {
     cv_ptr = cv_bridge::toCvCopy(msg, "mono8");
  }
  catch (cv_bridge::Exception& e)
  {
     ROS_ERROR("cv_bridge exception: %s", e.what());
     return;
  }

  // cv::Mat caml;
  frame1 = cv_ptr->image;
//imshow("1",caml);
  // fail if don't have waitKey(3).
 // cv::waitKey(3);
}

int main ( int argc, char** argv )
{
    // if ( argc != 3 )
    // {
    //     cout<<"usage: pose_estimation_2d2d img1 img2"<<endl;
    //     return 1;
    // }
    //-- 读取图像
    ros::init(argc, argv, "pose");
    ros::NodeHandle nh;
    image_transport::ImageTransport it(nh);
   image_transport::Subscriber sub0 = it.subscribe("camera/fisheye0", 1, imageCallback0);
   image_transport::Subscriber sub1 = it.subscribe("camera/fisheye1", 1, imageCallback1);
    ros::Publisher path_pub = nh.advertise<nav_msgs::Path>("trajectory",1, true);
    ros::Publisher odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 50);
    ros::Time current_time, last_time;
    current_time = ros::Time::now();
    last_time = ros::Time::now();

    nav_msgs::Path path;
    //nav_msgs::Path path;
    path.header.stamp=current_time;
    path.header.frame_id="odom";
 ros::Rate loop_rate(10);

 while (nh.ok())
 {

    // Mat img_1 = imread ( "/home/fsdh/桌面/slambook-master/ch7/1.png", CV_LOAD_IMAGE_COLOR );
    // Mat img_2 = imread ( "/home/fsdh/桌面/slambook-master/ch7/2.png", CV_LOAD_IMAGE_COLOR );
   if(!frame0.empty()&!frame1.empty()){
    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;
    find_feature_matches ( frame0, frame1, keypoints_1, keypoints_2, matches );
    cout<<"一共找到了"<<matches.size() <<"组匹配点"<<endl;
     current_time = ros::Time::now();
    //-- 估计两张图像间运动
    Mat R,t;
    pose_estimation_2d2d ( keypoints_1, keypoints_2, matches, R, t );

     Eigen::Matrix3d ER;
        ER<<R.at<double> ( 0,0 ),R.at<double> ( 0,1 ),R.at<double> ( 0,2 ),R.at<double> ( 1,0 ),R.at<double> ( 1,1 ),R.at<double> ( 1,2 ),R.at<double> ( 2,0 ),R.at<double> ( 2,1 ),R.at<double> ( 2,2 );
         Eigen::Quaterniond q = Eigen::Quaterniond(ER);
    // geometry_msgs::PoseStamped this_pose_stamped;
    //     this_pose_stamped.pose.position.x =t.at<double> ( 0,0 );
    //     this_pose_stamped.pose.position.y = 0;
    // this_pose_stamped.pose.position.z =0;

    //     this_pose_stamped.pose.orientation.x=q.x();
    //     this_pose_stamped.pose.orientation.y=q.y();
    //     this_pose_stamped.pose.orientation.z=q.z();
    //     this_pose_stamped.pose.orientation.w=q.w();

    //     this_pose_stamped.header.stamp=current_time;
    //     this_pose_stamped.header.frame_id="odom";
    //     path.poses.push_back(this_pose_stamped);

   nav_msgs::Odometry odom;
    odom.header.stamp = current_time;
    odom.header.frame_id = "odom";

    //set the position
    odom.pose.pose.position.x = 0;
    odom.pose.pose.position.y = 0;
    odom.pose.pose.position.z = 0.0;
    odom.pose.pose.orientation.x =q.x();
    odom.pose.pose.orientation.y =q.y();
    odom.pose.pose.orientation.z =q.z();
    odom.pose.pose.orientation.w =q.w();
    //set the velocity
    odom.child_frame_id = "base_link";
    odom.twist.twist.linear.x = 0;
    odom.twist.twist.linear.y = 0;
    odom.twist.twist.angular.z = 0;
        //path_pub.publish(path);

    odom_pub.publish(odom);
     //-- 验证E=t^R*scale
    // Mat t_x = ( Mat_<double> ( 3,3 ) <<
    //             0,                      -t.at<double> ( 2,0 ),     t.at<double> ( 1,0 ),
    //             t.at<double> ( 2,0 ),      0,                      -t.at<double> ( 0,0 ),
    //             -t.at<double> ( 1,0 ),     t.at<double> ( 0,0 ),      0 );

    // cout<<"t^R="<<endl<<t_x*R<<endl;

    // //-- 验证对极约束
    // Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
    // for ( DMatch m: matches )
    // {
    //     Point2d pt1 = pixel2cam ( keypoints_1[ m.queryIdx ].pt, K );
    //     Mat y1 = ( Mat_<double> ( 3,1 ) << pt1.x, pt1.y, 1 );
    //     Point2d pt2 = pixel2cam ( keypoints_2[ m.trainIdx ].pt, K );
    //     Mat y2 = ( Mat_<double> ( 3,1 ) << pt2.x, pt2.y, 1 );
    //     Mat d = y2.t() * t_x * R * y1;
    //     cout << "epipolar constraint = " << d << endl;
    // }
   }
   ros::spinOnce();
   last_time = current_time;
     loop_rate.sleep();   /* code for loop body */
 }

    return 0;
}

void find_feature_matches ( const Mat& img_1, const Mat& img_2,
                            std::vector<KeyPoint>& keypoints_1,
                            std::vector<KeyPoint>& keypoints_2,
                            std::vector< DMatch >& matches )
{
    //-- 初始化
    Mat descriptors_1, descriptors_2;
    // used in OpenCV3
    Ptr<FeatureDetector> detector = ORB::create();
    Ptr<DescriptorExtractor> descriptor = ORB::create();
    // use this if you are in OpenCV2
    // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
    // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
    Ptr<DescriptorMatcher> matcher  = DescriptorMatcher::create ( "BruteForce-Hamming" );
    //-- 第一步:检测 Oriented FAST 角点位置
    detector->detect ( img_1,keypoints_1 );
    detector->detect ( img_2,keypoints_2 );

    //-- 第二步:根据角点位置计算 BRIEF 描述子
    descriptor->compute ( img_1, keypoints_1, descriptors_1 );
    descriptor->compute ( img_2, keypoints_2, descriptors_2 );

    //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    vector<DMatch> match;
    //BFMatcher matcher ( NORM_HAMMING );
    matcher->match ( descriptors_1, descriptors_2, match );

    //-- 第四步:匹配点对筛选
    double min_dist=10000, max_dist=0;

    //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        double dist = match[i].distance;
        if ( dist < min_dist ) min_dist = dist;
        if ( dist > max_dist ) max_dist = dist;
    }

    printf ( "-- Max dist : %f \n", max_dist );
    printf ( "-- Min dist : %f \n", min_dist );

    //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        if ( match[i].distance <= max ( 2*min_dist, 30.0 ) )
        {
            matches.push_back ( match[i] );
        }
    }
     Mat img_goodmatch;

    drawMatches ( img_1, keypoints_1, img_2, keypoints_2, matches, img_goodmatch );

    imshow ( "优化后匹配点对", img_goodmatch );
    cv::waitKey(3);
}

Point2d pixel2cam ( const Point2d& p, const Mat& K )
{
    return Point2d
           (
               ( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ),
               ( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 )
           );
}

void pose_estimation_2d2d ( std::vector<KeyPoint> keypoints_1,
                            std::vector<KeyPoint> keypoints_2,
                            std::vector< DMatch > matches,
                            Mat& R, Mat& t )
{
    // 相机内参,TUM Freiburg2
    Mat K = ( Mat_<double> ( 3,3 ) << 259.8074, 0, 328.0466, 0, 261.0161, 236.2571, 0, 0, 1 );

    //-- 把匹配点转换为vector<Point2f>的形式
    vector<Point2f> points1;
    vector<Point2f> points2;

    for ( int i = 0; i < ( int ) matches.size(); i++ )
    {
        points1.push_back ( keypoints_1[matches[i].queryIdx].pt );
        points2.push_back ( keypoints_2[matches[i].trainIdx].pt );
    }

    //-- 计算基础矩阵
    Mat fundamental_matrix;
    fundamental_matrix = findFundamentalMat ( points1, points2, CV_FM_8POINT );
   // cout<<"fundamental_matrix is "<<endl<< fundamental_matrix<<endl;

    //-- 计算本质矩阵
    Point2d principal_point ( 325.1, 249.7 );	//相机光心, TUM dataset标定值
    double focal_length = 521;			//相机焦距, TUM dataset标定值
    Mat essential_matrix;
    essential_matrix = findEssentialMat ( points1, points2, focal_length, principal_point );
   // cout<<"essential_matrix is "<<endl<< essential_matrix<<endl;

    //-- 计算单应矩阵
    Mat homography_matrix;
    homography_matrix = findHomography ( points1, points2, RANSAC, 3 );
  //  cout<<"homography_matrix is "<<endl<<homography_matrix<<endl;

    //-- 从本质矩阵中恢复旋转和平移信息.
    recoverPose ( essential_matrix, points1, points2, R, t, focal_length, principal_point );
    // Eigen::Matrix3d ER;
    // ER<<R.at<double> ( 0,0 ),R.at<double> ( 0,1 ),R.at<double> ( 0,2 ),R.at<double> ( 1,0 ),R.at<double> ( 1,1 ),R.at<double> ( 1,2 ),R.at<double> ( 2,0 ),R.at<double> ( 2,1 ),R.at<double> ( 2,2 );
    //  Eigen::Quaterniond q = Eigen::Quaterniond(ER);
    cout<<"R is "<<endl<<R<<endl;
    cout<<"t is "<<endl<<t<<endl;
    // cout<<"ER is "<<endl<<q.coeffs()<<endl;

}


#include "domain/mesh/mesh_cartesian.hpp"

#include <sstream>
#include <algorithm>
#include <cstdlib>
#include <numeric>
#include <functional>
#include <vector>

#include <deal.II/base/point.h>
#include <deal.II/grid/tria.h>
#include <problem/parameter_types.hpp>

#include "test_helpers/test_helper_functions.h"
#include "test_helpers/gmock_wrapper.h"

namespace  {

using namespace bart;

template <typename DimensionWrapper>
class DomainMeshCartesianTest : public ::testing::Test {
 protected:
  static constexpr int dim = DimensionWrapper::value;
};

TYPED_TEST_CASE(DomainMeshCartesianTest, bart::testing::AllDimensions);

TYPED_TEST(DomainMeshCartesianTest, DescriptionTest) {
  constexpr int dim = this->dim;

  std::vector<double> spatial_max{test_helpers::RandomVector(dim, 0, 100)};
  std::vector<double> n_cells_double{test_helpers::RandomVector(dim, 1, 20)};
  std::vector<int> n_cells{n_cells_double.begin(), n_cells_double.end()};

  domain::mesh::MeshCartesian<dim> test_mesh(spatial_max, n_cells);

  std::string expected_description = "(default) deal.II Cartesian Mesh, " + std::to_string(dim) + "D, Size: {";

  auto int_comma_fold = [](std::string a, int b) { return std::move(a) + ", " + std::to_string(b); };
  auto double_comma_fold = [](std::string a, double b) { return std::move(a) + ", " + std::to_string(b); };

  std::string size_string = std::accumulate(std::next(spatial_max.begin()), spatial_max.end(),
                                            std::to_string(spatial_max.at(0)), double_comma_fold);
  std::string n_cells_string = std::accumulate(std::next(n_cells.begin()), n_cells.end(),
                                               std::to_string(n_cells.at(0)), int_comma_fold);
  expected_description += size_string + "}, N_cells: {" + n_cells_string + "}";

  EXPECT_EQ(expected_description, test_mesh.description());
}

TYPED_TEST(DomainMeshCartesianTest, FillTriangulationTest) {
  constexpr int dim = this->dim;

  std::vector<double> spatial_max{test_helpers::RandomVector(dim, 0, 100)};
  std::vector<double> n_cells_double{test_helpers::RandomVector(dim, 1, 20)};
  std::vector<int> n_cells{n_cells_double.begin(), n_cells_double.end()};

  int n_total_cells = std::accumulate(n_cells.begin(), n_cells.end(), 1, std::multiplies<int>());

  domain::mesh::MeshCartesian<dim> test_mesh(spatial_max, n_cells);
  dealii::Triangulation<dim> test_triangulation;

  for (int i = 0; i < dim; ++i) {
    EXPECT_EQ(test_mesh.spatial_max().at(i), spatial_max.at(i));
    EXPECT_EQ(test_mesh.n_cells().at(i), n_cells.at(i));
  }

  test_mesh.FillTriangulation(test_triangulation);
  EXPECT_EQ(test_triangulation.n_cells(), n_total_cells);
  EXPECT_FALSE(test_mesh.has_material_mapping());
  for (auto const &cell : test_triangulation.active_cell_iterators()) {
    for (int i = 0; i < dim; ++i) {
      EXPECT_THAT(cell->extent_in_direction(i), ::testing::DoubleNear(spatial_max[i]/n_cells[i], 1e-10));
    }
  }
}

TYPED_TEST(DomainMeshCartesianTest, FillBoundaryIDTest) {
  constexpr int dim = this->dim;
  using Boundary = problem::Boundary;

  std::vector<double> spatial_max(dim, 10.0);
  std::vector<int> n_cells(dim, 2);

  const double zero_tol = 1.0e-14;

  domain::mesh::MeshCartesian<dim> test_mesh(spatial_max, n_cells);
  dealii::Triangulation<dim> test_triangulation;

  test_mesh.FillTriangulation(test_triangulation);
  test_mesh.FillBoundaryID(test_triangulation);

  int faces_per_cell = dealii::GeometryInfo<dim>::faces_per_cell;

  for (auto const &cell : test_triangulation.active_cell_iterators()) {
    if (cell->is_locally_owned()) {
      for (int face_id = 0; face_id < faces_per_cell; ++face_id) {
        auto face = cell->face(face_id);
        dealii::Point<dim> face_center = face->center();
        switch (dim) {
          case 3: {
            if (std::fabs(face_center[2]) < zero_tol) {
              EXPECT_EQ(static_cast<Boundary>(face->boundary_id()),Boundary::kZMin);
              break;
            } else if (std::fabs(face_center[2] - spatial_max.at(2)) < zero_tol) {
              EXPECT_EQ(static_cast<Boundary>(face->boundary_id()), Boundary::kZMax);
              break;
            }
            [[fallthrough]];
          }
          case 2: {
            if (std::fabs(face_center[1]) < zero_tol) {
              EXPECT_EQ(static_cast<Boundary>(face->boundary_id()), Boundary::kYMin);
              break;
            } else if (std::fabs(face_center[1] - spatial_max.at(1)) < zero_tol) {
              EXPECT_EQ(static_cast<Boundary>(face->boundary_id()), Boundary::kYMax);
              break;
            }
            [[fallthrough]];
          }
            // Fall through to check x-direction
          case 1: {
            if (std::fabs(face_center[0]) < zero_tol) {
              EXPECT_EQ(static_cast<Boundary>(face->boundary_id()), Boundary::kXMin);
              break;
            } else if (std::fabs(face_center[0] - spatial_max.at(0)) < zero_tol) {
              EXPECT_EQ(static_cast<Boundary>(face->boundary_id()), Boundary::kXMax);
              break;
            }
            break;
          }
          default: {
            AssertThrow(false,
                        dealii::ExcMessage("Unsupported number of dimensions in FillBoundaryID"));
          }
        }
      }
    }
  }

}

TYPED_TEST(DomainMeshCartesianTest, SpatialSizeWrongDimension) {
  constexpr int dim = this->dim;

  std::vector<std::vector<double>> spatial_maxes{ {} };
  std::vector<std::vector<int>> n_cells{ {} };
  for (int dimension = 1; dimension <= 3; ++dimension) {
    spatial_maxes.emplace_back(test_helpers::RandomVector(dimension, 0, 100));
    const auto cells_int_vector{test_helpers::RandomVector(dimension, 10, 20)};
    n_cells.emplace_back(cells_int_vector.cbegin(), cells_int_vector.cend());
  }

  for (int i = 0; i <= 3; ++i) {
    if (i != dim) {
      EXPECT_ANY_THROW({domain::mesh::MeshCartesian<dim> test_mesh(spatial_maxes.at(i), n_cells.at(dim));});
      EXPECT_ANY_THROW({domain::mesh::MeshCartesian<dim> test_mesh(spatial_maxes.at(dim), n_cells.at(i));});
    }
  }
}

TYPED_TEST(DomainMeshCartesianTest, ZeroSpatialSize) {
  constexpr int dim = this->dim;
  const std::vector<double> spatial_size{ test_helpers::RandomVector(dim, 0, 100) };
  const std::vector<double> n_cells_double_vector { test_helpers::RandomVector(dim, 10, 20) };
  const std::vector<int> n_cells(n_cells_double_vector.cbegin(), n_cells_double_vector.cend());

  std::vector<double> bad_spatial_size{ spatial_size };
  bad_spatial_size.at(test_helpers::RandomInt(0, dim)) = 0;
  EXPECT_ANY_THROW({domain::mesh::MeshCartesian<dim> test_mesh(bad_spatial_size, n_cells);});
}

TYPED_TEST(DomainMeshCartesianTest, NCellsNegativeOrZeroValue) {
  constexpr int dim = this->dim;
  const std::vector<double> spatial_size{ test_helpers::RandomVector(dim, 0, 100) };
  const std::vector<double> n_cells_double_vector { test_helpers::RandomVector(dim, 10, 20) };
  const std::vector<int> n_cells(n_cells_double_vector.cbegin(), n_cells_double_vector.cend());

  std::vector<int> n_zero_cells{ n_cells };
  n_zero_cells.at(test_helpers::RandomInt(0, dim)) = 0;
  std::vector<int> n_negative_cells{ n_cells };
  n_negative_cells.at(test_helpers::RandomInt(0, dim)) *= -1;

  for (const auto& bad_cells : {n_zero_cells, n_negative_cells}) {
    EXPECT_ANY_THROW({domain::mesh::MeshCartesian<dim> test_mesh(spatial_size, bad_cells);});
  }
}

TYPED_TEST(DomainMeshCartesianTest, SingleMaterialMapping) {
  constexpr int dim = this->dim;
  std::vector<double> spatial_max{test_helpers::RandomVector(dim, 5, 20)};
  std::vector<double> n_cells_double{test_helpers::RandomVector(dim, 5, 20)};
  std::vector<int> n_cells{n_cells_double.cbegin(), n_cells_double.cend()};

  std::string material_mapping{'1'};
  domain::mesh::MeshCartesian<dim> test_mesh(spatial_max, n_cells, material_mapping);

  EXPECT_TRUE(test_mesh.has_material_mapping());

  // Random inner locations
  std::array<std::array<double, dim>, 10> test_locations;

  for (auto& location : test_locations) {
    for (int i = 0; i < dim; ++i) {
      location.at(i) = test_helpers::RandomDouble(0, spatial_max.at(i));
    }

    EXPECT_EQ(test_mesh.GetMaterialID(location), 1);
  }

  // Edges and corners
  std::array<double, dim> test_location;
  dealii::Point<dim> test_point;

  std::array<double, 3> x_locations{0, spatial_max.at(0)/2, spatial_max.at(0)};
  std::vector<double> y_locations{};
  std::vector<double> z_locations{};

  if (dim > 1) {
    y_locations = {0, spatial_max.at(1)/2, spatial_max.at(1)};
    if (dim > 2) {
      z_locations = {0, spatial_max.at(2)/2, spatial_max.at(2)};
    }
  }

  for (const int x : x_locations) {
    test_location.at(0) = x;
    test_point[0] = x;
    for (const int y : y_locations) {
      test_location.at(1) = y;
      test_point[1] = y;
      for (const int z : z_locations) {
        test_location.at(2) = z;
        test_point[2] = z;
      }
    }
    EXPECT_EQ(test_mesh.GetMaterialID(test_location), 1);
    EXPECT_EQ(test_mesh.GetMaterialID(test_point), 1);
  }
}

TYPED_TEST(DomainMeshCartesianTest, FillMaterialIDTest) {
  constexpr int dim = this->dim;
  using Boundary = problem::Boundary;

  std::vector<double> spatial_max(dim, 10.0);
  std::vector<int> n_cells(dim, 4);

  domain::mesh::MeshCartesian<dim> test_mesh(spatial_max, n_cells);
  dealii::Triangulation<dim> test_triangulation;

  std::string material_mapping;
  if (dim == 1) {
    material_mapping = "1 2";
  } else if (dim == 2) {
    material_mapping = "1 2\n3 4";
  } else {
    material_mapping = "1 2\n3 4\n\n5 6\n7 8";
  }

  test_mesh.ParseMaterialMap(material_mapping);
  test_mesh.FillTriangulation(test_triangulation);
  test_mesh.FillMaterialID(test_triangulation);

  for (auto const &cell : test_triangulation.active_cell_iterators()) {
    if (cell->is_locally_owned()) {
      EXPECT_EQ(cell->material_id(), test_mesh.GetMaterialID(cell->center()));
    }
  }
}

class DomainMeshCartesianMappingTest : public ::testing::Test {};

TEST_F(DomainMeshCartesianMappingTest, MultipleMaterialMapping1D) {
  std::vector<double> spatial_max{test_helpers::RandomVector(1, 5, 20)};
  std::vector<double> n_cells_double{test_helpers::RandomVector(1, 5, 20)};
  std::vector<int> n_cells{n_cells_double.cbegin(), n_cells_double.cend()};

  domain::mesh::MeshCartesian<1> test_mesh(spatial_max, n_cells);

  std::string material_mapping{"1 2"};

  test_mesh.ParseMaterialMap(material_mapping);
  EXPECT_TRUE(test_mesh.has_material_mapping());

  std::array<std::array<double, 1>, 5> test_locations;

  for (auto& location : test_locations) {
    location.at(0) = test_helpers::RandomDouble(0, spatial_max.at(0)/2);
    EXPECT_EQ(test_mesh.GetMaterialID(location), 1);
    location.at(0) = test_helpers::RandomDouble(spatial_max.at(0)/2, spatial_max.at(0));
    EXPECT_EQ(test_mesh.GetMaterialID(location), 2);
  }

  // Edge cases
  std::array<double, 1> origin{0};
  std::array<double, 1> midpoint{spatial_max.at(0)/2};
  std::array<double, 1> endpoint{spatial_max.at(0)};

  EXPECT_EQ(test_mesh.GetMaterialID(origin), 1);
  EXPECT_EQ(test_mesh.GetMaterialID(midpoint), 1);
  EXPECT_EQ(test_mesh.GetMaterialID(endpoint), 2);
}

TEST_F(DomainMeshCartesianMappingTest, MultipleMaterialMapping2D) {
  std::vector<double> spatial_max{test_helpers::RandomVector(2, 5, 20)};
  std::vector<double> n_cells_double{test_helpers::RandomVector(2, 5, 20)};
  std::vector<int> n_cells{n_cells_double.cbegin(), n_cells_double.cend()};

  domain::mesh::MeshCartesian<2> test_mesh(spatial_max, n_cells);

  std::string material_mapping{"1 2\n3 4"};
  test_mesh.ParseMaterialMap(material_mapping);

  double x_max = spatial_max.at(0), x_mid = spatial_max.at(0)/2;
  double y_max = spatial_max.at(1), y_mid = spatial_max.at(1)/2;

  EXPECT_TRUE(test_mesh.has_material_mapping());
  // Inner locations
  std::array<std::array<double, 2>, 5> test_locations;
  for (auto& location : test_locations) {
    location.at(0) = test_helpers::RandomDouble(0, x_mid);
    location.at(1) = test_helpers::RandomDouble(0, y_mid);
    EXPECT_EQ(test_mesh.GetMaterialID(location), 3);

    location.at(0) = test_helpers::RandomDouble(x_mid, x_max);
    location.at(1) = test_helpers::RandomDouble(0, y_mid);
    EXPECT_EQ(test_mesh.GetMaterialID(location), 4);

    location.at(0) = test_helpers::RandomDouble(0, x_mid);
    location.at(1) = test_helpers::RandomDouble(y_mid, y_max);
    EXPECT_EQ(test_mesh.GetMaterialID(location), 1);

    location.at(0) = test_helpers::RandomDouble(x_mid, x_max);
    location.at(1) = test_helpers::RandomDouble(y_mid, y_max);
    EXPECT_EQ(test_mesh.GetMaterialID(location), 2);
  }
  // Edges and corners
  std::map<std::array<double, 2>, int> locations_and_material_ids{
      {{0,0}, 3}, {{x_mid, 0}, 3}, {{x_max, 0}, 4},
      {{0, y_mid}, 3}, {{x_mid, y_mid}, 3}, {{x_max, y_mid}, 4},
      {{0, y_max}, 1}, {{x_mid, y_max}, 1}, {{x_max, y_max}, 2}
  };

  for (auto& location_and_material_id : locations_and_material_ids) {
    auto& [location, id] = location_and_material_id;
    EXPECT_EQ(test_mesh.GetMaterialID(location), id);
  }

}

TEST_F(DomainMeshCartesianMappingTest, MultipleMaterialMapping3D) {
  std::vector<double> spatial_max{test_helpers::RandomVector(3, 5, 20)};
  std::vector<double> n_cells_double{test_helpers::RandomVector(3, 5, 20)};
  std::vector<int> n_cells{n_cells_double.cbegin(), n_cells_double.cend()};

  domain::mesh::MeshCartesian<3> test_mesh(spatial_max, n_cells);

  std::string material_mapping{"1 2\n3 4\n\n5 6\n7 8"};
  test_mesh.ParseMaterialMap(material_mapping);

  double x_max = spatial_max.at(0), x_mid = spatial_max.at(0)/2;
  double y_max = spatial_max.at(1), y_mid = spatial_max.at(1)/2;
  double z_max = spatial_max.at(2), z_mid = spatial_max.at(2)/2;

  EXPECT_TRUE(test_mesh.has_material_mapping());
  // Inner locations
  std::array<std::array<double, 3>, 5> test_locations;
  for (auto& location : test_locations) {

    location.at(0) = test_helpers::RandomDouble(0, x_mid);
    location.at(1) = test_helpers::RandomDouble(0, y_mid);
    location.at(2) = test_helpers::RandomDouble(0, z_mid);
    EXPECT_EQ(test_mesh.GetMaterialID(location), 7);

    location.at(0) = test_helpers::RandomDouble(x_mid, x_max);
    location.at(1) = test_helpers::RandomDouble(0, y_mid);
    location.at(2) = test_helpers::RandomDouble(0, z_mid);
    EXPECT_EQ(test_mesh.GetMaterialID(location), 8);

    location.at(0) = test_helpers::RandomDouble(0, x_mid);
    location.at(1) = test_helpers::RandomDouble(y_mid, y_max);
    location.at(2) = test_helpers::RandomDouble(0, z_mid);
    EXPECT_EQ(test_mesh.GetMaterialID(location), 5);

    location.at(0) = test_helpers::RandomDouble(0, x_mid);
    location.at(1) = test_helpers::RandomDouble(0, y_mid);
    location.at(2) = test_helpers::RandomDouble(z_mid, z_max);
    EXPECT_EQ(test_mesh.GetMaterialID(location), 3);

    location.at(0) = test_helpers::RandomDouble(x_mid, x_max);
    location.at(1) = test_helpers::RandomDouble(y_mid, y_max);
    location.at(2) = test_helpers::RandomDouble(0, z_mid);
    EXPECT_EQ(test_mesh.GetMaterialID(location), 6);

    location.at(0) = test_helpers::RandomDouble(x_mid, x_max);
    location.at(1) = test_helpers::RandomDouble(y_mid, y_max);
    location.at(2) = test_helpers::RandomDouble(z_mid, z_max);
    EXPECT_EQ(test_mesh.GetMaterialID(location), 2);

    location.at(0) = test_helpers::RandomDouble(x_mid, x_max);
    location.at(1) = test_helpers::RandomDouble(0, y_mid);
    location.at(2) = test_helpers::RandomDouble(z_mid, z_max);
    EXPECT_EQ(test_mesh.GetMaterialID(location), 4);

    location.at(0) = test_helpers::RandomDouble(0, x_mid);
    location.at(1) = test_helpers::RandomDouble(y_mid, y_max);
    location.at(2) = test_helpers::RandomDouble(z_mid, z_max);
    EXPECT_EQ(test_mesh.GetMaterialID(location), 1);
  }
}

} // namespace


// Copyright (C) 2006-2009 Dmitry Bufistov and Andrey Parfenov

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

#include <sstream>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/adjacency_matrix.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/property_iter_range.hpp>
#include <boost/graph/howard_cycle_ratio.hpp>

#include <boost/test/minimal.hpp>

/**
 * @author Dmitry Bufistov
 * @author Andrey Parfenov
 */

/*!
* The graph has two equal cycles with ratio 2/3
*/
static const char test_graph1[] = "digraph G {\
  edge [w1=1, w2=1];\
  1->2\
  2->3 [w1=0]\
  3->4\
  4->2\
  1->5\
  5->6\
  6->7 [w1=0]\
  7->5 \
}";

/*!
* The graph has no cycles
*/
static const std::string test_graph2 = "digraph G {edge [w1=1]; 1->3 [w2=1]; 1->2 [w2=2]; 1->4 [w2=7]; }";

/*!
* Example from the paper "Nunerical computation of spectral elements"
* Maximum cycle ratio is 5.5
*/
static const char test_graph3[] = "\
digraph article {\
  edge [w2 =2];\
  1->1 [w1 = 1];\
  1->2 [w1 = 2];\
  1->4 [w1 = 7];\
  2->2 [w1 = 3];\
  2->3 [w1 = 5];\
  3->2 [w1 = 4];\
  3->4 [w1 = 3];\
  4->2 [w1 = 2];\
  4->3 [w1 = 8];\
}";

/*!
* Simple multigraph.
* Maximum cycle ratio is 2.5, minimum  0.5
*/
static const char test_graph4[] = "digraph G {\
edge [w2=1];\
a->b  [w1=1];\
b->a  [w1=0];\
a->b [w1=2];\
b->a [w1=3];\
}";

/*!
* The big graph with two equal cycles
*/
static const char test_graph5[]= "digraph G { edge [w2=1, w1=1]; n94->n8; n95->n8; n93->n8; n93->n9; n42->n9; n23->n13;\
n29->n13; n95->n14; n37->n14; n95->n19; n37->n19; n94->n23; n60->n26; n76->n26; n94->n29; n9->n33 [w1=0]; n80->n33;\
n14->n34 [w1=0];n19->n34; n94->n37; n94->n42; n95->n42; n8->n60; n26->n60; n26->n76; n106->n76; n93->n80; n42->n80;\
n33->n93; n60->n93; n13->n94; n60->n94; n34->n95; n60->n95; n94->n106; n95->n106; n93->n106;\
}";

/*!
* Random graph generated by hands.
* Maximum cycle ratio is 3.58, minimum is 0.294118
*/
static const char test_graph6[]= "digraph test_graph6 {\
  16;\
  17;\
\
  1->2 [w1=1, w2=0.1];\
  2->3 [w1 = 2, w2=3.6];\
  3->4 [w1=7, w2=8];\
  4->5 [w1=3.1,w2=0.8];\
  4->5 [w1 = 4.2, w2=0.6];\
  4->5 [w1 = 5.3, w2=0.4];\
  5->6 [w1=-10, w2 = 34.75]\
  6->1 [w1=100, w2 = 20]\
\
  1->7 [w1=10, w2 = 20];\
  7->8 [w1=3.75, w2 = 1.25];\
  7->8 [w1=30, w2 = 22.2];\
  8->9 [w1=10, w2 = 20];\
  9->10 [w1=-2.1, w2 = 30]\
  10->6 [w1=10, w2 = 20]\
\
  11->12 [w1 = 5, w2=0.45];\
  12->13 [w1 = 4, w2=0.2];\
  13->14 [w1 = 3, w2=15.75];\
  14->11 [w1 = -2.5, w2=0.6];\
  11->10 [w1 = -8, w2=0.9];\
  11->10 [w1 = -15, w2=2.9];\
\
  18 -> 19 [w1=18, w2=6];\
  18 -> 20 [w1=16.3, w2=8.2];\
  18 -> 21 [w1=-3, w2=15];\
  18 -> 18 [w1=2, w2=1];\
  19 -> 18 [w1=0.06, w2=0.01];\
  19 -> 19 [w1=1, w2=1.2];\
  19 -> 20 [w1=5, w2=2];\
  19 -> 21 [w1=3, w2=0.1];\
  20 -> 18 [w1=4, w2=0.2];\
  20 -> 19 [w1=11, w2=21];\
  20 -> 20 [w1=6, w2=5];\
  20 -> 21 [w1=7, w2=1];\
  21 -> 18 [w1=8, w2=2];\
  21 -> 19 [w1=12, w2=6];\
  21 -> 20 [w1=7.5, w2=4.3];\
  21 -> 21 [w1=1.25, w2=2.15];\
}";

using namespace boost;
typedef  property<vertex_index_t, int, property<boost::vertex_name_t, std::string> > vertex_props_t;
template <typename TW1, typename TW2> struct Graph
{
    typedef typename boost::property<
        boost::edge_weight_t, TW1,
        typename boost::property<
            boost::edge_weight2_t, TW2, property<boost::edge_index_t, int>
        >
    > edge_props_t;
    typedef typename boost::adjacency_list<
        boost::listS, boost::listS, boost::directedS, vertex_props_t,
        edge_props_t>
    type;
};
typedef Graph<int, int>::type diGraphInt;
typedef Graph<double, double>::type diGraphReal;

template <typename TW1, typename TW2>
struct CEdgeProps
{
  CEdgeProps(TW1 w1 = 1, TW2 w2 = 2) :
  m_w1(w1), m_w2(w2), m_edge_index((std::numeric_limits<int>::max)())
  {}
  TW1 m_w1;
  TW2 m_w2;
  int m_edge_index;
};
typedef  adjacency_matrix<directedS, no_property, CEdgeProps<int, int> > GraphMInt;

///Create "tokens_map" for reading graph properties from .dot file
template <typename TG>
void  make_dynamic_properties(TG &g, dynamic_properties &p)
{
  p.property("node_id", get(vertex_name, g));
  p.property("label", get(edge_weight, g));
  p.property("w1", get(edge_weight, g));
  p.property("w2", get(edge_weight2, g));
}

template <typename TG>
void read_data1(std::istream &is, TG &g)
{
  dynamic_properties p;
  make_dynamic_properties(g, p);
  read_graphviz(is, g, p);
  std::cout << "Number of vertices: " << num_vertices(g) << std::endl;
  std::cout << "Number of edges: " << num_edges(g) << std::endl;
  int i = 0;
  BGL_FORALL_VERTICES_T(vd, g, TG)
  {
    put(vertex_index, g, vd, i++);
  }
  i=0;
  BGL_FORALL_EDGES_T(ed, g, TG)
  {
    put(edge_index, g, ed, i++);
  }
}

template <typename TG>
void read_data(const char *file, TG &g)
{
  std::cout << "Reading data from file: " << file << std::endl;
  std::ifstream ifs(file);
  BOOST_REQUIRE(ifs.good());
  read_data1(ifs, g);
}

struct my_float : boost::mcr_float<>
{
  static double infinity()
  {
    return 1000;
  }
};

struct my_float2 : boost::mcr_float<>
{
  static double infinity() { return 2; }
};

int test_main(int argc, char* argv[])
{
  assert (argc >= 2);
  using std::endl; using std::cout;
  const double epsilon = 0.005;
  double min_cr, max_cr; ///Minimum and maximum cycle ratio
  typedef std::vector<graph_traits<diGraphInt>::edge_descriptor> ccInt_t;
  typedef std::vector<graph_traits<diGraphReal>::edge_descriptor> ccReal_t;
  ccInt_t cc; ///critical cycle

  diGraphInt tg;
  property_map<diGraphInt, vertex_index_t>::type vim = get(vertex_index, tg);
  property_map<diGraphInt, edge_weight_t>::type ew1m = get(edge_weight, tg);
  property_map<diGraphInt, edge_weight2_t>::type ew2m = ew2m;

  {
    std::istringstream  iss(test_graph1);
    assert(iss.good());
    read_data1(iss, tg);
    max_cr = maximum_cycle_ratio(tg, vim, ew1m, ew2m);
    cout << "Maximum cycle ratio is " << max_cr << endl;
    BOOST_CHECK(std::abs( max_cr - 0.666666666) < epsilon );
    tg.clear();
  }

  {
    std::istringstream  iss(test_graph2);
    read_data1(iss, tg);
    // TODO: This is causing a failuire, but I'm not really sure what it's doing per se.
    // Commented out for now.
    // BOOST_CHECK(std::abs(maximum_cycle_ratio(tg, vim, ew1m, ew2m) + (std::numeric_limits<double>::max)()) < epsilon );
    BOOST_CHECK(std::abs(boost::maximum_cycle_ratio(tg, vim, ew1m, ew2m,
                                                        static_cast<ccInt_t*>(0), my_float()) + 1000) < epsilon );
    tg.clear();
  }

  {
    std::istringstream iss(test_graph3);
    diGraphInt tgi;
    read_data1(iss, tgi);
    double max_cr = maximum_cycle_ratio(tgi, vim, ew1m, ew2m,
      static_cast<ccInt_t*>(0));
    cout << "Maximum cycle ratio is " << max_cr << endl;
    BOOST_CHECK(std::abs( max_cr - 2.75) < epsilon );
    double maxmc = maximum_cycle_mean(tgi, vim, ew1m, get(edge_index, tgi));
    cout << "Maximum cycle mean is " << maxmc << endl;
    BOOST_CHECK(std::abs( maxmc - 5.5) < epsilon );
    tg.clear();
  }

  {
    std::istringstream  iss(test_graph4);
    read_data1(iss, tg);
    max_cr = maximum_cycle_ratio(tg, vim, ew1m, ew2m);
    cout << "Maximum cycle ratio is " << max_cr << endl;
    BOOST_CHECK(std::abs( max_cr - 2.5) < epsilon );
    min_cr = minimum_cycle_ratio(tg, vim, ew1m, ew2m);
    cout << "Minimum cycle ratio is " << min_cr << endl;
    BOOST_CHECK(std::abs( min_cr - 0.5) < epsilon );
    tg.clear();
  }

  {
    std::istringstream  iss(test_graph5);
    read_data1(iss, tg);
        min_cr = minimum_cycle_ratio(tg, vim, ew1m, ew2m, &cc, my_float());
    BOOST_CHECK(std::abs( min_cr - 0.666666666) < epsilon );
    cout << "Minimum cycle ratio is " << min_cr << endl;
    cout << "Critical cycle is:\n";
    for (ccInt_t::iterator itr = cc.begin(); itr != cc.end(); ++itr)
    {
      cout << "(" << get(vertex_name, tg, source(*itr, tg)) <<
        "," << get(vertex_name, tg, target(*itr, tg)) << ") ";
    }
    cout << endl;
    tg.clear();
  }

  {
      read_data(argv[1], tg);
      min_cr = boost::minimum_cycle_ratio(tg, vim, ew1m, ew2m, &cc, my_float2());
      cout << "Minimum cycle ratio is " << min_cr << endl;
      BOOST_CHECK(std::abs(min_cr - 0.33333333333) < epsilon );
      cout << "Critical cycle is:" << endl;
      for (ccInt_t::iterator it = cc.begin(); it != cc.end(); ++it)
    {
          cout << "(" << get(vertex_name, tg, source(*it, tg)) << "," <<
            get(vertex_name, tg, target(*it, tg)) << ") ";
    }
      cout << endl;
      tg.clear();
  }

  {
    diGraphReal  tgr;
    ccReal_t  cc1;
    std::istringstream  iss(test_graph6);
    read_data1(iss, tgr);
    max_cr = maximum_cycle_ratio(tgr, get(vertex_index, tgr), get(edge_weight, tgr), get(edge_weight2, tgr));
    cout << "Maximum cycle ratio is " << max_cr << endl;
    min_cr = minimum_cycle_ratio(tgr, get(vertex_index, tgr), get(edge_weight, tgr),
                                     get(edge_weight2, tgr), &cc);
    cout << "Minimal cycle ratio is " << min_cr << endl;
    std::pair<double, double> cr(.0,.0);
    cout << "Critical cycle is:\n";
    for (ccReal_t::iterator itr = cc.begin(); itr != cc.end(); ++itr)
    {
      cr.first += get(edge_weight, tgr, *itr); cr.second += get(edge_weight2, tgr, *itr);
      cout << "(" << get(vertex_name, tgr, source(*itr, tgr)) << "," <<
        get(vertex_name, tgr, target(*itr, tgr)) << ") ";
    }
    BOOST_CHECK(std::abs(cr.first / cr.second - min_cr) < epsilon);
    cout << endl;
  }

  {
    GraphMInt gm(10);
    typedef graph_traits<GraphMInt>::vertex_iterator VertexItM;
    typedef graph_traits<GraphMInt>::edge_descriptor EdgeM;
    VertexItM  vi1, vi2, vi_end;
    for (boost::tie(vi1, vi_end) = vertices(gm); vi1 != vi_end; ++vi1)
    {
      for (vi2 = vertices(gm).first; vi2 != vi_end; ++vi2)
        add_edge(*vi1, *vi2, gm);
    }
    max_cr = maximum_cycle_ratio(gm, get(vertex_index, gm),
      get(&CEdgeProps<int, int>::m_w1, gm), get(&CEdgeProps<int, int>::m_w2, gm));
    BOOST_CHECK(std::abs(max_cr - 0.5) < epsilon);
  }

  return 0;
}