// Templated hybrid string_sort

//          Copyright Steven J. Ross 2001 - 2009.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

// See http://www.boost.org/libs/sort/ for library home page.

/*
Some improvements suggested by:
Phil Endecott and Frank Gennari
*/

#ifndef BOOST_STRING_SORT_HPP
#define BOOST_STRING_SORT_HPP
#include <algorithm>
#include <boost/range/begin.hpp>
#include <boost/range/end.hpp>
#include <boost/sort/spreadsort/detail/constants.hpp>
#include <boost/sort/spreadsort/detail/string_sort.hpp>
#include <boost/static_assert.hpp>
#include <cstring>
#include <limits>
#include <vector>

namespace boost {
namespace sort {
namespace spreadsort {

/*! \brief String sort algorithm using random access iterators, allowing
character-type overloads.\n (All variants fall back to @c boost::sort::pdqsort
if the data size is too small, < @c detail::min_sort_size).

  \details @c string_sort is a fast templated in-place hybrid radix/comparison
algorithm, which in testing tends to be roughly 50% to 2X faster than @c
std::sort for large tests (>=100kB).\n \par Worst-case performance is   O(N
* (lg(range)/s + s)) , so @c string_sort is asymptotically faster than pure
comparison-based algorithms. \n\n Some performance plots of runtime vs. n and
log(range) are provided:\n
windows_string_sort\n
osx_string_sort

   \tparam RandomAccessIter Random
access iterator \tparam Unsigned_char_type  Unsigned character type used for
string. \param[in] first Iterator pointer to first element. \param[in] last
Iterator pointing to one beyond the end of data. \param[in] unused value with
the same type as the result of the [] operator, defining the Unsigned_char_type.
The actual value is unused.

   \pre [@c first, @c last) is a valid range.
   \pre @c RandomAccessIter @c value_type is mutable.
   \pre @c RandomAccessIter @c value_type is LessThanComparable
   \pre @c RandomAccessIter @c value_type supports the @c operator>>,
   which returns an integer-type right-shifted a specified number of bits.
   \post The elements in the range [@c first, @c last) are sorted in ascending
order.

   \throws std::exception Propagates exceptions if any of the element
comparisons, the element swaps (or moves), the right shift, subtraction of
right-shifted elements, functors, or any operations on iterators throw.

   \warning Throwing an exception may cause data loss. This will also throw if a
small vector resize throws, in which case there will be no data loss. \warning
Invalid arguments cause undefined behaviour. \note @c spreadsort function
provides a wrapper that calls the fastest sorting algorithm available for a data
type, enabling faster generic-programming.

   \remark The lesser of  O(N*log(N))  comparisons and  O(N*log(K/S
+ S)) operations worst-case, where: \remark  *  N is @c last - @c first,
   \remark  *  K is the log of the range in bits (32 for 32-bit integers using
their full range), \remark  *  S is a constant called max_splits, defaulting to
11 (except for strings where it is the log of the character size).

*/

template <class RandomAccessIter, class Unsigned_char_type>
inline void string_sort(RandomAccessIter first, RandomAccessIter last,
                        Unsigned_char_type unused) {
  // Don't sort if it's too small to optimize
  if (last - first < detail::min_sort_size)
    boost::sort::pdqsort(first, last);
  else
    detail::string_sort(first, last, unused);
}

/*! \brief String sort algorithm using range, allowing character-type
overloads.\n (All variants fall back to @c boost::sort::pdqsort if the data size
is too small, < @c detail::min_sort_size).

  \details @c string_sort is a fast templated in-place hybrid radix/comparison
algorithm, which in testing tends to be roughly 50% to 2X faster than @c
std::sort for large tests (>=100kB).\n \par Worst-case performance is   O(N
* (lg(range)/s + s)) , so @c string_sort is asymptotically faster than pure
comparison-based algorithms. \n\n Some performance plots of runtime vs. n and
log(range) are provided:\n
windows_string_sort\n
osx_string_sort

   \tparam Unsigned_char_type  Unsigned character type used for string.
   \param[in] range Range [first, last) for sorting.
   \param[in] unused value with the same type as the result of the [] operator,
defining the Unsigned_char_type.  The actual value is unused.

   \pre [@c first, @c last) is a valid range.
   \post The elements in the range [@c first, @c last) are sorted in ascending
order.

   \throws std::exception Propagates exceptions if any of the element
comparisons, the element swaps (or moves), the right shift, subtraction of
right-shifted elements, functors, or any operations on iterators throw.

   \warning Throwing an exception may cause data loss. This will also throw if a
small vector resize throws, in which case there will be no data loss. \warning
Invalid arguments cause undefined behaviour. \note @c spreadsort function
provides a wrapper that calls the fastest sorting algorithm available for a data
type, enabling faster generic-programming.

   \remark The lesser of  O(N*log(N))  comparisons and  O(N*log(K/S
+ S)) operations worst-case, where: \remark  *  N is @c last - @c first,
   \remark  *  K is the log of the range in bits (32 for 32-bit integers using
their full range), \remark  *  S is a constant called max_splits, defaulting to
11 (except for strings where it is the log of the character size).

*/

template <class Range, class Unsigned_char_type>
inline void string_sort(Range &range, Unsigned_char_type unused) {
  string_sort(boost::begin(range), boost::end(range), unused);
}

/*! \brief String sort algorithm using random access iterators, wraps using
default of unsigned char. (All variants fall back to @c boost::sort::pdqsort if
the data size is too small, < @c detail::min_sort_size).

  \details @c string_sort is a fast templated in-place hybrid radix/comparison
algorithm, which in testing tends to be roughly 50% to 2X faster than @c
std::sort for large tests (>=100kB).\n Worst-case performance is   O(N *
(lg(range)/s + s)) , so @c string_sort is asymptotically faster than pure
comparison-based algorithms. \n\n Some performance plots of runtime vs. n and
log(range) are provided:\n
windows_string_sort \n
osx_string_sort

   \param[in] first Iterator pointer to first element.
   \param[in] last Iterator pointing to one beyond the end of data.

   \pre [@c first, @c last) is a valid range.
   \pre @c RandomAccessIter @c value_type is mutable.
   \pre @c RandomAccessIter @c value_type is LessThanComparable
   \pre @c RandomAccessIter @c value_type supports the @c operator>>,
   which returns an integer-type right-shifted a specified number of bits.
   \post The elements in the range [@c first, @c last) are sorted in ascending
order.

   \throws std::exception Propagates exceptions if any of the element
comparisons, the element swaps (or moves), the right shift, subtraction of
right-shifted elements, functors, or any operations on iterators throw.

   \warning Throwing an exception may cause data loss. This will also throw if a
small vector resize throws, in which case there will be no data loss. \warning
Invalid arguments cause undefined behaviour. \note @c spreadsort function
provides a wrapper that calls the fastest sorting algorithm available for a data
type, enabling faster generic-programming.

   \remark The lesser of  O(N*log(N))  comparisons and  O(N*log(K/S
+ S)) operations worst-case, where: \remark  *  N is @c last - @c first,
   \remark  *  K is the log of the range in bits (32 for 32-bit integers using
their full range), \remark  *  S is a constant called max_splits, defaulting to
11 (except for strings where it is the log of the character size).

*/
template <class RandomAccessIter>
inline void string_sort(RandomAccessIter first, RandomAccessIter last) {
  unsigned char unused = '\0';
  string_sort(first, last, unused);
}

/*! \brief String sort algorithm using range, wraps using default of unsigned
char. (All variants fall back to @c boost::sort::pdqsort if the data size is too
small, < @c detail::min_sort_size).

  \details @c string_sort is a fast templated in-place hybrid radix/comparison
algorithm, which in testing tends to be roughly 50% to 2X faster than @c
std::sort for large tests (>=100kB).\n Worst-case performance is   O(N *
(lg(range)/s + s)) , so @c string_sort is asymptotically faster than pure
comparison-based algorithms. \n\n Some performance plots of runtime vs. n and
log(range) are provided:\n
windows_string_sort \n
osx_string_sort

   \param[in] range Range [first, last) for sorting.

   \pre [@c first, @c last) is a valid range.
   \post The elements in the range [@c first, @c last) are sorted in ascending
order.

   \throws std::exception Propagates exceptions if any of the element
comparisons, the element swaps (or moves), the right shift, subtraction of
right-shifted elements, functors, or any operations on iterators throw.

   \warning Throwing an exception may cause data loss. This will also throw if a
small vector resize throws, in which case there will be no data loss. \warning
Invalid arguments cause undefined behaviour. \note @c spreadsort function
provides a wrapper that calls the fastest sorting algorithm available for a data
type, enabling faster generic-programming.

   \remark The lesser of  O(N*log(N))  comparisons and  O(N*log(K/S
+ S)) operations worst-case, where: \remark  *  N is @c last - @c first,
   \remark  *  K is the log of the range in bits (32 for 32-bit integers using
their full range), \remark  *  S is a constant called max_splits, defaulting to
11 (except for strings where it is the log of the character size).

*/
template <class Range> inline void string_sort(Range &range) {
  string_sort(boost::begin(range), boost::end(range));
}

/*! \brief String sort algorithm using random access iterators, allowing
character-type overloads.

  (All variants fall back to @c boost::sort::pdqsort if the data size is too
small, < detail::min_sort_size).

  \details @c string_sort is a fast templated in-place hybrid radix/comparison
algorithm, which in testing tends to be roughly 50% to 2X faster than @c
std::sort for large tests (>=100kB).\n \par Worst-case performance is   O(N
* (lg(range)/s + s)) , so @c string_sort is asymptotically faster than pure
comparison-based algorithms. \n\n Some performance plots of runtime vs. n and
log(range) are provided:\n
windows_string_sort\n
osx_string_sort

   \tparam RandomAccessIter Random
access iterator \tparam Comp Functor type to use for comparison. \tparam
Unsigned_char_type Unsigned character type used for string.

   \param[in] first Iterator pointer to first element.
   \param[in] last Iterator pointing to one beyond the end of data.
   \param[in] comp A binary functor that returns whether the first element
passed to it should go before the second in order. \param[in] unused value with
the same type as the result of the [] operator, defining the Unsigned_char_type.
The actual value is unused.

   \pre [@c first, @c last) is a valid range.
   \pre @c RandomAccessIter @c value_type is mutable.
   \pre @c RandomAccessIter @c value_type is LessThanComparable
   \pre @c RandomAccessIter @c value_type supports the @c operator>>,
   which returns an integer-type right-shifted a specified number of bits.
   \post The elements in the range [@c first, @c last) are sorted in ascending
order.

   \return @c void.

   \throws  std::exception Propagates exceptions if any of the element
comparisons, the element swaps (or moves), the right shift, subtraction of
right-shifted elements, functors, or any operations on iterators throw.

   \warning Throwing an exception may cause data loss. This will also throw if a
small vector resize throws, in which case there will be no data loss. \warning
Invalid arguments cause undefined behaviour. \note @c spreadsort function
provides a wrapper that calls the fastest sorting algorithm available for a data
type, enabling faster generic-programming.

   \remark The lesser of  O(N*log(N))  comparisons and  O(N*log(K/S
+ S)) operations worst-case, where: \remark  *  N is @c last - @c first,
   \remark  *  K is the log of the range in bits (32 for 32-bit integers using
their full range), \remark  *  S is a constant called max_splits, defaulting to
11 (except for strings where it is the log of the character size).
*/
template <class RandomAccessIter, class Compare, class Unsigned_char_type>
inline void reverse_string_sort(RandomAccessIter first, RandomAccessIter last,
                                Compare comp, Unsigned_char_type unused) {
  // Don't sort if it's too small to optimize.
  if (last - first < detail::min_sort_size)
    boost::sort::pdqsort(first, last, comp);
  else
    detail::reverse_string_sort(first, last, unused);
}

/*! \brief String sort algorithm using range, allowing character-type overloads.

  (All variants fall back to @c boost::sort::pdqsort if the data size is too
small, < detail::min_sort_size).

  \details @c string_sort is a fast templated in-place hybrid radix/comparison
algorithm, which in testing tends to be roughly 50% to 2X faster than @c
std::sort for large tests (>=100kB).\n Worst-case performance is   O(N *
(lg(range)/s + s)) , so @c string_sort is asymptotically faster than pure
comparison-based algorithms. \n\n Some performance plots of runtime vs. n and
log(range) are provided:\n
windows_integer_sort \n
osx_integer_sort

   \tparam Comp Functor type to use for comparison.
   \tparam Unsigned_char_type Unsigned character type used for string.

   \param[in] range Range [first, last) for sorting.
   \param[in] comp A binary functor that returns whether the first element
passed to it should go before the second in order. \param[in] unused value with
the same type as the result of the [] operator, defining the Unsigned_char_type.
The actual value is unused.

   \pre [@c first, @c last) is a valid range.
   \post The elements in the range [@c first, @c last) are sorted in ascending
order.

   \return @c void.

   \throws  std::exception Propagates exceptions if any of the element
comparisons, the element swaps (or moves), the right shift, subtraction of
right-shifted elements, functors, or any operations on iterators throw.

   \warning Throwing an exception may cause data loss. This will also throw if a
small vector resize throws, in which case there will be no data loss. \warning
Invalid arguments cause undefined behaviour. \note @c spreadsort function
provides a wrapper that calls the fastest sorting algorithm available for a data
type, enabling faster generic-programming.

   \remark The lesser of  O(N*log(N))  comparisons and  O(N*log(K/S
+ S)) operations worst-case, where: \remark  *  N is @c last - @c first,
   \remark  *  K is the log of the range in bits (32 for 32-bit integers using
their full range), \remark  *  S is a constant called max_splits, defaulting to
11 (except for strings where it is the log of the character size).
*/
template <class Range, class Compare, class Unsigned_char_type>
inline void reverse_string_sort(Range &range, Compare comp,
                                Unsigned_char_type unused) {
  reverse_string_sort(boost::begin(range), boost::end(range), comp, unused);
}

/*! \brief String sort algorithm using random access iterators,  wraps using
default of @c unsigned char.

  (All variants fall back to @c boost::sort::pdqsort if the data size is too
small, < @c detail::min_sort_size).

  \details @c string_sort is a fast templated in-place hybrid radix/comparison
algorithm, which in testing tends to be roughly 50% to 2X faster than @c
std::sort for large tests (>=100kB).\n \par Worst-case performance is   O(N
* (lg(range)/s + s)) , so @c string_sort is asymptotically faster than pure
comparison-based algorithms.\n\n Some performance plots of runtime vs. n and
log(range) are provided:\n
windows_string_sort\n
osx_string_sort

   \param[in] first Iterator pointer to first element.
   \param[in] last Iterator pointing to one beyond the end of data.
   \param[in] comp A binary functor that returns whether the first element
passed to it should go before the second in order.

   \pre [@c first, @c last) is a valid range.
   \pre @c RandomAccessIter @c value_type is mutable.
   \pre @c RandomAccessIter @c value_type is LessThanComparable
   \pre @c RandomAccessIter @c value_type supports the @c operator>>,
   which returns an integer-type right-shifted a specified number of bits.
   \post The elements in the range [@c first, @c last) are sorted in ascending
order.

   \return @c void.

   \throws  std::exception Propagates exceptions if any of the element
comparisons, the element swaps (or moves), the right shift, subtraction of
right-shifted elements, functors, or any operations on iterators throw.

   \warning Throwing an exception may cause data loss. This will also throw if a
small vector resize throws, in which case there will be no data loss. \warning
Invalid arguments cause undefined behaviour. \note @c spreadsort function
provides a wrapper that calls the fastest sorting algorithm available for a data
type, enabling faster generic-programming.

   \remark The lesser of  O(N*log(N))  comparisons and  O(N*log(K/S
+ S)) operations worst-case, where: \remark  *  N is @c last - @c first,
   \remark  *  K is the log of the range in bits (32 for 32-bit integers using
their full range), \remark  *  S is a constant called max_splits, defaulting to
11 (except for strings where it is the log of the character size).
*/
template <class RandomAccessIter, class Compare>
inline void reverse_string_sort(RandomAccessIter first, RandomAccessIter last,
                                Compare comp) {
  unsigned char unused = '\0';
  reverse_string_sort(first, last, comp, unused);
}

/*! \brief String sort algorithm using range, wraps using default of @c unsigned
char.

  (All variants fall back to @c boost::sort::pdqsort if the data size is too
small, < @c detail::min_sort_size).

  \details @c string_sort is a fast templated in-place hybrid radix/comparison
algorithm, which in testing tends to be roughly 50% to 2X faster than @c
std::sort for large tests (>=100kB).\n \par Worst-case performance is   O(N
* (lg(range)/s + s)) , so @c string_sort is asymptotically faster than pure
comparison-based algorithms. \n\n Some performance plots of runtime vs. n and
log(range) are provided:\n
windows_string_sort\n
osx_string_sort

   \param[in] range Range [first, last) for sorting.
   \param[in] comp A binary functor that returns whether the first element
passed to it should go before the second in order.

   \pre [@c first, @c last) is a valid range.
   \post The elements in the range [@c first, @c last) are sorted in ascending
order.

   \return @c void.

   \throws  std::exception Propagates exceptions if any of the element
comparisons, the element swaps (or moves), the right shift, subtraction of
right-shifted elements, functors, or any operations on iterators throw.

   \warning Throwing an exception may cause data loss. This will also throw if a
small vector resize throws, in which case there will be no data loss. \warning
Invalid arguments cause undefined behaviour. \note @c spreadsort function
provides a wrapper that calls the fastest sorting algorithm available for a data
type, enabling faster generic-programming.

   \remark The lesser of  O(N*log(N))  comparisons and  O(N*log(K/S
+ S)) operations worst-case, where: \remark  *  N is @c last - @c first,
   \remark  *  K is the log of the range in bits (32 for 32-bit integers using
their full range), \remark  *  S is a constant called max_splits, defaulting to
11 (except for strings where it is the log of the character size).
*/
template <class Range, class Compare>
inline void reverse_string_sort(Range &range, Compare comp) {
  reverse_string_sort(boost::begin(range), boost::end(range), comp);
}

/*! \brief String sort algorithm using random access iterators,  wraps using
default of @c unsigned char.

  (All variants fall back to @c boost::sort::pdqsort if the data size is too
small, < @c detail::min_sort_size).

  \details @c string_sort is a fast templated in-place hybrid radix/comparison
algorithm, which in testing tends to be roughly 50% to 2X faster than @c
std::sort for large tests (>=100kB).\n \par Worst-case performance is   O(N
* (lg(range)/s + s)) , so @c string_sort is asymptotically faster than pure
comparison-based algorithms. \n\n Some performance plots of runtime vs. n and
log(range) are provided:\n
windows_string_sort\n
osx_string_sort

   \param[in] first Iterator pointer to first element.
   \param[in] last Iterator pointing to one beyond the end of data.
   \param[in] get_character Bracket functor equivalent to @c operator[], taking
a number corresponding to the character offset. \param[in] length Functor to get
the length of the string in characters.

   \pre [@c first, @c last) is a valid range.
   \pre @c RandomAccessIter @c value_type is mutable.
   \pre @c RandomAccessIter @c value_type is LessThanComparable
   \pre @c RandomAccessIter @c value_type supports the @c operator>>,
   which returns an integer-type right-shifted a specified number of bits.
   \post The elements in the range [@c first, @c last) are sorted in ascending
order.

   \return @c void.

   \throws  std::exception Propagates exceptions if any of the element
comparisons, the element swaps (or moves), the right shift, subtraction of
right-shifted elements, functors, or any operations on iterators throw.

   \warning Throwing an exception may cause data loss. This will also throw if a
small vector resize throws, in which case there will be no data loss. \warning
Invalid arguments cause undefined behaviour. \note @c spreadsort function
provides a wrapper that calls the fastest sorting algorithm available for a data
type, enabling faster generic-programming.

   \remark The lesser of  O(N*log(N))  comparisons and  O(N*log(K/S
+ S)) operations worst-case, where: \remark  *  N is @c last - @c first,
   \remark  *  K is the log of the range in bits (32 for 32-bit integers using
their full range), \remark  *  S is a constant called max_splits, defaulting to
11 (except for strings where it is the log of the character size).

*/
template <class RandomAccessIter, class Get_char, class Get_length>
inline void string_sort(RandomAccessIter first, RandomAccessIter last,
                        Get_char get_character, Get_length length) {
  // Don't sort if it's too small to optimize
  if (last - first < detail::min_sort_size)
    boost::sort::pdqsort(first, last);
  else {
    // skipping past empties, which allows us to get the character type
    //.empty() is not used so as not to require a user declaration of it
    while (!length(*first)) {
      if (++first == last)
        return;
    }
    detail::string_sort(first, last, get_character, length,
                        get_character((*first), 0));
  }
}

/*! \brief String sort algorithm using range, wraps using default of @c unsigned
char.

  (All variants fall back to @c boost::sort::pdqsort if the data size is too
small, < @c detail::min_sort_size).

  \details @c string_sort is a fast templated in-place hybrid radix/comparison
algorithm, which in testing tends to be roughly 50% to 2X faster than @c
std::sort for large tests (>=100kB).\n \par Worst-case performance is   O(N
* (lg(range)/s + s)) , so @c string_sort is asymptotically faster than pure
comparison-based algorithms. \n\n Some performance plots of runtime vs. n and
log(range) are provided:\n
windows_string_sort\n
osx_string_sort

   \param[in] range Range [first, last) for sorting.
   \param[in] get_character Bracket functor equivalent to @c operator[], taking
a number corresponding to the character offset. \param[in] length Functor to get
the length of the string in characters.

   \pre [@c first, @c last) is a valid range.
   \post The elements in the range [@c first, @c last) are sorted in ascending
order.

   \return @c void.

   \throws  std::exception Propagates exceptions if any of the element
comparisons, the element swaps (or moves), the right shift, subtraction of
right-shifted elements, functors, or any operations on iterators throw.

   \warning Throwing an exception may cause data loss. This will also throw if a
small vector resize throws, in which case there will be no data loss. \warning
Invalid arguments cause undefined behaviour. \note @c spreadsort function
provides a wrapper that calls the fastest sorting algorithm available for a data
type, enabling faster generic-programming.

   \remark The lesser of  O(N*log(N))  comparisons and  O(N*log(K/S
+ S)) operations worst-case, where: \remark  *  N is @c last - @c first,
   \remark  *  K is the log of the range in bits (32 for 32-bit integers using
their full range), \remark  *  S is a constant called max_splits, defaulting to
11 (except for strings where it is the log of the character size).

*/
template <class Range, class Get_char, class Get_length>
inline void string_sort(Range &range, Get_char get_character,
                        Get_length length) {
  string_sort(boost::begin(range), boost::end(range), get_character, length);
}

/*! \brief String sort algorithm using random access iterators,  wraps using
default of @c unsigned char.

  (All variants fall back to @c boost::sort::pdqsort if the data size is too
small, < @c detail::min_sort_size).

  \details @c string_sort is a fast templated in-place hybrid radix/comparison
algorithm, which in testing tends to be roughly 50% to 2X faster than @c
std::sort for large tests (>=100kB).\n \par Worst-case performance is   O(N
* (lg(range)/s + s)) , so @c string_sort is asymptotically faster than pure
comparison-based algorithms. \n\n Some performance plots of runtime vs. n and
log(range) are provided:\n
windows_string_sort\n
osx_string_sort

   \param[in] first Iterator pointer to first element.
   \param[in] last Iterator pointing to one beyond the end of data.
   \param[in] get_character Bracket functor equivalent to @c operator[], taking
a number corresponding to the character offset. \param[in] length Functor to get
the length of the string in characters. \param[in] comp A binary functor that
returns whether the first element passed to it should go before the second in
order.

   \pre [@c first, @c last) is a valid range.
   \pre @c RandomAccessIter @c value_type is mutable.
   \pre @c RandomAccessIter @c value_type is LessThanComparable
   \post The elements in the range [@c first, @c last) are sorted in ascending
order.

   \return @c void.

   \throws std::exception Propagates exceptions if any of the element
comparisons, the element swaps (or moves), the right shift, subtraction of
right-shifted elements, functors, or any operations on iterators throw.

   \warning Throwing an exception may cause data loss. This will also throw if a
small vector resize throws, in which case there will be no data loss. \warning
Invalid arguments cause undefined behaviour. \note @c spreadsort function
provides a wrapper that calls the fastest sorting algorithm available for a data
type, enabling faster generic-programming.

   \remark The lesser of  O(N*log(N))  comparisons and  O(N*log(K/S
+ S)) operations worst-case, where: \remark  *  N is @c last - @c first,
   \remark  *  K is the log of the range in bits (32 for 32-bit integers using
their full range), \remark  *  S is a constant called max_splits, defaulting to
11 (except for strings where it is the log of the character size).

*/
template <class RandomAccessIter, class Get_char, class Get_length,
          class Compare>
inline void string_sort(RandomAccessIter first, RandomAccessIter last,
                        Get_char get_character, Get_length length,
                        Compare comp) {
  // Don't sort if it's too small to optimize
  if (last - first < detail::min_sort_size)
    boost::sort::pdqsort(first, last, comp);
  else {
    // skipping past empties, which allows us to get the character type
    //.empty() is not used so as not to require a user declaration of it
    while (!length(*first)) {
      if (++first == last)
        return;
    }
    detail::string_sort(first, last, get_character, length, comp,
                        get_character((*first), 0));
  }
}

/*! \brief String sort algorithm using range, wraps using default of @c unsigned
char.

  (All variants fall back to @c boost::sort::pdqsort if the data size is too
small, < @c detail::min_sort_size).

  \details @c string_sort is a fast templated in-place hybrid radix/comparison
algorithm, which in testing tends to be roughly 50% to 2X faster than @c
std::sort for large tests (>=100kB).\n \par Worst-case performance is   O(N
* (lg(range)/s + s)) , so @c string_sort is asymptotically faster than pure
comparison-based algorithms. \n\n Some performance plots of runtime vs. n and
log(range) are provided:\n
windows_string_sort\n
osx_string_sort

   \param[in] range Range [first, last) for sorting.
   \param[in] get_character Bracket functor equivalent to @c operator[], taking
a number corresponding to the character offset. \param[in] length Functor to get
the length of the string in characters. \param[in] comp A binary functor that
returns whether the first element passed to it should go before the second in
order.

   \pre [@c first, @c last) is a valid range.
   \post The elements in the range [@c first, @c last) are sorted in ascending
order.

   \return @c void.

   \throws std::exception Propagates exceptions if any of the element
comparisons, the element swaps (or moves), the right shift, subtraction of
right-shifted elements, functors, or any operations on iterators throw.

   \warning Throwing an exception may cause data loss. This will also throw if a
small vector resize throws, in which case there will be no data loss. \warning
Invalid arguments cause undefined behaviour. \note @c spreadsort function
provides a wrapper that calls the fastest sorting algorithm available for a data
type, enabling faster generic-programming.

   \remark The lesser of  O(N*log(N))  comparisons and  O(N*log(K/S
+ S)) operations worst-case, where: \remark  *  N is @c last - @c first,
   \remark  *  K is the log of the range in bits (32 for 32-bit integers using
their full range), \remark  *  S is a constant called max_splits, defaulting to
11 (except for strings where it is the log of the character size).

*/
template <class Range, class Get_char, class Get_length, class Compare>
inline void string_sort(Range &range, Get_char get_character, Get_length length,
                        Compare comp) {
  string_sort(boost::begin(range), boost::end(range), get_character, length,
              comp);
}

/*! \brief Reverse String sort algorithm using random access iterators.

  (All variants fall back to @c boost::sort::pdqsort if the data size is too
small, < @c detail::min_sort_size).

 \details @c string_sort is a fast templated in-place hybrid radix/comparison
algorithm, which in testing tends to be roughly 50% to 2X faster than @c
std::sort for large tests (>=100kB).\n \par Worst-case performance is   O(N
* (lg(range)/s + s)) , so @c string_sort is asymptotically faster than pure
comparison-based algorithms. \n\n Some performance plots of runtime vs. n and
log(range) are provided:\n
windows_string_sort\n
osx_string_sort

   \param[in] first Iterator pointer to first element.
   \param[in] last Iterator pointing to one beyond the end of data.
   \param[in] get_character Bracket functor equivalent to @c operator[], taking
a number corresponding to the character offset. \param[in] length Functor to get
the length of the string in characters. \param[in] comp A binary functor that
returns whether the first element passed to it should go before the second in
order.

   \pre [@c first, @c last) is a valid range.
   \pre @c RandomAccessIter @c value_type is mutable.
   \pre @c RandomAccessIter @c value_type is LessThanComparable
   \post The elements in the range [@c first, @c last) are sorted in ascending
order.

   \return @c void.

   \throws std::exception Propagates exceptions if any of the element
comparisons, the element swaps (or moves), the right shift, subtraction of
right-shifted elements, functors, or any operations on iterators throw.

   \warning Throwing an exception may cause data loss. This will also throw if a
small vector resize throws, in which case there will be no data loss. \warning
Invalid arguments cause undefined behaviour. \note @c spreadsort function
provides a wrapper that calls the fastest sorting algorithm available for a data
type, enabling faster generic-programming.

   \remark The lesser of  O(N*log(N))  comparisons and  O(N*log(K/S
+ S)) operations worst-case, where: \remark  *  N is @c last - @c first,
   \remark  *  K is the log of the range in bits (32 for 32-bit integers using
their full range), \remark  *  S is a constant called max_splits, defaulting to
11 (except for strings where it is the log of the character size).

*/
template <class RandomAccessIter, class Get_char, class Get_length,
          class Compare>
inline void reverse_string_sort(RandomAccessIter first, RandomAccessIter last,
                                Get_char get_character, Get_length length,
                                Compare comp) {
  // Don't sort if it's too small to optimize
  if (last - first < detail::min_sort_size)
    boost::sort::pdqsort(first, last, comp);
  else {
    // skipping past empties, which allows us to get the character type
    //.empty() is not used so as not to require a user declaration of it
    while (!length(*(--last))) {
      // If there is just one non-empty at the beginning, this is sorted
      if (first == last)
        return;
    }
    // making last just after the end of the non-empty part of the array
    detail::reverse_string_sort(first, last + 1, get_character, length, comp,
                                get_character((*last), 0));
  }
}

/*! \brief Reverse String sort algorithm using range.

  (All variants fall back to @c boost::sort::pdqsort if the data size is too
small, < @c detail::min_sort_size).

 \details @c string_sort is a fast templated in-place hybrid radix/comparison
algorithm, which in testing tends to be roughly 50% to 2X faster than @c
std::sort for large tests (>=100kB).\n \par Worst-case performance is   O(N
* (lg(range)/s + s)) , so @c string_sort is asymptotically faster than pure
comparison-based algorithms. \n\n Some performance plots of runtime vs. n and
log(range) are provided:\n
windows_string_sort\n
osx_string_sort

   \param[in] range Range [first, last) for sorting.
   \param[in] get_character Bracket functor equivalent to @c operator[], taking
a number corresponding to the character offset. \param[in] length Functor to get
the length of the string in characters. \param[in] comp A binary functor that
returns whether the first element passed to it should go before the second in
order.

   \pre [@c first, @c last) is a valid range.
   \post The elements in the range [@c first, @c last) are sorted in ascending
order.

   \return @c void.

   \throws std::exception Propagates exceptions if any of the element
comparisons, the element swaps (or moves), the right shift, subtraction of
right-shifted elements, functors, or any operations on iterators throw.

   \warning Throwing an exception may cause data loss. This will also throw if a
small vector resize throws, in which case there will be no data loss. \warning
Invalid arguments cause undefined behaviour. \note @c spreadsort function
provides a wrapper that calls the fastest sorting algorithm available for a data
type, enabling faster generic-programming.

   \remark The lesser of  O(N*log(N))  comparisons and  O(N*log(K/S
+ S)) operations worst-case, where: \remark  *  N is @c last - @c first,
   \remark  *  K is the log of the range in bits (32 for 32-bit integers using
their full range), \remark  *  S is a constant called max_splits, defaulting to
11 (except for strings where it is the log of the character size).

*/
template <class Range, class Get_char, class Get_length, class Compare>
inline void reverse_string_sort(Range &range, Get_char get_character,
                                Get_length length, Compare comp) {
  reverse_string_sort(boost::begin(range), boost::end(range), get_character,
                      length, comp);
}
} // namespace spreadsort
} // namespace sort
} // namespace boost

