////////////////////////////////////////////////////////////////////////////////
// Fields.hh
////////////////////////////////////////////////////////////////////////////////
/*! @file
//  Classes implementing vector/scalar/tensor fields. Each class also specifies
//  how the quantities are flattened into a single 1D array. Field samples are
//  stored as columns of a dim x |D| 2D array that is then flattened in column
//  major format. Here |D| is the size of the discrete domain.
//
//  This means, for symmetric tensor fields, there are two flattenings: first
//  each sample is flattened into a 6-vector (in 3D) using Voigt notation, then
//  each 6-vector is stored as a column in a 6 x |D| array, which is flattened
//  into a 6 |D| vector.
//
//  For vector fields, the resulting flattened vector looks like:
//      [v_0x, v_0y, v_0z, v_1x, ..., v_|D|z]
//  This vector can be obtained with the getFlattened() method.
*/
//  Author:  Julian Panetta (jpanetta), julian.panetta@gmail.com
//  Company:  New York University
//  Created:  02/13/2013 16:27:14
////////////////////////////////////////////////////////////////////////////////
#ifndef FIELDS_HH
#define FIELDS_HH
#include <Eigen/Dense>
#include <vector>
#include <string>
#include <cassert>
#include <algorithm>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <stdexcept>
#include <cmath>
#include <limits>

#include "Flattening.hh"
#include "SymmetricMatrix.hh"

#include "Algebra.hh"

typedef enum { FIELD_SCALAR, FIELD_VECTOR, FIELD_MATRIX} FieldType;
enum class DomainType { PER_ELEMENT = 0, PER_NODE = 1, ANY = 3, GUESS = 3, UNKNOWN = -1};

template<typename Real, size_t t_dim>
class VectorField : public VectorSpace<Real, VectorField<Real, t_dim>> {
public:
    typedef Eigen::Matrix<Real, Eigen::Dynamic, 1> FlattenedType;
    typedef Eigen::Matrix<Real, t_dim, Eigen::Dynamic> ArrayType;
    typedef typename ArrayType::ColXpr         ValueType;
    typedef typename ArrayType::ConstColXpr    ConstValueType;

    // Copy and move constructors/assignment
    VectorField(const VectorField  &b) = default;
    VectorField(      VectorField &&b) = default;
    VectorField &operator=(const VectorField  &b) = default;
    VectorField &operator=(      VectorField &&b) = default;

    // Flattened data constructor
    // Note: copies data
    VectorField(const FlattenedType &values) {
        size_t domainSize = values.rows() / t_dim;
        assert(t_dim * domainSize == (size_t) values.rows());
        m_values = Eigen::Map<const ArrayType>(values.data(), t_dim,
                                               domainSize);
    }

    // Flattened data constructor (std::vector version)
    template<typename Real2>
    VectorField(const std::vector<Real2> &values) {
        size_t domainSize = values.size() / t_dim;
        if (t_dim * domainSize != values.size())
            throw std::runtime_error("Invalid flattened field size (not an " + std::to_string(t_dim) + "D field)");
        m_values = Eigen::Map<const Eigen::Matrix<Real2, t_dim, Eigen::Dynamic> >
            (&values[0], t_dim, domainSize);
    }

    // Uninitialized allocation constructor
    explicit VectorField(size_t domainSize = 0)
        : m_values(t_dim, domainSize) { }

    ConstValueType operator()(size_t i) const {
        assert(i < (size_t) m_values.cols());
        return m_values.col(i);
    }

    ValueType operator()(size_t i) {
        assert(i < (size_t) m_values.cols());
        return m_values.col(i);
    }

    void clear() { m_values = ArrayType::Zero(dim(), domainSize()); }

    ////////////////////////////////////////////////////////////////////////////
    // VectorSpace requirements
    ////////////////////////////////////////////////////////////////////////////
    void Add(const VectorField &b) { m_values += b.m_values; }
    void Scale(Real scalar)        { m_values *= scalar; }

    // Normalize data so that the maximum column magnitude is 1.
    void maxColumnNormalize() { m_values /= maxMag(); }

    ////////////////////////////////////////////////////////////////////////////
    // Non-VectorSpace operations.
    ////////////////////////////////////////////////////////////////////////////
    Real maxMag() const {
        Real maxNorm = 0;
        for (size_t i = 0; i < domainSize(); ++i)
            maxNorm = std::max(maxNorm, m_values.col(i).norm());
        return maxNorm;
    }

    Real minMag() const {
        Real minNorm = std::numeric_limits<Real>::max();
        for (size_t i = 0; i < domainSize(); ++i)
            minNorm = std::min(minNorm, m_values.col(i).norm());
        return minNorm;
    }

    // Component wise abs.
    VectorField cwiseAbs() const { auto r = VectorField(*this); r.m_values = r.m_values.cwiseAbs(); return r; }

    // Set all coefficients to a constant
    void setConstant(Real val) { m_values.setConstant(val); }

    // Sum of squared norms of each vector.
    Real frobeniusNormSq() const {
        Real normSq = 0;
        for (size_t i = 0; i < domainSize(); ++i)
            normSq += m_values.col(i).squaredNorm();
        return normSq;
    }

    // Scalar-valued inner product between vector field: apply dot product
    // pointwise and sum.
    Real innerProduct(const VectorField &b) const {
        assert(domainSize() == b.domainSize());
        Real result = 0;
        for (size_t i = 0; i < domainSize(); ++i)
            result += m_values.col(i).dot(b.m_values.col(i));
        return result;
    }

    // Unweighted mean vector.
    Eigen::Matrix<Real, t_dim, 1> mean() const {
        Eigen::Matrix<Real, t_dim, 1> result;
        result.setZero();
        for (size_t i = 0; i < domainSize(); ++i)
            result += m_values.col(i);
        result *= (1.0 / domainSize());
        return result;
    }

    const ArrayType &data() const { return m_values; }
          ArrayType &data()       { return m_values; }

    size_t dim() const { return t_dim; }
    size_t N()   const { return dim(); }
    size_t domainSize() const { return m_values.cols(); }
    FieldType fieldType() const { return FIELD_VECTOR; }

    void resizeDomain(size_t dSize) {
        m_values.resize(Eigen::NoChange, dSize);
        clear();
    }

    // Flattened access
    size_t size() const { return dim() * domainSize(); }
    void resize(size_t i) { assert(i % dim() == 0); resizeDomain(i / dim()); }
          Real &operator[](size_t i)       { assert(i < size()); return m_values.data()[i]; }
    const Real &operator[](size_t i) const { assert(i < size()); return m_values.data()[i]; }

    template<typename Real2>
    void getFlattened(std::vector<Real2> &v) const {
        v.resize(size());
        for (size_t i = 0; i < size(); ++i)
            v[i] = operator[](i);
    }

    void print(std::ostream &os, const std::string &componentSeparator = "\t",
               const std::string &elementPrefix = "",
               const std::string &elementSuffix = "\n",
               const std::string &elementSeparator = "") const {
        for (size_t i = 0; i < domainSize(); ++i) {
            if (i) os << elementSeparator;
            ConstValueType v = (*this)(i);
            os << elementPrefix << v[0];
            for (size_t j = 1; j < t_dim; ++j) {
                os << componentSeparator << v[j];
            }
            os << elementSuffix;
        }
    }

    void dump(const std::string &path) const {
        std::ofstream of(path);
        if (!of.is_open())
            throw std::runtime_error(std::string("Couldn't open '") +
                        path + "' for writing.");
        of << std::scientific << std::setprecision(16);
        print(of);
    }

protected:
    /** Data storage */
    ArrayType m_values;
};

template<typename Real>
class ScalarField : public VectorField<Real, 1> {
public:
    using typename VectorField<Real, 1>::FlattenedType;
    typedef Real value_type;

    // ScalarField's value type should act both like a vector (to mimic
    // base class VectorField<Real, 1>) and like a scalar (via typecasts)
    class ValueType {
    public:
        ValueType(Real &val) : m_val(val) { }
              Real &operator[](size_t i)       { (void) (i); assert(i == 0); return m_val; }
        const Real &operator[](size_t i) const { (void) (i); assert(i == 0); return m_val; }
        operator Real&()      { return m_val; }
        operator Real() const { return m_val; }
        ValueType &operator=(Real val) { m_val = val; return *this; }
    private:
        Real &m_val;
    };
    class ConstValueType {
    public:
        ConstValueType(const Real &val) : m_val(val) { }
        Real  operator[](size_t i) const { (void) i; assert(i == 0); return m_val; }
        operator Real() const { return m_val; }
    private:
        const Real &m_val;
    };

    ScalarField(const FlattenedType &values)
        : VectorField<Real, 1>(values) { }
    explicit ScalarField(size_t domainSize = 0)
        : VectorField<Real, 1>(domainSize) { }
    template<typename Real2>
    ScalarField(const std::vector<Real2> &values)
        : VectorField<Real, 1>(values) { }
    // Allow construction from 1-dim vector.
    template<typename Real2>
    ScalarField(const VectorField<Real2, 1> &values)
        : VectorField<Real, 1>(values) { }

    FieldType fieldType() const { return FIELD_SCALAR; }

    Real squaredNorm() const { return m_values.squaredNorm(); }
    Real norm() const { return m_values.norm(); }
    Real  sum() const { return m_values.sum(); }
    Real  min() const { return m_values.minCoeff(); }
    Real  max() const { return m_values.maxCoeff(); }

    // Return the entry with maximum/minimum magnitude
    Real minMag() const { Real m = min(), M = max(); return (std::abs(m) < M) ? m : M; }
    Real maxMag() const { Real m = min(), M = max(); return (std::abs(m) > M) ? m : M; }

    // Component wise abs.
    ScalarField cwiseAbs() const { auto r = ScalarField(*this); r.m_values = r.m_values.cwiseAbs(); return r; }

    // operator() should return numbers rather than column vectors...
    ConstValueType operator()(size_t i) const {
        assert(i < (size_t) m_values.cols());
        return ConstValueType(m_values(0, i));
    }
    ValueType operator()(size_t i) {
        assert(i < (size_t) m_values.cols());
        return ValueType(m_values(0, i));
    }

    const Real *data() const { return m_values.data(); }
          Real *data()       { return m_values.data(); }
    template<size_t dim>
    VectorField<Real, dim> unflatten() const {
        return VectorField<Real, dim>(m_values);
    }

    const typename VectorField<Real, 1>::ArrayType &values() const { return m_values; }

    void minRelax(const ScalarField<Real> &b) { m_values = m_values.cwiseMin(b.m_values); }
    void maxRelax(const ScalarField<Real> &b) { m_values = m_values.cwiseMax(b.m_values); }
    void minRelax(Real b) { m_values = m_values.cwiseMin(b); }
    void maxRelax(Real b) { m_values = m_values.cwiseMax(b); }

private:
    using VectorField<Real, 1>::m_values;
};

// Handles both VectorField and ScalarField output.
template<typename Real, size_t N>
std::ostream &operator<<(std::ostream &os, const VectorField<Real, N> &vf) {
    for (size_t i = 0; i < vf.domainSize(); ++i) {
        for (size_t c = 0; c < N; ++c) {
            os << (c ? "\t" : "") << vf(i)[c];
        }
        os << std::endl;
    }

    return os;
}

// Symmetric matrix NxN fields need only store the upper triangle of the NxN
// matrix. This triangle is flattened into a 1D vector following Voigt notation.
//  [ 0 2 ]   [ 0 5 4 ]  ...  [ 0  N*(N+1)/2 -1  ]
//  [   1 ]   [   1 3 ]       [    1             ]
//            [     2 ]       [        2     ... ]
//                            [         ..   N+1 ]
//                            [           .. N   ]
//                            [              N-1 ]
// This is the typical stress/strain flattening that
// collects the diagonal xx, yy, ... entries at the beginning
// The total number of entries is sum_{i=1}^N i = (N * (N + 1)) / 2
// (because there are i entries in the ith column).
template<typename Real, size_t t_N>
class SymmetricMatrixField : public VectorSpace<Real, SymmetricMatrixField<Real, t_N>> {
public:
    typedef Eigen::Matrix<Real, Eigen::Dynamic, 1> FlattenedType;
    typedef Eigen::Matrix<Real, flatLen(t_N), Eigen::Dynamic> ArrayType;

    typedef SymmetricMatrixRef<t_N, typename ArrayType::ColXpr,
            const typename ArrayType::ColXpr> ValueType;
    typedef ConstSymmetricMatrixRef<t_N,
            typename ArrayType::ConstColXpr> ConstValueType;

    SymmetricMatrixField(const SymmetricMatrixField &b) : m_values(b.m_values) { }

    SymmetricMatrixField(size_t domainSize, const FlattenedType &values) {
        assert(dim() * domainSize == values.rows());
        m_values = Eigen::Map<const ArrayType>(values.data(), dim(),
                                               domainSize);
    }

    // Eigen ArrayType constructor
    SymmetricMatrixField(const ArrayType values) : m_values(values) { }

    SymmetricMatrixField(size_t domainSize = 0)
        : m_values(dim(), domainSize) { }

    constexpr size_t dim() const { return flatLen(t_N); }
    size_t N()   const { return t_N; }
    size_t domainSize() const { return m_values.cols(); }
    FieldType fieldType() const { return FIELD_MATRIX; }

