/*
* Copyright 2019 © Centre Interdisciplinaire de développement en Cartographie des Océans (CIDCO), Tous droits réservés
*/

#ifndef SYSTEMGEOMETRY_H
#define SYSTEMGEOMETRY_H

#include <Eigen/Dense>
#include <string>
#include <iostream>
#include <fstream>
#include <sstream>
#include "utils/Constants.hpp"
#include "utils/Exception.hpp"
#include "Attitude.hpp"

/*!
* \brief Survey system class
* \author Guillaume Labbe-Morissette, Jordan McManus
* \date August 20, 2018, 1:07 PM
*/
class SurveySystem {
public:

  /**Creates a Survey system*/
  SurveySystem();

  /**Destroys the Survey system*/
  ~SurveySystem();

  /**Returns the MBES model*/
  std::string & getMBES_model() {
    return MBES_model;
  }

  /**Returns the antenna position*/
  Eigen::Vector3d & getAntennaPosition() {
    return antennaPosition;
  }

  /**Returns the attitude accuracy*/
  Attitude * getAttitudeAccuracy() {
    return attitudeAccuracy;
  }

  /**Returns the boresigth patch test*/
  Attitude * getBoresightPatchTest() {
    return boresightPatchTest;
  }

  /**Returns the draft*/
  double getDraft() {
    return draft;
  }

  /**Returns the echo sounder receiver position*/
  Eigen::Vector3d & getEchosounderReceivererPosition() {
    return echosounderReceivererPosition;
  }

  /**Returns the echo sounder transmitter position*/
  Eigen::Vector3d & getEchosounderTransmitterPosition() {
    return echosounderTransmitterPosition;
  }

  /**Returns the position accuracy*/
  Eigen::Vector3d & getPositionAccuracy() {
    return positionAccuracy;
  }

  /**
  * Change the Survey system values by reading a file
  * Return false if the file is not valid
  *
  * @param filename name of the file that will be read
  */
  bool readFile(const std::string & fileName);

private:

  /**Name of the Survey system MBES model*/
  std::string MBES_model;

  /**Value of the Survey system draft (meter)*/
  double draft; // in meters

  // positions in the IMU reference frame in meters

  /**Vector3d of the Survey system antenna position*/
  Eigen::Vector3d antennaPosition;

  /**Vector3d of the Survey system echo sounder transmitter position*/
  Eigen::Vector3d echosounderTransmitterPosition;

  /**Vector3d of the Survey system echo sounder receiver position*/
  Eigen::Vector3d echosounderReceivererPosition;

  // angles in degrees
  // will be converted internally to radians
  /**Value of the Survey system boresigth path test (Attitude)*/
  Attitude* boresightPatchTest = NULL;

  // accuracy in 2 sigma
  /**Value of the Survey system attitude accuracy (Attitude)*/
  Attitude* attitudeAccuracy = NULL;

  /**Vector3d of the Survey system position accuracy*/
  Eigen::Vector3d positionAccuracy;
};

SurveySystem::SurveySystem() {
}

SurveySystem::~SurveySystem() {
  if (boresightPatchTest) {
    delete boresightPatchTest;
  }

  if (attitudeAccuracy) {
    delete attitudeAccuracy;
  }
}

bool SurveySystem::readFile(const std::string & fileName) {

  // Temporary placeholder variables
  std::string NameDevice;
  double Patch_Roll = 0, Patch_Pitch = 0, Patch_Heading = 0;
  double MBES_X = 0, MBES_Y = 0, MBES_Z = 0;
  double MBES_RX = 0, MBES_RY = 0, MBES_RZ = 0;
  double AntX = 0, AntY = 0, AntZ = 0;
  double Draft = 0;

  double PitchRollAcc, HeadingAcc;
  double PosHorAcc, PosVerAcc;

  //double Eroll = 0, Epitch = 0, Eyaw = 0, Rroll = 0, Rpitch = 0, Ryaw = 0, Mroll = 0, Mpitch = 0, Myaw = 0;

  // Open file
  std::string line;
  std::ifstream file(fileName);

  /*
  * This is cancer, no validation of correctness or presence of data
  * TODO:
  * Should return false if data is missing or is invalid
  */
  if (!file) {
    return false;
  } else {
    for (std::string line; getline(file, line);) {
      //make a stream for the line itself
      std::istringstream in(line);

      std::string type;
      in >> type;

      if (type == "MultibeamModel") {
        in >> NameDevice;
      } else if (type == "AntennaPositionOffsetX") {
        in >> AntX;
      } else if (type == "AntennaPositionOffsetY") {
        in >> AntY;
      } else if (type == "AntennaPositionOffsetZ") {
        in >> AntZ;
      } else if (type == "MBETransmitterOffsetX") {
        in >> MBES_X;
      } else if (type == "MBETransmitterOffsetY") {
        in >> MBES_Y;
      } else if (type == "MBETransmitterOffsetZ") {
        in >> MBES_Z;
      } else if (type == "MBEDraft") {
        in >> Draft;
      } else if (type == "MBEReceiverOffsetX") {
        in >> MBES_RX;
      } else if (type == "MBEReceiverOffsetY") {
        in >> MBES_RY;
      } else if (type == "MBEReceiverOffsetZ") {
        in >> MBES_RZ;
      } else if (type == "PositionAccuracy") {
        in >> PosHorAcc;
        PosVerAcc = PosHorAcc * 1.5;
      } else if (type == "PitchRollAccuracy") {
        in >> PitchRollAcc;
      } else if (type == "HeadingAccuracy") {
        in >> HeadingAcc;
      } else if (type == "RollAlignment") {
        in >> Patch_Roll;
      } else if (type == "PitchAlignment") {
        in >> Patch_Pitch;
      } else if (type == "HeadingAlignment") {
        in >> Patch_Heading;
      }/* else if (type == "MBEOffsetR") {
        in >> Eroll;
      } else if (type == "MBEOffsetP") {
      in >> Epitch;
    } else if (type == "MBEOffsetH") {
    in >> Eyaw;
  } else if (type == "MBEOffset2R") {
  in >> Rroll;
} else if (type == "MBEOffset2P") {
in >> Rpitch;
} else if (type == "MBEOffset2H") {
in >> Ryaw;
} else if (type == "MotionSensorR") {
in >> Mroll;
} else if (type == "MotionSensorP") {
in >> Mpitch;
} else if (type == "MotionSensorH") {
in >> Myaw;
}*/
}

file.close();

MBES_model = NameDevice;

draft = Draft;

antennaPosition << AntX, AntY, -AntZ;
echosounderTransmitterPosition << MBES_X, MBES_Y, -MBES_Z;
echosounderReceivererPosition << MBES_RX, MBES_RY, -MBES_RZ;

boresightPatchTest = new Attitude(0,Patch_Roll, Patch_Pitch, Patch_Heading);

attitudeAccuracy = new Attitude(0,PitchRollAcc, PitchRollAcc, HeadingAcc);

positionAccuracy << PosHorAcc, PosHorAcc, PosVerAcc;

return true;
}
}

#endif /* SYSTEMGEOMETRY_H */


#include <iostream>
#include <fstream>
#include <vector>
#include <sstream>
#include <iomanip>

#ifndef WIN64
    #define EIGEN_DONT_ALIGN_STATICALLY
#endif
#include <Eigen/Dense>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include "dvo.hpp"
#include "tum_benchmark.hpp"

#define STR1(x)  #x
#define STR(x)  STR1(x)

std::vector<Eigen::Matrix4f> PoseEst(const std::string dataFolder, std::vector<std::string> &filesColor, std::vector<std::string> &filesDepth,  std::vector<double> &timestampsDepth,
    std::vector<double> &timestampsColor, Eigen::Matrix3f K, std::vector<Eigen::Matrix4f> &initial_poses)
{

//     Eigen::Matrix3f K;
// #if 1
//     // initialize intrinsic matrix: fr1
//     K <<    517.3, 0.0, 318.6,
//             0.0, 516.5, 255.3,
//             0.0, 0.0, 1.0;
    //dataFolder = "/rgbd_dataset_freiburg1_xyz/";
//     //dataFolder = "/work/maierr/rgbd_data/rgbd_dataset_freiburg1_desk2/";
// #else
//     dataFolder = "/work/maierr/rgbd_data/rgbd_dataset_freiburg3_long_office_household/";
//     // initialize intrinsic matrix: fr3
//     K <<    535.4, 0.0, 320.1,
//             0.0, 539.2, 247.6,
//             0.0, 0.0, 1.0;
// #endif
    //std::cout << "Camera matrix: " << K << std::endl;

    // load file names

    //std::string dataFile_depth = dataFolder + "depth.txt";
    //std::string dataFile_color = dataFolder + "rgb.txt";

    //test

    int numFrames = filesDepth.size();

    //int maxFrames = -1;
    int maxFrames = 30;

    Eigen::Matrix4f absPose = Eigen::Matrix4f::Identity(); // for image 1
    std::vector<Eigen::Matrix4f> poses;
    std::vector<double> timestamps;
    poses.push_back(absPose);
    timestamps.push_back(timestampsDepth[0]);

    cv::Mat grayRef = loadGray(dataFolder + filesColor[0]);
    cv::Mat depthRef = loadDepth(dataFolder + filesDepth[0]);
    int w = depthRef.cols;
    int h = depthRef.rows;

    DVO dvo;
    dvo.init(w, h, K);

    std::vector<cv::Mat> grayRefPyramid;
    std::vector<cv::Mat> depthRefPyramid;
    dvo.buildPyramid(depthRef, grayRef, depthRefPyramid, grayRefPyramid);

    // process frames
    double runtimeAvg = 0.0;
    int framesProcessed = 0;
    for (size_t i = 1; (i < numFrames && i < maxFrames); ++i)
    {
        std::cout << "aligning frames " << (i-1) << " and " << i  << std::endl;

        // load input frame
        std::string fileColor1 = filesColor[i];
        std::string fileDepth1 = filesDepth[i];
        double timeDepth1 = timestampsDepth[i];
        //std::cout << "File " << i << ": " << fileColor1 << ", " << fileDepth1 << std::endl;
        cv::Mat grayCur = loadGray(dataFolder + fileColor1);
        cv::Mat depthCur = loadDepth(dataFolder + fileDepth1);
        // build pyramid
        std::vector<cv::Mat> grayCurPyramid;
        std::vector<cv::Mat> depthCurPyramid;
        dvo.buildPyramid(depthCur, grayCur, depthCurPyramid, grayCurPyramid);

        // frame alignment
        double tmr = (double)cv::getTickCount();

        //Eigen::Matrix4f relPose = Eigen::Matrix4f::Identity();
        //Eigen::Matrix4f relPose = initial_poses[i].inverse()*initial_poses[i-1];
        Eigen::Matrix4f relPose = initial_poses[i].inverse();
        if (relPose == Eigen::Matrix4f::Identity() & i > 0){
            relPose = absPose.inverse();
        }
        std::cout << "current initial pose is: " << std::endl;
        std::cout << relPose << std::endl;
        dvo.align(depthRefPyramid, grayRefPyramid, depthCurPyramid, grayCurPyramid, relPose);

        tmr = ((double)cv::getTickCount() - tmr)/cv::getTickFrequency();
        runtimeAvg += tmr;

        // concatenate poses
        //absPose = absPose * relPose.inverse();
        absPose = relPose.inverse();

        poses.push_back(absPose);
        timestamps.push_back(timeDepth1);

        //depthRefPyramid = depthCurPyramid;
        //grayRefPyramid = grayCurPyramid;

        // std::cout << "pose for " << framesProcessed << " image: " << absPose << std::endl;

        ++framesProcessed;
    }
    std::cout << "average runtime: " << (runtimeAvg / framesProcessed) * 1000.0 << " ms" << std::endl;

    // save poses
    savePoses(dataFolder + "traj.txt", poses, timestamps);

    // clean up
    //cv::destroyAllWindows();
    std::cout << "Direct Image Alignment finished." << std::endl;

    return poses;
}

 std::vector<Eigen::Matrix4f> ImageAlign(const std::string dataFolder, Eigen::Matrix3f K)
{
    std::string assocFile = dataFolder + "assocFile.txt";
    std::string trajFile = dataFolder + "initial_traj.txt";

    std::vector<std::string> filesColor;
    std::vector<std::string> filesDepth;
    std::vector<double> timestampsDepth;
    std::vector<double> timestampsColor;
    std::vector<Eigen::Matrix4f> initial_poses;

    if (!loadAssoc(assocFile, filesDepth, filesColor, timestampsDepth, timestampsColor))
    {
        std::cout << "Assoc file could not be loaded!" << std::endl;
        //return NULL;
    }

    if (!loadPoses(trajFile, initial_poses))
    {
        std::cout << "Initialize poses as Identity matrix!" << std::endl;
        int numFrames = filesDepth.size();
        Eigen::Matrix4f absPose = Eigen::Matrix4f::Identity();
        for (size_t i = 1; (i < (numFrames+1) ); ++i)
            {
                initial_poses.push_back(absPose);
            }

    }

    if (filesDepth.size()>initial_poses.size())
    {
        int numFrames = filesDepth.size();
        int numPoses = initial_poses.size();
        Eigen::Matrix4f absPose = Eigen::Matrix4f::Identity();
        for (size_t i = 1; (i<( numFrames - numPoses + 1) ); ++i)
            {
                initial_poses.push_back(absPose);
            }
    }

    std::vector<Eigen::Matrix4f> results;
    results = PoseEst(dataFolder, filesColor, filesDepth, timestampsDepth, timestampsColor, K, initial_poses);
    //std::cout << results[1].size() << std::endl;

    return results;

}