#endif


// Copyright (c) 2016
// Author: Chrono Law
#include <std.hpp>
using namespace std;

#include <boost/assign.hpp>
#include <boost/functional/hash.hpp>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
using namespace boost;

#include <boost/multi_index_container.hpp>
#include <boost/multi_index/hashed_index.hpp>
#include <boost/multi_index/key_extractors.hpp>

using namespace boost;
using namespace boost::multi_index;

namespace mi = boost::multi_index;

#include "person.hpp"

///////////////////////////////////////

typedef boost::tuple<int, string, string> hash_key_t;

struct hash_func
{
    typedef size_t result_type;
    size_t operator()(const hash_key_t& k) const        //必须是const
    {
        size_t seed = 2016;
        hash_combine(seed, k.get<1>());
        hash_combine(seed, k.get<2>());
        return seed;
    }
};

//定义相等比较函数对象
struct equal_func
{
    typedef bool result_type;
    bool operator()(const hash_key_t& k, const person& p) const
    {   return k.get<0>() == p.m_id;}
    bool operator()(const person& p, const hash_key_t& k) const
    {   return k.get<0>() == p.m_id;}
};

///////////////////////////////////////

int main()
{
    std::cout << "hello hashed_index" << std::endl;

    using namespace boost::assign;

    typedef multi_index_container<person,
            indexed_by<
                hashed_unique<mi::identity<person>>
                >
                > mic_t;

    mic_t mic;
    insert(mic)
        (person(2, "agent", "smith"))
        (person(1, "anderson", "neo"))
        (person(10, "lee", "bruce"));

    assert(mic.size() == 3);
    assert(mic.find(person(1, "anderson", "neo"))!= mic.end());

    assert(mic.count(boost::make_tuple(1, "anderson", "neo"),
            hash_func(), equal_func()) == 1);
    assert(mic.find(boost::make_tuple(10, "lee", "bruce"),
            hash_func(), equal_func())!= mic.end());

};