    void clear() { m_values = ArrayType::Zero(dim(), domainSize()); }
    void resizeDomain(size_t dSize) {
        m_values.resize(Eigen::NoChange, dSize);
        clear();
    }

    ConstValueType operator()(size_t i) const {
        return ConstValueType(m_values.col(i));
    }

    ValueType operator()(size_t i) {
        return ValueType(m_values.col(i));
    }

    ////////////////////////////////////////////////////////////////////////////
    // VectorSpace requirements
    ////////////////////////////////////////////////////////////////////////////
    void Add(const SymmetricMatrixField &b) { m_values += b.m_values; }
    void Scale(Real scalar)                 { m_values *= scalar; }

    ////////////////////////////////////////////////////////////////////////////
    // Non-VectorSpace operations.
    ////////////////////////////////////////////////////////////////////////////
    // MHS on Nov 3 3015
    SymmetricMatrixField &operator/=(const ScalarField<Real> &scalars) {
        assert(scalars.domainSize() == size_t(m_values.cols()));
        for (size_t i = 0; i < scalars.domainSize(); ++i)
            m_values.col(i) /= scalars(i);
        return *this;
    }

    // Component wise abs.
    SymmetricMatrixField cwiseAbs() const { return SymmetricMatrixField(m_values.cwiseAbs()); }
    // Set all coefficients to a constant
    void setConstant(Real val) { m_values.setConstant(val); }

    SymmetricMatrixField &operator=(const SymmetricMatrixField &b) {
        if (this == &b) return *this;
        m_values = b.m_values;
        return *this;
    }

    const ArrayType &data() const { return m_values; }
          ArrayType &data()       { return m_values; }

    void dump(const std::string &path) const {
        std::ofstream of(path);
        if (!of.is_open())
            throw std::runtime_error(std::string("Couldn't open '") +
                        path + "' for writing.");
        of << std::scientific << std::setprecision(16);
        for (size_t i = 0; i < domainSize(); ++i) {
            ConstValueType v = (*this)(i);
            of << v[0];
            for (size_t j = 1; j < dim(); ++j) {
                of << '\t' << v[j];
            }
            of << std::endl;
        }
    }

    void load(const std::string &path) {
        std::ifstream is(path);
        if (!is.is_open())
            throw std::runtime_error(std::string("Couldn't open '") + path);

        std::string line;
        std::vector<Real> data;
        while (std::getline(is >> std::ws, line)) {
            std::vector<Real> v;
            std::istringstream iss(line);
            Real c;
            size_t i = 0;
            while (iss >> c) { data.push_back(c); ++i; }
            if (i != dim()) throw std::runtime_error("Read wrong number of components.");
        }
        assert(data.size() % dim() == 0);
        int domSize = data.size() / dim();
        m_values = Eigen::Map<const ArrayType>(&data[0], dim(), domSize);
    }

private:
    /** Data storage */
    ArrayType m_values;
};

template<typename Real, size_t N>
std::ostream &operator<<(std::ostream &os, const SymmetricMatrixField<Real, N> &smf)
{
    for (size_t i = 0; i < smf.domainSize(); ++i) {
        for (size_t c = 0; c < smf.dim(); ++c) {
            os << (c ? "\t" : "") << smf(i)[c];
        }
        os << std::endl;
    }
    return os;
}

// Simple field class that can change dimension but is less efficient/statically
// checked.
// Stores in flattened x0 y0 x1 y1 ... format
template<typename _Real>
class DynamicField {
public:
    DynamicField(size_t dimensions, size_t domSize) {
        resize(dimensions, domSize);
    }

    DynamicField(const DynamicField &b) {
        m_dim = b.m_dim;
        m_storage = b.m_storage;
    }

    template<size_t _N>
    DynamicField(const VectorField<_Real, _N> &vf) {
        resize(vf.dim(), vf.domainSize());
        for (size_t i = 0; i < vf.dim(); ++i)
            for (size_t j = 0; j < vf.domainSize(); ++j)
                (*this)(i, j) = vf(j)[i];
    }

    void resize(size_t domSize) { m_storage.resize(domSize * m_dim); }
    void resize(size_t dim, size_t domSize) { m_dim = dim; resize(domSize); }

    size_t domainSize() const {
        assert(m_storage.size() % m_dim == 0);
        return m_storage.size() / m_dim;
    }

    size_t dim() const { return m_dim; }

    // Flattened access
          _Real &operator[](size_t i)       { return m_storage.at(i); }
    const _Real &operator[](size_t i) const { return m_storage.at(i); }

    _Real &operator()(size_t i, size_t j) {
        if (i >= dim() || j >= domainSize()) throw std::runtime_error("out of bounds access");
        return m_storage.at(j * dim() + i);
    }

    _Real  operator()(size_t i, size_t j) const {
        if (i >= dim() || j >= domainSize()) throw std::runtime_error("out of bounds access");
        return m_storage.at(j * dim() + i);
    }

    // Casts to Field types.
    operator ScalarField<_Real>() const {
        if (m_dim != 1) throw std::runtime_error("Illegal cast of vector field to scalar field.");
        return ScalarField<_Real>(m_storage);
    }
    template<size_t _dim>
    operator VectorField<_Real, _dim>() const {
        if (m_dim != _dim) throw std::runtime_error("Vector field cast dimension mismatch.");
        return VectorField<_Real, _dim>(m_storage);
    }
    template<size_t _dim>
    operator SymmetricMatrixField<_Real, _dim>() const {
        if (m_dim != _dim) throw std::runtime_error("Vector field cast dimension mismatch.");
        return SymmetricMatrixField<_Real, _dim>(m_storage);
    }

    friend std::ostream &operator<<(std::ostream &os, const DynamicField &f) {
        for (size_t j = 0; j < f.domainSize(); ++j) {
            for (size_t i = 0; i < f.dim(); ++i)
                os << (i ? "\t" : "") << f(i, j);
            os << std::endl;
        }
        return os;
    }

private:
    size_t m_dim;
    std::vector<_Real> m_storage;
};

#endif // FIELDS_HH


/* boost random/subtract_with_carry.hpp header file
 *
 * Copyright Jens Maurer 2002
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: subtract_with_carry.hpp 29116 2005-05-21 15:57:01Z dgregor $
 *
 * Revision history
 *  2002-03-02  created
 */

#ifndef BOOST_RANDOM_SUBTRACT_WITH_CARRY_HPP
#define BOOST_RANDOM_SUBTRACT_WITH_CARRY_HPP

#include <cmath>
#include <iostream>
#include <algorithm>     // std::equal
#include <stdexcept>
#include <cmath>         // std::pow
#include <boost/config.hpp>
#include <boost/limits.hpp>
#include <boost/cstdint.hpp>
#include <boost/static_assert.hpp>
#include <boost/detail/workaround.hpp>
#include <boost/random/linear_congruential.hpp>

namespace boost {
namespace random {

#if BOOST_WORKAROUND(_MSC_FULL_VER, BOOST_TESTED_AT(13102292)) && BOOST_MSVC > 1300
#  define BOOST_RANDOM_EXTRACT_SWC_01
#endif

#if defined(__APPLE_CC__) && defined(__GNUC__) && (__GNUC__ == 3) && (__GNUC_MINOR__ <= 3)
#  define BOOST_RANDOM_EXTRACT_SWC_01
#endif

# ifdef BOOST_RANDOM_EXTRACT_SWC_01
namespace detail
{
  template <class IStream, class SubtractWithCarry, class RealType>
  void extract_subtract_with_carry_01(
      IStream& is
      , SubtractWithCarry& f
      , RealType& carry
      , RealType* x
      , RealType modulus)
  {
    RealType value;
    for(unsigned int j = 0; j < f.long_lag; ++j) {
      is >> value >> std::ws;
      x[j] = value / modulus;
    }
    is >> value >> std::ws;
    carry = value / modulus;
  }
}
# endif
// subtract-with-carry generator
// Marsaglia and Zaman

template<class IntType, IntType m, unsigned int s, unsigned int r,
  IntType val>
class subtract_with_carry
{
public:
  typedef IntType result_type;
  BOOST_STATIC_CONSTANT(bool, has_fixed_range = true);
  BOOST_STATIC_CONSTANT(result_type, min_value = 0);
  BOOST_STATIC_CONSTANT(result_type, max_value = m-1);
  BOOST_STATIC_CONSTANT(result_type, modulus = m);
  BOOST_STATIC_CONSTANT(unsigned int, long_lag = r);
  BOOST_STATIC_CONSTANT(unsigned int, short_lag = s);

  subtract_with_carry() {
    // MSVC fails BOOST_STATIC_ASSERT with std::numeric_limits at class scope
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
    BOOST_STATIC_ASSERT(std::numeric_limits<result_type>::is_signed);
    BOOST_STATIC_ASSERT(std::numeric_limits<result_type>::is_integer);
#endif
    seed();
  }
  explicit subtract_with_carry(uint32_t value) { seed(value); }
  template<class Generator>
  explicit subtract_with_carry(Generator & gen) { seed(gen); }
  template<class It> subtract_with_carry(It& first, It last) { seed(first,last); }

  // compiler-generated copy ctor and assignment operator are fine

  void seed(uint32_t value = 19780503u)
  {
    random::linear_congruential<int32_t, 40014, 0, 2147483563, 0> intgen(value);
    seed(intgen);
  }

  // For GCC, moving this function out-of-line prevents inlining, which may
  // reduce overall object code size.  However, MSVC does not grok
  // out-of-line template member functions.
  template<class Generator>
  void seed(Generator & gen)
  {
    // I could have used std::generate_n, but it takes "gen" by value
    for(unsigned int j = 0; j < long_lag; ++j)
      x[j] = gen() % modulus;
    carry = (x[long_lag-1] == 0);
    k = 0;
  }

  template<class It>
  void seed(It& first, It last)
  {
    unsigned int j;
    for(j = 0; j < long_lag && first != last; ++j, ++first)
      x[j] = *first % modulus;
    if(first == last && j < long_lag)
      throw std::invalid_argument("subtract_with_carry::seed");
    carry = (x[long_lag-1] == 0);
    k = 0;
   }

  result_type min BOOST_PREVENT_MACRO_SUBSTITUTION () const { return min_value; }
  result_type max BOOST_PREVENT_MACRO_SUBSTITUTION () const { return max_value; }

  result_type operator()()
  {
    int short_index = k - short_lag;
    if(short_index < 0)
      short_index += long_lag;
    IntType delta;
    if (x[short_index] >= x[k] + carry) {
      // x(n) >= 0
      delta =  x[short_index] - (x[k] + carry);
      carry = 0;
    } else {
      // x(n) < 0
      delta = modulus - x[k] - carry + x[short_index];
      carry = 1;
    }
    x[k] = delta;
    ++k;
    if(k >= long_lag)
      k = 0;
    return delta;
  }

public:
  static bool validation(result_type x) { return x == val; }

#ifndef BOOST_NO_OPERATORS_IN_NAMESPACE

#ifndef BOOST_NO_MEMBER_TEMPLATE_FRIENDS
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os,
             const subtract_with_carry& f)
  {
    for(unsigned int j = 0; j < f.long_lag; ++j)
      os << f.compute(j) << " ";
    os << f.carry << " ";
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, subtract_with_carry& f)
  {
    for(unsigned int j = 0; j < f.long_lag; ++j)
      is >> f.x[j] >> std::ws;
    is >> f.carry >> std::ws;
    f.k = 0;
    return is;
  }
#endif

  friend bool operator==(const subtract_with_carry& x, const subtract_with_carry& y)
  {
    for(unsigned int j = 0; j < r; ++j)
      if(x.compute(j) != y.compute(j))
        return false;
    return true;
  }

  friend bool operator!=(const subtract_with_carry& x, const subtract_with_carry& y)
  { return !(x == y); }
#else
  // Use a member function; Streamable concept not supported.
  bool operator==(const subtract_with_carry& rhs) const
  {
    for(unsigned int j = 0; j < r; ++j)
      if(compute(j) != rhs.compute(j))
        return false;
    return true;
  }

  bool operator!=(const subtract_with_carry& rhs) const
  { return !(*this == rhs); }
#endif

private:
  // returns x(i-r+index), where index is in 0..r-1
  IntType compute(unsigned int index) const
  {
    return x[(k+index) % long_lag];
  }

  // state representation; next output (state) is x(i)
  //   x[0]  ... x[k] x[k+1] ... x[long_lag-1]     represents
  //  x(i-k) ... x(i) x(i+1) ... x(i-k+long_lag-1)
  // speed: base: 20-25 nsec
  // ranlux_4: 230 nsec, ranlux_7: 430 nsec, ranlux_14: 810 nsec
  // This state representation makes operator== and save/restore more
  // difficult, because we've already computed "too much" and thus
  // have to undo some steps to get at x(i-r) etc.

