//
// boost/radix/static_obitstream_msb.hpp
//
// Copyright (c) Chris Glover, 2017-2018
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_RADIX_STATICOBITSTREAMMSB_HPP
#define BOOST_RADIX_STATICOBITSTREAMMSB_HPP

#include <boost/radix/common.hpp>

#include <boost/radix/bitmask.hpp>
#include <boost/radix/detail/bits.hpp>
#include <boost/type_traits/integral_constant.hpp>

#include <boost/cstdint.hpp>

namespace boost { namespace radix {
namespace detail {

template <typename UnpackedSegment, typename OutputIterator>
OutputIterator pack(
    UnpackedSegment const& unpacked,
    OutputIterator out,
    boost::integral_constant<std::size_t, 1>) {
  // [1, 1, 1, 1]
  bits_type packed_bits = 0;
  for(int i = 0, j = 7; i < 8; ++i, --j) {
    packed_bits |= unpacked[i] << j;
  }

  *out++ = packed_bits;
  return out;
}

template <typename UnpackedSegment, typename OutputIterator>
OutputIterator pack(
    UnpackedSegment const& unpacked,
    OutputIterator out,
    boost::integral_constant<std::size_t, 2>) {
  // [2, 2, 2, 2]
  bits_type packed_bits = 0;
  packed_bits |= unpacked[0] << 6;
  packed_bits |= unpacked[1] << 4;
  packed_bits |= unpacked[2] << 2;
  packed_bits |= unpacked[3] << 0;
  *out++ = packed_bits;
  return out;
};

template <typename UnpackedSegment, typename OutputIterator>
OutputIterator pack(
    UnpackedSegment const& unpacked,
    OutputIterator out,
    boost::integral_constant<std::size_t, 3>) {
  // [3, 3, 2], [1, 3, 3, 1], [2, 3, 3]
  *out++ = (unpacked[0] << 5) | (unpacked[1] << 2) | (unpacked[2] >> 1);
  *out++ = (unpacked[2] << 7) | (unpacked[3] << 4) | (unpacked[4] << 1) |
           (unpacked[5] >> 2);
  *out++ = (unpacked[5] << 6) | (unpacked[6] << 3) | (unpacked[7] >> 0);
  return out;
}

template <typename UnpackedSegment, typename OutputIterator>
OutputIterator pack(
    UnpackedSegment const& unpacked,
    OutputIterator out,
    boost::integral_constant<std::size_t, 4>) {
  // [4, 4]
  *out++ = (unpacked[0] << 4) | (unpacked[1] >> 0);
  return out;
}

template <typename UnpackedSegment, typename OutputIterator>
OutputIterator pack(
    UnpackedSegment const& unpacked,
    OutputIterator out,
    boost::integral_constant<std::size_t, 5>) {
  // [5, 3], [2, 5, 1], [4, 4], [1, 5, 2], [3, 5]
  *out++ = (unpacked[0] << 3) | (unpacked[1] >> 2);
  *out++ = (unpacked[1] << 6) | (unpacked[2] << 1) | (unpacked[3] >> 4);
  *out++ = (unpacked[3] << 4) | (unpacked[4] >> 1);
  *out++ = (unpacked[4] << 7) | (unpacked[5] << 2) | (unpacked[6] >> 3);
  *out++ = (unpacked[6] << 5) | (unpacked[7] >> 0);
  return out;
}

template <typename UnpackedSegment, typename OutputIterator>
OutputIterator pack(
    UnpackedSegment const& unpacked,
    OutputIterator out,
    boost::integral_constant<std::size_t, 6>) {
  // [6, 2], [4, 4], [2, 6]
  *out++ = (unpacked[0] << 2) | (unpacked[1] >> 4);
  *out++ = (unpacked[1] << 4) | (unpacked[2] >> 2);
  *out++ = (unpacked[2] << 6) | (unpacked[3] >> 0);
  return out;
}

template <typename UnpackedSegment, typename OutputIterator>
OutputIterator pack(
    UnpackedSegment const& unpacked,
    OutputIterator out,
    boost::integral_constant<std::size_t, 7>) {
  // [7, 1], [6, 2], [5, 3], [4, 4], [3, 5], [2, 6], [1, 7]
  *out++ = (unpacked[0] << 1) | (unpacked[1] >> 6);
  *out++ = (unpacked[1] << 2) | (unpacked[2] >> 5);
  *out++ = (unpacked[2] << 3) | (unpacked[3] >> 4);
  *out++ = (unpacked[3] << 4) | (unpacked[4] >> 3);
  *out++ = (unpacked[4] << 5) | (unpacked[5] >> 2);
  *out++ = (unpacked[5] << 6) | (unpacked[6] >> 1);
  *out++ = (unpacked[6] << 7) | (unpacked[7] >> 0);
  return out;
}

template <typename UnpackedSegment, typename OutputIterator, std::size_t Bits>
OutputIterator pack_basic(
    UnpackedSegment const& unpacked,
    OutputIterator out,
    boost::integral_constant<std::size_t, Bits>) {
  using namespace bits;
  std::size_t const unpacked_size = to_unpacked_segment_size<Bits>::value;
  std::size_t const packed_size   = to_packed_segment_size<Bits>::value;

  BOOST_STATIC_ASSERT(packed_size <= sizeof(boost::uint64_t));

  boost::uint64_t packed = 0;
  for(std::size_t i = 0; i < unpacked_size; ++i) {
    packed <<= Bits;
    packed |= static_cast<bits_type>(unpacked[i] & mask<Bits>::value);
  }

  for(int i = static_cast<int>(packed_size) - 1; i >= 0; --i) {
    *out++ = static_cast<bits_type>(packed >> (i * 8));
  }

  return out;
}

} // namespace detail

template <std::size_t Bits>
class static_obitstream_msb {
 public:
  template <typename UnpackedSegment, typename OutputIterator>
  OutputIterator operator()(
      UnpackedSegment const& unpacked, OutputIterator out) const {
    return detail::pack(
        unpacked, out, boost::integral_constant<std::size_t, Bits>());
  }
};

}} // namespace boost::radix

#endif // BOOST_RADIX_STATICOBITSTREAMMSB_HPP


/******************************************************************************
 * Copyright (C) 2014 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 ******************************************************************************/

/** \file calibrator.cpp
    \brief This file estimates the odometry calibration from realworld data.
  */

#include <cmath>

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

#include <Eigen/Core>

#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <rosbag/message_instance.h>

#include <sm/kinematics/Transformation.hpp>
#include <sm/kinematics/rotations.hpp>
#include <sm/kinematics/EulerAnglesYawPitchRoll.hpp>
#include <sm/kinematics/quaternion_algebra.hpp>

#include <sm/BoostPropertyTree.hpp>

#include <sm/timing/TimestampCorrector.hpp>
#include <sm/timing/NsecTimeUtilities.hpp>

#include <poslv/VehicleNavigationSolutionMsg.h>
#include <poslv/VehicleNavigationPerformanceMsg.h>
#include <poslv/TimeTaggedDMIDataMsg.h>

#include <can_prius/FrontWheelsSpeedMsg.h>
#include <can_prius/RearWheelsSpeedMsg.h>
#include <can_prius/Steering1Msg.h>

#include "aslam/calibration/car/algo/CarCalibrator.h"
#include "aslam/calibration/car/algo/splinesToFile.h"
#include "aslam/calibration/car/data/WheelSpeedsMeasurement.h"
#include "aslam/calibration/car/data/SteeringMeasurement.h"
#include "aslam/calibration/car/data/DMIMeasurement.h"
#include "aslam/calibration/car/data/PoseMeasurement.h"
#include "aslam/calibration/car/data/VelocitiesMeasurement.h"
#include "aslam/calibration/car/geo/geodetic.h"