//
// Created by Can Yang on 2020/4/1.
//

#include "mm/fmm/ubodt_gen_app.hpp"
#include "mm/fmm/ubodt.hpp"
#include "network/network.hpp"
#include "network/network_graph.hpp"
#include <boost/archive/binary_oarchive.hpp>
#include "util/debug.hpp"
#include <omp.h>

using namespace FMM;
using namespace FMM::CORE;
using namespace FMM::NETWORK;
using namespace FMM::MM;
void UBODTGenApp::run() const {
  if (!config_.validate()) {
    SPDLOG_CRITICAL("Validation fail, program stop");
    return;
  };
  config_.print();
  std::chrono::steady_clock::time_point begin =
      std::chrono::steady_clock::now();
  SPDLOG_INFO("Write UBODT to file {}", config_.result_file);
  bool binary = config_.is_binary_output();
  if (config_.use_omp){
    precompute_ubodt_omp(config_.result_file, config_.delta, binary);
  } else {
    precompute_ubodt(config_.result_file, config_.delta, binary);
  }
  std::chrono::steady_clock::time_point end =
      std::chrono::steady_clock::now();
  double time_spent =
      std::chrono::duration_cast<std::chrono::milliseconds>
          (end - begin).count() / 1000.;
  SPDLOG_INFO("Time takes {}", time_spent);
}