  // state representation: next output (state) is x(i)
  //   x[0]  ... x[k] x[k+1]          ... x[long_lag-1]     represents
  //  x(i-k) ... x(i) x(i-long_lag+1) ... x(i-k-1)
  // speed: base 28 nsec
  // ranlux_4: 370 nsec, ranlux_7: 688 nsec, ranlux_14: 1343 nsec
  IntType x[long_lag];
  unsigned int k;
  int carry;
};

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
//  A definition is required even for integral static constants
template<class IntType, IntType m, unsigned int s, unsigned int r, IntType val>
const bool subtract_with_carry<IntType, m, s, r, val>::has_fixed_range;
template<class IntType, IntType m, unsigned int s, unsigned int r, IntType val>
const IntType subtract_with_carry<IntType, m, s, r, val>::min_value;
template<class IntType, IntType m, unsigned int s, unsigned int r, IntType val>
const IntType subtract_with_carry<IntType, m, s, r, val>::max_value;
template<class IntType, IntType m, unsigned int s, unsigned int r, IntType val>
const IntType subtract_with_carry<IntType, m, s, r, val>::modulus;
template<class IntType, IntType m, unsigned int s, unsigned int r, IntType val>
const unsigned int subtract_with_carry<IntType, m, s, r, val>::long_lag;
template<class IntType, IntType m, unsigned int s, unsigned int r, IntType val>
const unsigned int subtract_with_carry<IntType, m, s, r, val>::short_lag;
#endif

// use a floating-point representation to produce values in [0..1)
template<class RealType, int w, unsigned int s, unsigned int r, int val=0>
class subtract_with_carry_01
{
public:
  typedef RealType result_type;
  BOOST_STATIC_CONSTANT(bool, has_fixed_range = false);
  BOOST_STATIC_CONSTANT(int, word_size = w);
  BOOST_STATIC_CONSTANT(unsigned int, long_lag = r);
  BOOST_STATIC_CONSTANT(unsigned int, short_lag = s);

#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
  BOOST_STATIC_ASSERT(!std::numeric_limits<result_type>::is_integer);
#endif

  subtract_with_carry_01() { init_modulus(); seed(); }
  explicit subtract_with_carry_01(uint32_t value)
  { init_modulus(); seed(value);   }
  template<class It> subtract_with_carry_01(It& first, It last)
  { init_modulus(); seed(first,last); }

private:
  void init_modulus()
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    // allow for Koenig lookup
    using std::pow;
#endif
    _modulus = pow(RealType(2), word_size);
  }

public:
  // compiler-generated copy ctor and assignment operator are fine

  void seed(uint32_t value = 19780503u)
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    // allow for Koenig lookup
    using std::fmod;
#endif
    random::linear_congruential<int32_t, 40014, 0, 2147483563, 0> gen(value);
    unsigned long array[(w+31)/32 * long_lag];
    for(unsigned int j = 0; j < sizeof(array)/sizeof(unsigned long); ++j)
      array[j] = gen();
    unsigned long * start = array;
    seed(start, array + sizeof(array)/sizeof(unsigned long));
  }

  template<class It>
  void seed(It& first, It last)
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    // allow for Koenig lookup
    using std::fmod;
    using std::pow;
#endif
    unsigned long mask = ~((~0u) << (w%32));   // now lowest (w%32) bits set
    RealType two32 = pow(RealType(2), 32);
    unsigned int j;
    for(j = 0; j < long_lag && first != last; ++j) {
      x[j] = RealType(0);
      for(int i = 0; i < w/32 && first != last; ++i, ++first)
        x[j] += *first / pow(two32,i+1);
      if(first != last && mask != 0) {
        x[j] += fmod((*first & mask) / _modulus, RealType(1));
        ++first;
      }
    }
    if(first == last && j < long_lag)
      throw std::invalid_argument("subtract_with_carry_01::seed");
    carry = (x[long_lag-1] ? 0 : 1 / _modulus);
    k = 0;
  }

  result_type min BOOST_PREVENT_MACRO_SUBSTITUTION () const { return result_type(0); }
  result_type max BOOST_PREVENT_MACRO_SUBSTITUTION () const { return result_type(1); }

  result_type operator()()
  {
    int short_index = k - short_lag;
    if(short_index < 0)
      short_index += long_lag;
    RealType delta = x[short_index] - x[k] - carry;
    if(delta < 0) {
      delta += RealType(1);
      carry = RealType(1)/_modulus;
    } else {
      carry = 0;
    }
    x[k] = delta;
    ++k;
    if(k >= long_lag)
      k = 0;
    return delta;
  }

  static bool validation(result_type x)
  { return x == val/pow(RealType(2), word_size); }

#ifndef BOOST_NO_OPERATORS_IN_NAMESPACE

#ifndef BOOST_NO_MEMBER_TEMPLATE_FRIENDS
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os,
             const subtract_with_carry_01& f)
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    // allow for Koenig lookup
    using std::pow;
#endif
    std::ios_base::fmtflags oldflags = os.flags(os.dec | os.fixed | os.left);
    for(unsigned int j = 0; j < f.long_lag; ++j)
      os << (f.compute(j) * f._modulus) << " ";
    os << (f.carry * f._modulus);
    os.flags(oldflags);
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, subtract_with_carry_01& f)
  {
# ifdef BOOST_RANDOM_EXTRACT_SWC_01
      detail::extract_subtract_with_carry_01(is, f, f.carry, f.x, f._modulus);
# else
    // MSVC (up to 7.1) and Borland (up to 5.64) don't handle the template type
    // parameter "RealType" available from the class template scope, so use
    // the member typedef
    typename subtract_with_carry_01::result_type value;
    for(unsigned int j = 0; j < long_lag; ++j) {
      is >> value >> std::ws;
      f.x[j] = value / f._modulus;
    }
    is >> value >> std::ws;
    f.carry = value / f._modulus;
# endif
    f.k = 0;
    return is;
  }
#endif

  friend bool operator==(const subtract_with_carry_01& x,
                         const subtract_with_carry_01& y)
  {
    for(unsigned int j = 0; j < r; ++j)
      if(x.compute(j) != y.compute(j))
        return false;
    return true;
  }

  friend bool operator!=(const subtract_with_carry_01& x,
                         const subtract_with_carry_01& y)
  { return !(x == y); }
#else
  // Use a member function; Streamable concept not supported.
  bool operator==(const subtract_with_carry_01& rhs) const
  {
    for(unsigned int j = 0; j < r; ++j)
      if(compute(j) != rhs.compute(j))
        return false;
    return true;
  }

  bool operator!=(const subtract_with_carry_01& rhs) const
  { return !(*this == rhs); }
#endif

private:
  RealType compute(unsigned int index) const;
  unsigned int k;
  RealType carry;
  RealType x[long_lag];
  RealType _modulus;
};

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
//  A definition is required even for integral static constants
template<class RealType, int w, unsigned int s, unsigned int r, int val>
const bool subtract_with_carry_01<RealType, w, s, r, val>::has_fixed_range;
template<class RealType, int w, unsigned int s, unsigned int r, int val>
const int subtract_with_carry_01<RealType, w, s, r, val>::word_size;
template<class RealType, int w, unsigned int s, unsigned int r, int val>
const unsigned int subtract_with_carry_01<RealType, w, s, r, val>::long_lag;
template<class RealType, int w, unsigned int s, unsigned int r, int val>
const unsigned int subtract_with_carry_01<RealType, w, s, r, val>::short_lag;
#endif

template<class RealType, int w, unsigned int s, unsigned int r, int val>
RealType subtract_with_carry_01<RealType, w, s, r, val>::compute(unsigned int index) const
{
  return x[(k+index) % long_lag];
}

} // namespace random
} // namespace boost

#endif // BOOST_RANDOM_SUBTRACT_WITH_CARRY_HPP


// Copyright 2019 Hans Dembinski
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

//[ guide_fill_weighted_histogram

#include <boost/histogram.hpp>

int main() {
  using namespace boost::histogram;

  // Create a histogram with weight counters that keep track of a variance estimate.
  auto h = make_weighted_histogram(axis::regular<>(3, 0.0, 1.0));

  h(0.0, weight(1)); // weight 1 goes to first bin
  h(0.1, weight(2)); // weight 2 goes to first bin
  h(0.4, weight(3)); // weight 3 goes to second bin
  h(0.5, weight(4)); // weight 4 goes to second bin

  // Weight counters have methods to access the value (sum of weights) and the variance
  // (sum of weights squared, why this gives the variance is explained in the rationale)
  assert(h[0].value() == 1 + 2);
  assert(h[0].variance() == 1 * 1 + 2 * 2);
  assert(h[1].value() == 3 + 4);
  assert(h[1].variance() == 3 * 3 + 4 * 4);
}

//]


// Copyright Abel Sinkovics (abel@sinkovics.hu) 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <mpllibs/metamonad/eval_letrec.hpp>
#include <mpllibs/metamonad/lazy.hpp>
#include <mpllibs/metamonad/syntax.hpp>

#include <boost/test/unit_test.hpp>

#include <boost/mpl/plus.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/assert.hpp>

#include "common.hpp"

BOOST_AUTO_TEST_CASE(test_eval_letrec)
{
  using boost::mpl::equal_to;
  using boost::mpl::plus;

  using mpllibs::metamonad::eval_letrec;
  using mpllibs::metamonad::lazy;
  using mpllibs::metamonad::syntax;

  // test_evaluation_of_expression
  BOOST_MPL_ASSERT((
    equal_to<
      int13,
      eval_letrec<x, syntax<int11>, syntax<lazy<plus<int2, x> > > >::type
    >
  ));
}


//
// Created by serboba on 21.10.21.
//
//
// Created by serboba on 21.10.21.
//

#include <iostream>
#include <fstream>
#include <stdio.h>
#include <stdlib.h>
#include <random>
#include <Eigen/Dense>
#include <robowflex_library/util.h>
#include <robowflex_dart/point_collector.h>
#include <robowflex_dart/quaternion_factory.h>
#include <robowflex_dart/conversion_functions.h>
#include <tf2/LinearMath/Quaternion.h>

using namespace Eigen;

#include <boost/filesystem.hpp>

MatrixXd get_object_vertices(Vector3d joint_pos, Vector3d object_size){
    MatrixXd vertices(8,3);
    vertices <<
             //unten
            joint_pos.x()-object_size.x(), joint_pos.y()-object_size.y(), joint_pos.z()-object_size.z(), //links hinten    0
            joint_pos.x()+object_size.x(), joint_pos.y()-object_size.y(), joint_pos.z()-object_size.z(), //links vorne     1
            joint_pos.x()-object_size.x(), joint_pos.y()+object_size.y(), joint_pos.z()-object_size.z(), // rechts hinten  2
            joint_pos.x()+object_size.x(), joint_pos.y()+object_size.y(), joint_pos.z()-object_size.z(), // rechts vorne   3

            //oben
            joint_pos.x()-object_size.x(), joint_pos.y()-object_size.y(), joint_pos.z()+object_size.z(), //links hinten    4
            joint_pos.x()+object_size.x(), joint_pos.y()-object_size.y(), joint_pos.z()+object_size.z(), //links vorne     5
            joint_pos.x()-object_size.x(), joint_pos.y()+object_size.y(), joint_pos.z()+object_size.z(), //rechts hinten   6
            joint_pos.x()+object_size.x(), joint_pos.y()+object_size.y(), joint_pos.z()+object_size.z(); // rechts vorne   7
    return vertices;
}

MatrixXd get_rotated_vertex(Vector3d obj_rpy, Vector3d point, Vector3d joint_pos ){ // todo correct quaternion value with divide rpy

    MatrixXd q1 = rpy_to_quaternion(obj_rpy[0],obj_rpy[1],obj_rpy[2]);
    return (joint_pos + matrix_to_quaternion(q1)*(point-joint_pos));
}

MatrixXd get_rotated_object(Vector3d obj_rpy, Vector3d joint_pos, Vector3d object_size){ //rotate for all vertices
    MatrixXd vertices = get_object_vertices(joint_pos,object_size);
    for(int i = 0 ; i< vertices.rows(); i++){
        Vector3d row = vertices.row(i);
        MatrixXd new_v = get_rotated_vertex(obj_rpy, row, joint_pos);
        vertices.row(i) << new_v(0),new_v(1),new_v(2);
    }
    return vertices;
}

MatrixXd get_surface_equation(MatrixXd vertices){
    MatrixXd surface_equation(6,9);
    surface_equation <<
            vertices.row(0), vertices.row(1)- vertices.row(0), vertices.row(2)- vertices.row(0), // grundflaeche
            vertices.row(0), vertices.row(4)- vertices.row(0), vertices.row(1)- vertices.row(0), // seite links
            vertices.row(1), vertices.row(5)- vertices.row(1), vertices.row(3)- vertices.row(1), //seite vorne
            vertices.row(2), vertices.row(3)- vertices.row(2), vertices.row(6)- vertices.row(2), //seite rechts
            vertices.row(0), vertices.row(2)- vertices.row(0), vertices.row(4)- vertices.row(0), //seite hinten
            vertices.row(4), vertices.row(5)- vertices.row(4), vertices.row(6)- vertices.row(4); //seite oben
    return surface_equation;
}

MatrixXd extract_surface(int surface_no, MatrixXd surface_equation_matrix){
    return surface_equation_matrix.row(surface_no);
}

MatrixXd adjust_normals(MatrixXd normals){
    for(int i = 0; i < normals.rows() ; i++){
        for(int j = 0; j< normals.cols() ; j++){
            if(abs(normals(i,j)) < 0.01){
                normals(i,j) = 0.0;
            }else{
                if(normals(i,j) > 0.0){
                    normals(i,j) = 0.01;
                }else{
                    normals(i,j) = -0.01;
                }
            }
        }
    }
    return normals;
}