using namespace sm;
using namespace sm::timing;
using namespace sm::kinematics;
using namespace aslam::calibration;

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

  if (argc != 3) {
    std::cerr << "Usage: " << argv[0] << " <bag_file> <conf_file>" << std::endl;
    return -1;
  }

  BoostPropertyTree config;
  config.loadXml(argv[2]);

  const bool useDMI =
    config.getBool("car/calibrator/odometry/sensors/dmi/active");
  const bool useFw =
    config.getBool("car/calibrator/odometry/sensors/fws/active");
  const bool useRw =
    config.getBool("car/calibrator/odometry/sensors/rws/active");
  const bool useSt =
    config.getBool("car/calibrator/odometry/sensors/st/active");

  CarCalibrator calibrator(PropertyTree(config, "car/calibrator"));

  std::ofstream rwDataFile("rwData.txt");
  rwDataFile << std::fixed << std::setprecision(18);
  std::ofstream fwDataFile("fwData.txt");
  fwDataFile << std::fixed << std::setprecision(18);
  std::ofstream stDataFile("stData.txt");
  stDataFile << std::fixed << std::setprecision(18);
  std::ofstream dmiDataFile("dmiData.txt");
  dmiDataFile << std::fixed << std::setprecision(18);
  std::ofstream poseDataFile("poseData.txt");
  poseDataFile << std::fixed << std::setprecision(18);
  std::ofstream velDataFile("velData.txt");
  velDataFile << std::fixed << std::setprecision(18);

  rosbag::Bag bag(argv[1]);
  std::vector<std::string> topics;
  topics.push_back(config.getString(
    "car/calibrator/odometry/sensors/fws/topic"));
  topics.push_back(config.getString(
    "car/calibrator/odometry/sensors/rws/topic"));
  topics.push_back(config.getString(
    "car/calibrator/odometry/sensors/st/topic"));
  topics.push_back(config.getString(
    "car/calibrator/odometry/sensors/dmi/topic"));
  topics.push_back(config.getString(
    "car/calibrator/applanix/vns/topic"));
  topics.push_back(config.getString(
    "car/calibrator/applanix/vnp/topic"));
  rosbag::View view(bag, rosbag::TopicQuery(topics));
  TimestampCorrector<double> timestampCorrectorVns;
  TimestampCorrector<double> timestampCorrectorDmi;
  TimestampCorrector<double> timestampCorrectorFw;
  TimestampCorrector<double> timestampCorrectorRw;
  TimestampCorrector<double> timestampCorrectorSt;
  double lastDMITimestamp = -1;
  double lastDMIDistance = -1;
  bool firstVns = true;
  poslv::VehicleNavigationPerformanceMsgConstPtr lastVnp;
  Transformation m_T_e;
  const EulerAnglesYawPitchRoll ypr;
  Transformation m_T_r_0;
  for (auto it = view.begin(); it != view.end(); ++it) {
    if (it->getTopic() == config.getString(
        "car/calibrator/applanix/vnp/topic")) {
      poslv::VehicleNavigationPerformanceMsgConstPtr vnp(
        it->instantiate<poslv::VehicleNavigationPerformanceMsg>());
      lastVnp = vnp;
    }
    if (it->getTopic() == config.getString(
        "car/calibrator/applanix/vns/topic")) {
      if (!lastVnp)
        continue;
      poslv::VehicleNavigationSolutionMsgConstPtr vns(
        it->instantiate<poslv::VehicleNavigationSolutionMsg>());
      double x_ecef, y_ecef, z_ecef;
      wgs84ToEcef(deg2rad(vns->latitude), deg2rad(vns->longitude),
        vns->altitude, x_ecef, y_ecef, z_ecef);
      if (firstVns) {
        m_T_e = ecef2enu(x_ecef, y_ecef, z_ecef, deg2rad(vns->latitude),
          deg2rad(vns->longitude));
      }
      PoseMeasurement pose;
      pose.m_r_mr = m_T_e * Eigen::Vector3d(x_ecef, y_ecef, z_ecef);
      const Eigen::Matrix3d l_ned_R_r = ypr.parametersToRotationMatrix(
        Eigen::Vector3d(deg2rad(vns->heading), deg2rad(vns->pitch),
        deg2rad(vns->roll)));
      const Transformation e_T_l_ned = ned2ecef(x_ecef, y_ecef, z_ecef,
        deg2rad(vns->latitude), deg2rad(vns->longitude));
      pose.m_R_r = ypr.rotationMatrixToParameters(m_T_e.C() * e_T_l_ned.C() *
        l_ned_R_r);
      pose.sigma2_m_r_mr = Eigen::Vector3d(lastVnp->northPositionRMSError *
        lastVnp->northPositionRMSError, lastVnp->eastPositionRMSError *
        lastVnp->eastPositionRMSError, lastVnp->downPositionRMSError *
        lastVnp->downPositionRMSError).asDiagonal();
      pose.sigma2_m_R_r = Eigen::Vector3d(deg2rad(lastVnp->headingRMSError) *
        deg2rad(lastVnp->headingRMSError), deg2rad(lastVnp->pitchRMSError) *
        deg2rad(lastVnp->pitchRMSError), deg2rad(lastVnp->rollRMSError) *
        deg2rad(lastVnp->rollRMSError)).asDiagonal();
      poseDataFile << vns->header.stamp.toSec() << " " <<
        pose.m_r_mr.transpose() << " " << pose.m_R_r.transpose() <<
        " " << pose.sigma2_m_r_mr.diagonal().transpose() << " " <<
        pose.sigma2_m_R_r.diagonal().transpose() << std::endl;
      if (firstVns) {
        m_T_r_0 = Transformation(
          r2quat(ypr.parametersToRotationMatrix(pose.m_R_r)), pose.m_r_mr);
        firstVns = false;
      }
      VelocitiesMeasurement vel;
      vel.r_v_mr = l_ned_R_r.transpose() * Eigen::Vector3d(vns->northVelocity,
        vns->eastVelocity, vns->downVelocity);
      vel.sigma2_r_v_mr = Eigen::Vector3d(lastVnp->northVelocityRMSError *
        lastVnp->northVelocityRMSError, lastVnp->eastVelocityRMSError *
        lastVnp->eastVelocityRMSError, lastVnp->downVelocityRMSError *
        lastVnp->downVelocityRMSError).asDiagonal();
      vel.r_om_mr = Eigen::Vector3d(deg2rad(vns->angularRateLong),
        deg2rad(vns->angularRateTrans), deg2rad(vns->angularRateDown));
      vel.sigma2_r_om_mr = Eigen::Vector3d(deg2rad(lastVnp->rollRMSError) *
        deg2rad(lastVnp->rollRMSError), deg2rad(lastVnp->pitchRMSError) *
        deg2rad(lastVnp->pitchRMSError), deg2rad(lastVnp->headingRMSError) *
        deg2rad(lastVnp->headingRMSError)).asDiagonal();
      velDataFile << vns->header.stamp.toSec() << " " <<
        vel.r_v_mr.transpose() << " " << vel.r_om_mr.transpose() <<
        " " << vel.sigma2_r_v_mr.diagonal().transpose() << " " <<
        vel.sigma2_r_om_mr.diagonal().transpose() << std::endl;
      auto timestamp = std::round(timestampCorrectorVns.correctTimestamp(
        secToNsec(vns->timeDistance.time1), vns->header.stamp.toNSec()));
      calibrator.addPoseMeasurement(pose, timestamp);
      calibrator.addVelocitiesMeasurement(vel, timestamp);
    }
    if (it->getTopic() == config.getString(
        "car/calibrator/odometry/sensors/fws/topic") && useFw) {
      can_prius::FrontWheelsSpeedMsgConstPtr fws(
        it->instantiate<can_prius::FrontWheelsSpeedMsg>());
      WheelSpeedsMeasurement data;
      data.left = fws->Left;
      data.right = fws->Right;
      auto timestamp = std::round(timestampCorrectorFw.correctTimestamp(
        fws->header.seq, fws->header.stamp.toNSec()));
      calibrator.addFrontWheelsMeasurement(data, timestamp);
      fwDataFile << fws->header.stamp.toSec() << " " << data.left << " "
        << data.right << std::endl;
    }
    if (it->getTopic() == config.getString(
        "car/calibrator/odometry/sensors/rws/topic") && useRw) {
      can_prius::RearWheelsSpeedMsgConstPtr rws(
        it->instantiate<can_prius::RearWheelsSpeedMsg>());
      WheelSpeedsMeasurement data;
      data.left = rws->Left;
      data.right = rws->Right;
      auto timestamp = std::round(timestampCorrectorRw.correctTimestamp(
        rws->header.seq, rws->header.stamp.toNSec()));
      calibrator.addRearWheelsMeasurement(data, timestamp);
      rwDataFile << rws->header.stamp.toSec() << " " << data.left << " "
        << data.right << std::endl;
    }
    if (it->getTopic() == config.getString(
        "car/calibrator/odometry/sensors/st/topic") && useSt) {
      can_prius::Steering1MsgConstPtr st(
        it->instantiate<can_prius::Steering1Msg>());
      SteeringMeasurement data;
      data.value = st->value;
      auto timestamp = std::round(timestampCorrectorSt.correctTimestamp(
        st->header.seq, st->header.stamp.toNSec()));
      calibrator.addSteeringMeasurement(data, timestamp);
      stDataFile << st->header.stamp.toSec() << " " << data.value << std::endl;
    }
    if (it->getTopic() == config.getString(
        "car/calibrator/odometry/sensors/dmi/topic") && useDMI) {
      poslv::TimeTaggedDMIDataMsgConstPtr dmi(
        it->instantiate<poslv::TimeTaggedDMIDataMsg>());
      if (lastDMITimestamp != -1) {
        DMIMeasurement data;
        data.wheelSpeed = (dmi->signedDistanceTraveled - lastDMIDistance) /
          (dmi->timeDistance.time1 - lastDMITimestamp);
        calibrator.addDMIMeasurement(data,
          std::round(timestampCorrectorDmi.correctTimestamp(
          secToNsec(dmi->timeDistance.time1), dmi->header.stamp.toNSec())));
        dmiDataFile << dmi->header.stamp.toSec() << " " << data.wheelSpeed
          << std::endl;
      }
      lastDMITimestamp = dmi->timeDistance.time1;
      lastDMIDistance = dmi->signedDistanceTraveled;
    }
  }

  if (calibrator.unprocessedMeasurements())
    calibrator.addMeasurements();

  std::ofstream devFile("deviations.txt");
  devFile << std::fixed << std::setprecision(18);
  Eigen::VectorXd variances = calibrator.getOdometryVariablesVariance();
  devFile << "e_r: " << std::sqrt(variances(0)) << std::endl;
  devFile << "e_f: " << std::sqrt(variances(1)) << std::endl;
  devFile << "L: " << std::sqrt(variances(2)) << std::endl;
  devFile << "a0: " << std::sqrt(variances(3)) << std::endl;
  devFile << "a1: " << std::sqrt(variances(4)) << std::endl;
  devFile << "a2: " << std::sqrt(variances(5)) << std::endl;
  devFile << "a3: " << std::sqrt(variances(6)) << std::endl;
  devFile << "k_rl: " << std::sqrt(variances(7)) << std::endl;
  devFile << "k_rr: " << std::sqrt(variances(8)) << std::endl;
  devFile << "k_fl: " << std::sqrt(variances(9)) << std::endl;
  devFile << "k_fr: " << std::sqrt(variances(10)) << std::endl;
  devFile << "k_dmi: " << std::sqrt(variances(11)) << std::endl;
  devFile << "v_r_vr_1: " << std::sqrt(variances(12)) << std::endl;
  devFile << "v_r_vr_2: " << std::sqrt(variances(13)) << std::endl;
  devFile << "v_r_vr_3: " << std::sqrt(variances(14)) << std::endl;
  devFile << "v_R_r_1: " << std::sqrt(variances(15)) << std::endl;
  devFile << "v_R_r_2: " << std::sqrt(variances(16)) << std::endl;
  devFile << "v_R_r_3: " << std::sqrt(variances(17)) << std::endl;

  std::ofstream m_T_v_estFile("m_T_v_est.txt");
  m_T_v_estFile << std::fixed << std::setprecision(18);
  writeSplines(calibrator.getEstimator(), 0.01, m_T_v_estFile);

  std::ofstream infoGainHistFile("infoGainHist.txt");
  auto infoGainHist = calibrator.getInformationGainHistory();
  infoGainHistFile << std::fixed << std::setprecision(18);
  std::for_each(infoGainHist.cbegin(), infoGainHist.cend(), [&](decltype(
    *infoGainHist.cbegin()) x) {infoGainHistFile << x << std::endl;});

  std::ofstream calibHistFile("calibHist.txt");
  auto calibHist = calibrator.getOdometryVariablesHistory();
  calibHistFile << std::fixed << std::setprecision(18);
  std::for_each(calibHist.cbegin(), calibHist.cend(), [&](decltype(
    *calibHist.cbegin()) x) {calibHistFile << x.transpose() << std::endl;});

  return 0;
}


/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
First author: Francesco Banterle

This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

*/

#ifndef PIC_UTIL_POLYLINE_HPP
#define PIC_UTIL_POLYLINE_HPP

#include <vector>

#include "../base.hpp"

#include "../util/vec.hpp"

#ifndef PIC_DISABLE_EIGEN
#ifndef PIC_EIGEN_NOT_BUNDLED
    #include "../externals/Eigen/Dense"
#else
    #include <Eigen/Dense>
#endif
#endif

namespace pic {

/**
 * @brief The Polyline class
 */
template<unsigned int N, class T>
class Polyline
{
public:

    std::vector< Vec<N, T> > points;

    Polyline()
    {
        points.clear();
    }

    Polyline(std::vector< Vec<N, T> > &points)
    {
        this->points.assign(points.begin(), points.end());
    }

    /**
     * @brief add
     * @param point
     */
    void add(Vec<N, T> &point)
    {
        points.push_back(point);
    }

    /**
     * @brief simplify
     * @param target_n_points
     */
    void simplify(int target_n_points)
    {
        int n = int(points.size());

        if(n <= target_n_points) {
            return;
        }

        while(n > target_n_points) {
            float area_min = FLT_MAX;
            int index = -1;
            float a, b, c, area_tmp;
            for(int i = 0; i < (n - 2); i++) {

                auto p0 = points.at(i    );
                auto p1 = points.at(i + 1);
                auto p2 = points.at(i + 2);

                a = sqrtf(float(p0.distanceSq(p1)));
                b = sqrtf(float(p1.distanceSq(p2)));
                c = sqrtf(float(p2.distanceSq(p0)));

                float s = (a + b + c) / 2.0f ;

                float area_sq = s * (s - a) * (s - b) * (s - c);
                area_tmp = sqrtf(area_sq);

                if(area_tmp < area_min) {
                    area_min = area_tmp;
                    index = i;
                }
            }

            if(index > -1) {
                points.erase(points.begin() + index + 1);
            } else {

                break;
            }

            n = int(points.size());
        }
    }
};

/**
 * @brief Polyline2i
 */
typedef Polyline<2, int> Polyline2i;

#ifndef PIC_DISABLE_EIGEN
/**
 * @brief convertFromEigenToPolyLine
 * @param in
 * @param out
 */
PIC_INLINE void convertFromEigenToPolyLine(std::vector< Eigen::Vector2i > &in, Polyline2i &out)
{
    for(unsigned int i = 0; i < in.size(); i++) {
        auto tmp = Vec2i(in[i][0], in[i][1]);
        out.add(tmp);
    }
}
#endif

} // end namespace pic

#endif //PIC_UTIL_POLYLINE_HPP


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_TOINTS_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_TOINTS_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