void UBODTGenApp::precompute_ubodt(
    const std::string &filename, double delta, bool binary) const {
  int num_vertices = graph_.get_num_vertices();
  int step_size = num_vertices / 10;
  if (step_size < 10) step_size = 10;
  std::ofstream myfile(filename);
  SPDLOG_INFO("Start to generate UBODT with delta {}", delta);
  SPDLOG_INFO("Output format {}", (binary ? "binary" : "csv"));
  if (binary) {
    boost::archive::binary_oarchive oa(myfile);
    for (NodeIndex source = 0; source < num_vertices; ++source) {
      if (source % step_size == 0)
        SPDLOG_INFO("Progress {} / {}", source, num_vertices);
      PredecessorMap pmap;
      DistanceMap dmap;
      graph_.single_source_upperbound_dijkstra(source, delta, &pmap, &dmap);
      write_result_binary(oa, source, pmap, dmap);
    }
  } else {
    myfile << "source;target;next_n;prev_n;next_e;distance\n";
    for (NodeIndex source = 0; source < num_vertices; ++source) {
      if (source % step_size == 0)
        SPDLOG_INFO("Progress {} / {}", source, num_vertices);
      PredecessorMap pmap;
      DistanceMap dmap;
      graph_.single_source_upperbound_dijkstra(source, delta, &pmap, &dmap);
      write_result_csv(myfile, source, pmap, dmap);
    }
  }
  myfile.close();
}

// Parallelly generate ubodt using OpenMP
void UBODTGenApp::precompute_ubodt_omp(
    const std::string &filename, double delta,
    bool binary) const {
  int num_vertices = graph_.get_num_vertices();
  int step_size = num_vertices / 10;
  if (step_size < 10) step_size = 10;
  std::ofstream myfile(filename);
  SPDLOG_INFO("Start to generate UBODT with delta {}", delta);
  SPDLOG_INFO("Output format {}", (binary ? "binary" : "csv"));
  if (binary) {
    boost::archive::binary_oarchive oa(myfile);
    int progress = 0;
#pragma omp parallel
    {
#pragma omp for
      for (int source = 0; source < num_vertices; ++source) {
        ++progress;
        if (progress % step_size == 0) {
          SPDLOG_INFO("Progress {} / {}", progress, num_vertices);
        }
        PredecessorMap pmap;
        DistanceMap dmap;
        std::stringstream node_output_buf;
        graph_.single_source_upperbound_dijkstra(source, delta, &pmap, &dmap);
        write_result_binary(oa, source, pmap, dmap);
      }
    }
  } else {
    myfile << "source;target;next_n;prev_n;next_e;distance\n";
    int progress = 0;
#pragma omp parallel
    {
#pragma omp for
      for (int source = 0; source < num_vertices; ++source) {
        ++progress;
        if (progress % step_size == 0) {
          SPDLOG_INFO("Progress {} / {}", progress, num_vertices);
        }
        PredecessorMap pmap;
        DistanceMap dmap;
        std::stringstream node_output_buf;
        graph_.single_source_upperbound_dijkstra(source, delta, &pmap, &dmap);
        write_result_csv(myfile, source, pmap, dmap);
      }
    }
  }
  myfile.close();
}

/**
   * Write the result of routing from a single source node
   * @param stream output stream
   * @param s      source node
   * @param pmap   predecessor map
   * @param dmap   distance map
   */
void UBODTGenApp::write_result_csv(
    std::ostream &stream, NodeIndex s,
                                   PredecessorMap &pmap, DistanceMap &dmap) const {
  std::vector<Record> source_map;
  for (auto iter = pmap.begin(); iter != pmap.end(); ++iter) {
    NodeIndex cur_node = iter->first;
    if (cur_node != s) {
      NodeIndex prev_node = iter->second;
      NodeIndex v = cur_node;
      NodeIndex u;
      while ((u = pmap[v]) != s) {
        v = u;
      }
      NodeIndex successor = v;
      // Write the result to source map
      double cost = dmap[successor];
      EdgeIndex edge_index = graph_.get_edge_index(s, successor, cost);
      source_map.push_back(
          {s,
           cur_node,
           successor,
           prev_node,
           edge_index,
           dmap[cur_node],
           nullptr});
    }
  }
#pragma omp critical
  for (Record &r:source_map) {
    stream << r.source << ";"
           << r.target << ";"
           << r.first_n << ";"
           << r.prev_n << ";"
           << r.next_e << ";"
           << r.cost << "\n";
  }
}

/**
 * Write the result of routing from a single source node in
 * binary format
 *
 * @param stream output stream
 * @param s      source node
 * @param pmap   predecessor map
 * @param dmap   distance map
 */
void UBODTGenApp::write_result_binary(boost::archive::binary_oarchive &stream,
                                      NodeIndex s,
                                      PredecessorMap &pmap,
                                      DistanceMap &dmap) const {
  std::vector<Record> source_map;
  for (auto iter = pmap.begin(); iter != pmap.end(); ++iter) {
    NodeIndex cur_node = iter->first;
    if (cur_node != s) {
      NodeIndex prev_node = iter->second;
      NodeIndex v = cur_node;
      NodeIndex u;
      // When u=s, v is the next node visited
      while ((u = pmap[v]) != s) {
        v = u;
      }
      NodeIndex successor = v;
      // Write the result
      double cost = dmap[successor];
      EdgeIndex edge_index = graph_.get_edge_index(s, successor, cost);
      source_map.push_back(
          {s,
           cur_node,
           successor,
           prev_node,
           edge_index,
           dmap[cur_node],
           nullptr});
    }
  }
#pragma omp critical
  for (Record &r:source_map) {
    stream << r.source << r.target
           << r.first_n << r.prev_n << r.next_e << r.cost;
  }
}


//  i_dont_like_the_default_duration_behavior.cpp  ----------------------------------------------------------//

//  Copyright 2008 Howard Hinnant
//  Copyright 2008 Beman Dawes
//  Copyright 2009 Vicente J. Botet Escriba

//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

/*
This code was extracted by Vicente J. Botet Escriba from Beman Dawes time2_demo.cpp which
was derived by Beman Dawes from Howard Hinnant's time2_demo prototype.
Many thanks to Howard for making his code available under the Boost license.
The original code was modified to conform to Boost conventions and to section
20.9 Time utilities [time] of the C++ committee's working paper N2798.
See http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2008/n2798.pdf.

time2_demo contained this comment:

    Much thanks to Andrei Alexandrescu,
                   Walter Brown,
                   Peter Dimov,
                   Jeff Garland,
                   Terry Golubiewski,
                   Daniel Krugler,
                   Anthony Williams.
*/

#include <boost/chrono/chrono.hpp>
#include <boost/type_traits.hpp>

#include <iostream>

namespace I_dont_like_the_default_duration_behavior
{

// Here's how you override the duration's default constructor to do anything you want (in this case zero)

template <class R>
class zero_default
{
public:
    typedef R rep;

private:
    rep rep_;
public:
    zero_default(rep i = 0) : rep_(i) {}
    operator rep() const {return rep_;}

    zero_default& operator+=(zero_default x) {rep_ += x.rep_; return *this;}
    zero_default& operator-=(zero_default x) {rep_ -= x.rep_; return *this;}
    zero_default& operator*=(zero_default x) {rep_ *= x.rep_; return *this;}
    zero_default& operator/=(zero_default x) {rep_ /= x.rep_; return *this;}

    zero_default  operator+ () const {return *this;}
    zero_default  operator- () const {return zero_default(-rep_);}
    zero_default& operator++()       {++rep_; return *this;}
    zero_default  operator++(int)    {return zero_default(rep_++);}
    zero_default& operator--()       {--rep_; return *this;}
    zero_default  operator--(int)    {return zero_default(rep_--);}

    friend zero_default operator+(zero_default x, zero_default y) {return x += y;}
    friend zero_default operator-(zero_default x, zero_default y) {return x -= y;}
    friend zero_default operator*(zero_default x, zero_default y) {return x *= y;}
    friend zero_default operator/(zero_default x, zero_default y) {return x /= y;}

    friend bool operator==(zero_default x, zero_default y) {return x.rep_ == y.rep_;}
    friend bool operator!=(zero_default x, zero_default y) {return !(x == y);}
    friend bool operator< (zero_default x, zero_default y) {return x.rep_ < y.rep_;}
    friend bool operator<=(zero_default x, zero_default y) {return !(y < x);}
    friend bool operator> (zero_default x, zero_default y) {return y < x;}
    friend bool operator>=(zero_default x, zero_default y) {return !(x < y);}
};

typedef boost::chrono::duration<zero_default<long long>, boost::nano        > nanoseconds;
typedef boost::chrono::duration<zero_default<long long>, boost::micro       > microseconds;
typedef boost::chrono::duration<zero_default<long long>, boost::milli       > milliseconds;
typedef boost::chrono::duration<zero_default<long long>                      > seconds;
typedef boost::chrono::duration<zero_default<long long>, boost::ratio<60>   > minutes;
typedef boost::chrono::duration<zero_default<long long>, boost::ratio<3600> > hours;

void test()
{
    milliseconds ms;
    std::cout << ms.count() << '\n';
}

}  // I_dont_like_the_default_duration_behavior

int main()
{
    I_dont_like_the_default_duration_behavior::test();
    return 0;
}


//
// Copyright (c) 2020 Ryan Janson (ryand.janson@gmail.com)
// Copyright (c) 2016-2020 Vinnie Falco (vinnie.falco@gmail.com)
//
// 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)
//
// Official repository: https://github.com/ryanjanson/deflate
//

#include <boost/deflate/detail/crc.hpp>
#include "test_suite.hpp"

#include "zlib-1.2.11/zlib.h"

namespace boost {
  namespace deflate {
    namespace detail {

      class crc32_test {
      public:
        void run() {
            const auto eq = [](const char* data) {
              return crc32(reinterpret_cast<const unsigned char*>(data), std::strlen(data), crc32(nullptr, 0))
                     == ::crc32(::crc32(0, nullptr, 0), reinterpret_cast<const unsigned char*>(data), std::strlen(data));
            };
            BOOST_TEST(eq(""));
            BOOST_TEST(eq("0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz_"));
            BOOST_TEST(eq("Lorem ipsum dolor sit amet, consectetur adipiscing elit. Donec vitae justo orci." \
                        " Curabitur mattis venenatis tortor, et posuere dolor scelerisque quis. Proin in " \
                        "commodo ipsum. Aenean ullamcorper nibh non convallis rhoncus. Nam auctor, eros e" \
                        "get accumsan mollis, risus nulla commodo sapien, et accumsan eros nisl a leo. Nu" \
                        "nc lobortis, nisi et consequat aliquam, justo purus ultrices ligula, non condime" \
                        "ntum diam purus vitae tellus. Proin quis ullamcorper magna. Integer scelerisque " \
                        "metus nec justo ultricies ultricies. Sed purus urna, euismod quis nisi nec, port" \
                        "a finibus felis. Duis leo lacus, feugiat et malesuada ac, posuere ut metus. Viva" \
                        "mus dictum leo id semper posuere. Curabitur id nibh vel elit efficitur euismod s" \
                        "ed vitae nulla. Proin erat mi, gravida at suscipit non, rhoncus vitae nibh. Maec" \
                        "enas cursus maximus leo eu tristique."));

        }
      };

      TEST_SUITE(crc32_test, "crc32");

    } // detail
  } // deflate
} // boost


#include "tune_derivatives.hh"

#include "tune_instances.hh"

