///////////////////////////////////////////////////////////////////////////////
// Copyright Christopher Kormanyos 2014.
// Copyright John Maddock 2014.
// Copyright Paul Bristow 2014.
// 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)
//

// Implement quadruple-precision I/O stream operations.

#ifndef BOOST_MATH_CSTDFLOAT_IOSTREAM_2014_02_15_HPP_
  #define BOOST_MATH_CSTDFLOAT_IOSTREAM_2014_02_15_HPP_

  #include <boost/math/cstdfloat/cstdfloat_types.hpp>
  #include <boost/math/cstdfloat/cstdfloat_limits.hpp>
  #include <boost/math/cstdfloat/cstdfloat_cmath.hpp>

  #if defined(BOOST_CSTDFLOAT_NO_LIBQUADMATH_CMATH)
  #error You can not use <boost/math/cstdfloat/cstdfloat_iostream.hpp> with BOOST_CSTDFLOAT_NO_LIBQUADMATH_CMATH defined.
  #endif

  #if defined(BOOST_CSTDFLOAT_HAS_INTERNAL_FLOAT128_T) && defined(BOOST_MATH_USE_FLOAT128) && !defined(BOOST_CSTDFLOAT_NO_LIBQUADMATH_SUPPORT)

  #include <cstddef>
  #include <istream>
  #include <ostream>
  #include <sstream>
  #include <stdexcept>
  #include <string>
  #include <boost/static_assert.hpp>
  #include <boost/throw_exception.hpp>

//  #if (0)
  #if defined(__GNUC__)

  // Forward declarations of quadruple-precision string functions.
  extern "C" int quadmath_snprintf(char *str, size_t size, const char *format, ...) throw();
  extern "C" boost::math::cstdfloat::detail::float_internal128_t strtoflt128(const char*, char **) throw();

  namespace std
  {
    template<typename char_type, class traits_type>
    inline std::basic_ostream<char_type, traits_type>& operator<<(std::basic_ostream<char_type, traits_type>& os, const boost::math::cstdfloat::detail::float_internal128_t& x)
    {
      std::basic_ostringstream<char_type, traits_type> ostr;
      ostr.flags(os.flags());
      ostr.imbue(os.getloc());
      ostr.precision(os.precision());

      char my_buffer[64U];

      const int my_prec   = static_cast<int>(os.precision());
      const int my_digits = ((my_prec == 0) ? 36 : my_prec);

      const std::ios_base::fmtflags my_flags  = os.flags();

      char my_format_string[8U];

      std::size_t my_format_string_index = 0U;

      my_format_string[my_format_string_index] = '%';
      ++my_format_string_index;

      if(my_flags & std::ios_base::showpos)   { my_format_string[my_format_string_index] = '+'; ++my_format_string_index; }
      if(my_flags & std::ios_base::showpoint) { my_format_string[my_format_string_index] = '#'; ++my_format_string_index; }

      my_format_string[my_format_string_index + 0U] = '.';
      my_format_string[my_format_string_index + 1U] = '*';
      my_format_string[my_format_string_index + 2U] = 'Q';

      my_format_string_index += 3U;

      char the_notation_char;

      if     (my_flags & std::ios_base::scientific) { the_notation_char = 'e'; }
      else if(my_flags & std::ios_base::fixed)      { the_notation_char = 'f'; }
      else                                          { the_notation_char = 'g'; }

      my_format_string[my_format_string_index + 0U] = the_notation_char;
      my_format_string[my_format_string_index + 1U] = 0;

      const int v = ::quadmath_snprintf(my_buffer,
                                        static_cast<int>(sizeof(my_buffer)),
                                        my_format_string,
                                        my_digits,
                                        x);

      if(v < 0) { BOOST_THROW_EXCEPTION(std::runtime_error("Formatting of boost::float128_t failed internally in quadmath_snprintf().")); }

      if(v >= static_cast<int>(sizeof(my_buffer) - 1U))
      {
        // Evidently there is a really long floating-point string here,
        // such as a small decimal representation in non-scientific notation.
        // So we have to use dynamic memory allocation for the output
        // string buffer.

        char* my_buffer2 = static_cast<char*>(0U);

#ifndef BOOST_NO_EXCEPTIONS
        try
        {
#endif
          my_buffer2 = new char[v + 3];
#ifndef BOOST_NO_EXCEPTIONS
        }
        catch(const std::bad_alloc&)
        {
          BOOST_THROW_EXCEPTION(std::runtime_error("Formatting of boost::float128_t failed while allocating memory."));
        }
#endif
        const int v2 = ::quadmath_snprintf(my_buffer2,
                                            v + 3,
                                            my_format_string,
                                            my_digits,
                                            x);

        if(v2 >= v + 3)
        {
          BOOST_THROW_EXCEPTION(std::runtime_error("Formatting of boost::float128_t failed."));
        }

        static_cast<void>(ostr << my_buffer2);

        delete [] my_buffer2;
      }
      else
      {
        static_cast<void>(ostr << my_buffer);
      }

      return (os << ostr.str());
    }

    template<typename char_type, class traits_type>
    inline std::basic_istream<char_type, traits_type>& operator>>(std::basic_istream<char_type, traits_type>& is, boost::math::cstdfloat::detail::float_internal128_t& x)
    {
      std::string str;

      static_cast<void>(is >> str);

      char* p_end;

      x = strtoflt128(str.c_str(), &p_end);

      if(static_cast<std::ptrdiff_t>(p_end - str.c_str()) != static_cast<std::ptrdiff_t>(str.length()))
      {
        for(std::string::const_reverse_iterator it = str.rbegin(); it != str.rend(); ++it)
        {
          static_cast<void>(is.putback(*it));
        }

        is.setstate(ios_base::failbit);

        BOOST_THROW_EXCEPTION(std::runtime_error("Unable to interpret input string as a boost::float128_t"));
      }

      return is;
    }
  }

