#ifndef DART_NEURAL_IK_MAPPING_HPP_
#define DART_NEURAL_IK_MAPPING_HPP_

#include <memory>
#include <optional>
#include <string>

#include <Eigen/Dense>

#include "dart/dynamics/BodyNode.hpp"
#include "dart/dynamics/Skeleton.hpp"
#include "dart/neural/Mapping.hpp"

namespace dart {

namespace neural {

enum IKMappingEntryType
{
  NODE_SPATIAL,
  NODE_LINEAR,
  NODE_ANGULAR,
  COM
};

struct IKMappingEntry
{
  IKMappingEntryType type;
  std::string skelName;
  int bodyNodeOffset; // can be -1 for COM entries

  IKMappingEntry(IKMappingEntryType type, std::string skelName)
    : type(type), skelName(skelName), bodyNodeOffset(-1){};

  IKMappingEntry(IKMappingEntryType type, dynamics::BodyNode* node)
    : type(type),
      skelName(node->getSkeleton()->getName()),
      bodyNodeOffset(node->getIndexInSkeleton()){};
};

class IKMapping : public Mapping
{
public:
  IKMapping(std::shared_ptr<simulation::World> world);

  /// When we called setPosition(), we need to run an IK solve. This
  /// sets the limit on the number of iterations of our solver to run.
  void setIKIterationLimit(int limit);
  int getIKIterationLimit();

  /// This adds the spatial (6D) coordinates of a body node to the list,
  /// increasing Dim size by 6
  void addSpatialBodyNode(dynamics::BodyNode* node);

  /// This adds the linear (3D) coordinates of a body node to the list,
  /// increasing Dim size by 3
  void addLinearBodyNode(dynamics::BodyNode* node);

  /// This adds the angular (3D) coordinates of a body node to the list,
  /// increasing Dim size by 3
  void addAngularBodyNode(dynamics::BodyNode* node);

  int getPosDim() override;
  int getVelDim() override;
  int getControlForceDim() override;
  int getMassDim() override;

  void setPositions(
      std::shared_ptr<simulation::World> world,
      const Eigen::Ref<Eigen::VectorXs>& positions) override;
  void setVelocities(
      std::shared_ptr<simulation::World> world,
      const Eigen::Ref<Eigen::VectorXs>& velocities) override;
  void setControlForces(
      std::shared_ptr<simulation::World> world,
      const Eigen::Ref<Eigen::VectorXs>& forces) override;
  void setMasses(
      std::shared_ptr<simulation::World> world,
      const Eigen::Ref<Eigen::VectorXs>& masses) override;

  void getPositionsInPlace(
      std::shared_ptr<simulation::World> world,
      /* OUT */ Eigen::Ref<Eigen::VectorXs> positions) override;
  void getVelocitiesInPlace(
      std::shared_ptr<simulation::World> world,
      /* OUT */ Eigen::Ref<Eigen::VectorXs> velocities) override;
  void getControlForcesInPlace(
      std::shared_ptr<simulation::World> world,
      /* OUT */ Eigen::Ref<Eigen::VectorXs> forces) override;
  void getMassesInPlace(
      std::shared_ptr<simulation::World> world,
      /* OUT */ Eigen::Ref<Eigen::VectorXs> masses) override;

  /// This gets a Jacobian relating the changes in the inner positions (the
  /// "real" positions) to the corresponding outer positions (the "mapped"
  /// positions)
  Eigen::MatrixXs getRealPosToMappedPosJac(
      std::shared_ptr<simulation::World> world) override;

  /// This gets a Jacobian relating the changes in the inner velocities (the
  /// "real" velocities) to the corresponding outer positions (the "mapped"
  /// positions)
  Eigen::MatrixXs getRealVelToMappedPosJac(
      std::shared_ptr<simulation::World> world) override;

  /// This gets a Jacobian relating the changes in the inner velocity (the
  /// "real" velocity) to the corresponding outer velocity (the "mapped"
  /// velocity)
  Eigen::MatrixXs getRealVelToMappedVelJac(
      std::shared_ptr<simulation::World> world) override;

  /// This gets a Jacobian relating the changes in the inner position (the
  /// "real" position) to the corresponding outer velocity (the "mapped"
  /// velocity)
  Eigen::MatrixXs getRealPosToMappedVelJac(
      std::shared_ptr<simulation::World> world) override;

  /// This gets a Jacobian relating the changes in the inner force (the
  /// "real" force) to the corresponding outer force (the "mapped"
  /// force)
  Eigen::MatrixXs getRealForceToMappedForceJac(
      std::shared_ptr<simulation::World> world) override;

  /// This gets a Jacobian relating the changes in the inner mass (the
  /// "real" mass) to the corresponding outer mass (the "mapped"
  /// mass)
  Eigen::MatrixXs getRealMassToMappedMassJac(
      std::shared_ptr<simulation::World> world) override;

  Eigen::VectorXs getPositionLowerLimits(
      std::shared_ptr<simulation::World> world) override;
  Eigen::VectorXs getPositionUpperLimits(
      std::shared_ptr<simulation::World> world) override;
  Eigen::VectorXs getVelocityLowerLimits(
      std::shared_ptr<simulation::World> world) override;
  Eigen::VectorXs getVelocityUpperLimits(
      std::shared_ptr<simulation::World> world) override;
  Eigen::VectorXs getControlForceLowerLimits(
      std::shared_ptr<simulation::World> world) override;
  Eigen::VectorXs getControlForceUpperLimits(
      std::shared_ptr<simulation::World> world) override;
  Eigen::VectorXs getMassLowerLimits(
      std::shared_ptr<simulation::World> world) override;
  Eigen::VectorXs getMassUpperLimits(
      std::shared_ptr<simulation::World> world) override;

protected:
  /// This returns the number of dimensions that the IK mapping represents.
  int getDim();

  /// Computes a Jacobian that transforms changes in joint angle to changes in
  /// IK body positions (expressed in log space).
  Eigen::MatrixXs getPosJacobian(std::shared_ptr<simulation::World> world);

  /// Computes the pseudo-inverse of the pos Jacobian
  Eigen::MatrixXs getPosJacobianInverse(
      std::shared_ptr<simulation::World> world);

  /// Computes a Jacobian that transforms changes in joint vel to changes in
  /// IK body vels (expressed in log space).
  Eigen::MatrixXs getVelJacobian(std::shared_ptr<simulation::World> world);

  /// Computes the pseudo-inverse of the vel Jacobian
  Eigen::MatrixXs getVelJacobianInverse(
      std::shared_ptr<simulation::World> world);

  /// Computes a Jacobian of J(x)*vel wrt pos
  Eigen::MatrixXs getJacobianOfJacVelWrtPosition(
      std::shared_ptr<simulation::World> world);

  /// The brute force version of getJacobianOfJacVelWrtPosition()
  Eigen::MatrixXs bruteForceJacobianOfJacVelWrtPosition(
      std::shared_ptr<simulation::World> world);

  std::vector<IKMappingEntry> mEntries;

  int mMassDim;
  int mIKIterationLimit;
};

} // namespace neural
} // namespace dart

#endif


#include "kr_quadrotor_simulator/Quadrotor.h"

#include <Eigen/Geometry>
#include <boost/bind.hpp>
#include <boost/numeric/odeint.hpp>
#include <iostream>

namespace odeint = boost::numeric::odeint;