MatrixXd get_normal_of_plane(MatrixXd surface_equation){
    Map<MatrixXd> direction_vector1(surface_equation.data()+6*3,6,3);
    Map<MatrixXd> direction_vector2(surface_equation.data()+6*6,6,3);
    MatrixXd normals(6,3);
    for(int i =0; i < direction_vector1.rows() ; i++){
        Vector3d v1 = Vector3d(direction_vector2.row(i));
        Vector3d v2 = Vector3d(direction_vector1.row(i));
        MatrixXd n_v (1,3);
        n_v = MatrixXd(v1.cross(v2).transpose());
        if(i== 5){
            n_v *= -1;
        } // top surface normal always the opposite

        normals.row(i) << n_v;
    }
    return adjust_normals(normals);
}

MatrixXd sort_quaternion(MatrixXd rpy){ // ROLL =
    MatrixXd quaternions(6,4);
    if (rpy(0) == 0.0 && rpy(1) == 0.0 && rpy(2) == 0.0){
        quaternions<< get_quaternion_unten(rpy),get_quaternion_links(rpy),get_quaternion_hinten(rpy), get_quaternion_rechts(rpy), get_quaternion_vorne(rpy), get_quaternion_oben(rpy);
        return quaternions;
    }
    quaternions <<  quaternion_x_y_z(rpy);
    return quaternions;

}

MatrixXd translate_rotations(MatrixXd rpy){
     return sort_quaternion(rpy);
}

MatrixXd match_quaternion_to_surface(int quaternion_no, int surface_no, MatrixXd rpy_matrix){
    MatrixXd quaternions = translate_rotations(rpy_matrix); // calculate corresponding quaternions for pose (all poses)
    MatrixXd  q = quaternions.row(surface_no).block(0,quaternion_no*4,1,4); // quat row for obj is surface_no*3 + quaternion no
    return q;
}

std::vector<double> get_ratio_dir_vecs(){ // decides how far the point lies on the surface, todo adjust it better
    std::vector<double> ratio;
    srand ( time(NULL) );
    int i =0;
    while(i<2){ // eine ebene hat zwei richtungsvektoren also 2 mal
        double ratio_vec = (rand() %8); // ab 80% viel zu weit aussen der punkt
        if(ratio_vec== 0 || ratio_vec== 1|| ratio_vec== 2) // EDGES WAY TOO HARD TO GRASP AND MOSTLY INACCURATE SO MAKE IT TO THE MIDDLE
            ratio_vec = 5;
        ratio_vec /= 10;
        ratio.push_back(ratio_vec);
        i++;
    }
    return ratio;
}

MatrixXd get_point_position(MatrixXd surface_equation,std::vector<double> ratio_dir_vectors){

    Map<MatrixXd> position_vector(surface_equation.data(),1,3);
    Map<MatrixXd> direction_vector1(surface_equation.data()+3,1,3);
    Map<MatrixXd> direction_vector2(surface_equation.data()+6,1,3);
    direction_vector1 *= ratio_dir_vectors[0];
    direction_vector2 *= ratio_dir_vectors[1];
    return (position_vector+direction_vector1+direction_vector2);
}

MatrixXd get_random_point_from_surface(MatrixXd surface_equation_matrix, MatrixXd rpy_matrix, MatrixXd normals, int surface_no){ // eine ebenengleichung
    srand ( time(NULL) );
   // int surface_no =rand() %2; // SURFACE NUMBER-> NEED IT FOR QUATERNIONS !!! DONT LOSE IT
    std::vector<int> surfs;
    //surfs.push_back(1); // links
  //  surfs.push_back(4); // vorne
    //surfs.push_back(3); // rechts
   // surfs.push_back(5); // oben
   // surface_no = surfs[surface_no];
   surfs.push_back(surface_no);
   //int surface_no =5; // SURFACE NUMBER-> NEED IT FOR QUATERNIONS !!! DONT LOSE IT
    // (TODO) CHOOSED TOP SURFACE TO FASTER SOLUTION !
    int quaternion_no = 0; // todo +2 roll variations maybe but not important
    int surf_q_no = (surface_no*3)+quaternion_no; // surf no w quats
//    std::cout << " SURFACE NO: " << surface_no << std::endl;
//    std::cout << " QUAT NO: " << quaternion_no << std::endl;

    MatrixXd surface_equation = extract_surface(surface_no, surface_equation_matrix);
    MatrixXd quaternion = match_quaternion_to_surface(quaternion_no,surface_no,rpy_matrix);

    MatrixXd position(1,11);
    position << get_point_position(surface_equation,get_ratio_dir_vecs()),quaternion,normals.row(surface_no), surface_no;
    return position;

}

Eigen::MatrixXd get_pose_object(Object &obj, int surface_no){

    auto actual_position = get_rotated_vertex(obj.actual_rotation, (obj.actual_position), obj.joints.pos); // ADJUST JOINT POS W RESPECT TO LINK XYZ
    std::cout <<  "Actual position : " << obj.actual_position << std::endl;
    std::cout <<  "Actual rotation : " << obj.actual_rotation << std::endl;

    auto vertices = get_rotated_object(obj.actual_rotation, actual_position,obj.link.size);
    MatrixXd surface_equation = get_surface_equation(get_rotated_object(obj.actual_rotation, actual_position,obj.link.size));
    MatrixXd normals = get_normal_of_plane(surface_equation);
    MatrixXd rpy_matrix= vec_to_matrix(obj.link.rpy+obj.joints.rpy);
    MatrixXd random_pose = get_random_point_from_surface(surface_equation,rpy_matrix,normals, surface_no);

    return random_pose;
}


#pragma once

#include <boost/hana.hpp>

#include "calc/buttons.hpp"
#include "calc/specs/detail/spec.hpp"
#include "calc/specs/detail/operations.hpp"

namespace calc::buttons::specs {

    namespace hana = boost::hana;

    using hana::_;
    using detail::spec;

    // Concept map for operating on numbers
    // All buttons MUST implement it {*1}
    constexpr auto operator_spec_map = hana::make_map(
        // rhs bound buttons will be passed the number and their rhs as parameters
        // {*2}
        spec          <add> = _ + _,
        spec          <sub> = _ - _,
        spec          <mul> = _ * _,
        spec          <div> = detail::div,
        spec        <power> = detail::pow,
        spec       <append> = detail::append,
        spec     <meta_add> = hana::arg<1>,
        // stateful buttons will be passed the number and their own instance as parameters
        // {*3}
        spec      <replace> = detail::replace,
        spec        <store> = detail::store,
        // other buttons will only be passed the number to operate on
        // {*4}
        spec  <rotate_left> = detail::rotate_left,
        spec <rotate_right> = detail::rotate_right,
        spec   <erase_last> = _ / 10,
        spec     <opposite> = - _,
        spec      <reverse> = detail::reverse,
        spec          <sum> = detail::sum,
        spec       <mirror> = detail::mirror,
        spec        <inv10> = detail::inv10
    );

    constexpr auto mutate_number = [](number_t lhs, auto button) -> number_t {
        using namespace traits;
        constexpr auto t_button = hana::decltype_(button);

        // {*1}
        static_assert(
            t_button ^hana::in^ operator_spec_map,
            "Missing operator spec for button"
        );

        constexpr auto op_spec = operator_spec_map[t_button];

        // {*2}
        if constexpr (is_rhs_bound(t_button))
            return op_spec(lhs, button.rhs);
        // {*3}
        else if constexpr (is_stateful(t_button))
            return op_spec(lhs, button);
        // {*4}
        else
            return op_spec(lhs);
    };

    // Concept map for operating on the list of buttons
    // Implementation is optional
    // Buttons who choose to not implement it will simply let the list intact
    // {*5}
    constexpr auto buttons_modifier_spec_map = hana::make_map(
        // Implementers will be passed the list of buttons and their own instance as parameters
        // {*6}
        spec <meta_add> = detail::meta_add
    );

    constexpr auto mutate_buttons = [](const buttons_t & buttons, auto button) -> buttons_t {
        constexpr auto t_button = hana::decltype_(button);

        // {*6}
        if constexpr (t_button ^hana::in^ buttons_modifier_spec_map)
            return buttons_modifier_spec_map[t_button](buttons, button);
        // {*5}
        else
            return buttons;
    };

    constexpr auto press = [](const calculator & state, auto button) -> calculator {
        return {
            mutate_buttons(state.buttons, button),
            mutate_number(state.total, button),
            state.move_count + 1
        };
    };

}


#include <assert.h>
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>

#include <NTL/GF2X.h>
#include <NTL/GF2XFactoring.h>
#include <NTL/GF2E.h>
#include <NTL/GF2EX.h>
#include <NTL/GF2EXFactoring.h>

#include "gcm.h"

using namespace std;
using namespace NTL;

int main(int argc, char **argv)
{
    struct slice a1, c1, t1, a2, c2, t2;
    struct slice *sp[] = {&a1, &c1, &t1, &a2, &c2, &t2};
    int i;
    GF2EX p, p1, p2;
    vec_pair_GF2EX_long factors;
    char out[33];

    argc -= 1;
    argv += 1;

    assert(argc == 6);

    for (i = 0; i < argc; i += 1) {
        hex2bytes(sp[i], argv[i]);
    }

    initfield();
    buildpoly(p1, &a1, &c1, &t1);
    buildpoly(p2, &a2, &c2, &t2);
    p = p1 + p2;
    MakeMonic(p);
    CanZass(factors, p);

    for (i = 0; i < factors.length(); i += 1) {
        if (deg(factors[i].a) == 1) {
            felem2hex(out, factors[i].a[0]);
            printf("%s\n", out);
        }
    }

    return 0;
}


#include <iostream>
#include <vector>
#include <math.h>

#include <boost/timer.hpp>

#include "NRGclasses.hpp"
#include "NRGfunctions.hpp"

#include "TwoChQS.hpp"

