// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the 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
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#include "graph_filtering.hh"

#include <boost/python.hpp>

#include "graph.hh"
#include "graph_selectors.hh"
#include "graph_properties.hh"

#include "graph_avg_correlations.hh"

#include <iostream>

using namespace std;
using namespace boost;
using namespace graph_tool;

// implementations spread across different compile units to minimize memory
// usage during compilation
void graph_avg_corr_imp1(GraphInterface& g, boost::python::object& avg,
                         boost::python::object& dev, boost::python::object& ret_bins,
                         boost::any deg1, boost::any deg2,
                         boost::any weight,
                         const vector<long double>& bins);

typedef UnityPropertyMap<int,GraphInterface::edge_t> cweight_map_t;

boost::python::object
get_vertex_avg_correlation(GraphInterface& gi,
                           GraphInterface::deg_t deg1,
                           GraphInterface::deg_t deg2,
                           boost::any weight,
                           const vector<long double>& bins)
{
    boost::python::object avg, dev;
    boost::python::object ret_bins;

    any weight_prop;
    typedef DynamicPropertyMapWrap<long double, GraphInterface::edge_t>
        wrapped_weight_t;

    if (!weight.empty())
    {
        weight_prop = wrapped_weight_t(weight, edge_scalar_properties());
    }
    else
        weight_prop = cweight_map_t();

    try
    {
        run_action<>()(gi, get_avg_correlation<GetNeighborsPairs>
                       (avg, dev, bins, ret_bins),
                       scalar_selectors(), scalar_selectors(),
                       boost::mpl::vector<cweight_map_t>())
            (degree_selector(deg1), degree_selector(deg2), weight_prop);
    }
    catch (ActionNotFound&)
    {
        graph_avg_corr_imp1(gi, avg, dev, ret_bins,
                            degree_selector(deg1),
                            degree_selector(deg2), weight_prop, bins);
    }

    return boost::python::make_tuple(avg, dev, ret_bins);
}

using namespace boost::python;

void export_avg_correlations()
{
    def("vertex_avg_correlation", &get_vertex_avg_correlation);
}


#include <vector>
#include <list>
#include <boost/dynamic_bitset.hpp>

using std::list;
using std::vector;

class Label
{
public:
    Label(int num_bits)
    {
        is_new = false;
        resource = 0;
        cost = 0;
        sum_reachable = 0;
        reachables = boost::dynamic_bitset<>(num_bits);
        path = boost::dynamic_bitset<>(num_bits);
    }
    Label(bool n, int r, double c, int s,
          const boost::dynamic_bitset<> &rs,
          const boost::dynamic_bitset<> &p) : is_new(n), resource(r), cost(c), sum_reachable(s), reachables(rs), path(p) {}
    bool is_new;
    int resource;
    double cost;
    int sum_reachable;
    boost::dynamic_bitset<> reachables;
    boost::dynamic_bitset<> path;
};

class Esprc
{

public:
    Esprc(){}
    Esprc(int _num_customers,
          vector<vector<double>> _distances,
          vector<int> _demands,
          int _capacity);
    void solve(vector<double> duals, int k);
    void get_solution(vector<vector<int>>&, vector<double>&);

private:
    Label *get_from_pool(void);
    void return_to_pool(Label *l);
    Label *extend(const Label *li, int i, int j);
    bool dominates(const Label &l1, const Label &l2);
    bool add_label(Label *l1, int j);

    vector<vector<double>> distances;
    int num_customers;
    vector<int> demands;
    int capacity;
    vector<vector<int>> successors;
    vector<Label *> pool;
    int pool_size;
    int init_pool_size;
    vector<double> costs;
    vector<list<Label *>> labels;
    vector<double> route_costs;
    vector<vector<int>> routes;
};


/*!
@file
Defines `boost::hana::Comparable::laws`.

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

#ifndef BOOST_HANA_COMPARABLE_LAWS_HPP
#define BOOST_HANA_COMPARABLE_LAWS_HPP

#include <boost/hana/comparable/comparable.hpp>

#include <boost/hana/logical/logical.hpp>
#include <boost/hana/searchable/searchable.hpp>

namespace boost { namespace hana {
    struct Comparable::laws {
        template <typename ComparableObjects>
        static constexpr auto check(ComparableObjects objs) {
            auto implies = [](auto p, auto q) { return or_(not_(p), q); };
            return all(objs, [=](auto a) {
                return all(objs, [=](auto b) {
                    return all(objs, [=](auto c) {
                        return and_(
                            equal(a, a),
                            implies(equal(a, b), equal(b, a)),
                            implies(and_(equal(a, b), equal(b, c)), equal(a, c))
                        );
                    });
                });
            });
        }
    };
}} // end namespace boost::hana

#endif // !BOOST_HANA_COMPARABLE_LAWS_HPP


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

#ifndef NETKET_HILBERT_INDEX_HPP
#define NETKET_HILBERT_INDEX_HPP

#include <limits>
#include <map>
#include <nonstd/span.hpp>
#include <vector>

#include <Eigen/Dense>

#include "common_types.hpp"

namespace netket {

class HilbertIndex {
 public:
  HilbertIndex(std::vector<double> localstates, int local_size, int size);

  // converts a vector of quantum numbers into the unique integer identifier
  std::size_t StateToNumber(const Eigen::VectorXd &v) const;

  // converts a vector of quantum numbers into the unique integer identifier
  // this version assumes that a number representaiton is already known for the
  // given vector v, and this function is used to update it
  std::size_t DeltaStateToNumber(const Eigen::VectorXd &v,
                                 nonstd::span<const int> connector,
                                 nonstd::span<const double> newconf) const;

  // converts an integer into a vector of quantum numbers
  Eigen::VectorXd NumberToState(int i) const;

  /*constexpr*/ int NStates() const noexcept { return nstates_; }
  constexpr static int MaxStates = std::numeric_limits<int>::max() - 1;

 private:
  void Init();

  const std::vector<double> localstates_;
  const int localsize_;
  const int size_;
  std::map<double, int> statenumber_;
  std::vector<std::size_t> basis_;
  int nstates_;
};

class StateGenerator {
 public:
  using difference_type = Index;
  using value_type = Eigen::VectorXd;
  using reference = value_type;

  explicit StateGenerator(const HilbertIndex &index) : i_(0), index_(index) {}

  reference operator*() const { return index_.NumberToState(i_); }

  StateGenerator &operator++() {
    ++i_;
    return *this;
  }

  // TODO(C++17): Replace with comparison to special Sentinel type, since
  // C++17 allows end() to return a different type from begin().
  bool operator!=(const StateGenerator &) { return i_ < index_.NStates(); }
  // pybind11::make_iterator requires operator==
  bool operator==(const StateGenerator &other) { return !(*this != other); }

  StateGenerator begin() const { return *this; }
  StateGenerator end() const { return *this; }

 private:
  int i_;
  const HilbertIndex &index_;
};

}  // namespace netket

#endif  // NETKET_HILBERT_INDEX_HPP


#include <vector>
#include <boost/range.hpp>
#include <boost/iterator/cycle_iterator.hpp>
#include <boost/iterator/cycle_iterator_ext.hpp>

void example_iter(std::ostream& os){

    using namespace boost;
    typedef double val_;
    typedef std::vector<val_> vec_;
    typedef range_size<vec_>::type size_;
    typedef range_iterator<vec_>::type base_it_;
    const unsigned n = 10;
    vec_ vec(n);

    typedef cycle_iterator<base_it_,size_> it_;
    it_ it(boost::begin(vec),boost::end(vec),0);
    it_ it2;
    //it2 = it;

    typedef cycle_iterator_ext<base_it_,size_> it_ext_;
    it_ext_ it_ext(boost::begin(vec),boost::end(vec),0);

    it_ext_ it_ext2;
    it_ext2 = it_ext;

    os << "end";

}


#include <boost/compute/container/dynamic_bitset.hpp>


#ifndef YASMIC_UTIL_LOAD_CRM_GRAPH
#define YASMIC_UTIL_LOAD_CRM_GRAPH

/*
 * load_crm_graph.hpp
 * David Gleich
 * Stanford University
 * 26 January 2006
 */

/**
 * @file load_crm_graph.hpp
 * Load a graph or matrix into a crm data structure.
 */
#include <cctype>
#include <iostream>
#include <fstream>
#include <iterator>
#include <numeric>

#include <vector>

#include <boost/iterator/reverse_iterator.hpp>

#include <yasmic/ifstream_matrix.hpp>
#include <yasmic/binary_ifstream_matrix.hpp>
#include <yasmic/cluto_ifstream_matrix.hpp>

#ifdef BOOST_MSVC
#if _MSC_VER >= 1400
	// disable the warning for ifstream::read
    #pragma warning( push )
	#pragma warning( disable : 4996 )
#endif // _MSC_VER >= 1400
#endif // BOOST_MSVC

/**
 * This function actually loads the data from a matrix file.
 *
 * This function allocates and frees sizeof(Index)*nrows memory.
 */