namespace QuadrotorSimulator
{
Quadrotor::Quadrotor()
{
  g_ = 9.81;
  mass_ = 0.5;
  double Ixx = 2.64e-3, Iyy = 2.64e-3, Izz = 4.96e-3;
  prop_radius_ = 0.099;
  J_ = Eigen::Vector3d(Ixx, Iyy, Izz).asDiagonal();

  kf_ = 5.55e-8;
  // km_ = 2.5e-9; // from Nate
  // km = (Cq/Ct)*Dia*kf
  // Cq/Ct for 8 inch props from UIUC prop db ~ 0.07
  km_ = 0.07 * (2 * prop_radius_) * kf_;

  arm_length_ = 0.17;
  motor_time_constant_ = 1.0 / 20;
  min_rpm_ = 1500;
  max_rpm_ = 7500;

  state_.x = Eigen::Vector3d::Zero();
  state_.v = Eigen::Vector3d::Zero();
  state_.R = Eigen::Matrix3d::Identity();
  state_.omega = Eigen::Vector3d::Zero();
  state_.motor_rpm = Eigen::Array4d::Zero();
  updateInternalState();

  input_ = Eigen::Array4d::Zero();
  external_force_ = Eigen::Vector3d::Zero();
  external_moment_ = Eigen::Vector3d::Zero();
}

void Quadrotor::step(double dt)
{
  odeint::integrate(boost::ref(*this), internal_state_, 0.0, dt, dt);

  for(int i = 0; i < 3; i++)
  {
    state_.x(i) = internal_state_[0 + i];
    state_.v(i) = internal_state_[3 + i];
    state_.R(i, 0) = internal_state_[6 + i];
    state_.R(i, 1) = internal_state_[9 + i];
    state_.R(i, 2) = internal_state_[12 + i];
    state_.omega(i) = internal_state_[15 + i];
  }
  state_.motor_rpm(0) = internal_state_[18];
  state_.motor_rpm(1) = internal_state_[19];
  state_.motor_rpm(2) = internal_state_[20];
  state_.motor_rpm(3) = internal_state_[21];

  // Re-orthonormalize R (polar decomposition)
  Eigen::LLT<Eigen::Matrix3d> llt(state_.R.transpose() * state_.R);
  Eigen::Matrix3d P = llt.matrixL();
  Eigen::Matrix3d R = state_.R * P.inverse();
  state_.R = R;

  // Don't go below zero, simulate floor
  if(state_.x(2) < 0.0 && state_.v(2) < 0)
  {
    state_.x(2) = 0;
    state_.v(2) = 0;
  }
  updateInternalState();
}

void Quadrotor::operator()(const Quadrotor::InternalState &x, Quadrotor::InternalState &dxdt, const double /* t */)
{
  State cur_state;
  for(int i = 0; i < 3; i++)
  {
    cur_state.x(i) = x[0 + i];
    cur_state.v(i) = x[3 + i];
    cur_state.R(i, 0) = x[6 + i];
    cur_state.R(i, 1) = x[9 + i];
    cur_state.R(i, 2) = x[12 + i];
    cur_state.omega(i) = x[15 + i];
  }
  for(int i = 0; i < 4; i++)
  {
    cur_state.motor_rpm(i) = x[18 + i];
  }

  // Re-orthonormalize R (polar decomposition)
  Eigen::LLT<Eigen::Matrix3d> llt(cur_state.R.transpose() * cur_state.R);
  Eigen::Matrix3d P = llt.matrixL();
  Eigen::Matrix3d R = cur_state.R * P.inverse();

  Eigen::Vector3d x_dot, v_dot, omega_dot;
  Eigen::Matrix3d R_dot;
  Eigen::Array4d motor_rpm_dot;
  Eigen::Array4d motor_rpm_sq;
  Eigen::Matrix3d omega_hat(Eigen::Matrix3d::Zero());

  omega_hat(2, 1) = cur_state.omega(0);
  omega_hat(1, 2) = -cur_state.omega(0);
  omega_hat(0, 2) = cur_state.omega(1);
  omega_hat(2, 0) = -cur_state.omega(1);
  omega_hat(1, 0) = cur_state.omega(2);
  omega_hat(0, 1) = -cur_state.omega(2);

  if(motor_time_constant_ == 0)
  {
    motor_rpm_dot = Eigen::Array4d::Zero();
    // NOTE(Kartik): Directly modifying internal_state here
    internal_state_[18] = input_(0);
    internal_state_[19] = input_(1);
    internal_state_[20] = input_(2);
    internal_state_[21] = input_(3);
    cur_state.motor_rpm = input_;
  }
  else
    motor_rpm_dot = (input_ - cur_state.motor_rpm) / motor_time_constant_;

  motor_rpm_sq = cur_state.motor_rpm.square();

  double thrust = kf_ * motor_rpm_sq.sum();
  Eigen::Vector3d moments;
  moments(0) = kf_ * (motor_rpm_sq(2) - motor_rpm_sq(3)) * arm_length_;
  moments(1) = kf_ * (motor_rpm_sq(1) - motor_rpm_sq(0)) * arm_length_;
  moments(2) = km_ * (motor_rpm_sq(0) + motor_rpm_sq(1) - motor_rpm_sq(2) - motor_rpm_sq(3));

  x_dot = cur_state.v;
  v_dot = -Eigen::Vector3d(0, 0, g_) + thrust * R.col(2) / mass_ + external_force_ / mass_;
  if(drag_coefficient_ != 0)
  {
    Eigen::Matrix3d P;
    P << 1, 0, 0, 0, 1, 0, 0, 0, 0;
    v_dot -= drag_coefficient_ / mass_ * R * P * R.transpose() * cur_state.v;
  }
  R_dot = R * omega_hat;
  omega_dot = J_.inverse() * (moments - cur_state.omega.cross(J_ * cur_state.omega) + external_moment_);

  for(int i = 0; i < 3; i++)
  {
    dxdt[0 + i] = x_dot(i);
    dxdt[3 + i] = v_dot(i);
    dxdt[6 + i] = R_dot(i, 0);
    dxdt[9 + i] = R_dot(i, 1);
    dxdt[12 + i] = R_dot(i, 2);
    dxdt[15 + i] = omega_dot(i);
  }
  for(int i = 0; i < 4; i++)
  {
    dxdt[18 + i] = motor_rpm_dot(i);
  }
}

void Quadrotor::setInput(double u1, double u2, double u3, double u4)
{
  input_(0) = u1;
  input_(1) = u2;
  input_(2) = u3;
  input_(3) = u4;
  if(u1 != 0 || u2 != 0 || u3 != 0 || u4 != 0)  // Limit to min/max RPM if any of the RPMs are non-zero
  {
    for(int i = 0; i < 4; i++)
    {
      if(input_(i) > max_rpm_)
        input_(i) = max_rpm_;
      else if(input_(i) < min_rpm_)
        input_(i) = min_rpm_;
    }
  }
}

const Quadrotor::State &Quadrotor::getState() const
{
  return state_;
}
void Quadrotor::setState(const Quadrotor::State &state)
{
  state_.x = state.x;
  state_.v = state.v;
  state_.R = state.R;
  state_.omega = state.omega;
  state_.motor_rpm = state.motor_rpm;

  updateInternalState();
}

double Quadrotor::getMass() const
{
  return mass_;
}
void Quadrotor::setMass(double mass)
{
  mass_ = mass;
}

double Quadrotor::getDragCoefficient() const
{
  return drag_coefficient_;
}

void Quadrotor::setDragCoefficient(double drag_coefficient)
{
  drag_coefficient_ = drag_coefficient;
}

double Quadrotor::getGravity() const
{
  return g_;
}
void Quadrotor::setGravity(double g)
{
  g_ = g;
}

const Eigen::Matrix3d &Quadrotor::getInertia() const
{
  return J_;
}
void Quadrotor::setInertia(const Eigen::Matrix3d &inertia)
{
  if(inertia != inertia.transpose())
  {
    std::cerr << "Inertia matrix not symmetric, not setting" << std::endl;
    return;
  }
  J_ = inertia;
}

double Quadrotor::getArmLength() const
{
  return arm_length_;
}
void Quadrotor::setArmLength(double d)
{
  if(d <= 0)
  {
    std::cerr << "Arm length <= 0, not setting" << std::endl;
    return;
  }

  arm_length_ = d;
}

double Quadrotor::getPropRadius() const
{
  return prop_radius_;
}
void Quadrotor::setPropRadius(double r)
{
  if(r <= 0)
  {
    std::cerr << "Prop radius <= 0, not setting" << std::endl;
    return;
  }
  prop_radius_ = r;
}

double Quadrotor::getPropellerThrustCoefficient() const
{
  return kf_;
}
void Quadrotor::setPropellerThrustCoefficient(double kf)
{
  if(kf <= 0)
  {
    std::cerr << "Thrust coefficient <= 0, not setting" << std::endl;
    return;
  }

  kf_ = kf;
}

double Quadrotor::getPropellerMomentCoefficient() const
{
  return km_;
}
void Quadrotor::setPropellerMomentCoefficient(double km)
{
  if(km <= 0)
  {
    std::cerr << "Moment coefficient <= 0, not setting" << std::endl;
    return;
  }

  km_ = km;
}

double Quadrotor::getMotorTimeConstant() const
{
  return motor_time_constant_;
}
void Quadrotor::setMotorTimeConstant(double k)
{
  if(k < 0)
  {
    std::cerr << "Motor time constant <= 0, not setting" << std::endl;
    return;
  }

  motor_time_constant_ = k;
}

const Eigen::Vector3d &Quadrotor::getExternalForce() const
{
  return external_force_;
}
void Quadrotor::setExternalForce(const Eigen::Vector3d &force)
{
  external_force_ = force;
}

const Eigen::Vector3d &Quadrotor::getExternalMoment() const
{
  return external_moment_;
}
void Quadrotor::setExternalMoment(const Eigen::Vector3d &moment)
{
  external_moment_ = moment;
}

double Quadrotor::getMaxRPM() const
{
  return max_rpm_;
}
void Quadrotor::setMaxRPM(double max_rpm)
{
  if(max_rpm <= 0)
  {
    std::cerr << "Max rpm <= 0, not setting" << std::endl;
    return;
  }
  max_rpm_ = max_rpm;
}

double Quadrotor::getMinRPM() const
{
  return min_rpm_;
}
void Quadrotor::setMinRPM(double min_rpm)
{
  if(min_rpm < 0)
  {
    std::cerr << "Min rpm < 0, not setting" << std::endl;
    return;
  }
  min_rpm_ = min_rpm;
}

void Quadrotor::updateInternalState()
{
  for(int i = 0; i < 3; i++)
  {
    internal_state_[0 + i] = state_.x(i);
    internal_state_[3 + i] = state_.v(i);
    internal_state_[6 + i] = state_.R(i, 0);
    internal_state_[9 + i] = state_.R(i, 1);
    internal_state_[12 + i] = state_.R(i, 2);
    internal_state_[15 + i] = state_.omega(i);
  }
  internal_state_[18] = state_.motor_rpm(0);
  internal_state_[19] = state_.motor_rpm(1);
  internal_state_[20] = state_.motor_rpm(2);
  internal_state_[21] = state_.motor_rpm(3);
}

}  // namespace QuadrotorSimulator


#include "Util.h"
//#include <Eigen/Dense>
#include "cryptoTools/Common/Log.h"
#include "cryptoTools/Common/Matrix.h"
#include "cryptoTools/Crypto/PRNG.h"
#include "cryptoTools/Common/BitIterator.h"
#include <numeric>
#include <iomanip>
#include <future>
#include <cmath>

#include "Mtx.h"
#include <deque>
#include "LdpcSampler.h"

#ifdef ENABLE_ALGO994
extern "C" {
#include "libOTe/Tools/LDPC/Algo994/utils.h"
#include "libOTe/Tools/LDPC/Algo994/generations.h"
#include "libOTe/Tools/LDPC/Algo994/data_defs.h"
}
#endif

#include <fstream>

namespace osuCrypto
{

    //using Mtx = Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic>;

#ifdef ENABLE_ALGO994
    int alg994 = ALG_SAVED_UNROLLED;
    int num_saved_generators =  5;
    int num_cores = 4;
    int num_permutations = 5;
    int print_matrices = 0;
#endif

    int minDist(std::string path, u64 numTHreads, bool verbose)
    {
#ifdef ENABLE_ALGO994

        char* inputMatrix;
        int   info, k, n, dist;
        num_cores = numTHreads;

        // Read input matrix.
        info = read_char_matrix((char*)path.c_str(), &inputMatrix, &k, &n);
        if (info != 0) {
            fprintf(stderr, "\n");
            fprintf(stderr, "ERROR in read_char_matrix: ");
            fprintf(stderr, "Error while reading input file with matrix.\n\n\n");
        }

        assert(
            alg994 == ALG_BASIC || // 1
            alg994 == ALG_OPTIMIZED || // 2
            alg994 == ALG_STACK || // 3
            alg994 == ALG_SAVED || // 4
            alg994 == ALG_SAVED_UNROLLED || // 5
            alg994 == ALG_GRAY  // 10
        );

        // Compute distance of input matrix.
        dist = compute_distance_of_matrix_impl(inputMatrix, k, n,
            alg994,
            num_saved_generators,
            num_cores,
            num_permutations,
            print_matrices);

        // Remove matrices.
        free(inputMatrix);

        return dist;
#else
        throw std::runtime_error("also 994 not enabled. " LOCATION);
#endif
    }

    int minDist2(const DenseMtx& mtx, u64 nt, bool verbose)
    {
        std::string outPath("./deleteMe");
        std::fstream out(outPath, std::fstream::trunc | std::fstream::out);

        if (out.is_open() == false)
        {
            std::cout << "failed to open: " << outPath << std::endl;
            return 0;
        }

        std::vector<std::pair<u64, u64>> swaps;
        auto G = computeGen(mtx, swaps);

        out << G.rows() << " " << G.cols() << " matrix dimensions\n"
            << G << std::endl;

        out.close();

        u64 d;

        d = minDist(outPath, nt, false);

        std::remove(outPath.c_str());
        return (int)d;
    }

    u64 numNonzeroRows(const DenseMtx& mtx)
    {
        u64 r = 0;
        for (u64 i = 0; i < mtx.rows(); ++i)
        {
            if (mtx.row(i).isZero())
                ++r;
        }

        return mtx.rows() - r;
    }

    //u64 rank(const DenseMtx& mtx)
    //{
    //    DenseMtx m2 = gaussianElim(mtx);
    //    return numNonzeroRows(m2);
    //}