int main(int argc, const char *argv[])
{

    std::string dataFolder = argv[1];
    Eigen::Matrix3f K;
    for (int i=0; i<9; i++) {
        K(i/3, i%3) = atof(argv[i+2]);
    }

    std::cout << dataFolder << std::endl;

    std::vector<Eigen::Matrix4f> results;
    results = ImageAlign(dataFolder, K);
       return 0;
}


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

#include <boost/multiprecision/cpp_int.hpp>

struct E
{
   E(boost::multiprecision::cpp_rational const&)
   {
   }
};

void g(boost::multiprecision::cpp_rational const& r)
{
   std::cout << r << std::endl;
}

int main()
{
#if !defined(BOOST_MP_NO_CXX11_EXPLICIT_CONVERSION_OPERATORS) && !BOOST_WORKAROUND(BOOST_MSVC, < 1900) && !(defined(__APPLE_CC__) && defined(CI_SUPPRESS_KNOWN_ISSUES))
   boost::multiprecision::cpp_int r = 3;
   g(r);
   E x1(r); // triggers explicit conversion operator.
#endif
   return 0;
}


#define PY_ARRAY_UNIQUE_SYMBOL phist_PyArray_API
#define NO_IMPORT_ARRAY

#include <string>
#include <cmath>

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

#include <boost/array.hpp>

#include <vigra/numpy_array.hxx>
#include <vigra/numpy_array_converters.hxx>

#include "seglib/histogram/histogram.hxx"
#include "seglib/histogram/histogram_python.hxx"

#include <cmath>

namespace python = boost::python;

namespace histogram {

    void export_histogram_gradient(){

        /*
        python::def("_label_histogram_",vigra::registerConverters(&labelHistogram),
            (
                python::arg("img"),
                python::arg("sigma"),
                python::arg("r"),
                python::arg("outX")=python::object()
                python::arg("outY")=python::object()
            )
        );
        */
    }

}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <array>
#include <boost/functional/hash.hpp>  // IWYU pragma: keep
#include <cstddef>
#include <limits>
#include <string>
#include <type_traits>
#include <unordered_map>
#include <utility>

#include "DataStructures/DataBox/DataBoxTag.hpp"
#include "DataStructures/Tensor/Tensor.hpp"
#include "Domain/Element.hpp"  // IWYU pragma: keep
#include "ErrorHandling/Assert.hpp"
#include "NumericalAlgorithms/LinearOperators/MeanValue.hpp"
#include "Options/Options.hpp"
#include "Utilities/ForceInline.hpp"
#include "Utilities/Gsl.hpp"
#include "Utilities/MakeArray.hpp"
#include "Utilities/TMPL.hpp"
#include "Utilities/TaggedTuple.hpp"

/// \cond
class DataVector;
template <size_t VolumeDim>
class Direction;
template <size_t VolumeDim>
class ElementId;
template <size_t VolumeDim>
class Mesh;
template <size_t VolumeDim>
class OrientationMap;

namespace PUP {
class er;
}  // namespace PUP

namespace SlopeLimiters {
template <size_t VolumeDim, typename TagsToLimit>
class Minmod;
}  // namespace SlopeLimiters

namespace Tags {
template <size_t Dim, typename Frame>
struct Coordinates;
template <size_t VolumeDim>
struct Element;
template <size_t VolumeDim>
struct Mesh;
template <size_t VolumeDim>
struct SizeOfElement;
}  // namespace Tags
/// \endcond

namespace SlopeLimiters {
/// \ingroup SlopeLimitersGroup
/// \brief Possible types of the minmod slope limiter.
///
/// \see SlopeLimiters::Minmod
enum class MinmodType { LambdaPi1, LambdaPiN, Muscl };
}  // namespace SlopeLimiters

namespace Minmod_detail {
// Encodes the return status of the minmod_tvbm function.
struct MinmodResult {
  const double value;
  const bool activated;
};

// The TVBM-corrected minmod function, see e.g. Cockburn reference Eq. 2.26.
MinmodResult minmod_tvbm(double a, double b, double c,
                         double tvbm_scale) noexcept;

// Implements the minmod limiter for one Tensor<DataVector>.
//
// The interface is designed to erase the tensor structure information, because
// this way the implementation can be moved out of the header file. This is
// achieved by receiving Tensor<DataVector>::iterators into the tensor to limit,
// and Tensor<double>::iterators into the neighbor tensors.
//
// Note: because the interface erases the tensor structure information, we can
// no longer rely on the compiler to enforce that the local and neighbor tensors
// share the same Structure.
template <size_t VolumeDim>
bool limit_one_tensor(
    gsl::not_null<DataVector*> tensor_begin,
    gsl::not_null<DataVector*> tensor_end,
    const SlopeLimiters::MinmodType& minmod_type, double tvbm_constant,
    const Element<VolumeDim>& element, const Mesh<VolumeDim>& mesh,
    const tnsr::I<DataVector, VolumeDim, Frame::Logical>& logical_coords,
    const std::array<double, VolumeDim>& element_size,
    const std::unordered_map<
        std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>,
        gsl::not_null<const double*>,
        boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>&
        neighbor_tensor_begin,
    const std::unordered_map<
        std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>,
        std::array<double, VolumeDim>,
        boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>&
        neighbor_sizes) noexcept;

template <typename Tag>
struct to_tensor_double : db::PrefixTag, db::SimpleTag {
  using type = TensorMetafunctions::swap_type<double, db::item_type<Tag>>;
  using tag = Tag;
  static std::string name() noexcept {
    return "TensorDouble(" + Tag::name() + ")";
  }
};
}  // namespace Minmod_detail

namespace SlopeLimiters {
/// \ingroup SlopeLimitersGroup
/// \brief A generic Minmod slope limiter
///
/// Implements the minmod-based slope limiter from
/// \ref cockburn_ref "Cockburn (1999)", Section 2.4.
/// Three types of minmod limiter from the reference are implemented:
/// \f$\Lambda\Pi^1\f$, \f$\Lambda\Pi^N\f$, and MUSCL.
///
/// This minmod limiter has a generic implementation that can work on an
/// arbitrary set of tensors. The minmod limiting algorithm is applied to each
/// component of each tensor independently. In general, the limiter linearizes
/// the tensors on every DG element, each time it is applied; additionally, the
/// limiter may reduce the spatial slope of some tensor components if the data
/// look like they may contain oscillations.
///
/// The key features differentiating the three minmod limiter types are:
/// 1. The `Muscl` limiter is the most dissipative; it more aggressively reduces
///    the slopes of the data. This limiter may better handle strong shocks, but
///    also produces the most broadening of features.
/// 2. The `LambdaPiN` limiter is the least aggressive; its "troubled cell"
///    detector tries to avoid limiting in DG elements where the data look
///    smooth enough. Where `LambdaPiN` is able to avoid limiting, the data are
///    _not_ linearized, and the post-limiter data are identical to the
///    pre-limiter data.
/// 3. The `LambdaPi1` limiter is a middle-ground option between the other two.
///    It does not try to avoid limiting as much as `LambdaPiN`, but it allows
///    larger slopes in the data than `Muscl`.
///
/// For all three types of minmod limiter the "total variation bound in the
/// means" (TVBM) correction is implemented, enabling the limiter to avoid
/// limiting away smooth extrema in the solution that would otherwise look like
/// spurious oscillations. The limiter will not reduce the slope (but will still
/// linearize) on elements where the slope is less than \f$m h^2\f$, where
/// \f$m\f$ is the TVBM constant and \f$h\f$ is the size of the DG element.
///
/// The limiter acts in the `Frame::Logical` coordinates, because in these
/// coordinates it is straightforward to formulate the algorithm. This means the
/// limiter can operate on generic deformed grids. However, if the grid is too
/// strongly deformed, some things can start to break down:
/// 1. When an element is deformed so that the Jacobian (from `Frame::Logical`
///    to `Frame::Inertial`) varies across the element, then the limiter fails
///    to be conservative. In other words, the integral of a tensor `u` over the
///    element will change after the limiter activates on `u`. This error is
///    typically small.
/// 2. When there is a sudden change in the size of the elements (perhaps at an
///    h-refinement boundary, or at the boundary between two blocks with very
///    different mappings), a smooth solution in `Frame::Inertial` can appear
///    to have a kink in `Frame::Logical`. The Minmod implementation includes
///    some (untested) tweaks that try to reduce spurious limiter activations
///    near these fake kinks.
///
/// When an element has multiple neighbors in any direction, an effective mean
/// and neighbor size in this direction are computed by averaging over the
/// multiple neighbors. This simple generalization of the minmod limiter enables
/// it to operate on h-refined grids.
///
/// \tparam VolumeDim The number of spatial dimensions.
/// \tparam Tags A typelist of tags specifying the tensors to limit.
///
/// \anchor cockburn_ref [1] B. Cockburn,
/// Discontinuous Galerkin Methods for Convection-Dominated Problems,
/// [Springer (1999)](https://doi.org/10.1007/978-3-662-03882-6_2)
template <size_t VolumeDim, typename... Tags>
class Minmod<VolumeDim, tmpl::list<Tags...>> {
 public:
  /// \brief The MinmodType
  ///
  /// One of `SlopeLimiters::MinmodType`. See `SlopeLimiters::Minmod`
  /// documentation for details.
  struct Type {
    using type = MinmodType;
    static constexpr OptionString help = {"Type of minmod"};
  };
  /// \brief The TVBM constant
  ///
  /// See `SlopeLimiters::Minmod` documentation for details.
  struct TvbmConstant {
    using type = double;
    static type default_value() { return 0.0; }
    static type lower_bound() { return 0.0; }
    static constexpr OptionString help = {"TVBM constant 'm'"};
  };
  using options = tmpl::list<Type, TvbmConstant>;
  static constexpr OptionString help = {
      "A minmod-based slope limiter.\n"
      "The different types of minmod are more or less aggressive in trying\n"
      "to reduce slopes. The TVBM correction allows the limiter to ignore\n"
      "'small' slopes, and helps to avoid limiting of smooth extrema in the\n"
      "solution.\n"};

  /// \brief Constuct a Minmod slope limiter
  ///
  /// \param minmod_type The type of Minmod slope limiter.
  /// \param tvbm_constant The value of the TVBM constant (default: 0).
  explicit Minmod(const MinmodType minmod_type,
                  const double tvbm_constant = 0.0) noexcept
      : minmod_type_(minmod_type), tvbm_constant_(tvbm_constant) {
    ASSERT(tvbm_constant >= 0.0, "The TVBM constant must be non-negative.");
  }

  Minmod() noexcept = default;
  Minmod(const Minmod& /*rhs*/) = default;
  Minmod& operator=(const Minmod& /*rhs*/) = default;
  Minmod(Minmod&& /*rhs*/) noexcept = default;
  Minmod& operator=(Minmod&& /*rhs*/) noexcept = default;
  ~Minmod() = default;

  // clang-tidy: google-runtime-references
  void pup(PUP::er& p) noexcept {  // NOLINT
    p | minmod_type_;
    p | tvbm_constant_;
  }

  const MinmodType& minmod_type() const noexcept { return minmod_type_; }
  const double& tvbm_constant() const noexcept { return tvbm_constant_; }

  /// \brief Data to send to neighbor elements.
  struct PackagedData {
    tuples::TaggedTuple<Minmod_detail::to_tensor_double<Tags>...> means_;
    std::array<double, VolumeDim> element_size_ =
        make_array<VolumeDim>(std::numeric_limits<double>::signaling_NaN());

    // clang-tidy: google-runtime-references
    void pup(PUP::er& p) noexcept {  // NOLINT
      p | means_;
      p | element_size_;
    }
  };

  using package_argument_tags = tmpl::list<Tags..., ::Tags::Mesh<VolumeDim>,
                                           ::Tags::SizeOfElement<VolumeDim>>;