template <class InputMatrix, class RAIRows, class RAICols, class RAIVals>
bool load_matrix_to_crm(InputMatrix& m,
						RAIRows rows, RAICols cols, RAIVals vals,
						bool rows_populated = false)
{
	using namespace yasmic;
	using namespace std;

	typedef typename iterator_traits<RAIRows>::value_type index_type;

	typename smatrix_traits<InputMatrix>::size_type nr = nrows(m);
	typename smatrix_traits<InputMatrix>::nz_index_type nzcount = 0;

	typename smatrix_traits<InputMatrix>::nonzero_iterator nzi, nzend;

	int warning = 0;

	if (nr < 1)
	{
		using namespace std;
		cerr << "error: invalid number of rows" << endl;
		return (false);
	}

	if (rows_populated == false)
	{
		boost::tie(nzi, nzend) = nonzeros(m);
		for (; nzi != nzend; ++nzi)
		{
			warning = warning | (row(*nzi, m) >= nr) | (column(*nzi, m) >= nr);
			++rows[row(*nzi, m)*(!warning)+1];
			++nzcount;
		}

		if (warning)
		{
			using namespace std;
			cerr << "error: invalid matrix data, nrows or ncols exceeded" << endl;
			return (false);
		}

		if (nzcount != yasmic::nnz(m))
		{
			using namespace std;
			cerr << "error: number of nonzeros do not match nnz" << endl;
			return (false);
		}
	}

	// compute the reduction
	partial_sum(rows, rows+(nr+1), rows);

	index_type cr;
	index_type cc;

	nzcount = 0;
	tie(nzi, nzend) = yasmic::nonzeros(m);
    for (; nzi != nzend; ++nzi)
    {
		cr = row(*nzi, m);
		cc = column(*nzi, m);
		cols[rows[cr]] = cc;
		vals[rows[cr]] = value(*nzi,m);
		++rows[cr];
		++nzcount;

		warning = warning | (cr >= nr) | (cc >= nr);
	}

	if (warning)
	{
		using namespace std;
		cerr << "error: invalid matrix data, nrows or ncols exceeded" << endl;
		return (false);
	}

	if (nzcount != yasmic::nnz(m))
	{
		using namespace std;
		cerr << "error: number of nonzeros do not match nnz" << endl;
		return (false);
	}

	std::copy(boost::make_reverse_iterator(rows+nr-1),
		boost::make_reverse_iterator(rows),
		boost::make_reverse_iterator(rows+nr));

	rows[0] = 0;

	return (true);
}

/**
 * This function does most of the work loading the matrix.
 *
 * 1.  Allocate storage in the passed std::vectors.
 * 2.  Check for degrees metadata and read.
 * 3.  Load the data for the graph.
 */
template <class InputMatrix, class Index, class Value>
bool load_crm_graph_type(InputMatrix& m, std::string filename,
						 std::vector<Index>& rows,
						 std::vector<Index>& cols,
						 std::vector<Value>& vals,
						 Index& nr, Index& nc, Index& nzcount)
{
	using namespace yasmic;
	using namespace std;

	nr = nrows(m);
	nc = ncols(m);
	nzcount = nnz(m);

	//
	// 1.  Allocate storage
	//
	rows.resize(nr+1);
	cols.resize(nzcount);
	vals.resize(nzcount);

	//
	// 2.  Check for degrees metadata and read.
	//
	bool degrees_data = false;
	{
		// look at the extension...
		typedef std::string::size_type position;

		std::string filename_degrees = filename + ".degs";

		ifstream test(filename_degrees.c_str(), ios::binary);
		if (test.is_open())
		{
			// degrees CAN be binary; we test for a binary file in the following way...
			// 1.  If the filesize is exactly as predicted (sizeof(Index)*nrows)
			// 2.  If the filesize is at least as big as predicted and there
			//     are binary bytes in the first 100 characters
			ifstream::off_type begin = test.tellg();
			test.seekg (0, ios::end);
			ifstream::off_type end = test.tellg();

			bool binary1 = false;
			if (end-begin == sizeof(Index)*nr)
			{
				// passes test 1
				binary1 = true;
			}

			bool binary2 = false;
			if (!binary1 && (unsigned)(end-begin) >= sizeof(Index)*nr)
			{
				test.seekg (0, ios::end);
				// look for any non ASCII data (it would have to be a list of
				// of ints, so ...)
				for (int i = 0; i < 100; i++)
				{
					char c;
					test.get(c);
					// isspace takes care of newline, tabs, etc.
					if (!std::isdigit(c) && !std::isspace(c))
					{
						binary2 = true;
					}
				}
			}

			test.close();

			if (binary1 || binary2)
			{
				YASMIC_VERBOSE( std::cerr << "reading binary degree file..." << std::endl; )

				// the degrees file is binary
				std::ifstream degfile(filename_degrees.c_str(), ios::binary);

				typename std::vector<Index>::iterator i = rows.begin();
				typename std::vector<Index>::iterator iend = rows.end();

				// we read degrees into the second one
				++i;
				while (i != iend)
				{
					Index ci;
					degfile.read((char *)&ci, sizeof(Index));
					*i = ci;
					++i;
				}

				degrees_data = true;

			}
			else
			{
				YASMIC_VERBOSE( std::cerr << "reading ASCII degree file..." << std::endl; )

				// the degrees file is text
				std::ifstream degfile(filename_degrees.c_str());

				copy(istream_iterator<Index>(degfile),
					 istream_iterator<Index>(), ++rows.begin());

				degrees_data = true;
			}
		}
	}

	//
	// 3.  Load the matrix
	//
	return (load_matrix_to_crm(m, rows.begin(), cols.begin(), vals.begin(),
		degrees_data));
}

/**
 * Load a CRM graph from a file into a set of vectors.
 *
 * This operation is actually somewhat tricky.  We to do a few things.
 * 1.  Allocate storage in the passed std::vectors.
 * 2.  Check for degrees metadata and read.
 * 3.  Load the data for the graph.
 *
 * All three operations need an input matrix type, but we need
 * to do the same thing for all, so this function dumps all
 * the work on load_crm_graph_type.
 */
template <class Index, class Value>
bool load_crm_graph(std::string filename,
					std::vector<Index>& rows, std::vector<Index>& cols,
					std::vector<Value>& vals,
					Index &nr, Index &nc, Index &nzcount)
{
	using namespace std;

	// look at the extension...
	typedef string::size_type position;

	position dot = filename.find_last_of(".");

	if (dot != string::npos)
	{
		string ext = filename.substr(dot+1);
		transform(ext.begin(), ext.end(), ext.begin(), (int(*)(int))tolower);

		if (ext.compare("smat") == 0 || ext.compare("graph") == 0)
		{
			YASMIC_VERBOSE( std::cerr << "using smat loader..." << std::endl; )

			ifstream ifs(filename.c_str());
			yasmic::ifstream_matrix<> m(ifs);
			return (load_crm_graph_type(m, filename, rows, cols, vals,
						nr, nc, nzcount));
		}
		else if (ext.compare("bsmat") == 0)
		{
			YASMIC_VERBOSE( std::cerr << "using bsmat loader..." << std::endl; )

			ifstream ifs(filename.c_str(), ios::binary);
			yasmic::binary_ifstream_matrix<> m(ifs);
			return (load_crm_graph_type(m, filename, rows, cols, vals,
						nr, nc, nzcount));
		}
        else if (ext.compare("mat") == 0)
		{
			YASMIC_VERBOSE( std::cerr << "cluto mat loader..." << std::endl; )

			ifstream ifs(filename.c_str());
			yasmic::cluto_ifstream_matrix<> m(ifs);
			return (load_crm_graph_type(m, filename, rows, cols, vals,
						nr, nc, nzcount));
		}
		else
		{
			cerr << "Error: matrix type unknown." << endl;
		}
	}
	else
	{
		cerr << "Error: matrix type indeterminate." << endl;
		return (false);
	}

	return (false);
}

#ifdef BOOST_MSVC
#if _MSC_VER >= 1400
	// restore the warning for ifstream::read
	#pragma warning( pop )
#endif // _MSC_VER >= 1400
#endif // BOOST_MSVC

#endif // YASMIC_UTIL_LOAD_CRM_GRAPH


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

#ifndef NQS_BOSONS_HPP
#define NQS_BOSONS_HPP

#include <Eigen/Core>
#include <algorithm>
#include <cmath>
#include <vector>
#include "Graph/abstract_graph.hpp"
#include "Hilbert/abstract_hilbert.hpp"

namespace nqs {

/**
  Hilbert space for integer or bosons.
  The hilbert space is truncated to some maximum occupation number.
*/

class Boson : public AbstractHilbert {
 public:
  Boson(const AbstractGraph &graph, int nmax);
  Boson(const AbstractGraph &graph, int nmax, int nbosons);

  bool IsDiscrete() const override;
  int LocalSize() const override;
  int Size() const override;
  std::vector<double> LocalStates() const override;
  const AbstractGraph &GetGraph() const noexcept override;

  void RandomVals(Eigen::Ref<Eigen::VectorXd> state,
                  nqs::default_random_engine &rgen) const override;

  void UpdateConf(Eigen::Ref<Eigen::VectorXd> v,
                  nonstd::span<const int> tochange,
                  nonstd::span<const double> newconf) const override;

 private:
  void Init();
  inline void SetNbosons(int nbosons);
  inline bool CheckConstraint(Eigen::Ref<const Eigen::VectorXd> v) const;

 private:
  const AbstractGraph &graph_;

  int nsites_;

  std::vector<double> local_;

  // total number of bosons
  // if constraint is activated
  int nbosons_;

  bool constraintN_;

  // maximum local occupation number
  int nmax_;

  int nstates_;
};

}  // namespace nqs
#endif


/*
 * Acknowledgement: Hao ren (renh@upc.edu.cn)
 *                  Eigen3 http://eigen.tuxfamily.org/index.php?title=Main_Page
 *
 *        Author: Ionizing (https://github.com/Ionizing)
 */

#pragma once
#ifndef BASE_H
#define BASE_H

#if __cplusplus < 201103L
  #error "Deprecated compiler, use gcc4.9 or higher"
#endif

#include <iostream>
#include <fstream>
#include <complex>
#include <algorithm>
#include <fstream>
#include <tuple>
#include <iomanip>
#include <vector>
#include <cmath>

// #include <fftw3.h>

#define EIGEN_DEFAULT_TO_ROW_MAJOR
#include <unsupported/Eigen/CXX11/Tensor>
#include <Eigen/Eigen>

namespace ionizing {

  using namespace Eigen;

  static_assert(EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION == Eigen::RowMajor,
      "Eigen is using ColMajor!");