    void ithCombination(u64 index, u64 n, std::vector<u64>& set)
    {
        //'''Yields the items of the single combination that would be at the provided
        //(0-based) index in a lexicographically sorted list of combinations of choices
        //of k items from n items [0,n), given the combinations were sorted in
        //descending order. Yields in descending order.
        //'''
        u64 nCk = 1;
        u64 nMinusI = n;
        u64 iPlus1 = 1;

        auto k = set.size();

        // nMinusI, iPlus1 in zip(range(n, n - k, -1), range(1, k + 1)):
        for (; nMinusI != n - k; --nMinusI, ++iPlus1)
        {
            nCk *= nMinusI;
            nCk /= iPlus1;
        }

        //std::cout << "nCk " << nCk << std::endl;

        auto curIndex = nCk;
        for (auto kk = k; kk != 0ull; --kk)//in range(k, 0, -1):
        {
            //std::cout << "kk " << kk << " " <<  nCk << std::endl;
            nCk *= kk;
            nCk /= n;
            while (curIndex - nCk > index) {
                curIndex -= nCk;
                nCk *= (n - kk);
                nCk -= nCk % kk;
                n -= 1;
                nCk /= n;
            }
            n -= 1;

            set[kk - 1] = n;
        }
    }

    std::vector<u64> ithCombination(u64 index, u64 n, u64 k)
    {
        std::vector<u64> set(k);
        ithCombination(index, n, set);
        return set;
    }

    u64 choose(u64 n, u64 k)
    {
        if (k == 0) return 1;
        return (n * choose(n - 1, k - 1)) / k;
    }

    struct NChooseK
    {
        u64 mN;
        u64 mK;
        u64 mI, mEnd;
        std::vector<u64> mSet;

        NChooseK(u64 n, u64 k, u64 begin = 0, u64 end = -1)
            : mN(n)
            , mK(k)
            , mI(begin)
            , mEnd(std::min<u64>(choose(n, k), end))
        {
            assert(k <= n);
            mSet = ithCombination(begin, n, k);
        }

        const std::vector<u64>& operator*() const
        {
            return mSet;
        }

        void operator++()
        {
            ++mI;
            assert(mI <= mEnd);

            u64 i = 0;
            while (i < mK - 1 && mSet[i] + 1 == mSet[i + 1])
                ++i;

            //if (i == mK - 1 && mSet.back() == mN - 1)
            //{
            //    mSet.clear();
            //    return;
            //    //assert(mSet.back() != mN - 1);
            //}

            ++mSet[i];
            for (u64 j = 0; j < i; ++j)
                mSet[j] = j;
        }

        explicit operator bool() const
        {
            return mI < mEnd;
        }

    };

    bool isZero(const span<block>& sum)
    {
        for (auto& b : sum)
            if (b != oc::ZeroBlock)
            {
                return false;
                break;
            }
        return true;
    }
    bool isEq(const span<const block>& u, const span<const block>& v)
    {
        assert(u.size() == v.size());
        return memcmp(u.data(), v.data(), v.size_bytes()) == 0;
    }

    template <typename T>
    class queue
    {
    private:
        std::mutex              d_mutex;
        std::condition_variable d_condition;
        std::deque<T>           d_queue;
    public:
        void push(T const& value) {
            {
                std::unique_lock<std::mutex> lock(this->d_mutex);
                d_queue.push_front(value);
            }
            this->d_condition.notify_one();
        }
        T pop() {
            std::unique_lock<std::mutex> lock(this->d_mutex);
            this->d_condition.wait(lock, [=] { return !this->d_queue.empty(); });
            T rc(std::move(this->d_queue.back()));
            this->d_queue.pop_back();
            return rc;
        }
    };

    std::pair<double, std::vector<u64>> minDist(const DenseMtx& mtx, bool verbose, u64 numThreads)
    {
        assert(mtx.rows() < mtx.cols());

        u64 percision = 2;
        u64 p = (u64)std::pow(10, percision);

        std::mutex mut;
        queue<std::function<void()>> queue;
        std::vector<std::thread> thrds(numThreads);
        for (u64 i = 0; i < thrds.size(); ++i)
        {
            thrds[i] = std::thread([&queue]() {

                while (true)
                {
                    std::function<void()> fn =queue.pop();

                    if (!fn)
                        return;
                    fn();
                }
                });
        }

        u64 dd = mtx.mData.cols();
#define ASSUME_DD_1
#ifdef ASSUME_DD_1
        if (dd != 1)
            throw RTE_LOC;
#endif
        for (u64 weight = 2; weight < mtx.rows(); ++weight)
        {
            auto total = choose(mtx.cols(), weight);
            u64 next = 0;
            std::atomic<u64> ii(0);
            //std::atomic<u64> rem = numThreads;

            bool done = false;
            using Ret = std::pair<double, std::vector<u64>>;
            std::vector<std::promise<Ret>> prom(numThreads);
            //auto fu = prom.get_future();

            for (u64 i = 0; i < numThreads; ++i)
            {
                queue.push([&, i, weight]() {

                    auto begin = i * total / numThreads;
                    auto end = (i + 1) * total / numThreads;
                    auto iter = NChooseK(mtx.cols(), weight, begin, end);
                    //u64& mI = iter.mI;
                    std::vector<u64> set;
#ifdef ASSUME_DD_1
                    block sum;
#else
                    std::vector<block> sum(dd);
#endif
                    while (begin++ != end && !done)
                    {
                        set = *iter;

                        if (verbose && ii >= next)
                        {
                            std::lock_guard<std::mutex> lock(mut);
                            if (verbose && ii >= next)
                            {
                                auto cur = ii * p / total;
                                next = (cur + 1) * total / p;;
                                std::cout << "\r" << weight << "." << std::setw(percision) << std::setfill('0') << cur << "       " << std::flush;
                            }
                        }
#ifdef ASSUME_DD_1
                        auto ptr = mtx.mData.data();
                        sum = ptr[set[0]];
                        for (u64 i = 1; i < weight; ++i)
                        {
                            auto col = ptr[set[i]];
                            sum = sum ^ col;
                        }

                        auto linDep = sum == oc::ZeroBlock;
#else

                        auto v = mtx.col(set[0]);
                        std::copy(v.data(), v.data() + dd, sum.data());

                        for (u64 i = 1; i < weight; ++i)
                        {
                            auto col = mtx.col(set[i]);
                            for (u64 j = 0; j < dd; ++j)
                                sum[j] = sum[j] ^ col[j];
                        }

                        auto linDep = isZero(sum);
#endif
                        if (linDep)
                        {
                            std::lock_guard<std::mutex> lock(mut);
                            if (verbose)
                            {
                                std::cout << std::endl;
                            }
                            done = true;

                            prom[i].set_value(std::make_pair(weight + ii / double(total), set));
                            return;
                        }

                        ++ii;

                        //++iter;
                        {
                            //++(mI);
                            //assert(mI <= iter.mEnd);

                            u64 i = 0;
                            while (i < iter.mK - 1 && iter.mSet[i] + 1 == iter.mSet[i + 1])
                                ++i;

                            ++iter.mSet[i];
                            for (u64 j = 0; j < i; ++j)
                                iter.mSet[j] = j;
                        }
                    }

                    prom[i].set_value({});
                    return;
                    });
            }

            Ret ret;
            for (u64 i = 0; i < numThreads; ++i)
            {
                auto cc = prom[i].get_future().get();
                if (cc.second.size())
                    ret = cc;
            }

            if (done)
            {
                for (u64 i = 0; i < numThreads; ++i)
                    queue.push({});
                for (u64 i = 0; i < numThreads; ++i)
                    thrds[i].join();
                return ret;
            }

        }
        assert(0);
        return {};
    }

    std::pair<double, std::vector<u64>> minDist(const DenseMtx& mtx, bool verbose)
    {
        assert(mtx.rows() < mtx.cols());

        u64 percision = 2;
        u64 p = (u64) pow(10, percision);

        for (u64 weight = 2; weight < mtx.rows(); ++weight)
        {
            auto iter = NChooseK(mtx.cols(), weight);
            auto total = choose(mtx.cols(), weight);
            u64 prev = -1;
            u64 ii = 0;
            std::vector<block> sum(mtx.mData.cols());

            while (iter)
            {
                auto& set = *iter;

                auto cur = ii * p / total;
                if (verbose && prev != cur)
                {
                    prev = cur;
                    std::cout << "\r" << weight << "." << std::setw(percision) << std::setfill('0') << cur << "       " << std::flush;
                }

                auto v = mtx.col(set[0]);
                std::copy(v.begin(), v.end(), sum.begin());

                for (u64 i = 1; i < set.size(); ++i)
                {
                    auto col = mtx.col(set[i]);
                    for (u64 j = 0; j < sum.size(); ++j)
                        sum[j] = sum[j] ^ col[j];
                }

                if (isZero(sum))
                {
                    if (verbose)
                        std::cout << std::endl;
                    return std::make_pair(weight + ii / double(total), set);
                }

                ++ii;
                ++iter;
            }
        }
        assert(0);
        return {};
    }

    DenseMtx computeGen(DenseMtx& H)
    {
        assert(H.rows() < H.cols());

        auto n = H.cols();
        auto m = H.rows();
        auto k = n - m;

        auto mtx = H.subMatrix(0, k, m, m);

        auto P = H.subMatrix(0, 0, m, k);

        for (u64 i = 0; i < m; ++i)
        {
            if (mtx(i, i) == 0)
            {
                for (u64 j = i + 1; j < m; ++j)
                {
                    if (mtx(j, i) == 1)
                    {
                        mtx.row(i).swap(mtx.row(j));
                        P.row(i).swap(P.row(j));
                        break;
                    }
                }

                if (mtx(i, i) == 0)
                {
                    //std::cout << mtx << std::endl;
                    return {};
                }
            }

            for (u64 j = 0; j < m; ++j)
            {
                if (j != i && mtx(j, i))
                {
                    for (u64 l = 0; l < m; ++l)
                    {
                        mtx(j, l) ^= mtx(i, l);
                    }

                    for (u64 l = 0; l < k; ++l)
                        P(j, l) ^= P(i, l);
                }
            }

        }

        DenseMtx G(k, n);
        for (u64 i = 0; i < k; ++i)
            G(i, i) = 1;

        for (u64 i = 0; i < m; ++i)
        {
            for (u64 j = 0; j < k; ++j)
            {
                G(j, i + k) = P(i, j);
            }
        }

        return G;
    }