namespace boost { namespace simd { namespace tag
  {
    /*!
      @brief  toints generic tag

      Represents the toints function in generic contexts.

      @par Models:
      Hierarchy
    **/
    struct toints_ : ext::elementwise_<toints_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<toints_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_toints_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
    template<class Site>
    BOOST_FORCEINLINE generic_dispatcher<tag::toints_, Site> dispatching_toints_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
    {
      return generic_dispatcher<tag::toints_, Site>();
    }
    template<class... Args>
    struct impl_toints_;
  }
  /*!
    Convert to integer by saturated truncation.

    @par semantic:
    For any given value @c x of type @c T:

    @code
    as_integer<T> r = toints(x);
    @endcode

    The code is similar to:

    @code
    as_integer<T> r = static_cast<as_integer<T> >(saturate<as_integer<T> >(x))
    @endcode

    @par Notes:

    The Inf -Inf and Nan values are treated properly and go respectively to
    Valmax, Valmin and Zero of the destination integral type
    All values superior (resp.) less than Valmax (resp. Valmin) of the return type
    are saturated accordingly.

    @par Alias

    @c ifix,  @c itrunc, @c saturated_toint

    @param  a0

    @return      a value of the integer same type associated to the input.

  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::toints_, toints, 1)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::toints_, ifix, 1)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::toints_, itrunc, 1)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::toints_, saturated_toint, 1)

} }

#include <boost/simd/operator/specific/common.hpp>

#endif


/**
 * @file labels_python.cpp
 * @date 2021-08-16
 *
 * Defines the function getLabelsPython in KMedoidsWrapper class
 * which is used in Python bindings.
 *
 */

#include "kmedoids_pywrapper.hpp"

#include <carma>
#include <armadillo>
#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>

namespace py = pybind11;

/**
 *  \brief Returns the medoid assignments for each datapoint
 *
 *  Returns as a numpy array the medoid each input datapoint is assigned to
 *  after KMedoids::fit is called and the final medoids have been identified
 */
py::array_t<double> KMedoidsWrapper::getLabelsPython() {
    return carma::row_to_arr<double>(KMedoids::getLabels()).squeeze();
}

void labels_python(py::class_<KMedoidsWrapper> &cls) {
    cls.def_property_readonly("labels", &KMedoidsWrapper::getLabelsPython);
}


// Contains boost::bind and placeholders.
#include <boost/bind.hpp>

// Utility stuff required by samples.
#include <boost/array.hpp>
#include <algorithm>
#include <functional>
#include <string>
#include <cassert>

void sample1() {
    const boost::array<int, 12> v = {{
        1, 2, 3, 4, 5, 6, 7, 100, 99, 98, 97, 96
    }};

    const std::size_t count0 = std::count_if(v.begin(), v.end(),
        [](int x) { return 5 < x; }
    );
    const std::size_t count1 = std::count_if(v.begin(), v.end(),
        boost::bind(std::less<int>(), 5, _1)
    );
    assert(count0 == count1);
}

void sample2() {
    const boost::array<std::string, 3> v = {{
        "We ", "are", " the champions!"
    }};

    const std::size_t count0 = std::count_if(v.begin(), v.end(),
        [](const std::string& s) { return s.empty(); }
    );
    const std::size_t count1 = std::count_if(v.begin(), v.end(),
        boost::bind(&std::string::empty, _1)
    );
    assert(count0 == count1);
}

void sample3() {
    const boost::array<std::string, 3> v = {{
        "We ", "are", " the champions!"
    }};

    const std::size_t count0 = std::count_if(v.begin(), v.end(),
        [](const std::string& s) {  return s.size() < 5; }
    );
    const std::size_t count1 = std::count_if(v.begin(), v.end(),
        boost::bind(
            std::less<std::size_t>(),
            boost::bind(&std::string::size, _1),
            5
        )
    );
    assert(count0 == count1);
}

void sample4() {
    const boost::array<std::string, 3> v = {{
        "We ", "are", " the champions!"
    }};
    std::string s(
        "Expensive copy constructor is called when binding"
    );

    const std::size_t count0 = std::count_if(v.begin(), v.end(),
        [&s](const std::string& x) {  return x < s; }
    );
    const std::size_t count1 = std::count_if(v.begin(), v.end(),
        boost::bind(std::less<std::string>(), _1, s)
    );
    assert(count0 == count1);
}

#include <boost/ref.hpp>

void sample5() {
    const boost::array<std::string, 3> v = {{
        "We ", "are", " the champions!"
    }};
    std::string s(
        "Expensive copy constructor is NOT called when binding"
    );

    const std::size_t count1 = std::count_if(v.begin(), v.end(),
        boost::bind(std::less<std::string>(), _1, boost::cref(s))
    );
    // ...

    const std::size_t count0 = std::count_if(v.begin(), v.end(), [&s](const std::string& x) {
        return x < s;
    });
    assert(count0 == count1);
}

void sample6() {
    const auto twice = boost::bind(std::plus<int>(), _1, _1);
    assert(twice(2) == 4);

    const auto minus_from_second = boost::bind(std::minus<int>(), _2, _1);
    assert(minus_from_second(2, 4) == 2);

    const auto sum_second_and_third = boost::bind(
        std::plus<int>(), _2, _3
    );
    assert(sum_second_and_third(10, 20, 30) == 50);
}

int main () {
    sample1();
    sample2();
    sample3();
    sample4();
    sample5();
    sample6();
}


//  Copyright John Maddock 2006.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// Basic sanity check that header <boost/math/tools/roots.hpp>
// #includes all the files that it needs to.
//
#include <boost/math/tools/roots.hpp>
//
// Note this header includes no other headers, this is
// important if this test is to be meaningful:
//
inline void check_result_imp(std::pair<float, float>, std::pair<float, float>){}
inline void check_result_imp(std::pair<double, double>, std::pair<double, double>){}
inline void check_result_imp(std::pair<long double, long double>, std::pair<long double, long double>){}

#include "test_compile_result.hpp"

void compile_and_link_test()
{
   typedef double (*F)(double);
   typedef std::pair<double, double> (*F2)(double);
   typedef std::tr1::tuple<double, double, double> (*F3)(double);
   typedef boost::math::tools::eps_tolerance<double> Tol;
   Tol tol(u);
   boost::uintmax_t max_iter = 0;
   F f = 0;
   F2 f2 = 0;
   F3 f3 = 0;

   check_result<std::pair<double, double> >(boost::math::tools::bisect<F, double, Tol>(f, d, d, tol, max_iter));
   check_result<std::pair<double, double> >(boost::math::tools::bisect<F, double, Tol>(f, d, d, tol));
   check_result<double>(boost::math::tools::newton_raphson_iterate<F2, double>(f2, d, d, d, i, max_iter));
   check_result<double>(boost::math::tools::halley_iterate<F3, double>(f3, d, d, d, i, max_iter));
   check_result<double>(boost::math::tools::schroeder_iterate<F3, double>(f3, d, d, d, i, max_iter));
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE LibmeshEntitySetTester
#include <boost/test/included/unit_test.hpp>

#include <algorithm>
#include <cassert>
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <sstream>
#include <vector>

#include <LibmeshEntityExtraData.hpp>
#include <LibmeshEntitySet.hpp>

#include <Teuchos_Array.hpp>
#include <Teuchos_ArrayRCP.hpp>
#include <Teuchos_CommHelpers.hpp>
#include <Teuchos_DefaultComm.hpp>
#include <Teuchos_DefaultMpiComm.hpp>
#include <Teuchos_OpaqueWrapper.hpp>
#include <Teuchos_RCP.hpp>
#include <Teuchos_Tuple.hpp>
#include <Teuchos_TypeTraits.hpp>

#include <libmesh/cell_hex8.h>
#include <libmesh/libmesh.h>
#include <libmesh/mesh.h>
#include <libmesh/node.h>
#include <libmesh/parallel.h>
#include <libmesh/point.h>

//---------------------------------------------------------------------------//
// MPI Setup
//---------------------------------------------------------------------------//

template <class Ordinal>
Teuchos::RCP<const Teuchos::Comm<Ordinal>> getDefaultComm()
{
#ifdef HAVE_MPI
    return Teuchos::DefaultComm<Ordinal>::getComm();
#else
    return Teuchos::rcp( new Teuchos::SerialComm<Ordinal>() );
#endif
}

//---------------------------------------------------------------------------//
// Hex-8 test.
BOOST_AUTO_TEST_CASE( checkLibmeshEntitySet )
{
	const std::string argv_string = "unit_test --keep-cout";
	const char *argv_char = argv_string.c_str();
	Teuchos::GlobalMPISession mpiSession(
			&boost::unit_test::framework::master_test_suite().argc,
			&boost::unit_test::framework::master_test_suite().argv);
	auto comm = Teuchos::DefaultComm<int>::getComm();
	auto mpi_comm = Teuchos::rcp_dynamic_cast<const Teuchos::MpiComm<int>>(
			comm);
	auto opaque_comm = mpi_comm->getRawMpiComm();
	auto raw_comm = (*opaque_comm)();

    // Create the mesh.
    int space_dim = 3;
    libMesh::LibMeshInit libmesh_init( 1, &argv_char, raw_comm );
    BOOST_VERIFY( libMesh::initialized() );
    BOOST_VERIFY( (int)libmesh_init.comm().rank() == comm->getRank() );
    Teuchos::RCP<libMesh::Mesh> mesh =
        Teuchos::rcp( new libMesh::Mesh( libmesh_init.comm(), space_dim ) );

    // Create the nodes.
    int rank = comm->getRank();
    Teuchos::Array<libMesh::Node *> nodes( 8 );
    double node_coords[3];
    node_coords[0] = 0.0;
    node_coords[1] = 0.0;
    node_coords[2] = 0.0;
    nodes[0] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 0,
        rank );

    node_coords[0] = 1.0;
    node_coords[1] = 0.0;
    node_coords[2] = 0.0;
    nodes[1] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 1,
        rank );

    node_coords[0] = 1.0;
    node_coords[1] = 1.0;
    node_coords[2] = 0.0;
    nodes[2] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 2,
        rank );

    node_coords[0] = 0.0;
    node_coords[1] = 1.0;
    node_coords[2] = 0.0;
    nodes[3] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 3,
        rank );

    node_coords[0] = 0.0;
    node_coords[1] = 0.0;
    node_coords[2] = 1.0;
    nodes[4] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 4,
        rank );

    node_coords[0] = 1.0;
    node_coords[1] = 0.0;
    node_coords[2] = 1.0;
    nodes[5] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 5,
        rank );

    node_coords[0] = 1.0;
    node_coords[1] = 1.0;
    node_coords[2] = 1.0;
    nodes[6] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 6,
        rank );

    node_coords[0] = 0.0;
    node_coords[1] = 1.0;
    node_coords[2] = 1.0;
    nodes[7] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 7,
        rank );

    // Make a hex-8.
    libMesh::Elem *hex_elem = mesh->add_elem( new libMesh::Hex8 );
    hex_elem->processor_id() = rank;
    hex_elem->set_id() = 2 * rank;
    for ( int i = 0; i < 8; ++i )
        hex_elem->set_node( i ) = nodes[i];

    // Make 2 subdomains and put the hex-8 in the first subdomain.
    int subdomain_1_id = 1;
    int subdomain_2_id = 2;
    std::set<libMesh::subdomain_id_type> subdomain_ids;
    subdomain_ids.insert( subdomain_1_id );
    subdomain_ids.insert( subdomain_2_id );
    hex_elem->subdomain_id() = subdomain_1_id;

    // Make 2 boundaries and add the first elem side to one and first node to
    // the second.
    int boundary_1_id = 1;
    int boundary_2_id = 2;
    mesh->get_boundary_info().add_side( hex_elem, 0, boundary_1_id );
    mesh->get_boundary_info().add_node( nodes[0], boundary_2_id );

    // Check libmesh validity.
    mesh->libmesh_assert_valid_parallel_ids();

    // Create an entity set.
    auto entity_set =
        Teuchos::rcp( new LibmeshAdapter::LibmeshEntitySet( mesh ) );

    // Test the set.
    Teuchos::RCP<const Teuchos::Comm<int>> set_comm =
        entity_set->communicator();
    BOOST_VERIFY( set_comm->getRank() == comm->getRank() );
    BOOST_VERIFY( set_comm->getSize() == comm->getSize() );
    BOOST_VERIFY( space_dim == entity_set->physicalDimension() );

    // Make an iterator for the hex.
    LibmeshAdapter::ElemPredicateFunction all_pred =
        [=]( LibmeshAdapter::LibmeshEntity<libMesh::Elem> ) { return true; };
    auto volume_iterator =
        entity_set->entityIterator( all_pred );

    // Test the volume iterator.
    BOOST_VERIFY( volume_iterator.size() == 1 );
    BOOST_VERIFY( volume_iterator == volume_iterator.begin() );
    BOOST_VERIFY( volume_iterator != volume_iterator.end() );

    // Test the volume under the iterator.
    BOOST_VERIFY( hex_elem->id() == volume_iterator->id() );
    BOOST_VERIFY( comm->getRank() == volume_iterator->ownerRank() );
    BOOST_VERIFY( space_dim == volume_iterator->topologicalDimension() );
    BOOST_VERIFY( space_dim == volume_iterator->physicalDimension() );

   auto extra_data_1 =
        volume_iterator->extraData();
   BOOST_VERIFY( hex_elem ==
                   Teuchos::rcp_dynamic_cast<
                       LibmeshAdapter::LibmeshEntityExtraData<libMesh::Elem>>(
                       extra_data_1 )
                       ->d_libmesh_geom.getRawPtr() );

    Teuchos::Tuple<double, 6> hex_bounds_1;
    volume_iterator->boundingBox( hex_bounds_1 );
    BOOST_VERIFY( 0.0 == hex_bounds_1[0] );
    BOOST_VERIFY( 0.0 == hex_bounds_1[1] );
    BOOST_VERIFY( 0.0 == hex_bounds_1[2] );
    BOOST_VERIFY( 1.0 == hex_bounds_1[3] );
    BOOST_VERIFY( 1.0 == hex_bounds_1[4] );
    BOOST_VERIFY( 1.0 == hex_bounds_1[5] );

    // Test the end of the iterator.
    volume_iterator++;
    BOOST_VERIFY( volume_iterator != volume_iterator.begin() );
    BOOST_VERIFY( volume_iterator == volume_iterator.end() );

    // Make an iterator for the nodes.
    LibmeshAdapter::NodePredicateFunction all_node_pred =
            [=]( LibmeshAdapter::LibmeshEntity<libMesh::Node> ) { return true; };
   auto node_iterator =
        entity_set->entityIterator( all_node_pred );

    // Test the node iterator.
    unsigned num_nodes = 8;
    BOOST_VERIFY( node_iterator.size() == num_nodes );
    BOOST_VERIFY( node_iterator == node_iterator.begin() );
    BOOST_VERIFY( node_iterator != node_iterator.end() );
    auto node_begin = node_iterator.begin();
    auto node_end = node_iterator.end();
    auto node_id_it = nodes.begin();
    for ( node_iterator = node_begin; node_iterator != node_end;
          ++node_iterator, ++node_id_it )
    {
    	BOOST_VERIFY( node_iterator->id() == ( *node_id_it )->id() );
    }

    // Get each entity and check.
    LibmeshAdapter::LibmeshEntity<libMesh::Elem> set_hex;
    entity_set->getEntity( hex_elem->id(), set_hex );
    BOOST_VERIFY( set_hex.id() == hex_elem->id() );

    for ( unsigned i = 0; i < num_nodes; ++i )
    {
    	LibmeshAdapter::LibmeshEntity<libMesh::Node> set_node;
        entity_set->getEntity( nodes[i]->id(), set_node );
        BOOST_VERIFY( set_node.id() == nodes[i]->id() );
    }

    // Check the adjacency function.
    Teuchos::Array<LibmeshAdapter::LibmeshEntity<libMesh::Elem>> hex_adjacent_volumes;
    entity_set->getAdjacentEntities( set_hex, hex_adjacent_volumes );
    BOOST_VERIFY( 0 == hex_adjacent_volumes.size() );

    Teuchos::Array<LibmeshAdapter::LibmeshEntity<libMesh::Node>> hex_adjacent_nodes;
    entity_set->getAdjacentEntities( set_hex, hex_adjacent_nodes );
    BOOST_VERIFY( num_nodes == hex_adjacent_nodes.size() );
    for ( unsigned i = 0; i < num_nodes; ++i )
    {
    	BOOST_VERIFY( hex_adjacent_nodes[i].id() == nodes[i]->id() );
    }

    for ( unsigned i = 0; i < num_nodes; ++i )
    {
        Teuchos::Array<LibmeshAdapter::LibmeshEntity<libMesh::Elem>> node_adjacent_volumes;
        entity_set->getAdjacentEntities( hex_adjacent_nodes[i],
                                         node_adjacent_volumes );
        BOOST_VERIFY( 1 == node_adjacent_volumes.size() );
        BOOST_VERIFY( node_adjacent_volumes[0].id() == hex_elem->id() );
    }

}