  static_assert(sizeof(size_t) == sizeof(uint64_t),
      "Uncompatible compiler: sizeof(size_t) != 8 !");
  static_assert(sizeof(double) == 8,
      "Uncompatible compiler: sizeof(double) != 8 !");
  static_assert(sizeof(float)  == 4,
      "Uncompatible compiler: sizeof( float) != 4 !");
  static_assert(sizeof(int)    == 4,
      "Uncompatible compiler: sizeof(   int) != 4 !");
  static_assert(sizeof(std::complex<double>) == 16,
      "Uncompatible compiler: sizeof(complex<double>) != 16 !");
  static_assert(sizeof(std::complex< float>) ==  8,
      "Uncompatible compiler: sizeof(complex< float>) !=  8 !");

// global type aliasing
  using Vecd   = VectorXd;
  using Veccd  = VectorXcd;
  using Matd   = Matrix<              double, Dynamic, Dynamic>;
  using Matcd  = Matrix<std::complex<double>, Dynamic, Dynamic>;
  using Cubd   = Tensor<              double,                3, RowMajor>;
  using Cubcd  = Tensor<std::complex<double>,                3, RowMajor>;
  using Mat33d = Matrix<double,                     3,       3>;
  using MatX3d = Matrix<double,               Dynamic,       3>;
  using MatX3b = Matrix<bool,                 Dynamic,       3>;

  template <typename T>
    using MatT    = Matrix<T, Dynamic, Dynamic>;

  template <typename T>
    using ColVecT = Matrix<T, Dynamic,       1>;

  template <typename T>
    using RowVecT = Matrix<T,       1, Dynamic>;

  template <typename T>
    using VecT = ColVecT<T>;

// genenral string aliases
  using std::string;
  using VecStr = std::vector<string>;

// global format layout format
  static IOFormat CommaInitFmt{StreamPrecision, DontAlignCols, ", ", ", ", "", "", " << ", ";"};
  static IOFormat CleanFmt{4, 0, ", ", "\n", "[", "]"};
  static IOFormat OctaveFmt{StreamPrecision, 0, ", ", ";\n", "", "", "[", "]"};
  static IOFormat HeavyFmt{FullPrecision    , 0, ", ", ";\n", "[", "]", "[", "]"};
  static IOFormat PosMatFormat{ 10, 0, "  ", "\n", "   "};

// test if is RowMajor
static_assert(
    std::is_same<Matd, Matrix<double, Dynamic, Dynamic, RowMajor>>::value,
    "Eigen is using ColMajor for Matd");

static_assert(
    std::is_same<Matcd,
    Matrix<std::complex<double>, Dynamic, Dynamic, RowMajor>>::value,
    "Eigen is using ColMajor for Matcd");

static_assert(
    std::is_same<Mat33d, Matrix<double, 3, 3, RowMajor>>::value,
    "Eigen is using ColMajor for Matd");

static_assert(
    std::is_same<MatX3d,
    Matrix<double, Dynamic, 3, RowMajor>>::value,
    "Eigen is using ColMajor for Matcd");

static_assert(
    std::is_same<Cubd, Tensor<double, 3, RowMajor>>::value,
    "Eigen is using ColMajor for Cubd");

static_assert(
    std::is_same<Cubcd,
    Tensor<std::complex<double>, 3, RowMajor>>::value,
    "Eigen is using ColMajor for Cubcd"
    );

}

#endif // BASE_H


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// 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 <iostream>

#include <geometry_test_common.hpp>

#include <boost/core/ignore_unused.hpp>

#include <boost/geometry/geometries/concepts/segment_concept.hpp>

#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/segment.hpp>

#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>

#include <boost/geometry/io/dsv/write.hpp>

#include <test_common/test_point.hpp>
#include <test_geometries/custom_segment.hpp>

BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

template <typename P>
void test_all()
{
    typedef bg::model::referring_segment<P> S;

    P p1;
    P p2;
    S s(p1, p2);
    BOOST_CHECK_EQUAL(&s.first, &p1);
    BOOST_CHECK_EQUAL(&s.second, &p2);

    // Compilation tests, all things should compile.
    BOOST_CONCEPT_ASSERT( (bg::concepts::ConstSegment<S>) );
    BOOST_CONCEPT_ASSERT( (bg::concepts::Segment<S>) );

    typedef typename bg::coordinate_type<S>::type T;
    typedef typename bg::point_type<S>::type SP;
    boost::ignore_unused<T, SP>();

    //std::cout << sizeof(typename coordinate_type<S>::type) << std::endl;

    typedef bg::model::referring_segment<P const> refseg_t;
    //BOOST_CONCEPT_ASSERT( (concepts::ConstSegment<refseg_t>) );

    refseg_t seg(p1, p2);

    typedef typename bg::coordinate_type<refseg_t>::type CT;
    typedef typename bg::point_type<refseg_t>::type CSP;
    boost::ignore_unused<CT, CSP>();
}

template <typename S>
void test_custom()
{
    S seg;
    bg::set<0,0>(seg, 1);
    bg::set<0,1>(seg, 2);
    bg::set<1,0>(seg, 3);
    bg::set<1,1>(seg, 4);
    std::ostringstream out;
    out << bg::dsv(seg);
    BOOST_CHECK_EQUAL(out.str(), "((1, 2), (3, 4))");
}

int test_main(int, char* [])
{
    test_all<int[3]>();
    test_all<float[3]>();
    test_all<double[3]>();
    //test_all<test_point>();
    test_all<bg::model::point<int, 3, bg::cs::cartesian> >();
    test_all<bg::model::point<float, 3, bg::cs::cartesian> >();
    test_all<bg::model::point<double, 3, bg::cs::cartesian> >();

    test_custom<test::custom_segment>();
    test_custom<test::custom_segment_of<bg::model::point<double, 2, bg::cs::cartesian> > >();
    test_custom<test::custom_segment_of<test::custom_point_for_segment> >();
    test_custom<test::custom_segment_4>();

    return 0;
}


#pragma once

#include <boost/hana.hpp>

#include "calc/calculator.hpp"

namespace calc::solver::detail {

    namespace hana = boost::hana;

    struct node {
        calculator state;
        actions_t actions = { };
    };

    constexpr auto operator<(const node & n1, const node & n2) {
        return n1.state.move_count > n2.state.move_count;
    }

    using open_set_t = std::priority_queue<node, std::deque<node>>;

    template <action::type_e action_type>
    constexpr auto action_type_c = hana::int_c<static_cast<int>(action_type)>;

    using buttons::specs::press;
    using buttons::specs::hold;

    constexpr auto action_map = hana::make_map(
        hana::make_pair(action_type_c<action::type_e::Press>, press),
        hana::make_pair(action_type_c<action::type_e::Hold>, hold)
    );

    template <action::type_e action_type>
    constexpr auto action_path = [](const node & n, auto button) -> node {
        constexpr auto act = action_map[action_type_c<action_type>];

        auto new_action = action { action_type, button };
        auto new_actions = n.actions.push_back(new_action);

        return { act(n.state, button), new_actions };
    };

    constexpr auto queue_reachable_paths = [](const node & node, open_set_t & set, auto button) {
        constexpr auto t_button = hana::decltype_(button);
        constexpr auto is_holdable = buttons::traits::is_holdable(t_button);

        set.push(action_path<action::type_e::Press>(node, button));
        if constexpr (is_holdable)
            set.push(action_path<action::type_e::Hold>(node, button));
    };
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG, www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also tools/license/license.mtl.txt in the distribution.

#ifndef MTL_VEC_SQRT_INCLUDE
#define MTL_VEC_SQRT_INCLUDE

#include <boost/numeric/mtl/vector/map_view.hpp>

namespace mtl { namespace vec {

    /// Square root of \a v element-wise
    template <typename Vector>
    sqrt_view<Vector> sqrt(const Vector& v)
    {
        return sqrt_view<Vector>(v);
    }

    /// Inverse square root of \a v element-wise
    template <typename Vector>
    rsqrt_view<Vector> rsqrt(const Vector& v)
    {
        return rsqrt_view<Vector>(v);
    }

}} // namespace mtl::vec

#endif // MTL_VEC_SQRT_INCLUDE


//  Copyright (c) 2017 Hartmut Kaiser
//
//  SPDX-License-Identifier: BSL-1.0
//  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)

#pragma once

#include <hpx/config.hpp>

#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/statistics/rolling_window.hpp>
#include <boost/accumulators/statistics_fwd.hpp>

namespace hpx { namespace util { namespace detail {
    template <typename Sample>
    struct rolling_min_impl : boost::accumulators::accumulator_base
    {
        typedef Sample float_type;

        // for boost::result_of
        typedef float_type result_type;

        template <typename Args>
        rolling_min_impl(Args const& /* args */)
          : min_(boost::numeric::as_max(Sample()))
          , is_dirty_(false)
        {
        }

        template <typename Args>
        void operator()(Args const& args)
        {
            boost::accumulators::find_accumulator<
                boost::accumulators::tag::rolling_window>(
                args[boost::accumulators::accumulator])(args);
            is_dirty_ = true;
        }

        template <typename Args>
        result_type result(Args const& args) const
        {
            if (is_dirty_)
            {
                using namespace boost::accumulators;

                min_ = boost::numeric::as_max(Sample());

                // work around problem in older Boost versions
                auto r = rolling_window_plus1(args);
                bool full = impl::is_rolling_window_plus1_full(args);

                for (auto const& s : r.advance_begin(full ? 1 : 0))
                {
                    boost::numeric::min_assign(min_, s);
                }

                is_dirty_ = false;
            }
            return min_;
        }

    private:
        mutable Sample min_;
        mutable bool is_dirty_;
    };
}}}    // namespace hpx::util::detail

///////////////////////////////////////////////////////////////////////////////
// tag::rolling_min
namespace boost { namespace accumulators {
    namespace tag {
        struct rolling_min : depends_on<rolling_window>
        {
            struct impl
            {
                template <typename Sample, typename Weight>
                struct apply
                {
                    typedef hpx::util::detail::rolling_min_impl<Sample> type;
                };
            };
        };
    }    // namespace tag