  /// \brief Package data for sending to neighbor elements.
  ///
  /// The following quantities are stored in `PackagedData` and communicated
  /// between neighboring elements:
  /// - the cell-averaged mean of each tensor component, and
  /// - the size of the cell along each logical coordinate direction.
  ///
  /// \param packaged_data The data package to fill with this element's values.
  /// \param tensors The tensors to be averaged and packaged.
  /// \param mesh The mesh on which the tensor values are measured.
  /// \param element_size The size of the element in inertial coordinates, along
  ///        each dimension of logical coordinates.
  /// \param orientation_map The orientation of the neighbor
  void package_data(const gsl::not_null<PackagedData*>& packaged_data,
                    const db::item_type<Tags>&... tensors,
                    const Mesh<VolumeDim>& mesh,
                    const std::array<double, VolumeDim>& element_size,
                    const OrientationMap<VolumeDim>& orientation_map) const
      noexcept {
    const auto wrap_compute_means =
        [&mesh, &packaged_data ](auto tag, const auto& tensor) noexcept {
      for (size_t i = 0; i < tensor.size(); ++i) {
        // Compute the mean using the local orientation of the tensor and mesh:
        // this avoids the work of reorienting the tensor while giving the same
        // result.
        get<Minmod_detail::to_tensor_double<decltype(tag)>>(
            packaged_data->means_)[i] = mean_value(tensor[i], mesh);
      }
      return '0';
    };
    expand_pack(wrap_compute_means(Tags{}, tensors)...);
    packaged_data->element_size_ =
        orientation_map.permute_from_neighbor(element_size);
  }

  using limit_tags = tmpl::list<Tags...>;
  using limit_argument_tags =
      tmpl::list<::Tags::Element<VolumeDim>, ::Tags::Mesh<VolumeDim>,
                 ::Tags::Coordinates<VolumeDim, Frame::Logical>,
                 ::Tags::SizeOfElement<VolumeDim>>;

  /// \brief Limits the solution on the element.
  ///
  /// For each component of each tensor, the limiter will (in general) linearize
  /// the data, then possibly reduce its slope, dimension-by-dimension, until it
  /// no longer looks oscillatory.
  ///
  /// \param tensors The tensors to be limited.
  /// \param element The element on which the tensors to limit live.
  /// \param mesh The mesh on which the tensor values are measured.
  /// \param logical_coords The logical coordinates of the mesh gridpoints.
  /// \param element_size The size of the element, in the inertial coordinates.
  /// \param neighbor_data The data from each neighbor.
  ///
  /// \return whether the limiter modified the solution or not.
  ///
  /// \note The return value is false if the limiter knows it has not modified
  /// the solution. True return values can indicate:
  /// - The solution was limited to reduce the slope, whether by a large factor
  ///   or by a factor only roundoff away from unity.
  /// - The solution was linearized but not limited.
  /// - The solution is identical to the input, if the input was a linear
  ///   function on a higher-order mesh, so that the limiter cannot know that
  ///   the linearization step did not actually modify the data. This is
  ///   somewhat contrived and is unlikely to occur outside of code tests or
  ///   test cases with very clean initial data.
  bool operator()(
      const gsl::not_null<std::add_pointer_t<db::item_type<Tags>>>... tensors,
      const Element<VolumeDim>& element, const Mesh<VolumeDim>& mesh,
      const tnsr::I<DataVector, VolumeDim, Frame::Logical>& logical_coords,
      const std::array<double, VolumeDim>& element_size,
      const std::unordered_map<
          std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>, PackagedData,
          boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>&
          neighbor_data) const noexcept {
    bool limiter_activated = false;
    const auto wrap_limit_one_tensor = [
      this, &limiter_activated, &element, &mesh, &logical_coords, &element_size,
      &neighbor_data
    ](auto tag, const auto& tensor) noexcept {
      // Because we hide the types of Tags from limit_one_tensor (we do this so
      // that its implementation isn't templated on Tags and can be moved out of
      // this header file), we cannot pass it PackagedData as currently
      // implemented. So we unpack everything from PackagedData. In the future
      // we may want a PackagedData type that erases types inherently, as this
      // would avoid the need for unpacking as done here.
      //
      // Get iterators into the local and neighbor tensors, because these are
      // independent from the structure of the tensor being limited.
      const auto tensor_begin = make_not_null(tensor->begin());
      const auto tensor_end = make_not_null(tensor->end());
      const auto neighbor_tensor_begin = [&neighbor_data]() noexcept {
        std::unordered_map<
            std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>,
            gsl::not_null<const double*>,
            boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>
            result;
        for (const auto& neighbor_and_data : neighbor_data) {
          result.insert(std::make_pair(
              neighbor_and_data.first,
              make_not_null(get<Minmod_detail::to_tensor_double<decltype(tag)>>(
                                neighbor_and_data.second.means_)
                                .cbegin())));
        }
        return result;
      }
      ();
      const auto neighbor_sizes = [&neighbor_data]() noexcept {
        std::unordered_map<
            std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>,
            std::array<double, VolumeDim>,
            boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>
            result;
        for (const auto& neighbor_and_data : neighbor_data) {
          result.insert(std::make_pair(neighbor_and_data.first,
                                       neighbor_and_data.second.element_size_));
        }
        return result;
      }
      ();

      limiter_activated =
          Minmod_detail::limit_one_tensor<VolumeDim>(
              tensor_begin, tensor_end, minmod_type_, tvbm_constant_, element,
              mesh, logical_coords, element_size, neighbor_tensor_begin,
              neighbor_sizes) or
          limiter_activated;
      return '0';
    };
    expand_pack(wrap_limit_one_tensor(Tags{}, tensors)...);
    return limiter_activated;
  }

 private:
  MinmodType minmod_type_;
  double tvbm_constant_;
};

template <size_t VolumeDim, typename TagList>
SPECTRE_ALWAYS_INLINE bool operator==(
    const Minmod<VolumeDim, TagList>& lhs,
    const Minmod<VolumeDim, TagList>& rhs) noexcept {
  return lhs.minmod_type() == rhs.minmod_type() and
         lhs.tvbm_constant() == rhs.tvbm_constant();
}

template <size_t VolumeDim, typename TagList>
SPECTRE_ALWAYS_INLINE bool operator!=(
    const Minmod<VolumeDim, TagList>& lhs,
    const Minmod<VolumeDim, TagList>& rhs) noexcept {
  return not(lhs == rhs);
}

}  // namespace SlopeLimiters

template <>
struct create_from_yaml<SlopeLimiters::MinmodType> {
  static SlopeLimiters::MinmodType create(const Option& options);
};


#include "tag_filter_rule.hpp"
#include "tag_filter_context.hpp"
#include "lua_context.hpp"

#include <boost/format.hpp>

using namespace std ;

namespace tag_filter {

Literal::Literal(const std::string &val, bool auto_conv)
{
    if ( auto_conv ) {
        char * e;
        double x = std::strtod(val.c_str(), &e);

        if (*e != 0 ||  errno != 0 )  {
            type_ = String ;
            string_val_ = val ;
        }
        else
        {
            type_ = Number ;
            number_val_ = x ;
        }
    }
    else {
        type_ = String ;
        string_val_ = val ;
    }

}

bool Literal::toBoolean() const {

    switch ( type_ ) {
    case Null: return false ;
    case Boolean: return boolean_val_ ;
    case Number: return number_val_ != 0.0 ;
    case String: return ( !string_val_.empty()) ;
    }
}

double Literal::toNumber() const {

    switch ( type_ ) {
    case Null: return 0 ;
    case Boolean: return (double)boolean_val_ ;
    case Number: return number_val_ ;
    case String: return atof(string_val_.c_str()) ;
    }
}

string Literal::toString() const {
    switch ( type_ ) {
    case Null: return "" ;
    case Boolean: ( boolean_val_ ) ? "TRUE" : "FALSE" ;
    case Number: return str(boost::format("%f") % number_val_) ;
    case String: return string_val_ ;
    }
}

Literal BooleanOperator::eval(TagFilterContext &ctx)
{
    switch ( op_ ) {
    case And:
        return ( lhs_->eval(ctx).toBoolean() && rhs_->eval(ctx).toBoolean() ) ;
    case Or:
        return ( lhs_->eval(ctx).toBoolean() || rhs_->eval(ctx).toBoolean() ) ;
    case Not: //?
        return !( lhs_->eval(ctx).toBoolean() ) ;
    }
}

Literal ComparisonPredicate::eval(TagFilterContext &ctx)
{
    Literal lhs = lhs_->eval(ctx) ;
    Literal rhs = rhs_->eval(ctx) ;

    if ( lhs.isNull() || rhs.isNull() ) return false ;

    switch ( op_ ) {
    case Equal:
    {
        if ( lhs.type_ == Literal::String && lhs.type_ == Literal::String )
            return Literal(lhs.string_val_ == rhs.string_val_) ;
        else return Literal(lhs.toNumber() == rhs.toNumber()) ;
    }
    case NotEqual:
        if ( lhs.type_ == Literal::String && lhs.type_ == Literal::String )
            return Literal(lhs.string_val_ != rhs.string_val_) ;
        else return Literal(lhs.toNumber() != rhs.toNumber()) ;
    case Less:
        return lhs.toNumber() < rhs.toNumber() ;
    case Greater:
        return lhs.toNumber() > rhs.toNumber() ;
    case LessOrEqual:
        return lhs.toNumber() <= rhs.toNumber() ;
    case GreaterOrEqual:
        return lhs.toNumber() >= rhs.toNumber() ;
    }

    return Literal() ;
}
static string globToRegex(const char *pat)
{
    // Convert pattern
    string rx = "(?i)^", be ;

    int i = 0;
    const char *pc = pat ;
    int clen = strlen(pat) ;
    bool inCharClass = false ;

    while (i < clen)
    {
        char c = pc[i++];

        switch (c)
        {
        case '*':
            rx += "[^\\\\/]*" ;
            break;
        case '?':
            rx += "[^\\\\/]" ;
            break;
        case '$':  //Regex special characters
        case '(':
        case ')':
        case '+':
        case '.':
        case '|':
            rx += '\\';
            rx += c;
            break;
        case '\\':
            if ( pc[i] == '*' ) rx += "\\*" ;
            else if ( pc[i] == '?' )  rx += "\\?" ;
            ++i ;
            break ;
        case '[':
        {
            if ( inCharClass )  rx += "\\[";
            else {
                inCharClass = true ;
                be += c ;
            }
            break ;
        }
        case ']':
        {
            if ( inCharClass ) {
                inCharClass = false ;
                rx += be ;
                rx += c ;
                rx += "{1}" ;
                be.clear() ;
            }
            else rx += "\\]" ;

            break ;
        }
        case '%':
        {
            boost::regex rd("(0\\d)?d") ;
            boost::smatch what;

            if ( boost::regex_match(std::string(pat + i), what, rd,  boost::match_extra) )
            {

                rx += "[[:digit:]]" ;

                if ( what.size() == 2 )
                {
                    rx +=  "{" ;
                    rx += what[1] ;
                    rx += "}" ;
                }
                else
                    rx += "+" ;

                i += what.size() ;
            }
            else
            {
                if ( inCharClass ) be += c ;
                else rx += c;
            }
            break ;

        }
        default:
            if ( inCharClass ) be += c ;
            else rx += c;
        }
    }

    rx += "$" ;
    return rx ;
}

LikeTextPredicate::LikeTextPredicate(ExpressionNodePtr op, const std::string &pattern, bool is_pos):
    exp_(op), is_pos_(is_pos)
{
    if ( !pattern.empty() )
        pattern_ = boost::regex(globToRegex(pattern.c_str())) ;

}

Literal LikeTextPredicate::eval(TagFilterContext &ctx)
{
    Literal op = exp_->eval(ctx) ;

    return boost::regex_match(op.toString(), pattern_) ;

}

ListPredicate::ListPredicate(const string &id, const std::deque<ExpressionNodePtr> &op, bool is_pos):
    id_(id), children_(op.begin(), op.end()), is_pos_(is_pos)
{
    TagFilterContext ctx ;

    for(int i=0 ; i<children_.size() ; i++)
    {
        string lval = children_[i]->eval(ctx).toString() ;
        lvals_.push_back(lval) ;
    }

}

Literal ListPredicate::eval(TagFilterContext &ctx)
{
    if ( !ctx.has_tag(id_) ) return Literal() ;

    string val = ctx.value(id_) ;

    for(int i=0 ; i<lvals_.size() ; i++)
        if ( val == lvals_[i] ) return is_pos_ ;

    return !is_pos_ ;
}

/*
Literal IsTypePredicate::eval(TagFilterContext &ctx)
{
    if ( keyword_ == "node")
    {
        return ctx.type() == TagFilterContext::Node ;
    }
    else if ( keyword_ == "way" )
    {
        return ctx.type() == TagFilterContext::Way ;
    }

}
*/

Literal Attribute::eval(TagFilterContext &ctx)
{
    if ( !ctx.has_tag(name_) ) return Literal() ;

    return ctx.value(name_) ;
}

Literal BinaryOperator::eval(TagFilterContext &ctx)
{
    Literal op1 = lhs_->eval(ctx) ;
    Literal op2 = rhs_->eval(ctx) ;

    if ( op_ == '+' )
    {
        if ( op1.type_ == Literal::String && op2.type_ == Literal::String )
            return op1.toString() + op2.toString() ;
        else return op1.toNumber() + op2.toNumber() ;
    }
    else if ( op_ == '-' )
    {
        return op1.toNumber() + op2.toNumber() ;
    }
    else if ( op_ == '.' )
    {
        return op1.toString() + op2.toString() ;
    }
    else if ( op_ == '*' )
    {
        return op1.toNumber() + op2.toNumber() ;
    }
    else if ( op_ == '/' ) {
        if ( op2.toNumber() == 0.0 ) return Literal() ;
        else return op1.toNumber()/op2.toNumber() ;
    }

}

Literal UnaryPredicate::eval(TagFilterContext &ctx)
{
    return exp_->eval(ctx).toBoolean() ;
}

Literal Function::eval(TagFilterContext &ctx)
{
    if ( name_ == "is_poi" ) {
        return ( ctx.type() == TagFilterContext::Node )  ;
    }
    else if ( name_ == "is_way" ) {
        return ctx.type() == TagFilterContext::Way ;
    }
    else if ( name_ == "is_relation" ) {
        return ctx.type() == TagFilterContext::Relation ;
    }
    else {
        vector<Literal> vals ;
        for(uint i=0 ; i<args_.size() ; i++)
            vals.push_back(args_[i]->eval(ctx)) ;
        return lua_->call(name_, vals) ;
    }

}
}