//  #elif defined(__GNUC__)
  #elif defined(BOOST_INTEL)

  // The section for I/O stream support for the ICC compiler is particularly
  // long, because these functions must be painstakingly synthesized from
  // manually-written routines (ICC does not support I/O stream operations
  // for its _Quad type).

  // The following string-extraction routines are based on the methodology
  // used in Boost.Multiprecision by John Maddock and Christopher Kormanyos.
  // This methodology has been slightly modified here for boost::float128_t.

  #include <cstring>
  #include <cctype>
  #include <boost/lexical_cast.hpp>

  namespace boost { namespace math { namespace cstdfloat { namespace detail {

  template<class string_type>
  void format_float_string(string_type& str,
                            int my_exp,
                            int digits,
                            const std::ios_base::fmtflags f,
                            const bool iszero)
  {
    typedef typename string_type::size_type size_type;

    const bool scientific = ((f & std::ios_base::scientific) == std::ios_base::scientific);
    const bool fixed      = ((f & std::ios_base::fixed)      == std::ios_base::fixed);
    const bool showpoint  = ((f & std::ios_base::showpoint)  == std::ios_base::showpoint);
    const bool showpos    = ((f & std::ios_base::showpos)    == std::ios_base::showpos);

    const bool b_neg = ((str.size() != 0U) && (str[0] == '-'));

    if(b_neg)
    {
      str.erase(0, 1);
    }

    if(digits == 0)
    {
      digits = static_cast<int>((std::max)(str.size(), size_type(16)));
    }

    if(iszero || str.empty() || (str.find_first_not_of('0') == string_type::npos))
    {
      // We will be printing zero, even though the value might not
      // actually be zero (it just may have been rounded to zero).
      str = "0";

      if(scientific || fixed)
      {
        str.append(1, '.');
        str.append(size_type(digits), '0');

        if(scientific)
        {
          str.append("e+00");
        }
      }
      else
      {
        if(showpoint)
        {
          str.append(1, '.');
          if(digits > 1)
          {
            str.append(size_type(digits - 1), '0');
          }
        }
      }

      if(b_neg)
      {
        str.insert(0U, 1U, '-');
      }
      else if(showpos)
      {
        str.insert(0U, 1U, '+');
      }

      return;
    }

    if(!fixed && !scientific && !showpoint)
    {
      // Suppress trailing zeros.
      typename string_type::iterator pos = str.end();

      while(pos != str.begin() && *--pos == '0') { ; }

      if(pos != str.end())
      {
        ++pos;
      }

      str.erase(pos, str.end());

      if(str.empty())
      {
        str = '0';
      }
    }
    else if(!fixed || (my_exp >= 0))
    {
      // Pad out the end with zero's if we need to.

      int chars = static_cast<int>(str.size());
      chars = digits - chars;

      if(scientific)
      {
        ++chars;
      }

      if(chars > 0)
      {
        str.append(static_cast<size_type>(chars), '0');
      }
    }

    if(fixed || (!scientific && (my_exp >= -4) && (my_exp < digits)))
    {
      if((1 + my_exp) > static_cast<int>(str.size()))
      {
        // Just pad out the end with zeros.
        str.append(static_cast<size_type>((1 + my_exp) - static_cast<int>(str.size())), '0');

        if(showpoint || fixed)
        {
          str.append(".");
        }
      }
      else if(my_exp + 1 < static_cast<int>(str.size()))
      {
        if(my_exp < 0)
        {
          str.insert(0U, static_cast<size_type>(-1 - my_exp), '0');
          str.insert(0U, "0.");
        }
        else
        {
          // Insert the decimal point:
          str.insert(static_cast<size_type>(my_exp + 1), 1, '.');
        }
      }
      else if(showpoint || fixed) // we have exactly the digits we require to left of the point
      {
        str += ".";
      }

      if(fixed)
      {
        // We may need to add trailing zeros.
        int l = static_cast<int>(str.find('.') + 1U);
        l = digits - (static_cast<int>(str.size()) - l);

        if(l > 0)
        {
          str.append(size_type(l), '0');
        }
      }
    }
    else
    {
      // Scientific format:
      if(showpoint || (str.size() > 1))
      {
        str.insert(1U, 1U, '.');
      }

      str.append(1U, 'e');
      string_type e = boost::lexical_cast<string_type>(std::abs(my_exp));

      if(e.size() < 2U)
      {
        e.insert(0U, 2U - e.size(), '0');
      }

      if(my_exp < 0)
      {
        e.insert(0U, 1U, '-');
      }
      else
      {
        e.insert(0U, 1U, '+');
      }

      str.append(e);
    }

    if(b_neg)
    {
      str.insert(0U, 1U, '-');
    }
    else if(showpos)
    {
      str.insert(0U, 1U, '+');
    }
  }

  template<class float_type, class type_a> inline void eval_convert_to(type_a* pa,    const float_type& cb)                        { *pa  = static_cast<type_a>(cb); }
  template<class float_type, class type_a> inline void eval_add       (float_type& b, const type_a& a)                             { b   += a; }
  template<class float_type, class type_a> inline void eval_subtract  (float_type& b, const type_a& a)                             { b   -= a; }
  template<class float_type, class type_a> inline void eval_multiply  (float_type& b, const type_a& a)                             { b   *= a; }
  template<class float_type>               inline void eval_multiply  (float_type& b, const float_type& cb, const float_type& cb2) { b    = (cb * cb2); }
  template<class float_type, class type_a> inline void eval_divide    (float_type& b, const type_a& a)                             { b   /= a; }
  template<class float_type>               inline void eval_log10     (float_type& b, const float_type& cb)                        { b    = std::log10(cb); }
  template<class float_type>               inline void eval_floor     (float_type& b, const float_type& cb)                        { b    = std::floor(cb); }

  inline void round_string_up_at(std::string& s, int pos, int& expon)
  {
    // This subroutine rounds up a string representation of a
    // number at the given position pos.

    if(pos < 0)
    {
      s.insert(0U, 1U, '1');
      s.erase(s.size() - 1U);
      ++expon;
    }
    else if(s[pos] == '9')
    {
      s[pos] = '0';
      round_string_up_at(s, pos - 1, expon);
    }
    else
    {
      if((pos == 0) && (s[pos] == '0') && (s.size() == 1))
      {
        ++expon;
      }

      ++s[pos];
    }
  }

  template<class float_type>
  std::string convert_to_string(float_type& x,
                                std::streamsize digits,
                                const std::ios_base::fmtflags f)
  {
    const bool isneg  = (x < 0);
    const bool iszero = ((!isneg) ? bool(+x < (std::numeric_limits<float_type>::min)())
                                  : bool(-x < (std::numeric_limits<float_type>::min)()));
    const bool isnan  = (x != x);
    const bool isinf  = ((!isneg) ? bool(+x > (std::numeric_limits<float_type>::max)())
                                  : bool(-x > (std::numeric_limits<float_type>::max)()));

    int expon = 0;

    if(digits <= 0) { digits = std::numeric_limits<float_type>::max_digits10; }

    const int org_digits = static_cast<int>(digits);

    std::string result;

    if(iszero)
    {
      result = "0";
    }
    else if(isinf)
    {
      if(x < 0)
      {
        return "-inf";
      }
      else
      {
        return ((f & std::ios_base::showpos) == std::ios_base::showpos) ? "+inf" : "inf";
      }
    }
    else if(isnan)
    {
      return "nan";
    }
    else
    {
      // Start by figuring out the base-10 exponent.
      if(isneg) { x = -x; }

      float_type t;
      float_type ten = 10;

      eval_log10(t, x);
      eval_floor(t, t);
      eval_convert_to(&expon, t);

      if(-expon > std::numeric_limits<float_type>::max_exponent10 - 3)
      {
        int e = -expon / 2;

        const float_type t2 = boost::math::cstdfloat::detail::pown(ten, e);

        eval_multiply(t, t2, x);
        eval_multiply(t, t2);

        if((expon & 1) != 0)
        {
          eval_multiply(t, ten);
        }
      }
      else
      {
        t = boost::math::cstdfloat::detail::pown(ten, -expon);
        eval_multiply(t, x);
      }

      // Make sure that the value lies between [1, 10), and adjust if not.
      if(t < 1)
      {
        eval_multiply(t, 10);

        --expon;
      }
      else if(t >= 10)
      {
        eval_divide(t, 10);

        ++expon;
      }

      float_type digit;
      int        cdigit;

      // Adjust the number of digits required based on formatting options.
      if(((f & std::ios_base::fixed) == std::ios_base::fixed) && (expon != -1))
      {
        digits += (expon + 1);
      }

      if((f & std::ios_base::scientific) == std::ios_base::scientific)
      {
        ++digits;
      }

      // Extract the base-10 digits one at a time.
      for(int i = 0; i < digits; ++i)
      {
        eval_floor(digit, t);
        eval_convert_to(&cdigit, digit);

        result += static_cast<char>('0' + cdigit);

        eval_subtract(t, digit);
        eval_multiply(t, ten);
      }

      // Possibly round the result.
      if(digits >= 0)
      {
        eval_floor(digit, t);
        eval_convert_to(&cdigit, digit);
        eval_subtract(t, digit);

        if((cdigit == 5) && (t == 0))
        {
          // Use simple bankers rounding.

          if((static_cast<int>(*result.rbegin() - '0') & 1) != 0)
          {
            round_string_up_at(result, static_cast<int>(result.size() - 1U), expon);
          }
        }
        else if(cdigit >= 5)
        {
          round_string_up_at(result, static_cast<int>(result.size() - 1), expon);
        }
      }
    }

    while((result.size() > static_cast<std::string::size_type>(digits)) && result.size())
    {
      // We may get here as a result of rounding.

      if(result.size() > 1U)
      {
        result.erase(result.size() - 1U);
      }
      else
      {
        if(expon > 0)
        {
          --expon; // so we put less padding in the result.
        }
        else
        {
          ++expon;
        }

        ++digits;
      }
    }

    if(isneg)
    {
      result.insert(0U, 1U, '-');
    }

    format_float_string(result, expon, org_digits, f, iszero);

    return result;
  }

  template <class float_type>
  bool convert_from_string(float_type& value, const char* p)
  {
    value = 0;

    if((p == static_cast<const char*>(0U)) || (*p == static_cast<char>(0)))
    {
      return;
    }

    bool is_neg       = false;
    bool is_neg_expon = false;

    BOOST_CONSTEXPR_OR_CONST int ten = 10;

    int expon       = 0;
    int digits_seen = 0;

    BOOST_CONSTEXPR_OR_CONST int max_digits = std::numeric_limits<float_type>::max_digits10 + 1;

    if(*p == static_cast<char>('+'))
    {
      ++p;
    }
    else if(*p == static_cast<char>('-'))
    {
      is_neg = true;
      ++p;
    }

    const bool isnan = ((std::strcmp(p, "nan") == 0) || (std::strcmp(p, "NaN") == 0) || (std::strcmp(p, "NAN") == 0));

    if(isnan)
    {
      eval_divide(value, 0);

      if(is_neg)
      {
        value = -value;
      }

      return true;
    }

    const bool isinf = ((std::strcmp(p, "inf") == 0) || (std::strcmp(p, "Inf") == 0) || (std::strcmp(p, "INF") == 0));

    if(isinf)
    {
      value = 1;
      eval_divide(value, 0);

      if(is_neg)
      {
        value = -value;
      }

      return true;
    }

    // Grab all the leading digits before the decimal point.
    while(std::isdigit(*p))
    {
      eval_multiply(value, ten);
      eval_add(value, static_cast<int>(*p - '0'));
      ++p;
      ++digits_seen;
    }

    if(*p == static_cast<char>('.'))
    {
      // Grab everything after the point, stop when we've seen
      // enough digits, even if there are actually more available.

      ++p;

      while(std::isdigit(*p))
      {
        eval_multiply(value, ten);
        eval_add(value, static_cast<int>(*p - '0'));
        ++p;
        --expon;

        if(++digits_seen > max_digits)
        {
          break;
        }
      }

      while(std::isdigit(*p))
      {
        ++p;
      }
    }

    // Parse the exponent.
    if((*p == static_cast<char>('e')) || (*p == static_cast<char>('E')))
    {
      ++p;

      if(*p == static_cast<char>('+'))
      {
        ++p;
      }
      else if(*p == static_cast<char>('-'))
      {
        is_neg_expon = true;
        ++p;
      }

      int e2 = 0;

      while(std::isdigit(*p))
      {
        e2 *= 10;
        e2 += (*p - '0');
        ++p;
      }

      if(is_neg_expon)
      {
        e2 = -e2;
      }

      expon += e2;
    }

    if(expon)
    {
      // Scale by 10^expon. Note that 10^expon can be outside the range
      // of our number type, even though the result is within range.
      // If that looks likely, then split the calculation in two parts.
      float_type t;
      t = ten;

      if(expon > (std::numeric_limits<float_type>::min_exponent10 + 2))
      {
        t = boost::math::cstdfloat::detail::pown(t, expon);
        eval_multiply(value, t);
      }
      else
      {
        t = boost::math::cstdfloat::detail::pown(t, (expon + digits_seen + 1));
        eval_multiply(value, t);
        t = ten;
        t = boost::math::cstdfloat::detail::pown(t, (-digits_seen - 1));
        eval_multiply(value, t);
      }
    }

    if(is_neg)
    {
      value = -value;
    }

    return (*p == static_cast<char>(0));
  }
  } } } } // boost::math::cstdfloat::detail

  namespace std
  {
    template<typename char_type, class traits_type>
    inline std::basic_ostream<char_type, traits_type>& operator<<(std::basic_ostream<char_type, traits_type>& os, const boost::math::cstdfloat::detail::float_internal128_t& x)
    {
      boost::math::cstdfloat::detail::float_internal128_t non_const_x = x;

      const std::string str = boost::math::cstdfloat::detail::convert_to_string(non_const_x,
                                                                                os.precision(),
                                                                                os.flags());

      std::basic_ostringstream<char_type, traits_type> ostr;
      ostr.flags(os.flags());
      ostr.imbue(os.getloc());
      ostr.precision(os.precision());

      static_cast<void>(ostr << str);

      return (os << ostr.str());
    }

    template<typename char_type, class traits_type>
    inline std::basic_istream<char_type, traits_type>& operator>>(std::basic_istream<char_type, traits_type>& is, boost::math::cstdfloat::detail::float_internal128_t& x)
    {
      std::string str;

      static_cast<void>(is >> str);

      const bool conversion_is_ok = boost::math::cstdfloat::detail::convert_from_string(x, str.c_str());

      if(false == conversion_is_ok)
      {
        for(std::string::const_reverse_iterator it = str.rbegin(); it != str.rend(); ++it)
        {
          static_cast<void>(is.putback(*it));
        }

        is.setstate(ios_base::failbit);

        BOOST_THROW_EXCEPTION(std::runtime_error("Unable to interpret input string as a boost::float128_t"));
      }

      return is;
    }
  }

  #endif // Use __GNUC__ or BOOST_INTEL libquadmath

  #endif // Not BOOST_CSTDFLOAT_NO_LIBQUADMATH_SUPPORT (i.e., the user would like to have libquadmath support)

#endif // BOOST_MATH_CSTDFLOAT_IOSTREAM_2014_02_15_HPP_


/**
 * @file cl_cbs.cpp
 * @author Licheng Wen (wenlc@zju.edu.cn)
 * @brief The implement of CL-CBS
 * @date 2020-11-12
 *
 * @copyright Copyright (c) 2020
 *
 */
#include "cl_cbs.hpp"

#include <sys/stat.h>
#include <unistd.h>
#include <yaml-cpp/yaml.h>

#include <boost/functional/hash.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/program_options.hpp>
#include <fstream>
#include <iostream>

#include "environment.hpp"
#include "timer.hpp"

using libMultiRobotPlanning::CL_CBS;
using libMultiRobotPlanning::Neighbor;
using libMultiRobotPlanning::PlanResult;
using namespace libMultiRobotPlanning;

// calculate agent collision more precisely BUT need LONGER time
// #define PRCISE_COLLISION

struct Location {
  Location(double x, double y) : x(x), y(y) {}
  double x;
  double y;

  bool operator<(const Location& other) const {
    return std::tie(x, y) < std::tie(other.x, other.y);
  }

  bool operator==(const Location& other) const {
    return std::tie(x, y) == std::tie(other.x, other.y);
  }

  friend std::ostream& operator<<(std::ostream& os, const Location& c) {
    return os << "(" << c.x << "," << c.y << ")";
  }
};

namespace std {
template <>
struct hash<Location> {
  size_t operator()(const Location& s) const {
    size_t seed = 0;
    boost::hash_combine(seed, s.x);
    boost::hash_combine(seed, s.y);
    return seed;
  }
};
}  // namespace std

struct State {
  State(double x, double y, double yaw, int time = 0)
      : time(time), x(x), y(y), yaw(yaw) {
    rot.resize(2, 2);
    rot(0, 0) = cos(-this->yaw);
    rot(0, 1) = -sin(-this->yaw);
    rot(1, 0) = sin(-this->yaw);
    rot(1, 1) = cos(-this->yaw);
#ifdef PRCISE_COLLISION
    corner1 = Point(
        this->x -
            sqrt(pow(Constants::carWidth / 2 * 1.1, 2) +
                 pow(Constants::LB * 1.1, 2)) *
                cos(atan2(Constants::carWidth / 2, Constants::LB) - this->yaw),
        this->y -
            sqrt(pow(Constants::carWidth / 2 * 1.1, 2) +
                 pow(Constants::LB * 1.1, 2)) *
                sin(atan2(Constants::carWidth / 2, Constants::LB) - this->yaw));
    corner2 = Point(
        this->x -
            sqrt(pow(Constants::carWidth / 2 * 1.1, 2) +
                 pow(Constants::LB * 1.1, 2)) *
                cos(atan2(Constants::carWidth / 2, Constants::LB) + this->yaw),
        this->y +
            sqrt(pow(Constants::carWidth / 2 * 1.1, 2) +
                 pow(Constants::LB * 1.1, 2)) *
                sin(atan2(Constants::carWidth / 2, Constants::LB) + this->yaw));
    corner3 = Point(
        this->x +
            sqrt(pow(Constants::carWidth / 2 * 1.1, 2) +
                 pow(Constants::LF * 1.1, 2)) *
                cos(atan2(Constants::carWidth / 2, Constants::LF) - this->yaw),
        this->y +
            sqrt(pow(Constants::carWidth / 2 * 1.1, 2) +
                 pow(Constants::LF * 1.1, 2)) *
                sin(atan2(Constants::carWidth / 2, Constants::LF) - this->yaw));
    corner4 = Point(
        this->x +
            sqrt(pow(Constants::carWidth / 2 * 1.1, 2) +
                 pow(Constants::LF * 1.1, 2)) *
                cos(atan2(Constants::carWidth / 2, Constants::LF) + this->yaw),
        this->y -
            sqrt(pow(Constants::carWidth / 2 * 1.1, 2) +
                 pow(Constants::LF * 1.1, 2)) *
                sin(atan2(Constants::carWidth / 2, Constants::LF) + this->yaw));
#endif
  }