    DenseMtx computeGen(DenseMtx H, std::vector<std::pair<u64, u64>>& colSwaps)
    {
        assert(H.rows() < H.cols());

        auto n = H.cols();
        auto m = H.rows();
        auto k = n - m;
        colSwaps.clear();

        for (u64 row = 0, col = k; row < m; ++row, ++col)
        {
            //std::cout << row << std::endl << H << std::endl;;

            if (H(row, col) == 0)
            {
                bool found = false;
                // look fow a row swap
                for (u64 row2 = row + 1; row2 < m && found == false; ++row2)
                {
                    if (H(row2, col) == 1)
                    {
                        H.row(row).swap(H.row(row2));
                        found = true;
                    }
                }

                if (found == false)
                {
                    // look for a col swap

                    for (u64 col2 = 0; col2 < k && found == false; ++col2)
                    {
                        for (u64 row2 = row; row2 < m && found == false; ++row2)
                        {
                            if (H(row2, col2) == 1)
                            {
                                H.row(row).swap(H.row(row2));

                                // swap columns.
                                colSwaps.push_back({ col,col2 });
                                auto c0 = H.col(col);
                                auto c1 = H.col(col2);
                                std::swap_ranges(c0.begin(), c0.end(), c1.begin());
                                found = true;
                            }
                        }
                    }
                }

                if (found == false)
                {
                    // can not be put in systematic form.
                    //std::cout  << H << std::endl;

                    return {};
                }
            }

            // clear all other ones from the current column.
            for (u64 row2 = 0; row2 < m; ++row2)
            {
                if (row2 != row && H(row2, col))
                {
                    // row2 = row ^ row2
                    for (u64 col2 = 0; col2 < n; ++col2)
                    {
                        H(row2, col2) ^= H(row, col2);
                    }
                }
            }

        }

        auto P = H.subMatrix(0, 0, m, k);

        DenseMtx G(k, n);
        for (u64 i = 0; i < k; ++i)
            G(i, i) = 1;

        for (u64 i = 0; i < m; ++i)
        {
            for (u64 j = 0; j < k; ++j)
            {
                G(j, i + k) = P(i, j);
            }
        }

        return G;
    }

    namespace tests
    {
        void computeGen_test(const oc::CLP& cmd)
        {
            u64 n = 30;
            u64 k = n / 4;
            u64 m = n - k;
            u64 t = 100;

            oc::PRNG prng(block(cmd.getOr("s", 0), 0));

            DenseMtx W(t, k);

            for (u64 i = 0; i < W.rows(); ++i)
                for (u64 j = 0; j < W.cols(); ++j)
                    W(i, j) = prng.getBit();

            DenseMtx H(m, n), G;

            while (G.rows() == 0)
            {
                for (u64 i = 0; i < H.rows(); ++i)
                {
                    //H(i, k + i) = 1;

                    for (u64 j = 0; j < H.cols(); ++j)
                        H(i, j) = prng.getBit();
                }

                G = computeGen(H);
            }

            //std::cout << H << std::endl;
            //std::cout << G << std::endl;

            auto C = W * G;

            auto S = H * C.transpose();

            for (u64 i = 0; i < S.rows(); ++i)
                for (u64 j = 0; j < S.cols(); ++j)
                    assert(S(i, j) == 0);
        }

        void computeGen_test2(const oc::CLP& cmd)
        {
            u64 n = 100;
            u64 k = n / 4;
            u64 m = n - k;
            u64 t = 100;

            oc::PRNG prng(block(cmd.getOr("s", 0), 0));

            DenseMtx W(t, k);

            for (u64 i = 0; i < W.rows(); ++i)
                for (u64 j = 0; j < W.cols(); ++j)
                    W(i, j) = prng.getBit();

            DenseMtx H(m, n), G;
            std::vector<std::pair<u64, u64>> swaps;

            while (G.rows() == 0)
            {
                for (u64 i = 0; i < H.rows(); ++i)
                {
                    //H(i, k + i) = 1;

                    for (u64 j = 0; j < H.cols(); ++j)
                        H(i, j) = prng.getBit();
                }

                G = computeGen(H, swaps);
            }

            //std::cout << H << std::endl;
            //std::cout << G << std::endl;

            auto C = W * G;

            for(auto s : swaps)
            {
                auto c0 = C.col(s.first);
                auto c1 = C.col(s.second);
                std::swap_ranges(c0.begin(), c0.end(), c1.begin());
            }

            auto S = H * C.transpose();

            for (u64 i = 0; i < S.rows(); ++i)
                for (u64 j = 0; j < S.cols(); ++j)
                    assert(S(i, j) == 0);
        }
    }

    struct selectPrt
    {
        const DenseMtx& mMtx;
        const std::vector<u64>& mCols;

        selectPrt(const DenseMtx& m, const std::vector<u64>& c)
            : mMtx(m)
            , mCols(c)
        {}
    };

    std::ostream& operator<<(std::ostream& o, const selectPrt& p)
    {
        for (u64 i = 0; i < p.mMtx.rows(); ++i)
        {
            auto iter = p.mCols.begin();
            for (u64 j = 0; j < p.mMtx.cols(); ++j)
            {
                if (iter != p.mCols.end() && *iter == j)
                    o << oc::Color::Green;

                o << p.mMtx(i, j) << " ";

                if (iter != p.mCols.end() && *iter == j)
                {
                    o << oc::Color::Default;
                    ++iter;
                }
            }

            o << std::endl;
        }
        return o;
    }

    //
    //DenseMtx uniformFixedColWeight(u64 rows, u64 cols, u64 w, PRNG& prng)
    //{
    //    DenseMtx mtx(rows, cols);
    //    mtx.setZero();
    //    std::vector<u64> rem; rem.reserve(cols * w);
    //    for (u64 i = 0; i < cols; ++i)
    //    {
    //        for (u64 j = 0; j < w; ++j)
    //            rem.push_back(i);
    //    }
    //
    //    std::shuffle(rem.begin(), rem.end(), prng);
    //
    //    while (rem.size())
    //    {
    //        auto i = prng.get<u64>() % rows;
    //        mtx(i, rem.back()) = 1;
    //        rem.pop_back();
    //    }
    //    return mtx;
    //}

    void rank(oc::CLP& cmd)
    {

        //u64 n = 6, k = 4;

        //NChooseK nCk(n, k);
        //u64 i = 0;
        //while (nCk)
        //{
        //    auto set0 = *nCk;
        //    auto set1 = ithCombination(i, n, k);

        //    std::cout << i << ":\n";
        //    for (u64 j = 0; j < k; ++j)
        //        std::cout << set0[j] << " ";
        //    std::cout << "  \n";
        //    for (u64 j = 0; j < k; ++j)
        //        std::cout << set1[j] << " ";
        //    std::cout << std::endl;

        //    ++i;
        //    ++nCk;
        //}
        //return;

        u64 rows = cmd.getOr("m", 20);
        u64 cols = static_cast<u64>(rows * cmd.getOr("e", 2.0));
        u64 weight = cmd.getOr("w", 4);
        auto gaps = cmd.getManyOr<u64>("g", { 0 });

        u64 trials = cmd.getOr("t", 1);
        bool verbose = cmd.isSet("v");
        u64 thrds = cmd.getOr("thrds", std::thread::hardware_concurrency());

        assert(cols > rows);
        assert(rows > weight);
        oc::PRNG prng(block(0, cmd.getOr("s", 0)));

        DenseMtx mtx(rows, cols);

        for (auto gap : gaps)
        {

            u64 dWeight = cmd.getOr("wd", (1 + gap) / 2);
            double avg = 0;

            for (u64 t = 0; t < trials; ++t)
            {
                mtx.setZero();

                if (gap)
                {
                    mtx = sampleTriangularBand(
                        rows, cols, weight, gap, dWeight, false, prng).dense();
                }
                else
                {
                    mtx = sampleFixedColWeight(rows, cols, weight, prng, false).dense();

                }

                auto d = minDist(mtx, verbose, thrds);

                if (verbose)
                {
                    std::cout << " " << d.first;
                    std::cout << "\n" << selectPrt(mtx, d.second) << std::endl;
                }

                avg += d.first;
            }

            std::cout << " ~~ " << gap << " " << avg / trials << std::endl;

        }
        //std::cout << "minDist = " << d.size() << std::endl
        //    << "[";
        //for (u64 i = 0; i < d.size(); ++i)
        //    std::cout << d[i] << " ";
        //std::cout << "]" << std::endl;

        //std::cout << selectPrt(mtx, d) << std::endl;

        //auto m2 = mtx.block(0, 0, rows, rows).transpose();
        //std::cout << m2 << std::endl << std::endl;

        //auto m3 = gaussianElim(m2);

        //std::cout << m3 << std::endl << std::endl;

        //auto r = rank(m3);
        //std::cout << "rank " << r << " / " << m3.rows() << std::endl;
        return;
        //Matrix5x3 m = Matrix5x3::Random();
        //std::cout << "Here is the matrix m:" << endl << m << endl;
        //Eigen::FullPivLU<Mtx> lu(mtx);
        //std::cout << "Here is, up to permutations, its LU decomposition matrix:"
        //    << std::endl << lu.matrixLU() << std::endl;
        //std::cout << "Here is the L part:" << std::endl;
        //Mtx l = Mtx::Identity();
        //l.block<5, 3>(0, 0).triangularView<StrictlyLower>() = lu.matrixLU();
        //cout << l << endl;
        //cout << "Here is the U part:" << endl;
        //Matrix5x3 u = lu.matrixLU().triangularView<Upper>();
        //cout << u << endl;
        //cout << "Let us now reconstruct the original matrix m:" << endl;
        //cout << lu.permutationP().inverse() * l * u * lu.permutationQ().inverse() << endl;
    }
}


#include <boost/math/concepts/real_concept.hpp>


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

#include <Eigen/Geometry>

namespace a2d2_to_ros {

/**
 * @brief Given an X and a Y axis in 3-space, compute a right-handed orthonormal
 * basis with Z orthogonal to the input X and Y, and a new Y orthogonal to X and
 * the computed Z.
 * @pre Vectors X and Y are unique and non-singular.
 * @return A 3x3 matrix whose columns [X, Y, Z] form a right-handed orthonormal
 * basis. If input X and Y are not valid, a zero matrix is returned.
 */
Eigen::Matrix3d get_orthonormal_basis(const Eigen::Vector3d& X,
                                      const Eigen::Vector3d& Y, double epsilon);

/**
 * @brief Compute transform between sensor and global coordinates.
 */
Eigen::Affine3d Tx_global_sensor(const Eigen::Matrix3d& basis,
                                 const Eigen::Vector3d& origin);

}  // namespace a2d2_to_ros

#endif  // A2D2_TO_ROS__TRANSFORM_UTILS_HPP_


//=======================================================================
// Copyright 2015 - 2020 Jeff Linahan
//
// 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 "EmbedStruct.h"
#include <boost/graph/boyer_myrvold_planar_test.hpp>
#include <iostream>
using namespace std;
using namespace boost;

typedef boost::graph_traits<Graph>::vertex_iterator VertIter;

bool test_planar_workaround(Embedding& em, Graph* g)
{
	if( num_vertices(*g) < 5) return true;
	// boyer_myrvold_planarity_test sometimes crashes when given strange graphs

        // workaround for https://github.com/boostorg/graph/issues/195
        VertIter vit, vjt;
        tie(vit, vjt) = vertices(*g);
        for( VertIter next = vit; vit != vjt; ++vit){
                if( 0 == in_degree(*vit, *g) + out_degree(*vit, *g) ){
                        add_edge(*vit, *vit, *g);
                }
        }

	return boyer_myrvold_planarity_test(boyer_myrvold_params::graph = *g, boyer_myrvold_params::embedding = em);
}

EmbedStruct ctor_workaround(Graph* g)
{
	EmbedStruct es;
	es.g = g;
	es.storage = EmbeddingStorage(num_vertices(*g));
	es.em = es.storage.begin();
	bool p = test_planar_workaround(es.em, g);
	return es;

}