// Boost.Geometry

// Copyright (c) 2020-2021, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_UTIL_TYPE_TRAITS_HPP
#define BOOST_GEOMETRY_UTIL_TYPE_TRAITS_HPP

#include <boost/geometry/core/tag.hpp>
#include <boost/geometry/core/tags.hpp>
#include <boost/geometry/util/type_traits_std.hpp>

namespace boost { namespace geometry
{

namespace util
{

template <typename T>
struct is_geometry
    : bool_constant<! std::is_void<typename tag<T>::type>::value>
{};

template <typename T>
struct is_not_geometry
    : std::is_void<typename tag<T>::type>
{};

template <typename T>
struct is_point
    : std::is_same<point_tag, typename tag<T>::type>
{};

template <typename T>
struct is_multi_point
    : std::is_same<multi_point_tag, typename tag<T>::type>
{};

template <typename T>
struct is_pointlike
    : std::is_base_of<pointlike_tag, typename tag<T>::type>
{};

template <typename T>
struct is_segment
    : std::is_same<segment_tag, typename tag<T>::type>
{};

template <typename T>
struct is_linestring
    : std::is_same<linestring_tag, typename tag<T>::type>
{};

template <typename T>
struct is_multi_linestring
    : std::is_same<multi_linestring_tag, typename tag<T>::type>
{};

template <typename T>
struct is_polylinear
    : std::is_base_of<polylinear_tag, typename tag<T>::type>
{};

template <typename T>
struct is_linear
    : std::is_base_of<linear_tag, typename tag<T>::type>
{};

template <typename T>
struct is_box
    : std::is_same<box_tag, typename tag<T>::type>
{};

template <typename T>
struct is_ring
    : std::is_same<ring_tag, typename tag<T>::type>
{};

template <typename T>
struct is_polygon
    : std::is_same<polygon_tag, typename tag<T>::type>
{};

template <typename T>
struct is_multi_polygon
    : std::is_same<multi_polygon_tag, typename tag<T>::type>
{};

template <typename T>
struct is_polygonal
    : std::is_base_of<polygonal_tag, typename tag<T>::type>
{};

template <typename T>
struct is_areal
    : std::is_base_of<areal_tag, typename tag<T>::type>
{};

template <typename T>
struct is_segmental
    : bool_constant<is_linear<T>::value || is_polygonal<T>::value>
{};

template <typename T>
struct is_polysegmental
    : bool_constant<is_polylinear<T>::value || is_polygonal<T>::value>
{};

template <typename T>
struct is_multi
    : std::is_base_of<multi_tag, typename tag<T>::type>
{};

template <typename T>
struct is_multi_element
    : bool_constant<is_point<T>::value || is_linestring<T>::value || is_polygon<T>::value>
{};

template <typename T>
struct is_single
    : std::is_base_of<single_tag, typename tag<T>::type>
{};

template <typename T>
struct is_geometry_collection
    : std::is_same<geometry_collection_tag, typename tag<T>::type>
{};

template <typename T>
struct is_dynamic_geometry
    : std::is_same<dynamic_geometry_tag, typename tag<T>::type>
{};

template <typename Geometry, typename T = void>
struct enable_if_point
    : std::enable_if<is_point<Geometry>::value, T>
{};

template <typename Geometry, typename T = void>
using enable_if_point_t = typename enable_if_point<Geometry, T>::type;

template <typename Geometry, typename T = void>
struct enable_if_multi_point
    : std::enable_if<is_multi_point<Geometry>::value, T>
{};

template <typename Geometry, typename T = void>
using enable_if_multi_point_t = typename enable_if_multi_point<Geometry, T>::type;

template <typename Geometry, typename T = void>
struct enable_if_pointlike
    : std::enable_if<is_pointlike<Geometry>::value, T>
{};

template <typename Geometry, typename T = void>
using enable_if_pointlike_t = typename enable_if_pointlike<Geometry, T>::type;

template <typename Geometry, typename T = void>
struct enable_if_segment
    : std::enable_if<is_segment<Geometry>::value, T>
{};

template <typename Geometry, typename T = void>
using enable_if_segment_t = typename enable_if_segment<Geometry, T>::type;

template <typename Geometry, typename T = void>
struct enable_if_linestring
    : std::enable_if<is_linestring<Geometry>::value, T>
{};

template <typename Geometry, typename T = void>
using enable_if_linestring_t = typename enable_if_linestring<Geometry, T>::type;

template <typename Geometry, typename T = void>
struct enable_if_multi_linestring
    : std::enable_if<is_multi_linestring<Geometry>::value, T>
{};

template <typename Geometry, typename T = void>
using enable_if_multi_linestring_t = typename enable_if_multi_linestring<Geometry, T>::type;

template <typename Geometry, typename T = void>
struct enable_if_polylinear
    : std::enable_if<is_polylinear<Geometry>::value, T>
{};

template <typename Geometry, typename T = void>
using enable_if_polylinear_t = typename enable_if_polylinear<Geometry, T>::type;

template <typename Geometry, typename T = void>
struct enable_if_linear
    : std::enable_if<is_linear<Geometry>::value, T>
{};

template <typename Geometry, typename T = void>
using enable_if_linear_t = typename enable_if_linear<Geometry, T>::type;

template <typename Geometry, typename T = void>
struct enable_if_box
    : std::enable_if<is_box<Geometry>::value, T>
{};

template <typename Geometry, typename T = void>
using enable_if_box_t = typename enable_if_box<Geometry, T>::type;

template <typename Geometry, typename T = void>
struct enable_if_ring
    : std::enable_if<is_ring<Geometry>::value, T>
{};

template <typename Geometry, typename T = void>
using enable_if_ring_t = typename enable_if_ring<Geometry, T>::type;

template <typename Geometry, typename T = void>
struct enable_if_polygon
    : std::enable_if<is_polygon<Geometry>::value, T>
{};

template <typename Geometry, typename T = void>
using enable_if_polygon_t = typename enable_if_polygon<Geometry, T>::type;

template <typename Geometry, typename T = void>
struct enable_if_multi_polygon
    : std::enable_if<is_multi_polygon<Geometry>::value, T>
{};

template <typename Geometry, typename T = void>
using enable_if_multi_polygon_t = typename enable_if_multi_polygon<Geometry, T>::type;

template <typename Geometry, typename T = void>
struct enable_if_polygonal
    : std::enable_if<is_polygonal<Geometry>::value, T>
{};

template <typename Geometry, typename T = void>
using enable_if_polygonal_t = typename enable_if_polygonal<Geometry, T>::type;

template <typename Geometry, typename T = void>
struct enable_if_areal
    : std::enable_if<is_areal<Geometry>::value, T>
{};

template <typename Geometry, typename T = void>
using enable_if_areal_t = typename enable_if_areal<Geometry, T>::type;

template <typename Geometry, typename T = void>
struct enable_if_polysegmental
    : std::enable_if<is_polysegmental<Geometry>::value, T>
{};

template <typename Geometry, typename T = void>
using enable_if_polysegmental_t = typename enable_if_polysegmental<Geometry, T>::type;

template <typename Geometry, typename T = void>
struct enable_if_dynamic_geometry
    : std::enable_if<is_dynamic_geometry<Geometry>::value, T>
{};

template <typename Geometry, typename T = void>
using enable_if_dynamic_geometry_t = typename enable_if_dynamic_geometry<Geometry, T>::type;

template <typename Geometry, typename T = void>
struct enable_if_geometry_collection
    : std::enable_if<is_geometry_collection<Geometry>::value, T>
{};

template <typename Geometry, typename T = void>
using enable_if_geometry_collection_t = typename enable_if_geometry_collection<Geometry, T>::type;

} // namespace util

// Deprecated utilities, defined for backward compatibility but might be
// removed in the future.

/*!
    \brief Meta-function defining "true" for areal types (box, (multi)polygon, ring),
    \note Used for tag dispatching and meta-function finetuning
    \note Also a "ring" has areal properties within Boost.Geometry
    \ingroup core
*/
using util::is_areal;

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_DETAIL_HPP


// Boost.Polygon library voronoi_ctypes_test.cpp file

//          Copyright Andrii Sydorchuk 2010-2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

// See http://www.boost.org for updates, documentation, and revision history.

#include <ctime>
#include <vector>

#define BOOST_TEST_MODULE voronoi_ctypes_test
#include <boost/mpl/list.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/test/test_case_template.hpp>

#include <boost/polygon/detail/voronoi_ctypes.hpp>
using namespace boost::polygon::detail;

type_converter_fpt to_fpt;

BOOST_AUTO_TEST_CASE(ulp_comparison_test1) {
  ulp_comparison<double> ulp_cmp;
  uint64 a = 22;
  uint64 b = 27;
  fpt64 da, db;
  std::memcpy(&da, &a, sizeof(uint64));
  std::memcpy(&db, &b, sizeof(uint64));
  BOOST_CHECK_EQUAL(ulp_cmp(da, db, 1), ulp_cmp.LESS);
  BOOST_CHECK_EQUAL(ulp_cmp(db, da, 1), ulp_cmp.MORE);
  BOOST_CHECK_EQUAL(ulp_cmp(da, db, 4), ulp_cmp.LESS);
  BOOST_CHECK_EQUAL(ulp_cmp(da, db, 5), ulp_cmp.EQUAL);
  BOOST_CHECK_EQUAL(ulp_cmp(da, db, 6), ulp_cmp.EQUAL);
}

BOOST_AUTO_TEST_CASE(ulp_comparison_test2) {
  ulp_comparison<fpt64> ulp_cmp;
  uint64 a = 0ULL;
  uint64 b = 0x8000000000000002ULL;
  fpt64 da, db;
  std::memcpy(&da, &a, sizeof(uint64));
  std::memcpy(&db, &b, sizeof(uint64));
  BOOST_CHECK_EQUAL(ulp_cmp(da, db, 1), ulp_cmp.MORE);
  BOOST_CHECK_EQUAL(ulp_cmp(db, da, 1), ulp_cmp.LESS);
  BOOST_CHECK_EQUAL(ulp_cmp(da, db, 2), ulp_cmp.EQUAL);
  BOOST_CHECK_EQUAL(ulp_cmp(da, db, 3), ulp_cmp.EQUAL);
}

BOOST_AUTO_TEST_CASE(extended_exponent_fpt_test1) {
  boost::mt19937_64 gen(static_cast<uint32>(time(NULL)));
  fpt64 b = 0.0;
  efpt64 eeb(b);
  for (int i = 0; i < 1000; ++i) {
    fpt64 a = to_fpt(static_cast<int64>(gen()));
    efpt64 eea(a);
    efpt64 neg = -eea;
    efpt64 sum = eea + eeb;
    efpt64 dif = eea - eeb;
    efpt64 mul = eea * eeb;
    BOOST_CHECK_EQUAL(to_fpt(neg), -a);
    BOOST_CHECK_EQUAL(to_fpt(sum), a + b);
    BOOST_CHECK_EQUAL(to_fpt(dif), a - b);
    BOOST_CHECK_EQUAL(to_fpt(mul), a * b);
  }
}

BOOST_AUTO_TEST_CASE(extended_exponent_fpt_test2) {
  boost::mt19937_64 gen(static_cast<uint32>(time(NULL)));
  fpt64 a = 0.0;
  efpt64 eea(a);
  for (int i = 0; i < 1000; ++i) {
    fpt64 b = to_fpt(static_cast<int64>(gen()));
    if (b == 0.0)
      continue;
    efpt64 eeb(b);
    efpt64 neg = -eea;
    efpt64 sum = eea + eeb;
    efpt64 dif = eea - eeb;
    efpt64 mul = eea * eeb;
    efpt64 div = eea / eeb;
    BOOST_CHECK_EQUAL(to_fpt(neg), -a);
    BOOST_CHECK_EQUAL(to_fpt(sum), a + b);
    BOOST_CHECK_EQUAL(to_fpt(dif), a - b);
    BOOST_CHECK_EQUAL(to_fpt(mul), a * b);
    BOOST_CHECK_EQUAL(to_fpt(div), a / b);
  }
}

BOOST_AUTO_TEST_CASE(extended_exponent_fpt_test3) {
  boost::mt19937_64 gen(static_cast<uint32>(time(NULL)));
  for (int i = 0; i < 1000; ++i) {
    fpt64 a = to_fpt(static_cast<int64>(gen()));
    fpt64 b = to_fpt(static_cast<int64>(gen()));
    if (b == 0.0)
      continue;
    efpt64 eea(a);
    efpt64 eeb(b);
    efpt64 neg = -eea;
    efpt64 sum = eea + eeb;
    efpt64 dif = eea - eeb;
    efpt64 mul = eea * eeb;
    efpt64 div = eea / eeb;
    BOOST_CHECK_EQUAL(to_fpt(neg), -a);
    BOOST_CHECK_EQUAL(to_fpt(sum), a + b);
    BOOST_CHECK_EQUAL(to_fpt(dif), a - b);
    BOOST_CHECK_EQUAL(to_fpt(mul), a * b);
    BOOST_CHECK_EQUAL(to_fpt(div), a / b);
  }
}

BOOST_AUTO_TEST_CASE(extended_exponent_fpt_test4) {
  for (int exp = 0; exp < 64; ++exp)
  for (int i = 1; i < 100; ++i) {
    fpt64 a = i;
    fpt64 b = to_fpt(1LL << exp);
    efpt64 eea(a);
    efpt64 eeb(b);
    efpt64 neg = -eea;
    efpt64 sum = eea + eeb;
    efpt64 dif = eea - eeb;
    efpt64 mul = eea * eeb;
    efpt64 div = eea / eeb;
    BOOST_CHECK_EQUAL(to_fpt(neg), -a);
    BOOST_CHECK_EQUAL(to_fpt(sum), a + b);
    BOOST_CHECK_EQUAL(to_fpt(dif), a - b);
    BOOST_CHECK_EQUAL(to_fpt(mul), a * b);
    BOOST_CHECK_EQUAL(to_fpt(div), a / b);
  }
}

BOOST_AUTO_TEST_CASE(extended_exponent_fpt_test5) {
  for (int i = 0; i < 100; ++i) {
    efpt64 a(to_fpt(i * i));
    efpt64 b = a.sqrt();
    BOOST_CHECK_EQUAL(to_fpt(b), to_fpt(i));
  }
}

BOOST_AUTO_TEST_CASE(extended_exponent_fpt_test6) {
  for (int i = -10; i <= 10; ++i) {
    efpt64 a(to_fpt(i));
    BOOST_CHECK_EQUAL(is_pos(a), i > 0);
    BOOST_CHECK_EQUAL(is_neg(a), i < 0);
    BOOST_CHECK_EQUAL(is_zero(a), !i);
  }
}

BOOST_AUTO_TEST_CASE(extended_int_test1) {
  typedef extended_int<1> eint32;
  eint32 e1(0), e2(32), e3(-32);
  BOOST_CHECK_EQUAL(e1.count(), 0);
  BOOST_CHECK_EQUAL(e1.size(), 0U);
  BOOST_CHECK_EQUAL(e2.count(), 1);
  BOOST_CHECK_EQUAL(e2.chunks()[0], 32U);
  BOOST_CHECK_EQUAL(e2.size(), 1U);
  BOOST_CHECK_EQUAL(e3.count(), -1);
  BOOST_CHECK_EQUAL(e3.chunks()[0], 32U);
  BOOST_CHECK_EQUAL(e3.size(), 1U);
}

BOOST_AUTO_TEST_CASE(extended_int_test2) {
  typedef extended_int<2> eint64;
  int64 val64 = 0x7fffffffffffffffLL;
  eint64 e1(0), e2(32), e3(-32), e4(val64), e5(-val64);
  BOOST_CHECK_EQUAL(e1.count(), 0);
  BOOST_CHECK_EQUAL(e2.count(), 1);
  BOOST_CHECK_EQUAL(e2.chunks()[0], 32U);
  BOOST_CHECK_EQUAL(e3.count(), -1);
  BOOST_CHECK_EQUAL(e3.chunks()[0], 32U);
  BOOST_CHECK_EQUAL(e4.count(), 2);
  BOOST_CHECK_EQUAL(e4.chunks()[0], 0xffffffff);
  BOOST_CHECK_EQUAL(e4.chunks()[1], val64 >> 32);
  BOOST_CHECK_EQUAL(e5.count(), -2);
  BOOST_CHECK_EQUAL(e5.chunks()[0], 0xffffffff);
  BOOST_CHECK_EQUAL(e5.chunks()[1], val64 >> 32);
}

BOOST_AUTO_TEST_CASE(extended_int_test3) {
  typedef extended_int<2> eint64;
  std::vector<uint32> chunks;
  chunks.push_back(1);
  chunks.push_back(2);
  eint64 e1(chunks, true), e2(chunks, false);
  BOOST_CHECK_EQUAL(e1.count(), 2);
  BOOST_CHECK_EQUAL(e1.chunks()[0], 2U);
  BOOST_CHECK_EQUAL(e1.chunks()[1], 1U);
  BOOST_CHECK_EQUAL(e2.count(), -2);
  BOOST_CHECK_EQUAL(e2.chunks()[0], 2U);
  BOOST_CHECK_EQUAL(e2.chunks()[1], 1U);
}

BOOST_AUTO_TEST_CASE(extended_int_test4) {
  typedef extended_int<2> eint64;
  std::vector<uint32> chunks;
  chunks.push_back(1);
  chunks.push_back(2);
  eint64 e1(chunks, true), e2(chunks, false);
  BOOST_CHECK_EQUAL(e1 == e2, false);
  BOOST_CHECK_EQUAL(e1 == -e2, true);
  BOOST_CHECK_EQUAL(e1 != e2, true);
  BOOST_CHECK_EQUAL(e1 != -e2, false);
  BOOST_CHECK_EQUAL(e1 < e2, false);
  BOOST_CHECK_EQUAL(e1 < -e2, false);
  BOOST_CHECK_EQUAL(e1 <= e2, false);
  BOOST_CHECK_EQUAL(e1 <= -e2, true);
  BOOST_CHECK_EQUAL(e1 > e2, true);
  BOOST_CHECK_EQUAL(e1 > -e2, false);
  BOOST_CHECK_EQUAL(e1 >= e2, true);
  BOOST_CHECK_EQUAL(e1 >= -e2, true);
}

BOOST_AUTO_TEST_CASE(extended_int_test5) {
  typedef extended_int<2> eint64;
  boost::mt19937_64 gen(static_cast<uint32>(time(NULL)));
  for (int i = 0; i < 1000; ++i) {
    int64 i1 = static_cast<int64>(gen());
    int64 i2 = static_cast<int64>(gen());
    eint64 e1(i1), e2(i2);
    BOOST_CHECK_EQUAL(e1 == e2, i1 == i2);
    BOOST_CHECK_EQUAL(e1 != e2, i1 != i2);
    BOOST_CHECK_EQUAL(e1 > e2, i1 > i2);
    BOOST_CHECK_EQUAL(e1 >= e2, i1 >= i2);
    BOOST_CHECK_EQUAL(e1 < e2, i1 < i2);
    BOOST_CHECK_EQUAL(e1 <= e2, i1 <= i2);
  }
}

BOOST_AUTO_TEST_CASE(extended_int_test6) {
  typedef extended_int<1> eint32;
  eint32 e1(32);
  eint32 e2 = -e1;
  BOOST_CHECK_EQUAL(e2.count(), -1);
  BOOST_CHECK_EQUAL(e2.size(), 1U);
  BOOST_CHECK_EQUAL(e2.chunks()[0], 32U);
}

BOOST_AUTO_TEST_CASE(extended_int_test7) {
  typedef extended_int<2> eint64;
  boost::mt19937_64 gen(static_cast<uint32>(time(NULL)));
  for (int i = 0; i < 1000; ++i) {
    int64 i1 = static_cast<int64>(gen()) >> 2;
    int64 i2 = static_cast<int64>(gen()) >> 2;
    eint64 e1(i1), e2(i2), e3(i1 + i2), e4(i1 - i2);
    BOOST_CHECK(e1 + e2 == e3);
    BOOST_CHECK(e1 - e2 == e4);
  }
}

BOOST_AUTO_TEST_CASE(extended_int_test8) {
  typedef extended_int<2> eint64;
  boost::mt19937 gen(static_cast<uint32>(time(NULL)));
  for (int i = 0; i < 1000; ++i) {
    int64 i1 = static_cast<int32>(gen());
    int64 i2 = static_cast<int32>(gen());
    eint64 e1(i1), e2(i2), e3(i1 * i2);
    BOOST_CHECK(e1 * e2 == e3);
  }
}

BOOST_AUTO_TEST_CASE(exnteded_int_test9) {
  typedef extended_int<1> eint32;
  for (int i = -10; i <= 10; ++i) {
    for (int j = -10; j <= 10; ++j) {
      eint32 e1(i), e2(j), e3(i+j), e4(i-j), e5(i*j);
      BOOST_CHECK(e1 + e2 == e3);
      BOOST_CHECK(e1 - e2 == e4);
      BOOST_CHECK(e1 * e2 == e5);
    }
  }
}

BOOST_AUTO_TEST_CASE(extended_int_test10) {
  typedef extended_int<2> eint64;
  boost::mt19937_64 gen(static_cast<uint32>(time(NULL)));
  for (int i = 0; i < 100; ++i) {
    int64 i1 = static_cast<int64>(gen()) >> 20;
    int64 i2 = i1 >> 32;
    eint64 e1(i1), e2(i2);
    BOOST_CHECK(to_fpt(e1) == static_cast<fpt64>(i1));
    BOOST_CHECK(to_fpt(e2) == static_cast<fpt64>(i2));
  }
}

BOOST_AUTO_TEST_CASE(extened_int_test11) {
  typedef extended_int<1> eint32;
  typedef extended_int<64> eint2048;
  eint2048 two(2), value(1);
  for (int i = 0; i < 1024; ++i)
    value = value * two;
  BOOST_CHECK_EQUAL(value.count(), 33);
  for (std::size_t i = 1; i < value.size(); ++i)
    BOOST_CHECK_EQUAL(value.chunks()[i-1], 0U);
  BOOST_CHECK_EQUAL(value.chunks()[32], 1U);
}


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

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

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

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