//---------------------------------------------------------------------------//
// end tstLibmeshEntitySet.cpp
//---------------------------------------------------------------------------//


//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/elliptic/include/functions/scalar/ellipke.hpp>
#include <nt2/elliptic/include/functions/simd/ellipke.hpp>

#include <nt2/include/constants/pi.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/pio_2.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/eps.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <boost/fusion/include/vector_tie.hpp>

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

NT2_TEST_CASE_TPL ( ellipke_real,  NT2_SIMD_REAL_TYPES)
{

  using nt2::ellipke;
  using nt2::tag::ellipke_;
  using boost::simd::native;

  typedef native<T,BOOST_SIMD_DEFAULT_EXTENSION>            vT;
  vT a[] = {nt2::One<vT>(), nt2::Zero<vT>(), nt2::Half<vT>(), nt2::Two<vT>()};
  size_t N =  sizeof(a)/sizeof(vT);
  vT e1[] = {nt2::Inf<vT>(), nt2::Pio_2<vT>(), nt2::splat<vT>(1.854074677301372), nt2::Nan<vT>()};
  vT e2[] = {nt2::One<vT>(), nt2::Pio_2<vT>(), nt2::splat<vT>(1.350643881047675), nt2::Nan<vT>()};

  NT2_TEST_TYPE_IS( (typename boost::dispatch::meta::call<ellipke_(vT)>::type)
                  , (std::pair<vT,vT>)
                  );
  NT2_TEST_TYPE_IS( (typename boost::dispatch::meta::call<ellipke_(vT, T)>::type)
                  , (std::pair<vT,vT>)
                  );
  NT2_TEST_TYPE_IS( (typename boost::dispatch::meta::call<ellipke_(vT, T, vT&)>::type)
                  , (vT)
                  );
  NT2_TEST_TYPE_IS( (typename boost::dispatch::meta::call<ellipke_(vT, T, vT&, vT&)>::type)
                  , (void)
                  );

  {
    vT s, c;
    for(size_t i=0; i < N; ++i)
    {
      ellipke(a[i],nt2::Eps<T>(), s, c);
      NT2_TEST_ULP_EQUAL(s, e1[i], 1);
      NT2_TEST_ULP_EQUAL(c, e2[i], 1);
    }
  }

  {
    vT s, c;
    for(size_t i=0; i < N; ++i)
    {
      s = ellipke(a[i], nt2::Eps<T>(), c);
      NT2_TEST_ULP_EQUAL(s, e1[i], 1);
      NT2_TEST_ULP_EQUAL(c, e2[i], 1);
    }
  }

  {
    vT s, c;
    for(size_t i=0; i < N; ++i)
    {
      boost::fusion::vector_tie(s, c) = ellipke(a[i]);
      NT2_TEST_ULP_EQUAL(s, e1[i], 1);
      NT2_TEST_ULP_EQUAL(c, e2[i], 1);
    }
  }

  {
    for(size_t i=0; i < N; ++i)
    {
      std::pair<vT,vT> p = ellipke(a[i]);
      NT2_TEST_ULP_EQUAL(p.first,  e1[i], 1);
      NT2_TEST_ULP_EQUAL(p.second, e2[i], 1);
    }
  }
}


#include <iostream>
#include <boost/thread/thread.hpp>
#include <unistd.h> // for delay
#include <sys/stat.h>
#include <sys/types.h>

#include <pcl/common/common_headers.h>
#include <pcl/io/pcd_io.h> // Allows for .pcd to be read from disk
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/registration/gicp6d.h>
#include <pcl/console/parse.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h> // allows for transforms
#include <pcl/point_cloud.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/passthrough.h>

#include <pcl/filters/voxel_grid.h>

// The USAGE of the program
void printUsage (const char* progName)
{
    	std::cout << "Usage: " << progName << "[options]\n" << "Options:\n" << "-h for help\n" << "[file_name.pcd]" << std::endl;
}

void filterPointCloud(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pointcloud_in, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pointcloud_out)
{
	float mean = 50;
	float stddev = 1;

	// statistical filter the point cloud
	pcl::StatisticalOutlierRemoval<pcl::PointXYZRGBA> sor;
	sor.setInputCloud (pointcloud_in);
	sor.setMeanK(mean);
	sor.setStddevMulThresh(stddev);
	sor.filter(*pointcloud_out);
}

int main (int argc, char** argv)
{ // START of MAIN
 	typedef pcl::PointXYZRGBA PointT;
    typedef pcl::PointCloud<PointT> PointCloud;
    typedef pcl::PointCloud<PointT>::Ptr PointCloudPtr;
	std::string filedir;

	if (argc > 1)
	{
		filedir = argv[1];
		std::string sys_cmd = "mkdir -p ";
		sys_cmd.append(filedir);
		sys_cmd.append("output/");
		system(sys_cmd.c_str());
		std::cout << sys_cmd << std::endl;
	}
	else
	{
		std::cout << "Input path not specified" << std::endl;
		std::cout << "\t Usage: ./exec ./data/dataset/" << std::endl;
		return -1;
	}

	// This is where we want to read in our own .pcd files to view on the viewer
	// ------------------------------------------------------------------------------------------------------

	//pcl::PointCloud<pcl::PointXYZRGB>& point_cloud = *point_cloud_ptr;
	// std::vector<int> pcd_filename_indices = pcl::console::parse_file_extension_argument (argc, argv, "pcd"); // fetches files from args

	//Creating the viewer
	pcl::visualization::PCLVisualizer viewer("Point Cloud Visualizer");
	//setting the background to black
	viewer.setBackgroundColor (0, 0, 0); // set background to black

	// if (!pcd_filename_indices.empty ()) // checks to make sure a .pcd has been specified
	// {
		// Need to call updatePointCloud() to change the point cloud each iteration
		// Creating for loop to go through each .pcd in the file specified

		std::vector<PointCloudPtr> data_s;
		viewer.spinOnce();
		std::string infile;

		for(int i = 0; i < 24; i++)
		{
			infile = filedir;
			infile = infile.append(std::to_string(i+1));
			infile = infile.append(".pcd");

			std::cout << " Loading File: " << infile << std::endl;
			PointCloudPtr data (new PointCloud);
			if (pcl::io::loadPCDFile (infile, *data) < 0)
			{
				std::cout << "Error loading point cloud " << infile << std::endl << std::endl;
				return -1;
			}
			// filter the data
			filterPointCloud(data, data);

			data_s.push_back(data); // pushing the .pcd data into the data_s vector
		}

		if (data_s.empty())
		{
			PCL_ERROR ("The data_s vector is empty! \n") ;
			return (-1);
		}

		pcl::visualization::PCLVisualizer *p;
		p = new pcl::visualization::PCLVisualizer("Point Cloud Window");
	//	p->createViewPort(0.0, 0, 0.5, 1.0, v1); // the sizes of each viewport
	//	p->createViewPort(0.5, 0, 1.0, 1.0, v2);

		PointCloudPtr source (new PointCloud);
		PointCloudPtr target (new PointCloud);
		//std::cout << data_s.size() << std::endl;
		for(size_t i = 1; i < data_s.size(); ++i)
		{
			source = data_s[i-1];
			target = data_s[i];

			PointCloudPtr pc1_sample (new PointCloud);
			PointCloudPtr pc2_sample (new PointCloud);

			pcl::VoxelGrid<PointT> vox;
			vox.setLeafSize(0.001f, 0.001f, 0.001f);
			vox.setInputCloud(source);
			vox.filter(*pc1_sample);
			vox.setInputCloud(target);
			vox.filter(*pc2_sample);

			p->removePointCloud ("_target");
  			p->removePointCloud ("_source");

			// manually rotate by 15 degrees
			Eigen::Affine3f myMat = Eigen::Affine3f::Identity();
			float theta = M_PI/24;
			myMat.rotate(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitY()));

			// we rotate first, since we know the clouds are 15degrees apart
			pcl::transformPointCloud(*pc1_sample, *pc1_sample, myMat);
			pcl::transformPointCloud(*source, *source, myMat);

    		PointCloudPtr pcout (new PointCloud);

			 // register
			pcl::GeneralizedIterativeClosestPoint6D reg;
			// we want to transform pc2 to match pc1
			reg.setInputSource(pc1_sample);
			reg.setInputTarget(pc2_sample);
			reg.setMaximumIterations(100);
			reg.setTransformationEpsilon(1e-7);
			reg.align(*pcout);

			Eigen::Matrix4f FunMat = Eigen::Matrix4f::Identity();
			FunMat = reg.getFinalTransformation();
			std::cout << "has converged?: " << reg.hasConverged() << ". Score: " << reg.getFitnessScore() << std::endl;
			std::cout << FunMat << std::endl;

    		pcl::transformPointCloud(*pc1_sample, *pc1_sample, FunMat);

			pcl::visualization::PointCloudColorHandlerRGBField<PointT> tgt_h (pc2_sample);
  			pcl::visualization::PointCloudColorHandlerRGBField<PointT> src_h (pc1_sample);

			//Rotation Function ------------------------------------------------------------------------------------------------------------------------------------
			float angle = 3.14159;   // angle of rotation, radians = 180 degrees
			// Using Eigen::Affine3f for 3D point cloud
			Eigen::Affine3f transform = Eigen::Affine3f::Identity();
			Eigen::Affine3f shift_right = Eigen::Affine3f::Identity();
			shift_right.translation() << 0.0, 0.0, 1.0; // translate with zoom and shift
			transform.translation() << 0.0, 0.0, 1.0; // translate with a zoom in Z-axis
			transform.rotate (Eigen::AngleAxisf (angle, Eigen::Vector3f::UnitX())); // x rotation
			shift_right.rotate(Eigen::AngleAxisf (angle, Eigen::Vector3f::UnitX())); // same rotation as before
			//std::cout << transform.matrix() << std::endl; // display the rotation matrix
			PointCloudPtr transformed_source (new PointCloud); // doing the transformation
		 	PointCloudPtr transformed_target (new PointCloud);
			 // applying the transformation to the cloud
			pcl::transformPointCloud (*pc1_sample, *transformed_source, transform);
			// Translation for second point cloud to keep in the same viewport, rather than two
			pcl::transformPointCloud (*pc2_sample, *transformed_target, shift_right);

			p->addPointCloud (transformed_target, tgt_h, "_target");
  			p->addPointCloud (transformed_source, src_h, "_source");

			// Save the source cloud to the output directory
			std::string outfile = filedir;
			outfile.append("output/");
			outfile.append(std::to_string(i));
			outfile.append(".pcd");
			std::cout << "trying to save Point cloud to: " << outfile << std::endl;
			pcl::io::savePCDFileASCII(outfile, *transformed_source);
			std::cout << "Point cloud saved to: " << outfile << std::endl;

 			p->spin();
		}

	// }
	return 0;
} //END of MAIN