#include "../../util/stream/config.hh"
#include "../../util/stream/chain.hh"
#include "../../util/stream/io.hh"
#include "../../util/stream/typed_stream.hh"

#define BOOST_TEST_MODULE DerivativeTest
#include <boost/test/unit_test.hpp>

namespace lm { namespace interpolate {

class MockInstances : public Instances {
  public:
    MockInstances() : chain_(util::stream::ChainConfig(ReadExtensionsEntrySize(), 2, 100)), write_(chain_.Add()) {
      extensions_subsequent_.reset(new util::stream::FileBuffer(util::MakeTemp("/tmp/")));
      chain_ >> extensions_subsequent_->Sink() >> util::stream::kRecycle;
    }

    Matrix &LNUnigrams() { return ln_unigrams_; }

    BackoffMatrix &LNBackoffs() { return ln_backoffs_; }

    WordIndex &BOS() { return bos_; }

    Vector &NegLNCorrectSum() { return neg_ln_correct_sum_; }

    // Extensions must be provided sorted!
    void AddExtension(const Extension &extension) {
      *write_ = extension;
      ++write_;
    }

    void DoneExtending() {
      write_.Poison();
      chain_.Wait(true);
    }

  private:
    util::stream::Chain chain_;
    util::stream::TypedStream<Extension> write_;
};

namespace {

BOOST_AUTO_TEST_CASE(Small) {
  MockInstances mock;

  {
    // Three vocabulary words plus <s>, two models.
    Matrix unigrams(4, 2);
    unigrams <<
      0.1, 0.6,
      0.4, 0.3,
      0.5, 0.1,
      // <s>
      1.0, 1.0;
    mock.LNUnigrams() = unigrams.array().log();
  }
  mock.BOS() = 3;

  // One instance
  mock.LNBackoffs().resize(1, 2);
  mock.LNBackoffs() << 0.2, 0.4;
  mock.LNBackoffs() = mock.LNBackoffs().array().log();

  // Sparse extensions: model 0 word 2 and model 1 word 1.

  // Assuming that model 1 only matches word 1, this is p_1(1 | context)
  Accum model_1_word_1 = 1.0 - .6 * .4 - .1 * .4;

  mock.NegLNCorrectSum().resize(2);
  // We'll suppose correct has WordIndex 1, which backs off in model 0, and matches in model 1
  mock.NegLNCorrectSum() << (0.4 * 0.2), model_1_word_1;
  mock.NegLNCorrectSum() = -mock.NegLNCorrectSum().array().log();

  Accum model_0_word_2 = 1.0 - .1 * .2 - .4 * .2;

  Extension ext;

  ext.instance = 0;
  ext.word = 1;
  ext.model = 1;
  ext.ln_prob = log(model_1_word_1);
  mock.AddExtension(ext);

  ext.instance = 0;
  ext.word = 2;
  ext.model = 0;
  ext.ln_prob = log(model_0_word_2);
  mock.AddExtension(ext);

  mock.DoneExtending();

  Vector weights(2);
  weights << 0.9, 1.2;

  Vector gradient(2);
  Matrix hessian(2,2);
  Derivatives(mock, weights, gradient, hessian);
  // TODO: check perplexity value coming out.

  // p_I(x | context)
  Vector p_I(3);
  p_I <<
    pow(0.1 * 0.2, 0.9) * pow(0.6 * 0.4, 1.2),
    pow(0.4 * 0.2, 0.9) * pow(model_1_word_1, 1.2),
    pow(model_0_word_2, 0.9) * pow(0.1 * 0.4, 1.2);
  p_I /= p_I.sum();

  Vector expected_gradient = mock.NegLNCorrectSum();
  expected_gradient(0) += p_I(0) * log(0.1 * 0.2);
  expected_gradient(0) += p_I(1) * log(0.4 * 0.2);
  expected_gradient(0) += p_I(2) * log(model_0_word_2);
  BOOST_CHECK_CLOSE(expected_gradient(0), gradient(0), 0.01);

  expected_gradient(1) += p_I(0) * log(0.6 * 0.4);
  expected_gradient(1) += p_I(1) * log(model_1_word_1);
  expected_gradient(1) += p_I(2) * log(0.1 * 0.4);
  BOOST_CHECK_CLOSE(expected_gradient(1), gradient(1), 0.01);

  Matrix expected_hessian(2, 2);
  expected_hessian(1, 0) =
    // First term
    p_I(0) * log(0.1 * 0.2) * log(0.6 * 0.4) +
    p_I(1) * log(0.4 * 0.2) * log(model_1_word_1) +
    p_I(2) * log(model_0_word_2) * log(0.1 * 0.4);
  expected_hessian(1, 0) -=
    (p_I(0) * log(0.1 * 0.2) + p_I(1) * log(0.4 * 0.2) + p_I(2) * log(model_0_word_2)) *
    (p_I(0) * log(0.6 * 0.4) + p_I(1) * log(model_1_word_1) + p_I(2) * log(0.1 * 0.4));
  expected_hessian(0, 1) = expected_hessian(1, 0);
  BOOST_CHECK_CLOSE(expected_hessian(1, 0), hessian(1, 0), 0.01);
  BOOST_CHECK_CLOSE(expected_hessian(0, 1), hessian(0, 1), 0.01);
}

}}} // namespaces


#pragma once
#ifndef HEADER_metaSMT_TAG_QF_BV_HPP
#define HEADER_metaSMT_TAG_QF_BV_HPP

#include "Logic.hpp"

#include <boost/mpl/vector/vector40.hpp>
#include <boost/variant.hpp>

namespace metaSMT {
  namespace logic {
    namespace QF_BV {
      /**
       * @brief tags for SMT metaSMT::logic::QF_BV
       **/
      namespace tag {

#define PRINT(Tag, body) template<typename STREAM> \
  friend STREAM & operator<< (STREAM & out, Tag const & self) \
  { return (out << body); }
#define TAG( NAME, ATTR ) struct NAME##_tag { \
  typedef attr::ATTR attribute; \
  bool operator<(NAME##_tag const &) const {return false;} \
  PRINT(NAME##_tag, #NAME) \
};

        // variable tag
        struct var_tag { unsigned id; unsigned width;
          typedef attr::ignore attribute;

          PRINT(var_tag, "bv_var_tag[" << self.id << ',' << self.width << "]")
          bool operator< (var_tag const & other) const {
            return id < other.id;
          }
        };

        // operation tags
        TAG(bit0, constant)
        TAG(bit1, constant)

        // unary
        TAG(bvnot, unary)
        TAG(bvneg, unary)

        // bitwise binary
        TAG(bvand, binary)
        TAG(bvnand, binary)
        TAG(bvor, binary)
        TAG(bvnor, binary)
        TAG(bvxor, binary)
        TAG(bvxnor, binary)

        TAG(bvcomp, binary)

        // bitvec arithmetic
        TAG(bvadd, binary)
        TAG(bvmul, binary)
        TAG(bvsub, binary)
        TAG(bvsdiv, binary)
        TAG(bvsrem, binary)
        TAG(bvudiv, binary)
        TAG(bvurem, binary)

        // bitvector constant creation
        TAG(bvuint, constant)
        TAG(bvsint, constant)
        TAG(bvbin, constant)
        TAG(bvhex, constant)

        // modifying bv length
        TAG(concat, binary)
        TAG(extract, unary)
        TAG(repeat, ignore)
        TAG(zero_extend, unary)
        TAG(sign_extend, unary)

        // bitvector shifting
        TAG(bvshl, binary)
        TAG(bvshr, binary)
        TAG(bvashr, binary)

        // comparison operators
        TAG(bvslt, binary)
        TAG(bvsgt, binary)
        TAG(bvsle, binary)
        TAG(bvsge, binary)
        TAG(bvult, binary)
        TAG(bvugt, binary)
        TAG(bvule, binary)
        TAG(bvuge, binary)

#undef PRINT
#undef TAG

      // tag variant QF_BV_Tag
      typedef boost::mpl::vector39<
          nil
        , bit0_tag
        , bit1_tag
        , bvnot_tag
        , bvneg_tag
        , bvand_tag
        , bvnand_tag
        , bvor_tag
        , bvnor_tag
        , bvxor_tag
        , bvxnor_tag
        , bvcomp_tag
        , bvadd_tag
        , bvmul_tag
        , bvsub_tag
        , bvsrem_tag
        , bvsdiv_tag
        , bvurem_tag
        , bvudiv_tag
        , bvuint_tag
        , bvsint_tag
        , bvbin_tag
        , bvhex_tag
        , bvslt_tag
        , bvsgt_tag
        , bvsle_tag
        , bvsge_tag
        , bvult_tag
        , bvugt_tag
        , bvule_tag
        , bvuge_tag
        , concat_tag
        , extract_tag
        , zero_extend_tag
        , sign_extend_tag
        , bvshl_tag
        , bvshr_tag
        , bvashr_tag
        , var_tag
      >::type QF_BV_Tags;

      typedef boost::make_variant_over<QF_BV_Tags>::type QF_BV_Tag;

      } // namespace metaSMT::logic::QF_BV::tag
    } // namespace metaSMT::logic::QF_BV
  } // namespace metaSMT::logic
} // namespace metaSMT
#endif // HEADER_metaSMT_TAG_QF_BV_HPP
//  vim: ft=cpp:ts=2:sw=2:expandtab


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/include/functions/slice.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <nt2/include/functions/splat.hpp>
#include <nt2/include/functions/enumerate.hpp>
#include <nt2/include/constants/valmin.hpp>
#include <nt2/include/constants/valmax.hpp>
#include <nt2/sdk/complex/complex.hpp>

#include <boost/dispatch/functor/meta/call.hpp>
#include <boost/fusion/include/vector_tie.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

template<typename T>
struct big_enough : boost::mpl::bool_< (BOOST_SIMD_BYTES/sizeof(T) > 2) >
{};

NT2_TEST_CASE_TPL_MPL ( slice
                      , NT2_TEST_SEQ_MPL_FILTER ( NT2_SIMD_REAL_TYPES
                                                , big_enough<_>
                                                )
                      )
{
  using boost::simd::slice;
  using boost::simd::tag::slice_;
  using boost::simd::native;
  using boost::simd::splat;
  using boost::simd::enumerate;
  using boost::simd::meta::vector_of;

  typedef BOOST_SIMD_DEFAULT_EXTENSION                    ext_t;
  typedef std::complex<T>                                 cT;
  typedef native<cT,ext_t>                                wT;
  typedef native<T,ext_t>                                 uT;
  typedef typename vector_of<cT, wT::static_size/2>::type vT;

  NT2_TEST_TYPE_IS( (typename boost::dispatch::meta::call<slice_(wT)>::type)
                  , (std::pair<vT,vT>)
                  );

  vT ref0, ref1;
  std::size_t i=0;

  for(; i!=vT::static_size; ++i)
    ref0[i] = cT(i,i);

  for(; i!=vT::static_size*2; ++i)
    ref1[i-vT::static_size] = cT(i,i);

  wT base(enumerate<uT>(0),enumerate<uT>(0));

  {
    vT out0, out1;
    slice(base, out0, out1);
    NT2_TEST_EQUAL(out0, ref0);
    NT2_TEST_EQUAL(out1, ref1);
  }

  {
    vT out0, out1;
    out0 = slice(base, out1);
    NT2_TEST_EQUAL(out0, ref0);
    NT2_TEST_EQUAL(out1, ref1);
  }

  {
    vT out0, out1;
    boost::fusion::vector_tie(out0,out1) = slice(base);
    NT2_TEST_EQUAL(out0, ref0);
    NT2_TEST_EQUAL(out1, ref1);
  }

  {
    std::pair<vT,vT> p;
    p = slice(base);
    NT2_TEST_EQUAL(p.first, ref0);
    NT2_TEST_EQUAL(p.second, ref1);
  }

  {
    vT f,s;

    slice(boost::simd::Valmax<wT>(), f, s);
    NT2_TEST_EQUAL(f, boost::simd::Valmax<vT>() );
    NT2_TEST_EQUAL(s, boost::simd::Valmax<vT>() );

    slice(boost::simd::Valmin<wT>(), f, s);
    NT2_TEST_EQUAL(f, boost::simd::Valmin<vT>() );
    NT2_TEST_EQUAL(s, boost::simd::Valmin<vT>() );
  }
}


#include "hal_core/netlist/boolean_function/types.h"

#include <boost/spirit/home/x3.hpp>
#include <mutex>
#include <numeric>

namespace hal
{
    namespace SMT
    {
        namespace ModelParser
        {
            namespace x3 = boost::spirit::x3;