  State() = default;

  bool operator==(const State& s) const {
    return std::tie(time, x, y, yaw) == std::tie(s.time, s.x, s.y, s.yaw);
  }

  bool agentCollision(const State& other) const {
#ifndef PRCISE_COLLISION
    if (pow(this->x - other.x, 2) + pow(this->y - other.y, 2) <
        pow(2 * Constants::LF, 2) + pow(Constants::carWidth, 2))
      return true;
    return false;
#else
    std::vector<Segment> rectangle1{Segment(this->corner1, this->corner2),
                                    Segment(this->corner2, this->corner3),
                                    Segment(this->corner3, this->corner4),
                                    Segment(this->corner4, this->corner1)};
    std::vector<Segment> rectangle2{Segment(other.corner1, other.corner2),
                                    Segment(other.corner2, other.corner3),
                                    Segment(other.corner3, other.corner4),
                                    Segment(other.corner4, other.corner1)};
    for (auto seg1 = rectangle1.begin(); seg1 != rectangle1.end(); seg1++)
      for (auto seg2 = rectangle2.begin(); seg2 != rectangle2.end(); seg2++) {
        if (boost::geometry::intersects(*seg1, *seg2)) return true;
      }
    return false;
#endif
  }

  bool obsCollision(const Location& obstacle) const {
    boost::numeric::ublas::matrix<double> obs(1, 2);
    obs(0, 0) = obstacle.x - this->x;
    obs(0, 1) = obstacle.y - this->y;

    auto rotated_obs = boost::numeric::ublas::prod(obs, rot);
    if (rotated_obs(0, 0) > -Constants::LB - Constants::obsRadius &&
        rotated_obs(0, 0) < Constants::LF + Constants::obsRadius &&
        rotated_obs(0, 1) > -Constants::carWidth / 2.0 - Constants::obsRadius &&
        rotated_obs(0, 1) < Constants::carWidth / 2.0 + Constants::obsRadius)
      return true;
    return false;
  }

  friend std::ostream& operator<<(std::ostream& os, const State& s) {
    return os << "(" << s.x << "," << s.y << ":" << s.yaw << ")@" << s.time;
  }

  int time;
  double x;
  double y;
  double yaw;

 private:
  boost::numeric::ublas::matrix<double> rot;
  Point corner1, corner2, corner3, corner4;
};

namespace std {
template <>
struct hash<State> {
  size_t operator()(const State& s) const {
    size_t seed = 0;
    boost::hash_combine(seed, s.time);
    boost::hash_combine(seed, s.x);
    boost::hash_combine(seed, s.y);
    boost::hash_combine(seed, s.yaw);
    return seed;
  }
};
}  // namespace std

using Action = int;  // int<7 int ==6 wait

struct Conflict {
  int time;
  size_t agent1;
  size_t agent2;

  State s1;
  State s2;

  friend std::ostream& operator<<(std::ostream& os, const Conflict& c) {
    os << c.time << ": Collision [ " << c.agent1 << c.s1 << " , " << c.agent2
       << c.s2 << " ]";
    return os;
  }
};

struct Constraint {
  Constraint(int time, State s, size_t agentid)
      : time(time), s(s), agentid(agentid) {}
  Constraint() = default;
  int time;
  State s;
  size_t agentid;

  bool operator<(const Constraint& other) const {
    return std::tie(time, s.x, s.y, s.yaw, agentid) <
           std::tie(other.time, other.s.x, other.s.y, other.s.yaw,
                    other.agentid);
  }

  bool operator==(const Constraint& other) const {
    return std::tie(time, s.x, s.y, s.yaw, agentid) ==
           std::tie(other.time, other.s.x, other.s.y, other.s.yaw,
                    other.agentid);
  }

  friend std::ostream& operator<<(std::ostream& os, const Constraint& c) {
    return os << "Constraint[" << c.time << "," << c.s << "from " << c.agentid
              << "]";
  }

  bool satisfyConstraint(const State& state) const {
    if (state.time < this->time ||
        state.time > this->time + Constants::constraintWaitTime)
      return true;
    return !this->s.agentCollision(state);
  }
};

namespace std {
template <>
struct hash<Constraint> {
  size_t operator()(const Constraint& s) const {
    size_t seed = 0;
    boost::hash_combine(seed, s.time);
    boost::hash_combine(seed, s.s.x);
    boost::hash_combine(seed, s.s.y);
    boost::hash_combine(seed, s.s.yaw);
    boost::hash_combine(seed, s.agentid);
    return seed;
  }
};
}  // namespace std

// FIXME: modidy data struct, it's not the best option
struct Constraints {
  std::unordered_set<Constraint> constraints;

  void add(const Constraints& other) {
    constraints.insert(other.constraints.begin(), other.constraints.end());
  }

  bool overlap(const Constraints& other) {
    for (const auto& c : constraints) {
      if (other.constraints.count(c) > 0) return true;
    }
    return false;
  }

  friend std::ostream& operator<<(std::ostream& os, const Constraints& cs) {
    for (const auto& c : cs.constraints) {
      os << c << std::endl;
    }
    return os;
  }
};

void readAgentConfig() {
  YAML::Node car_config;
  std::string test(__FILE__);
  boost::replace_all(test, "cl_cbs.cpp", "config.yaml");
  try {
    car_config = YAML::LoadFile(test.c_str());
  } catch (std::exception& e) {
    std::cerr << "\033[1m\033[33mWARNING: Failed to load agent config file: "
              << test << "\033[0m , Using default params. \n";
  }
  // int car_r = car_config["r"].as<int>();
  Constants::r = car_config["r"].as<double>();
  Constants::deltat = car_config["deltat"].as<double>();
  Constants::penaltyTurning = car_config["penaltyTurning"].as<double>();
  Constants::penaltyReversing = car_config["penaltyTurning"].as<double>();
  Constants::penaltyCOD = car_config["penaltyTurning"].as<double>();
  // map resolution
  Constants::mapResolution = car_config["mapResolution"].as<double>();
  // change to set calcIndex resolution
  Constants::xyResolution = Constants::r * Constants::deltat;
  Constants::yawResolution = Constants::deltat;

  Constants::carWidth = car_config["carWidth"].as<double>();
  Constants::LF = car_config["LF"].as<double>();
  Constants::LB = car_config["LB"].as<double>();
  // obstacle default radius
  Constants::obsRadius = car_config["obsRadius"].as<double>();
  // least time to wait for constraint
  Constants::constraintWaitTime = car_config["constraintWaitTime"].as<double>();

  Constants::dx = {Constants::r * Constants::deltat,
                   Constants::r * sin(Constants::deltat),
                   Constants::r * sin(Constants::deltat),
                   -Constants::r * Constants::deltat,
                   -Constants::r * sin(Constants::deltat),
                   -Constants::r * sin(Constants::deltat)};
  Constants::dy = {0,
                   -Constants::r * (1 - cos(Constants::deltat)),
                   Constants::r * (1 - cos(Constants::deltat)),
                   0,
                   -Constants::r * (1 - cos(Constants::deltat)),
                   Constants::r * (1 - cos(Constants::deltat))};
  Constants::dyaw = {0, Constants::deltat,  -Constants::deltat,
                     0, -Constants::deltat, Constants::deltat};
}

int main(int argc, char* argv[]) {
  namespace po = boost::program_options;
  // Declare the supported options.
  po::options_description desc("Allowed options");
  std::string inputFile;
  std::string outputFile;
  int batchSize;
  desc.add_options()("help", "produce help message")(
      "input,i", po::value<std::string>(&inputFile)->required(),
      "input file (YAML)")("output,o",
                           po::value<std::string>(&outputFile)->required(),
                           "output file (YAML)")(
      "batchsize,b", po::value<int>(&batchSize)->default_value(10),
      "batch size for iter");

  try {
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help") != 0u) {
      std::cout << desc << "\n";
      return 0;
    }
  } catch (po::error& e) {
    std::cerr << e.what() << std::endl << std::endl;
    std::cerr << desc << std::endl;
    return 1;
  }

  readAgentConfig();

  YAML::Node map_config;
  try {
    map_config = YAML::LoadFile(inputFile);
  } catch (std::exception& e) {
    std::cerr << "\033[1m\033[31mERROR: Failed to load map file: " << inputFile
              << "\033[0m \n";
    return 0;
  }
  const auto& dim = map_config["map"]["dimensions"];
  int dimx = dim[0].as<int>();
  int dimy = dim[1].as<int>();

  std::unordered_set<Location> obstacles;
  std::multimap<int, State> dynamic_obstacles;
  std::vector<State> goals;
  std::vector<State> startStates;
  for (const auto& node : map_config["map"]["obstacles"]) {
    obstacles.insert(Location(node[0].as<double>(), node[1].as<double>()));
  }
  for (const auto& node : map_config["agents"]) {
    const auto& start = node["start"];
    const auto& goal = node["goal"];
    startStates.emplace_back(State(start[0].as<double>(), start[1].as<double>(),
                                   start[2].as<double>()));
    // std::cout << "s: " << startStates.back() << std::endl;
    goals.emplace_back(State(goal[0].as<double>(), goal[1].as<double>(),
                             goal[2].as<double>()));
  }

  std::cout << "Calculating Solution...\n";
  double timer = 0;
  bool success = false;
  std::vector<PlanResult<State, Action, double>> solution;
  for (size_t iter = 0; iter < (double)goals.size() / batchSize; iter++) {
    size_t first = iter * batchSize;
    size_t last = first + batchSize;
    if (last >= goals.size()) last = goals.size();
    std::vector<State> m_goals(goals.begin() + first, goals.begin() + last);
    std::vector<State> m_starts(startStates.begin() + first,
                                startStates.begin() + last);

    Environment<Location, State, Action, double, Conflict, Constraint,
                Constraints>
        mapf(dimx, dimy, obstacles, dynamic_obstacles, m_goals);
    if (!mapf.startAndGoalValid(m_starts, iter, batchSize)) {
      success = false;
      break;
    }
    for (auto goal = goals.begin() + last; goal != goals.end(); goal++) {
      dynamic_obstacles.insert(
          std::pair<int, State>(-1, State(goal->x, goal->y, goal->yaw)));
    }
    CL_CBS<State, Action, double, Conflict, Constraints,
           Environment<Location, State, Action, double, Conflict, Constraint,
                       Constraints>>
        cbsHybrid(mapf);
    std::vector<PlanResult<State, Action, double>> m_solution;
    Timer iterTimer;
    success = cbsHybrid.search(m_starts, m_solution);
    iterTimer.stop();

    if (!success) {
      std::cout << "\033[1m\033[31m No." << iter
                << "iter fail to find a solution \033[0m\n";
      break;
    } else {
      solution.insert(solution.end(), m_solution.begin(), m_solution.end());
      for (size_t a = 0; a < m_solution.size(); ++a) {
        for (const auto& state : m_solution[a].states)
          dynamic_obstacles.insert(std::pair<int, State>(
              state.first.time,
              State(state.first.x, state.first.y, state.first.yaw)));
        State lastState = m_solution[a].states.back().first;
        dynamic_obstacles.insert(std::pair<int, State>(
            -lastState.time, State(lastState.x, lastState.y, lastState.yaw)));
      }
      timer += iterTimer.elapsedSeconds();
      std::cout << "Complete " << iter
                << " iter. Runtime:" << iterTimer.elapsedSeconds()
                << " Expand high-level nodes:" << mapf.highLevelExpanded()
                << " Average Low-level-search time:"
                << iterTimer.elapsedSeconds() / mapf.highLevelExpanded() /
                       m_goals.size()
                << std::endl;
    }
    dynamic_obstacles.erase(-1);
  }

  std::ofstream out;
  out = std::ofstream(outputFile);

  if (success) {
    std::cout << "\033[1m\033[32m Successfully find solution! \033[0m\n";

    double makespan = 0, flowtime = 0, cost = 0;
    for (const auto& s : solution) cost += s.cost;

    for (size_t a = 0; a < solution.size(); ++a) {
      // calculate makespan
      double current_makespan = 0;
      for (size_t i = 0; i < solution[a].actions.size(); ++i) {
        // some action cost have penalty coefficient

        if (solution[a].actions[i].second < Constants::dx[0])
          current_makespan += solution[a].actions[i].second;
        else if (solution[a].actions[i].first % 3 == 0)
          current_makespan += Constants::dx[0];
        else
          current_makespan += Constants::r * Constants::deltat;
      }
      flowtime += current_makespan;
      if (current_makespan > makespan) makespan = current_makespan;
    }
    std::cout << " Runtime: " << timer << std::endl
              << " Makespan:" << makespan << std::endl
              << " Flowtime:" << flowtime << std::endl
              << " cost:" << cost << std::endl;
    // output to file
    out << "statistics:" << std::endl;
    out << "  cost: " << cost << std::endl;
    out << "  makespan: " << makespan << std::endl;
    out << "  flowtime: " << flowtime << std::endl;
    out << "  runtime: " << timer << std::endl;
    out << "schedule:" << std::endl;
    for (size_t a = 0; a < solution.size(); ++a) {
      // std::cout << "Solution for: " << a << std::endl;
      // for (size_t i = 0; i < solution[a].actions.size(); ++i) {
      //   std::cout << solution[a].states[i].second << ": "
      //             << solution[a].states[i].first << "->"
      //             << solution[a].actions[i].first
      //             << "(cost: " << solution[a].actions[i].second << ")"
      //             << std::endl;
      // }
      // std::cout << solution[a].states.back().second << ": "
      //           << solution[a].states.back().first << std::endl;

      out << "  agent" << a << ":" << std::endl;
      for (const auto& state : solution[a].states) {
        out << "    - x: " << state.first.x << std::endl
            << "      y: " << state.first.y << std::endl
            << "      yaw: " << state.first.yaw << std::endl
            << "      t: " << state.first.time << std::endl;
      }
    }
  } else {
    std::cout << "\033[1m\033[31m Fail to find paths \033[0m\n";
  }
}