void TwoChQS_DiagHN(vector<double> Params,
		     CNRGbasisarray* pAbasis,CNRGbasisarray* pSingleSite,
		     CNRGmatrix* Qm1fNQ, CNRGarray* pAeig){

  CNRGmatrix HN(*pAbasis);
  double OldEl[2];

  double chi_N[2]={Params[0],Params[1]};
  double Lambda=Params[2];
  double auxEl;

  // Check time
  boost::timer t;

  // New approach
  HN.UpperTriangular=true;

  int icount=0;
  for (int ibl=0;ibl<pAbasis->NumBlocks();ibl++)
    {
      // HN is block diagonal (ibl,ibl) so we are setting the blocks
      HN.MatBlockMap.push_back(ibl);
      HN.MatBlockMap.push_back(ibl);

      // Each MatBlock is Nbl x Nbl in lenght
      HN.MatBlockBegEnd.push_back(icount);
      //icount+=pAbasis->GetBlockSize(ibl)*pAbasis->GetBlockSize(ibl);
      // New
      int sizebl=pAbasis->GetBlockSize(ibl);
      if (HN.UpperTriangular)
 	icount+=(sizebl*(sizebl+1))/2;
      else
	icount+=sizebl*sizebl;

      HN.MatBlockBegEnd.push_back(icount-1);

      cout << "  Setting up H_(N = " << pAbasis->Nshell << ") Block : " << ibl ;
      cout << " of " << pAbasis->NumBlocks();
      cout << "  size: " << pAbasis->GetBlockSize(ibl) << endl;
      t.restart();

      double Q1i=pAbasis->GetQNumber(ibl,0);
      double Si=pAbasis->GetQNumber(ibl,1);

      // Calculate matrix elements
      for (int ist=pAbasis->GetBlockLimit(ibl,0);
	   ist<=pAbasis->GetBlockLimit(ibl,1);ist++)
	{
	  int type_i=pAbasis->iType[ist];

// 	  for (int jst=pAbasis->GetBlockLimit(ibl,0);
//  	       jst<=pAbasis->GetBlockLimit(ibl,1);jst++)
//       NEW: calculate only half of the matrix elements
//
	  int j0;
	  // NEW THING! only calculates half of matrix els!
	  if (HN.UpperTriangular) j0=ist;
	  else j0=pAbasis->GetBlockLimit(ibl,0);
	  for (int jst=j0;
	           jst<=pAbasis->GetBlockLimit(ibl,1);jst++)
	    {
	      int type_j=pAbasis->iType[jst];

	      if (ist==jst)   // Diagonal terms
		{
		  auxEl=sqrt(Lambda)*(pAbasis->dEn[ist]);
		  HN.MatEl.push_back(auxEl);
		}
	      else 		  //Off-diagonal terms
		{
		  // Loop in channels
		  auxEl=0.0;
		  for (int ich=1;ich<=2;ich++)
		    {
		      OldEl[ich-1]=Qm1fNQ[ich-1].GetMatEl(pAbasis->StCameFrom[ist],pAbasis->StCameFrom[jst]);
		      int typep=type_i;
		      int type=type_j;

		      // if zero, try the h.c term
		      if (dEqual(fabs(OldEl[ich-1]),0.0))
			{
			  OldEl[ich-1]=Qm1fNQ[ich-1].GetMatEl(pAbasis->StCameFrom[jst],
						       pAbasis->StCameFrom[ist]);
			  typep=type_j;
			  type=type_i;
			}

		      // Will this work?
		      // if still zero, next.
		      //if (dEqual(fabs(OldEl[ich-1]),0.0)) break;

		      // Get SingleSite QNumbers
		      int pos=0;
		      int iblssp=pSingleSite->GetBlockFromSt(typep,pos);
		      int iblss=pSingleSite->GetBlockFromSt(type,pos);

		      double Qtildep=pSingleSite->GetQNumber(iblssp,0);
		      double Stildep=pSingleSite->GetQNumber(iblssp,1);
		      double Sztildep=pSingleSite->GetQNumber(iblssp,2);

		      double Qtilde=pSingleSite->GetQNumber(iblss,0);
		      double Stilde=pSingleSite->GetQNumber(iblss,1);
		      double Sztilde=pSingleSite->GetQNumber(iblss,2);

		      double Soldp=Si-Sztildep;
		      double Sold=Si-Sztilde;

		      // Check Fermi Sign
		      double FermiSign=1.0;
		      // will be -1 if only one of them is zero (A XOR B)
		      // Check this one.
		      // Fermi sign now depends on which type the thing is!!!
                      // In other words, it will be -1 if the state has only one
		      // electron in only one of the channels
		      // Single site states +-1,+-1/2!!
                      //
		      if ( (dEqual(Sztilde,0.5)||dEqual(Sztilde,-0.5)) ) FermiSign=-1.0;

		      double siteqnumsp[]={Qtildep,Stildep,Sztildep};
		      double siteqnums[]={Qtilde,Stilde,Sztilde};

		      //Loop in spins
		      for (int sigma=-1;sigma<=1;sigma+=2)
			{
			  double dSigma=0.5*(double)sigma;
			  double FullMatEl=0.0;
			  double auxCG[]={0.0,0.0,0.0,0.0};

			  // Loop in Szold
			  for (double Szold=Sold;Szold>=-Sold;Szold-=1.0)
			    {
			      double Szoldp=Szold-dSigma;
			      Sztilde=Si-Szold;
			      Sztildep=Sztilde+dSigma;
			      // Changing dSigma changes Sztildep
			      // Need an extra loop in the "p" block

			      // Site matrix element: finds the block
			      siteqnums[2]=Sztilde;
			      siteqnumsp[2]=Sztildep;
			      int siteblock=pSingleSite->GetBlockFromQNumbers(siteqnums);
			      int siteblockp=pSingleSite->GetBlockFromQNumbers(siteqnumsp);

// 			      if ((ist==302)&&( (jst==304)||(jst==305) ))
// 				cout << " ist = " << ist
// 				     << " jst = " << jst
// 				     << " ich = " << ich
// 				     << " sigma = " << sigma
// 				     << " Szold = " << Szold
// 				     << " Sztildep = " << Sztildep
// 				     << " Sztilde = " << Sztilde
// 				     << endl;

			      if ( (dLEqual(fabs(Sztildep),Stildep))&&
				   (dLEqual(fabs(Szold),Sold))&&
				   (dLEqual(fabs(Szoldp),Soldp)) )
				{
				  // CG coefs
				  auxCG[0]=CGordan(Sold,Szold,
						   Stilde,Sztilde,
						   Si,Si);

				  auxCG[1]=CGordan(Soldp,Szoldp,
						   Stildep,Sztildep,
						   Si,Si);

				  auxCG[2]=CGordan(Soldp,Szoldp,
						   0.5,dSigma,
						   Sold,Szold);

				  //Loop in site blocks!! Necessary!
				  for (int sitestatep=pSingleSite->GetBlockLimit(siteblockp,0);sitestatep<=pSingleSite->GetBlockLimit(siteblockp,1);sitestatep++)
				    {
				      for (int sitestate=pSingleSite->GetBlockLimit(siteblock,0);sitestate<=pSingleSite->GetBlockLimit(siteblock,1);sitestate++)
					{
					  double SpSm[2]={0.0,0.0};
					  SpSm[0]=TwoChQS_SpSm_table(sitestate,type);
					  SpSm[1]=TwoChQS_SpSm_table(sitestatep,typep);
					  auxCG[3]=TwoChQS_fd_table(ich,sigma,
								    sitestatep,sitestate);
//  					  if ((ist==302)&&( (jst==304)||(jst==305) ))
// 					    cout << " ist = " << ist
// 						 << " jst = " << jst
// 						 << " ich = " << ich
// 						 << " sigma = " << sigma
// 						 << " fd_table("
// 						 << sitestatep << ","
// 						 << sitestate << ") = "
// 						 << auxCG[3] << endl
// 						 << " SpSm1("
// 						 << sitestate << "," << type
// 						 << ") = " << SpSm[0]
// 						 << endl
// 						 << " SpSm2("
// 						 << sitestatep << "," << typep
// 						 << ") = " << SpSm[1]
// 						 << endl;

					  FullMatEl+=auxCG[0]*auxCG[1]*auxCG[2]*auxCG[3]*FermiSign*SpSm[0]*SpSm[1];

					}
				      // end loop in site block
				    }
				  // end loop in site blockp
				}
			      // END Calc coefs safeguard
			    }
			  // End loop in Szold

			auxEl+=chi_N[ich-1]*OldEl[ich-1]*FullMatEl;

// 			if ((ist==302)&&( (jst==304)||(jst==305) ))
// 			  cout << " ich = " << ich
// 			       << " OldEl = " << OldEl[ich-1]
// 			       << " FullMatEl = " << FullMatEl
// 			       << " auxEl = " << auxEl
// 			       << endl;

			}
		      // end loop in sigma
		    }
		  //end loop in channels

		  HN.MatEl.push_back(auxEl);
		  // NEW: add jst,ist as well
		  //HN.MatEl.push_back(auxEl);

		}
	      // END if ist=jst
	    }
	  //END loop in jst
	}
      // END loop in ist

      cout << " ...Done. Set-up Time: " << t.elapsed() << endl;
    }
  // END Loop in blocks (ibl)

  cout << "Updating Aeig " << endl;
  pAeig->ClearAll();

  // Syncronize with HN
  *pAeig=HN;

  // Set dEn,dEigVec
  pAeig->dEn.clear();
  pAeig->dEigVec.clear();

  for (int ibl=0;ibl<HN.NumBlocks();ibl++){
      //// NEW
    //       cout << " Regularizing block " << ibl << " of " << HN.NumBlocks()-1 << endl;
    //       HN.PutInRegularForm(ibl);
    //pAbasis->PrintBlockBasis(ibl);
    //HN.PrintMatBlock(ibl,ibl);
    cout << " Diagonalizing block " << ibl << " of " << HN.NumBlocks()-1 << endl;
    HN.DiagBlock(ibl,pAeig->dEn,pAeig->dEigVec);
    //pAeig->PrintBlockEn(ibl);
  }

  //uset this later
  pAeig->SetE0zero();

}
// end subroutine
////////////////////////
////////////////////////
////////////////////////

// Scrap code
			  // Sum in Sztilde
// 			  for (Sztilde=-Stilde;Sztilde<=Stilde;Sztilde+=1.0)
// 			    {
// 			      double Sztildep=Sztilde+dSigma;
// 			      double Szold=Si-Sztilde;
// 			      double Szoldp=Si-Sztildep;

// 			      // CG coefs
// 			      auxCG[0]=CGordan(Sold,Szold,
// 					       Stilde,Sztilde,Si,Si);
// 			      auxCG[1]=CGordan(Soldp,Szoldp,
// 					       Stildep,Sztildep,Si,Si);
// 			      auxCG[2]=CGordan(Soldp,Szoldp,
// 					       0.5,dSigma,Sold,Szold);
// 			      // Site matrix element
// 			      siteqnums[2]=Sztilde;
// 			      siteqnumsp[2]=Sztildep;
// 			      int sitestate=pSingleSite->GetBlockFromQNumbers(siteqnums);
// 			      int sitestatep=pSingleSite->GetBlockFromQNumbers(siteqnumsp);
// 			      auxCG[3]=TwoChQS_fd_table(ich,sigma,
// 							sitestatep,sitestate);

// 			      FullMatEl+=auxCG[0]*auxCG[1]*auxCG[2]*OldEl[ich-1]*auxCG[3]*FermiSign;

// 			    }
// 			  // END Loop in Sztilde


#include "poli/DiscretePolicyManager.h"
#include "percepto_msgs/DiscreteAction.h"

#include "argus_utils/random/RandomUtils.hpp"
#include "argus_utils/utils/MatrixUtils.h"
#include "argus_utils/utils/MathUtils.h"

#include "modprop/utils/Randomization.hpp"

#include <boost/random/random_device.hpp>

using namespace percepto_msgs;
using namespace argus;

namespace percepto
{

DiscretePolicyManager::DiscretePolicyManager() {}

void DiscretePolicyManager::Initialize( DiscretePolicyInterface::Ptr& interface,
                                        ros::NodeHandle& nh,
                                        ros::NodeHandle& ph )
{
	_interface = interface;

	_actionPub = ph.advertise<DiscreteAction>( "actions", 0 );
	_paramSub = nh.subscribe( "param_updates",
	                          1,
	                          &DiscretePolicyManager::ParamCallback,
	                          this );

	ros::NodeHandle subh( ph.resolveName("input_streams") );
	_inputStreams.Initialize( subh );

	unsigned int inputDim = _inputStreams.GetDim();
	unsigned int outputDim = _interface->GetOutputSizes().array().prod();
	unsigned int numHiddenLayers, layerWidth;
	GetParamRequired( ph, "network/num_hidden_layers", numHiddenLayers );
	GetParamRequired( ph, "network/layer_width", layerWidth );
	_network = std::make_shared<NetworkType>( inputDim,
	                                          outputDim,
	                                          numHiddenLayers,
	                                          layerWidth );
	_network->SetInputSource( &_networkInput );
	_networkParameters = _network->CreateParameters();
	VectorType w = _networkParameters->GetParamsVec();
	percepto::randomize_vector( w, -0.1, 0.1 );
	_networkParameters->SetParamsVec( w );

	if( HasParam( ph, "seed" ) )
	{
		int seed;
		GetParam( ph, "seed", seed );
		_engine.seed( seed );
	}
	else
	{
		boost::random::random_device rng;
		_engine.seed( rng() );
	}

	double updateRate;
	GetParamRequired( ph, "update_rate", updateRate );
	_timer = ph.createTimer( ros::Duration( 1.0/updateRate ),
	                         &DiscretePolicyManager::UpdateCallback,
	                         this );
}

const DiscretePolicyManager::NetworkType&
DiscretePolicyManager::GetPolicyModule() const
{
	return *_network;
}

void DiscretePolicyManager::UpdateCallback( const ros::TimerEvent& event )
{
	StampedFeatures inputs;
	if( !_inputStreams.ReadStream( event.current_real, inputs ) )
	{
		ROS_WARN_STREAM( "Could not read input stream." );
		return;
	}

	// Generate probability mass function values
	_networkInput.SetOutput( inputs.features );
	_networkInput.Invalidate();
	_networkInput.Foreprop();
	VectorType pmf = _network->GetProbabilitySource().GetOutput();

	// Sample from PMF
	std::vector<double> weights( pmf.data(), pmf.data() + pmf.size() );
	std::vector<unsigned int> draws;
	NaiveWeightedSample( weights, 1, draws, _engine );
	unsigned int actionIndex = draws[0];

	// Convert single index into multiple indices
	std::vector<unsigned int> indices;
	indices = multibase_long_div( actionIndex, _interface->GetOutputSizes() );
	_interface->SetOutput( indices );

	// TODO name policy
	DiscreteParamAction action( event.current_real,
	                            inputs.features,
	                            actionIndex );
	_actionPub.publish( action.ToMsg() );
}

void DiscretePolicyManager::ParamCallback( const FloatVectorStamped::ConstPtr& msg )
{
	_networkParameters->SetParamsVec( GetVectorView( msg->values ) );
}

}


#include <iostream>
#include <fstream>

#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <geometry/draw.hpp>
#include <imgproc/image_operator.hpp>
#include <imgproc/derivative_gradient.hpp>
#include <edge/nms.hpp>

#include <edge/edge_linking.hpp>
#include <edge/split.hpp>
#include <edge/spe.hpp>
#include <edge/fit.hpp>

#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/format.hpp>

using namespace lsfm;
using namespace std;
namespace fs = boost::filesystem;

constexpr int runs = 10;

float th_low = 0.004, th_high = 0.012;
typedef DerivativeGradient<uchar, short, int, float, SobelDerivative, QuadraticMagnitude> Grad;
typedef NonMaximaSuppression<short, int, float, FastNMS8<short, int, float>> NMS;
typedef EsdLinking<int> Edge;
typedef std::vector<cv::Point> PointVector;
Grad sobel;
NMS nms(th_low, th_high);
Edge edge(10, 3, 3, sobel.magnitudeThreshold(th_low));

struct EntryBase {
    EntryBase(const std::string& n)
        : name(n), time(0), images(0) {}

    std::string name;
    int64 time;
    int images;

    virtual void process(const EdgeSegmentVector& segs, const PointVector& points) = 0;
};

template<class FIT>
struct Entry : public EntryBase {
    Entry() {}

    Entry(const std::string& n, ValueManager::InitializerList list = ValueManager::InitializerList())
        : EntryBase(n), fit(list) {}

    FIT fit;