            /// 'ParserContext' refers to the data structure that stores parser results.
            struct ParserContext
            {
                /// stores mapping from variables to its (1) value and (2) bit-size
                std::map<std::string, std::tuple<u64, u16>> model;

                /// stores the current name of a variable assignment
                std::string current_name;
                /// stores the current size of a variable assignment
                std::string current_size;
                /// stores the current value of a variable assignment
                std::string current_value;
            };

            // Parser context to handle
            ParserContext parser_context;

            // Action to store a translated name in the parser context.
            const auto NameAction = [](const auto& ctx) { parser_context.current_name = _attr(ctx); };
            // Action to store a translated size in the parser context.
            const auto SortAction = [](const auto& ctx) { parser_context.current_size = boost::fusion::at_c<0>(_attr(ctx)); };
            // Action to store a translated binary value in the parser context.
            const auto BinaryValueAction = [](const auto& ctx) { parser_context.current_value = "0b" + _attr(ctx); };
            // Action to store a translated hexadecimal value in the parser context.
            const auto HexValueAction = [](const auto& ctx) { parser_context.current_value = "0x" + _attr(ctx); };

            // Action to store a translated signal assignment in the parser context.
            const auto SignalAssigmentAction = [](const auto& /* ctx */) {
                const auto value = (parser_context.current_value.at(1) == 'b') ? strtoull(parser_context.current_value.substr(2, parser_context.current_value.length() - 2).c_str(), nullptr, 2)
                                                                               : strtoull(parser_context.current_value.c_str(), nullptr, 0);

                const auto size = strtoul(parser_context.current_size.c_str(), nullptr, 0);

                parser_context.model.insert({parser_context.current_name, {value, size}});
            };

            // Parser rule to translate a signal assignment name.
            //
            // Example: "A"
            const auto NameRule = (+x3::char_("0-9a-zA-Z_"))[NameAction];
            // Parser rule to translate a signal assignment sort.
            //
            // Example: "(_ BitVec 1)"
            const auto SortRule = (x3::lit("(_ BitVec ") >> +x3::digit >> x3::lit(")"))[SortAction];

            // Parser rule to translate a signal assignment binary constant value.
            //
            // Example: "#b0"
            const auto BinaryValueRule = (x3::lit("#b") >> +x3::char_("0-1"))[BinaryValueAction];
            // Parser rule to translate a signal assignment hexadecimal constant value.
            //
            // Example: "#x1337c0de"
            const auto HexValueRule = (x3::lit("#x") >> +x3::xdigit)[HexValueAction];
            // General parser rule to translate signal assignment constant values.
            const auto ValueRule = BinaryValueRule | HexValueRule;

            // Parser rule to translate a given signal assignment into its signal
            // name, its bit-size and the constant value.
            //
            // Example: "(define-fun A () (_ BitVec 1) #b0)"
            const auto SignalAssignmentRule = (x3::lit("(define-fun") >> NameRule >> x3::lit("()") >> SortRule >> ValueRule >> x3::lit(")"))[SignalAssigmentAction];

            // Boolector grammar to parse a model for a satisfiable formula.
            //
            // Example: "(model (define-fun A () (_ BitVec 1) #b0))"
            const auto BOOLECTOR_MODEL_GRAMMAR = +(x3::lit("(model") >> +SignalAssignmentRule >> x3::lit(")"));

            // Z3 grammar to parse a model for a satisfiable formula.
            //
            // Example: "((define-fun A () (_ BitVec 1) #b0))"
            const auto Z3_MODEL_GRAMMAR = x3::lit("(") >> (+SignalAssignmentRule) >> x3::lit(")");
        }    // namespace ModelParser

        QueryConfig& QueryConfig::with_solver(SolverType _solver)
        {
            this->solver = _solver;
            return *this;
        }

        QueryConfig& QueryConfig::with_local_solver()
        {
            this->local = true;
            return *this;
        }

        QueryConfig& QueryConfig::with_remote_solver()
        {
            this->local = false;
            return *this;
        }

        QueryConfig& QueryConfig::with_model_generation()
        {
            this->generate_model = true;
            return *this;
        }

        QueryConfig& QueryConfig::without_model_generation()
        {
            this->generate_model = false;
            return *this;
        }

        QueryConfig& QueryConfig::with_timeout(u64 seconds)
        {
            this->timeout_in_seconds = seconds;
            return *this;
        }

        std::ostream& operator<<(std::ostream& out, const QueryConfig& config)
        {
            out << "{";
            out << "solver : " << enum_to_string(config.solver) << ", ";
            out << "local : " << std::boolalpha << config.local << ", ";
            out << "generate_model : " << std::boolalpha << config.generate_model << ", ";
            out << "timeout : " << std::dec << config.timeout_in_seconds << "s";
            out << "}";
            return out;
        }

        Constraint::Constraint(BooleanFunction _lhs, BooleanFunction _rhs) : lhs(_lhs), rhs(_rhs)
        {
        }

        std::ostream& operator<<(std::ostream& out, const Constraint& constraint)
        {
            out << constraint.lhs << " = " << constraint.rhs;
            return out;
        }

        Model::Model(std::map<std::string, std::tuple<u64, u16>> _model) : model(_model)
        {
        }

        bool Model::operator==(const Model& other) const
        {
            return this->model == other.model;
        }

        bool Model::operator!=(const Model& other) const
        {
            return !(*this == other);
        }

        std::ostream& operator<<(std::ostream& out, const Model& model)
        {
            auto s = std::accumulate(model.model.begin(), model.model.end(), std::string(), [](auto accumulator, auto entry) -> std::string {
                return accumulator + ", " + std::get<0>(entry) + ":" + std::to_string(std::get<0>(std::get<1>(entry)));
            });

            out << "{" << s << "}";
            return out;
        }

        std::tuple<bool, Model> Model::parse(std::string s, const SolverType& type)
        {
            // TODO:
            // check how to attach a local parser context variable to the x3 parser
            // in order to prevent the necessity of a mutex and thus speed-up
            // parallel processing.

            // to transform the current parser implementation into a thread-safe
            // implementation, we use a static mutex variable that is locked-upon
            // entry in order to prevent any overwrite of the parser_context data
            static std::mutex mutex;

            // (1) reset current context in a thread-safe way
            std::lock_guard<std::mutex> lock(mutex);
            ModelParser::parser_context = ModelParser::ParserContext();

            // (2) parse model using the SMT-LIB grammars for the different solver
            auto iter = s.begin();
            auto ok   = [&]() -> bool {
                switch (type)
                {
                    case SolverType::Z3:
                        return boost::spirit::x3::phrase_parse(iter, s.end(), ModelParser::Z3_MODEL_GRAMMAR, boost::spirit::x3::space);
                    case SolverType::Boolector:
                        return boost::spirit::x3::phrase_parse(iter, s.end(), ModelParser::BOOLECTOR_MODEL_GRAMMAR, boost::spirit::x3::space);

                    default: return false;
                }
            }();

            switch (ok && (iter == s.end()))
            {
                case true: return {true, Model(ModelParser::parser_context.model)};
                default:   return {false, Model()};
            }
        }

        Result::Result(ResultType _type, std::optional<Model> _model) : type(_type), model(_model)
        {
        }

        Result Result::Sat(const std::optional<Model>& model)
        {
            return Result(ResultType::Sat, model);
        }

        Result Result::UnSat()
        {
            return Result(ResultType::UnSat, {});
        }

        Result Result::Unknown()
        {
            return Result(ResultType::Unknown, {});
        }

        bool Result::is(const ResultType& _type) const
        {
            return this->type == _type;
        }

        bool Result::is_sat() const
        {
            return this->is(ResultType::Sat);
        }

        bool Result::is_unsat() const
        {
            return this->is(ResultType::UnSat);
        }

        bool Result::is_unknown() const
        {
            return this->is(ResultType::Unknown);
        }

        std::ostream& operator<<(std::ostream& out, const Result& result)
        {
            out << "{";
            out << "type : " << enum_to_string(result.type) << ", ";
            out << "model: ";
            if (result.model.has_value())
            {
                out << *result.model;
            }
            else
            {
                out << "none";
            }
            out << "}";
            return out;
        }
    }    // namespace SMT

    template<>
    std::vector<std::string> EnumStrings<SMT::SolverType>::data = {"Z3", "Boolector", "Unknown"};

    template<>
    std::vector<std::string> EnumStrings<SMT::ResultType>::data = {"sat", "unsat", "unknown"};
}    // namespace hal


//////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::map_pdf::ratio_pdf::include.hpp                    //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  Use, modification and distribution are 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)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_MAP_PDF_RATIO_PDF_INCLUDE_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_MAP_PDF_RATIO_PDF_INCLUDE_HPP_ER_2009

#include <boost/statistics/detail/distribution_toolkit/map_pdf/inverse_pdf/include.hpp>
#include <boost/statistics/detail/distribution_toolkit/map_pdf/product_pdf/include.hpp>
#include <boost/statistics/detail/distribution_toolkit/map_pdf/ratio_pdf/ratio_pdf.hpp>

#endif


// 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 (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_MATRIX_IDENTITY_INCLUDE
#define MTL_MATRIX_IDENTITY_INCLUDE

// #include <boost/numeric/linear_algebra/identity.hpp>
// #include <boost/numeric/mtl/mtl_fwd.hpp>
// #include <boost/numeric/mtl/matrix/parameter.hpp>
// #include <boost/numeric/mtl/matrix/diagonal_setup.hpp>

#include <boost/numeric/mtl/matrix/identity2D.hpp>

namespace mtl { namespace mat {

inline identity2D identity(std::size_t nrows, std::size_t ncols)
{
    return identity2D(nrows, ncols);
}

inline identity2D identity(std::size_t nrows)
{
    return identity2D(nrows, nrows);
}

}} // namespace mtl::matrix

#endif // MTL_MATRIX_IDENTITY_INCLUDE


/*
 * 	vision_server.hpp
 *
 *	Author(s): Tamas D. Nagy
 *	Created on: 2017-08-30
 *
 *  Generic vision server prototype, with arbitrary input
 *  and output message types. The image processing algorithm
 *  can be implemented in a separate class, see
 *  dummy_image_processor.hpp.
 *  See an assembled example for the usage of the class
 *  in vision_server_test_dummy.cpp.
 *
 */

#ifndef VISION_SERVER_HPP_
#define VISION_SERVER_HPP_

#include <iostream>
#include <sstream>
#include <vector>
#include <cmath>
#include <ros/ros.h>
#include <ros/package.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <sensor_msgs/Image.h>
#include <stereo_msgs/DisparityImage.h>

#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include "irob_utils/pose.hpp"
#include "irob_utils/utils.hpp"

namespace saf {

template <class MsgT, class ImageProcessor>
class VisionServer {
public:

private:

    ros::NodeHandle nh;
    double rate;
    ImageProcessor img_proc;

    // States
    cv_bridge::CvImagePtr image_left_ptr;
    cv_bridge::CvImagePtr image_right_ptr;
    cv_bridge::CvImagePtr color_image_left_ptr;
    cv_bridge::CvImagePtr color_image_right_ptr;
   	cv_bridge::CvImagePtr disparity_ptr;