// This file is part of RegSeg
//
// Copyright 2014-2017, Oscar Esteban <code@oscaresteban.es>
//
// Permission is hereby granted, free of charge, to any person
// obtaining a copy of this software and associated documentation
// files (the "Software"), to deal in the Software without
// restriction, including without limitation the rights to use,
// copy, modify, merge, publish, distribute, sublicense, and/or
// sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following
// conditions:
//
// The above copyright notice and this permission notice shall be
// included in all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
// OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
// WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
// OTHER DEALINGS IN THE SOFTWARE.

#ifndef __WeightedCovarianceSampleFilter_hxx
#define __WeightedCovarianceSampleFilter_hxx

#include "WeightedCovarianceSampleFilter.h"
#include <itkWeightedMeanSampleFilter.h>

#include <boost/math/special_functions/digamma.hpp>

namespace bm = boost::math;

namespace itk
{
template< typename TSample >
WeightedCovarianceSampleFilter< TSample >
::WeightedCovarianceSampleFilter():
 m_RemoveOutliers(true),
 Superclass()
{
  this->ProcessObject::SetNthInput(1, ITK_NULLPTR);

  this->ProcessObject::SetNumberOfRequiredOutputs(4);
  this->ProcessObject::SetNthOutput( 2, this->MakeOutput(2) );
  this->ProcessObject::SetNthOutput( 3, this->MakeOutput(3) );
}

template< typename TSample >
WeightedCovarianceSampleFilter< TSample >
::~WeightedCovarianceSampleFilter()
{}

template< typename TSample >
void
WeightedCovarianceSampleFilter< TSample >
::PrintSelf(std::ostream & os, itk::Indent indent) const
{
  Superclass::PrintSelf(os, indent);
  // m_Weights
  os << indent << "Weights: " << this->GetWeightsInput() << std::endl;
}

template< typename TSample >
inline void
WeightedCovarianceSampleFilter< TSample >
::GenerateData()
{
  // if weight array is specified use it to compute the covariance
  const InputWeightArrayObjectType *weightArrayObject =
    this->GetWeightsInput();

  if ( weightArrayObject != ITK_NULLPTR )
    {
    this->ComputeCovarianceMatrixWithWeights();
    return;
    }

  // Otherwise compute the regular covariance matrix ( without weight
  // coefficients)
  Superclass::GenerateData();
}

template< typename TSample >
typename WeightedCovarianceSampleFilter< TSample >::DataObjectPointer
WeightedCovarianceSampleFilter< TSample >
::MakeOutput(DataObjectPointerArraySizeType index)
{
  MeasurementVectorSizeType measurementVectorSize = this->GetMeasurementVectorSize();

  if ( index == 0 )
    {
    MatrixType covarianceMatrix(measurementVectorSize, measurementVectorSize);
    covarianceMatrix.SetIdentity();
    typename MatrixDecoratedType::Pointer decoratedCovarianceMatrix = MatrixDecoratedType::New();
    decoratedCovarianceMatrix->Set(covarianceMatrix);
    return decoratedCovarianceMatrix.GetPointer();
    }

  if ( index >= 1 )
    {
    MeasurementVectorRealType mean;
    (void)mean; // for complainty pants : valgrind
    NumericTraits<MeasurementVectorRealType>::SetLength(mean, this->GetMeasurementVectorSize());
    // NumericTraits::SetLength also initializes array to zero
    typename MeasurementVectorDecoratedType::Pointer decoratedMean = MeasurementVectorDecoratedType::New();
    decoratedMean->Set( mean );
    return decoratedMean.GetPointer();
    }
  itkExceptionMacro("Trying to create output of index " << index << " larger than the number of output");
}

template< typename TSample >
inline void
WeightedCovarianceSampleFilter< TSample >
::ComputeCovarianceMatrixWithWeights()
{
  // set up input / output
  const SampleType *input = this->GetInput();

  MeasurementVectorSizeType measurementVectorSize = input->GetMeasurementVectorSize();

  MatrixDecoratedType *decoratedOutput =
    itkDynamicCastInDebugMode< MatrixDecoratedType * >( this->ProcessObject::GetOutput(0) );

  MatrixType output = decoratedOutput->Get();
  output.SetSize( measurementVectorSize, measurementVectorSize );
  output.Fill( NumericTraits< typename MatrixType::ValueType >::Zero );

  MeasurementVectorDecoratedType *decoratedMeanOutput =
    itkDynamicCastInDebugMode< MeasurementVectorDecoratedType * >( this->ProcessObject::GetOutput(1) );

  WeightArrayType weightsArray(this->GetWeights());
  std::vector<std::vector<MeasurementType>> sampleComponents(measurementVectorSize);

  typename SampleType::ConstIterator iter =      input->Begin();
  const typename SampleType::ConstIterator end = input->End();

  MeasurementVectorType mean;
  MeasurementVectorType pbottom, ptop;
  itk::NumericTraits<MeasurementVectorType>::SetLength( pbottom, measurementVectorSize );
  itk::NumericTraits<MeasurementVectorType>::SetLength( ptop, measurementVectorSize );
  pbottom.Fill(itk::NumericTraits<MeasurementType>::min());
  ptop.Fill(itk::NumericTraits<MeasurementType>::max());

  if( this->m_RemoveOutliers ) {
	  MeasurementVectorType median;
	  // calculate percentiles
	  for ( unsigned int sampleVectorIndex = 0; iter != end; ++iter, ++sampleVectorIndex ) {
	    const MeasurementVectorType & measurement = iter.GetMeasurementVector();
	    const WeightValueType rawWeight = weightsArray[sampleVectorIndex];

	    if( rawWeight >= 0.9 ) {
	    	for(size_t c = 0; c < measurementVectorSize; c++)
	    		sampleComponents[c].push_back(measurement[c]);
	    }
	  }

	  itk::NumericTraits<MeasurementVectorType>::SetLength( median, measurementVectorSize );

	  size_t sampleSize = sampleComponents[0].size();
	  if (sampleSize > 0) {
		  for(size_t c = 0; c < measurementVectorSize; c++) {
			  std::sort(sampleComponents[c].begin(), sampleComponents[c].end());
			  pbottom[c] = sampleComponents[c][int(0.02 * sampleSize)];
			  ptop[c] = sampleComponents[c][int(0.98 * sampleSize)];
			  median[c] = sampleComponents[c][int(0.50 * sampleSize)];
		  }
		  // mean = median;
	  }
  }

  MeasurementVectorDecoratedType *decoratedRangeMaxOutput =
	    itkDynamicCastInDebugMode< MeasurementVectorDecoratedType * >( this->ProcessObject::GetOutput(2) );
  decoratedRangeMaxOutput->Set( ptop );

  MeasurementVectorDecoratedType *decoratedRangeMinOutput =
	    itkDynamicCastInDebugMode< MeasurementVectorDecoratedType * >( this->ProcessObject::GetOutput(3) );
  decoratedRangeMinOutput->Set( pbottom );

  // reset sample iterator
  iter = input->Begin();
  // fills the lower triangle and the diagonal cells in the covariance matrix
  for ( unsigned int sampleVectorIndex = 0;
        iter != end;
        ++iter, ++sampleVectorIndex ) {
	  const MeasurementVectorType & measurement = iter.GetMeasurementVector();
	  WeightValueType rawWeight = weightsArray[sampleVectorIndex];

	  if (rawWeight > 0.0) {
		    for(size_t c = 0; c < measurementVectorSize; c++) {
		    	if (measurement[c] > ptop[c] || measurement[c] < pbottom[c] ) {
		    		weightsArray[sampleVectorIndex] = 0.0;
		    		continue;
		    	}
		    }
	  }
  }

  // calculate mean
  typedef itk::Statistics::WeightedMeanSampleFilter< SampleType > WeightedMeanFilterType;
  typename WeightedMeanFilterType::Pointer meanFilter = WeightedMeanFilterType::New();

  meanFilter->SetInput( input );
  meanFilter->SetWeights( weightsArray );
  meanFilter->Update();

  mean = meanFilter->GetMean();
  decoratedMeanOutput->Set( mean );

  // covariance algorithm
  MeasurementVectorRealType diff;
  itk::NumericTraits<MeasurementVectorRealType>::SetLength( diff, measurementVectorSize );

  WeightValueType totalWeight = itk::NumericTraits< WeightValueType >::Zero;
  WeightValueType totalSquaredWeight = itk::NumericTraits< WeightValueType >::Zero;

  // reset sample iterator
  iter = input->Begin();

  // fills the lower triangle and the diagonal cells in the covariance matrix
  for ( unsigned int sampleVectorIndex = 0;
        iter != end;
        ++iter, ++sampleVectorIndex )
    {
    const MeasurementVectorType & measurement = iter.GetMeasurementVector();
    const typename SampleType::AbsoluteFrequencyType frequency = iter.GetFrequency();
    const WeightValueType rawWeight = weightsArray[sampleVectorIndex];

    if ( rawWeight < 1.0e-8 )
    	continue;

    WeightValueType weight = ( rawWeight * static_cast< WeightValueType >( frequency ) );

    totalWeight += weight;
    totalSquaredWeight += ( weight * weight );

    for ( unsigned int dim = 0; dim < measurementVectorSize; ++dim )
      {
      const MeasurementRealType component =
        static_cast< MeasurementRealType >( measurement[dim] );

      diff[dim] = ( component - mean[dim] );
      }

    // updates the covariance matrix
    for ( unsigned int row = 0; row < measurementVectorSize; ++row )
      {
      for ( unsigned int col = 0; col < row + 1; ++col )
        {
        output(row, col) +=
          ( static_cast< MeasurementRealType >( weight ) * diff[row] * diff[col] );
        }
      }
    }

  // fills the upper triangle using the lower triangle
  for ( unsigned int row = 1; row < measurementVectorSize; ++row )
    {
    for ( unsigned int col = 0; col < row; ++col ) {
    	output(col, row) = output(row, col);
      }
    }

  const double normalizationFactor = ( totalWeight - ( totalSquaredWeight / totalWeight ) );

  if( normalizationFactor > vnl_math::eps )
    {
    const double inverseNormalizationFactor = 1.0 / normalizationFactor;

    output *= inverseNormalizationFactor;
    }
  else
    {
    itkExceptionMacro("Normalization factor was too close to zero. Value = " << normalizationFactor );
    }

  // Bias estimation
  // See http://en.wikipedia.org/wiki/Estimation_of_covariance_matrices#Bias_of_the_sample_covariance_matrix
  float p = measurementVectorSize;
  float n = totalWeight;
  float beta = (1/p) * (p * log(n) + p - bm::digamma(n-p+1) + (n - p + 1) * bm::digamma(n - p + 2) + bm::digamma(n+1) - (n+1)* bm::digamma(n+2));

  MatrixType ident;
  ident.SetSize( measurementVectorSize, measurementVectorSize );
  ident.SetIdentity();
  output+= output * exp( -beta );

  decoratedOutput->Set( output );
}

template< typename TSample >
const typename WeightedCovarianceSampleFilter< TSample >::MeasurementVectorDecoratedType *
WeightedCovarianceSampleFilter< TSample >
::GetRangeMaxOutput() const
{
  return static_cast< const MeasurementVectorDecoratedType * >( this->ProcessObject::GetOutput(2) );
}

template< typename TSample >
const typename WeightedCovarianceSampleFilter< TSample >::MeasurementVectorRealType
WeightedCovarianceSampleFilter< TSample >
::GetRangeMax() const
{
  return this->GetRangeMaxOutput()->Get();
}

template< typename TSample >
const typename WeightedCovarianceSampleFilter< TSample >::MeasurementVectorDecoratedType *
WeightedCovarianceSampleFilter< TSample >
::GetRangeMinOutput() const
{
  return static_cast< const MeasurementVectorDecoratedType * >( this->ProcessObject::GetOutput(3) );
}

template< typename TSample >
const typename WeightedCovarianceSampleFilter< TSample >::MeasurementVectorRealType
WeightedCovarianceSampleFilter< TSample >
::GetRangeMin() const
{
  return this->GetRangeMinOutput()->Get();
}

} // end of namespace itk