 /*!

    @ingroup group-predicates
    Function object implementing lt capabilities

    Returns True< or False according x is less than y or not.
   Infix notation can be used with operator '<'.

    This is a convenient alias of @ref is_less
  **/
  const boost::dispatch::functor<tag::lt_> lt = {};
} }
#endif

#include <boost/simd/function/scalar/is_less.hpp>
#include <boost/simd/function/simd/lt.hpp>

#endif


///////////////////////////////////////////////////////////////////////////////
// range_ex::chain::detail::result_of_nest.hpp                               //
//                                                                           //
//  Copyright 2008 Erwann Rogard. 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_STATISTICS_DETAIL_RANGE_EX_CHAIN_DETAIL_RESULT_OF_NEST_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_RANGE_EX_CHAIN_DETAIL_RESULT_OF_NEST_HPP_ER_2010
#include <boost/type_traits.hpp>

#include <boost/fusion/include/back.hpp>
#include <boost/fusion/include/pop_back.hpp>
#include <boost/fusion/include/size.hpp>

#include <boost/statistics/detail/range_ex/chain/detail/nest.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace range{
namespace impl{
namespace result_of{

    // Case size(seq) > 1
    template<typename Seq>
    struct nest_chain : range::impl::nest_chain<
        typename boost::remove_cv<
            typename boost::remove_reference<
                typename boost::fusion::result_of::pop_back<Seq>::type
            >::type
        >::type
        ,
        typename boost::remove_cv<
            typename boost::remove_reference<
                typename boost::fusion::result_of::back<Seq>::type
            >::type
        >::type
    >{};

}// result_of
}// impl
}// range
}// detail
}// statistics
}// boost

#endif