    void process(const EdgeSegmentVector& segs, const PointVector& points) {
        std::vector < LineSegment<float, Vec2>> lsegs;
        int64 start;
        for (int i = 0; i != runs; ++i) {
            start = cv::getTickCount();
            fit.apply(segs, points, lsegs);
            time += cv::getTickCount() - start;
            ++images;
        }
    }

};

typedef std::shared_ptr<EntryBase> EntryPtr;

typedef std::vector<EntryPtr> EntryVector;

void parseFolder(const fs::path &folder, std::vector<fs::path> &files) {
    fs::directory_iterator end_iter;
    for_each(fs::directory_iterator(folder), fs::directory_iterator(), [&files](const fs::path& file) {
        if (fs::is_regular_file(file))
        {
            std::string ext = file.extension().generic_string();
            boost::algorithm::to_lower(ext);
            if (ext == ".jpg" || ext == ".png") {
                files.push_back(file);
            }
        }
        if (fs::is_directory(file))
            parseFolder(file, files);
    });
}

void processPath(EntryVector &entries, const std::pair<fs::path, std::string> &path) {
    std::cout << "processing " << path.first << std::endl;
    std::vector<fs::path> files;
    parseFolder(path.first, files);

    for_each(entries.begin(), entries.end(), [&](EntryPtr &e) {
        e->time = 0;
        e->images = 0;
    });

    std::for_each(files.begin(), files.end(), [&](const fs::path& file) {
        cv::Mat src = cv::imread(file.generic_string(), cv::IMREAD_GRAYSCALE);
        if (src.empty())
        {
            cout << "Can not open " << file.generic_string() << endl;
        }
        std::cout << file << std::endl;

        sobel.process(src);
        nms.process(sobel);
        edge.detect(sobel, nms);
        PointVector points;
        PixelEstimator<float,cv::Point>::convert(edge.points(), points, sobel.magnitude(), nms.directionMap());

        for_each(entries.begin(), entries.end(), [&](EntryPtr &e) {
            e->process(edge.segments(), points);
        });
    });
}

int main(int argc, char** argv)
{
    char c;
    std::cin >> c;

    std::vector<std::pair<fs::path, std::string>> sets;
    //sets.push_back(std::pair<fs::path, std::string>("../../images/Selection", "Selection"));
    //sets.push_back(std::pair<fs::path, std::string>("../../images/BSDS500", "BSDS500"));
    sets.push_back(std::pair<fs::path, std::string>("../../images/MDB/MiddEval3-Q", "MDB-Q"));
    //sets.push_back(std::pair<fs::path, std::string>("../../images/MDB/MiddEval3-H", "MDB-H"));
    //sets.push_back(std::pair<fs::path, std::string>("../../images/MDB/MiddEval3-F", "MDB-F"));

    EntryVector fit;
    fit.push_back(EntryPtr(new Entry<FitLine<RegressionFit<float,cv::Point>>>("RegressionFit")));
    fit.push_back(EntryPtr(new Entry<FitLine<EigenFit<float, cv::Point>>>("EigenFit")));
    fit.push_back(EntryPtr(new Entry<FitLine<EigenCVFit<float, cv::Point>>>("EigenCVFit")));
    fit.push_back(EntryPtr(new Entry<MEstimatorFitLine<float, cv::Point>>("MEstimatorFit")));

    int rows = fit.size() + 1;
    int cols = sets.size() + 1;
    std::vector<std::vector<std::string>> table;
    table.resize(rows);
    for_each(table.begin(), table.end(), [&](std::vector<std::string> &row) {
        row.resize(cols);
    });

    table[0][0] = "Method";

    int row = 1;
    for_each(fit.begin(), fit.end(), [&](const EntryPtr &e) {
        table[row++][0] = e->name;
    });

    int col = 1;
    for_each(sets.begin(), sets.end(), [&](const std::pair<fs::path, std::string> &data) {

        processPath(fit, data);

        table[0][col] = data.second;
        row = 1;
        for_each(fit.begin(), fit.end(), [&](const EntryPtr &e) {
            table[row++][col] = boost::str(boost::format("%.3f") % (static_cast<double>(e->time * 1000) / (e->images * cv::getTickFrequency()))) + "ms";
        });
        ++col;
    });

    std::ofstream ofs;
    ofs.open("line_fit_profiling.csv");

    for_each(table.begin(), table.end(), [&](const std::vector<std::string> &row) {
        for_each(row.begin(), row.end(), [&](const std::string &cell) {
            std::cout << cell << "\t";
            ofs << cell << ";";
        });
        std::cout << std::endl;
        ofs << std::endl;
    });

    ofs.close();

    return 0;
}


/*!
 * @file
 * @brief
 * @copyright alphya 2020-2021
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#ifndef NYARUGA_UTIL_CARTESIAN_PRODUCT_HPP
#define NYARUGA_UTIL_CARTESIAN_PRODUCT_HPP

#pragma once

// this code almost from
// https://stackoverflow.com/questions/16686942/how-to-iterate-over-two-stl-like-containers-cartesian-product

// ネストした for 文が書きやすくなります
// 速度はブレ幅が大きいですが、for 文をネストした場合より 2 倍以上遅くなることはないと思います
// パソコンの調子によって、1.5 倍くらいだったり、同じくらいだったりしました

// 例

/*
    array b = { false, true };
    array i = { 0, 1 };
    array s = { "Hello", "World" };

    // i, j, k は、それぞれの添え字に関してネストした for 文と
    // 同じような添え字を走る(直積)
    // 引数はイテレータが使える型や組み込みの配列なら OK
    // (使用する操作：std::end(), std::begin(), it++)
    for (auto&& [i,j,k]: cartesian_product(b,i,s))
    {
        std::cout << std::boolalpha << i << ", ";
        std::cout << j << ", ";
        std::cout << k << "\n";
    }

    //   for (int i = 0; i != 2; ++i)
    //   for (int j = 1; j != 3; ++j)
    //   for (int k = 2; k != 4; ++k)
   for (auto&& [i,j,k]: cartesian_product({0,2},{1,3},{2,4}))
    {
        std::cout << i << ", ";
        std::cout << j << ", ";
        std::cout << k << "\n";
    }

*/

#include <tuple>                        // make_tuple, tuple
#include <utility>                      // pair
#include <vector>                       // vector
#include <boost/coroutine2/coroutine.hpp>

namespace nyaruga {

namespace util {

/*

namespace impl1 { // コルーチン実装

namespace detail {

// the lambda is fully bound with one element from each of the ranges
template<class Op>
void insert_tuples(Op&& op) noexcept
{
    // evaluating the lambda will insert the currently bound tuple
    op();
}

// "peal off" the first range from the remaining tuple of ranges
template<class Op, class InputIterator1, class... InputIterator2>
void insert_tuples(Op&& op, std::pair<InputIterator1, InputIterator1> head, std::pair<InputIterator2, InputIterator2>... tail)
{
    // "peal off" the elements from the first of the remaining ranges
    // NOTE: the recursion will effectively generate the multiple nested for-loops
    for (auto it = head.first; it != head.second; ++it) {
        // bind the first free variable in the lambda, and
        // keep one free variable for each of the remaining ranges
        detail::insert_tuples(
            [&op,&it](InputIterator2... elems) { op(it, elems...); },
            tail...
        );
    }
}

}   // namespace detail

// convert a tuple of ranges to the range of tuples representing the Cartesian product
template<class... InputIterator>
void cartesian_product_impl(auto & sink, std::pair<InputIterator, InputIterator>&&... dimensions)
{
    detail::insert_tuples(
         [&sink](InputIterator... elems) { sink(std::make_tuple(*elems...)); },
         std::forward<decltype(dimensions)>(dimensions)...
    );
}

template<class...Args>
decltype(auto) cartesian_product(Args&&... args)
{
    using coro_type = boost::coroutines2::coroutine<
        typename std::tuple<std::remove_reference_t<decltype(*std::begin(std::declval<Args&>()))>...>
    >;

    return typename coro_type::pull_type(
    [...args=std::forward<Args>(args)](typename coro_type::push_type & sink)
    {
        cartesian_product_impl(sink, std::make_pair(std::begin(args), std::end(args))...);
    });
};
} //namespace impl1

*/

// from: https://stackoverflow.com/questions/16686942/how-to-iterate-over-two-stl-like-containers-cartesian-product
namespace impl2 { // ほぼ stack overflow から

namespace detail {

// the lambda is fully bound with one element from each of the ranges
template<class Op>
void insert_tuples(Op&& op)
{
        // evaluating the lambda will insert the currently bound tuple
        op();
}

// "peal off" the first range from the remaining tuple of ranges
template<class Op, class InputIterator1, class... InputIterator2>
void insert_tuples(Op&& op, std::pair<InputIterator1, InputIterator1> head, std::pair<InputIterator2, InputIterator2>... tail)
{
        // "peal off" the elements from the first of the remaining ranges
        // NOTE: the recursion will effectively generate the multiple nested for-loops
        for (auto it = head.first; it != head.second; ++it) {
                // bind the first free variable in the lambda, and
                // keep one free variable for each of the remaining ranges
                detail::insert_tuples(
                        [&op,&it](InputIterator2... elems) mutable { op(it, elems...); },
                        tail...
                );
        }
}

// convert a tuple of ranges to the range of tuples representing the Cartesian product
template<class OutputIterator, class... InputIterator>
void cartesian_product_impl(OutputIterator result, std::pair<InputIterator, InputIterator>... dimensions)
{
        insert_tuples(
                 [=](InputIterator... elems) mutable { *result++ = std::make_tuple(*elems...); },
                 dimensions...
        );
}

}       // namespace detail

template <typename...Args>
auto cartesian_product(Args&&...args)
{
    std::vector< typename std::tuple<std::remove_reference_t<decltype(*std::begin(std::declval<Args&>()))>...>  > result;
    detail::cartesian_product_impl(
            std::back_inserter(result),
            std::make_pair(std::begin(args), std::end(args))...
    );
    return result;
}

} // namespace impl2 (almost original)

using namespace impl2; // impl1 より倍速い場合があった 速度はパソコンの調子によって大きく左右される

} // namespace nyaruga::util

/* how to use

#include <string>
#include <iostream>
#include <array>
#include <stdio.h>
#include <chrono>

using std::array;

int main()
{
    array b = { false, true };
    array i = { 0, 1 };
    array s = { "Hello", "World" };

    using namespace std;
    chrono::system_clock::time_point start, end;

    start = chrono::system_clock::now();

    for (int f = 0; f < 1000; ++f)
    // now use a single flat loop over result to do your own thing
    for (auto&& [i,j,k]: cartesian_product(b,i,s)) {
        std::cout << std::boolalpha << i << ", ";
        std::cout << j << ", ";
        std::cout << k << "\n";
    }

        // 何かの処理

    end = chrono::system_clock::now();

    double time = static_cast<double>(chrono::duration_cast<chrono::microseconds>(end - start).count() / 1000.0);
    printf("time %lf[ms]\n", time);

}

*/

#include <initializer_list>

namespace nyaruga::util {

namespace detail_index {

template<class Op>
void insert_tuples(Op&& op)
{
        op();
}

void insert_tuples(auto&& op,auto head, auto... tail)
{
    for (auto i = *head.begin(); i < *(head.begin()+1); ++i) {
        insert_tuples(
            [&op,&i](typename decltype(tail)::value_type... elems) mutable { op(i, elems...); },
            tail...
        );
    }
}

void cartesian_product_impl(auto&& result, auto&&... dimensions)
{
    insert_tuples(
         [=](auto... elems) mutable { *result++ = std::make_tuple(elems...); },
         std::forward<decltype(dimensions)>(dimensions)...
    );
}

}       // namespace detail

template<typename... T>
auto cartesian_product(std::initializer_list<T>...args)
{
    std::vector<typename std::tuple<T...>> result;
    detail_index::cartesian_product_impl(std::back_inserter(result), args...);
    return result;
}

}

/* how to use
#include <string>
#include <iostream>
#include <array>
#include <stdio.h>
#include <chrono>

using std::array;

int main()
{

    using namespace std;
    chrono::system_clock::time_point start, end;

    start = chrono::system_clock::now();

    for (int f = 0; f < 1000; ++f)

   for (auto&& [i,j,k]: cartesian_product({0,2},{1,3},{2,4}))
//   equal to
//   for (int i = 0; i != 2; ++i)
//   for (int j = 1; j != 3; ++j)
//   for (int k = 2; k != 4; ++k)
    {
        std::cout << i << ", ";
        std::cout << j << ", ";
        std::cout << k << "\n";
    }

    end = chrono::system_clock::now();

    double time = static_cast<double>(chrono::duration_cast<chrono::microseconds>(end - start).count() / 1000.0);
    printf("time %lf[ms]\n", time);

}
*/

#endif // NYARUGA_UTPL_CARTESIAN_PRODUCT_HPP


/* ----------------------------------------------------------------------------
 * Copyright 2018, Ross Hartley <m.ross.hartley@gmail.com>
 * All Rights Reserved
 * See LICENSE for the license information
 * -------------------------------------------------------------------------- */

/**
 *  @file   propagation_speed.cpp
 *  @author Ross Hartley
 *  @brief  Test to determine average propagation speed
 *  @date   September 25, 2018
 **/
#include <iostream>
#include <fstream>
#include <string>
#include <cstdlib>
#include <vector>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <Eigen/Dense>
#include<Eigen/StdVector>
#include <boost/algorithm/string.hpp>
#include "inekf/inekf.hpp"

#define DT_MIN 1e-6
#define DT_MAX 1

using namespace std;
using namespace inekf;
using namespace boost::posix_time;

typedef vector<pair<double,Eigen::Matrix<double,6,1> > > vectorPairIntVector6d;
typedef vector<pair<double,Eigen::Matrix<double,6,1> > >::const_iterator vectorPairIntVector6dIterator;

double stod98(const std::string &s) {
    return atof(s.c_str());
}

int stoi98(const std::string &s) {
    return atoi(s.c_str());
}

int main()
{
    // Initialize filter
    Eigen::MatrixXd X = Eigen::MatrixXd::Identity(10,10);
    RobotState state(X);
    InEKF filter(state);

    cout << "Robot's state is initialized to: \n";
    cout << filter.getState() << endl;

    ifstream infile("../data/propagation_speed_test_data.txt");
    string line;
    Eigen::Matrix<double,6,1> m, m_last;
    double t, t_last;
    m_last << 0,0,0,0,0,0;
    t_last = 0;
    vectorPairIntVector6d measurements_vec;

    // Loop through data file and read in measurements line by line
    while (getline(infile, line)){
        vector<string> measurement;
        boost::split(measurement,line,boost::is_any_of(" "));
        // Handle measurements
        if (measurement[0].compare("IMU")==0){
            t = stod98(measurement[1]);
            m << stod98(measurement[2]),
                 stod98(measurement[3]),
                 stod98(measurement[4]),
                 stod98(measurement[5]),
                 stod98(measurement[6]),
                 stod98(measurement[7]);
            measurements_vec.push_back(pair<double,Eigen::Matrix<double,6,1> > (t, m));
        }
    }

    // Propagate all IMU data
    cout << "Propagating " << measurements_vec.size() << " IMU measurements...\n";
    int64_t max_duration = 0;
    int64_t sum_duration = 0;
    for (vectorPairIntVector6dIterator it=measurements_vec.begin(); it!=measurements_vec.end(); ++it) {
        // Propagate using IMU data
        t = it->first;
        m = it->second;
        double dt = t - t_last;
        if (dt > DT_MIN && dt < DT_MAX) {
            ptime start_time = second_clock::local_time();
            filter.Propagate(m_last, dt);
            ptime end_time = second_clock::local_time();
            int64_t duration = (end_time - start_time).total_nanoseconds();
            //cout << "duration: " <<  duration << endl;
            sum_duration += duration;
            if (duration > max_duration)
                max_duration = duration;

            //cout << filter.getState() << endl;
        }
        // Store previous timestamp
        t_last = t;
        m_last = m;
    }

    cout << "max duration: " <<  max_duration << endl;
    cout << "average duration: " <<  double(sum_duration)/measurements_vec.size() << endl;

    return 0;
}


// Check blitz/minmax.h, which provides a templated, type-promoting
// version of min(a,b) and max(a,b).

// Some MS Windows and boost header files provide macro definitions of min/max
// that can conflict with our templated functions in the blitz::extrema
// namespace.  Placing these macros here as a test that we can work around
// this problem by enclosing our min/max invocations in a set of parentheses.
#define min(a,b) (a < b ? a : b)
#define max(a,b) (a > b ? a : b)

#include "testsuite.h"
#include <blitz/minmax.h>

int main()
{
    double a = 1.0;
    float b = 2.0;
    int c = 3;

    BZTEST(static_cast<int>((blitz::extrema::min)(a,b)) == 1);
    BZTEST(static_cast<int>((blitz::extrema::max)(a,b)) == 2);
    BZTEST(static_cast<int>((blitz::extrema::min)(a,c)) == 1);
    BZTEST(static_cast<int>((blitz::extrema::min)(b,c)) == 2);
    BZTEST(static_cast<int>((blitz::extrema::max)(b,c)) == 3);
    BZTEST(static_cast<int>((blitz::extrema::max)(a,c)) == 3);

    return 0;
}


/* boost random/bernoulli_distribution.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: bernoulli_distribution.hpp 60755 2010-03-22 00:45:06Z steven_watanabe $
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_BERNOULLI_DISTRIBUTION_HPP
#define BOOST_RANDOM_BERNOULLI_DISTRIBUTION_HPP

#include <cassert>
#include <iostream>
#include <boost/random/detail/config.hpp>

namespace boost {

/**
 * Instantiations of class template \bernoulli_distribution model a
 * \random_distribution. Such a random distribution produces bool values
 * distributed with probabilities P(true) = p and P(false) = 1-p. p is
 * the parameter of the distribution.
 */
template<class RealType = double>
class bernoulli_distribution
{
public:
  // In principle, this could work with both integer and floating-point
  // types.  Generating floating-point random numbers in the first
  // place is probably more expensive, so use integer as input.
  typedef int input_type;
  typedef bool result_type;