EmbedStruct::EmbedStruct(Graph const* g) : g(g), storage(num_vertices(*g)), em(storage.begin())
{
		cout << "EmbedStruct ctor\n";
		bool p = test_planar();
		if (!p){
				cout << "this graph is not planar\n";
		}
}

bool EmbedStruct::test_planar() const
{
	// boyer_myrvold_planarity_test sometimes crashes when given strange graphs
	return boyer_myrvold_planarity_test(boyer_myrvold_params::graph = *g, boyer_myrvold_params::embedding = em);
}

void EmbedStruct::print() const
{
		cout << "\n************** Embedding ************\n";
		VertIter vi, vend;
		for( tie(vi, vend) = vertices(*g); vi != vend; ++vi ){
				cout << "vert " << *vi << ": ";
				for( auto ei = em[*vi].begin(); ei != em[*vi].end(); ++ei ){
						auto src = source(*ei, *g);
						auto tar = target(*ei, *g);
						if( tar == *vi ) swap(src, tar);
						cout << tar << ' ';
				}
				cout << "\n";
		}
		cout << "*************************************\n";
}


// nth_element example
#include <iostream>
#include <algorithm>
#include <vector>
#include <utility>
#include <boost/lambda/lambda.hpp>
#include <boost/lambda/bind.hpp>

using namespace std;
using namespace boost::lambda;

bool mycompare(int i, int j, int x) { return i < j; }

bool myfunction (int i,int j) { return (i<j); }

int main () {
  vector<int> myvector;
  vector<int>::iterator it;

  // set some values:
  for (int i=1; i<10; i++) myvector.push_back(i);   // 1 2 3 4 5 6 7 8 9

  random_shuffle (myvector.begin(), myvector.end());

  // using default comparison (operator <):
  nth_element (myvector.begin(), myvector.begin()+5, myvector.end());

  // using function as comp
  nth_element (myvector.begin(), myvector.begin()+5, myvector.end(),myfunction);

  // print out content:
  cout << "myvector contains:";
  for (it=myvector.begin(); it!=myvector.end(); ++it)
    cout << " " << *it;

  cout << endl;

  std::pair<int, int> a = make_pair(3, 11);
  std::pair<int, int> b = make_pair(5, 8);
  std::pair<int, int> c = make_pair(7, 3);
  std::pair<int, int> z = std::min(a, min(b, c));

  cout << z.first << ", " << z.second << endl;

  random_shuffle (myvector.begin(), myvector.end());

  for (it=myvector.begin(); it!=myvector.end(); ++it)
    cout << " " << *it;
  cout << endl;

  nth_element(myvector.begin(), myvector.begin()+5, myvector.end(),
      bind(mycompare, _1, _2, 4));

  for (it=myvector.begin(); it!=myvector.end(); ++it)
    cout << " " << *it;
  cout << endl;

  return 0;
}


#include <boost/foreach.hpp>
#include <pcl/common/centroid.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp_nl.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <tf/transform_listener.h>
#include <tf_conversions/tf_eigen.h>

#include "six_point_two_eight/point_cloud_utilities.h"

namespace point_cloud_utilities {
  // 実装。

  typedef pcl::PointXYZRGB Point;
  typedef pcl::PointCloud<Point> PointCloud;

  tf::TransformListener transform_listener_;
  pcl::visualization::PCLVisualizer::Ptr visualizer_;

  PointCloud::Ptr downsamplePointCloud(PointCloud::ConstPtr point_cloud, double leaf_size) {
    pcl::VoxelGrid<Point> voxel_grid;
    voxel_grid.setInputCloud(point_cloud);
    voxel_grid.setLeafSize(leaf_size, leaf_size, leaf_size);

    PointCloud::Ptr downsampled_point_cloud(new PointCloud());
    voxel_grid.filter(*downsampled_point_cloud);

    return downsampled_point_cloud;
  }

  PointCloud::Ptr transformPointCloud(PointCloud::ConstPtr point_cloud, Eigen::Matrix4f transform) {
    PointCloud::Ptr transformed_point_cloud(new PointCloud());
    pcl::transformPointCloud(*point_cloud, *transformed_point_cloud, transform);

    return transformed_point_cloud;
  }

  PointCloud::Ptr transformPointCloud(PointCloud::ConstPtr point_cloud, const std::string& target_frame, double x_adjust) {
    // 奥行きがずれているようだったので、補正します。
    PointCloud::Ptr x_adjusted_point_cloud(new PointCloud(*point_cloud));
    BOOST_FOREACH(Point& point, *x_adjusted_point_cloud) {
      point.z += x_adjust;  // 深度センサーの生データは座標系が異なります。右がX軸で下がY軸、前がZ軸です。
    }

    std_msgs::Header header = pcl_conversions::fromPCL(point_cloud->header);
    if (!transform_listener_.waitForTransform(target_frame, header.frame_id, header.stamp, ros::Duration(1.0))) {
      ROS_WARN_STREAM("Can't wait...");
      throw std::exception();
    }

    PointCloud::Ptr transformed_point_cloud(new PointCloud());
    if (!pcl_ros::transformPointCloud(target_frame, *x_adjusted_point_cloud, *transformed_point_cloud, transform_listener_)) {
      ROS_WARN("Transform failed...");
      throw std::exception();
    }

    return transformed_point_cloud;
  }

  template <typename PointT>
  class SphereCondition : public pcl::ConditionBase<PointT> {
  private:
    Eigen::Vector3f centroid_;
    double radius_;

  public:
    SphereCondition(const Eigen::Vector3f& centroid, double radius)
      : centroid_(centroid), radius_(radius)
    {
      ;
    }

    virtual bool evaluate(const PointT& point) const {
      return (point.getVector3fMap() - centroid_).norm() < radius_;
    }
  };

  PointCloud::Ptr getSpherePointCloud(PointCloud::ConstPtr point_cloud, const Eigen::Vector3f& centroid, double radius) {
    SphereCondition<Point>::Ptr condition(new SphereCondition<Point>(centroid, radius));

    pcl::ConditionalRemoval<Point> conditional_removal;
    conditional_removal.setCondition(condition);
    conditional_removal.setInputCloud(point_cloud);

    PointCloud::Ptr sphere_point_cloud(new PointCloud());
    conditional_removal.filter(*sphere_point_cloud);

    return sphere_point_cloud;
  }

  PointCloud::Ptr removeIndicesFromPointCloud(PointCloud::ConstPtr point_cloud, pcl::PointIndices::ConstPtr point_indices, bool keep_organized = false) {
    pcl::ExtractIndices<Point> extract_indices;
    extract_indices.setInputCloud(point_cloud);
    extract_indices.setIndices(point_indices);
    extract_indices.setKeepOrganized(keep_organized);
    extract_indices.setNegative(true);

    PointCloud::Ptr indices_removed_point_cloud(new PointCloud());
    extract_indices.filter(*indices_removed_point_cloud);

    return indices_removed_point_cloud;
  }

  pcl::PointIndices::Ptr getFloorPointIndices(PointCloud::ConstPtr point_cloud) {
    if (point_cloud->size() == 0) {
      ROS_WARN_STREAM("Can't search plane more...");
      throw std::exception();
    }

    pcl::SACSegmentation<Point> segmentation;
    segmentation.setOptimizeCoefficients(true);
    segmentation.setModelType(pcl::SACMODEL_PLANE);
    segmentation.setMethodType(pcl::SAC_RANSAC);
    segmentation.setDistanceThreshold(0.025);
    segmentation.setInputCloud(point_cloud);

    pcl::ModelCoefficients::Ptr floor_model_coefficients(new pcl::ModelCoefficients());
    pcl::PointIndices::Ptr floor_point_indices(new pcl::PointIndices());
    segmentation.segment(*floor_point_indices, *floor_model_coefficients);

    if (floor_point_indices->indices.size() == 0) {
      ROS_WARN_STREAM("Can't find plane...");
      throw std::exception();
    }

    if (std::abs(floor_model_coefficients->values[0]) < 0.2 && std::abs(floor_model_coefficients->values[1]) < 0.2 && floor_model_coefficients->values[2] > 0.9 && std::abs(floor_model_coefficients->values[3] < 0.2)) {
      return floor_point_indices;
    }

    ROS_WARN_STREAM("Retry finding floor... Coefficients are " << floor_model_coefficients->values[0] << ", " << floor_model_coefficients->values[1] << ", " << floor_model_coefficients->values[2] << " and " << floor_model_coefficients->values[3]);
    return getFloorPointIndices(removeIndicesFromPointCloud(point_cloud, floor_point_indices, true));
  }

  PointCloud::Ptr removeFloorFromPointCloud(PointCloud::ConstPtr point_cloud) {
    return removeIndicesFromPointCloud(point_cloud, getFloorPointIndices(point_cloud));
  }

  Eigen::Vector4f getCentroidOfPointCloud(PointCloud::ConstPtr point_cloud) {
    Eigen::Vector4f centroid;
    pcl::compute3DCentroid(*point_cloud, centroid);

    return centroid;
  }

  unsigned long strtoul(const std::string& string) {
    char* e = 0;
    return std::strtoul(string.c_str(), &e, 10);
  }

  Eigen::Matrix4f registerPointCloud(PointCloud::ConstPtr source_point_cloud, PointCloud::ConstPtr target_point_cloud, double max_correspondence_distance) {
    pcl::IterativeClosestPointNonLinear<Point, Point> icp;
    icp.setInputSource(source_point_cloud);
    icp.setInputTarget(target_point_cloud);

    icp.setMaxCorrespondenceDistance(max_correspondence_distance);

    icp.setMaximumIterations(500);
    icp.setTransformationEpsilon(1.0e-9);

    // pcl::console::VERBOSITY_LEVEL verbosity_level = pcl::console::getVerbosityLevel();  // デバッグのために、ログ・レベルを設定します。
    // pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);

    PointCloud::Ptr registered_point_cloud(new PointCloud());
    icp.align(*registered_point_cloud);

    // pcl::console::setVerbosityLevel(verbosity_level);  // ログ・レベルを元に戻します。

    if (!icp.hasConverged()) {
      ROS_WARN_STREAM("Can't register...");
      throw std::exception();
    }

    return icp.getFinalTransformation();
  }

  PointCloud::Ptr loadPointCloudFile(const std::string& file_path) {
    PointCloud::Ptr loaded_point_cloud(new PointCloud());
    pcl::io::loadPCDFile(file_path, *loaded_point_cloud);

    // ファイルにはROSヘッダーに相当する情報がないので、別途設定します。
    std_msgs::Header header;
    header.stamp = ros::Time().fromNSec(point_cloud_utilities::strtoul(file_path.substr(file_path.size() - 4 - 19, 19)));
    loaded_point_cloud->header = pcl_conversions::toPCL(header);

    return loaded_point_cloud;
  }

  std::string filePathString(PointCloud::ConstPtr point_cloud) {
    std::stringstream file_path_stringstream;
    file_path_stringstream << "/tmp/six_point_two_eight_" << std::setfill('0') << std::setw(19) << pcl_conversions::fromPCL(point_cloud->header).stamp.toNSec() << ".pcd";

    return file_path_stringstream.str();
  }