#endif


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

// modification: defines and mesh() code moved to separate file to avoid multiple inclusion
// saving PLY files, both in ASCII and binary formats, added to this file

#ifndef MARCHING_CUBES_HPP
#define MARCHING_CUBES_HPP

#include <memory>
#include <boost/progress.hpp>

#include "types.hpp"

namespace marching_cubes {

/**
 * @brief Vertex struct to manage handling points with normals
 */
struct vertex {
    Eigen::Vector3f point;
    Eigen::Vector3f normal;

    //int reference = -1; // If this vertex should be replaced by another one, store its ID here.
	int reference;

    // Color
    unsigned char r;
    unsigned char g;
    unsigned char b;

    // Location in the voxel cube
    int x;
    int y;
    int z;

	vertex(Eigen::Vector3f p, Eigen::Vector3f n, unsigned char rv, unsigned char gv, unsigned char bv, int xv, int yv, int zv) : point(p), normal(n), reference(-1), r(rv), g(gv), b(bv), x(xv), y(yv), z(zv) {}
    vertex(vertex const& o) : point(o.point), normal(o.normal), reference(o.reference), r(o.r), g(o.g), b(o.b), x(o.x), y(o.y), z(o.z) {}
    vertex(int ref) : reference(ref) {}

    // Equality and inequality operators for searching
    bool operator==(vertex other) {
        return ((reference == other.reference && reference != -1) ||
                (point == other.point && normal == other.normal));
    }
    bool operator!=(vertex other) {
        return ((reference != other.reference) ||
                (point != other.point) ||
                (normal != other.normal));
    }
};

/**
 * @brief Holds a triangle
 */
struct triangle {
    std::vector<vertex> points;

    triangle() {}
    triangle(std::vector<vertex> const& p) : points(p) {}
    triangle(triangle const& o) : points(o.points) {}
};

Eigen::Vector3f interpolate(Eigen::Vector3f p1, Eigen::Vector3f p2, float valp1, float valp2);
Eigen::Vector3f norm_at(sdf &s, int x, int y, int z);
std::vector<triangle> getCubeCase(sdf &s, int x, int y, int z);

} // namespace reconstruct::marching_cubes

/**
 * @brief mesh Mesh and scale a sdf object
 * @param s Finished SDF data
 */
std::vector<marching_cubes::triangle> mesh(sdf &s);
std::vector<marching_cubes::triangle> mesh(plain_sdf &s);
std::vector<marching_cubes::triangle> mesh_valid_only(sdf &s);

std::vector<marching_cubes::triangle> mesh_world(sdf &s, Eigen::Vector3f lower_left);

void save_mesh_ply(std::vector<marching_cubes::triangle> &tris, std::string const& filename, bool allow_duplicates=false );
void save_mesh_binary_ply( std::vector<marching_cubes::triangle> tris, std::string const& filename, bool allow_duplicates=false );
void save_mesh_binary_ply(std::vector<marching_cubes::triangle> tris, std::vector<marching_cubes::vertex> points, int element_vertex_count, std::string const& filename);

void save_point_cloud_ply( std::vector<Eigen::Vector3f> pts, std::vector<Eigen::Vector3f> rgbs, std::string const& filename );
void save_point_cloud_binary_ply( std::vector<Eigen::Vector3f> pts, std::vector<Eigen::Vector3f> rgbs, std::string const& filename );

void postprocess_triangles(std::vector<marching_cubes::triangle> &tris, bool allow_duplicates, std::vector<marching_cubes::vertex> &points, Eigen::Vector3f &point_coord_mean, int &element_vertex_count);

#endif // MARCHING_CUBES_HPP


#include <iostream>
#include <fstream>
#include <cstring>
#include <cassert>
#include <vector>
#include <unordered_map>
#include <chrono>
#include "hyperKMP.hpp"

#include <emmintrin.h>
#include <pmmintrin.h>
#include <smmintrin.h>