#include <stdio.h>
#include <iostream>
#include <queue>
#include <vector>
#include <sstream>
//#include <boost/date_time/gregorian/gregorian.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/format.hpp>
#include <fstream>

#include "writeHddThread.h"
using int32 = int;
using float64 = double;
using int8 = char;

int32       bufferSize = 80814;

int decode(std::string source, std::string target)
{
	//using DataPoint = std::pair < int32, int32 >;
	//std::vector<DataPoint> result;
	std::ofstream ofs;
	std::ifstream ifs;
	//writeFileMutex.lock();
	ifs.open(source, std::ios::binary);
	ofs.open(target);
	bool first = true;
	int32 position = 0;
	bool status;
	while (!ifs.eof())
	{
		//header
		using Header = WriteHddThread::Header;
		Header header;
		ifs.read((char*)&header, sizeof(Header));
		if (ifs.eof())
			break;
		ofs << "#" << boost::posix_time::to_iso_extended_string(header.ptime) << std::endl;
		if (first)
		{
			ofs << position << "\t" << int32(header.first) << "\n";
			status = !(header.first == 0);
			first = false;
		}
		else
		{
			if (status != !(header.first == 0))
			{
				int8 value = status ? 1 : 0;
				ofs << position - 1 << "\t" << int32(value) << std::endl;
				ofs << position << "\t" << 1 - value << std::endl;
				status = !status;
			}
		}
		int32 *buffer = new int32[header.bodySize];
		ifs.read((char*)buffer, sizeof(int32)*header.bodySize);
		int i;
		for (i = 0; i < header.bodySize - 1; i++)
		{
			position += buffer[i];
			int8 value = status ? 1 : 0;
			ofs << position - 1 << "\t" << int32(value) << std::endl;
			ofs << position << "\t" << 1 - value << std::endl;
			status = !status;
		}
		position += buffer[i];
		std::cout << "position:" << position << std::endl;
		delete buffer;
	}
	int8 value = status ? 1 : 0;
	position--;
	ofs << position << "\t" << int32(value) << std::endl;
	ifs.close();
	ofs.close();
	//writeFileMutex.unlock();
	return 0;
}

int decode_raw(std::string source, std::string target)
{
	std::ofstream ofs;
	std::ifstream ifs;
	ifs.open(source, std::ios::binary);
	ofs.open(target);
	int count = 0;
	double_t *buffer = new double_t[bufferSize];
	while (!ifs.eof())
	{

		ifs.read((char*)buffer, bufferSize * sizeof(double_t));
		for (int i = 0; i < ifs.gcount() / sizeof(double_t); i++)
		{
			ofs << count << "\t" << buffer[i] << std::endl;
			count++;
		}
	}
	delete[] buffer;
	return 0;
}

int timeDiff(std::string source, std::string target)
{
	//using DataPoint = std::pair < int32, int32 >;
	//std::vector<DataPoint> result;
	std::ofstream ofs;
	std::ifstream ifs;
	//writeFileMutex.lock();
	ifs.open(source, std::ios::binary);
	ofs.open(target);
	bool first = true;
	int32 position = 0;
	bool status;
	boost::posix_time::ptime ptime;
	while (!ifs.eof())
	{
		//header
		using Header = WriteHddThread::Header;
		Header header;
		ifs.read((char*)&header, sizeof(Header));
		if (ifs.eof())
			break;
    //ofs << "#" << boost::posix_time::to_iso_extended_string(header.ptime) << std::endl;
        if (first)
        {
	        ptime = header.ptime;
	        first = false;
        }
        else
        {
	        ofs << (header.ptime - ptime).total_microseconds() / 1000000.0 << std::endl;
	        ptime = header.ptime;
        }
        int32 *buffer = new int32[header.bodySize];
        ifs.read((char*)buffer, sizeof(int32)*header.bodySize);
        int i;
        for (i = 0; i < header.bodySize - 1; i++)
        {
	        position += buffer[i];
	        int8 value = status ? 1 : 0;
	        //ofs << position - 1 << "\t" << int32(value) << std::endl;
	        //ofs << position << "\t" << 1 - value << std::endl;
	        status = !status;
        }
        position += buffer[i];
        std::cout << "position:" << position << std::endl;
        delete buffer;
	}
	int8 value = status ? 1 : 0;
	position--;
	//ofs << position << "\t" << int32(value) << std::endl;
	ifs.close();
	ofs.close();
	//writeFileMutex.unlock();
	return 0;
}

int main(int argc, char *argv[])
{
	if (argc == 1)
	{
		return 0;
	}
	std::string cmd = argv[1];
	if (cmd == "-d")
	{
		/*
		if (argc == 4)
		{
			std::string source = argv[2];
			std::string target = argv[3];
			decode(source, target);
			return 0;
		}
		*/
		if (argc >= 3)
		{
			for (int i = 2; i < argc; i++)
			{
				std::string source = argv[i];
				std::string target = source + ".out";
				decode(source, target);
			}
			return 0;
		}
	}
	else if (cmd == "-dr")
	{
		if (argc >= 3)
		{
			for (int i = 2; i < argc; i++)
			{
				std::string source = argv[i];
				std::string target = source + ".out";
				decode_raw(source, target);
			}
			return 0;
		}
	}
	else if (cmd == "-dt")
	{
		if (argc >= 3)
		{
			for (int i = 2; i < argc; i++)
			{
				std::string source = argv[i];
				std::string target = source + ".tdiff";
				timeDiff(source, target);
			}
			return 0;
		}
	}
	std::cout << "Wrong use\n" << std::endl;
	return 0;
}


//=======================================================================
// Copyright (c)
//
// 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)
//=======================================================================
/**
 * @file value_query_example.cpp
 * @brief
 * @author Robert Rosolek
 * @version 1.0
 * @date 2014-6-5
 */

#include "paal/auctions/auction_components.hpp"

#include <boost/range/algorithm/copy.hpp>

#include <cassert>
#include <iostream>
#include <iterator>
#include <string>
#include <set>
#include <unordered_set>
#include <vector>

//! [Value Query Auction Components Example]

namespace pa = paal::auctions;

using Bidder = std::string;
using Item = std::string;
using Value = unsigned int;

const std::vector<Bidder> bidders {"Pooh Bear", "Rabbit"};

const std::vector<Item> items {"honey", "baby carrot", "carrot", "jam"};

struct get_copies_num_func {
   int operator()(Item item) const
   {
      return item == "baby carrot" ? 2 : 1;
   }
};

struct value_query_func {
   template <class ItemSet>
   Value operator()(Bidder bidder, const ItemSet& item_set) const
   {
      if (bidder == "Pooh Bear")
         return item_set.count("honey") > 0 ? 10 : 0;
      assert(bidder == "Rabbit");
      Value res = 0;
      for (Item item: item_set)
         if (item.find("carrot") != std::string::npos) ++res;
      return res;
   }
};

//! [Value Query Auction Components Example]

int main()
{
   //! [Value Query Auction Create Example]
   auto const auction = pa::make_value_query_auction_components(
      bidders, items, value_query_func(), get_copies_num_func()
   );
   //! [Value Query Auction Create Example]

   //! [Value Query Auction Use Example]
   std::cout << "bidders: ";
   boost::copy(
      auction.get<pa::bidders>(), std::ostream_iterator<Bidder>(std::cout, ", ")
   );
   std::cout << std::endl;

   std::cout << "items with copies numbers: ";
   for (auto item: auction.get<pa::items>())
      std::cout << item << " = " << auction.call<pa::get_copies_num>(item) << ", ";
   std::cout << std::endl;

   std::cout << "pooh bear valuation: " <<
      auction.call<pa::value_query>("Pooh Bear", std::set<Item>{"jam", "honey"}) <<
      std::endl;

   std::cout << "rabbit valuation: " <<
      auction.call<pa::value_query>(
         "Rabbit", std::unordered_set<Item>{"carrot", "baby carrot"}) <<
      std::endl;
   //! [Value Query Auction Use Example]

   return 0;
}


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

#include <fstream>
#include <boost/lexical_cast.hpp>
#include <boost/regex.hpp>

#include "luxrays/core/color/spds/irregular.h"
#include "slg/textures/fresnel/fresnelsopra.h"
#include "slg/textures/fresnel/fresnelconst.h"

using namespace std;
using namespace luxrays;
using namespace slg;

//------------------------------------------------------------------------------
// Fresnel Sopra texture
//------------------------------------------------------------------------------

// Converts photon energy (in eV) to wavelength (in nm)
static float eVtolambda(const float eV) {
	// lambda = hc / E, where
	//  h is planck's constant in eV*s
	//  c is speed of light in m/s
	return (4.13566733e-15f * 299792458.f * 1e9f) / eV;
}

// Converts wavelength (in micrometer) to wavelength (in nm)
static float umtolambda(const float um) {
	return um * 1000.f;
}

// Converts wavelength (in cm-1) to wavelength (in nm)
static float invcmtolambda(const float invcm) {
	return 1e7f / invcm;
}

// Converts wavelength (in nm) to wavelength (in nm)
static float nmtolambda(const float nm) {
	return nm;
}

FresnelTexture *slg::AllocFresnelSopraTex(const Properties &props, const string &propName) {
	const string fileName = props.Get(Property(propName + ".file")("sopra.nk")).Get<string>();

	ifstream fs;
	fs.open(fileName.c_str());
	string line;
	if (!getline(fs, line).good())
		throw runtime_error("Unable to read sopra file: " + fileName);

	boost::smatch m;

	// Read initial line, containing metadata
	boost::regex header_expr("(\\d+)[^\\.[:digit:]]+(\\d*\\.?\\d+(?:[eE]-?\\d+)?)[^\\.[:digit:]]+(\\d*\\.?\\d+(?:[eE]-?\\d+)?)[^\\.[:digit:]]+(\\d+)");

	if (!boost::regex_search(line, m, header_expr))
		throw runtime_error("Bad sopra header in: " + fileName);

	// Used to convert file units to wavelength in nm
	float (*tolambda)(float) = NULL;
	float lambda_first, lambda_last;

	if (m[1] == "1") {
		// lambda in eV
		// low eV -> high lambda
		lambda_first = boost::lexical_cast<float>(m[3]);
		lambda_last = boost::lexical_cast<float>(m[2]);
		tolambda = &eVtolambda;
	} else if (m[1] == "2") {
		// lambda in um
		lambda_first = boost::lexical_cast<float>(m[2]);
		lambda_last = boost::lexical_cast<float>(m[3]);
		tolambda = &umtolambda;
	} else if (m[1] == "3") {
		// lambda in cm-1
		lambda_first = boost::lexical_cast<float>(m[3]);
		lambda_last = boost::lexical_cast<float>(m[2]);
		tolambda = &invcmtolambda;
	} else if (m[1] == "4") {
		// lambda in nm
		lambda_first = boost::lexical_cast<float>(m[2]);
		lambda_last = boost::lexical_cast<float>(m[3]);
		tolambda = &nmtolambda;
	} else
		throw runtime_error("Unknown sopra unit code '" + m[1] + "' in: " + fileName);

	// Number of lines of nk data
	const int count = boost::lexical_cast<int>(m[4]);

	// Read nk data
	boost::regex sample_expr("(\\d*\\.?\\d+(?:[eE]-?\\d+)?)[^\\.[:digit:]]+(\\d*\\.?\\d+(?:[eE]-?\\d+)?)");

	vector<float> wl(count + 1);
	vector<float> n(count + 1);
	vector<float> k(count + 1);

	// We want lambda to go from low to high
	// so reverse direction
	const float delta = (lambda_last - lambda_first) / count;
	for (int i = count; i >= 0; --i) {
		if (getline(fs, line).bad())
			throw runtime_error("Not enough sopra data in: " + fileName);

		if (!boost::regex_search(line, m, sample_expr))
			throw runtime_error("Unparseable sopra data at data line " + ToString(count - i) + "' in: " + fileName);

		// Linearly interpolate units in file
		// then convert to wavelength in nm
		wl[i] = tolambda(lambda_first + delta * i);
		n[i] = boost::lexical_cast<float>(m[1]);
		k[i] = boost::lexical_cast<float>(m[2]);
	}

	IrregularSPD N(&wl[0], &n[0], wl.size());
	IrregularSPD K(&wl[0], &k[0], wl.size());

	ColorSystem colorSpace;
	const RGBColor Nrgb = colorSpace.ToRGBConstrained(N.ToNormalizedXYZ());
	const RGBColor Krgb = colorSpace.ToRGBConstrained(K.ToNormalizedXYZ());

	return new FresnelConstTexture(Nrgb, Krgb);
}