  PointCloud::Ptr savePointCloudFile(PointCloud::ConstPtr point_cloud) {
    std::string file_path_string = filePathString(point_cloud);

    pcl::io::savePCDFileBinary(file_path_string, *point_cloud);
    ROS_INFO_STREAM("PointCloud is saved to " << file_path_string);

    return PointCloud::Ptr(new PointCloud(*point_cloud));
  }

  PointCloud::Ptr removePointCloudFile(PointCloud::ConstPtr point_cloud) {
    std::remove(filePathString(point_cloud).c_str());

    return PointCloud::Ptr(new PointCloud(*point_cloud));
  }

  PointCloud::Ptr showPointCloud(PointCloud::ConstPtr point_cloud, const std::string& id, int rgb) {
    if (!visualizer_) {
      visualizer_.reset(new pcl::visualization::PCLVisualizer("Viewer"));
    }

    pcl::visualization::PointCloudColorHandlerCustom<Point> point_cloud_color(point_cloud, (rgb >> 16) & 0x00ff, (rgb >> 8) & 0x00ff, rgb & 0x00ff);
    if (!visualizer_->updatePointCloud(point_cloud, point_cloud_color, id)) {
      visualizer_->addPointCloud(point_cloud, point_cloud_color, id);
      visualizer_->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, id);
    }

    return PointCloud::Ptr(new PointCloud(*point_cloud));
  }

  void spinVisualizer() {
    visualizer_->spinOnce(100);
  }

  // 型変換。

  PointCloud::Ptr fromROSMsg(sensor_msgs::PointCloud2ConstPtr points) {
    PointCloud::Ptr converted_point_cloud(new PointCloud());
    pcl::fromROSMsg(*points, *converted_point_cloud);

    return converted_point_cloud;
  }

  sensor_msgs::PointCloud2Ptr toROSMsg(PointCloud::ConstPtr point_cloud) {
    sensor_msgs::PointCloud2Ptr converted_points(new sensor_msgs::PointCloud2());
    pcl::toROSMsg(*point_cloud, *converted_points);

    return converted_points;
  }

  Eigen::Matrix4f fromROSMsg(const geometry_msgs::Transform& transform) {
    tf::Transform tf_transform;
    tf::transformMsgToTF(transform, tf_transform);

    Eigen::Matrix4f converted_transform;
    pcl_ros::transformAsMatrix(tf_transform, converted_transform);

    return converted_transform;
  }

  geometry_msgs::Transform toROSMsg(const Eigen::Matrix4f& transform) {
    tf::Transform tf_transform;
    tf::transformEigenToTF(Eigen::Affine3d(transform.cast<double>()), tf_transform);

    geometry_msgs::Transform converted_transform;
    tf::transformTFToMsg(tf_transform, converted_transform);

    return converted_transform;
  }
}

// インターフェース。

sensor_msgs::PointCloud2Ptr six_point_two_eight::downsamplePointCloud2(sensor_msgs::PointCloud2ConstPtr points, double leaf_size) {
  return point_cloud_utilities::toROSMsg(point_cloud_utilities::downsamplePointCloud(point_cloud_utilities::fromROSMsg(points), leaf_size));
}

geometry_msgs::Point six_point_two_eight::getCentroidOfPointCloud2(sensor_msgs::PointCloud2ConstPtr points) {
  Eigen::Vector4f centroid = point_cloud_utilities::getCentroidOfPointCloud(point_cloud_utilities::fromROSMsg(points));

  geometry_msgs::Point point;
  point.x = centroid.x();
  point.y = centroid.y();
  point.z = centroid.z();

  return point;
}

sensor_msgs::PointCloud2Ptr six_point_two_eight::getSpherePointCloud2(sensor_msgs::PointCloud2ConstPtr points, const geometry_msgs::Point& centroid, double radius) {
  return point_cloud_utilities::toROSMsg(point_cloud_utilities::getSpherePointCloud(point_cloud_utilities::fromROSMsg(points), Eigen::Vector3f(centroid.x, centroid.y, centroid.z), radius));
}

sensor_msgs::PointCloud2Ptr six_point_two_eight::loadPointCloud2File(const std::string& file_path) {
  return point_cloud_utilities::toROSMsg(point_cloud_utilities::loadPointCloudFile(file_path));
}

geometry_msgs::Transform six_point_two_eight::registerPointCloud2(sensor_msgs::PointCloud2ConstPtr source_points, sensor_msgs::PointCloud2ConstPtr target_points, double max_correspondence_distance) {
  return point_cloud_utilities::toROSMsg(point_cloud_utilities::registerPointCloud(point_cloud_utilities::fromROSMsg(source_points), point_cloud_utilities::fromROSMsg(target_points), max_correspondence_distance));
}

sensor_msgs::PointCloud2Ptr six_point_two_eight::removeFloorFromPointCloud2(sensor_msgs::PointCloud2ConstPtr points) {
  return point_cloud_utilities::toROSMsg(point_cloud_utilities::removeFloorFromPointCloud(point_cloud_utilities::fromROSMsg(points)));
}

sensor_msgs::PointCloud2Ptr six_point_two_eight::removePointCloud2File(sensor_msgs::PointCloud2ConstPtr points) {
  return point_cloud_utilities::toROSMsg(point_cloud_utilities::removePointCloudFile(point_cloud_utilities::fromROSMsg(points)));
}

sensor_msgs::PointCloud2Ptr six_point_two_eight::savePointCloud2File(sensor_msgs::PointCloud2ConstPtr points) {
  return point_cloud_utilities::toROSMsg(point_cloud_utilities::savePointCloudFile(point_cloud_utilities::fromROSMsg(points)));
}

sensor_msgs::PointCloud2Ptr six_point_two_eight::showPointCloud2(sensor_msgs::PointCloud2ConstPtr points, const std::string& id, int rgb) {
  return point_cloud_utilities::toROSMsg(point_cloud_utilities::showPointCloud(point_cloud_utilities::fromROSMsg(points), id, rgb));
}

void six_point_two_eight::spinVisualizer() {
  point_cloud_utilities::spinVisualizer();
}

sensor_msgs::PointCloud2Ptr six_point_two_eight::transformPointCloud2(sensor_msgs::PointCloud2ConstPtr points, const geometry_msgs::Transform& transform) {
  return point_cloud_utilities::toROSMsg(point_cloud_utilities::transformPointCloud(point_cloud_utilities::fromROSMsg(points), point_cloud_utilities::fromROSMsg(transform)));
}

sensor_msgs::PointCloud2Ptr six_point_two_eight::transformPointCloud2(sensor_msgs::PointCloud2ConstPtr points, const std::string& target_frame, double x_adjust) {
  return point_cloud_utilities::toROSMsg(point_cloud_utilities::transformPointCloud(point_cloud_utilities::fromROSMsg(points), target_frame, x_adjust));
}

// PCL1.7.2はC++14に対応していないので、C++03で書いています。地味にキツイです。。。


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_INTERLEAVE_ODD_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_INTERLEAVE_ODD_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-swar
    Function object implementing interleave_odd capabilities

    Computes a vector from a combination of the two inputs.

    @par Semantic:

    For every parameters of types respectively T0:

    @code
    T0 r = interleave_odd(a,b);
    @endcode

    is equivalent to :

    @code
    r = [ a[1] b[1] a[3] b[3] ... a[n/2+1] b[n/2+1] ]
    @endcode

    with <tt> n = cardinal_of<T>::value </tt>

  **/
  const boost::dispatch::functor<tag::interleave_odd_> interleave_odd = {};
} }
#endif

#include <boost/simd/function/scalar/interleave_odd.hpp>
#include <boost/simd/function/simd/interleave_odd.hpp>

#endif


#include <array>
#include <iostream>
#include <random>
#include <sstream>
//
#include <perfevent/perfevent.hpp>
//
#include <boost/random/mersenne_twister.hpp>
//
#include <pxart/pxart.hpp>

using namespace std;

struct benchmark {
  explicit benchmark(size_t samples) : n{samples} {
    cout << "# iteration = " << n << "\n"
         << "# cache size = " << cache_size << " B\n"
         << "# samples = " << n * cache_size / sizeof(uint32_t) << "\n"
         << "\n";
  }

  template <typename result_type, typename RNG>
  benchmark& run(const char* name, RNG&& rng) noexcept {
    constexpr auto cache_count = cache_size / sizeof(result_type);
    array<result_type, cache_count> cache;

    for (int i = 0; i < 100; ++i)
      for (auto& x : cache) x = rng();

    {
      params.setParam("npc",  // numbers per call
                      to_string(sizeof(result_type) / sizeof(uint32_t)));
      params.setParam("#     name", name);
      PerfEventBlock e(n * cache_count, params, header);
      for (decltype(n) i = 0; i < n; ++i)
        for (auto& x : cache) x = rng();
    }

    // cout << reinterpret_cast<uint32_t&>(
    //             cache[reinterpret_cast<uint32_t&>(cache[0]) % cache_count])
    //      << "\n";
    header = false;
    return *this;
  }

  template <typename RNG>
  benchmark& run(const char* name, RNG&& rng) noexcept {
    return run<decltype(rng())>(name, forward<RNG>(rng));
  }

  template <typename result_type, typename RNG>
  benchmark& run(const char* name, RNG&& rng1, RNG&& rng2) noexcept {
    constexpr auto cache_count = cache_size / sizeof(result_type);
    array<result_type, cache_count> cache;

    for (int i = 0; i < 100; ++i) {
      for (unsigned int j = 0; j < cache_count; j += 2) {
        cache[j] = rng1();
        cache[j + 1] = rng2();
      }
    }

    {
      params.setParam("npc",  // numbers per call
                      to_string(sizeof(result_type) / sizeof(uint32_t)));
      params.setParam("#     name", name);
      PerfEventBlock e(n * cache_count, params, header);
      for (decltype(n) i = 0; i < n; ++i) {
        for (unsigned int j = 0; j < cache_count; j += 2) {
          cache[j] = rng1();
          cache[j + 1] = rng2();
        }
      }
    }

    // cout << reinterpret_cast<uint32_t&>(
    //             cache[reinterpret_cast<uint32_t&>(cache[0]) % cache_count])
    //      << "\n";
    header = false;
    return *this;
  }

  template <typename RNG>
  benchmark& run(const char* name, RNG&& rng1, RNG&& rng2) noexcept {
    return run<decltype(rng1())>(name, forward<RNG>(rng1), forward<RNG>(rng2));
  }

  benchmark& separate() noexcept {
    header = true;
    cout << "\n";
    return *this;
  }

  benchmark& heading(const char* text) noexcept {
    cout << text << "\n";
    return *this;
  }

  static constexpr int cache_size = 1 << 14;
  size_t n{};
  BenchmarkParameters params{};
  bool header = true;
};