/*
 * Software License Agreement (Apache License)
 *
 * Copyright (c) 2014, Southwest Research Institute
 *
 * 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 <ros/ros.h>
#include <ros/package.h>
#include <ros/console.h>
#include <boost/foreach.hpp>

using std::string;

typedef struct point3d
{
  double x, y, z;
} Point3d;

int main(int argc, char** argv)
{
  ros::init(argc, argv, "target_locator_service");
  ros::NodeHandle node_handle;
  ros::NodeHandle pnh("~");
  int rows, cols;
  double diameter, spacing;
  std::string target_name;
  std::string target_frame;
  std::string target_file_path;
  std::string transform_interface;
  int target_type;

  if (!pnh.getParam("target_rows", rows))
  {
    ROS_ERROR("Must set param:  target_rows");
  }
  if (!pnh.getParam("target_cols", cols))
  {
    ROS_ERROR("Must set param:  target_cols");
  }
  if (!pnh.getParam("target_circle_dia", diameter))
  {
    ROS_ERROR("Must set param:  target_circle_dia");
  }
  if (!pnh.getParam("target_spacing", spacing))
  {
    ROS_ERROR("Must set param:  target_spacing");
  }
  if (!pnh.getParam("target_type", target_type))
  {
    ROS_ERROR("Must set param:  target_type");
  }
  if (!pnh.getParam("target_name", target_name))
  {
    ROS_ERROR("Must set param:  target_name");
  }
  if (!pnh.getParam("target_frame", target_frame))
  {
    ROS_ERROR("Must set param:  target_frame");
  }
  if (!pnh.getParam("target_file_path", target_file_path))
  {
    ROS_ERROR("Must set param:  target_file_path");
  }
  if (!pnh.getParam("transform_interface", transform_interface))
  {
    ROS_ERROR("Must set param:  transform_interface");
  }
  // generate points
  std::vector<Point3d> pts;
  pts.clear();
  for (int i = 0; i < rows; i++)
  {
    for (int j = 0; j < cols; j++)
    {
      Point3d point;
      point.x = j * spacing;
      point.y = (rows - 1 - i) * spacing;
      point.z = 0.0;
      pts.push_back(point);
    }
  }

  // write target
  FILE* fp;
  if ((fp = fopen(target_file_path.c_str(), "w")) != NULL)
  {
    fprintf(fp, "---\n\n");
    fprintf(fp, "static_targets:\n");
    fprintf(fp, "-\n");
    fprintf(fp, "     target_name: %s\n", target_name.c_str());
    fprintf(fp, "     target_type: %d\n", target_type);
    fprintf(fp, "     circle_dia: %f\n", diameter);
    fprintf(fp, "     target_frame: %s\n", target_frame.c_str());
    fprintf(fp, "     transform_interface: %s\n", transform_interface.c_str());
    fprintf(fp, "     xyz_aaxis_pose: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]\n");
    fprintf(fp, "     target_rows: %d\n", rows);
    fprintf(fp, "     target_cols: %d\n", cols);
    fprintf(fp, "     num_points: %d\n", (int)pts.size());
    fprintf(fp, "     points: \n");
    BOOST_FOREACH (Point3d pt, pts)
    {
      fprintf(fp, "     - pnt: [ %7.4lf,  %7.4lf,  %7.4lf]\n", pt.x, pt.y, pt.z);
    }
    fclose(fp);
  }
  else
  {
    ROS_ERROR("Could not open target file %s", target_file_path.c_str());
  }
  return 1;
}


#include <boost/icl/interval_map.hpp>


#include <boost/math/special_functions/next.hpp>
#include <boost/random.hpp>

#include <limits>

#include "caffe/common.hpp"
#include "caffe/util/math_functions.hpp"
#include "caffe/util/rng.hpp"

namespace caffe {

}  // namespace caffe


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

#include <angles/angles.h>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>

#include <collvoid_local_planner/ROSAgent.h>

#include <collvoid_local_planner/orca.h>
#include <collvoid_local_planner/collvoid_publishers.h>

namespace collvoid {

    tf::TransformListener *tf_;
    std::string base_frame_, global_frame_, odom_frame_;
    std::vector <std::pair<double, geometry_msgs::PoseStamped>> pose_array_weighted_;
    double footprint_radius_, radius_, cur_loc_unc_radius_;
    std::vector <Vector2> minkowski_footprint_;
    geometry_msgs::PolygonStamped footprint_msg_;
    double eps_;

    void computeNewLocUncertainty();

    void computeNewMinkowskiFootprint();

    void amclPoseArrayWeightedCallback(const amcl::PoseArrayWeighted::ConstPtr &msg);

    double CalcArea(std::vector < Vector2 > list);

    bool compareConvexHullPointsPosition(const ConvexHullPoint &p1, const ConvexHullPoint &p2) {
        return collvoid::absSqr(p1.point) <= collvoid::absSqr(p2.point);
    }

    bool comparePoint(const Vector2 &p1, const Vector2 &p2) {
        return collvoid::absSqr(p1) <= collvoid::absSqr(p2);
    }

    void computeNewLocUncertainty() {
        std::vector <ConvexHullPoint> points;

        for (int i = 0; i < (int) pose_array_weighted_.size(); i++) {
            ConvexHullPoint p;
            p.point = Vector2(pose_array_weighted_[i].second.pose.position.x,
                              pose_array_weighted_[i].second.pose.position.y);
            p.weight = pose_array_weighted_[i].first;
            p.index = i;
            p.orig_index = i;
            points.push_back(p);
        }

        std::sort(points.begin(), points.end(), compareConvexHullPointsPosition);

        // ROS_ERROR("points in list = %d", (int)points.size());
        // double testSum = 0;
        // for (int i = 0; i<(int)points.size(); i++) {
        //   testSum += points[i].weight;
        //   //ROS_ERROR("%f, weight = %f", abs(points[i].point), points[i].weight);
        // }
        // ROS_ERROR("%f testSUm ", testSum);

        double sum = 0.0;
        int j = 0;
        while (sum <= 1.0 - eps_ && j < (int) points.size()) {
            sum += points[j].weight;
            j++;
        }
        // ROS_ERROR("CALU points %d", j);
        // for (int i = 0; i<j; i++) {
        //   ROS_ERROR("%f", abs(points[i].point));
        // }
        cur_loc_unc_radius_ = collvoid::abs(points[j - 1].point);
        //ROS_ERROR("Loc Uncertainty = %f", cur_loc_unc_radius_);
        //radius_ = footprint_radius_ + cur_loc_unc_radius_;
    }

    void computeNewMinkowskiFootprint() {
        bool done = false;
        std::vector <ConvexHullPoint> convex_hull;
        std::vector <ConvexHullPoint> points;
        points.clear();

        for (int i = 0; i < (int) pose_array_weighted_.size(); i++) {
            ConvexHullPoint p;
            p.point = Vector2(pose_array_weighted_[i].second.pose.position.x,
                              pose_array_weighted_[i].second.pose.position.y);
            p.weight = pose_array_weighted_[i].first;
            p.index = i;
            p.orig_index = i;
            points.push_back(p);
        }
        std::sort(points.begin(), points.end(), compareVectorsLexigraphically);
        for (int i = 0; i < (int) points.size(); i++) {
            points[i].index = i;
        }

        double total_sum = 0;
        std::vector <int> skip_list;

        while (!done && points.size() >= 3) {
            double sum = 0;
            convex_hull.clear();
            convex_hull = convexHull(points, true);

            skip_list.clear();
            for (int j = 0; j < (int) convex_hull.size() - 1; j++) {
                skip_list.push_back(convex_hull[j].index);
                sum += convex_hull[j].weight;
            }
            total_sum += sum;

            //ROS_WARN("SUM = %f (%f), num Particles = %d, eps = %f", sum, total_sum, (int) points.size(), eps_);

            if (total_sum >= eps_) {
                done = true;
                break;
            }

            std::sort(skip_list.begin(), skip_list.end());
            for (int i = (int) skip_list.size() - 1; i >= 0; i--) {
                points.erase(points.begin() + skip_list[i]);
            }

            for (int i = 0; i < (int) points.size(); i++) {
                points[i].index = i;
            }
        }

        std::vector <Vector2> localization_footprint, own_footprint;
        for (int i = 0; i < (int) convex_hull.size(); i++) {
            localization_footprint.push_back(convex_hull[i].point);
        }

        minkowski_footprint_ = localization_footprint;
        //    ROS_ERROR("COLLVOID points %d", (int) minkowski_footprint_.size());

        // for (int i = 0; i<(int)minkowski_footprint_.size(); i++) {
        //   ROS_ERROR("%f", abs(minkowski_footprint_[i]));
        // }

        // BOOST_FOREACH(geometry_msgs::Point32 p, footprint_msg_.polygon.points) {
        //   own_footprint.push_back(Vector2(p.x, p.y));
        //   //      ROS_WARN("footprint point p = (%f, %f) ", footprint_[i].x, footprint_[i].y);
        // }
        // minkowski_footprint_ = minkowskiSum(localization_footprint, own_footprint);

        // //publish footprint
        // geometry_msgs::PolygonStamped msg_pub = createFootprintMsgFromVector2(minkowski_footprint_);
        // polygon_pub_.publish(msg_pub);

    }

    void amclPoseArrayWeightedCallback(const amcl::PoseArrayWeighted::ConstPtr &msg) {
        pose_array_weighted_.clear();
        geometry_msgs::PoseStamped in;
        in.header = msg->header;
        for (int i = 0; i < (int) msg->poses.size(); i++) {
            in.pose = msg->poses[i];
            geometry_msgs::PoseStamped result;
            try {
                tf_->waitForTransform(global_frame_, base_frame_, msg->header.stamp, ros::Duration(0.3));
                tf_->transformPose(base_frame_, in, result);
                pose_array_weighted_.push_back(std::make_pair(msg->weights[i], result));
            }
            catch (tf::TransformException ex) {
                ROS_ERROR("%s", ex.what());
                ROS_ERROR("point transform failed");
            };
        }
        //    if (!convex_ || orca_) {

        std::vector <double> circle, convex;

        for (eps_ = 0; eps_ <= 1; eps_ += 0.05) {
            computeNewLocUncertainty();
            computeNewMinkowskiFootprint();
            double area = CalcArea(minkowski_footprint_);
            //ROS_ERROR("eps = %f", eps_);
            //ROS_ERROR("radius = %f, area = %f", cur_loc_unc_radius_, M_PI*sqr(cur_loc_unc_radius_));
            circle.push_back(M_PI * sqr(cur_loc_unc_radius_));
            convex.push_back(area);
            //std::sort(minkowski_footprint_.begin(),minkowski_footprint_.end(), comparePoint);

            //ROS_ERROR("area conv_ = %f", area);
            //ROS_ERROR("furthest point= %f", collvoid::abs(minkowski_footprint_[int(minkowski_footprint_.size())-1]));
        }
        for (int i = 0; i < (int) circle.size(); i++) {
            std::cout << circle[i] << ", ";
        }
        for (int i = 0; i < (int) circle.size(); i++) {
            std::cout << convex[i] << ", ";
        }
        std::cout << ";" << std::endl;

    }

    void odomCallback(const nav_msgs::Odometry::ConstPtr &msg) {
        //we assume that the odometry is published in the frame of the base
        tf::Stamped <tf::Pose> global_pose;
        //let's get the pose of the robot in the frame of the plan
        global_pose.setIdentity();
        global_pose.frame_id_ = base_frame_;
        global_pose.stamp_ = msg->header.stamp;

        try {
            tf_->waitForTransform(global_frame_, base_frame_, global_pose.stamp_, ros::Duration(0.2));
            tf_->transformPose(global_frame_, global_pose, global_pose);
        }
        catch (tf::TransformException ex) {
            ROS_ERROR("%s", ex.what());
            ROS_ERROR("point odom transform failed");
            return;
        };

        geometry_msgs::PoseStamped pose_msg;
        tf::poseStampedTFToMsg(global_pose, pose_msg);

        tf::Stamped <tf::Pose> odom_pose;
        //let's get the pose of the robot in the frame of the plan
        odom_pose.setIdentity();
        odom_pose.frame_id_ = base_frame_;
        odom_pose.stamp_ = msg->header.stamp;

        try {
            tf_->waitForTransform(odom_frame_, base_frame_, odom_pose.stamp_, ros::Duration(0.2));
            tf_->transformPose(odom_frame_, odom_pose, odom_pose);
        }
        catch (tf::TransformException ex) {
            ROS_ERROR("%s", ex.what());
            ROS_ERROR("point odom transform failed");
            return;
        };

        geometry_msgs::PoseStamped odom_pose_msg;
        tf::poseStampedTFToMsg(odom_pose, odom_pose_msg);

        //std::cout << "[" <<pose_msg.pose.position.x << ", " << pose_msg.pose.position.y <<"]" << std::endl;
        //std::cout <<"[" <<odom_pose_msg.pose.position.x << ", " << odom_pose_msg.pose.position.y <<"]" << std::endl;
    }

    double CalcArea(std::vector < Vector2 > list) {
        double area = 0; // Total area
        double diff = 0; // Difference Of Y{i + 1} - Y{i - 1}
        unsigned int last = list.size() - 1; // Size Of Vector - 1
        /* Given vertices from 1 to n, we first loop through
               the vertices 2 to n - 1. We will take into account
           vertex 1 and vertex n sepereately */
        for (unsigned int i = 1; i < last; i++) {
            diff = list[i + 1].y() - list[i - 1].y();
            area += list[i].x() * diff;
        }

        /* Now We Consider The Vertex 1 And The Vertex N */
        diff = list[1].y() - list[last].y();
        area += list[0].x() * diff; // Vertex 1
        diff = list[0].y() - list[last - 1].y();
        area += list[last].x() * diff; // Vertex N
        /* Calculate The Final Answer */
        area = 0.5 * std::abs(area);
        return area; // Return The area}
    }

}