#include <boost/iostreams/device/mapped_file.hpp>
//---------------------------------------------------------------------------
#define HYPER_MODE 0
#define NORMAL_MODE 1
#define TEST_MODE 2
//---------------------------------------------------------------------------
using namespace std;
//---------------------------------------------------------------------------
hyperKMP::hyperKMP() {
}
//---------------------------------------------------------------------------
hyperKMP::hyperKMP(char* pattern, unsigned mode) {
  this->mode = mode;
  this->length = strlen(pattern);
  this->pattern = new char[this->length + 2];
  strcpy(this->pattern, pattern); // the last char should be null

  // Transform pattern without underscores
  for (unsigned index = 0; index < this->length; ++index)
      if (this->pattern[index] == '_')
        this->pattern[index] = ' ';
  cerr << "modified pattern: " << this->pattern << endl;

  // For benchmarking
  this->hyperSum = this->hyperMaximum = this->normalSum = this->normalMaximum = 0;

  this->valid = new bool[this->length + 1]();
  this->psi = new unsigned[this->length + 1]();
  this->omega = new unsigned[this->length + 1]();
  this->degrees = new unsigned[this->length + 1]();
  this->edges = new unsigned*[this->length + 1];
  compressPi();

  // Only for benchmark reasons
  this->pi = new unsigned[this->length + 1]();
  normalPi();
}
//---------------------------------------------------------------------------
void hyperKMP::normalPi()
// compute the normal pi[] table
{
  unsigned k = 0;
  for (unsigned q = 1; q < this->length; ++q) {
    while ((k > 0) && (pattern[q] != pattern[k]))
      k = pi[k - 1];
    if (pattern[q] == pattern[k])
      k++;
    pi[q] = k;
  }
#if 1
  cerr << "Debug pi[]" << endl;
  for (unsigned index = 0; index < this->length; ++index) {
    cerr << index << " -> " << pi[index] << endl;
  }
  cerr << endl;
#endif
}
//---------------------------------------------------------------------------
void hyperKMP::compressPi()
// compress pi[] and create two new arrays: psi[] and omega[]
// description in README
{
  // Initialize the first values
  // 0 is the first state. At this step it receives 2 sons
  psi[0] = 0;
  psi[1] = 0;
  degrees[0] = 2;
  // Compute pi and psi
  unsigned k = 0;
  for (unsigned q = 1; q < this->length; ++q) {
    // Compute psi
    while ((k > 0) && (pattern[q] != pattern[k])) {
      k = psi[k];
    }
    if (pattern[q] == pattern[k]) {
      k++;
    }
    // Compress possible path
    // For understandability reasons, keep the formula like this
    // TODO: it could be replaced by: (pattern[q + 1] == pattern[k]) ? psi[k] : k
#if 1
    psi[q + 1] = (!k) ? 0 : ((pattern[q + 1] == pattern[k]) ? psi[k] : k);
#else
    // If no jump, connect directly to 0
    pi[q] = k;
    if (pi[q] == 0) {
      psi[q + 1] = 0;
    } else if (pattern[q + 1] == pattern[pi[q]]) {
      // Try to hang the edge (q, pi[q]) at the root of pi[q]
      psi[q + 1] = psi[pi[q]];
    } else {
      // Put the edge back where it was
      psi[q + 1] = pi[q];
    }
    // build the graph of psi, by counting the degree of each node
#endif
    degrees[psi[q + 1]]++;
  }

  // Alloc the graph and reset the degrees to 0
  for (unsigned q = 0; q <= this->length; ++q) {
    if (degrees[q])
      edges[q] = new unsigned[degrees[q]];
    degrees[q] = 0;
  }
  // Compute the graph
  for (unsigned q = 1; q <= this->length; ++q) {
    edges[psi[q]][degrees[psi[q]]++] = q;
  }

#if 0
  cerr << "Debug" << endl;
  for (unsigned index = 0; index <= this->length; ++index) {
    cerr << "Main node : " << index << " ";
    for (unsigned ptr = 0; ptr < degrees[index]; ++ptr) {
      cerr << edges[index][ptr] << " ";
    }
    cerr << endl;
  }
  cerr << endl;
#endif

  // Compute omega[] with dfs
  unordered_map<char, unsigned> indexes;
  vector<unsigned> activeNodes;
  dfs(0, indexes, activeNodes, 0);
  assert(indexes.empty());
  assert(activeNodes.empty());

#if 1
  cerr << "Debug psi[]" << endl;
  for (unsigned index = 0; index <= this->length; ++index) {
    cerr << index << " with " << pattern[index] << " psi = " << psi[index] << " and omega = " << omega[index] << endl;
  }
  cerr << endl;
#endif

  // delete the graph
  for (unsigned index = 0; index <= this->length; ++index) {
    if (degrees[index])
      delete[] edges[index];
  }
  delete[] degrees;
}
//---------------------------------------------------------------------------
void hyperKMP::dfs(unsigned state, unordered_map<char, unsigned>& indexes, vector<unsigned>& activeNodes, unsigned minimumHalt)
// compute the omega[] in linear time
{
  // Save into the modified recursion stack
  activeNodes.push_back(state);
  valid[state] = true;

  // Additional variables for reset
  bool replaced = false;
  unsigned replacer;
  if (state) {
    // Get the last index of the curren state
    auto iter = indexes.find(state);

    // Is it the first time we see it?
    if (iter == indexes.end()) {
      // Add its index
      indexes[pattern[state]] = state;
    } else {
      // Replace the last with the new position
      replaced = true;
      replacer = iter->second;

      // Unmark the last index and update with the current index
      valid[replacer] = false;
      indexes[pattern[state]] = state;
    }

    // Compute the minimum halt
    while (!valid[activeNodes[minimumHalt]])
      ++minimumHalt;
  }
  omega[state] = activeNodes[minimumHalt];

  // Explore the neighbours
  for (unsigned index = 0; index < degrees[state]; ++index)
    dfs(edges[state][index], indexes, activeNodes, minimumHalt);

  // Reset the stack
  activeNodes.pop_back();
  valid[state] = false;
  if (state) {
    if (!replaced) {
      // Erase the state
      indexes.erase(pattern[state]);
    } else {
      // Mark the last index and update it
      valid[replacer] = true;
      indexes[pattern[state]] = replacer;
    }
  }
}
//---------------------------------------------------------------------------
bool hyperKMP::search(const char* str, unsigned n)
// checks if the pattern can be found in str
{
  if (this->length > n)
    return false;

  switch (this->mode) {
    case HYPER_MODE : {
      unsigned q = 0; // current state
      for (unsigned index = 0; index < n; ++index) {
        // Search only on psi now
#if 1
        while ((q > 0) && (str[index] != pattern[q]))
          q = psi[q];
#else
        // lastHalt represents the last state which should be checked (starting from q)
        unsigned lastHalt = omega[q];
        while ((q > 0) && (str[index] != pattern[q]))
          q = psi[q];
#endif
        if (str[index] == pattern[q]) {
          ++q;
        }
        // Found?
        if (q == this->length)
          return true;
      }
      return false;
    }
    case NORMAL_MODE : {
      unsigned q = 0; // current state
      for (unsigned index = 0; index < n; ++index) {
        while ((q > 0) && (str[index] != pattern[q]))
          q = pi[q - 1];
        if (str[index] == pattern[q]) {
          ++q;
        }
        // Found?
        if (q == this->length)
          return true;
      }
      return false;
    }
    case TEST_MODE : {
      unsigned q = 0;
      unsigned hyperLoopsCtr, normalLoopsCtr;
      for (unsigned index = 0; index < n; ++index) {
        hyperLoopsCtr = 0, normalLoopsCtr = 0;
        // lastHalt represents the last state which should be discovered (starting from q)
        unsigned before = q;
        unsigned lastHalt = omega[q];
        while ((q > lastHalt) && (str[index] != pattern[q])) {
          q = psi[q];
          hyperLoopsCtr++;
        }
        unsigned after = q;

        q = before;
        //if (q >= this->length / 2 && str[index] != pattern[q] && pi[q - 1] != 0) cerr << '#' << endl;
        while ((q > 0) && (str[index] != pattern[q])) {
          q = pi[q - 1];
          normalLoopsCtr++;
        }
        assert(q == after);
#if 0
        //cerr << q << " vs " << after << endl;
        if (q != after) {
          cerr << "assert : at " << index << " " << q << " vs " << after << endl;
          assert(0);
        }
#endif
        // Compute the average and the maximum loop steps
        hyperSum += hyperLoopsCtr;
        normalSum += normalLoopsCtr;
        hyperMaximum = std::max(hyperMaximum, hyperLoopsCtr);
        normalMaximum = std::max(normalMaximum, normalLoopsCtr);

        // Continue after testing
        if (str[index] == pattern[q]) {
          ++q;
        }
        // Found?
        if (q == this->length) {
          return true;
        }
      }
      return false;
    }
  }
  cerr << "Something went wrong!" << endl;
  return false;
}
//---------------------------------------------------------------------------
void hyperKMP::benchmark() {
  cerr << "Show benchmark" << endl;
  cerr << "Hyper: sum = " << hyperSum << ", max = " << hyperMaximum << endl;
  cerr << "Normal: sum = " << normalSum << ", max = " << normalMaximum << endl;
  cerr << "Win " << (normalSum - hyperSum) << " comparisons saved. Relative saving = " << 100 * ((double)(normalSum - hyperSum) / normalSum) << "%" << endl;
}
//---------------------------------------------------------------------------
static inline const char* findNl(const char* reader, const char* readerLimit)
{
#if 0
   auto bars=_mm_set1_epi8('\n');
   auto limit=readerLimit-16;
   for (;reader<=limit;reader+=16) {
      auto nextChars=_mm_lddqu_si128(reinterpret_cast<const __m128i*>(reader));
      auto cmp=_mm_cmpeq_epi8(nextChars,bars);
      unsigned mask=_mm_movemask_epi8(cmp);
      if (mask)
         return reader=reader+__builtin_ctzl(mask);
   }
   for (;reader<=readerLimit;++reader)
      if ((*reader)=='\n')
         return reader;
   return nullptr;
#else
   return static_cast<const char*>(memchr(reader, '\n', readerLimit-reader));
#endif
}
//---------------------------------------------------------------------------
int main(int argc, char** argv) {
  if (argc < 4) {
    cerr << "Usage: " << argv[0] << "\n<pattern> (spaces are also allowed, simply an underscore instead: 'aa bb' -> 'aa_bb')\n<file>\n<mode>(0 -> hyperKMP, 1 -> normalKMP, 2 -> benchmark to see comparisons)" << endl;
    return 0;
  }
  // ifstream in;
  // in.open(argv[2]);

  unsigned mode = atoi(argv[3]);
  if (mode > TEST_MODE) {
    cerr << "Mode not available!" << endl;
    exit(0);
  }
  hyperKMP hyper(argv[1], mode);

  const char* str;
  unsigned testCases = 0;
  unsigned countMatches = 0;

  boost::iostreams::mapped_file_source f(argv[2]);
  auto fileBegin = f.data();
  auto fileEnd = fileBegin + f.size();
  auto reader = fileBegin;

  auto start = std::chrono::high_resolution_clock::now();
  while (true) {
    if (!reader)
      break;
    ++testCases;

    str = reader;
    unsigned n;
    {
      auto eol = findNl(reader, fileEnd);
      n = eol ? (eol - reader) : (fileEnd - reader);
      reader = eol? (eol + 1) : nullptr;
    }
    countMatches += hyper.search(str, n);
  }
  cout << "Matches = " << countMatches << " out of " << testCases << endl;
  if (mode == TEST_MODE) {
    hyper.benchmark();
  } else {
    auto stop = std::chrono::high_resolution_clock::now();
    auto duration = stop - start;
    cerr << "Mode (0 -> hyperKMP, 1 -> kmp, 2 -> benchmark) " << mode << " took: " << duration.count() * 1e-6 << "ms" << endl;
  }
  return 0;
}


/// \file   walras.hpp
///
/// \brief  A quote is the variable along which trade negotation takes place
///
/// \authors    Maarten P. Scholl
/// \date       2019-04-04
/// \copyright  Copyright 2017-2019 The Institute for New Economic Thinking,
///             Oxford Martin School, University of Oxford
///
///             Licensed under the Apache License, Version 2.0 (the "License");
///             you may not use this file except in compliance with the License.
///             You may obtain a copy of the License at
///
///                 http://www.apache.org/licenses/LICENSE-2.0
///
///             Unless required by applicable law or agreed to in writing,
///             software distributed under the License is distributed on an "AS
///             IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either
///             express or implied. See the License for the specific language
///             governing permissions and limitations under the License.
///
///             You may obtain instructions to fulfill the attribution
///             requirements in CITATION.cff
///
#ifndef ESL_QUOTE_HPP
#define ESL_QUOTE_HPP

#include <variant>
#include <utility>
#include <type_traits>

#include <esl/economics/exchange_rate.hpp>
#include <esl/economics/price.hpp>
#include <esl/exception.hpp>

namespace esl::economics::markets {

    ///
    /// \brief  A quote is a numerical value around which the market is
    ///     organised. Most commonly, this is a monetary value
    ///         (a price), but different markets may use different types of
    ///         quotes such as interest rates in a mortgage market, or a ratio
    ///         (exchange rate) when bartering.
    ///
    ///
    struct quote
    {
    private:
        constexpr void assert_equal_type_(const quote& other) const
        {
            if(type.index() != other.type.index()){
                throw esl::exception("comparing quotes of different types");
            }
        }
    public:
        std::variant<exchange_rate, price> type;

        uint64_t lot;

        explicit quote(const exchange_rate &er = exchange_rate(),
                       uint64_t lot            = 1)
        : type(er)
        , lot(lot)
        {
            if(0 >= lot){
                throw esl::exception("lot size must be strictly positive");
            }
        }

        ///
        /// \param p
        /// \param lot
        explicit quote(const price &p, uint64_t lot = 1)
        : type(p)
        , lot(lot )
        {
            if(0 >= lot){
                throw esl::exception("lot size must be strictly positive");
            }
        }

        explicit quote(double f, const quote &similar)
        {
            *this = std::visit([&] (const auto& k) {
                using variant_ = std::decay_t<decltype(k)>;
                return quote(variant_(f, k), similar.lot);
                }, similar.type);
        }

        quote(const quote &q)
        : type(q.type)
        , lot(q.lot)
        {
            if(0 >= lot){
                throw esl::exception("lot size must be strictly positive");
            }
        }

        ///
        /// \brief  When converting to floating point, we divide by the lot size
        ///         so that we get the quote per unit of goods.
        ///
        /// \tparam floating_point_t_
        /// \return
        template<typename floating_point_t_>
        explicit operator floating_point_t_() const
        {
            return std::visit(
                [this](auto &&arg) { return floating_point_t_(arg) / lot; },
                type);
        }

        ///
        /// \param o
        /// \return
        quote &operator = (const quote &o) = default;

        ///
        /// \param other
        /// \return
        [[nodiscard]] constexpr bool operator == (const quote &other) const
        {
            assert_equal_type_(other);

            return std::visit([&] (const auto& k) {
                using variant_ = std::decay_t<decltype(k)>;

                if(auto *vp = std::get_if<variant_>(&other.type)  ){
                    return (lot * k) == ( (*vp) * other.lot );
                }
                throw esl::exception("quote variants do not match");
            }, type);
        }

        [[nodiscard]] constexpr bool operator != (const quote &other) const
        {
            assert_equal_type_(other);

            return std::visit([&] (const auto& k) {
                using variant_ = std::decay_t<decltype(k)>;
                if(auto *vp = std::get_if<variant_>(&other.type)  ){
                    return (lot * k) != ((*vp) * other.lot );
                }
                throw esl::exception("quote variants do not match");
            }, type);
        }

        [[nodiscard]] constexpr bool operator < (const quote &other) const
        {
            assert_equal_type_(other);

            return std::visit([&] (const auto& k) {
                using variant_ = std::decay_t<decltype(k)>;
                //return (lot * k) < (std::get<variant_>(other.type) * other.lot );
                if(auto *vp = std::get_if<variant_>(&other.type)  ){
                    return (lot * k) < ((*vp) * other.lot );
                }
                throw esl::exception("quote variants do not match");
            }, type);
        }

        [[nodiscard]] constexpr bool operator > (const quote &other) const
        {
            assert_equal_type_(other);

            return std::visit([&] (const auto& k) {
                using variant_ = std::decay_t<decltype(k)>;
                //return (lot * k) > (std::get<variant_>(other.type) * other.lot );
                if(auto *vp = std::get_if<variant_>(&other.type)  ){
                    return (lot * k) > ((*vp) * other.lot );
                }
                throw esl::exception("quote variants do not match");
            }, type);
        }

        [[nodiscard]] constexpr bool operator <= (const quote &other) const
        {
            assert_equal_type_(other);

            return std::visit([&] (const auto& k) {
                using variant_ = std::decay_t<decltype(k)>;
                //return (lot * k) <= (std::get<variant_>(other.type) * other.lot );
                if(auto *vp = std::get_if<variant_>(&other.type)  ){
                    return (lot * k) <= ((*vp) * other.lot );
                }
                throw esl::exception("quote variants do not match");
            }, type);
        }