    // Subscribers
    ros::Subscriber image_left_sub;
    ros::Subscriber image_right_sub;
    ros::Subscriber color_image_left_sub;
    ros::Subscriber color_image_right_sub;
    ros::Subscriber disparity_sub;

    // Publishers
    ros::Publisher result_pub;

    void subscribeTopics();
    void advertiseTopics();

public:
	VisionServer(ros::NodeHandle, double);
	~VisionServer();

	void loopImageProcessing();

    // Callbacks
    void imageLeftCB(
    		const sensor_msgs::ImageConstPtr &);

    void imageRightCB(
    		const sensor_msgs::ImageConstPtr &);

    void colorImageLeftCB(
    		const sensor_msgs::ImageConstPtr &);

    void colorImageRightCB(
    		const sensor_msgs::ImageConstPtr &);

   	void disparityCB(
    		const stereo_msgs::DisparityImageConstPtr &);

};

template <class MsgT, class ImageProcessor>
VisionServer<MsgT, ImageProcessor>::VisionServer(ros::NodeHandle nh,
																 double rate):
										nh(nh), rate(rate), img_proc(nh)
{
	subscribeTopics();
	advertiseTopics();
	loopImageProcessing();
}

template <class MsgT, class ImageProcessor>
VisionServer<MsgT, ImageProcessor>::~VisionServer() {}

template <class MsgT, class ImageProcessor>
void VisionServer<MsgT, ImageProcessor>::subscribeTopics()
{
	image_left_sub = nh.subscribe<sensor_msgs::Image>(
   					"left/image", 1000,
   					&VisionServer<MsgT, ImageProcessor>::imageLeftCB,this);

   	image_right_sub = nh.subscribe<sensor_msgs::Image>(
   					"right/image", 1000,
   					&VisionServer<MsgT, ImageProcessor>::imageRightCB,this);

   	color_image_left_sub = nh.subscribe<sensor_msgs::Image>(
   					"left/color_image", 1000,
   					&VisionServer<MsgT, ImageProcessor>::colorImageLeftCB,this);

   	color_image_right_sub = nh.subscribe<sensor_msgs::Image>(
   					"right/color_image", 1000,
   					&VisionServer<MsgT, ImageProcessor>::colorImageRightCB,this);

   	disparity_sub = nh.subscribe<stereo_msgs::DisparityImage>(
   					"disparity", 1000,
   					&VisionServer<MsgT, ImageProcessor>::disparityCB,this);
}

template <class MsgT, class ImageProcessor>
void VisionServer<MsgT, ImageProcessor>::advertiseTopics()
{
	result_pub = nh.advertise<MsgT>("result", 1000);
}

// Callbacks
template <class MsgT, class ImageProcessor>
void VisionServer<MsgT, ImageProcessor>::imageLeftCB(
    		const sensor_msgs::ImageConstPtr& msg)
{
    try
    {
    	image_left_ptr =
    		cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::MONO8);
    }
    catch (cv_bridge::Exception& e)
    {
      	ROS_ERROR("cv_bridge exception: %s", e.what());
      	return;
    }
}

template <class MsgT, class ImageProcessor>
void VisionServer<MsgT, ImageProcessor>::imageRightCB(
    		const sensor_msgs::ImageConstPtr& msg)
{
    try
    {
    	image_right_ptr =
    		cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::MONO8);
    }
    catch (cv_bridge::Exception& e)
    {
      	ROS_ERROR("cv_bridge exception: %s", e.what());
      	return;
    }
}

template <class MsgT, class ImageProcessor>
void VisionServer<MsgT, ImageProcessor>::colorImageLeftCB(
    		const sensor_msgs::ImageConstPtr& msg)
{
    try
    {
    	color_image_left_ptr =
    		cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
      	ROS_ERROR("cv_bridge exception: %s", e.what());
      	return;
    }
}

template <class MsgT, class ImageProcessor>
void VisionServer<MsgT, ImageProcessor>::colorImageRightCB(
    		const sensor_msgs::ImageConstPtr& msg)
{
    try
    {
    	color_image_right_ptr =
    		cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
      	ROS_ERROR("cv_bridge exception: %s", e.what());
      	return;
    }
}

template <class MsgT, class ImageProcessor>
void VisionServer<MsgT, ImageProcessor>::disparityCB(
    		const stereo_msgs::DisparityImageConstPtr& msg)
{
    try
    {
    	disparity_ptr =
    		cv_bridge::toCvCopy(msg->image, sensor_msgs::image_encodings::MONO8);
    }
    catch (cv_bridge::Exception& e)
    {
      	ROS_ERROR("cv_bridge exception: %s", e.what());
      	return;
    }
}

template <class MsgT, class ImageProcessor>
void VisionServer<MsgT, ImageProcessor>::loopImageProcessing()
{
	ros::Rate loop_rate(rate);

	while (ros::ok())
  	{
  		ros::spinOnce();

  		MsgT res = img_proc.processImages(image_left_ptr,
  										image_right_ptr,
  										color_image_left_ptr,
  										color_image_right_ptr,
  										disparity_ptr);
  		result_pub.publish(res);
  		ros::spinOnce();

  		loop_rate.sleep();
  	}
}

}
#endif /* VISION_SERVER_HPP_ */


//
//  maplocal.hpp
//  hybrid_fem_bie
//
//  Created by Max on 2/6/18.
//
//

#ifndef maplocal_hpp
#define maplocal_hpp

#include <stdio.h>
#include <Eigen/Eigen>
using namespace Eigen;
void maplocal(const ArrayXi &index, const VectorXd &u_global , VectorXd &u_local);

#endif /* maplocal_hpp */


// Copyright 2004, 2005 The Trustees of Indiana University.

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

//  Authors: Jeremiah Willcock
//           Douglas Gregor
//           Andrew Lumsdaine
#ifndef BOOST_GRAPH_ERDOS_RENYI_GENERATOR_HPP
#define BOOST_GRAPH_ERDOS_RENYI_GENERATOR_HPP

#include <cassert>
#include <iterator>
#include <utility>
#include <boost/shared_ptr.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/random/geometric_distribution.hpp>
#include <boost/type_traits/is_base_and_derived.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/config/no_tr1/cmath.hpp>

namespace boost {

  template<typename RandomGenerator, typename Graph>
  class erdos_renyi_iterator
  {
    typedef typename graph_traits<Graph>::directed_category directed_category;
    typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
    typedef typename graph_traits<Graph>::edges_size_type edges_size_type;

    BOOST_STATIC_CONSTANT
      (bool,
       is_undirected = (is_base_and_derived<undirected_tag,
                                            directed_category>::value
                        || is_same<undirected_tag, directed_category>::value));

  public:
    typedef std::input_iterator_tag iterator_category;
    typedef std::pair<vertices_size_type, vertices_size_type> value_type;
    typedef const value_type& reference;
    typedef const value_type* pointer;
    typedef void difference_type;

    erdos_renyi_iterator() : gen(), n(0), edges(0), allow_self_loops(false) {}
    erdos_renyi_iterator(RandomGenerator& gen, vertices_size_type n,
                         double fraction = 0.0, bool allow_self_loops = false)
      : gen(&gen), n(n), edges(edges_size_type(fraction * n * n)),
        allow_self_loops(allow_self_loops)
    {
      if (is_undirected) edges = edges / 2;
      next();
    }

    erdos_renyi_iterator(RandomGenerator& gen, vertices_size_type n,
                         edges_size_type m, bool allow_self_loops = false)
      : gen(&gen), n(n), edges(m),
        allow_self_loops(allow_self_loops)
    {
      next();
    }

    reference operator*() const { return current; }
    pointer operator->() const { return &current; }

    erdos_renyi_iterator& operator++()
    {
      --edges;
      next();
      return *this;
    }

    erdos_renyi_iterator operator++(int)
    {
      erdos_renyi_iterator temp(*this);
      ++(*this);
      return temp;
    }

    bool operator==(const erdos_renyi_iterator& other) const
    { return edges == other.edges; }

    bool operator!=(const erdos_renyi_iterator& other) const
    { return !(*this == other); }

  private:
    void next()
    {
      uniform_int<vertices_size_type> rand_vertex(0, n-1);
      current.first = rand_vertex(*gen);
      do {
        current.second = rand_vertex(*gen);
      } while (current.first == current.second && !allow_self_loops);
    }

    RandomGenerator* gen;
    vertices_size_type n;
    edges_size_type edges;
    bool allow_self_loops;
    value_type current;
  };

  template<typename RandomGenerator, typename Graph>
  class sorted_erdos_renyi_iterator
  {
    typedef typename graph_traits<Graph>::directed_category directed_category;
    typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
    typedef typename graph_traits<Graph>::edges_size_type edges_size_type;

    BOOST_STATIC_CONSTANT
      (bool,
       is_undirected = (is_base_and_derived<undirected_tag,
                                            directed_category>::value
                        || is_same<undirected_tag, directed_category>::value));

  public:
    typedef std::input_iterator_tag iterator_category;
    typedef std::pair<vertices_size_type, vertices_size_type> value_type;
    typedef const value_type& reference;
    typedef const value_type* pointer;
    typedef void difference_type;

    sorted_erdos_renyi_iterator()
      : gen(), rand_vertex(0.5), n(0), allow_self_loops(false),
    src((std::numeric_limits<vertices_size_type>::max)()), tgt(0), prob(0) {}
    sorted_erdos_renyi_iterator(RandomGenerator& gen, vertices_size_type n,
                    double prob = 0.0,
                                bool allow_self_loops = false)
      : gen(),
        // The "1.0 - prob" in the next line is to work around a Boost.Random
        // (and TR1) bug in the specification of geometric_distribution.  It
        // should be replaced by "prob" when the issue is fixed.
        rand_vertex(1.0 - prob),
    n(n), allow_self_loops(allow_self_loops), src(0), tgt(0), prob(prob)
    {
      this->gen.reset(new uniform_01<RandomGenerator>(gen));

      if (prob == 0.0) {src = (std::numeric_limits<vertices_size_type>::max)(); return;}
      next();
    }

    reference operator*() const { return current; }
    pointer operator->() const { return &current; }

    sorted_erdos_renyi_iterator& operator++()
    {
      next();
      return *this;
    }

    sorted_erdos_renyi_iterator operator++(int)
    {
      sorted_erdos_renyi_iterator temp(*this);
      ++(*this);
      return temp;
    }

    bool operator==(const sorted_erdos_renyi_iterator& other) const
    { return src == other.src && tgt == other.tgt; }

    bool operator!=(const sorted_erdos_renyi_iterator& other) const
    { return !(*this == other); }

  private:
    void next()
    {
      using std::sqrt;
      using std::floor;

      // In order to get the edges from the generator in sorted order, one
      // effective (but slow) procedure would be to use a
      // bernoulli_distribution for each legal (src, tgt) pair.  Because of the
      // O(n^2) cost of that, a geometric distribution is used.  The geometric
      // distribution tells how many times the bernoulli_distribution would
      // need to be run until it returns true.  Thus, this distribution can be
      // used to step through the edges which are actually present.  Everything
      // beyond "tgt += increment" is done to effectively convert linear
      // indexing (the partial sums of the geometric distribution output) into
      // graph edges.
      assert (src != (std::numeric_limits<vertices_size_type>::max)());
      vertices_size_type increment = rand_vertex(*gen);
      tgt += increment;
      if (is_undirected) {
    // Update src and tgt based on position of tgt
    // Basically, we want the greatest src_increment such that (in \bbQ):
    // src_increment * (src + allow_self_loops + src_increment - 1/2) <= tgt
    // The result of the LHS of this, evaluated with the computed
    // src_increment, is then subtracted from tgt
    double src_minus_half = (src + allow_self_loops) - 0.5;
    double disc = src_minus_half * src_minus_half + 2 * tgt;
    double src_increment_fp = floor(sqrt(disc) - src_minus_half);
    vertices_size_type src_increment = vertices_size_type(src_increment_fp);
    if (src + src_increment >= n) {
      src = n;
    } else {
      tgt -= (src + allow_self_loops) * src_increment +
         src_increment * (src_increment - 1) / 2;
      src += src_increment;
    }
      } else {
    // Number of out edge positions possible from each vertex in this graph
    vertices_size_type possible_out_edges = n - (allow_self_loops ? 0 : 1);
    src += (std::min)(n - src, tgt / possible_out_edges);
    tgt %= possible_out_edges;
      }
      // Set end of graph code so (src, tgt) will be the same as for the end
      // sorted_erdos_renyi_iterator
      if (src >= n) {src = (std::numeric_limits<vertices_size_type>::max)(); tgt = 0;}
      // Copy (src, tgt) into current
      current.first = src;
      current.second = tgt;
      // Adjust for (src, src) edge being forbidden
      if (!allow_self_loops && tgt >= src) ++current.second;
    }