int main(int argc, char** argv) {
  uint64_t n = 100'000;

  if (2 != argc) {
    cout << "usage:\n" << argv[0] << " <number of iterations>\n";
  } else {
    stringstream input{argv[1]};
    input >> n;
  }

  random_device rd{};

  benchmark{n}  //
      .heading("# STL MT19937")
      .run<uint32_t>("scalar", std::mt19937{rd()})
      .separate()
      .heading("# Boost MT19937")
      .run("scalar", boost::random::mt19937{rd()})
      .separate()
      .heading("# MT19937")
      .run("scalar", pxart::mt19937{pxart::mt19937::default_seeder{rd()}})
#ifdef __AVX2__
      .run("AVX", pxart::simd256::mt19937{pxart::mt19937::default_seeder{rd()}})
#endif  // __AVX2__
#ifdef __SSE4_1__
      .run("SSE", pxart::simd128::mt19937{pxart::mt19937::default_seeder{rd()}})
#endif  // __SSE4_1__
      .separate()
      .heading("# Xoroshiro128+")
      .run("scalar", pxart::xrsr128p{rd})
      .run("2 x scalar", pxart::xrsr128p{rd}, pxart::xrsr128p{rd})
#ifdef __AVX2__
      .run("AVX", pxart::simd256::xrsr128p{rd})
      .run("2 x AVX", pxart::simd256::xrsr128p{rd},
           pxart::simd256::xrsr128p{rd})
#endif  // __AVX2__
#ifdef __SSE4_1__
      .run("SSE", pxart::simd128::xrsr128p{rd})
      .run("2 x SSE", pxart::simd128::xrsr128p{rd},
           pxart::simd128::xrsr128p{rd})
#endif  // __SSE4_1__
      .separate()
      .heading("# MSWS")
      .run("scalar", pxart::msws{rd})
      .run("2 x scalar", pxart::msws{rd}, pxart::msws{rd})
#ifdef __AVX2__
      .run("AVX", pxart::simd256::msws{rd})
      .run("2 x AVX", pxart::simd256::msws{rd}, pxart::simd256::msws{rd})
#endif  // __AVX2__
#ifdef __SSE4_1__
      .run("SSE", pxart::simd128::msws{rd})
      .run("2 x SSE", pxart::simd128::msws{rd}, pxart::simd128::msws{rd})
#endif  // __SSE4_1__
      //
      ;
}


//
// Copyright (c) 2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH.
// Copyright (c) 2017, University of Bremen
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice, this
//   list of conditions and the following disclaimer.
//
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
#define BOOST_TEST_MODULE ToolsTest
#include <boost/test/unit_test.hpp>

#include <maps/tools/SurfaceIntersection.hpp>

#include <iostream>

#include <boost/test/results_collector.hpp>

inline bool current_test_passing()
{
  using namespace boost::unit_test;
  test_case::id_t id = framework::current_test_case().p_id;
  test_results rez = results_collector.results(id);
  return rez.passed();
}