        [[nodiscard]] constexpr bool operator >= (const quote &other) const
        {
            assert_equal_type_(other);

            return std::visit([&] (const auto& k) {
                using variant_ = std::decay_t<decltype(k)>;
                //return (lot * k) >= (std::get<variant_>(other.type) * other.lot );
                if(auto *vp = std::get_if<variant_>(&other.type)  ){
                    return (lot * k) >= ((*vp) * other.lot );
                }
                throw esl::exception("quote variants do not match");
            }, type);
        }

        template<class archive_t>
        void save(archive_t &archive, const unsigned int version) const
        {
            (void)version;

            archive << BOOST_SERIALIZATION_NVP(lot);
            size_t index_ = type.index();
            archive &BOOST_SERIALIZATION_NVP(index_);
            switch(index_) {
            case 0:
                if(auto *vp = std::get_if<exchange_rate>(&type)  ){
                    archive << boost::serialization::make_nvp(
                        "exchange_rate", (vp));
                    break;
                }
            case 1:
                if(auto *vp = std::get_if<price>(&type)  ){
                    archive << boost::serialization::make_nvp(
                        "price", *vp);
                    break;
                }
            default:
                throw esl::exception("variant quote not supported");
            }
        }

        template<class archive_t>
        void load(archive_t &archive, const unsigned int version)
        {
            (void)version;

            archive >> BOOST_SERIALIZATION_NVP(lot);

            size_t index_ = 0;
            archive >> BOOST_SERIALIZATION_NVP(index_);
            if(0 == index_) {
                exchange_rate re;
                archive >> boost::serialization::make_nvp("exchange_rate", re);
                type.emplace<0>(re);
            } else if(1 == index_) {
                price p;
                archive >> boost::serialization::make_nvp("price", p);
                type.emplace<1>(p);
            }
        }

        template<class archive_t>
        void serialize(archive_t &archive, const unsigned int version)
        {
            boost::serialization::split_member(archive, *this, version);
        }

//        std::ostream &operator << (std::ostream &stream) const
//        {
//            stream << lot << '@';
//            std::visit([&](const auto &elem)
//                       {
//                           stream << elem;
//                       },
//                       type);
//            return stream;
//        }

        friend std::ostream &operator << (std::ostream &stream, const quote &q)
        {
            stream << q.lot << '@';
            std::visit([&](const auto &elem)
                       {
                           stream << elem;
                       },
                       q.type);
            return stream;
        }
    };

}  // namespace esl::economics

#ifdef WITH_MPI
#include <boost/mpi.hpp>
namespace boost { namespace mpi {
    template<>
    struct is_mpi_datatype<esl::economics::markets::quote>
    : public boost::mpl::true_//is_mpi_datatype<std::variant<esl::economics::exchange_rate
             //                             , esl::economics::price>>::value
    {

    };
}}      // namespace boost::mpi
#endif  // WITH_MPI

#endif  // ESL_QUOTE_HPP


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

#include <boost/foreach.hpp>
#include <boost/progress.hpp>

#ifdef _OPENMP // this should really not be a check for Windows, but a check for OpenMP
    #include <omp.h>
#endif

#include "point.hpp"
#include "triangle.hpp"
#include "batchdropcutter.hpp"

namespace ocl
{

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

BatchDropCutter::BatchDropCutter() {
    clpoints = new std::vector<CLPoint>();
    nCalls = 0;
#ifdef _OPENMP
    nthreads = omp_get_num_procs(); // figure out how many cores we have
#endif
    cutter = NULL;
    bucketSize = 1;
    root = new KDTree<Triangle>();
}

BatchDropCutter::~BatchDropCutter() {
    clpoints->clear();
    delete clpoints;
    delete root;
}

void BatchDropCutter::setSTL(const STLSurf &s) {
    std::cout << "bdc::setSTL()\n";
    surf = &s;
    root->setXYDimensions(); // we search for triangles in the XY plane, don't care about Z-coordinate
    root->setBucketSize( bucketSize );
    root->build(s.tris);
    std::cout << "bdc::setSTL() done.\n";
}

void BatchDropCutter::appendPoint(CLPoint& p) {
    clpoints->push_back(p);
}

// drop cutter against all triangles in surface
void BatchDropCutter::dropCutter1() {
    std::cout << "dropCutterSTL1 " << clpoints->size() <<
              " cl-points and " << surf->tris.size() << " triangles...";
    nCalls = 0;
    BOOST_FOREACH(CLPoint &cl, *clpoints) {
        BOOST_FOREACH( const Triangle& t, surf->tris) {// test against all triangles in s
            cutter->dropCutter(cl,t);
            ++nCalls;
        }
    }
    std::cout << "done.\n";
    return;
}

// first search for triangles under the cutter
// then only drop cutter against found triangles
void BatchDropCutter::dropCutter2() {
    std::cout << "dropCutterSTL2 " << clpoints->size() <<
            " cl-points and " << surf->tris.size() << " triangles.\n";
    std::cout.flush();
    nCalls = 0;
    std::list<Triangle> *triangles_under_cutter;
    BOOST_FOREACH(CLPoint &cl, *clpoints) { //loop through each CL-point
        triangles_under_cutter = root->search_cutter_overlap( cutter , &cl);
        BOOST_FOREACH( const Triangle& t, *triangles_under_cutter) {
            cutter->dropCutter(cl,t);
            ++nCalls;
        }
        delete triangles_under_cutter;
    }

    std::cout << "done. " << nCalls << " dropCutter() calls.\n";
    std::cout.flush();
    return;
}

// compared to dropCutter2, add an additional explicit overlap-test before testing triangle
void BatchDropCutter::dropCutter3() {
    std::cout << "dropCutterSTL3 " << clpoints->size() <<
            " cl-points and " << surf->tris.size() << " triangles.\n";
    nCalls = 0;
    boost::progress_display show_progress( clpoints->size() );
    std::list<Triangle> *triangles_under_cutter;
    BOOST_FOREACH(CLPoint &cl, *clpoints) { //loop through each CL-point
        triangles_under_cutter = root->search_cutter_overlap( cutter , &cl);
        BOOST_FOREACH( const Triangle& t, *triangles_under_cutter) {
            if (cutter->overlaps(cl,t)) {
                if ( cl.below(t) ) {
                    cutter->dropCutter(cl,t);
                    ++nCalls;
                }
            }
        }
        ++show_progress;
        delete triangles_under_cutter;
    }

    std::cout << "done. " << nCalls << " dropCutter() calls.\n";
    return;
}

// use OpenMP to share work between threads
void BatchDropCutter::dropCutter4() {
    std::cout << "dropCutterSTL4 " << clpoints->size() <<
            " cl-points and " << surf->tris.size() << " triangles.\n";
    boost::progress_display show_progress( clpoints->size() );
    nCalls = 0;
    int calls=0;
    long int ntris = 0;
    std::list<Triangle>* tris;
    unsigned int n;
    unsigned int Nmax = clpoints->size();
    std::vector<CLPoint>& clref = *clpoints;
    int nloop=0;
#ifdef _OPENMP
    omp_set_num_threads(nthreads); // the constructor sets number of threads right
                                   // or the user can explicitly specify something else
#endif
    std::list<Triangle>::iterator it;
    #pragma omp parallel for shared( nloop, ntris, calls, clref) private(n,tris,it)
        for (n=0;n< Nmax ;n++) { // PARALLEL OpenMP loop!
#ifdef _OPENMP
            if ( n== 0 ) { // first iteration
                if (omp_get_thread_num() == 0 )
                    std::cout << "Number of OpenMP threads = "<< omp_get_num_threads() << "\n";// print out how many threads we are using
            }
#endif
            nloop++;
            tris = root->search_cutter_overlap( cutter, &clref[n] );
//            assert( tris->size() <= ntriangles ); // can't possibly find more triangles than in the STLSurf
            for( it=tris->begin(); it!=tris->end() ; ++it) { // loop over found triangles
                if ( cutter->overlaps(clref[n],*it) ) { // cutter overlap triangle? check
                    if (clref[n].below(*it)) {
                        cutter->vertexDrop( clref[n],*it);
                        ++calls;
                    }
                }
            }
            for( it=tris->begin(); it!=tris->end() ; ++it) { // loop over found triangles
                if ( cutter->overlaps(clref[n],*it) ) { // cutter overlap triangle? check
                    if (clref[n].below(*it))
                        cutter->facetDrop( clref[n],*it);
                }
            }
            for( it=tris->begin(); it!=tris->end() ; ++it) { // loop over found triangles
                if ( cutter->overlaps(clref[n],*it) ) { // cutter overlap triangle? check
                    if (clref[n].below(*it))
                        cutter->edgeDrop( clref[n],*it);
                }
            }
            ntris += tris->size();
            delete( tris );
            ++show_progress;
        } // end OpenMP PARALLEL for
    nCalls = calls;
    std::cout << " " << nCalls << " dropCutter() calls.\n";
    return;
}

// use OpenMP to share work between threads
void BatchDropCutter::dropCutter5() {
    std::cout << "dropCutterSTL5 " << clpoints->size() <<
            " cl-points and " << surf->tris.size() << " triangles.\n";
    boost::progress_display show_progress( clpoints->size() );
    nCalls = 0;
    int calls=0;
    long int ntris = 0;
    std::list<Triangle>* tris;
    unsigned int n;
    unsigned int Nmax = clpoints->size();
    std::vector<CLPoint>& clref = *clpoints;
    int nloop=0;

#ifdef _OPENMP
    omp_set_num_threads(nthreads); // the constructor sets number of threads right
                                   // or the user can explicitly specify something else
#endif
    std::list<Triangle>::iterator it;
    #pragma omp parallel for schedule(dynamic) shared( nloop, ntris, calls, clref ) private(n,tris,it)
        for (n=0;n<Nmax;++n) { // PARALLEL OpenMP loop!
#ifdef _OPENMP
            if ( n== 0 ) { // first iteration
                if (omp_get_thread_num() == 0 )
                    std::cout << "Number of OpenMP threads = "<< omp_get_num_threads() << "\n";
            }
#endif
            nloop++;
            tris = root->search_cutter_overlap( cutter, &clref[n] );
            assert( tris );
//			assert( tris->size() <= ntriangles ); // can't possibly find more triangles than in the STLSurf
            for( it=tris->begin(); it!=tris->end() ; ++it) { // loop over found triangles
                if ( cutter->overlaps(clref[n],*it) ) { // cutter overlap triangle? check
                    if (clref[n].below(*it)) {
                        cutter->dropCutter( clref[n],*it);
                        ++calls;
                    }
                }
            }
            ntris += tris->size();
            delete( tris );
            ++show_progress;
        } // end OpenMP PARALLEL for
    nCalls = calls;
    std::cout << "\n " << nCalls << " dropCutter() calls.\n";
    return;
}

}// end namespace
// end file batchdropcutter.cpp


//=======================================================================
// Copyright (c) Aaron Windsor 2007
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#ifndef __FACE_HANDLES_HPP__
#define __FACE_HANDLES_HPP__

#include <list>
#include <boost/graph/graph_traits.hpp>
#include <boost/shared_ptr.hpp>

// A "face handle" is an optimization meant to serve two purposes in
// the implementation of the Boyer-Myrvold planarity test: (1) it holds
// the partial planar embedding of a particular vertex as it's being
// constructed, and (2) it allows for efficient traversal around the
// outer face of the partial embedding at that particular vertex. A face
// handle is lightweight, just a shared pointer to the actual implementation,
// since it is passed around/copied liberally in the algorithm. It consists
// of an "anchor" (the actual vertex it's associated with) as well as a
// sequence of edges. The functions first_vertex/second_vertex and
// first_edge/second_edge allow fast access to the beginning and end of the
// stored sequence, which allows one to traverse the outer face of the partial
// planar embedding as it's being created.
//
// There are some policies below that define the contents of the face handles:
// in the case no embedding is needed (for example, if one just wants to use
// the Boyer-Myrvold algorithm as a true/false test for planarity, the
// no_embedding class can be passed as the StoreEmbedding policy. Otherwise,
// either std_list (which uses as std::list) or recursive_lazy_list can be
// passed as this policy. recursive_lazy_list has the best theoretical
// performance (O(n) for a sequence of interleaved concatenations and reversals
// of the underlying list), but I've noticed little difference between std_list
// and recursive_lazy_list in my tests, even though using std_list changes
// the worst-case complexity of the planarity test to O(n^2)
//
// Another policy is StoreOldHandlesPolicy, which specifies whether or not
// to keep a record of the previous first/second vertex/edge - this is needed
// if a Kuratowski subgraph needs to be isolated.