  /**
   * Constructs a \bernoulli_distribution object.
   * p is the parameter of the distribution.
   *
   * Requires: 0 <= p <= 1
   */
  explicit bernoulli_distribution(const RealType& p_arg = RealType(0.5))
    : _p(p_arg)
  {
    assert(_p >= 0);
    assert(_p <= 1);
  }

  // compiler-generated copy ctor and assignment operator are fine

  /**
   * Returns: The "p" parameter of the distribution.
   */
  RealType p() const { return _p; }
  /**
   * Effects: Subsequent uses of the distribution do not depend
   * on values produced by any engine prior to invoking reset.
   */
  void reset() { }

  /**
   * Returns: a random variate distributed according to the
   * \bernoulli_distribution.
   */
  template<class Engine>
  result_type operator()(Engine& eng)
  {
    if(_p == RealType(0))
      return false;
    else
      return RealType(eng() - (eng.min)()) <= _p * RealType((eng.max)()-(eng.min)());
  }

#ifndef BOOST_RANDOM_NO_STREAM_OPERATORS
  /**
   * Writes the parameters of the distribution to a @c std::ostream.
   */
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const bernoulli_distribution& bd)
  {
    os << bd._p;
    return os;
  }

  /**
   * Reads the parameters of the distribution from a @c std::istream.
   */
  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, bernoulli_distribution& bd)
  {
    is >> std::ws >> bd._p;
    return is;
  }
#endif

private:
  RealType _p;
};

} // namespace boost

#endif // BOOST_RANDOM_BERNOULLI_DISTRIBUTION_HPP


#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <iomanip>
#include <iostream>
#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <math.h>

class SubscribeAndPublish
{
public:
    double x;
    double y ;
    double th;

    double vx;
    double vy;
    double vth;

    ros::NodeHandle n;
    ros::Publisher odom_pub;
    ros::Subscriber sub;
    ros::Time current_time, last_time;
    tf::TransformBroadcaster odom_broadcaster;

    SubscribeAndPublish(std::string talker_topic)
    {
        odom_pub = n.advertise<nav_msgs::Odometry>("odom", 100);
        sub = n.subscribe(talker_topic, 10, &SubscribeAndPublish::chatterCallback,this); //监听
    }

    void chatterCallback(const geometry_msgs::Twist & msg)
    {
        current_time = ros::Time::now();
        vx = msg.linear.x;
        vy = msg.linear.y;
        vth = msg.angular.z;
        if(vx <= -5.0 || vx >= 5.0)
            vx = 0.0;
        if(vth <= -5.0 || vth >= 5.0)
            vth = 0.0;
        double dt = (current_time - last_time).toSec();
        double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
        double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
        double delta_th = vth * dt;

        x += delta_x;
        y += delta_y;
        th += delta_th;

        geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);

        geometry_msgs::TransformStamped odom_trans;
        odom_trans.header.stamp = current_time;
        odom_trans.header.frame_id = "odom";
        odom_trans.child_frame_id = "base_link";

        odom_trans.transform.translation.x = x;
        odom_trans.transform.translation.y = y;
        odom_trans.transform.translation.z = 0.0;
        odom_trans.transform.rotation = odom_quat;

        odom_broadcaster.sendTransform(odom_trans);

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

        odom.pose.pose.position.x = x;
        odom.pose.pose.position.y = y;
        odom.pose.pose.position.z = 0.0;
        odom.pose.pose.orientation = odom_quat;

        odom.child_frame_id = "base_link";
        odom.twist.twist.linear.x = vx;
        odom.twist.twist.linear.y = vy;
        odom.twist.twist.angular.z = vth;

        odom_pub.publish(odom);
        last_time = current_time;
    }
};

int main(int argc, char **argv)
{

    ros::init(argc, argv, "Odometry_publisher");
    ros::NodeHandle nh_private("~");
    std::string talker_topic = "";
    if(!nh_private.getParam("talker_topic", talker_topic)){
        talker_topic = "/talker_boost_serial";
    }

    SubscribeAndPublish SAPObject(talker_topic);

    SAPObject.current_time = ros::Time::now();
    SAPObject.last_time = ros::Time::now();
    ros::spin();
}


#pragma once

#include <Eigen/Dense>

#include "perceive/foundation.hpp"
#include "perceive/utils/sdbm-hash.hpp"
#include "vector-2.hpp"
#include "vector-3.hpp"