#include "mp_class.h"
#include <boost/multiprecision/miller_rabin.hpp>
#include <boost/mpl/int.hpp>
#include <utility>
#include <cmath>
#include <symengine/symengine_assert.h>
#include <symengine/symengine_exception.h>

using boost::multiprecision::numerator;
using boost::multiprecision::denominator;
using boost::multiprecision::miller_rabin_test;
using boost::multiprecision::detail::find_lsb;
using boost::mpl::int_;

#if SYMENGINE_INTEGER_CLASS == SYMENGINE_BOOSTMP

namespace SymEngine
{

integer_class pow(const integer_class &a, unsigned long b)
{
    return boost::multiprecision::pow(a, numeric_cast<unsigned>(b));
}

void mp_fdiv_qr(integer_class &q, integer_class &r, const integer_class &a,
                const integer_class &b)
{
    /*boost::multiprecision doesn't have a built-in fdiv_qr (floored division).
      Its divide_qr uses truncated division, as does its
      modulus operator. Thus, using boost::multiprecision::divide_qr we get:
      divide_qr(-5, 3, quo, rem) //quo == -1, rem == -2
      divide_qr(5, -3, quo, rem) //quo == -1, rem == 2
      but we want:
      mp_fdiv_r(quo, rem, -5, 3) //quo == -2, rem == 1
      mp_fdiv_r(quo, rem, 5, -3) //rem == -2, rem == -1
      The problem arises only when the quotient is negative.  To convert
      a truncated result into a floored result in this case, simply subtract
      one from the truncated quotient and add the divisor to the truncated
      remainder.
      */

    // must copy a and b before calling divide_qr because a or b may refer to
    // the same
    // object as q or r, causing incorrect results
    integer_class a_cpy = a, b_cpy = b;
    bool neg_quotient = ((a < 0 && b > 0) || (a > 0 && b < 0)) ? true : false;
    boost::multiprecision::divide_qr(a_cpy, b_cpy, q, r);
    // floor the quotient if necessary
    if (neg_quotient && r != 0) {
        q -= 1;
    }
    // remainder should have same sign as divisor
    if ((b_cpy > 0 && r < 0) || (b_cpy < 0 && r > 0)) {
        r += b_cpy;
        return;
    }
}

void mp_cdiv_qr(integer_class &q, integer_class &r, const integer_class &a,
                const integer_class &b)
{
    integer_class a_cpy = a, b_cpy = b;
    bool pos_quotient = ((a < 0 && b < 0) || (a > 0 && b > 0)) ? true : false;
    boost::multiprecision::divide_qr(a_cpy, b_cpy, q, r);
    // ceil the quotient if necessary
    if (pos_quotient && r != 0) {
        q += 1;
    }
    // remainder should have opposite sign as divisor
    if ((b_cpy > 0 && r > 0) || (b_cpy < 0 && r < 0)) {
        r -= b_cpy;
        return;
    }
}

void mp_gcdext(integer_class &gcd, integer_class &s, integer_class &t,
               const integer_class &a, const integer_class &b)
{

    integer_class this_s(1);
    integer_class this_t(0);
    integer_class next_s(0);
    integer_class next_t(1);
    integer_class this_r(a);
    integer_class next_r(b);
    integer_class q;
    while (next_r != 0) {
        // should use truncated division, so use
        // boost::multiprecision::divide_qr
        // beware of overwriting this_r during internal operations of divide_qr
        // copy it first
        integer_class this_r_cpy = this_r;
        boost::multiprecision::divide_qr(this_r_cpy, next_r, q, this_r);
        this_s -= q * next_s;
        this_t -= q * next_t;
        std::swap(this_s, next_s);
        std::swap(this_t, next_t);
        std::swap(this_r, next_r);
    }
    // normalize the gcd, s and t
    if (this_r < 0) {
        this_r *= -1;
        this_s *= -1;
        this_t *= -1;
    }
    gcd = std::move(this_r);
    s = std::move(this_s);
    t = std::move(this_t);
}

bool mp_invert(integer_class &res, const integer_class &a,
               const integer_class &m)
{
    integer_class gcd, s, t;
    mp_gcdext(gcd, s, t, a, m);
    if (gcd != 1) {
        res = 0;
        return false;
    } else {
        mp_fdiv_r(s, s, m); // reduce s modulo m.  undefined behavior when m ==
                            // 0, so don't need to check
        if (s < 0) {
            s += mp_abs(m);
        } // give the canonical representative of s
        res = s;
        return true;
    }
}

// floored modulus
integer_class fmod(const integer_class &a, const integer_class &mod)
{
    integer_class res = a % mod;
    if (res < 0) {
        res += mod;
    }
    return res;
}

void mp_pow_ui(rational_class &res, const rational_class &i, unsigned long n)
{
    integer_class num = numerator(i);   // copy
    integer_class den = denominator(i); // copy
    num = pow(num, n);
    den = pow(den, n);
    res = rational_class(std::move(num), std::move(den));
}

void mp_powm(integer_class &res, const integer_class &base,
             const integer_class &exp, const integer_class &m)
{
    // if exp is negative, interpret as follows
    // base**(exp) mod m 	== (base**(-1))**abs(exp) mod m
    // 						== (base**(-1) mod m) ** abs(exp) mod m
    // where base**(-1) mod m is the modular inverse
    if (exp < 0) {
        integer_class base_inverse;
        if (!mp_invert(base_inverse, base, m)) {
            throw SymEngine::SymEngineException("negative exponent undefined "
                                                "in powm if base is not "
                                                "invertible mod m");
        }
        res = boost::multiprecision::powm(base_inverse, mp_abs(exp), m);
        return;
    } else {
        res = boost::multiprecision::powm(base, exp, m);
        // boost's powm calculates base**exp % m, but uses truncated
        // modulus, e.g. powm(-2,3,5) == -3.  We want powm(-2,3,5) == 2
        if (res < 0) {
            res += m;
        }
    }
}

integer_class step(const unsigned long &n, const integer_class &i,
                   integer_class &x)
{
    SYMENGINE_ASSERT(n > 1);
    unsigned long m = n - 1;
    integer_class &&x_m = pow(x, m);
    return integer_class((integer_class(m * x) + integer_class(i / x_m)) / n);
}

bool positive_root(integer_class &res, const integer_class &i,
                   const unsigned long n)
{
    integer_class x
        = 1; // TODO: make a better starting guess based on (number of bits)/n
    integer_class y = step(n, i, x);
    do {
        x = y;
        y = step(n, i, x);
    } while (y < x);
    res = x;
    if (pow(x, n) == i) {
        return true;
    }
    return false;
}

// return true if i is a perfect nth power, i.e. res**i == n
bool mp_root(integer_class &res, const integer_class &i, const unsigned long n)
{
    if (n == 0) {
        throw std::runtime_error("0th root is undefined");
    }
    if (n == 1) {
        res = i;
        return true;
    }
    if (i == 0) {
        res = 0;
        return true;
    }
    if (i > 0) {
        return positive_root(res, i, n);
    }
    if (i < 0 && (n % 2 == 0)) {
        throw std::runtime_error("even root of a negative is non-real");
    }
    bool b = positive_root(res, -i, n);
    res *= -1;
    return b;
}

integer_class mp_sqrt(const integer_class &i)
{
    // as of 11/1/2016, boost::multiprecision::sqrt() is buggy:
    // https://svn.boost.org/trac/boost/ticket/12559
    // implement with mp_root for now
    integer_class res;
    mp_root(res, i, 2);
    return res;
}

void mp_rootrem(integer_class &a, integer_class &b, const integer_class &i,
                unsigned long n)
{
    mp_root(a, i, n);
    integer_class p = pow(a, n);
    ;
    b = i - p;
}

void mp_sqrtrem(integer_class &a, integer_class &b, const integer_class &i)
{
    a = mp_sqrt(i);
    b = i - boost::multiprecision::pow(a, 2);
}

// return nonzero if i is probably prime.
int mp_probab_prime_p(const integer_class &i, unsigned retries)
{
    if (i % 2 == 0)
        return (i == 2);
    return miller_rabin_test(i, retries);
}

void mp_nextprime(integer_class &res, const integer_class &i)
{
    // simple implementation:  just check all odds bigger than i for primality
    if (i < 2) {
        res = 2;
        return;
    }
    integer_class candidate;
    candidate = (i % 2 == 0) ? i + 1 : i + 2;
    // Knuth recommends 25 trials for a pretty strong likelihood that candidate
    // is prime
    while (!mp_probab_prime_p(candidate, 25)) {
        candidate += 2;
    }
    res = std::move(candidate);
}

unsigned long mp_scan1(const integer_class &i)
{
    if (i == 0) {
        return ULONG_MAX;
    }
    return find_lsb(i, int_<0>());
}

// define simple 2x2 matrix with exponentiation by repeated squaring
// to use in logarithmic-time fibonacci calculation

struct two_by_two_matrix {
    integer_class data[2][2]; // data[1][0] is row 1, column 0 entry of matrix

    two_by_two_matrix(integer_class a, integer_class b, integer_class c,
                      integer_class d)
        : data{{a, b}, {c, d}}
    {
    }
    two_by_two_matrix() : data{{0, 0}, {0, 0}}
    {
    }
    two_by_two_matrix &operator=(const two_by_two_matrix &other)
    {
        this->data[0][0] = other.data[0][0];
        this->data[0][1] = other.data[0][1];
        this->data[1][0] = other.data[1][0];
        this->data[1][1] = other.data[1][1];
        return *this;
    }
    two_by_two_matrix operator*(const two_by_two_matrix &other)
    {
        two_by_two_matrix res;
        res.data[0][0] = this->data[0][0] * other.data[0][0]
                         + this->data[0][1] * other.data[1][0];
        res.data[0][1] = this->data[0][0] * other.data[0][1]
                         + this->data[0][1] * other.data[1][1];
        res.data[1][0] = this->data[1][0] * other.data[0][0]
                         + this->data[1][1] * other.data[1][0];
        res.data[1][1] = this->data[1][0] * other.data[0][1]
                         + this->data[1][1] * other.data[1][1];
        return res;
    }
    // recursive repeated squaring
    two_by_two_matrix pow(unsigned long n)
    {
        if (n == 0) {
            return two_by_two_matrix::identity();
        }
        if (n == 1) {
            return *this;
        }
        if (n == 2) {
            return (*this) * (*this);
        }
        if (n % 2 == 0) {
            return (this->pow(n / 2)).pow(2);
        }
        return ((this->pow((n - 1) / 2)).pow(2)) * (*this);
    }