namespace boost { namespace graph { namespace detail {

  //face handle policies

  //EmbeddingStorage policy
  struct store_embedding {};
  struct recursive_lazy_list : public store_embedding {};
  struct std_list : public store_embedding {};
  struct no_embedding {};

  //StoreOldHandlesPolicy
  struct store_old_handles {};
  struct no_old_handles {};

  template<typename DataType>
  struct lazy_list_node
  {
    typedef shared_ptr< lazy_list_node<DataType> > ptr_t;

    lazy_list_node(const DataType& data) :
      m_reversed(false),
      m_data(data),
      m_has_data(true)
    {}

    lazy_list_node(ptr_t left_child, ptr_t right_child) :
      m_reversed(false),
      m_has_data(false),
      m_left_child(left_child),
      m_right_child(right_child)
    {}

    bool m_reversed;
    DataType m_data;
    bool m_has_data;
    shared_ptr<lazy_list_node> m_left_child;
    shared_ptr<lazy_list_node> m_right_child;
  };

  template <typename StoreOldHandlesPolicy, typename Vertex, typename Edge>
  struct old_handles_storage;

  template <typename Vertex, typename Edge>
  struct old_handles_storage<store_old_handles, Vertex, Edge>
  {
    Vertex first_vertex;
    Vertex second_vertex;
    Edge first_edge;
    Edge second_edge;
  };

  template <typename Vertex, typename Edge>
  struct old_handles_storage<no_old_handles, Vertex, Edge>
  {};

  template <typename StoreEmbeddingPolicy, typename Edge>
  struct edge_list_storage;

  template <typename Edge>
  struct edge_list_storage<no_embedding, Edge>
  {
    typedef void type;

    void push_back(Edge) {}
    void push_front(Edge) {}
    void reverse() {}
    void concat_front(edge_list_storage<no_embedding,Edge>) {}
    void concat_back(edge_list_storage<no_embedding,Edge>) {}
    template <typename OutputIterator>
    void get_list(OutputIterator) {}
  };

  template <typename Edge>
  struct edge_list_storage<recursive_lazy_list, Edge>
  {
    typedef lazy_list_node<Edge> node_type;
    typedef shared_ptr< node_type > type;
    type value;

    void push_back(Edge e)
    {
      type new_node(new node_type(e));
      value = type(new node_type(value, new_node));
    }

    void push_front(Edge e)
    {
      type new_node(new node_type(e));
      value = type(new node_type(new_node, value));
    }

    void reverse()
    {
      value->m_reversed = !value->m_reversed;
    }

    void concat_front(edge_list_storage<recursive_lazy_list, Edge> other)
    {
      value = type(new node_type(other.value, value));
    }

    void concat_back(edge_list_storage<recursive_lazy_list, Edge> other)
    {
      value = type(new node_type(value, other.value));
    }

    template <typename OutputIterator>
    void get_list(OutputIterator out)
    {
      get_list_helper(out, value);
    }

  private:

    template <typename OutputIterator>
    void get_list_helper(OutputIterator o_itr,
                         type root,
                         bool flipped = false
                         )
    {
      if (!root)
        return;

      if (root->m_has_data)
        *o_itr = root->m_data;

      if ((flipped && !root->m_reversed) ||
          (!flipped && root->m_reversed)
          )
        {
          get_list_helper(o_itr, root->m_right_child, true);
          get_list_helper(o_itr, root->m_left_child, true);
        }
      else
        {
          get_list_helper(o_itr, root->m_left_child, false);
          get_list_helper(o_itr, root->m_right_child, false);
        }

    }

  };

  template <typename Edge>
  struct edge_list_storage<std_list, Edge>
  {
    typedef std::list<Edge> type;
    type value;

    void push_back(Edge e)
    {
      value.push_back(e);
    }

    void push_front(Edge e)
    {
      value.push_front(e);
    }

    void reverse()
    {
      value.reverse();
    }

    void concat_front(edge_list_storage<std_list,Edge> other)
    {
      value.splice(value.begin(), other.value);
    }

    void concat_back(edge_list_storage<std_list, Edge> other)
    {
      value.splice(value.end(), other.value);
    }

    template <typename OutputIterator>
    void get_list(OutputIterator out)
    {
      std::copy(value.begin(), value.end(), out);
    }

  };

  template<typename Graph,
           typename StoreOldHandlesPolicy,
           typename StoreEmbeddingPolicy
           >
  struct face_handle_impl
  {
    typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;
    typedef typename graph_traits<Graph>::edge_descriptor edge_t;
    typedef typename edge_list_storage<StoreEmbeddingPolicy, edge_t>::type
      edge_list_storage_t;

    face_handle_impl() :
      cached_first_vertex(graph_traits<Graph>::null_vertex()),
      cached_second_vertex(graph_traits<Graph>::null_vertex()),
      true_first_vertex(graph_traits<Graph>::null_vertex()),
      true_second_vertex(graph_traits<Graph>::null_vertex()),
      anchor(graph_traits<Graph>::null_vertex())
    {
      initialize_old_vertices_dispatch(StoreOldHandlesPolicy());
    }

    void initialize_old_vertices_dispatch(store_old_handles)
    {
      old_handles.first_vertex = graph_traits<Graph>::null_vertex();
      old_handles.second_vertex = graph_traits<Graph>::null_vertex();
    }

    void initialize_old_vertices_dispatch(no_old_handles) {}

    vertex_t cached_first_vertex;
    vertex_t cached_second_vertex;
    vertex_t true_first_vertex;
    vertex_t true_second_vertex;
    vertex_t anchor;
    edge_t cached_first_edge;
    edge_t cached_second_edge;

    edge_list_storage<StoreEmbeddingPolicy, edge_t> edge_list;
    old_handles_storage<StoreOldHandlesPolicy, vertex_t, edge_t> old_handles;

  };

  template <typename Graph,
            typename StoreOldHandlesPolicy = store_old_handles,
            typename StoreEmbeddingPolicy = recursive_lazy_list
            >
  class face_handle
  {
  public:
    typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;
    typedef typename graph_traits<Graph>::edge_descriptor edge_t;
    typedef face_handle_impl
      <Graph, StoreOldHandlesPolicy, StoreEmbeddingPolicy> impl_t;
    typedef face_handle
      <Graph, StoreOldHandlesPolicy, StoreEmbeddingPolicy> self_t;

    face_handle(vertex_t anchor = graph_traits<Graph>::null_vertex()) :
      pimpl(new impl_t())
    {
      pimpl->anchor = anchor;
    }

    face_handle(vertex_t anchor, edge_t initial_edge, const Graph& g) :
      pimpl(new impl_t())
    {
      vertex_t s(source(initial_edge,g));
      vertex_t t(target(initial_edge,g));
      vertex_t other_vertex = s == anchor ? t : s;
      pimpl->anchor = anchor;
      pimpl->cached_first_edge = initial_edge;
      pimpl->cached_second_edge = initial_edge;
      pimpl->cached_first_vertex = other_vertex;
      pimpl->cached_second_vertex = other_vertex;
      pimpl->true_first_vertex = other_vertex;
      pimpl->true_second_vertex = other_vertex;

      pimpl->edge_list.push_back(initial_edge);
      store_old_face_handles_dispatch(StoreOldHandlesPolicy());
    }

    //default copy construction, assignment okay.

    void push_first(edge_t e, const Graph& g)
    {
      pimpl->edge_list.push_front(e);
      pimpl->cached_first_vertex = pimpl->true_first_vertex =
        source(e, g) == pimpl->anchor ? target(e,g) : source(e,g);
      pimpl->cached_first_edge = e;
    }

    void push_second(edge_t e, const Graph& g)
    {
      pimpl->edge_list.push_back(e);
      pimpl->cached_second_vertex = pimpl->true_second_vertex =
        source(e, g) == pimpl->anchor ? target(e,g) : source(e,g);
      pimpl->cached_second_edge = e;
    }

    inline void store_old_face_handles()
    {
      store_old_face_handles_dispatch(StoreOldHandlesPolicy());
    }

    inline vertex_t first_vertex() const
    {
      return pimpl->cached_first_vertex;
    }

    inline vertex_t second_vertex() const
    {
      return pimpl->cached_second_vertex;
    }

    inline vertex_t true_first_vertex() const
    {
      return pimpl->true_first_vertex;
    }

    inline vertex_t true_second_vertex() const
    {
      return pimpl->true_second_vertex;
    }

    inline vertex_t old_first_vertex() const
    {
      return pimpl->old_handles.first_vertex;
    }

    inline vertex_t old_second_vertex() const
    {
      return pimpl->old_handles.second_vertex;
    }

    inline edge_t old_first_edge() const
    {
      return pimpl->old_handles.first_edge;
    }

    inline edge_t old_second_edge() const
    {
      return pimpl->old_handles.second_edge;
    }

    inline edge_t first_edge() const
    {
      return pimpl->cached_first_edge;
    }

    inline edge_t second_edge() const
    {
      return pimpl->cached_second_edge;
    }

    inline vertex_t get_anchor() const
    {
      return pimpl->anchor;
    }

    void glue_first_to_second
      (face_handle<Graph,StoreOldHandlesPolicy,StoreEmbeddingPolicy>& bottom)
    {
      pimpl->edge_list.concat_front(bottom.pimpl->edge_list);
      pimpl->true_first_vertex = bottom.pimpl->true_first_vertex;
      pimpl->cached_first_vertex = bottom.pimpl->cached_first_vertex;
      pimpl->cached_first_edge = bottom.pimpl->cached_first_edge;
    }

    void glue_second_to_first
      (face_handle<Graph,StoreOldHandlesPolicy,StoreEmbeddingPolicy>& bottom)
    {
      pimpl->edge_list.concat_back(bottom.pimpl->edge_list);
      pimpl->true_second_vertex = bottom.pimpl->true_second_vertex;
      pimpl->cached_second_vertex = bottom.pimpl->cached_second_vertex;
      pimpl->cached_second_edge = bottom.pimpl->cached_second_edge;
    }

    void flip()
    {
      pimpl->edge_list.reverse();
      std::swap(pimpl->true_first_vertex, pimpl->true_second_vertex);
      std::swap(pimpl->cached_first_vertex, pimpl->cached_second_vertex);
      std::swap(pimpl->cached_first_edge, pimpl->cached_second_edge);
    }

    template <typename OutputIterator>
    void get_list(OutputIterator o_itr)
    {
      pimpl->edge_list.get_list(o_itr);
    }

    void reset_vertex_cache()
    {
      pimpl->cached_first_vertex = pimpl->true_first_vertex;
      pimpl->cached_second_vertex = pimpl->true_second_vertex;
    }

    inline void set_first_vertex(vertex_t v)
    {
      pimpl->cached_first_vertex = v;
    }

    inline void set_second_vertex(vertex_t v)
    {
      pimpl->cached_second_vertex = v;
    }

  private:

    void store_old_face_handles_dispatch(store_old_handles)
    {
      pimpl->old_handles.first_vertex = pimpl->true_first_vertex;
      pimpl->old_handles.second_vertex = pimpl->true_second_vertex;
      pimpl->old_handles.first_edge = pimpl->cached_first_edge;
      pimpl->old_handles.second_edge = pimpl->cached_second_edge;
    }

    void store_old_face_handles_dispatch(no_old_handles) {}

    boost::shared_ptr<impl_t> pimpl;

  };

} /* namespace detail */ } /* namespace graph */ } /* namespace boost */

#endif //__FACE_HANDLES_HPP__