    shared_ptr<uniform_01<RandomGenerator> > gen;
    geometric_distribution<vertices_size_type> rand_vertex;
    vertices_size_type n;
    bool allow_self_loops;
    vertices_size_type src, tgt;
    value_type current;
    double prob;
  };

} // end namespace boost

#endif // BOOST_GRAPH_ERDOS_RENYI_GENERATOR_HPP


/*

Copyright (c) 2005-2019, 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.

*/

#ifndef ABSTRACTGROWINGDOMAINPDEMODIFIER_HPP_
#define ABSTRACTGROWINGDOMAINPDEMODIFIER_HPP_

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

#include "AbstractPdeModifier.hpp"

/**
 * An abstract modifier class containing functionality common to EllipticGrowingDomainPdeModifier
 * and ParabolicGrowingDomainPdeModifier, which both solve a linear elliptic or parabolic PDE
 * coupled to a cell-based simulation on an evolving domain defined by the cell population.
 */
template<unsigned DIM>
class AbstractGrowingDomainPdeModifier : public AbstractPdeModifier<DIM>
{
    friend class TestEllipticGrowingDomainPdeModifier;
    friend class TestParabolicGrowingDomainPdeModifier;

private:

    /** Needed for serialization. */
    friend class boost::serialization::access;
    /**
     * Boost Serialization method for archiving/checkpointing.
     * Archives the object and its member variables.
     *
     * @param archive  The boost archive.
     * @param version  The current version of this class.
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        archive & boost::serialization::base_object<AbstractPdeModifier<DIM> >(*this);
    }

public:

    /**
     * Constructor.
     *
     * @param pPde A shared pointer to a linear PDE object (defaults to NULL)
     * @param pBoundaryCondition A shared pointer to an abstract boundary condition
     *     (defaults to NULL, corresponding to a constant boundary condition with value zero)
     * @param isNeumannBoundaryCondition Whether the boundary condition is Neumann (defaults to true)
     * @param solution solution vector (defaults to NULL)
     */
    AbstractGrowingDomainPdeModifier(boost::shared_ptr<AbstractLinearPde<DIM,DIM> > pPde=boost::shared_ptr<AbstractLinearPde<DIM,DIM> >(),
                                     boost::shared_ptr<AbstractBoundaryCondition<DIM> > pBoundaryCondition=boost::shared_ptr<AbstractBoundaryCondition<DIM> >(),
                                     bool isNeumannBoundaryCondition=true,
                                     Vec solution=nullptr);

    /**
     * Destructor.
     */
    virtual ~AbstractGrowingDomainPdeModifier();

    /**
     * Helper method to generate the mesh from the Cell population.
     *
     * @param rCellPopulation reference to the cell population
     */
    void GenerateFeMesh(AbstractCellPopulation<DIM,DIM>& rCellPopulation);

    /**
     * Helper method to copy the PDE solution to CellData
     *
     * Here there is a 1-1 correspondence between cells and nodes in the FE mesh
     *
     * @param rCellPopulation reference to the cell population
     */
    void UpdateCellData(AbstractCellPopulation<DIM,DIM>& rCellPopulation);

    /**
     * Overridden OutputSimulationModifierParameters() method.
     * Output any simulation modifier parameters to file.
     *
     * @param rParamsFile the file stream to which the parameters are output
     */
    void OutputSimulationModifierParameters(out_stream& rParamsFile);
};

#include "SerializationExportWrapper.hpp"
TEMPLATED_CLASS_IS_ABSTRACT_1_UNSIGNED(AbstractGrowingDomainPdeModifier)

#endif /*ABSTRACTGROWINGDOMAINPDEMODIFIER_HPP_*/


#include <fcntl.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <iostream>
#include <vector>
#include <cstring>
#include <string>
#include <fstream>
#include <utility> // std::pair
#include <stdexcept> // std::runtime_error
#include <sstream> // std::stringstream
#include <chrono>
#include <regex>

#include <boost/filesystem.hpp>
#include <boost/iostreams/filtering_streambuf.hpp>
#include <boost/iostreams/copy.hpp>
#include <boost/iostreams/filter/gzip.hpp>

bool test_point(double lat, double lng) {
  if(lat >= 33.57 && lat <= 33.99 && lng >= -84.49 && lng <= -84.12) {
    return true;
  }
  return false;
}

std::vector<boost::filesystem::path> get_all(boost::filesystem::path const & root, std::string const & ext) {
  std::vector<boost::filesystem::path> paths;
  if (boost::filesystem::exists(root) && boost::filesystem::is_directory(root)) {
    for (auto const & entry : boost::filesystem::recursive_directory_iterator(root)) {
      if (boost::filesystem::is_regular_file(entry) && entry.path().extension() == ext)
        paths.emplace_back(entry.path());
    }
  }
  return paths;
}

bool is_number( std::string token ) {
  return std::regex_match( token,
      std::regex( ( "((\\+|-)?[[:digit:]]+)(\\.(([[:digit:]]+)?))?" ) ) );
}

std::string make_new_path(const std::string& filename) {
  std::string word = "xmode-standard-export-gold-usa-obfuscated";
  std::string phrase = filename;
  for (int i = 0; i < phrase.length(); i++) {
    if (phrase[i] == ' ') {
      phrase[i + 1] = std::toupper(phrase[i + 1]);
    }
  }

  size_t pos = phrase.find(word); //find location of word
  phrase.erase(0,pos+word.size()); //delete everything prior to location found
  return phrase;
}

static int decompress(const std::string& filename, const std::string& outfolder) {
  auto new_path = make_new_path(filename);
  boost::filesystem::path op = boost::filesystem::absolute(outfolder) / new_path;
  std::string on = op.string();
  auto outfile_name = on.substr(0, on.size()-3);
  auto outfile_path = boost::filesystem::path(outfile_name);
  boost::filesystem::create_directories(outfile_path.parent_path());
  using namespace std;
  stringstream ss;
  stringstream decompressed;
  std::string result;
  std::cout << "in: " << filename << std::endl;
  std::cout << "out: " << outfile_name << std::endl;
  ifstream file(filename, ios_base::in | ios_base::binary);
  boost::iostreams::filtering_streambuf<boost::iostreams::input> in;
  in.push(boost::iostreams::gzip_decompressor());
  in.push(file);
  boost::iostreams::copy(in, decompressed);
  int total_lines = 0;
  int included_lines = 0;
  for (std::string line; std::getline(decompressed, line); ){
    std::istringstream s(line);
    ++total_lines;
    std::string field;
    std::string lat, lng;
    getline(s, field, ',');
    getline(s, field, ',');
    getline(s, field, ',');
    getline(s, lat, ',');
    getline(s, lng, ',');
    if(is_number(lat) && is_number(lng)) {
      if(test_point(stod(lat), stod(lng))) {
        ss << line << "\n";
        ++included_lines;
      }
    }
  }
  std::cout << "writing " << included_lines << " of " << total_lines << " lines." << std::endl;
  std::ofstream outfile;
  outfile.open(outfile_name);
  outfile << ss.rdbuf();
  return 0;
}

int main(int argc, char **argv) {
  auto outfolder = boost::filesystem::current_path() / "data";
  std::vector<boost::filesystem::path> paths = {boost::filesystem::absolute(argv[1])};
  if(boost::filesystem::is_directory(argv[1])) {
    paths = get_all(argv[1], ".gz");
  }
  int size = paths.size();
  int count = 1;
  for(auto p : paths) {
    auto start = std::chrono::high_resolution_clock::now();
    decompress(p.string(), outfolder.string());
    auto stop = std::chrono::high_resolution_clock::now();
    auto t = std::chrono::duration_cast<std::chrono::seconds>(stop - start);
    std::cout << "decompressed " << std::to_string(count) << " of " << std::to_string(size) << " in " << t.count() << " seconds." << std::endl;
    ++count;
  }
}


#include "ni/layers/depthgradientrectify.h"

#include <boost/assign/list_of.hpp>

#include "elm/core/cv/mat_utils.h"
#include "elm/core/inputname.h"
#include "elm/core/layerconfig.h"
#include "elm/core/signal.h"
#include "elm/ts/layerattr_.h"

using namespace std;
using namespace cv;
using namespace elm;
using namespace ni;

// paramters
const string DepthGradientRectify::PARAM_MAX_GRAD = "max_grad";

// input keys
const string DepthGradientRectify::KEY_INPUT_GRAD_X = "grad_x";
const string DepthGradientRectify::KEY_INPUT_GRAD_Y = "grad_y";
const string DepthGradientRectify::KEY_INPUT_GRAD_SMOOTH = "grad_smooth";

template <>
elm::MapIONames LayerAttr_<DepthGradientRectify>::io_pairs = boost::assign::map_list_of
        ELM_ADD_INPUT_PAIR(DepthGradientRectify::KEY_INPUT_GRAD_X)
        ELM_ADD_INPUT_PAIR(DepthGradientRectify::KEY_INPUT_GRAD_Y)
        ELM_ADD_INPUT_PAIR(DepthGradientRectify::KEY_INPUT_GRAD_SMOOTH)
        ELM_ADD_OUTPUT_PAIR(detail::BASE_MATOUTPUT_LAYER__KEY_OUTPUT_RESPONSE)
        ;

DepthGradientRectify::DepthGradientRectify()
    : base_MatOutputLayer()
{
    Clear();
}

void DepthGradientRectify::Clear()
{
    m_ = Mat1f();
}

void DepthGradientRectify::Reset(const LayerConfig &config)
{
    Reconfigure(config);
}

void DepthGradientRectify::Reconfigure(const LayerConfig &config)
{
    max_ = config.Params().get<float>(PARAM_MAX_GRAD);
}

void DepthGradientRectify::InputNames(const LayerInputNames &io)
{
    name_in_grad_x_ = io.Input(KEY_INPUT_GRAD_X);
    name_in_grad_y_ = io.Input(KEY_INPUT_GRAD_Y);
    name_in_grad_smooth_ = io.Input(KEY_INPUT_GRAD_SMOOTH);
}

void DepthGradientRectify::Activate(const Signal &signal)
{
    Mat1f grad_x = signal.MostRecentMat1f(name_in_grad_x_);
    Mat1f grad_y = signal.MostRecentMat1f(name_in_grad_y_);
    Mat1f grad_s = signal.MostRecentMat1f(name_in_grad_smooth_).clone();

    const float NAN_VALUE = std::numeric_limits<float>::quiet_NaN();

    // set elements that were originally NaN and > threshold to NaN
    // in smoothed gradient's y component

    Mat1b mask_nan = elm::isnan(grad_x);
    cv::bitwise_or(mask_nan, elm::isnan(grad_y), mask_nan);

    grad_s.setTo(NAN_VALUE, mask_nan);
    grad_s.setTo(NAN_VALUE, cv::abs(grad_x) > max_);
    grad_s.setTo(NAN_VALUE, cv::abs(grad_y) > max_);
    //grad_s.setTo(NAN_VALUE, cv::abs(grad_s) > max_);

    m_ = grad_s;
}