    static two_by_two_matrix identity()
    {
        return two_by_two_matrix(1, 0, 0, 1);
    }
};

inline two_by_two_matrix fib_matrix(unsigned long n)
{
    two_by_two_matrix x(1, 1, 1, 0);
    return x.pow(n);
}

void mp_fib_ui(integer_class &res, unsigned long n)
{
    // reference: https://www.nayuki.io/page/fast-fibonacci-algorithms
    res = fib_matrix(n).data[0][1];
}

// sets a = Fibonacci(n) and b = Fibonacci(n-1)
void mp_fib2_ui(integer_class &a, integer_class &b, unsigned long n)
{
    // reference: https://www.nayuki.io/page/fast-fibonacci-algorithms
    two_by_two_matrix result_matrix = fib_matrix(n);
    a = result_matrix.data[0][1];
    b = result_matrix.data[1][1];
}

inline two_by_two_matrix luc_matrix(unsigned long n)
{
    two_by_two_matrix multiplier(1, 1, 1, 0);
    two_by_two_matrix start(1, 0, 2, 0);
    return multiplier.pow(n) * start;
}

void mp_lucnum_ui(integer_class &res, unsigned long n)
{
    // implementation based on the following fact:
    // [[1,1],[1,0]]^(n-1)*[2,0,1,0] = [[L(n+1),0][L(n),0]]
    // where L(n) is the nth Lucas number
    res = luc_matrix(n).data[1][0];
}

void mp_lucnum2_ui(integer_class &a, integer_class &b, unsigned long n)
{
    if (n == 0) {
        throw std::runtime_error("index of lucas number cannot be negative");
    }
    two_by_two_matrix result_matrix = luc_matrix(n - 1);
    a = result_matrix.data[0][0];
    b = result_matrix.data[1][0];
}

void mp_fac_ui(integer_class &res, unsigned long n)
{
    // couldn't make boost's template version of factorial work,
    // so implement slow, naive version for now
    res = 1;
    for (unsigned long i = 2; i <= n; ++i) {
        res *= i;
    }
}

void mp_bin_ui(integer_class &res, const integer_class &n, unsigned long r)
{
    // slow, naive implementation
    integer_class x = n - r;
    res = 1;
    for (unsigned long i = 1; i <= r; ++i) {
        res *= x + i;
        res /= i;
    }
}

// this is extremely slow!
bool mp_perfect_power_p(const integer_class &i)
{
    if (i == 0 || i == 1 || i == -1) {
        return true;
    }
    // if i == a**k, with k == pq for some integers p,q
    // then i == a**(pq) == (a**p)**q == (a**q)**p
    // hence i is a pth power and a qth power
    // Hence if i == p**k, then i is a pth power for any p
    // in the prime factorization of k, and it suffices
    // to check whether i is a prime power.

    // the largest possible prime p would arise from the
    // case where i is a prime power of two
    // so check all prime roots up to log(i) base 2.

    unsigned long max = std::ilogb(i.convert_to<double>());

    // treat case p=2 separately b/c mp_root throws exception
    // with an even root of a negative
    if (mp_perfect_square_p(i)) {
        return true;
    }
    integer_class p(2);
    integer_class root(0);
    while (true) {
        mp_nextprime(p, p);
        if (p > max) {
            return false;
        }
        if (mp_root(root, i, p.convert_to<unsigned long>())) {
            return true;
        }
    }
}

bool mp_perfect_square_p(const integer_class &i)
{
    if (i < 0) {
        return false;
    }
    integer_class root;
    return mp_root(root, i, 2);
}

// according to the gmp documentation, the behavior of the
// corresponding function mpz_legendre is
// undefined if n is not a positive odd prime.
// hence we treat n as though it were a positive odd prime
// but it is up to the caller to very this.
int mp_legendre(const integer_class &a, const integer_class &n)
{
    integer_class res;
    mp_powm(res, a, integer_class((n - 1) / 2), n);
    return res <= 1 ? res.convert_to<int>() : -1;
}

// private function that computes jacobi symbols
// without checking that arguments satisfy a >= 0
// and n is odd.
int unchecked_jacobi(const integer_class &a, const integer_class &n)
{
    // https://en.wikipedia.org/wiki/Jacobi_symbol#Calculating_the_Jacobi_symbol
    if (a == 1) {
        return 1;
    }
    integer_class num = a;
    integer_class den = n;
    // (1) Reduce the "numerator" modulo the "denominator"
    num = fmod(num, den);

    // (2) Extract any factors of 2 from the "numerator"
    unsigned long factors_of_two = 0;
    while (num % 2 == 0 && num != 0) {
        num /= 2; // use a shift instead of division here? faster?
        ++factors_of_two;
    }
    int product_of_twos = 1; // (2 | den)**factors_of_two

    // (2 | den) is -1 iff den % 8 = 3 or den % 8 = 5.
    // If factors_of_two is odd and (2 | den) is -1,
    // then (2 | den)**factors_of_two is -1.
    // Otherwise, (2 | den)**factors_of_two is 1.
    int den_mod_8 = fmod(den, 8).convert_to<int>();
    if ((factors_of_two % 2 == 1) && (den_mod_8 == 3 || den_mod_8 == 5)) {
        product_of_twos = -1;
    }

    // (3) If the remaining "numerator" (after extraction of twos) is 1,
    // then the remaining jacobi symbol is 1.
    // If the "numerator" and "denominator" are not coprime, result is 0.
    if (num == 1) {
        return product_of_twos;
    }
    if (boost::multiprecision::gcd(num, den) != 1) {
        return 0;
    }

    // (4) Otherwise, the "numerator" and "denominator" are now odd
    // positive coprime integers, so we can flip the symbol
    // with quadratic reciprocity, then return to step (1)

    // (num | den) == (den | num), unless num % 4 and den %4 are both
    // 3, in which case (num | den) == (-1) * (den | num)
    int quadratic_reciprocity_factor = 1;
    if ((fmod(num, 4) == 3) && (fmod(den, 4) == 3)) {
        quadratic_reciprocity_factor = -1;
    }
    return product_of_twos * quadratic_reciprocity_factor
           * unchecked_jacobi(den, num);
}

// public interface for computing jacobi symbols.  performs checking.
int mp_jacobi(const integer_class &a, const integer_class &n)
{
    if (n < 0) {
        throw std::runtime_error("jacobi denominator must be positive");
    }
    if (n % 2 == 0) {
        throw std::runtime_error("jacobi denominator must be odd");
    }
    return unchecked_jacobi(a, n);
}

int mp_kronecker(const integer_class &a, const integer_class &n)
{
    /*
    https://en.wikipedia.org/wiki/Kronecker_symbol
    We compute the Kronecker symbol in terms of the Jacobi symbol.
    For an integer n!=0, let n = u * PRODUCT (p_i**e_i), where u = -1 or 1 and
    the p_i's and e_i's are the primes and corresponding powers
    in the prime factorization of |n|.
    Then for an integer a, the Kronecker symbol is given by
    (a | n) = (a | u) * PRODUCT [(a | p_i)**e_i]
    where
    (a | u) is -1 if u is -1 and a < 0. Otherwise it is 1.
    (a | 2) is 0 if a is even, 1 if (a mod 8) == 1 or 7, and -1 if (a mod 8) ==
    3 or 5.
    (a | p) is the Legendre symbol if p is an odd prime.

    Let j be the power of the prime 2 in n's prime factorization, and let
    m = |n|/(2**j) -- that is, m is |n| with all factors of 2 extracted.

    Notice that, if p_1 is 2, then
    PRODUCT [(a | p_i)**e_i]
    == (a | 2)**j * PRODUCT [(a | p_i)**e_i]	here the p_i's are the remaining
    (odd) prime factors
    == (a | 2)**j * (a | m), 					here (a | m) is the Jacobi
    symbol, because m>0 is odd

    Thus,
    (a | n) == (a | u) * (a | 2)**j * (a | m)	if n is even
    (a | n) == (a | u) * (a | m) 				if n is odd
    */

    if (n == 0) {
        throw std::runtime_error("second arg of Kronecker cannot be zero");
    }

    // Compute (a | u)
    int kr_a_u = 1;
    if (n.sign() == -1 && a < 0) {
        kr_a_u = -1;
    }

    // Compute m, j
    integer_class m = boost::multiprecision::abs(n);
    unsigned long j = 0;
    while (m % 2 == 0 && m != 0) { // while m is even
        m /= 2;                    // implement with shift?  faster?
        ++j;
    }

    // if n is even, compute (a | 2)**j
    int kr_a_2 = 0;
    int kr_a_2_to_j = 0;
    int a_mod_8 = fmod(a, 8).convert_to<int>();

    if (a % 2 != 0) {
        kr_a_2 = (a_mod_8 == 1 || a_mod_8 == 7) ? 1 : -1;
        kr_a_2_to_j = (kr_a_2 == -1 && (j % 2 != 0))
                          ? -1
                          : 1; //(-1)**odd == -1; (-1)**even=(1)**integer=1
    }

    if (n % 2 == 0) {
        return kr_a_u * kr_a_2_to_j * unchecked_jacobi(a, m);
    } else {
        return kr_a_u * unchecked_jacobi(a, m);
    }
}

} // SymEngine

#endif // SYMENGINE_INTEGER_CLASS == SYMENGINE_BOOSTMP


//=======================================================================
// Copyright (c)
//
// 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)
//=======================================================================
/**
 * @file knapsack_two_app_long_test.cpp
 * @brief
 * @author Piotr Wygocki
 * @version 1.0
 * @date 2013-09-20
 */

#include "test_utils/logger.hpp"
#include "test_utils/read_knapsack.hpp"
#include "test_utils/test_result_check.hpp"
#include "test_utils/get_test_dir.hpp"
#include "test_utils/system.hpp"

#include "paal/greedy/knapsack_0_1_two_app.hpp"
#include "paal/greedy/knapsack_unbounded_two_app.hpp"
#include "paal/utils/floating.hpp"
#include "paal/utils/parse_file.hpp"

#include <boost/test/unit_test.hpp>

#include <fstream>

using namespace paal;
using namespace paal::utils;

BOOST_AUTO_TEST_CASE(KnapsackTwoAppLong) {
    std::string test_dir = paal::system::get_test_data_dir("KNAPSACK");
    using paal::system::build_path;

    parse(build_path(test_dir, "cases.txt"), [&](const std::string & line, std::istream &) {
        int testId = std::stoi(line);
        LOGLN("test >>>>>>>>>>>>>>>>>>>>>>>>>>>> " << testId);

        int capacity;
        std::vector<std::pair<int, int>> sizes_values;
        std::vector<int> optimal;
        auto size = [](std::pair<int, int> object) { return object.first; }
        ;
        auto value = [](std::pair<int, int> object) { return object.second; }
        ;

        read(build_path(test_dir, "cases/"), testId, capacity, sizes_values, optimal);
        LOGLN("capacity " << capacity);
        //      LOGLN("size - value pairs ");
        //        LOG_COPY_RANGE_DEL(sizes_values, " ");
        LOGLN("");
        // KNAPSACK 0/1
        {
            std::vector<std::pair<int, int>> result;
            auto value_sum = [&](int sum, int i) {
                return sum + sizes_values[i].second;
            }
            ;
            auto opt = boost::accumulate(optimal, 0, value_sum);
            LOGLN("Knapsack 0/1");
            auto maxValue =
                knapsack_0_1_two_app(sizes_values, capacity,
                                     std::back_inserter(result), value, size);

            LOGLN("Max value " << maxValue.first << ", Total size "
                               << maxValue.second);
            //       LOG_COPY_RANGE_DEL(result, " ");
            LOGLN("");
            LOGLN("OPT");
            LOG_COPY_RANGE_DEL(optimal, " ");
            LOGLN("");

            BOOST_CHECK(maxValue.second <= capacity);
            check_result(maxValue.first, opt, 0.5, greater_equal{});
        }

        // KNAPSACK
        {
            std::vector<std::pair<int, int>> result;
            LOGLN("Knapsack unbounded");
            auto maxValue = knapsack_unbounded_two_app(
                sizes_values, capacity, std::back_inserter(result), value,
                size);

            LOGLN("Max value " << maxValue.first << ", Total size "
                               << maxValue.second);
            //         LOG_COPY_RANGE_DEL(result, " ");
            LOGLN("");
            BOOST_CHECK(maxValue.second <= capacity);
        }
    });
}

#include <iostream>