BOOST_AUTO_TEST_CASE(test_surface_intersection)
{
    typedef Eigen::Vector3d Vec;
    typedef Eigen::AlignedBox3d Box;
    typedef Eigen::Hyperplane<double, 3> Plane;
    for(int i=0; i<100000; ++i)
    {
        // generate random box:
        Box box(Vec::Random());
        box.extend(Vec::Random());

        Vec inner = box.sample();
        BOOST_CHECK_SMALL(box.squaredExteriorDistance(inner), 1e-4);
        Vec normal = Vec::Random().normalized();
        BOOST_CHECK_CLOSE(normal.squaredNorm(), 1.0, 1e-6);
        Plane plane(normal, inner);

        std::vector<Vec> intersections;
        maps::tools::SurfaceIntersection::computeIntersections(plane, box, intersections);
        BOOST_CHECK_GE(intersections.size(), 3);
        BOOST_CHECK_LE(intersections.size(), 6);
        for(size_t j=0; j<intersections.size(); ++j)
        {
            const Vec &v = intersections[j];
            BOOST_CHECK_SMALL(plane.signedDistance(v), 1e-4); // v must be on the plane
            BOOST_CHECK_SMALL(box.squaredExteriorDistance(v), 1e-4); // v must not be outside the box

            // next point:
            const Vec &next = intersections[(j+1)%intersections.size()];
            BOOST_CHECK_SMALL((v-next).cwiseAbs().minCoeff(), 1e-6); // at least one coordinate must be equal to the next
        }

        // If anything failed, break and output input values:
        BOOST_REQUIRE_MESSAGE(current_test_passing(),
                "FAILED at i=" << i << " with box = [" << box.min().transpose() << " : " << box.max().transpose() << "], "
                        "plane = (" << normal.transpose() << "; " << inner.transpose() << ')');
    }
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
//
// Copyright (c) 2012-2014 Barend Gehrels, Amsterdam, the Netherlands.

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

#include <geometry_test_common.hpp>

#include <boost/algorithm/string/trim.hpp>
#include <boost/assign/list_of.hpp>

#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/algorithms/detail/get_left_turns.hpp>

#if defined(TEST_WITH_SVG)
# include <boost/geometry/io/svg/svg_mapper.hpp>
#endif

NOTE: this unit test is out of date.
get_left_turns is used by buffer and might be used in the future by solving self-tangencies in overlays.
it is currently being changed by buffer.

namespace bglt = boost::geometry::detail::left_turns;

#if defined(TEST_WITH_SVG)
template <typename Point>
inline Point further_than(Point const& p, Point const& origin, int mul, int div)
{
    typedef Point vector_type;

    vector_type v = p;
    bg::subtract_point(v, origin);

    bg::divide_value(v, div);
    bg::multiply_value(v, mul);
    Point result = origin;
    bg::add_point(result, v);
    return result;
}

inline std::string get_color(int index)
{
    switch (index)
    {
        case 0 : return "rgb(0,192,0)";
        case 1 : return "rgb(0,0,255)";
        case 2 : return "rgb(255,0,0)";
        case 3 : return "rgb(255,255,0)";
    }
    return "rgb(128,128,128)";
}
#endif

template <typename Point>
void test_one(std::string const& caseid,
                Point const& p,
                std::vector<bglt::turn_angle_info<Point> > const& angles,
                std::string const& expected_sorted_indices,
                std::string const& expected_left_indices)
{
    typedef Point vector_type;

    std::vector<bglt::angle_info<Point> > sorted;
    for (typename std::vector<bglt::turn_angle_info<Point> >::const_iterator it =
        angles.begin(); it != angles.end(); ++it)
    {
        for (int i = 0; i < 2; i++)
        {
            bglt::angle_info<Point> info(it->seg_id, i == 0, it->points[i]);
            sorted.push_back(info);
        }
    }

    // Sort on angle
    std::sort(sorted.begin(), sorted.end(), bglt::angle_less<Point>(p));

    // Block all turns on the right side of any turn
    bglt::block_turns_on_right_sides(angles, sorted);

    // Check the sorting
    {
        std::ostringstream out;
        out << std::boolalpha;
        for (typename std::vector<bglt::angle_info<Point> >::const_iterator it =
            sorted.begin(); it != sorted.end(); ++it)
        {
            out << " " << it->seg_id.segment_index
                << "-" << it->incoming;
        }
        std::string detected = boost::trim_copy(out.str());
        BOOST_CHECK_EQUAL(expected_sorted_indices, detected);
    }

    // Check outgoing lines
    std::vector<bglt::left_turn> seg_ids;
    bglt::get_left_turns(sorted, p, seg_ids);
    {
        std::ostringstream out;
        out << std::boolalpha;
        for (std::vector<bglt::left_turn>::const_iterator it =
            seg_ids.begin(); it != seg_ids.end(); ++it)
        {
            out
                << " " << it->from.segment_index
                << "->" << it->to.segment_index
                ;
        }
        std::string detected = boost::trim_copy(out.str());
        BOOST_CHECK_EQUAL(expected_left_indices, detected);
    }

#if defined(TEST_WITH_SVG)
    {
        std::ostringstream filename;
        filename << "get_left_turns_" << caseid
            << "_" << string_from_type<typename bg::coordinate_type<Point>::type>::name()
            << ".svg";

        std::ofstream svg(filename.str().c_str());

        bg::svg_mapper<Point> mapper(svg, 500, 500);
        mapper.add(p);
        for (typename std::vector<bglt::turn_angle_info<Point> >::const_iterator it =
            angles.begin(); it != angles.end(); ++it)
        {
            // Add a point further then it->to_point, just for the mapping
            for (int i = 0; i < 2; i++)
            {
                mapper.add(further_than(it->points[i], p, 12, 10));
            }
        }

        int color_index = 0;
        typedef bg::model::referring_segment<Point const> segment_type;
        for (typename std::vector<bglt::turn_angle_info<Point> >::const_iterator it =
            angles.begin(); it != angles.end(); ++it, color_index++)
        {
            for (int i = 0; i < 2; i++)
            {
                std::string style = "opacity:0.5;stroke-width:1;stroke:rgb(0,0,0);fill:" + get_color(color_index);

                bool const incoming = i == 0;
                Point const& pf = incoming ? it->points[i] : p;
                Point const& pt = incoming ? p : it->points[i];
                vector_type v = pt;
                bg::subtract_point(v, pf);

                bg::divide_value(v, 10.0);

                // Generate perpendicular vector to right-side
                vector_type perpendicular;
                bg::set<0>(perpendicular, bg::get<1>(v));
                bg::set<1>(perpendicular, -bg::get<0>(v));

                bg::model::ring<Point> ring;
                ring.push_back(pf);
                ring.push_back(pt);

                // Extra point at 9/10
                {
                    Point pe = pt;
                    bg::add_point(pe, perpendicular);
                    ring.push_back(pe);
                }
                {
                    Point pe = pf;
                    bg::add_point(pe, perpendicular);
                    ring.push_back(pe);
                }
                ring.push_back(pf);

                mapper.map(ring, style);

                segment_type s(pf, pt);
                mapper.map(s, "opacity:0.9;stroke-width:4;stroke:rgb(0,0,0);");
            }
        }

        // Output angles for left-turns
        for (std::vector<bglt::left_turn>::const_iterator ltit =
            seg_ids.begin(); ltit != seg_ids.end(); ++ltit)
        {
            for (typename std::vector<bglt::angle_info<Point> >::const_iterator sit =
                sorted.begin(); sit != sorted.end(); ++sit, color_index++)
            {
                Point pf, pt;
                int factor = 0;
                if (sit->seg_id == ltit->from && sit->incoming)
                {
                    pf = sit->point;
                    pt = p;
                    factor = -1; // left side
                }
                else if (sit->seg_id == ltit->to && ! sit->incoming)
                {
                    pf = p;
                    pt = sit->point;
                    factor = -1; // left side
                }
                if (factor != 0)
                {
                    vector_type v = pt;
                    bg::subtract_point(v, pf);
                    bg::divide_value(v, 10.0);

                    // Generate perpendicular vector to right-side
                    vector_type perpendicular;
                    bg::set<0>(perpendicular, factor * bg::get<1>(v));
                    bg::set<1>(perpendicular, -factor * bg::get<0>(v));

                    bg::add_point(pf, v);
                    bg::subtract_point(pt, v);

                    bg::add_point(pf, perpendicular);
                    bg::add_point(pt, perpendicular);

                    segment_type s(pf, pt);
                    mapper.map(s, "opacity:0.9;stroke-width:4;stroke:rgb(255,0,0);");
                }
            }
        }

        // Output texts with info about sorted/blocked
        int index = 0;
        for (typename std::vector<bglt::angle_info<Point> >::const_iterator it =
            sorted.begin(); it != sorted.end(); ++it, ++index)
        {
            std::ostringstream out;
            out << std::boolalpha;
            out << " seg:" << it->seg_id.segment_index
                << " " << (it->incoming ? "in" : "out")
                << " idx:" << index
                << (it->blocked ? " blocked" : "")
                ;
            mapper.text(further_than(it->point, p, 11, 10), out.str(), "fill:rgb(0,0,0);font-family='Verdana'");
        }

        mapper.map(p, "fill:rgb(255,0,0)");

    }

#endif

}

template <typename P>
void test_all()
{
    using bglt::turn_angle_info;

    test_one<P>("cross",
        bg::make<P>(50, 50),   // ip
        boost::assign::list_of
            (turn_angle_info<P>(bg::segment_identifier(0, -1, -1, 1), bg::make<P>(100, 100), bg::make<P>(0, 0)))
            (turn_angle_info<P>(bg::segment_identifier(0, -1, -1, 2), bg::make<P>(100, 0), bg::make<P>(0, 100)))
            , "1-true 2-true 1-false 2-false"
            , "2->1"
        );

    test_one<P>("occupied",
        bg::make<P>(50, 50),   // ip
        boost::assign::list_of
            (turn_angle_info<P>(bg::segment_identifier(0, -1, -1, 1), bg::make<P>(100, 100), bg::make<P>(0, 0)))
            (turn_angle_info<P>(bg::segment_identifier(0, -1, -1, 2), bg::make<P>(100, 0), bg::make<P>(0, 100)))
            (turn_angle_info<P>(bg::segment_identifier(0, -1, -1, 3), bg::make<P>(0, 30), bg::make<P>(100, 70)))
            , "1-true 3-false 2-true 1-false 3-true 2-false"
            , ""
        );

    test_one<P>("uu",
        bg::make<P>(50, 50),   // ip
        boost::assign::list_of
            (turn_angle_info<P>(bg::segment_identifier(0, -1, -1, 1), bg::make<P>(0, 0), bg::make<P>(100, 0)))
            (turn_angle_info<P>(bg::segment_identifier(0, -1, -1, 2), bg::make<P>(100, 100), bg::make<P>(0, 100)))
            , "2-true 1-false 1-true 2-false"
            , "2->1 1->2"
        );

    test_one<P>("uu2",
        bg::make<P>(50, 50),   // ip
        boost::assign::list_of
            (turn_angle_info<P>(bg::segment_identifier(0, -1, -1, 1), bg::make<P>(0, 0), bg::make<P>(100, 0)))
            (turn_angle_info<P>(bg::segment_identifier(0, -1, -1, 2), bg::make<P>(100, 100), bg::make<P>(0, 100)))
            (turn_angle_info<P>(bg::segment_identifier(0, -1, -1, 3), bg::make<P>(0, 50), bg::make<P>(100, 50)))
            , "2-true 3-false 1-false 1-true 3-true 2-false"
            , "2->3 3->2"
        );

    test_one<P>("uu3",
        bg::make<P>(50, 50),   // ip
        boost::assign::list_of
            (turn_angle_info<P>(bg::segment_identifier(0, -1, -1, 1), bg::make<P>(0, 0), bg::make<P>(100, 0)))
            (turn_angle_info<P>(bg::segment_identifier(0, -1, -1, 2), bg::make<P>(100, 100), bg::make<P>(0, 100)))
            (turn_angle_info<P>(bg::segment_identifier(0, -1, -1, 3), bg::make<P>(50, 0), bg::make<P>(50, 100)))
            , "3-false 2-true 1-false 3-true 1-true 2-false"
            , "1->2"
        );

    test_one<P>("longer",
        bg::make<P>(50, 50),   // ip
        boost::assign::list_of
            (turn_angle_info<P>(bg::segment_identifier(0, -1, -1, 1), bg::make<P>(100, 100), bg::make<P>(0, 0)))
            (turn_angle_info<P>(bg::segment_identifier(0, -1, -1, 2), bg::make<P>(100, 0), bg::make<P>(0, 100)))
            (turn_angle_info<P>(bg::segment_identifier(0, -1, -1, 3), bg::make<P>(90, 10), bg::make<P>(10, 10)))
            , "1-true 2-true 3-true 1-false 3-false 2-false"
            , "3->1"
        );
}

int test_main( int , char* [] )
{
    test_all<bg::model::d2::point_xy<int> >();

    return 0;
}


#ifndef DIST_TABLE_WRAPPER_H
#define DIST_TABLE_WRAPPER_H

#include <vector>
#include <utility>
#include <boost/assert.hpp>
#include <cstddef>

namespace osrm
{
namespace util
{

// This Wrapper provides an easier access to a distance table that is given as an linear vector

template <typename T> class DistTableWrapper
{
  public:
    using Iterator = typename std::vector<T>::iterator;
    using ConstIterator = typename std::vector<T>::const_iterator;

    DistTableWrapper(std::vector<T> table, std::size_t number_of_nodes)
        : table_(std::move(table)), number_of_nodes_(number_of_nodes)
    {
        BOOST_ASSERT_MSG(table.size() == 0, "table is empty");
        BOOST_ASSERT_MSG(number_of_nodes_ * number_of_nodes_ <= table_.size(),
                         "number_of_nodes_ is invalid");
    };

    std::size_t GetNumberOfNodes() const { return number_of_nodes_; }

    std::size_t size() const { return table_.size(); }

    EdgeWeight operator()(NodeID from, NodeID to) const
    {
        BOOST_ASSERT_MSG(from < number_of_nodes_, "from ID is out of bound");
        BOOST_ASSERT_MSG(to < number_of_nodes_, "to ID is out of bound");

        const auto index = from * number_of_nodes_ + to;

        BOOST_ASSERT_MSG(index < table_.size(), "index is out of bound");

        return table_[index];
    }

    ConstIterator begin() const { return std::begin(table_); }

    Iterator begin() { return std::begin(table_); }

    ConstIterator end() const { return std::end(table_); }

    Iterator end() { return std::end(table_); }

    NodeID GetIndexOfMaxValue() const
    {
        return std::distance(table_.begin(), std::max_element(table_.begin(), table_.end()));
    }

    std::vector<T> GetTable() const { return table_; }

  private:
    std::vector<T> table_;
    const std::size_t number_of_nodes_;
};
}
}

#endif // DIST_TABLE_WRAPPER_H


/////////////////////////////////////////////////////////////////////////////////////////////
// Copyright (c) 2021 Andreas Milton Maniotis.
//
// Email: andreas.maniotis@gmail.com
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
/////////////////////////////////////////////////////////////////////////////////////////////

#include "aml/structure/conslist_structure.hpp"
#include "aml/structure/select_indices_structure.hpp"
#include "aml/structure/term_structure.hpp"

#include "aml/exponent.hpp"
#include "aml/string.hpp"

#include "aml/linker.hpp"

#include <iostream>
#include <type_traits>

#include <boost/core/demangle.hpp>

using aml::operator""_;

namespace test::linker
{
    template<typename... X>
    using array = typename
        aml::signatures
        <
        aml::subtype<decltype("list"_), aml::structure::list>,

        aml::subtype<decltype("random_access"_), aml::structure::array>

        //                    aml::subtype<decltype("term"_), aml::structure::term>

        >::template link_with<X...>;

    void t0()
    {
        using t = array<int, double, void*>;

        //        static_assert(std::is_same<t::at<2>, void*>::value, "");
        std::cout << boost::core::demangle(typeid( t::tail ).name() ) << std::endl;

        //        static_assert(std::is_same< t::tail::at<1>, void*>::value, "");
    }

}

int main()
{
    void (*test_set[])() = { test::linker::t0 };

    for ( auto test : test_set )
        test();

    std::cout << __FILE__ << ": " << sizeof(test_set)/sizeof(test_set[0])  << " tests passed." << std::endl;

}


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

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

//[example_code
#define BOOST_TEST_MODULE runtime_configuration4

#include <boost/test/included/unit_test.hpp>
#include <boost/test/data/test_case.hpp>

#include <iostream>
#include <functional>
#include <sstream>
#include <fstream>

// this dataset loads a file that contains a list of strings
// this list is used to create a dataset test case.
class file_dataset
{
private:
    std::string m_filename;
    std::size_t m_line_start;
    std::size_t m_line_end;

public:
    enum { arity = 2 };

public:
    file_dataset(std::size_t line_start = 0, std::size_t line_end = std::size_t(-1))
    : m_line_start(line_start)
    , m_line_end(line_end)
    {
      int argc = boost::unit_test::framework::master_test_suite().argc;
      char** argv = boost::unit_test::framework::master_test_suite().argv;

      if(argc != 3)
        throw std::logic_error("Incorrect number of arguments");
      if(std::string(argv[1]) != "--test-file")
        throw std::logic_error("First argument != '--test-file'");
      if(!(m_line_start < std::size_t(-1)))
        throw std::logic_error("Incorrect line start/end");

      m_filename = argv[2];

      std::ifstream file(m_filename);
      if(!file.is_open())
        throw std::logic_error("Cannot open the file '" + m_filename + "'");
      std::size_t nb_lines = std::count_if(
        std::istreambuf_iterator<char>(file),
        std::istreambuf_iterator<char>(),
        [](char c){ return c == '\n';});

      m_line_end = (std::min)(nb_lines, m_line_end);
      if(!(m_line_start <= m_line_end))
        throw std::logic_error("Incorrect line start/end");
    }

    struct iterator {
        iterator(std::string const& filename, std::size_t line_start)
        : file(filename, std::ios::binary) {
          if(!file.is_open())
            throw std::runtime_error("Cannot open the file");
          for(std::size_t i = 0; i < line_start; i++) {
            getline(file, m_current_line);
          }
        }

        auto operator*() const -> std::tuple<float, float> {
          float a, b;
          std::istringstream istr(m_current_line);
          istr >> a >> b;
          return std::tuple<float, float>(a, b);
        }

        void operator++() {
          getline(file, m_current_line);
        }
    private:
        std::ifstream file;
        std::string m_current_line;
    };

    // size of the DS
    boost::unit_test::data::size_t size() const {
      return m_line_end - m_line_start;
    }

    // iterator over the lines of the file
    iterator begin() const   {
      return iterator(m_filename, m_line_start);
    }
};

namespace boost { namespace unit_test { namespace data {

namespace monomorphic {
  template <>
  struct is_dataset<file_dataset> : boost::mpl::true_ {};
}
}}}

BOOST_DATA_TEST_CASE(command_line_test_file,
    boost::unit_test::data::make_delayed<file_dataset>( 3, 10 ),
    input, expected) {
    BOOST_TEST(input <= expected);
}
//]