int main(int argc, char **argv) {
    ros::init(argc, argv, "Helper");
    ros::NodeHandle nh;
    tf::TransformListener tf;
    collvoid::pose_array_weighted_.clear();
    collvoid::odom_frame_ = "odom";
    collvoid::global_frame_ = "/map";
    collvoid::base_frame_ = "base_link";

    collvoid::tf_ = &tf;
    message_filters::Subscriber <amcl::PoseArrayWeighted> amcl_posearray_sub;
    amcl_posearray_sub.subscribe(nh, "particlecloud_weighted", 10);
    tf::MessageFilter <amcl::PoseArrayWeighted> *tf_filter;

    tf_filter = new tf::MessageFilter<amcl::PoseArrayWeighted>(amcl_posearray_sub, *collvoid::tf_,
                                                               collvoid::global_frame_, 10);
    tf_filter->registerCallback(boost::bind(&collvoid::amclPoseArrayWeightedCallback, _1));
    //ros::Subscriber odom_sub = nh.subscribe("odom", 1, collvoid::odomCallback);

    collvoid::eps_ = 0.1;
    ros::spin();

    delete tf_filter;
}


#include <opencv2/opencv.hpp>
#include <string>
#include <iostream>
#include <Eigen/Eigen>
#include <vector>
#include <chrono>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>
#include <fstream>
#include "bvftdescriptors.h"

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>

#include <python2.7/Python.h>

using namespace cv;
using namespace std;
int readPointCloud(pcl::PointCloud<pcl::PointXYZ> &point_cloud, const std::string filename)
{
  point_cloud.clear();
  std::ifstream binfile(filename.c_str(),std::ios::binary);
  if(!binfile)
  {
    throw std::runtime_error("file cannot open");
    return -1;
  }
  else
  {
    std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
    vector<float> tmp;
    while(1)
    {
      short   s;

      pcl::PointXYZ point;
      binfile.read((char*)&s,sizeof(short));

      if(binfile.eof()) break;
      point.x = s*0.005;
      binfile.read((char*)&s,sizeof(short));
      point.y = s*0.005;
      binfile.read((char*)&s,sizeof(short));
      point.z = s*0.005;

      point_cloud.push_back(point);
   }

   std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
  }
  binfile.close();
  return 1;
}

std::vector<std::string> pyListSortDir(std::string path)
{
    std::vector<std::string> ret;

    Py_Initialize();

    //PyRun_SimpleString("import os");
    PyObject* module_name = PyString_FromString("os");
    PyObject* os_module = PyImport_Import(module_name);
    PyObject* os_list = PyObject_GetAttrString(os_module, "listdir");
    //PyObject* os_list = PyObject_GetAttrString(os_module, "");

    PyObject *ArgList = PyTuple_New(1);
    PyObject* py_path = PyString_FromString(path.c_str());
    PyTuple_SetItem(ArgList, 0, py_path);

    PyObject* files = PyObject_CallObject(os_list, ArgList);
    //PyObject*
    PyList_Sort(files);
    //PyObject_CallMethod(PyList,"sort", 'O', files);
    //PyObject* files_sort_ = PyObject_CallObject(files_sort,ArgList);
    for(int i=0; i<PyList_Size(files); i++)
    {
        char  *temp;
        PyObject* item = PyList_GetItem(files,i);
        //std::cout << item << std::endl;
        PyArg_Parse(item,"s",&temp);
        //std::cout << std::string(temp) << std::endl;
        ret.push_back(std::string(temp));
    }
    //PyObject* re = PyRun_SimpleString("os.list()");
    Py_Finalize();
    return ret;

}
int imagePadding(Mat& img, int &cor_x, int & cor_y)
{
    copyMakeBorder(img, img, pad_size/2,pad_size/2,pad_size/2,pad_size/2, BORDER_CONSTANT, Scalar(10));

    //Extending image
	int m = getOptimalDFTSize(img.rows);
	int n = getOptimalDFTSize(img.cols);
    int row_pad = (m - img.rows)/2;
    int col_pad = (n - img.cols)/2;
    //take this step to make fft faster
    copyMakeBorder(img, img,row_pad , (m - img.rows)%2?row_pad+1:row_pad,
                col_pad, (n - img.cols)%2?col_pad+1:col_pad , BORDER_CONSTANT, Scalar(10));
    cor_x += col_pad+pad_size/2;
    cor_y += row_pad+pad_size/2;
}

void generateImage(pcl::PointCloud<pcl::PointXYZ> &point_cloud, int &x_max_ind, int &y_max_ind, Mat &mat_local_image)
{
    float resolution = 0.4;
    pcl::VoxelGrid<pcl::PointXYZ> down_size_filter;
    down_size_filter.setLeafSize(resolution, resolution, resolution/2);
    down_size_filter.setInputCloud(point_cloud.makeShared());
    down_size_filter.filter(point_cloud);

    float x_min = 10000, y_min = 10000, x_max=-100000,y_max=-100000;
    for(int i=0; i<point_cloud.size(); i++)
    {
        if(point_cloud.points[i].y< x_min) x_min=point_cloud.points[i].y;
        if(point_cloud.points[i].y> x_max) x_max=point_cloud.points[i].y;
        if(point_cloud.points[i].x< y_min) y_min=point_cloud.points[i].x;
        if(point_cloud.points[i].x> y_max) y_max=point_cloud.points[i].x;
    }

    int x_min_ind = int(x_min/resolution);
    x_max_ind = int(x_max/resolution);
    int y_min_ind = int(y_min/resolution);
    y_max_ind = int(y_max/resolution);
    //std::cout << x_min << ' ' << x_max << ' ' <<  y_min << ' ' << y_max << std::endl;
    //std::cout << x_min_ind << ' ' << x_max_ind << ' ' <<  y_min_ind << ' ' << y_max_ind << std::endl;

    int x_num = x_max_ind-x_min_ind+1;
    int y_num = y_max_ind-y_min_ind+1;
    mat_local_image=Mat( y_num, x_num,CV_8UC1, cv::Scalar::all(0));

    for(int i=0; i<point_cloud.size(); i++)
    {
        int x_ind = x_max_ind-int((point_cloud.points[i].y)/resolution);
        int y_ind = y_max_ind-int((point_cloud.points[i].x)/resolution);
        if(x_ind>=x_num || y_ind>=y_num ) continue;
        mat_local_image.at<uint8_t>( y_ind,x_ind) += 1;
        //std::cout << int(mat_local_image.at<uint8_t>(x_ind, y_ind)) << std::endl;
    }
    uint8_t max_pixel = 0;
    for(int i=0; i<x_num; i++)
        for(int j=0; j<y_num; j++)
        {
            if (mat_local_image.at<uint8_t>(j, i)>max_pixel) max_pixel=mat_local_image.at<uint8_t>(j, i);
        }
    for(int i=0; i<x_num; i++)
        for(int j=0; j<y_num; j++)
        {
            if(uint8_t(mat_local_image.at<uint8_t>(j, i)*10)>122) {mat_local_image.at<uint8_t>(j, i)=122;continue;}
            mat_local_image.at<uint8_t>(j, i)=uint8_t(mat_local_image.at<uint8_t>(j, i)*10);//1.0/max_pixel*255);
            if(uint8_t(mat_local_image.at<uint8_t>(j, i))==0) {mat_local_image.at<uint8_t>(j, i)=10;continue;}
        }
}

int main(int argc, char** argv)
{
    string bin_path = argv[1];
    string  seq = argv[2];

    vector<string> bin_file_names = pyListSortDir(bin_path);
    cout << bin_file_names.size() << endl;

    chrono::steady_clock::time_point t_start = chrono::steady_clock::now();
    for(int i=0; i<bin_file_names.size(); i++)
    {

        pcl::PointCloud<pcl::PointXYZ> point_cloud;
        int max_local_x_ind,max_local_y_ind;
        Mat mat_local_image;

        readPointCloud(point_cloud, bin_path+bin_file_names[i]);

        //generate BV image, recording the cornet points
        generateImage(point_cloud, max_local_x_ind, max_local_y_ind, mat_local_image);

        //padding to make fft faster
        cout << "processing: " << i << "/" << bin_file_names.size() << "," << bin_path  << endl;
        imagePadding(mat_local_image, max_local_x_ind, max_local_y_ind);

        BVFT bvft = detectBVFT(mat_local_image);

        //save descriptor to mat
        writeMatToBin(bvft.descriptors,(seq+"des_"+to_string(10000+i)+"_"+to_string(max_local_x_ind)+","+to_string(max_local_y_ind)+".bin").c_str());
    }
    chrono::steady_clock::time_point t_end_3 = chrono::steady_clock::now();
    chrono::duration<float> time_used=chrono::duration_cast<
    chrono::duration<float>>(t_end_3-t_start);
    cout <<seq << " size " << bin_file_names.size() << " dectection uses " << time_used.count()  << " seconds"<< endl;
}


#ifndef CAFFE_TRACKER_FREGRESSOR_BASE_H
#define CAFFE_TRACKER_FREGRESSOR_BASE_H

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <boost/shared_ptr.hpp>

#include "caffe/caffe.hpp"
#include "caffe/tracker/bounding_box.hpp"

namespace caffe {

/**
 * ##和F##的区别：
 * ##是使用ROI-Patch
 * F##是使用ROI-Blobs(特征Blobs)
 * 这是一个使用ROI特征Blob的回归器基类
 * 注意：该回归器接收前后两帧的特征Blobs，然后输出一个Box坐标
 */

// 回归器
template <typename Dtype>
class FRegressorBase {
public:
  FRegressorBase();

  /**
   * 回归方法：对单目标或多目标进行估计
   * @param curr [当前Blobs]
   * @param prev [历史Blobs]
   * @param bbox [回归的结果]
   */
  virtual void Regress(const Blob<Dtype>& curr, const Blob<Dtype>& prev, BoundingBox<Dtype>* bbox) = 0;
  virtual void Regress(const std::vector<boost::shared_ptr<Blob<Dtype> > >& curr,
                       const std::vector<boost::shared_ptr<Blob<Dtype> > >& prev,
                       std::vector<BoundingBox<Dtype> >* bboxes) = 0;

  /**
   * 初始化
   */
  virtual void Init() {}

  // 网络对象
  boost::shared_ptr<caffe::Net<Dtype> > get_net() { return net_; }

protected:
  // 网络对象
  boost::shared_ptr<caffe::Net<Dtype> > net_;
};

}

#endif


/*Authors: Marcin Zalewski / Jesun Sahariar Firoz*/

#include <algorithm>
#include <cassert>
#include <fstream>
#include <iostream>
#include <limits>
#include <map>
#include <mutex>
#include <numeric>
#include <regex>
#include <sstream>
#include <sys/time.h>
#include <utility>
#include <vector>

#include <extensions/allocator/allocator.hpp>
#include <upcxx/allocate.hpp>
#include <upcxx/atomic.hpp>
#include <upcxx/backend.hpp>
#include <upcxx/rget.hpp>
#include <upcxx/rpc.hpp>
#include <upcxx/rput.hpp>
#include <upcxx/upcxx.hpp>

#include <boost/asio.hpp>
#include <boost/dynamic_bitset.hpp>

#if defined NDEBUG
const bool  debug{false};
#else
const bool debug{true};
#endif
#define dout \
    if (!debug) {} \
    else std::cerr

namespace ip = boost::asio::ip;

typedef unsigned long long ve_type;
typedef uint64_t           IndexType;

using element                     = std::tuple<ve_type, ve_type>;
using edge_list                   = std::vector<std::tuple<ve_type, ve_type>>;
std::string edgelistFile          = "";
uint64_t    num_vertices          = 0;
uint64_t    num_vertices_per_rank = 0;

// retain a per-destination queue for current and next iteration
std::vector<std::vector<std::pair<uint64_t, uint64_t>,
                        upcxxc::allocator<std::pair<uint64_t, uint64_t>>>>
    nextFrontierQarr[2];