namespace perceive
{
// An "axis-aligned" bounding box
#pragma pack(push, 1)
template<typename T> class AABBT
{
 public:
   CUSTOM_NEW_DELETE(AABBT)

   using value_type = T;

   T left{T(0.0)}, top{T(0.0)}, right{T(0.0)}, bottom{T(0.0)};

   AABBT() noexcept             = default;
   AABBT(const AABBT&) noexcept = default;
   AABBT(AABBT&&) noexcept      = default;
   ~AABBT()                     = default;
   AABBT& operator=(const AABBT&) noexcept = default;
   AABBT& operator=(AABBT&&) noexcept = default;

   AABBT(T in_left, T in_top, T in_right, T in_bottom)
   noexcept
       : left(in_left)
       , top(in_top)
       , right(in_right)
       , bottom(in_bottom)
   {}
   AABBT(Vector2T<T> lefttop, Vector2T<T> rightbottom)
   noexcept
       : left(lefttop.x)
       , top(lefttop.y)
       , right(rightbottom.x)
       , bottom(rightbottom.y)
   {}

   template<class Iterator> AABBT(Iterator first, Iterator last) noexcept
   {
      *this = minmax();
      union_with(first, last);
   }

   static AABBT nan() noexcept { return AABBT(T(NAN), T(NAN), T(NAN), T(NAN)); }
   static AABBT minmax() noexcept
   {
      auto mmax = std::numeric_limits<T>::max();
      auto mmin = std::numeric_limits<T>::lowest();
      return AABBT<T>(mmax, mmax, mmin, mmin);
   }

   bool is_finite() const noexcept
   {
      return true && std::isfinite(left) && std::isfinite(top)
             && std::isfinite(right) && std::isfinite(bottom);
   }

   // Union a point with the AABB
   void union_point(T x, T y) noexcept
   {
      if(std::isfinite(x)) {
         if(x < left) left = x;
         if(x > right) right = x;
      }
      if(std::isfinite(y)) {
         if(y < top) top = y;
         if(y > bottom) bottom = y;
      }
   }

   void union_point(const Vector2T<T>& p) noexcept { union_point(p.x, p.y); }
   void union_point(const T p[2]) noexcept { union_point(p[0], p[1]); }

   bool contains(T x, T y) const noexcept
   {
      if constexpr(std::is_floating_point<T>::value) {
         return is_close_between(x, left, right)
                and is_close_between(y, top, bottom);
      }
      return (x >= left) && (x <= right) && (y >= top) && (y <= bottom);
   }
   bool contains(const Vector2T<T>& p) const noexcept
   {
      return contains(p.x, p.y);
   }
   bool contains(const T p[2]) const noexcept { return contains(p[0], p[1]); }

   template<class Iterator>
   void union_with(Iterator first, Iterator last) noexcept
   {
      while(first != last) union_point(*first++);
   }

   T w() const noexcept
   {
      return right - left;
      if constexpr(std::is_integral_v<T>)
         return abs(right - left);
      else
         return fabs(right - left);
   }
   T h() const noexcept
   {
      return bottom - top;
      if constexpr(std::is_integral_v<T>)
         return abs(bottom - top);
      else
         return fabs(bottom - top);
   }
   T width() const noexcept { return w(); }
   T height() const noexcept { return h(); }

   T area() const noexcept { return w() * h(); }

   AABBT& grow(T inc) noexcept
   {
      left -= inc;
      top -= inc;
      right += inc;
      bottom += inc;
      return *this;
   }

   static AABBT intersection(const AABBT& a, const AABBT& b) noexcept
   {
      AABBT<T> c = a;

      c.left   = std::max(a.left, b.left);
      c.right  = std::min(a.right, b.right);
      c.top    = std::max(a.top, b.top);
      c.bottom = std::min(a.bottom, b.bottom);

      if(c.w() < 0) {
         c.left  = T(0.5 * double(c.left + c.right));
         c.right = c.left;
      }

      if(c.h() < 0) {
         c.top    = T(0.5 * double(c.top + c.bottom));
         c.bottom = c.top;
      }

      return c;
   }

   static T intersection_area(const AABBT& a, const AABBT& b) noexcept
   {
      AABBT<T> c = a;

      c.left   = std::max(a.left, b.left);
      c.right  = std::min(a.right, b.right);
      c.top    = std::max(a.top, b.top);
      c.bottom = std::min(a.bottom, b.bottom);

      T w = c.w();
      T h = c.h();

      return (w > 0.0 && h > 0.0) ? (w * h) : T(0.0);
   }

   // Raster order in images has a "flipped" y-axis
   AABBT reflect_raster() const noexcept
   {
      AABBT o = *this;
      std::swap(o.top, o.bottom);
      return o;
   }

   T intersection_area(const AABBT& o) const noexcept
   {
      return intersection_area(*this, o);
   }

   static T union_area(const AABBT& a, const AABBT& b) noexcept
   {
      return a.area() + b.area() - intersection_area(a, b);
   }
   T union_area(const AABBT& o) const noexcept { return union_area(*this, o); }

   Vector2T<T> centre() const noexcept
   {
      return Vector2T<T>(T(left + 0.5 * (right - left)),
                         T(top + 0.5 * (bottom - top)));
   }
   Vector2T<T> center() const noexcept { return centre(); }

   real aspect_ratio() const noexcept { return real(width()) / real(height()); }

   // Scale the AABB by some size
   AABBT& operator*=(T scalar) noexcept
   {
      Vector2T<T> c = centre();
      T w           = this->w() * scalar * 0.5;
      T h           = this->h() * scalar * 0.5;
      left          = c.x - w;
      right         = c.x + w;
      top           = c.y - h;
      bottom        = c.y + h;
      return *this;
   }

   AABBT& operator/=(T scalar) noexcept { return *this *= (1.0 / scalar); }
   AABBT operator*(T scalar) const noexcept
   {
      AABBT res(*this);
      res *= scalar;
      return res;
   }
   AABBT operator/(T scalar) const noexcept
   {
      AABBT res(*this);
      res /= scalar;
      return res;
   }

   Vector2T<T> left_top() const noexcept { return Vector2T<T>(left, top); }
   Vector2T<T> top_left() const noexcept { return Vector2T<T>(left, top); }
   Vector2T<T> right_bottom() const noexcept
   {
      return Vector2T<T>(right, bottom);
   }
   Vector2T<T> bottom_right() const noexcept
   {
      return Vector2T<T>(right, bottom);
   }

   Vector2T<T> corner(int index) const noexcept
   {
      switch(index) {
      case 0: return Vector2T<T>(left, bottom);
      case 1: return Vector2T<T>(left, top);
      case 2: return Vector2T<T>(right, top);
      case 3: return Vector2T<T>(right, bottom);
      }
      FATAL("kBAM!");
      return {};
   }

   std::array<Vector2T<T>, 4> to_array_polygon() const noexcept
   {
      std::array<Vector2T<T>, 4> X;
      X[0] = Vector2T<T>(left, top);
      X[1] = Vector2T<T>(right, top);
      X[2] = Vector2T<T>(right, bottom);
      X[3] = Vector2T<T>(left, bottom);
      return X;
   }

   vector<Vector2T<T>> to_polygon() const noexcept(false)
   {
      vector<Vector2T<T>> X(4);
      X[0] = Vector2T<T>(left, top);
      X[1] = Vector2T<T>(right, top);
      X[2] = Vector2T<T>(right, bottom);
      X[3] = Vector2T<T>(left, bottom);
      return X;
   }

   Vector3T<T> left_line() const noexcept
   {
      T l = this->left, t = this->top, r = this->right, b = this->bottom;
      return to_homgen_line(Vector3T<T>(l, t, 1), Vector3T<T>(l, b, 1));
   }

   Vector3T<T> right_line() const noexcept
   {
      T l = this->left, t = this->top, r = this->right, b = this->bottom;
      return to_homgen_line(Vector3T<T>(r, t, 1), Vector3T<T>(r, b, 1));
   }

   Vector3T<T> top_line() const noexcept
   {
      T l = this->left, t = this->top, r = this->right, b = this->bottom;
      return to_homgen_line(Vector3T<T>(l, t, 1), Vector3T<T>(r, t, 1));
   }

   Vector3T<T> bottom_line() const noexcept
   {
      T l = this->left, t = this->top, r = this->right, b = this->bottom;
      return to_homgen_line(Vector3T<T>(l, b, 1), Vector3T<T>(r, b, 1));
   }

   Vector3T<T> line(int ind) const noexcept
   {
      switch(ind) {
      case 0: return left_line();
      case 1: return top_line();
      case 2: return right_line();
      case 3: return bottom_line();
      }
      return left_line();
   }

   bool operator!=(const AABBT<T>& rhs) const noexcept
   {
      return !(*this == rhs);
   }
   bool operator==(const AABBT<T>& rhs) const noexcept
   {
      return is_close(left, rhs.left) and is_close(top, rhs.top)
             and is_close(right, rhs.right) and is_close(bottom, rhs.bottom);
   }

   AABBT& set_to(const T& l, const T& t, const T& r, const T& b) noexcept
   {
      left   = l;
      top    = t;
      right  = r;
      bottom = b;
      return *this;
   }
   AABBT& set_to(T a[4]) noexcept
   {
      set_to(a[0], a[1], a[2], a[3]);
      return *this;
   }

   T* copy_to(T a[4]) const noexcept
   {
      a[0] = left;
      a[1] = top;
      a[2] = right;
      a[3] = bottom;
      return a;
   }

   T* ptr() noexcept { return &left; }
   const T* ptr() const noexcept { return &left; }
   T& operator[](int idx) noexcept
   {
#ifdef DEBUG_BUILD
      assert(idx >= 0 && idx < 4);
#endif
      return ptr()[idx];
   }

   const T& operator[](int idx) const noexcept
   {
#ifdef DEBUG_BUILD
      assert(idx >= 0 && idx < 4);
#endif
      return ptr()[idx];
   }

   T& operator()(int idx) noexcept
   {
#ifdef DEBUG_BUILD
      assert(idx >= 0 && idx < 4);
#endif
      return ptr()[idx];
   }
   const T& operator()(int idx) const noexcept
   {
#ifdef DEBUG_BUILD
      assert(idx >= 0 && idx < 4);
#endif
      return ptr()[idx];
   }

   unsigned size() const noexcept { return 4; }

   std::string to_string(const char* fmt = "[{} {} {} {}]") const
   {
      return format(fmt, left, top, right, bottom);
   }

   std::string to_str() const { return to_string(); }

   size_t hash() const noexcept { return sdbm_hash(ptr(), sizeof(T) * size()); }

   friend std::string str(const AABBT<T>& aabb) { return aabb.to_string(); }
};
#pragma pack(pop)

template<typename T>
AABBT<T> intersection(const AABBT<T>& a, const AABBT<T>& b) noexcept
{
   return AABBT<T>::intersection(a, b);
}

template<typename T>
T intersection_area(const AABBT<T>& a, const AABBT<T>& b) noexcept
{
   return AABBT<T>::intersection_area(a, b);
}

template<typename T> T union_area(const AABBT<T>& a, const AABBT<T>& b) noexcept
{
   return AABBT<T>::union_area(a, b);
}

// Scalar multiples
template<typename T> AABBT<T> operator*(float a, const AABBT<T>& v) noexcept
{
   return v * a;
}
template<typename T> AABBT<T> operator/(float a, const AABBT<T>& v) noexcept
{
   return v / a;
}
template<typename T> AABBT<T> operator*(double a, const AABBT<T>& v) noexcept
{
   return v * a;
}
template<typename T> AABBT<T> operator/(double a, const AABBT<T>& v) noexcept
{
   return v / a;
}

// Intersection of homogeneous line ('line') with AABB
template<typename T>
std::pair<Vector2T<T>, Vector2T<T>>
intersect(const AABBT<T>& aabb, const Vector3T<T>& in_line) noexcept
{
   array<Vector3T<T>, 4> lines;
   array<Vector3T<T>, 4> isects;
   array<bool, 4> bounded;
   std::pair<Vector2T<T>, Vector2T<T>> ret
       = {Vector2T<T>::nan(), Vector2T<T>::nan()};

   const auto line = in_line.normalised_line();

   { // What are the lines of the AABB?, the order is [left, top, right, bot]
      int counter = 0;
      std::generate(
          begin(lines), end(lines), [&]() { return aabb.line(counter++); });
   }

   { // Where does 'line' intersect with each line of the AABB?
      std::transform(
          cbegin(lines), cend(lines), begin(isects), [&](const auto& ll) {
             return cross(line, ll).normalise_point();
          });
   }

   { // Are inserections on the border of the AABB?
     // Point 'X' is between lines 'l1' and 'l2'
      auto is_between = [&](const Vector3T<T>& X,
                            const Vector3T<T>& l1,
                            const Vector3T<T>& l2) -> bool {
         return X.is_finite() && (dot(X, l1) < 0.0) == (dot(X, l2) < 0.0);
      };

      bounded[0] = is_close_between(isects[0].y, aabb.top, aabb.bottom);
      bounded[1] = is_close_between(isects[1].x, aabb.left, aabb.right);
      bounded[2] = is_close_between(isects[2].y, aabb.top, aabb.bottom);
      bounded[3] = is_close_between(isects[3].x, aabb.left, aabb.right);
   }

   unsigned pos     = 0;
   auto push_result = [&](const Vector3T<T>& X) {
      if(pos < 2) {
         auto& p = (pos++ == 0) ? ret.first : ret.second;
         p(0)    = X(0);
         p(1)    = X(1);
      } else {
         const auto v  = Vector2T<T>(X(0), X(1));
         const auto q0 = (ret.first - ret.second).quadrance();
         const auto q1 = (ret.first - v).quadrance();
         if(q1 > q0) ret.second = v;
      }
   };

   for(size_t i = 0; i < 4; ++i)
      if(bounded[i]) push_result(isects[i]);

   if(false) {
      INFO("FEEDBACK");
      cout << format("AABB  = {}", str(aabb)) << endl;
      cout << format("line  = {}", str(line)) << endl;
      for(auto i = 0; i < 4; ++i) {
         cout << format(" {:c} ll = {}, isect = {{}, {}}",
                        (bounded[size_t(i)] ? '*' : ' '),
                        str(lines[size_t(i)]),
                        isects[size_t(i)].x,
                        isects[size_t(i)].y)
              << endl;
      }
      cout << format("out   = {{}, {}} -> {{}, {}}",
                     ret.first.x,
                     ret.first.y,
                     ret.second.x,
                     ret.second.y)
           << endl
           << endl;
   }

   return ret;
}

template<typename T>
std::pair<Vector2T<T>, Vector2T<T>> intersect(const AABBT<T>& aabb,
                                              const Vector2T<T>& A,
                                              const Vector2T<T>& B) noexcept
{
   const bool A_in = aabb.contains(A);
   const bool B_in = aabb.contains(B);

   if(A_in and B_in) return {A, B};

   const auto ll     = to_homgen_line(A, B);
   const auto [U, V] = intersect(aabb, ll);

   auto orthogonal_line_at = [&](const auto& U) -> auto
   {
      return Vector3T<T>(-ll.y, ll.x, ll.y * U.x - ll.x * U.y);
   };

   auto is_between_A_B = [&](const auto& U) -> bool {
      const auto tt = orthogonal_line_at(U);
      const auto da = tt.x * A.x + tt.y * A.y + tt.z;
      const auto db = tt.x * B.x + tt.y * B.y + tt.z;
      return std::signbit(da) != std::signbit(db);
   };

   auto subline
       = [&](const auto& X, const auto& Y, const auto& U, const auto& V) {
            return is_between_A_B(U) ? U : V;
         };

   if(A_in)
      return {A, subline(A, B, U, V)};
   else if(B_in)
      return {subline(B, A, U, V), B};
   if(is_between_A_B(U) and is_between_A_B(V)) return {U, V};
   return {Vector2T<T>::nan(), Vector2T<T>::nan()};
}

// The returned homography maps points in the 'src' AABB to the 'dst' AABB
template<typename T>
inline Eigen::Matrix3d rescale_homography(const AABBT<T>& src,
                                          const AABBT<T>& dst) noexcept
{
   Eigen::Matrix3d H = Eigen::Matrix3d::Identity();
   H(0, 0)           = dst.width() / src.width();
   H(1, 1)           = dst.height() / src.height();
   H(0, 2)           = dst.left - H(0, 0) * src.left;
   H(1, 2)           = dst.top - H(1, 1) * src.top;

   // auto tryit = [&] (Vector2 s, Vector2 d) {
   //     Vector3r x = H * Vector3r(s.x, s.y, 1.0);
   //     x /= x(2);
   //     cout << format("{} => {} ({}, {})",
   //                    s.to_string(), d.to_string(), x(0), x(1))
   //     << endl;
   // };

   // tryit(src.top_left(), dst.top_left());
   // tryit(src.right_bottom(), dst.right_bottom());

   return H;
}

template<typename T>
inline bool in_bounding_box(const Vector3T<T>& X,
                            const Vector3T<T>& A,
                            const Vector3T<T>& B)
{
   for(auto i = 0; i < 3; ++i)
      if(X(i) < A(i)) return false;
   for(auto i = 0; i < 3; ++i)
      if(X(i) > B(i)) return false;
   return true;
};

} // namespace perceive