    ///////////////////////////////////////////////////////////////////////////////
    // extract::rolling_min
    namespace extract {
        extractor<tag::rolling_min> const rolling_min = {};
    }
}}    // namespace boost::accumulators

namespace hpx { namespace util {
    namespace tag {
        using boost::accumulators::tag::rolling_min;
    }

    using boost::accumulators::extract::rolling_min;
}}    // namespace hpx::util


#pragma once

#include <type_traits>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <nlohmann/json.hpp>

struct type {
  template<typename T>
  using base_type = typename std::remove_cv<typename std::remove_reference<T>::type>::type;
  template<typename T>
  static constexpr const char *dtype() {
    if constexpr(std::is_same<base_type<T>, char>::value) { return "u1"; }
    if constexpr(std::is_same<base_type<T>, std::uint8_t>::value) { return "u1"; }
    else if constexpr(std::is_same<base_type<T>, std::uint16_t>::value) { return "u2"; }
    else if constexpr(std::is_same<base_type<T>, std::uint32_t>::value) { return "u4"; }
    else if constexpr(std::is_same<base_type<T>, std::uint64_t>::value) { return "u8"; }
    else if constexpr(std::is_same<base_type<T>, std::int8_t>::value) { return "i1"; }
    else if constexpr(std::is_same<base_type<T>, std::int16_t>::value) { return "i2"; }
    else if constexpr(std::is_same<base_type<T>, std::int32_t>::value) { return "i4"; }
    else if constexpr(std::is_same<base_type<T>, std::int64_t>::value) { return "i8"; }
    else if constexpr(std::is_same<base_type<T>, float>::value) { return "f4"; }
    else if constexpr(std::is_same<base_type<T>, double>::value) { return "f8"; }
    else if constexpr(std::is_same<base_type<T>, long double>::value) { return "f8"; }
    else { return "unknown type"; }
  }
};

namespace Eigen {
template<typename T>
void to_json(nlohmann::json &j, const T &matrix) {
  typename T::Index rows = matrix.rows(), cols = matrix.cols();
  auto data_begin = const_cast<typename T::Scalar *>(matrix.data());
  std::vector<typename T::Scalar> v(data_begin, data_begin + rows * cols);

  bool is_row_major = matrix.IsRowMajor;

  j = {
      {"dtype", type::dtype<typename T::Scalar>()},
      {"shape", {rows, cols}},
      {"data", v},
      {"order", is_row_major ? "C" : "F"}
  };
}
}


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/constant/nbdigits.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/as.hpp>
#include <limits>
#include <scalar_test.hpp>

STF_CASE_TPL( "Check inf behavior for integral types"
            , (std::uint8_t)(std::uint16_t)(std::uint32_t)(std::uint64_t)
              (std::int8_t)(std::int16_t)(std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::nbdigits;
  using boost::simd::Nbdigits;

  STF_TYPE_IS(decltype(Nbdigits<T>()), T);
  STF_EQUAL(Nbdigits<T>(), T(0));
}

STF_CASE_TPL( "Check inf behavior for float type"
            , (float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::nbdigits;
  using boost::simd::Nbdigits;

  STF_TYPE_IS(decltype(Nbdigits<T>()), boost::dispatch::as_integer_t<T>);
  STF_EQUAL(Nbdigits<T>(), 24 );
}
STF_CASE_TPL( "Check inf behavior for double type"
            , (double)
            )
{
  using boost::simd::as;
  using boost::simd::detail::nbdigits;
  using boost::simd::Nbdigits;

  STF_TYPE_IS(decltype(Nbdigits<T>()), boost::dispatch::as_integer_t<T>);
  STF_EQUAL(Nbdigits<T>(), 53 );
}


// Demonstrating the use of ranges built using a qi::spirit parser adapted to be a range directly through
// the range-v3 facade
#include <string>
#include <iostream>
#include <sstream>

#include <boost/spirit/include/qi.hpp>

#include <range/v3/all.hpp>
#include <range/v3/iterator/default_sentinel.hpp>

#include "nmea.h"

// create a Range that (underneath) uses Spirit to parse the "next" value
class gga_range_t : public ranges::view_facade<gga_range_t>
{
    friend ranges::range_access;
    struct cursor;

 public:

    typedef boost::spirit::istream_iterator iter_t;

    gga_range_t(std::istream& is) : m_stream(&is), m_it(*m_stream), m_parse_good(true) {
        using namespace boost::spirit;
        m_parser =
            qi::lit("$GPGGA") >> ',' >> qi::int_ >> ',' >>
            qi::double_ >> ',' >> qi::char_("NS") >> ',' >>
            qi::double_ >> ',' >> qi::char_("EW") >> ',' >>
            qi::int_ >> ',' >> qi::int_ >> ',' >>
            qi::double_ >> ',' >>
            qi::double_ >> ',' >> qi::char_ >> ',' >>
            qi::double_ >> ',' >> qi::char_ >> ',' >>
            -qi::int_ >> ',' >> -qi::int_ >> ",*" >> qi::int_ >> -qi::eol;
    }

private:
    cursor begin_cursor() {
        return cursor(*this);
    }

    gga_t cached() const {
        return m_g;
    }

    bool done() const {
        // EOF or something I don't recognize
        return !m_parse_good || !m_stream;
    }

    std::istream* m_stream;
    gga_t m_g;
    iter_t m_it;
    bool m_parse_good;
    boost::spirit::qi::rule<iter_t, gga_t()> m_parser;

    void next() {
        m_parse_good = boost::spirit::qi::parse(m_it, iter_t(), m_parser, m_g);
    }

    struct cursor {
        cursor(void) = default;
        explicit cursor(gga_range_t& rng) : m_rng(&rng) {
            next();    // to load the first value
        }

        void next() {
            m_rng->next();
        }

        gga_t read() const {
            return m_rng->cached();
        }

        bool equal(ranges::v3::default_sentinel_t) const {
            return m_rng->done();
        }

        gga_range_t* m_rng;
    };
};

std::ostream& operator<<(std::ostream& os, gga_t const& g) {
    os << '(' << g.latitude << g.lat_hemi << ',' << g.longitude << g.long_hemi << ')';
    return os;
}

int main() {
    std::string test("$GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,,*47");
    test +=        "\n$GPGGA,170834,4124.8963,N,08151.6838,W,1,05,1.5,280.2,M,-34.0,M,,,*59";

    std::istringstream ss(test);
    ss.unsetf(std::ios::skipws);
    using namespace ranges::v3;
    ranges::for_each(gga_range_t(ss),
                     [](gga_t const& g) {
                         std::cout << g << "\n";
                     });
}


/*

Copyright (c) 2005-2021, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

/*
Megan E. Marsh, Raymond J. Spiteri
Numerical Simulation Laboratory
University of Saskatchewan
December 2011
Partial support provided by research grants from the National
Science and Engineering Research Council (NSERC) of Canada
and the MITACS/Mprime Canadian Network of Centres of Excellence.
*/

#ifndef _GRL2IVPODESOLVER_HPP_
#define _GRL2IVPODESOLVER_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>
#include "AbstractOneStepIvpOdeSolver.hpp"

/**
 * A concrete one step ODE solver class that employs the GRL2 second-order solver.
 * Method is described in J. Sundnes, R. Artebrant, O. Skavhaug, and A. Tveito.
 * A second-order algorithm for solving dynamic cell membrane equations.
 * IEEE Trans. Biomed. Eng., 56(10):2546-2548, 2009.
 */
class GRL2IvpOdeSolver : public AbstractOneStepIvpOdeSolver
{
private:

    /** Needed for serialization. */
    friend class boost::serialization::access;
    /**
     * Archive the abstract IVP Solver, never used directly - boost uses this.
     *
     * @param archive the archive
     * @param version the current version of this class
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        // This calls serialize on the base class.
        archive & boost::serialization::base_object<AbstractOneStepIvpOdeSolver>(*this);
    }

    /** Working memory for the solver */
    std::vector<double> mEvalF;

    /** Working memory for the solver */
    std::vector<double> mPartialF;

    /** Working memory for the solver */
    std::vector<double> mTemp;

    /** Working memory for the solver */
    std::vector<double> mYinit;

protected:

    /**
     * Calculate the solution to the ODE system at the next timestep.
     *
     * @param pAbstractOdeSystem  the ODE system to solve
     * @param timeStep  dt
     * @param time  the current time
     * @param rCurrentYValues  the current (initial) state
     * @param rNextYValues  the state at the next timestep
     */
    void CalculateNextYValue(AbstractOdeSystem* pAbstractOdeSystem,
                             double timeStep,
                             double time,
                             std::vector<double>& rCurrentYValues,
                             std::vector<double>& rNextYValues);

public:

    /**
     * Constructor.
     */
    GRL2IvpOdeSolver()
    {}
};

#include "SerializationExportWrapper.hpp"
CHASTE_CLASS_EXPORT(GRL2IvpOdeSolver)

#endif //_GRL2IVPODESOLVER_HPP_


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

#ifndef NETKET_METROPOLISEXCHANGE_HPP
#define NETKET_METROPOLISEXCHANGE_HPP

#include <mpi.h>
#include <Eigen/Dense>
#include <iostream>
#include "Utils/parallel_utils.hpp"
#include "Utils/random_utils.hpp"
#include "abstract_sampler.hpp"

namespace netket {

// Metropolis sampling generating local exchanges
template <class WfType>
class MetropolisExchange : public AbstractSampler<WfType> {
  WfType &psi_;

  const Hilbert &hilbert_;

  // number of visible units
  const int nv_;

  netket::default_random_engine rgen_;

  // states of visible units
  Eigen::VectorXd v_;

  Eigen::VectorXd accept_;
  Eigen::VectorXd moves_;

  int mynode_;
  int totalnodes_;

  // clusters to do updates
  std::vector<std::vector<int>> clusters_;

  // Look-up tables
  typename WfType::LookupType lt_;

 public:
  template <class G>
  MetropolisExchange(G &graph, WfType &psi, int dmax = 1)
      : psi_(psi), hilbert_(psi.GetHilbert()), nv_(hilbert_.Size()) {
    Init(graph, dmax);
  }

  // Json constructor
  MetropolisExchange(Graph &graph, WfType &psi, const json &pars)
      : psi_(psi), hilbert_(psi.GetHilbert()), nv_(hilbert_.Size()) {
    int dmax = FieldOrDefaultVal(pars["Sampler"], "Dmax", 1);
    Init(graph, dmax);
  }

  template <class G>
  void Init(G &graph, int dmax) {
    v_.resize(nv_);

    MPI_Comm_size(MPI_COMM_WORLD, &totalnodes_);
    MPI_Comm_rank(MPI_COMM_WORLD, &mynode_);

    accept_.resize(1);
    moves_.resize(1);

    GenerateClusters(graph, dmax);

    Seed();

    Reset(true);

    InfoMessage() << "Metropolis Exchange sampler is ready " << std::endl;
    InfoMessage() << dmax << " is the maximum distance for exchanges"
                  << std::endl;
  }

  template <class G>
  void GenerateClusters(G &graph, int dmax) {
    auto dist = graph.Distances();

    assert(int(dist.size()) == nv_);

    for (int i = 0; i < nv_; i++) {
      for (int j = 0; j < nv_; j++) {
        if (dist[i][j] <= dmax && i != j) {
          clusters_.push_back({i, j});
        }
      }
    }
  }

  void Seed(int baseseed = 0) {
    std::random_device rd;
    std::vector<int> seeds(totalnodes_);

    if (mynode_ == 0) {
      for (int i = 0; i < totalnodes_; i++) {
        seeds[i] = rd() + baseseed;
      }
    }

    SendToAll(seeds);

    rgen_.seed(seeds[mynode_]);
  }

  void Reset(bool initrandom = false) override {
    if (initrandom) {
      if (initrandom) {
        hilbert_.RandomVals(v_, rgen_);
      }
    }

    psi_.InitLookup(v_, lt_);

    accept_ = Eigen::VectorXd::Zero(1);
    moves_ = Eigen::VectorXd::Zero(1);
  }

  void Sweep() override {
    std::vector<int> tochange(2);
    std::uniform_real_distribution<double> distu;
    std::uniform_int_distribution<int> distcl(0, clusters_.size() - 1);

    std::vector<double> newconf(2);

    for (int i = 0; i < nv_; i++) {
      int rcl = distcl(rgen_);
      assert(rcl < int(clusters_.size()));
      int si = clusters_[rcl][0];
      int sj = clusters_[rcl][1];

      assert(si < nv_ && sj < nv_);

      if (std::abs(v_(si) - v_(sj)) > std::numeric_limits<double>::epsilon()) {
        tochange = clusters_[rcl];
        newconf[0] = v_(sj);
        newconf[1] = v_(si);

        double ratio =
            std::norm(std::exp(psi_.LogValDiff(v_, tochange, newconf, lt_)));

        if (ratio > distu(rgen_)) {
          accept_[0] += 1;
          psi_.UpdateLookup(v_, tochange, newconf, lt_);
          hilbert_.UpdateConf(v_, tochange, newconf);
        }
      }
      moves_[0] += 1;
    }
  }

  Eigen::VectorXd Visible() override { return v_; }

  void SetVisible(const Eigen::VectorXd &v) override { v_ = v; }

  WfType &Psi() override { return psi_; }

  Eigen::VectorXd Acceptance() const override {
    Eigen::VectorXd acc = accept_;
    for (int i = 0; i < 1; i++) {
      acc(i) /= moves_(i);
    }
    return acc;
  }
};

}  // namespace netket

#endif


//
// Copyright (c) 2002--2010
// Toon Knapen, Karl Meerbergen, Kresimir Fresl,
// Thomas Klimpel and Rutger ter Borg
//
// 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)
//
// THIS FILE IS AUTOMATICALLY GENERATED
// PLEASE DO NOT EDIT!
//

#ifndef BOOST_NUMERIC_BINDINGS_BLAS_LEVEL1_SWAP_HPP
#define BOOST_NUMERIC_BINDINGS_BLAS_LEVEL1_SWAP_HPP

#include <boost/assert.hpp>
#include <boost/numeric/bindings/begin.hpp>
#include <boost/numeric/bindings/has_linear_array.hpp>
#include <boost/numeric/bindings/is_mutable.hpp>
#include <boost/numeric/bindings/remove_imaginary.hpp>
#include <boost/numeric/bindings/size.hpp>
#include <boost/numeric/bindings/stride.hpp>
#include <boost/numeric/bindings/value_type.hpp>
#include <boost/static_assert.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/remove_const.hpp>

//
// The BLAS-backend is selected by defining a pre-processor variable,
//  which can be one of
// * for CBLAS, define BOOST_NUMERIC_BINDINGS_BLAS_CBLAS
// * for CUBLAS, define BOOST_NUMERIC_BINDINGS_BLAS_CUBLAS
// * netlib-compatible BLAS is the default
//
#if defined BOOST_NUMERIC_BINDINGS_BLAS_CBLAS
#include <boost/numeric/bindings/blas/detail/cblas.h>
#include <boost/numeric/bindings/blas/detail/cblas_option.hpp>
#elif defined BOOST_NUMERIC_BINDINGS_BLAS_CUBLAS
#include <boost/numeric/bindings/blas/detail/cublas.h>
#include <boost/numeric/bindings/blas/detail/blas_option.hpp>
#else
#include <boost/numeric/bindings/blas/detail/blas.h>
#include <boost/numeric/bindings/blas/detail/blas_option.hpp>
#endif

namespace boost {
namespace numeric {
namespace bindings {
namespace blas {

//
// The detail namespace contains value-type-overloaded functions that
// dispatch to the appropriate back-end BLAS-routine.
//
namespace detail {

#if defined BOOST_NUMERIC_BINDINGS_BLAS_CBLAS
//
// Overloaded function for dispatching to
// * CBLAS backend, and
// * float value-type.
//
inline void swap( const int n, float* x, const int incx, float* y,
        const int incy ) {
    cblas_sswap( n, x, incx, y, incy );
}

//
// Overloaded function for dispatching to
// * CBLAS backend, and
// * double value-type.
//
inline void swap( const int n, double* x, const int incx, double* y,
        const int incy ) {
    cblas_dswap( n, x, incx, y, incy );
}

//
// Overloaded function for dispatching to
// * CBLAS backend, and
// * complex<float> value-type.
//
inline void swap( const int n, std::complex<float>* x, const int incx,
        std::complex<float>* y, const int incy ) {
    cblas_cswap( n, x, incx, y, incy );
}

//
// Overloaded function for dispatching to
// * CBLAS backend, and
// * complex<double> value-type.
//
inline void swap( const int n, std::complex<double>* x, const int incx,
        std::complex<double>* y, const int incy ) {
    cblas_zswap( n, x, incx, y, incy );
}

#elif defined BOOST_NUMERIC_BINDINGS_BLAS_CUBLAS
//
// Overloaded function for dispatching to
// * CUBLAS backend, and
// * float value-type.
//
inline void swap( const int n, float* x, const int incx, float* y,
        const int incy ) {
    cublasSswap( n, x, incx, y, incy );
}

//
// Overloaded function for dispatching to
// * CUBLAS backend, and
// * double value-type.
//
inline void swap( const int n, double* x, const int incx, double* y,
        const int incy ) {
    cublasDswap( n, x, incx, y, incy );
}

//
// Overloaded function for dispatching to
// * CUBLAS backend, and
// * complex<float> value-type.
//
inline void swap( const int n, std::complex<float>* x, const int incx,
        std::complex<float>* y, const int incy ) {
    cublasCswap( n, x, incx, y, incy );
}

//
// Overloaded function for dispatching to
// * CUBLAS backend, and
// * complex<double> value-type.
//
inline void swap( const int n, std::complex<double>* x, const int incx,
        std::complex<double>* y, const int incy ) {
    cublasZswap( n, x, incx, y, incy );
}

#else
//
// Overloaded function for dispatching to
// * netlib-compatible BLAS backend (the default), and
// * float value-type.
//
inline void swap( const fortran_int_t n, float* x, const fortran_int_t incx,
        float* y, const fortran_int_t incy ) {
    BLAS_SSWAP( &n, x, &incx, y, &incy );
}

//
// Overloaded function for dispatching to
// * netlib-compatible BLAS backend (the default), and
// * double value-type.
//
inline void swap( const fortran_int_t n, double* x, const fortran_int_t incx,
        double* y, const fortran_int_t incy ) {
    BLAS_DSWAP( &n, x, &incx, y, &incy );
}

//
// Overloaded function for dispatching to
// * netlib-compatible BLAS backend (the default), and
// * complex<float> value-type.
//
inline void swap( const fortran_int_t n, std::complex<float>* x,
        const fortran_int_t incx, std::complex<float>* y,
        const fortran_int_t incy ) {
    BLAS_CSWAP( &n, x, &incx, y, &incy );
}

//
// Overloaded function for dispatching to
// * netlib-compatible BLAS backend (the default), and
// * complex<double> value-type.
//
inline void swap( const fortran_int_t n, std::complex<double>* x,
        const fortran_int_t incx, std::complex<double>* y,
        const fortran_int_t incy ) {
    BLAS_ZSWAP( &n, x, &incx, y, &incy );
}

#endif

} // namespace detail

//
// Value-type based template class. Use this class if you need a type
// for dispatching to swap.
//
template< typename Value >
struct swap_impl {

    typedef Value value_type;
    typedef typename remove_imaginary< Value >::type real_type;
    typedef void result_type;

    //
    // Static member function that
    // * Deduces the required arguments for dispatching to BLAS, and
    // * Asserts that most arguments make sense.
    //
    template< typename VectorX, typename VectorY >
    static result_type invoke( VectorX& x, VectorY& y ) {
        namespace bindings = ::boost::numeric::bindings;
        BOOST_STATIC_ASSERT( (is_same< typename remove_const<
                typename bindings::value_type< VectorX >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorY >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (bindings::has_linear_array< VectorX >::value) );
        BOOST_STATIC_ASSERT( (bindings::has_linear_array< VectorY >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorX >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorY >::value) );
        detail::swap( bindings::size(x), bindings::begin_value(x),
                bindings::stride(x), bindings::begin_value(y),
                bindings::stride(y) );
    }
};

//
// Functions for direct use. These functions are overloaded for temporaries,
// so that wrapped types can still be passed and used for write-access. Calls
// to these functions are passed to the swap_impl classes. In the
// documentation, the const-overloads are collapsed to avoid a large number of
// prototypes which are very similar.
//

//
// Overloaded function for swap. Its overload differs for
//
template< typename VectorX, typename VectorY >
inline typename swap_impl< typename bindings::value_type<
        VectorX >::type >::result_type
swap( VectorX& x, VectorY& y ) {
    swap_impl< typename bindings::value_type<
            VectorX >::type >::invoke( x, y );
}

} // namespace blas
} // namespace bindings
} // namespace numeric
} // namespace boost

#endif


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

#ifndef __sot_torque_control_TorqueOffsetEstimator_H__
#define __sot_torque_control_TorqueOffsetEstimator_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

#if defined (WIN32)
#  if defined (torque_offset_estimator_EXPORTS)
#    define TORQUEOFFSETESTIMATOR_EXPORT __declspec(dllexport)
#  else
#    define TORQUEOFFSETESTIMATOR_EXPORT __declspec(dllimport)
#  endif
#else
#  define TORQUEOFFSETESTIMATOR_EXPORT
#endif

//#define VP_DEBUG 1        /// enable debug output
//#define VP_DEBUG_MODE 20

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

/* HELPER */
#include <sot/torque_control/signal-helper.hh>
#include <sot/torque_control/utils/stop-watch.hh>
#include <sot/torque_control/utils/logger.hh>
#include <sot/torque_control/common.hh>
#include <boost/circular_buffer.hpp>
#include <Eigen/StdVector>

/*Motor model*/
#include <sot/torque_control/motor-model.hh>
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
namespace dynamicgraph {
  namespace sot {
    namespace torque_control {

      class TORQUEOFFSETESTIMATOR_EXPORT TorqueOffsetEstimator
          :public ::dynamicgraph::Entity
      {
        DYNAMIC_GRAPH_ENTITY_DECL();

      public:  /* --- SIGNALS --- */
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
          typedef int dummy;
        typedef
          std::vector<Eigen::VectorXd,Eigen::aligned_allocator<Eigen::VectorXd> > stdAlignedVector;

        /** --- CONSTRUCTOR ---- */
        TorqueOffsetEstimator( const std::string & name );
        void init(const std::string &urdfFile,
                  const Eigen::Matrix4d& _m_torso_X_imu,
                  const double& gyro_epsilon,
                  const std::string& ffJointName,
                  const std::string& torsoJointName);
        void computeOffset(const int& nIterations, const double& epsilon);

        virtual void display( std::ostream& os ) const;

        DECLARE_SIGNAL_IN(base6d_encoders,          dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(accelerometer,            dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(gyroscope,                dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(jointTorques,             dynamicgraph::Vector);
        DECLARE_SIGNAL_OUT(jointTorquesEstimated,   dynamicgraph::Vector);
        DECLARE_SIGNAL_INNER(collectSensorData,     dummy);

      protected:
        RobotUtil *       m_robot_util;
        se3::Model        m_model;            /// Pinocchio robot model
        se3::Data         *m_data;            /// Pinocchio robot data
        int n_iterations;   //Number of iterations to consider
        double epsilon;
        double gyro_epsilon;

        int ffIndex, torsoIndex;  //Index of the free-flyer and torso frames
        Eigen::VectorXd jointTorqueOffsets;
        se3::SE3 m_torso_X_imu; // Definition of the imu in the chest frame.

        // stdAlignedVector encSignals;
        // stdAlignedVector accSignals;
        // stdAlignedVector gyrSignals;
        // stdAlignedVector tauSignals;

        // stdAlignedVector stdVecJointTorqueOffsets;

        void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
        {
          getLogger().sendMsg("["+name+"] "+msg, t, file, line);
        }

      private:
        enum {
          PRECOMPUTATION,
          INPROGRESS,
          COMPUTED } sensor_offset_status;

        // void calculateSensorOffsets();
        int current_progress;
      }; // class TorqueOffsetEstimator

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

#endif // #ifndef __sot_torque_control_TorqueOffsetEstimator_H__


#include <boost/functional/hash.hpp>
#include "common/help.h"
#include "common/time.h"
#include "common/type.h"

#include <fstream>
#include <iostream>
#include <algorithm>
#include <vector>
#include <string>

#include <cstdlib>
#include <cstring>
#include <cassert>

using namespace std;

long long cand_num;
long long res_num;

const int Q = 1;

int THRESHOLD;
int N1 = 2;
int N2 = 6;
string CHARSET = " `~!@#$%^&*()-_+=[]{};:<>,./?\\|'\"abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ1234567890";

int char_table[256];
int power[Q + 1];
int N, K, K2;

int **range_start;
int **range_end;

vector<vector<int>> subs;
int sig_len;

void prepare_power()
{
	power[0] = 1;
	for (int k = 1; k <= Q; k++)
		power[k] = power[k - 1] * CHARSET.length();

	N = power[Q];
	K = 2 * THRESHOLD * Q;
	K2 = (K + 1) / N1 - 1;

	// important fix
	if ((K + 1) % N1 != 0) K2++;

	if (N1 > K + 1 || N1 * N2 <= K + 1)
	{
		exit(-3);
	}
}

void prepare_char_table()
{
	random_shuffle(CHARSET.begin(), CHARSET.end());
	for (int k = 0; k < 256; k++)
	{
		string::size_type pos = CHARSET.find((char)k);
		if (pos == string::npos)
		{
			char_table[k] = -1;
		} else {
			char_table[k] = pos;
		}
	}
}

void prepare_range()
{
	int *t = new int[N1 * N2];
	range_start = new int*[N1];
	for (int k = 0; k < N1; k++)
		range_start[k] = t + k * N2;

	t = new int[N1 * N2];
	range_end = new int*[N1];
	for (int k = 0; k < N1; k++)
		range_end[k] = t + k * N2;

	for (int i = 0; i < N1; i++)
		for (int j = 0; j < N2; j++)
		{
			range_start[i][j] = N * (N2 * i + j) / N1 / N2;
			range_end[i][j] = N * (N2 * i + j + 1) / N1 / N2;
		}
}

void prepare_subsets(int n, int k)
{
	vector<int> sub(k);

	int i;
	for (i = 0; i < k; i++)
		sub[i] = i;
	subs.push_back(sub);

	while (sub[0] < n - k) {
		for (i = 0; i < k; i++)
			if (sub[k - i - 1] < n - i - 1)
				break;
		i = k - i - 1;
		sub[i]++;
		i++;
		for (; i < k; i++)
			sub[i] = sub[i - 1] + 1;
		subs.push_back(sub);
	}

	sig_len = N1 * subs.size();
}

void convert_to_token(vector<string> &input, vector<vector<int>> &token)
{
	for (vector<string>::iterator iter = input.begin();
			iter != input.end(); iter++)
	{
		string &line = *iter;
		assert((int)line.length() >= Q);

		Set<int> unique_token;

		// change token algorithm here to switch ed/jaccard
		for (int start = 0; start < (int)line.length() - Q + 1; start++)
		{
			int num = 0;
			for (int p = 0; p < Q; p++)
			{
				int pos = char_table[(unsigned char)(line[start + p])];
				num += power[p] * pos;
				assert(pos != -1);
			}
			unique_token.insert(num);
		}

		token.push_back(vector<int>());
		token.back().insert(token.back().end(), unique_token.begin(), unique_token.end());
	}
}

void convert_to_signature(vector<string> &input, vector<vector<int>> &token, HashMap<int, vector<int>> *sig_map)
{
	boost::hash<vector<int>> vector_hash;
	bool *checked_flag = new bool[input.size()];
	for (int id = 0; id < (int)token.size(); id++)
	{
		int index = -1;
		vector<int> prepare;
		memset(checked_flag, 0, id);

		for (int i = 0; i < N1; i++)
			for (vector<vector<int>>::iterator sub = subs.begin(); sub != subs.end(); sub++)
			{
				index++;
				prepare.clear();
				for (vector<int>::iterator j = sub->begin(); j != sub->end(); j++)
				{
					vector<int>::iterator s_pos = lower_bound(token[id].begin(), token[id].end(), range_start[i][*j]);
					vector<int>::iterator e_pos = lower_bound(token[id].begin(), token[id].end(), range_end[i][*j]);
					while (s_pos != e_pos)
					{
						prepare.push_back(*s_pos);
						s_pos++;
					}
				}

				int hash_value = vector_hash(prepare);
				vector<int> &candidate = sig_map[index][hash_value];
				for (vector<int>::iterator cand_id = candidate.begin(); cand_id != candidate.end(); cand_id++)
				{
					if (!checked_flag[*cand_id])
					{
						cand_num++;
						checked_flag[*cand_id] = 1;
						if (edit_distance(input[*cand_id], input[id], THRESHOLD) <= THRESHOLD)
						{
							res_num++;
						}
					}
				}
				candidate.push_back(id);
			}
	}
	delete[] checked_flag;
}

int main(int argc, char **argv)
{
	if (argc != 5) return -1;
	log_start();

	ifstream in(argv[1]);
	THRESHOLD = atoi(argv[2]);
	N1 = atoi(argv[3]);
	N2 = atoi(argv[4]);

	assert(Q <= 4);

	string line;
	vector<string> input;
	while (getline(in, line))
	{
		input.push_back(line);
	}
	in.close();

	prepare_power();
	prepare_char_table();
	prepare_subsets(N2, N2 - K2);
	prepare_range();

	vector<vector<int>> token;
	convert_to_token(input, token);

	HashMap<int, vector<int>> sig_map[sig_len];
	convert_to_signature(input, token, sig_map);

	cout << res_num << endl;
	cout << cand_num << endl;

	return 0;
}


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

#include "theia/sfm/estimate_track.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <memory>
#include <vector>

#include "theia/math/util.h"
#include "theia/sfm/bundle_adjustment/bundle_adjustment.h"
#include "theia/sfm/estimators/estimate_triangulation.h"
#include "theia/sfm/feature.h"
#include "theia/sfm/reconstruction.h"
#include "theia/sfm/reconstruction_estimator_utils.h"
#include "theia/sfm/track.h"
#include "theia/sfm/triangulation/triangulation.h"
#include "theia/sfm/types.h"
#include "theia/util/map_util.h"
#include "theia/util/threadpool.h"

namespace theia {

namespace {

void GetObservationsFromTrackViews(
    const TrackId track_id,
    const Reconstruction& reconstruction,
    std::vector<ViewId>* view_ids,
    std::vector<Eigen::Vector2d>* features,
    std::vector<Eigen::Vector3d>* origins,
    std::vector<Eigen::Vector3d>* ray_directions) {
  const Track* track = reconstruction.Track(track_id);
  for (const ViewId view_id : track->ViewIds()) {
    const View* view = reconstruction.View(view_id);

    // Skip this view if it does not exist or has not been estimated yet.
    if (view == nullptr || !view->IsEstimated()) {
      continue;
    }

    // If the feature is not in the view then we have an ill-formed
    // reconstruction.
    const Feature* feature = CHECK_NOTNULL(view->GetFeature(track_id));
    const Eigen::Vector3d image_ray =
        view->Camera().PixelToUnitDepthRay(*feature).normalized();

    features->emplace_back(*feature);
    view_ids->emplace_back(view_id);
    origins->emplace_back(view->Camera().GetPosition());
    ray_directions->emplace_back(image_ray);
  }
}

// Returns false if the reprojection error of the triangulated point is greater
// than the max allowable reprojection error (for any observation) and true
// otherwise.
bool AcceptableReprojectionError(
    const Reconstruction& reconstruction,
    const TrackId& track_id,
    const std::vector<ViewId>& view_ids,
    const std::vector<Eigen::Vector2d>& features,
    const double sq_max_reprojection_error_pixels) {
  const Track& track = *reconstruction.Track(track_id);
  int num_projections = 0;
  double mean_sq_reprojection_error = 0;
  for (int i = 0; i < view_ids.size(); i++) {
    // We do not need to check if the view is present or estimated, since this
    // was done prior to the input.
    const View* view = reconstruction.View(view_ids[i]);
    const Camera& camera = view->Camera();
    Eigen::Vector2d reprojection;
    if (camera.ProjectPoint(track.Point(), &reprojection) < 0) {
      return false;
    }

    mean_sq_reprojection_error += (features[i] - reprojection).squaredNorm();
    ++num_projections;
  }

  return (mean_sq_reprojection_error / static_cast<double>(num_projections)) <
         sq_max_reprojection_error_pixels;
}

}  // namespace

// Estimate only the tracks supplied by the user.
TrackEstimator::Summary TrackEstimator::EstimateAllTracks() {
  // Estimate all tracks that are seen by estimated views.
  const auto& view_ids = reconstruction_->ViewIds();
  std::unordered_set<TrackId> tracks;
  for (const ViewId view_id : view_ids) {
    const View* view = reconstruction_->View(view_id);
    if (view == nullptr || !view->IsEstimated()) {
      continue;
    }
    const auto& tracks_in_view = view->TrackIds();
    tracks.insert(tracks_in_view.begin(), tracks_in_view.end());
  }

  return EstimateTracks(tracks);
}

TrackEstimator::Summary TrackEstimator::EstimateTracks(
    const std::unordered_set<TrackId>& track_ids) {
  tracks_to_estimate_.clear();
  summary_ = TrackEstimator::Summary();
  num_bad_angles_ = 0;
  num_failed_triangulations_ = 0;
  num_bad_reprojections_ = 0;

  // Get all unestimated track ids.
  tracks_to_estimate_.reserve(track_ids.size());
  for (const TrackId track_id : track_ids) {
    Track* track = reconstruction_->MutableTrack(track_id);
    if (!track->IsEstimated()) {
      tracks_to_estimate_.emplace_back(track_id);
    }
  }
  summary_.input_num_estimated_tracks =
      track_ids.size() - tracks_to_estimate_.size();
  summary_.num_triangulation_attempts = tracks_to_estimate_.size();

  // Exit early if there are no tracks to estimate.
  if (tracks_to_estimate_.size() == 0) {
    return summary_;
  }

  // Estimate the tracks in parallel. Instead of 1 threadpool worker per track,
  // we let each worker estimate a fixed number of tracks at a time (e.g. 20
  // tracks). Since estimating the tracks is so fast, this strategy is better
  // helps speed up multithreaded estimation by reducing the overhead of
  // starting/stopping threads.
  const int num_threads = std::min(
      options_.num_threads, static_cast<int>(tracks_to_estimate_.size()));
  const int interval_step =
      std::min(options_.multithreaded_step_size,
               static_cast<int>(tracks_to_estimate_.size()) / num_threads);

  std::unique_ptr<ThreadPool> pool(new ThreadPool(num_threads));
  for (int i = 0; i < tracks_to_estimate_.size(); i += interval_step) {
    const int end_interval = std::min(
        static_cast<int>(tracks_to_estimate_.size()), i + interval_step);
    pool->Add(&TrackEstimator::EstimateTrackSet, this, i, end_interval);
  }

  // Wait for all tracks to be estimated.
  pool.reset(nullptr);

  LOG(INFO) << summary_.estimated_tracks.size() << " tracks were estimated of "
            << summary_.num_triangulation_attempts << " possible tracks. "
            << num_bad_angles_
            << " triangulations failed due to bad triangulation angles and "
            << num_bad_reprojections_
            << " triangulations failed with too high reprojection errors.";
  return summary_;
}

void TrackEstimator::EstimateTrackSet(const int start, const int end) {
  std::unordered_set<TrackId> estimated_tracks;
  for (int i = start; i < end; i++) {
    if (EstimateTrack(tracks_to_estimate_[i])) {
      estimated_tracks.emplace(tracks_to_estimate_[i]);
    }
  }

  // Add the estimated tracks to the output summary.
  std::lock_guard<std::mutex> guard(summary_mutex_);
  summary_.estimated_tracks.insert(estimated_tracks.begin(),
                                   estimated_tracks.end());
}

bool TrackEstimator::EstimateTrack(const TrackId track_id) {
  static const int kMinNumObservationsForTriangulation = 2;

  Track* track = reconstruction_->MutableTrack(track_id);
  CHECK(!track->IsEstimated()) << "Track " << track_id
                               << " is already estimated.";

  // Gather projection matrices and features.
  std::vector<ViewId> view_ids;
  std::vector<Eigen::Vector2d> features;
  std::vector<Eigen::Vector3d> origins, ray_directions;
  GetObservationsFromTrackViews(track_id,
                                *reconstruction_,
                                &view_ids,
                                &features,
                                &origins,
                                &ray_directions);

  // Check the angle between views.
  if (view_ids.size() < kMinNumObservationsForTriangulation ||
      !SufficientTriangulationAngle(ray_directions,
                                    options_.min_triangulation_angle_degrees)) {
    ++num_bad_angles_;
    return false;
  }

  // Triangulate the track.
  if (!TriangulateMidpoint(origins, ray_directions, track->MutablePoint())) {
    ++num_failed_triangulations_;
    return false;
  }

  // Bundle adjust the track.
  if (options_.bundle_adjustment) {
    track->SetEstimated(true);
    const BundleAdjustmentSummary summary =
        BundleAdjustTrack(options_.ba_options, track_id, reconstruction_);
    track->SetEstimated(false);
    if (!summary.success) {
      return false;
    }
  }

  // Ensure the reprojection errors are acceptable.
  const double sq_max_reprojection_error_pixels =
      options_.max_acceptable_reprojection_error_pixels *
      options_.max_acceptable_reprojection_error_pixels;

  if (!AcceptableReprojectionError(*reconstruction_,
                                   track_id,
                                   view_ids,
                                   features,
                                   sq_max_reprojection_error_pixels)) {
    ++num_bad_reprojections_;
    return false;
  }

  track->SetEstimated(true);
  return true;
}

}  // namespace theia


#include "./pybind11/include/pybind11/pybind11.h"
#include "./pybind11/include/pybind11/stl_bind.h"
#include "./pybind11/include/pybind11/numpy.h"

#include <algorithm>
#include <dogm/dogm.h>
#include <dogm/dogm_types.h>
#include <dogm/mapping/laser_to_meas_grid.h>
#include <iostream>

#include <Eigen/Dense>

namespace py = pybind11;

PYBIND11_MAKE_OPAQUE(std::vector<float>);
PYBIND11_MAKE_OPAQUE(std::vector<dogm::GridCell>);

namespace dogm {

  // https://stackoverflow.com/questions/48982143/returning-and-passing-around-raw-pod-pointers-arrays-with-python-c-and-pyb
  template <class T> class ptr_wrapper
  {
  public:
    ptr_wrapper() : ptr(nullptr) {}
    ptr_wrapper(T* ptr) : ptr(ptr) {}
    ptr_wrapper(const ptr_wrapper& other) : ptr(other.ptr) {}
    T& operator* () const { return *ptr; }
    T* operator->() const { return  ptr; }
    T* get() const { return ptr; }
    void destroy() { delete ptr; }
    T& operator[](std::size_t idx) const { return ptr[idx]; }
  private:
    T* ptr;
  };

  ptr_wrapper<MeasurementCell> wrap_laser_grid( LaserMeasurementGrid& lmg, const std::vector<float>& measurements ) {
    ptr_wrapper<MeasurementCell> ptr(lmg.generateGrid( measurements ));
    return ptr;
  }

  void wrap_dogm_grid( DOGM& grid, ptr_wrapper<MeasurementCell>& ptr, float x, float y, float yaw, float dt, bool device){
    grid.updateGrid( ptr.get(), x, y, yaw, dt, device );
  }

  float pignisticTransformation(float free_mass, float occ_mass) {
    return occ_mass + 0.5f * (1.0f - occ_mass - free_mass);
  }

  void hsvToRGB(float hue, float saturation, float value, float &r, float &g, float &b) {

    if (saturation == 0.0f) {
      r = g = b = value;
    } else {
      int i = static_cast<int>(hue * 6.0f);
      float f = (hue * 6.0f) - i;
      float p = value * (1.0f - saturation);
      float q = value * (1.0f - saturation * f);
      float t = value * (1.0f - saturation * (1.0f - f));
      int res = i % 6;

      switch (res) {
        case 0:
          r = value;
          g = t;
          b = p;
          break;
        case 1:
          r = q;
          g = value;
          b = p;
          break;
        case 2:
          r = p;
          g = value;
          b = t;
          break;
        case 3:
          r = p;
          g = q;
          b = value;
          break;
        case 4:
          r = t;
          g = p;
          b = value;
          break;
        case 5:
          r = value;
          g = p;
          b = q;
          break;
        default:
          r = g = b = value;
      }
    }
  }

  py::array_t<float> render_dynamic_occupancy_grid( const DOGM& grid, double occupancy_threshold,
                                                            double dynamic_threshold, double max_velocity ) {
    auto width = grid.getGridSize();
    auto height = grid.getGridSize();
    auto depth = 3;
    float *grid_ptr = new float[ depth * width * height ];

    const auto grid_cells = grid.getGridCells();
    #pragma omp parallel for
    for( int y = 0; y < height; y++ ) {
      for( int x = 0; x < width; x++ ) {
        auto cell = grid_cells[y*width + x];
        float occ = pignisticTransformation(cell.free_mass, cell.occ_mass);

        Eigen::Vector2f vel;
        vel << cell.mean_x_vel, cell.mean_y_vel;

        Eigen::Matrix2f covar;
        covar << cell.var_x_vel, cell.covar_xy_vel, cell.covar_xy_vel, cell.var_y_vel;

        auto mdist = vel.transpose() * covar.inverse() * vel;

        float r, g, b;
        if (occ >= occupancy_threshold && mdist >= dynamic_threshold ) {
          float angle = atan2(cell.mean_y_vel, cell.mean_x_vel) + M_PI;

          auto value = std::min(1.0, sqrt(cell.mean_x_vel * cell.mean_x_vel +
                                          cell.mean_y_vel * cell.mean_y_vel) / max_velocity );

          hsvToRGB(angle / M_PI, 1.0f, value, r, g, b);
        } else {
          r = g = b = 1.0f - occ;
        }
        grid_ptr[ depth * ( y*width + x ) ] = r;
        grid_ptr[ depth * ( y*width + x ) + 1 ] = g;
        grid_ptr[ depth * ( y*width + x ) + 2 ] = b;
      }
    }

    // Create a Python object that will free the allocated
    // memory when destroyed:
    py::capsule delete_fn(grid_ptr, [](void *p) {
      float *ptr = reinterpret_cast<float *>(p);
      delete[] ptr;
    });

    return py::array_t<float>(
            {height, width, depth}, // shape
            {depth*width*sizeof(float), depth*sizeof(float), sizeof(float)}, // C-style contiguous strides
            grid_ptr, // the data pointer
            delete_fn); // numpy array references this parent
  }

  py::array_t<float> render_occupancy_grid( const DOGM& grid ) {
    auto width = grid.getGridSize();
    auto height = grid.getGridSize();
    float *grid_ptr = new float[ width * height ];

    const auto grid_cells = grid.getGridCells();
  #pragma omp parallel for
    for( int y = 0; y < height; y++ ) {
      for( int x = 0; x < width; x++ ) {
        auto cell = grid_cells[y*width + x];
        float prob = pignisticTransformation( cell.free_mass, cell.occ_mass );
        grid_ptr[ y*width + x ] = 1.0f - prob;
      }
    }

    // Create a Python object that will free the allocated
    // memory when destroyed:
    py::capsule delete_fn(grid_ptr, [](void *p) {
      float *ptr = reinterpret_cast<float *>(p);
      delete[] ptr;
    });

    return py::array_t<float>(
            {height, width}, // shape
            {width*sizeof(float), sizeof(float)}, // C-style contiguous strides
            grid_ptr, // the data pointer
            delete_fn); // numpy array references this parent
  }

  PYBIND11_MODULE(dogm_py, m) {
    // bind a vector of floats to pass into the laser measurement grid
    py::bind_vector<std::vector<float>>(m, "VectorFloat", py::buffer_protocol());

    m.doc() = "Python bindings for the Dynamic Occupancy Grid Map Library";

    py::class_<ptr_wrapper<MeasurementCell>>(m, "MeasurementCellPtr")
      .def(py::init<>(), "Wrapper for CUDA pointer returned by LaserMeasurementGrid" );

    m.def( "generateMeasurements", &wrap_laser_grid, py::arg( "laser_measurement_grid"),
           py::arg("measurements") );

    m.def( "updateGrid", &wrap_dogm_grid, "Wrapper function for grid update",
           py::arg("grid"), py::arg("measurement_ptr"), py::arg("x"),
           py::arg("y"), py::arg("yaw"),
           py::arg("dt"), py::arg("device"));

    py::class_<LaserMeasurementGrid::Params>(m, "LaserMeasurementGridParams")
            .def(py::init<float, float, float, float>(),
                 R"lmgPARAM(
    struct Params {
      float max_range;
      float resolution;
      float fov;
      float angle_increment;
    };
                        )lmgPARAM", py::arg("max_range"), py::arg("resolution"),
                        py::arg("fov"), py::arg("angle_increment"))
            .def_readwrite("max_range", &LaserMeasurementGrid::Params::max_range)
            .def_readwrite("resolution", &LaserMeasurementGrid::Params::resolution)
            .def_readwrite("fov", &LaserMeasurementGrid::Params::fov)
            .def_readwrite("angle_increment", &LaserMeasurementGrid::Params::angle_increment)
            ;

    py::class_<LaserMeasurementGrid>(m, "LaserMeasurementGrid")
            .def(py::init<const LaserMeasurementGrid::Params&, float, float>(),
                 py::arg("params"), py::arg("size"), py::arg("resolution"))
            .def("generateGrid", &LaserMeasurementGrid::generateGrid, "Create a grid representation of supplied laser measurements",
                 py::arg("measurements"));

    py::class_<DOGM::Params>(m, "DOGMParams")
            .def(py::init<float, float, int, int, float, float, float, float, float, float>(),
                    R"DogmPARAM(
    struct Params {
      float size;                          // Grid size [m]
      float resolution;                    // Grid cell size [m/cell]
      int particle_count;                  // Number of persistent particles
      int new_born_particle_count;         // Number of birth particles
      float persistence_prob;              // Probability of persistence
      float stddev_process_noise_position; // Process noise position
      float stddev_process_noise_velocity; // Process noise velocity
      float birth_prob;                    // Probability of birth
      float stddev_velocity;               // Velocity to sample birth particles from (normal distribution) [m/s]
      float init_max_velocity;             // Velocity to sample the initial particles from (uniform distribution) [m/s]
    }
                        )DogmPARAM", py::arg("size"),py::arg("resolution"),py::arg("particle_count"),
                   py::arg("new_born_particle_count"),py::arg("persistance_prob"),py::arg("stddev_process_noise_position"),
                   py::arg("stddev_process_noise_velocity"), py::arg("birth_prob"), py::arg("stddev_velocity"),
                   py::arg("init_max_velocity"));

    // define a vector of GridCells for return types
    py::class_<GridCell>(m, "GridCell")
            .def(py::init<>())
            .def_readwrite("occ_mass", &GridCell::occ_mass)
            .def_readwrite("free_mass", &GridCell::free_mass);

    py::bind_vector<std::vector<GridCell>>(m, "VectorGridCell");

    py::class_<DOGM>(m, "DOGM")
            .def(py::init<const DOGM::Params&>(), py::arg("params") )
            .def("updateGrid", &DOGM::updateGrid, "Update the current state with constructed measurement grid",
                 py::arg("cellData"), py::arg("x"), py::arg("y"), py::arg("yaw"), py::arg("dt"), py::arg("device"))
            .def("getGridCells", &DOGM::getGridCells)
            .def("getMeasurementCells", &DOGM::getMeasurementCells)
            .def("getParticles", &DOGM::getParticles)
            .def("getGridSize", &DOGM::getGridSize)
            .def("getResolution", &DOGM::getResolution)
            .def("getPositionX", &DOGM::getPositionX)
            .def("getPositionY", &DOGM::getPositionY)
            .def("getYaw", &DOGM::getYaw)
            .def("getIteration", &DOGM::getIteration);

    m.def( "renderOccupancyGrid", &render_occupancy_grid, "Convert the occupancy grid into a numpy array",
           py::arg("grid"));

    m.def( "renderDynamicOccupancyGrid", &render_dynamic_occupancy_grid, "Convert the occupancy grid into a RGB numpy array encoding of dynamic elements",
           py::arg("grid"), py::arg("occupancy_threshold"), py::arg("dynamic_threshold"), py::arg("max_velocity"));

  }

}