// received buffer is never exported, so no need for current and next
std::vector<std::pair<uint64_t, uint64_t>> received_buffer;

double GetCurrentTime() {
  static struct timeval  tv;
  static struct timezone tz;
  gettimeofday(&tv, &tz);
  return tv.tv_sec + 1.e-6 * tv.tv_usec;
}

double TimeDifference(double& a, double& b) { return 1000 * (b - a); }

double ElapsedMillis(double start, double stop) {
  return TimeDifference(start, stop);
}

struct gptr_and_len {
  upcxx::global_ptr<uint64_t> p;    // pointer to first element in adjacencies
  int                         n;    // number of elements
};

struct gptr_and_len_pair {
  upcxx::global_ptr<std::pair<uint64_t, uint64_t>>
      p;    // pointer to first element in destination buffer
  int n;    // number of elements
};

std::vector<upcxx::global_ptr<gptr_and_len>> bases;

using BaseType = std::vector<upcxx::global_ptr<gptr_and_len>>;

using BaseQType = std::vector<upcxx::global_ptr<gptr_and_len_pair>>;

uint64_t index_to_vertex_id(size_t index) {
  // muliply for rows, add for row offset
  return index * upcxx::rank_n() + upcxx::rank_me();
}

size_t vertex_id_to_index(uint64_t vertex_id) {
  // It's ok to round off, as every vertex in a row in cyclic dist. will have the same index on every rank
  return vertex_id / upcxx::rank_n();
}

auto print_graph() {
  // For each vertex
  for (uint64_t i = 0; i < num_vertices_per_rank; i++) {
    const auto vtx_ptr =
        bases[upcxx::rank_me()].local()
            [i];    // <--This gives the local ptr to the gbl ptr and length for a vtx
    const auto adj_list_start = vtx_ptr.p.local();
    const auto adj_list_len   = vtx_ptr.n;

    auto current_vertex_id = index_to_vertex_id(i);

    std::cout << "i = " << i << ", vertex id = " << current_vertex_id
              << ", adjs: ";
    // For each neighbor of the vertex, get the adjacency  list and do the set intersection.
    for (auto j = 0; j < vtx_ptr.n; j++) {
      auto neighbor = adj_list_start[j];
      std::cout << neighbor << ", ";
    }
    std::cout << std::endl;
  }
}

upcxx::intrank_t vertex_id_to_rank(uint64_t v_id) {
  return v_id % upcxx::rank_n();
}

auto vertex_id_to_offset(uint64_t v_id) { return v_id / upcxx::rank_n(); }

void init_adjs(std::istream& infile, BaseType& bases) {
  infile.read(reinterpret_cast<char*>(&num_vertices), sizeof(num_vertices));
  std::cout << num_vertices << std::endl;

  bases.reserve(upcxx::rank_n());
  num_vertices_per_rank =
      (num_vertices + upcxx::rank_n() - 1 - upcxx::rank_me()) / upcxx::rank_n();
  std::cout << "No of vertices per rank: " << num_vertices_per_rank
            << std::endl;
  bases[upcxx::rank_me()] =
      upcxx::new_array<gptr_and_len>(num_vertices_per_rank);

  for (int r = 0; r < upcxx::rank_n(); r++) {
    bases[r] = upcxx::broadcast(bases[r], r).wait();
  }

  for (uint64_t i = upcxx::rank_me(); i < num_vertices; i += upcxx::rank_n()) {
    gptr_and_len pn;
    uint64_t     adj_indices[2];
    const auto   index_seekg = 1 + i;
    infile.seekg(index_seekg * sizeof(uint64_t), infile.beg);
    infile.read(reinterpret_cast<char*>(adj_indices), 2 * sizeof(uint64_t));
    auto cur_adj_len = adj_indices[1] - adj_indices[0];
    pn.n             = cur_adj_len;
    pn.p                      = upcxx::new_array<uint64_t>(cur_adj_len);
    const auto adj_list_seekg = 2 + num_vertices + adj_indices[0];
    infile.seekg(adj_list_seekg * sizeof(uint64_t), infile.beg);
    infile.read(reinterpret_cast<char*>(pn.p.local()),
                cur_adj_len * sizeof(uint64_t));
    for (auto edg = 0; edg < cur_adj_len; edg++) {
      std::cout << pn.p.local()[edg] << " ";
    }

    const auto base_index                       = vertex_id_to_index(i);
    bases[upcxx::rank_me()].local()[base_index] = pn;
  }
}

void readBinaryFormat(const std::string& filename, BaseType& bases) {
  std::ifstream inputFile;
  inputFile.exceptions(std::ifstream::failbit);
  try {
    inputFile.open(filename);
    init_adjs(inputFile, bases);
    inputFile.close();
  } catch (std::ios_base::failure& fail) {
    std::cerr << "Something went wrong with reading the matrix from file "
              << filename << std::endl;
    throw fail;
  }
}

int main(int argc, char* argv[]) {
  upcxx::init();

  boost::asio::io_service io_service;

  std::string h = ip::host_name();
  std::cout << "hostname: " << h << 'n';

  int         argIndex = 1;
  std::string arg_t(argv[argIndex]);
  uint64_t    source = 0;
  while (argIndex < argc) {
    std::string arg(argv[argIndex]);
    if (arg == "--edgelistfile") {
      ++argIndex;
      edgelistFile = std::string(argv[argIndex]);
      ++argIndex;
    }
    if (arg == "--source") {
      ++argIndex;
      source = std::stoul(argv[argIndex], nullptr, 0);
      ++argIndex;
    }
  }
  readBinaryFormat(edgelistFile, bases);

  upcxx::barrier();

  boost::dynamic_bitset<> color_map(num_vertices_per_rank);
  std::vector<uint64_t>   parent_map(num_vertices_per_rank);

  auto level               = 0;
  auto current_queue_index = [&]() { return level % 2; };
  auto nextFrontierQ       = [&]() -> auto& {
    dout << "nextFrontierQarr[" << current_queue_index() << "]"
              << std::endl;
    return nextFrontierQarr[current_queue_index()];
  };
  for (size_t i = 0; i < 2; ++i) {
    nextFrontierQarr[i].resize(upcxx::rank_n());
  }
  // find the rank of the source
  auto rank = vertex_id_to_rank(source);
  // if I am the owner of the source
  if (rank == upcxx::rank_me()) {
    auto v_index = vertex_id_to_index(source);
    // Set source's colormap to 1.
    color_map.set(v_index);
    parent_map[v_index] = source;    // source is its own parent
    auto vtx_ptr        = bases[upcxx::rank_me()].local()[v_index];
    auto adj_list_start = vtx_ptr.p.local();
    auto adj_list_len   = vtx_ptr.n;
    // For each neighbor of the vertex, put it in appropriate buffer
    for (auto j = 0; j < vtx_ptr.n; j++) {
      auto neighbor     = adj_list_start[j];
      auto neighborRank = vertex_id_to_rank(neighbor);
      nextFrontierQ()[neighborRank].push_back(std::make_pair(neighbor, source));
    }
  }

  BaseQType gpNextFrontierQarr[2];
  // access current queue
  auto gpNextFrontierQ = [&]() -> auto& {
    return gpNextFrontierQarr[current_queue_index()];
  };
  // initialize and exchange
  for (auto i = 0; i < 2; ++i) {
    gpNextFrontierQarr[i].resize(upcxx::rank_n());
    gpNextFrontierQarr[i][upcxx::rank_me()] =
        upcxx::new_array<gptr_and_len_pair>(upcxx::rank_n());
    for (int r = 0; r < upcxx::rank_n(); r++) {
      gpNextFrontierQarr[i][r] =
          upcxx::broadcast(gpNextFrontierQarr[i][r], r).wait();
    }
  }

  double start{0};
  double stop{0};
  if (upcxx::rank_me() == 0) start = GetCurrentTime();

  while (true) {
    // for each rank
    size_t localFrontierSize = 0;
    for (upcxx::intrank_t r = 0; r < upcxx::rank_n(); r++) {
      // sort and remove duplicates
      std::sort(nextFrontierQ()[r].begin(), nextFrontierQ()[r].end(),
                [](auto& a, auto& b) { return a.first < b.first; });
      std::unique(nextFrontierQ()[r].begin(), nextFrontierQ()[r].end(),
                  [](auto& a, auto& b) { return a.first == b.first; });

      // Now copy the neighbor list per rank to the dist obj
      gptr_and_len_pair pn;
      auto              destination_buffer_size = nextFrontierQ()[r].size();
      pn.n                                      = destination_buffer_size;
      dout << "nextFrontierQ()[r].data() = " << nextFrontierQ()[r].data()
                << ", r = " << r << std::endl;
      pn.p = upcxx::try_global_ptr(nextFrontierQ()[r].data());
      if (pn.p == nullptr) {
        if (nextFrontierQ()[r].size() > 0) {
          throw std::runtime_error(
              "Could not get a global pointer from upcxx allocator vector.");
        }
      }
      gpNextFrontierQ()[upcxx::rank_me()].local()[r] = pn;

      // reduce local frontier sizes
      localFrontierSize += nextFrontierQ()[r].size();
    }

    // Do a reduction to check whether we have reached the end
    //////////////////////////////////////////////////////////
    size_t totalFrontierSize = 0;
    auto done_reduction =
        upcxx::reduce_all(&localFrontierSize, &totalFrontierSize, 1,
                          [](size_t a, size_t b) { return a + b; });
    done_reduction.wait();

    // zero out receive buffer
    received_buffer.resize(0);
    // the start of the conjoined future
    upcxx::future<> fut_all = upcxx::make_future();

    if (upcxx::rank_me() == 0) {
      std::cout << "Level: " << level << " Size: " << totalFrontierSize
                << std::endl;
    }

    if (totalFrontierSize == 0) break;

    // Get vertices targeted for me from each rank
    for (upcxx::intrank_t r = 0; r < upcxx::rank_n(); r++) {
      upcxx::future<> fut =
          upcxx::rget(    // TODO: skip me
              gpNextFrontierQ()[r] + upcxx::rank_me())
              .then([=](gptr_and_len_pair pn) {
                std::vector<std::pair<uint64_t, uint64_t>> target_neighbor_list(
                    pn.n);
                return upcxx::rget(pn.p, target_neighbor_list.data(), pn.n)
                    .then([=, target_neighbor_list =
                                  std::move(target_neighbor_list)]() {
                      received_buffer.insert(received_buffer.end(),
                                             target_neighbor_list.begin(),
                                             target_neighbor_list.end());
                    });
              });
      // conjoin the futures
      fut_all = upcxx::when_all(fut_all, fut);
    }

    // wait for all the conjoined futures to complete
    fut_all.wait();
    dout << "fut_all" << std::endl;

    // Important: do not increment the level until rgets are finished or the rgets will work with the wrong level
    level += 1;

    // After level is increased, we can clear the data from the previous level that is still in the queue
    for (upcxx::intrank_t r = 0; r < upcxx::rank_n(); r++) {
      nextFrontierQ()[r].resize(0);
    }

    // At this point everyone should have the next frontier for the next iteration
    // sort and remove duplicates
    std::sort(received_buffer.begin(), received_buffer.end(),
              [](auto& a, auto& b) { return a.first < b.first; });
    std::unique(received_buffer.begin(), received_buffer.end(),
                [](auto& a, auto& b) { return a.first == b.first; });

    for (auto vertex_p : received_buffer) {
      auto vtx    = vertex_p.first;
      auto parent = vertex_p.second;
      // Check whether the vertex has already been visited.
      auto v_index = vertex_id_to_index(vtx);
      if (color_map[v_index]) {    //Already visited
        continue;
      } else {    // Not visited yet
        dout << "Marking " << vtx << " as visited. " << std::endl;
        color_map.set(v_index);
        parent_map[v_index] = parent;
        // Put all its neighbors into the nextfrontier
        auto vtx_ptr        = bases[upcxx::rank_me()].local()[v_index];
        auto adj_list_start = vtx_ptr.p.local();
        auto adj_list_len   = vtx_ptr.n;
        // For each neighbor of the vertex, put it in appropriate buffer
        for (auto j = 0; j < vtx_ptr.n; j++) {
          auto neighbor     = adj_list_start[j];
          auto neighborRank = vertex_id_to_rank(neighbor);
          nextFrontierQ()[neighborRank].push_back(
              std::make_pair(neighbor, vtx));
        }
      }
    }
  }

  if (upcxx::rank_me() == 0) {
    stop          = GetCurrentTime();
    float elapsed = ElapsedMillis(start, stop);
    std::cout << "Total time " << elapsed << " ms." << std::endl;
  }

  // TODO: clean up after each barrier, empty the next frontier ds, received buffer ds etc.
  upcxx::finalize();
  return 0;
}