BOOST_AUTO_TEST_CASE(KnapsackTwoAppLongPerf) {
    std::srand(42);
    const int OBJECTS_NR = static_cast<int>(1e7);
    const int MAX = static_cast<int>(1e2);

    using Objects = std::vector<std::pair<int, int>>;
    Objects objects(OBJECTS_NR);

    auto rand_from_range = [ = ]() { return (std::rand() % MAX) + 1; }
    ;
    auto rand_pair = [ = ]() {
        return std::make_pair(rand_from_range(), rand_from_range());
    }
    ;
    std::generate_n(objects.begin(), OBJECTS_NR, rand_pair);

    auto total_size =
        boost::accumulate(objects, 0, [&](int sum, std::pair<int, int> p) {
        return sum + p.first;
    });

    auto size = [](std::pair<int, int> object) { return object.first; }
    ;
    auto value = [](std::pair<int, int> object) { return object.second; }
    ;

    auto capacity = total_size / 2;
    LOGLN("capacity = " << capacity);

    // KNAPSACK
    {
        Objects result;
        LOGLN("Knapsack unbounded");
        auto maxValue = knapsack_unbounded_two_app(
            objects, capacity, std::back_inserter(result), value, size);

        LOGLN("Max value " << maxValue.first << ", Total size "
                           << maxValue.second);
        BOOST_CHECK(maxValue.second <= capacity);
    }

    // KNAPSACK 0/1
    {
        Objects result;
        LOGLN("Knapsack 0/1");
        auto maxValue = knapsack_0_1_two_app(
            objects, capacity, std::back_inserter(result), value, size);

        LOGLN("Max value " << maxValue.first << ", Total size "
                           << maxValue.second);
        BOOST_CHECK(maxValue.second <= capacity);
    }
}


/*
==============================================================================
KratosULFApplication
A library based on:
Kratos
A General Purpose Software for Multi-Physics Finite Element Analysis
Version 1.0 (Released on march 05, 2007).

Copyright 2007
Pooyan Dadvand, Riccardo Rossi, Janosch Stascheit, Felix Nagel
pooyan@cimne.upc.edu
rrossi@cimne.upc.edu
- CIMNE (International Center for Numerical Methods in Engineering),
Gran Capita' s/n, 08034 Barcelona, Spain

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 condition:

Distribution of this code for  any  commercial purpose  is permissible
ONLY BY DIRECT ARRANGEMENT WITH THE COPYRIGHT OWNERS.

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.

==============================================================================
*/

//
//   Project Name:        Kratos
//   Last modified by:    $Author: anonymous $
//   Date:                $Date: 2008-04-07 09:50:09 $
//   Revision:            $Revision: 1.3 $
//
//

// System includes

// External includes
#include <boost/python.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include <boost/timer.hpp>

// Project includes
#include "includes/define.h"
#include "custom_python/add_custom_strategies_to_python.h"

#include "spaces/ublas_space.h"

//builder_and_solvers
#include "custom_strategies/builder_and_solvers/residualbased_elimination_quasiincompresible_builder_and_solver.h"

//linear solvers
#include "linear_solvers/linear_solver.h"

namespace Kratos
{

	namespace Python
	{
		using namespace boost::python;

		void  AddCustomStrategiesToPython()
		{
			typedef UblasSpace<double, CompressedMatrix, Vector> SparseSpaceType;
			typedef UblasSpace<double, Matrix, Vector> LocalSpaceType;

			typedef LinearSolver<SparseSpaceType, LocalSpaceType > LinearSolverType;

			typedef BuilderAndSolver<SparseSpaceType, LocalSpaceType, LinearSolverType> BuilderAndSolverType;
//			typedef ResidualBasedEliminationBuilderAndSolver<SparseSpaceType, LocalSpaceType, LinearSolverType> ResidualBasedEliminationBuilderAndSolverType;

			//********************************************************************
			//********************************************************************
			//typedef ResidualBasedEliminationQuasiIncompressibleBuilderAndSolver< SparseSpaceType, LocalSpaceType, LinearSolverType, 2> //ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType2D;

			typedef ResidualBasedEliminationQuasiIncompressibleBuilderAndSolver< SparseSpaceType, LocalSpaceType, LinearSolverType, 2> ResidualBasedIncompressibleBuilderType2D;

			//class_< ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType2D, boost::noncopyable>
//("ResidualBasedEliminationQuasiIncompressibleBuilderAndSolver2D", init< LinearSolverType::Pointer>() )
//
//

			class_< ResidualBasedIncompressibleBuilderType2D, bases< BuilderAndSolverType >, boost::noncopyable>
("ResidualBasedIncompressibleBuilder2D", init< LinearSolverType::Pointer>() )

			//class < ResidualBasedEliminationQuasiIncompressibleBuilderAndSolver< TSparseSpace,TDenseSpace,TLinearSolver>,
			.def("AssembleLHS", &ResidualBasedIncompressibleBuilderType2D::AssembleLHS )
			.def("AssembleRHS", &ResidualBasedIncompressibleBuilderType2D::AssembleRHS )
			.def("BuildAndSolve", &ResidualBasedIncompressibleBuilderType2D::BuildAndSolve)
			.def("SetUpDofSet", &ResidualBasedIncompressibleBuilderType2D::SetUpDofSet)
			.def("SetUpSystem", &ResidualBasedIncompressibleBuilderType2D::SetUpSystem)
			.def("ResizeAndInitializeVectors", &ResidualBasedIncompressibleBuilderType2D::ResizeAndInitializeVectors)
			.def("Build", &ResidualBasedIncompressibleBuilderType2D::Build)
			.def("ConstructMatrixStructure", &ResidualBasedIncompressibleBuilderType2D::ConstructMatrixStructure)
			.def("ConstructMatrixStructure_Mconsistent", &ResidualBasedIncompressibleBuilderType2D::ConstructMatrixStructure_Mconsistent)
			.def("ConstructMatrixStructure_DivergenceMatrixD", &ResidualBasedIncompressibleBuilderType2D::ConstructMatrixStructure_DivergenceMatrixD)
			.def("BuildAuxiliaries", &ResidualBasedIncompressibleBuilderType2D::BuildAuxiliaries)
			.def("AssembleMassMatrices", &ResidualBasedIncompressibleBuilderType2D::AssembleMassMatrices)
			.def("calc_GMinvD_prod", &ResidualBasedIncompressibleBuilderType2D::calc_GMinvD_prod)
			.def("CalculatePreconditionerDiagonalMatrix",  &ResidualBasedIncompressibleBuilderType2D::CalculatePreconditionerDiagonalMatrix)
			.def("calc_prod_precond_vec", &ResidualBasedIncompressibleBuilderType2D::calc_prod_precond_vec)
			.def("ModifyForDirichlet", &ResidualBasedIncompressibleBuilderType2D::ModifyForDirichlet)
			.def("UpdatePressures", &ResidualBasedIncompressibleBuilderType2D::UpdatePressures)
			.def("ReturnDx", &ResidualBasedIncompressibleBuilderType2D::ReturnDx)
			.def("UpdatePressuresNew", &ResidualBasedIncompressibleBuilderType2D::UpdatePressuresNew)
			.def("CalculateNodalPressureForce", &ResidualBasedIncompressibleBuilderType2D::CalculateNodalPressureForce )
			.def("ConvergenceCheck", &ResidualBasedIncompressibleBuilderType2D::ConvergenceCheck)
			;

			typedef ResidualBasedEliminationQuasiIncompressibleBuilderAndSolver< SparseSpaceType, LocalSpaceType, LinearSolverType, 3> ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D;

			class_< ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D, bases< BuilderAndSolverType >, boost::noncopyable> ("ResidualBasedEliminationQuasiIncompressibleBuilderAndSolver3D", init< LinearSolverType::Pointer>() )

			//class < ResidualBasedEliminationQuasiIncompressibleBuilderAndSolver< TSparseSpace,TDenseSpace,TLinearSolver>,
			.def("AssembleLHS", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::AssembleLHS )
			.def("AssembleRHS", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::AssembleRHS )
			.def("BuildAndSolve", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::BuildAndSolve)
			.def("SetUpDofSet", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::SetUpDofSet)
			.def("SetUpSystem", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::SetUpSystem)
			.def("ResizeAndInitializeVectors", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::ResizeAndInitializeVectors)
			.def("Build", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::Build)
			.def("ConstructMatrixStructure", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::ConstructMatrixStructure)
			.def("ConstructMatrixStructure_Mconsistent", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::ConstructMatrixStructure_Mconsistent)
			.def("ConstructMatrixStructure_DivergenceMatrixD", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::ConstructMatrixStructure_DivergenceMatrixD)
			.def("BuildAuxiliaries", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::BuildAuxiliaries)
			.def("AssembleMassMatrices", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::AssembleMassMatrices)
			.def("calc_GMinvD_prod", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::calc_GMinvD_prod)
			.def("CalculatePreconditionerDiagonalMatrix",  &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::CalculatePreconditionerDiagonalMatrix)
			.def("calc_prod_precond_vec", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::calc_prod_precond_vec)
			.def("ModifyForDirichlet", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::ModifyForDirichlet)
			.def("UpdatePressures", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::UpdatePressures)
			.def("ReturnDx", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::ReturnDx)
			.def("UpdatePressuresNew", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::UpdatePressuresNew)
			.def("CalculateNodalPressureForce", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::CalculateNodalPressureForce )
			.def("ConvergenceCheck", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::ConvergenceCheck)
			;
			//********************************************************************
			//********************************************************************

		}

	}  // namespace Python.

} // Namespace Kratos


#include <PRM.h>
#include <iostream>
#include <Eigen/Dense>
#include <vector>
#include <cstdlib>
#include <ctime>
#include <fstream>

using namespace std;
using namespace Eigen;

double random(double start, double end)
{
    return start+(end-start)*rand()/(RAND_MAX + 1.0);
}

int main(int argc, char *argv[]) {
    uint N = 10;
    if (argc > 1) {
        N = atoi(argv[1]);
    }
    cout << "n = " << N << endl;

    srand(unsigned(time(0)));
    PRM prm(N);

    cout << endl;
    for(uint i = 0; i < N; ++i) {
        for(uint j = 0; j < prm.edge[i].size(); ++j) {
            cout << prm.vertex[i].transpose() << " " << prm.vertex[prm.edge[i][j]].transpose() << endl;
        }
        // cout << prm.vertex[i].transpose() << " " << prm.vertex[i].transpose() << endl;
    }

    // Vector3d source(random(0,10), random(0,10), random(0,10));
    // Vector3d goal(random(90,100), random(90,100), random(90,100));

    // Vector3d source(random(0,10), random(0,10), 0);
    // Vector3d goal(random(90,100), random(90,100), 0);

    // Vector3d source(0,1,0);
    // Vector3d goal(1,3,0);

    // cout << "==================" << endl;
    // cout << source.transpose() << endl;
    // prm.SearchPathConstrain(source, goal);
    // cout << goal.transpose() << endl;

    // cout << "==================" << endl;
    // cout << source.transpose() << endl;
    // prm.SearchPath(source, goal);
    // cout << goal.transpose() << endl;

    vector<int> sources;
    vector<int> goals;

    // sources.push_back(0);
    // sources.push_back(4);

    // goals.push_back(5);
    // goals.push_back(1);

    for(int i = 0; i < 50; ++i) {
        sources.push_back(i);
        goals.push_back(i + 50);
    }
    vector< vector<int>> paths = prm.ComflictBasedSearch(sources, goals);

    // for(uint i = 0; i < paths.size(); ++i) {
    //     cout << "=========== " << i << "  ====" << endl;
    //     for(auto j = paths[i].begin(); j!=paths[i].end(); ++j) {
    //         // cout << *j << endl;
    //         cout << prm.vertex[*j].transpose() << endl;
    //     }
    //     cout << endl;
    // }

    ofstream fout("output.txt");
    for(uint i = 0; i < paths.size(); ++i) {
        fout << "#" << endl;
        for(auto j = paths[i].begin(); j!=paths[i].end(); ++j) {
            fout << prm.vertex[*j](0) << ',' <<
                    prm.vertex[*j](1) << ',' <<
                    prm.vertex[*j](2) <<
                    endl;
        }
    }
    fout.close();
    // // int rgba[3] = {0x05, 0x19, 0x37};
    // // int rgbb[3] = {0xa8, 0xeb, 0x12};
    // int rgba[3] = {0x84, 0x5e, 0xc2};
    // int rgbb[3] = {0xf9, 0xf8, 0x71};

    // int step[3];
    // for(int i = 0; i < 3; ++i) {
    //     step[i] = ((rgbb[i] - rgba[i] + 255) % 255) / 5;
    //     cout << step[i] << endl;
    // }
    // for(int j = 0; j < 6; ++j) {
    //     for(int i = 0; i < 3; ++i) {
    //         cout << hex;
    //         cout << rgba[i] + j * step[i] << ',';
    //     }
    //     cout << endl;
    // }

    return 1;
}