#include <math.h>

#include <nav_msgs/Odometry.h>
#include <opencv/cv.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/filters/passthrough.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/common/feature_histogram.h>
#include <pcl/common/centroid.h>
#include <pcl/conversions.h>

#include <ros/ros.h>
#include <ros/console.h>
#include <std_msgs/String.h>
#include <std_msgs/Int32.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>

#include <Eigen/Dense>

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

typedef pcl::PointXYZI  PointType;

class AdapIntegraDetector{

private:

    ros::NodeHandle nh;

    ros::Subscriber subLaserCloud;

    ros::Publisher pubMapCloud;
    ros::Publisher pubClusters;
    ros::Publisher pubLaserCloud2;
    ros::Publisher pubLaserCloudFilter1;
    ros::Publisher pubLaserCloudFilter2;
    ros::Publisher pubObjCloud;
    ros::Publisher pubObjPose;

    pcl::PointCloud<PointType>::Ptr ig_Cloud_1      ; // Integrating cloud
    pcl::PointCloud<PointType>::Ptr ig_Cloud_2      ; // Integrating cloud
    pcl::PointCloud<PointType>::Ptr tmpCloud        ;
    pcl::PointCloud<PointType>::Ptr interated_Cloud ;
    pcl::PointCloud<PointType>::Ptr latest_Cloud ;
    pcl::PointCloud<PointType>::Ptr dynamic_Cloud ;
    pcl::PointCloud<PointType>::Ptr obj_Cloud ;
    pcl::PointCloud<PointType>::Ptr map_cloud ;

    int ig_cnt = 0;
    bool map_built = false;

public:
    AdapIntegraDetector():nh("~") {
        ig_Cloud_1.reset(       new pcl::PointCloud<PointType>());
        ig_Cloud_2.reset(       new pcl::PointCloud<PointType>());
        tmpCloud.reset(         new pcl::PointCloud<PointType>());
        latest_Cloud.reset(     new pcl::PointCloud<PointType>());
        interated_Cloud.reset(  new pcl::PointCloud<PointType>());
        dynamic_Cloud.reset(    new pcl::PointCloud<PointType>());
        obj_Cloud.reset(        new pcl::PointCloud<PointType>());
        map_cloud.reset(        new pcl::PointCloud<PointType>());

        interated_Cloud->header.frame_id    = "/livox_frame";
        latest_Cloud->header.frame_id       = "/livox_frame";
        tmpCloud->header.frame_id           = "/livox_frame";
        ig_Cloud_1->header.frame_id         = "/livox_frame";
        ig_Cloud_2->header.frame_id         = "/livox_frame";
        dynamic_Cloud->header.frame_id      = "/livox_frame";
        obj_Cloud->header.frame_id          = "/livox_frame";
        map_cloud->header.frame_id          = "/livox_frame";

        map_built = false;

        subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/livox/lidar", 1, &AdapIntegraDetector::laserCloudHandler, this);

        pubMapCloud     = nh.advertise<pcl::PointCloud<PointType>>("/livox/map", 2);
        pubClusters     = nh.advertise<pcl::PointCloud<PointType>>("/livox/cluster", 2);
        pubLaserCloud2  = nh.advertise<pcl::PointCloud<PointType>>("/livox/ig_cloud_2", 2);
        pubLaserCloudFilter1 = nh.advertise<pcl::PointCloud<PointType>>("/livox/ig_cloud_1_filter", 2);
        pubLaserCloudFilter2 = nh.advertise<pcl::PointCloud<PointType>>("/livox/ig_cloud_2_filter", 2);
        pubObjCloud     = nh.advertise<pcl::PointCloud<PointType>>("/livox/obj_cloud", 2);
        pubObjPose      = nh.advertise<geometry_msgs::PoseStamped>("/livox/obj_pose", 2);
    }

    void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
        // timestamp = laserCloudMsg->header.stamp.toSec();
        // last_time = laserCloudMsg->header.stamp;
        // frame_seq = laserCloudMsg->header.seq;
        tmpCloud->clear();
        pcl::fromROSMsg(*laserCloudMsg, *tmpCloud);

        // ROS_INFO_STREAM(  " MAP BUILT? " << map_built );
        if(map_built){
            if (ig_cnt < 5){
                ig_cnt++;
                *interated_Cloud = *interated_Cloud + *tmpCloud;
            }
            else{
                latest_Cloud->clear();
                *latest_Cloud = *latest_Cloud + *tmpCloud;
                obj_inmap_detecting(interated_Cloud);
                interated_Cloud->clear();
                ig_cnt = 0;
            }
            return;
        }

        int itg_num = 80;
        // ROS_INFO_STREAM("Map initing ... ");
        if(ig_cnt < itg_num ){
            *interated_Cloud = *interated_Cloud + *tmpCloud;
            ig_cnt++;

            if(ig_cnt % 10 == 0){
                ROS_INFO_STREAM("Map initing - Cloud 1 is inegrating ... " << ig_cnt << " / " << itg_num);
            }

        }else if(ig_cnt == itg_num ){
            ROS_INFO_STREAM("Cloud 1 is ready ! ");
            *ig_Cloud_1 = *ig_Cloud_1 + *interated_Cloud;
            simple_map_building(ig_Cloud_1);
            // pubLaserCloud1.publish(ig_Cloud_1);
            interated_Cloud->clear();
            ig_cnt++;

        }else if(ig_cnt > itg_num + 30 && ig_cnt < 2 * itg_num + 30){
            *interated_Cloud = *interated_Cloud + *tmpCloud;
            ig_cnt++;
            if(ig_cnt % 10 == 0){
                ROS_INFO_STREAM("Map initing - Cloud 2 is inegrating ... " << ig_cnt << "/ " << itg_num);
            }
        }else if(ig_cnt == 2 * itg_num + 30){
            latest_Cloud->clear();
            *ig_Cloud_2 = *ig_Cloud_2 + *interated_Cloud;
            *latest_Cloud = *latest_Cloud + *tmpCloud;
            // pubLaserCloud2.publish(ig_Cloud_2);
            ig_cnt++;
            ROS_INFO_STREAM("Cloud 2 is ready ! ");
            map_building(ig_Cloud_1, ig_Cloud_2);
            interated_Cloud->clear();
        }else{
            if(!map_built)
                ig_cnt++;
            else
                ig_cnt = 0;
        }
    }

    // Take current clouds, substract points belong to static objects, extract the objects points
    void obj_inmap_detecting(pcl::PointCloud<PointType>::Ptr cloud){

        pcl::KdTreeFLANN<PointType>::Ptr kdtreePts(new pcl::KdTreeFLANN<PointType>());
        kdtreePts->setInputCloud(map_cloud);
        std::vector<int> pointIdxRadiusSearch;
        std::vector<float> pointRadiusSquaredDistance;

        dynamic_Cloud->points.clear();
        float radius = 0.1;
        for(int nIdex = 0; nIdex < cloud->points.size(); nIdex++){
            if ( kdtreePts->radiusSearch(cloud->points[nIdex], radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 )
            {
            }else{
                dynamic_Cloud->points.push_back(cloud->points[nIdex]);
            }
        }

        if(dynamic_Cloud->points.size() == 0){
            ROS_INFO_STREAM("No dynamic points detected ! ");
            return;
        }else{
            ROS_INFO_STREAM("Found dynamics points "<< dynamic_Cloud->points.size());
        }

        // Cluster the object
        pcl::search::KdTree<PointType>::Ptr tree (new pcl::search::KdTree<PointType>);
        tree->setInputCloud(dynamic_Cloud);

        std::vector<pcl::PointIndices> cluster_indices;
        pcl::EuclideanClusterExtraction<PointType> ec;
        ec.setClusterTolerance (0.1); // 5cm
        ec.setMinClusterSize (10);
        ec.setMaxClusterSize (1000);
        ec.setSearchMethod (tree);
        ec.setInputCloud (dynamic_Cloud);
        ec.extract (cluster_indices);

        pcl::PointCloud<PointType>::Ptr cloud_cluster (new pcl::PointCloud<PointType>);
        for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
        {
            // pcl::PointCloud<PointType>::Ptr cloud_cluster (new pcl::PointCloud<PointType>);

            cloud_cluster->header.frame_id = "/livox_frame";
            for (const auto& idx : it->indices)
            cloud_cluster->push_back ((*dynamic_Cloud)[idx]); //*
            cloud_cluster->width = cloud_cluster->size ();
            cloud_cluster->height = 1;
            cloud_cluster->is_dense = true;

            ROS_INFO_STREAM("PointCloud Trajectory Clustering: "  << cloud_cluster->size () << " data points.");
            pubClusters.publish(*cloud_cluster);
        }

        if(cloud_cluster->size() > 0 ){
            // Find the object pose from latest frame
            pointIdxRadiusSearch.resize(0);
            pointRadiusSquaredDistance.resize(0);
            kdtreePts->setInputCloud(cloud_cluster);

            radius = 0.2;
            obj_Cloud->clear();
            for(int nIdex = 0; nIdex < latest_Cloud->points.size(); nIdex++){
                if ( kdtreePts->radiusSearch(latest_Cloud->points[nIdex], radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 )
                {
                    obj_Cloud->points.push_back(latest_Cloud->points[nIdex]);
                }
            }
            ROS_INFO_STREAM("Publishing Object PointCloud: " << obj_Cloud->size () << " data points.");
            pubObjCloud.publish(*obj_Cloud);
        }

        // Caculate the pose and the covariance of the pose
        Eigen::Vector4f centroid;
        Eigen::Matrix3f covariance;
        pcl::compute3DCentroid(*obj_Cloud, centroid);
        pcl::computeCovarianceMatrix(*obj_Cloud, centroid, covariance);

        std::cout << "Pose:  covariance \n" <<  covariance << std::endl;

        geometry_msgs::PoseStamped  obj_pose;
        obj_pose.header.frame_id = "/livox_frame";
        obj_pose.pose.position.x = centroid(0,0);
        obj_pose.pose.position.y = centroid(1,0);
        obj_pose.pose.position.z = centroid(2,0);
        pubObjPose.publish(obj_pose);

    }

    void simple_map_building(pcl::PointCloud<PointType>::Ptr cloud1){

        pcl::PCLPointCloud2::Ptr point_cloud1(new pcl::PCLPointCloud2());

        pcl::toPCLPointCloud2(*cloud1, *point_cloud1);

        pcl::PCLPointCloud2::Ptr cloud_grided_1(new pcl::PCLPointCloud2());
        cloud_grided_1->header.frame_id = "/livox_frame";

        // Cloud 1
        pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
        sor.setInputCloud(point_cloud1);
        sor.setLeafSize (0.05f, 0.05f, 0.05f);
        sor.filter (*cloud_grided_1);
        pubLaserCloudFilter1.publish(*cloud_grided_1);

        pcl::PointCloud<PointType>::Ptr ccloud1 (new pcl::PointCloud<PointType>());
        pcl::fromPCLPointCloud2(*cloud_grided_1, *ccloud1);

        *map_cloud = *map_cloud + *ccloud1;

        ROS_INFO_STREAM("Pub map points "<< map_cloud->points.size());
        pubMapCloud.publish(*map_cloud);

        if(map_cloud->points.size() > 0){
            map_built = true;
            ROS_INFO_STREAM("A map has been build with points "<< map_cloud->points.size());
        }

    }

    void map_building(pcl::PointCloud<PointType>::Ptr cloud1, pcl::PointCloud<PointType>::Ptr cloud2){

        pcl::PCLPointCloud2::Ptr point_cloud1(new pcl::PCLPointCloud2());
        pcl::PCLPointCloud2::Ptr point_cloud2(new pcl::PCLPointCloud2());

        pcl::toPCLPointCloud2(*cloud1, *point_cloud1);
        pcl::toPCLPointCloud2(*cloud2, *point_cloud2);

        pcl::PCLPointCloud2::Ptr cloud_grided_1(new pcl::PCLPointCloud2());
        pcl::PCLPointCloud2::Ptr cloud_grided_2(new pcl::PCLPointCloud2());
        cloud_grided_1->header.frame_id = "/livox_frame";
        cloud_grided_2->header.frame_id = "/livox_frame";

        // Cloud 1
        pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
        sor.setInputCloud(point_cloud1);
        sor.setLeafSize (0.05f, 0.05f, 0.05f);
        sor.filter (*cloud_grided_1);
        pubLaserCloudFilter1.publish(*cloud_grided_1);

        // Cloud 2
        sor.setInputCloud(point_cloud2);
        sor.setLeafSize (0.05f, 0.05f, 0.05f);
        sor.filter (*cloud_grided_2);
        pubLaserCloudFilter2.publish(*cloud_grided_2);

        pcl::PointCloud<PointType>::Ptr ccloud1 (new pcl::PointCloud<PointType>());
        pcl::PointCloud<PointType>::Ptr ccloud2 (new pcl::PointCloud<PointType>());
        pcl::fromPCLPointCloud2(*cloud_grided_1, *ccloud1);
        pcl::fromPCLPointCloud2(*cloud_grided_2, *ccloud2);
        pcl::PointCloud<PointType>::Ptr diffcloud (new pcl::PointCloud<PointType>());
        diffcloud->header.frame_id = "/livox_frame";

        pcl::KdTreeFLANN<PointType>::Ptr kdtreePts(new pcl::KdTreeFLANN<PointType>());
        kdtreePts->setInputCloud(ccloud1);
        std::vector<int> pointIdxRadiusSearch;
        std::vector<float> pointRadiusSquaredDistance;

        float radius = 0.1;
        for(int nIdex = 0; nIdex < ccloud2->points.size(); nIdex++){
            if ( kdtreePts->radiusSearch(ccloud2->points[nIdex], radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 )
            {
                map_cloud->points.push_back(ccloud2->points[nIdex]); // Common area are treated as map(static objects)
            }else{
                diffcloud->points.push_back(ccloud2->points[nIdex]);
            }
        }

        ROS_INFO_STREAM("Pub map points "<< map_cloud->points.size());
        pubMapCloud.publish(*map_cloud);

        if(map_cloud->points.size() > 0){
            map_built = true;
            ROS_INFO_STREAM("A map has been build with points "<< map_cloud->points.size());
        }

    }
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "livox_drone_detector");

    AdapIntegraDetector DD;
    // ros::MultiThreadedSpinner spinner(15);
    // spinner.spin();
    ros::spin();

    return 0;
}


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

  @copyright 2016 NumScale SAS

  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_ARCH_COMMON_SIMD_FUNCTION_NEGATENZ_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_NEGATENZ_HPP_INCLUDED

#include <boost/simd/function/bitofsign.hpp>
#include <boost/simd/function/bitwise_xor.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/signnz.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD_IF ( negatenz_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::signed_<A0>, X>
                          , bs::pack_<bd::signed_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
    {
      return multiplies(a0, signnz(a1));
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( negatenz_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::unsigned_<A0>, X>
                          , bs::pack_<bd::unsigned_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0, A0) const BOOST_NOEXCEPT
    {
      return a0;
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( negatenz_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          , bs::pack_<bd::floating_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
    {
      return bitwise_xor(bitofsign(a1), a0);
    }
  };
} } }

#endif


/**
 * @file Connectivity.hpp
 * @author bwu
 * @brief Connectivity extraction algorithm to build connectivity graph on layer based geometries
 * @version 0.1
 * @date 2022-02-22
 */
#ifndef GENERIC_GEOMETRY_CONNECTIVITY_HPP
#define GENERIC_GEOMETRY_CONNECTIVITY_HPP
#include "generic/topology/IndexGraph.hpp"
#include "generic/thread/ThreadPool.hpp"
#include "generic/geometry/Utility.hpp"
#include <boost/geometry/index/rtree.hpp>
#include <boost/variant.hpp>
#include <unordered_map>
#include <unordered_set>
#include <vector>
#include <tuple>
#include <list>
#include <set>
namespace generic  {
namespace geometry {

using namespace topology;
using GeomConnGraph = SparseIndexGraph;

/**
 * @brief extracts objects's physical connection
 *
 * @tparam T object type, could be object or pointer.
 * @tparam GeomGetter a functor type to convert T to internal geometry type
 * @note the GeomGetter functor should have operator() (const & T) function
 * @note the GeomGetter functor returns type should be object or reference in one of the Triangle2D, Box2D, Polygon2D, PolygonWithHoles2D
 * @param[in] objects a vector of objects for connectivity extraction
 * @param[in] geomGetter the GeomGetter functor object to convert T to internal geometry type
 * @param[out] connection return the connection index graph in adjacent list, the index is same as objects's vector index
 */
template <typename T, typename GeomGetter,
          typename std::enable_if<traits::is_2d_surf_geom_t<
          typename std::remove_const<typename std::remove_reference<
          typename std::result_of<GeomGetter(const T&)>::type>::type>::type>::value, bool>::type = true>
inline void ConnectivityExtraction(const std::vector<T> & objects,  GeomGetter && geomGetter, std::vector<std::set<int> > & connection)
{
    using coor_t = typename std::decay<typename std::result_of<GeomGetter(const T&)>::type>::type::coor_t;

    connection.clear();
    index_t size = objects.size();
    if(0 == size) return;

    connection.resize(size);
    boost::polygon::connectivity_extraction<coor_t> ce;
    for(index_t i = 0; i < size; ++i)
        GENERIC_ASSERT(i == ce.insert(geomGetter(objects[i])))

    ce.extract(connection);
}

/// A function class for layer geometry connectivity extraction
template <typename num_type>
class ConnectivityExtractor
{
    using Jumpwire = std::tuple<index_t, index_t, index_t, Segment2D<num_type> >;//<index, layer1, layer2, positions>
    using Geometry = boost::variant<Jumpwire, Triangle2D<num_type>, Box2D<num_type>, Polygon2D<num_type>, PolygonWithHoles2D<num_type> >;
    using GeometryContainer = std::vector<Geometry>;
    using GeomIndexContainer = std::vector<index_t>;
    using LayerGeometriesMap = std::unordered_map<index_t, GeomIndexContainer>;
    using LayerConnections = UndirectedIndexEdgeSet;
    using GeomConnections = std::unordered_map<index_t, std::unordered_set<index_t> >;
    using JumpwireContainer = std::vector<index_t>;
    using RtValue = std::pair<Box2D<num_type>, index_t>;
    using Rtree = boost::geometry::index::rtree<RtValue, boost::geometry::index::dynamic_rstar>;

    class BoxExtent : public boost::static_visitor<Box2D<num_type> >
    {
    public:
        Box2D<num_type> operator() (const Jumpwire & jumpwire) const { return Extent(std::get<3>(jumpwire)); }
        Box2D<num_type> operator() (const Triangle2D<num_type> & tri) const { return Extent(tri); }
        Box2D<num_type> operator() (const Box2D<num_type> & box) const { return box; }
        Box2D<num_type> operator() (const Polygon2D<num_type> & polygon) const { return Extent(polygon); }
        Box2D<num_type> operator() (const PolygonWithHoles2D<num_type> & pwh) const { return Extent(pwh); }
    };

    class PointInside : public boost::static_visitor<bool>
    {
        Point2D<num_type> m_point;
    public:
        explicit PointInside(const Point2D<num_type> & p) : m_point(p) { }
        bool operator() (const Jumpwire & ) const { return false; }
        bool operator() (const Triangle2D<num_type> & tri) const { return Contains(tri, m_point, true); }
        bool operator() (const Box2D<num_type> & box) const { return Contains(box, m_point, true); }
        bool operator() (const Polygon2D<num_type> & polygon) const { return Contains(polygon, m_point, true); }
        bool operator() (const PolygonWithHoles2D<num_type> & pwh) const
        {
            if(!Contains(pwh.outline, m_point, true)) return false;
            for(const auto & hole : pwh.holes){
                if(Contains(hole, m_point, false)) return false;
            }
            return true;
        }
    };

public:
    /** @brief geometry at nullLayer connect nothing */
    const static index_t nullLayer = std::numeric_limits<index_t>::max();
    virtual ~ConnectivityExtractor() = default;

    /**
     * @brief add object to connectivity extractor
     *
     * @tparam T object type, could be object, reference or pointer
     * @tparam GeomGetter a functor type to convert T to internal geometry type
     * @note the GeomGetter functor should have operator() (const & T) function, the return type should be one of the Triangle2D, Box2D, Polygon2D, PolygonWithHoles2D
     * @note the coor of the return type should be consitant with class template num_type
     * @param layer the index of the layer the obj belongs to
     * @param obj the geometry object for connectivity extraction
     * @param getter the GeomGetter functor object to convert T to internal geometry type
     * @return index_t global index of obj in the extraction result graph
     */
    template <typename T, typename GeomGetter,
              typename std::enable_if<traits::is_2d_surf_geom_t<
              typename std::result_of<GeomGetter(const T&)>::type>::value, bool>::type = true>
    index_t AddObject(index_t layer, const T & obj, GeomGetter && getter)
    {
        using coor_t = typename std::result_of<GeomGetter(const T&)>::type::coor_t;
        static_assert(std::is_same<coor_t, num_type>::value, "different coordinate type of GeomGetter return type and ConnectivityExtractor!");

        index_t index = m_geometries.size();
        m_geometries.push_back(getter(obj));
        auto iter = m_layerGeoms.find(layer);
        if(iter == m_layerGeoms.end())
            iter = m_layerGeoms.insert(std::make_pair(layer, GeomIndexContainer())).first;
        iter->second.push_back(index);
        return index;
    }

    /**
     * @brief adds objects to connectivity extractor by iterator
     *
     * @tparam Iterator objects container iterator
     * @param begin the begin iterator of objects container
     * @param end the end iterator of objects container
     * @return std::pair<index_t, index_t> global indices range from iterator begin to end in the extraction result graph
     */
    template <typename Iterator, typename GeomGetter,
              typename std::enable_if<traits::is_2d_surf_geom_t<
              typename std::result_of<GeomGetter(const typename Iterator::value_type&)>::type>::value, bool>::type = true>
    std::pair<index_t, index_t> AddObjects(index_t layer, Iterator begin, Iterator end, GeomGetter && getter)
    {
        using coor_t = typename std::result_of<GeomGetter(const typename Iterator::value_type&)>::type::coor_t;
        static_assert(std::is_same<coor_t, num_type>::value, "different coordinate type of GeomGetter return type and ConnectivityExtractor!");

        auto iter = m_layerGeoms.find(layer);
        if(iter == m_layerGeoms.end())
            iter = m_layerGeoms.insert(std::make_pair(layer, GeomIndexContainer())).first;

        index_t index = m_geometries.size();
        auto range = std::make_pair(index, index);
        for(auto it = begin; it != end; ++it){
            m_geometries.push_back(getter(*it));
            iter->second.push_back(range.second++);
        }
        range.second--;
        return range;
    }

    /**
     * @brief adds connection that connected by jumpwire
     *
     * @param layer1 start layer of jumpwire
     * @param layer2 end layer of jumpwire
     * @param jumpwire segment with start/end point that connect two geometries
     * @return index_t global index of obj in the extraction result graph
     */
    index_t AddJumpwire(index_t layer1, index_t layer2, Segment2D<num_type> jumpwire)
    {
        index_t index = m_geometries.size();
        m_geometries.push_back(std::make_tuple(index, layer1, layer2, jumpwire));
        m_jumpwires.push_back(index);
        return index;
    }

    /**
     * @brief adds connection b/w two layers
     *
     * @param layer1 index of layer 1
     * @param layer2 index of layer 2
     */
    void AddLayerConnection(index_t layer1, index_t layer2)
    {
        auto nullLayer = ConnectivityExtractor::nullLayer;
        if(layer1 != nullLayer && layer2 != nullLayer)
            m_layerConns.insert({layer1, layer2});
    }

    /**
     * @brief extracts connection result of input objects.
     *
     * @param threads threads number allowed in multi-threading extraction.
     * @return std::unique_ptr<GeomConnGraph> the extraction result, represented by a spares index graph
     * @note function return nullptr if no geometry exists.
     */
    std::unique_ptr<GeomConnGraph> Extract(index_t threads = 1)
    {
        if(0 == m_geometries.size()) return nullptr;
        auto graph = std::make_unique<GeomConnGraph>(m_geometries.size());

        std::unordered_set<index_t> unconnectedLayers;
        for(const auto & layerGeom : m_layerGeoms)
            unconnectedLayers.insert(layerGeom.first);

        std::list<std::future<std::unique_ptr<GeomConnections> > > futures;
        thread::ThreadPool pool(threads);

        //external
        for(const auto & layerConn : m_layerConns){
            const auto & layer1 = layerConn.v1();
            const auto & layer2 = layerConn.v2();
            if(m_layerGeoms.count(layer1) && m_layerGeoms.count(layer2)){
                auto connection = pool.Submit(std::bind(&ConnectivityExtractor<num_type>::ExtractLayersConnection, this, layer1, layer2));
                futures.emplace_back(std::move(connection));
                unconnectedLayers.erase(layer1);
                unconnectedLayers.erase(layer2);
            }
        }

        //internal
        auto nullLayer = ConnectivityExtractor::nullLayer;
        unconnectedLayers.erase(nullLayer);
        for(const auto & layer : unconnectedLayers){
            auto connection = pool.Submit(std::bind(&ConnectivityExtractor<num_type>::ExtractLayerConnection, this, layer));
            futures.emplace_back(std::move(connection));
        }

        for(auto & future : futures){
            auto connection = future.get();
            AddConnection(*graph, std::move(connection));
        }

        //jumpwires
        ExtractJumpwiresConnection(*graph, threads);

        return graph;
    }

    /// @brief clears former geometries, layer connections and jumpwires
    void Clear()
    {
        m_layerGeoms.clear();
        m_geometries.clear();
        m_layerConns.clear();
        m_jumpwires.clear();
    }

private:
    void FillLayerGeometries(index_t layer, std::list<index_t> & geomIndices)
    {
        if(m_layerGeoms.count(layer)){
            const auto & geoms = m_layerGeoms[layer];
            geomIndices.insert(geomIndices.end(), geoms.begin(), geoms.end());
        }
    }

    void AddConnection(GeomConnGraph & graph, std::unique_ptr<GeomConnections> connection)
    {
        if(nullptr == connection) return;
        for(const auto & conn : *connection){
            index_t i = conn.first;
            for(index_t j : conn.second)
                AddEdge(i, j, graph);
        }
    }

    std::unique_ptr<GeomConnections> ExtractLayerConnection(index_t layer)
    {
        std::list<index_t> geomIndices;
        FillLayerGeometries(layer, geomIndices);
        return ExtractGeomConnection(geomIndices);
    }

    std::unique_ptr<GeomConnections> ExtractLayersConnection(index_t layer1, index_t layer2)
    {
        std::list<index_t> geomIndices;
        FillLayerGeometries(layer1, geomIndices);
        FillLayerGeometries(layer2, geomIndices);
        return ExtractGeomConnection(geomIndices);
    }

    std::unique_ptr<GeomConnections> ExtractGeomConnection(const std::list<index_t> & geomIndices)
    {
        index_t size = geomIndices.size();
        if(0 == size) return nullptr;

        std::vector<std::set<int> > tmp(size);
        std::unordered_map<int, index_t> idxMap;
        boost::polygon::connectivity_extraction<num_type> ce;
        auto addOneGeom = [this, &ce](index_t index)
        {
            int id = -1;
            if(auto * tri = boost::get<Triangle2D<num_type> >(&m_geometries[index]))
                id = ce.template insert<Triangle2D<num_type> >(*tri);
            else if(auto * box = boost::get<Box2D<num_type> >(&m_geometries[index]))
                id = ce.template insert<Box2D<num_type> >(*box);
            else if(auto * poly = boost::get<Polygon2D<num_type> >(&m_geometries[index]))
                id = ce.template insert<Polygon2D<num_type> >(*poly);
            else if(auto * pwh = boost::get<PolygonWithHoles2D<num_type> >(&m_geometries[index]))
                id = ce.template insert<PolygonWithHoles2D<num_type> >(*pwh);
            else GENERIC_THROW(std::runtime_error("unknow geometry type!"))

            return id;
        };

        for(const auto & index : geomIndices){
            idxMap.insert(std::make_pair(addOneGeom(index), index));
        }
        ce.extract(tmp);

        auto connection = std::make_unique<GeomConnections>();
        for(int i = 0; i < static_cast<int>(tmp.size()); ++i){
            if(tmp.at(i).empty()) continue;
            if(!(connection->count(idxMap.at(i))))
                connection->insert(std::make_pair(idxMap.at(i), std::unordered_set<index_t>()));
            auto & connectObj = connection->at(idxMap.at(i));
            for(auto j : tmp.at(i)){
                connectObj.insert(idxMap.at(j));
            }
        }
        return connection;
    }

    void ExtractJumpwiresConnection(GeomConnGraph & graph, index_t threads)
    {
        std::unordered_set<index_t> layers;
        for(auto index : m_jumpwires){
            if(auto * jumpwire = boost::get<Jumpwire>(&m_geometries[index])){
                layers.insert(std::get<1>(*jumpwire));
                layers.insert(std::get<2>(*jumpwire));
            }
            else GENERIC_THROW(std::runtime_error("unknow jumpwire type!"))
        }

        if(layers.empty()) return;

        auto trees = BuildGeomRtreeForLayers(layers, threads);
        for(auto index : m_jumpwires){
            if(auto * jumpwire = boost::get<Jumpwire>(&m_geometries[index]))
                ExtractJumpwireConnection(trees, *jumpwire, graph);
            else GENERIC_THROW(std::runtime_error("unknow jumpwire type!"))
        }
    }

    void ExtractJumpwireConnection(const std::unordered_map<index_t, std::shared_ptr<Rtree> > & trees, Jumpwire & jumpwire, GeomConnGraph & graph)
    {
        auto layer1 = std::get<1>(jumpwire);
        auto layer2 = std::get<2>(jumpwire);
        if(!trees.count(layer1) || !trees.count(layer2)) return;

        auto tree1 = trees.at(layer1);
        auto tree2 = trees.at(layer2);

        std::vector<index_t> geoms;
        auto index = std::get<0>(jumpwire);
        const auto & segment = std::get<3>(jumpwire);

        GetJumpwireConnectGeoms(segment[0], tree1, geoms);
        for(auto geom : geoms) AddEdge(index, geom, graph);

        GetJumpwireConnectGeoms(segment[1], tree2, geoms);
        for(auto geom : geoms) AddEdge(index, geom, graph);
    }

    void GetJumpwireConnectGeoms(const Point2D<num_type> & p, std::shared_ptr<Rtree> tree, std::vector<index_t> & geoms)
    {
        geoms.clear();
        if(nullptr == tree) return;
        Box2D<num_type> searchBox(p, p);
        std::vector<RtValue> boardPhase;
        tree->query(boost::geometry::index::intersects(searchBox), std::back_inserter(boardPhase));

        PointInside inside(p);
        for(auto value : boardPhase){
            if(boost::apply_visitor(inside, m_geometries.at(value.second)))
                geoms.push_back(value.second);
        }
    }

    std::unordered_map<index_t, std::shared_ptr<Rtree> > BuildGeomRtreeForLayers(const std::unordered_set<index_t> & layers, index_t threads)
    {
        std::unordered_map<index_t, std::shared_ptr<Rtree> > trees;
        std::list<std::future<std::pair<index_t, std::shared_ptr<Rtree> > > > futures;
        thread::ThreadPool pool(threads);

        for(auto layer : layers){
            auto result = pool.Submit(std::bind(&ConnectivityExtractor<num_type>::BuildGeomRtreeEachLayer, this, layer));
            futures.emplace_back(std::move(result));
        }

        for(auto & future : futures)
            trees.insert(future.get());
        return trees;
    }

    std::pair<index_t, std::shared_ptr<Rtree> > BuildGeomRtreeEachLayer(index_t layer) const
    {
        auto nullLayer = ConnectivityExtractor::nullLayer;
        if(layer == nullLayer || !m_layerGeoms.count(layer))
            return std::make_pair(layer, nullptr);

        boost::geometry::index::dynamic_rstar para(16);
        auto tree = std::make_shared<Rtree>(para);

        BoxExtent ext;
        const auto & geomIndices = m_layerGeoms.at(layer);
        for(auto index : geomIndices){
            auto bbox = boost::apply_visitor(ext, m_geometries.at(index));
            tree->insert(std::make_pair(bbox, index));
        }

        return std::make_pair(layer, tree);
    }

private:
    LayerGeometriesMap m_layerGeoms;
    GeometryContainer  m_geometries;
    LayerConnections   m_layerConns;
    JumpwireContainer  m_jumpwires;
};

}//namespace geometry
}//namespace generic

#endif//GENERIC_GEOMETRY_CONNECTIVITY_HPP


#include <algorithm>
#include <chrono>
#include <cmath>
#include <fstream>
#include <iostream>
#include <numeric>
#include <sstream>
#include <vector>

#include <immintrin.h>

// AVX2/AVX512 aligned memory access requires 256/512 bit alignment. Also Intel
// processors typically use 64 byte (i.e. 512 bit) cache alignment. Hence 512
// bit alignment is a safe choice for both correctness and performance.
#include <boost/align/aligned_allocator.hpp>
template <typename T>
using vec = std::vector<T, boost::alignment::aligned_allocator<T, 512>>;

const int32_t MAX = std::numeric_limits<int32_t>::max() / 2;

enum State : int32_t {
    STATE_UPPER = -1,
    STATE_TREE  =  0,
    STATE_LOWER =  1
};

enum Dir : int32_t {
    DIR_DOWN = -1,
    DIR_UP   =  1
};

static std::chrono::duration<double> find_entering_arc_time;
static std::chrono::duration<double> find_join_node_time;
static std::chrono::duration<double> find_leaving_arc_time;
static std::chrono::duration<double> change_flows_time;
static std::chrono::duration<double> change_states_time;
static std::chrono::duration<double> update_tree_time;
static std::chrono::duration<double> update_pots_time;

struct Graph {
    vec<int32_t> tails;
    vec<int32_t> heads;
    vec<int32_t> flows;
    vec<int32_t> uppers;
    vec<int32_t> costs;
    vec<State>   states;

    vec<int32_t> diffs;
    vec<int32_t> preds;
    vec<Dir>     dirs;
    vec<int32_t> parents;
    vec<int32_t> threads;
    vec<int32_t> rev_threads;
    vec<int32_t> num_succs;
    vec<int32_t> last_succs;
    vec<int32_t> pots;

    int32_t n, N;
    int32_t m, M;

    int32_t root;

    int32_t a_in, i_in, j_in, i_out, j_out;
    int32_t i_join;
    int32_t delta;
    bool    change;

    int32_t a_curr;
    int32_t block_size;

    vec<int32_t> dirty_revs;
};

static Graph
read_dimacs(std::istream& inp)
{
    Graph graph;

    char c;

    std::string p;
    std::string line;

    while (getline(inp, line)) {
        std::istringstream ss(line);
        ss >> c;
        switch (c) {
        case 'p': // problem line
            ss >> p >> graph.n >> graph.m;
            break;
        }
        if (inp.peek() == 'n' || inp.peek() == 'a') { break; }
    }

    graph.N = graph.n + 1;
    graph.M = graph.m + graph.n;

    graph.tails.resize       (graph.M);
    graph.heads.resize       (graph.M);
    graph.flows.resize       (graph.M);
    graph.uppers.resize      (graph.M);
    graph.costs.resize       (graph.M);
    graph.states.resize      (graph.M);

    graph.diffs.resize       (graph.N);
    graph.preds.resize       (graph.N);
    graph.dirs.resize        (graph.N);
    graph.parents.resize     (graph.N);
    graph.threads.resize     (graph.N);
    graph.rev_threads.resize (graph.N);
    graph.num_succs.resize   (graph.N);
    graph.last_succs.resize  (graph.N);
    graph.pots.resize        (graph.N);

    int32_t node;
    int32_t diff;

    int32_t tail, head;
    int32_t lower, upper;
    int32_t cost;

    int32_t curr = 0;
    while (getline(inp, line)) {
        std::istringstream ss(line);
        ss >> c;
        switch (c) {
        case 'n': // node descriptor
            ss >> node >> diff;
            graph.diffs[node-1] = diff;
            break;
        case 'a': // arc descriptor
            ss >> tail >> head >> lower >> upper >> cost;
            graph.tails  [curr] = tail - 1;
            graph.heads  [curr] = head - 1;
            graph.flows  [curr] = 0;
            graph.uppers [curr] = upper;
            graph.costs  [curr] = cost;
            graph.states [curr] = STATE_LOWER;
            ++curr;
            break;
        }
    }

    graph.root = graph.n;

    graph.parents     [graph.root] = -1;
    graph.preds       [graph.root] = -1;
    graph.threads     [graph.root] = 0;
    graph.rev_threads [0]          = graph.root;
    graph.num_succs   [graph.root] = graph.N;
    graph.last_succs  [graph.root] = graph.root - 1;
    graph.diffs       [graph.root] = 0;
    graph.pots        [graph.root] = 0;

    for (int32_t i = 0, a = graph.m; i < graph.n; ++i, ++a) {
        graph.parents     [i]   = graph.root;
        graph.preds       [i]   = a;
        graph.threads     [i]   = i + 1;
        graph.rev_threads [i+1] = i;
        graph.num_succs   [i]   = 1;
        graph.last_succs  [i]   = i;
        graph.uppers      [a]   = MAX;
        graph.states      [a]   = STATE_TREE;
        if (graph.diffs[i] >= 0) {
            graph.dirs  [i] = DIR_UP;
            graph.pots  [i] = 0;
            graph.tails [a] = i;
            graph.heads [a] = graph.root;
            graph.flows [a] = graph.diffs[i];
            graph.costs [a] = 0;
        } else {
            graph.dirs  [i] = DIR_DOWN;
            graph.pots  [i] = MAX;
            graph.tails [a] = graph.root;
            graph.heads [a] = i;
            graph.flows [a] = -graph.diffs[i];
            graph.costs [a] = MAX;
        }
    }

    graph.a_curr = 0;

    return graph;
}

static void
print_solution(Graph& graph, bool flows)
{
    long solution = 0;
    for (int32_t a = 0; a < graph.m; ++a) {
        if (graph.costs[a] > 0) {
            if (flows) {
                std::cout
                    << "f"
                    << " " << graph.tails[a] + 1
                    << " " << graph.heads[a] + 1
                    << " " << graph.flows[a]
                    << "\n";
            }
            solution += (long)graph.flows[a] * (long)graph.costs[a];
        }
    }
    std::cout << "s " << solution << std::endl;
}

struct elem_t {
    int32_t ind;
    int32_t val;
};

#if defined(OMP)
#pragma omp declare \
    reduction(min : elem_t : omp_out = omp_in.val < omp_out.val ? omp_in : omp_out) \
    initializer(omp_priv = {0, 0})

static elem_t
find_entering_arc_range_omp(Graph& graph, int32_t beg_a, int32_t end_a)
{
    elem_t min = {0, 0};

#pragma omp parallel for reduction(min:min)
    for (int32_t a = beg_a; a < end_a; ++a) {
        int32_t c = graph.states[a] *
            (graph.costs[a] +
             graph.pots[graph.tails[a]] -
             graph.pots[graph.heads[a]]);
        if (min.val > c) {
            min.val = c;
            min.ind = a;
        }
    }

    return min;
}
#elif defined(AVX2)
static elem_t
find_entering_arc_range_avx2(Graph& graph, int32_t beg_a, int32_t end_a)
{
    int32_t a;

    elem_t min = {0, 0};

    __m256i incs = _mm256_set1_epi32(8);
    __m256i inds = _mm256_setr_epi32(beg_a, beg_a+1, beg_a+2, beg_a+3, beg_a+4, beg_a+5, beg_a+6, beg_a+7);
    __m256i mininds = inds;
    __m256i minvals = _mm256_set1_epi32(0);

    for (a = beg_a; a < end_a-8; a += 8) {
        __m256i tail  = _mm256_load_si256((__m256i*)&graph.tails[a]);
        __m256i head  = _mm256_load_si256((__m256i*)&graph.heads[a]);
        __m256i state = _mm256_load_si256((__m256i*)&graph.states[a]);
        __m256i cost  = _mm256_load_si256((__m256i*)&graph.costs[a]);
        __m256i pot_tail = _mm256_i32gather_epi32(&graph.pots[0], tail, 4);
        __m256i pot_head = _mm256_i32gather_epi32(&graph.pots[0], head, 4);
        cost = _mm256_add_epi32(cost, pot_tail);
        cost = _mm256_sub_epi32(cost, pot_head);
        cost = _mm256_mullo_epi32(cost, state);
        __m256i mask = _mm256_cmpgt_epi32(minvals, cost);
        mininds = _mm256_blendv_epi8(mininds, inds, mask);
        minvals = _mm256_min_epi32(cost, minvals);
        inds = _mm256_add_epi32(inds, incs);
    }

    int32_t* inds_iptr = (int32_t*)&mininds;
    int32_t* vals_iptr = (int32_t*)&minvals;

    for (int32_t i = 0; i < 8; i++) {
        if (min.val > vals_iptr[i]) {
            min.val = vals_iptr[i];
            min.ind = inds_iptr[i];
        }
    }

    for (; a < end_a; ++a) {
        int32_t c = graph.states[a] *
            (graph.costs[a] +
             graph.pots[graph.tails[a]] -
             graph.pots[graph.heads[a]]);
        if (min.val > c) {
            min.val = c;
            min.ind = a;
        }
    }

    return min;
}
#elif defined(OMP_AVX2)
struct elem_m256i_t {
    __m256i ind;
    __m256i val;
};

static void
min_m256i_f(elem_m256i_t& out, const elem_m256i_t& in)
{
    __m256i mask = _mm256_cmpgt_epi32(out.val, in.val);
    out.ind = _mm256_blendv_epi8(out.ind, in.ind, mask);
    out.val = _mm256_min_epi32(in.val, out.val);
}

#pragma omp declare \
    reduction(min : elem_m256i_t : min_m256i_f(omp_out, omp_in)) \
    initializer(omp_priv = {_mm256_set1_epi32(0), _mm256_set1_epi32(0)})

static elem_t
find_entering_arc_range_omp_avx2(Graph& graph, int32_t beg_a, int32_t end_a)
{
    int32_t a;

    elem_t min = {0, 0};

    elem_m256i_t min_m256i = {_mm256_set1_epi32(0), _mm256_set1_epi32(0)};

#pragma omp parallel for reduction(min:min_m256i) lastprivate(a)
    for (a = beg_a; a < end_a-8; a += 8) {
        __m256i inds  = _mm256_setr_epi32(a, a+1, a+2, a+3, a+4, a+5, a+6, a+7);
        __m256i tail  = _mm256_load_si256((__m256i*)&graph.tails[a]);
        __m256i head  = _mm256_load_si256((__m256i*)&graph.heads[a]);
        __m256i state = _mm256_load_si256((__m256i*)&graph.states[a]);
        __m256i cost  = _mm256_load_si256((__m256i*)&graph.costs[a]);
        __m256i pot_tail = _mm256_i32gather_epi32(&graph.pots[0], tail, 4);
        __m256i pot_head = _mm256_i32gather_epi32(&graph.pots[0], head, 4);
        cost = _mm256_add_epi32(cost, pot_tail);
        cost = _mm256_sub_epi32(cost, pot_head);
        cost = _mm256_mullo_epi32(cost, state);
        min_m256i_f(min_m256i, elem_m256i_t{inds, cost});
    }

    int32_t* inds_iptr = (int32_t*)&min_m256i.ind;
    int32_t* vals_iptr = (int32_t*)&min_m256i.val;

    for (int32_t i = 0; i < 8; i++) {
        if (min.val > vals_iptr[i]) {
            min.val = vals_iptr[i];
            min.ind = inds_iptr[i];
        }
    }

    for (; a < end_a; ++a) {
        int32_t c = graph.states[a] *
            (graph.costs[a] +
             graph.pots[graph.tails[a]] -
             graph.pots[graph.heads[a]]);
        if (min.val > c) {
            min.val = c;
            min.ind = a;
        }
    }

    return min;
}
#elif defined(AVX512)
static elem_t
find_entering_arc_range_avx512(Graph& graph, int32_t beg_a, int32_t end_a)
{
    int32_t a;

    elem_t min = {0, 0};

    __m512i incs = _mm512_set1_epi32(16);
    __m512i inds = _mm512_setr_epi32(beg_a, beg_a+1, beg_a+2, beg_a+3, beg_a+4, beg_a+5, beg_a+6, beg_a+7, beg_a+8, beg_a+9, beg_a+10, beg_a+11, beg_a+12, beg_a+13, beg_a+14, beg_a+15);
    __m512i mininds = inds;
    __m512i minvals = _mm512_set1_epi32(0);

    for (a = beg_a; a < end_a-16; a += 16) {
        __m512i tail  = _mm512_load_si512((__m512i*)&graph.tails[a]);
        __m512i head  = _mm512_load_si512((__m512i*)&graph.heads[a]);
        __m512i state = _mm512_load_si512((__m512i*)&graph.states[a]);
        __m512i cost  = _mm512_load_si512((__m512i*)&graph.costs[a]);
        __m512i pot_tail = _mm512_i32gather_epi32(tail, &graph.pots[0], 4);
        __m512i pot_head = _mm512_i32gather_epi32(head, &graph.pots[0], 4);
        cost = _mm512_add_epi32(cost, pot_tail);
        cost = _mm512_sub_epi32(cost, pot_head);
        cost = _mm512_mullo_epi32(cost, state);
        __mmask16 mask = _mm512_cmpgt_epi32_mask(minvals, cost);
        mininds = _mm512_mask_blend_epi32(mask, mininds, inds);
        minvals = _mm512_min_epi32(cost, minvals);
        inds = _mm512_add_epi32(inds, incs);
    }

    int32_t* inds_iptr = (int32_t*)&mininds;
    int32_t* vals_iptr = (int32_t*)&minvals;

    for (int32_t i = 0; i < 16; i++) {
        if (min.val > vals_iptr[i]) {
            min.val = vals_iptr[i];
            min.ind = inds_iptr[i];
        }
    }

    for (; a < end_a; ++a) {
        int32_t c = graph.states[a] *
            (graph.costs[a] +
             graph.pots[graph.tails[a]] -
             graph.pots[graph.heads[a]]);
        if (min.val > c) {
            min.val = c;
            min.ind = a;
        }
    }

    return min;
}
#elif defined(OMP_AVX512)
struct elem_m512i_t {
    __m512i ind;
    __m512i val;
};

void
min_m512i_f(elem_m512i_t& out, const elem_m512i_t& in)
{
    __mmask16 mask = _mm512_cmpgt_epi32_mask(out.val, in.val);
    out.ind = _mm512_mask_blend_epi32(mask, out.ind, in.ind);
    out.val = _mm512_min_epi32(in.val, out.val);
}

#pragma omp declare \
    reduction(min : elem_m512i_t : min_m512i_f(omp_out, omp_in)) \
    initializer(omp_priv = {_mm512_set1_epi32(0), _mm512_set1_epi32(0)})

static elem_t
find_entering_arc_range_omp_avx512(Graph& graph, int32_t beg_a, int32_t end_a)
{
    int32_t a;

    elem_t min = {0, 0};

    elem_m512i_t min_m512i = {_mm512_set1_epi32(0), _mm512_set1_epi32(0)};

#pragma omp parallel for reduction(min:min_m512i) lastprivate(a)
    for (a = beg_a; a < end_a-16; a += 16) {
        __m512i inds  = _mm512_setr_epi32(a, a+1, a+2, a+3, a+4, a+5, a+6, a+7, a+8, a+9, a+10, a+11, a+12, a+13, a+14, a+15);
        __m512i tail  = _mm512_load_si512((__m512i*)&graph.tails[a]);
        __m512i head  = _mm512_load_si512((__m512i*)&graph.heads[a]);
        __m512i state = _mm512_load_si512((__m512i*)&graph.states[a]);
        __m512i cost  = _mm512_load_si512((__m512i*)&graph.costs[a]);
        __m512i pot_tail = _mm512_i32gather_epi32(tail, &graph.pots[0], 4);
        __m512i pot_head = _mm512_i32gather_epi32(head, &graph.pots[0], 4);
        cost = _mm512_add_epi32(cost, pot_tail);
        cost = _mm512_sub_epi32(cost, pot_head);
        cost = _mm512_mullo_epi32(cost, state);
        min_m512i_f(min_m512i, elem_m512i_t{inds, cost});
    }

    int32_t* inds_iptr = (int32_t*)&min_m512i.ind;
    int32_t* vals_iptr = (int32_t*)&min_m512i.val;

    for (int32_t i = 0; i < 16; i++) {
        if (min.val > vals_iptr[i]) {
            min.val = vals_iptr[i];
            min.ind = inds_iptr[i];
        }
    }

    for (; a < end_a; ++a) {
        int32_t c = graph.states[a] *
            (graph.costs[a] +
             graph.pots[graph.tails[a]] -
             graph.pots[graph.heads[a]]);
        if (min.val > c) {
            min.val = c;
            min.ind = a;
        }
    }

    return min;
}
#else
static elem_t
find_entering_arc_range(Graph& graph, int32_t beg_a, int32_t end_a)
{
    elem_t min = {0, 0};

    for (int32_t a = beg_a; a < end_a; ++a) {
        int32_t c = graph.states[a] *
            (graph.costs[a] +
             graph.pots[graph.tails[a]] -
             graph.pots[graph.heads[a]]);
        if (min.val > c) {
            min.val = c;
            min.ind = a;
        }
    }

    return min;
}
#endif

static bool
find_entering_arc(Graph& graph)
{
    auto beg_time = std::chrono::steady_clock::now();

    int32_t end_a = graph.a_curr;

    elem_t min = {0, 0};

    for (int32_t beg_a = graph.a_curr; beg_a < graph.m; beg_a = end_a) {
        end_a = beg_a + graph.block_size;

        if (end_a > graph.m) {
            end_a = graph.m;
        }

#if defined(OMP)
        min = find_entering_arc_range_omp(graph, beg_a, end_a);
#elif defined(AVX2)
        min = find_entering_arc_range_avx2(graph, beg_a, end_a);
#elif defined(OMP_AVX2)
        min = find_entering_arc_range_omp_avx2(graph, beg_a, end_a);
#elif defined(AVX512)
        min = find_entering_arc_range_avx512(graph, beg_a, end_a);
#elif defined(OMP_AVX512)
        min = find_entering_arc_range_omp_avx512(graph, beg_a, end_a);
#else
        min = find_entering_arc_range(graph, beg_a, end_a);
#endif

        if (min.val < 0) { break; }
    }

    if (min.val >= 0) {
        for (int32_t beg_a = 0; beg_a < graph.a_curr; beg_a = end_a) {
            end_a = beg_a + graph.block_size;

            if (end_a > graph.a_curr) {
                end_a = graph.a_curr;
            }

#if defined(OMP)
            min = find_entering_arc_range_omp(graph, beg_a, end_a);
#elif defined(AVX2)
            min = find_entering_arc_range_avx2(graph, beg_a, end_a);
#elif defined(OMP_AVX2)
            min = find_entering_arc_range_omp_avx2(graph, beg_a, end_a);
#elif defined(AVX512)
            min = find_entering_arc_range_avx512(graph, beg_a, end_a);
#elif defined(OMP_AVX512)
            min = find_entering_arc_range_omp_avx512(graph, beg_a, end_a);
#else
            min = find_entering_arc_range(graph, beg_a, end_a);
#endif

            if (min.val < 0) { break; }
        }
    }

    graph.a_in = min.ind;

    graph.a_curr = end_a;

    find_entering_arc_time += std::chrono::steady_clock::now() - beg_time;

    return min.val < 0;
}

static void
find_join_node(Graph& graph)
{
    auto beg_time = std::chrono::steady_clock::now();

    int32_t i = graph.tails[graph.a_in];
    int32_t j = graph.heads[graph.a_in];

    while (i != j) {
        if (graph.num_succs[i] < graph.num_succs[j]) {
            i = graph.parents[i];
        } else {
            j = graph.parents[j];
        }
    }

    graph.i_join = i;

    find_join_node_time += std::chrono::steady_clock::now() - beg_time;
}

static void
find_leaving_arc(Graph& graph)
{
    auto beg_time = std::chrono::steady_clock::now();

    int32_t first, second;

    if (graph.states[graph.a_in] == STATE_LOWER) {
        first  = graph.tails[graph.a_in];
        second = graph.heads[graph.a_in];
    } else {
        first  = graph.heads[graph.a_in];
        second = graph.tails[graph.a_in];
    }

    graph.delta = graph.uppers[graph.a_in];

    int32_t result = 0;

    for (int32_t i = first; i != graph.i_join; i = graph.parents[i]) {
        int32_t a = graph.preds[i];
        int32_t f = graph.flows[a];
        if (graph.dirs[i] == DIR_DOWN) {
            int32_t u = graph.uppers[a];
            f = u >= MAX ? MAX : u - f;
        }
        if (f < graph.delta) {
            graph.delta = f;
            graph.i_out = i;
            result = 1;
        }
    }

    for (int32_t i = second; i != graph.i_join; i = graph.parents[i]) {
        int32_t a = graph.preds[i];
        int32_t f = graph.flows[a];
        if (graph.dirs[i] == DIR_UP) {
            int32_t u = graph.uppers[a];
            f = u >= MAX ? MAX : u - f;
        }
        if (f <= graph.delta) {
            graph.delta = f;
            graph.i_out = i;
            result = 2;
        }
    }

    if (result == 1) {
        graph.i_in = first;
        graph.j_in = second;
    } else {
        graph.i_in = second;
        graph.j_in = first;
    }

    graph.change = (result != 0);

    find_leaving_arc_time += std::chrono::steady_clock::now() - beg_time;
}

static void
change_flows(Graph& graph)
{
    if (graph.delta == 0) { return; }

    auto beg_time = std::chrono::steady_clock::now();

    int32_t f = graph.states[graph.a_in] * graph.delta;

    graph.flows[graph.a_in] += f;

    for (int32_t i = graph.tails[graph.a_in];
            i != graph.i_join;
            i = graph.parents[i]) {
        graph.flows[graph.preds[i]] -= graph.dirs[i] * f;
    }

    for (int32_t i = graph.heads[graph.a_in];
            i != graph.i_join;
            i = graph.parents[i]) {
        graph.flows[graph.preds[i]] += graph.dirs[i] * f;
    }

    change_flows_time += std::chrono::steady_clock::now() - beg_time;
}

static void
change_states(Graph& graph)
{
    auto beg_time = std::chrono::steady_clock::now();

    int32_t a_in = graph.a_in;
    int32_t a_out = graph.preds[graph.i_out];

    if (graph.change) {
        graph.states[a_in] = STATE_TREE;
        graph.states[a_out] = graph.flows[a_out] == 0 ? STATE_LOWER : STATE_UPPER;
    } else {
        graph.states[a_in] = (State)(-graph.states[a_in]);
    }

    change_states_time += std::chrono::steady_clock::now() - beg_time;
}

// adapted from lemon-1.3.1/lemon/network_simplex.h
static void
update_tree(Graph& graph)
{
    auto beg_time = std::chrono::steady_clock::now();

    int32_t old_rev_thread = graph.rev_threads[graph.i_out];
    int32_t old_succ_num = graph.num_succs[graph.i_out];
    int32_t old_last_succ = graph.last_succs[graph.i_out];
    graph.j_out = graph.parents[graph.i_out];

    // Check if i_in and i_out coincide
    if (graph.i_in == graph.i_out) {
        // Update parent, pred, dir
        graph.parents[graph.i_in] = graph.j_in;
        graph.preds[graph.i_in] = graph.a_in;
        graph.dirs[graph.i_in] = graph.i_in == graph.tails[graph.a_in] ?
            DIR_UP : DIR_DOWN;

        // Update thread and rev_thread
        if (graph.threads[graph.j_in] != graph.i_out) {
            int32_t after = graph.threads[old_last_succ];
            graph.threads[old_rev_thread] = after;
            graph.rev_threads[after] = old_rev_thread;
            after = graph.threads[graph.j_in];
            graph.threads[graph.j_in] = graph.i_out;
            graph.rev_threads[graph.i_out] = graph.j_in;
            graph.threads[old_last_succ] = after;
            graph.rev_threads[after] = old_last_succ;
        }
    } else {
        // Handle the case when old_rev_thread equals to j_in
        // (it also means that join and j_out coincide)
        int32_t thread_continue = old_rev_thread == graph.j_in ?
            graph.threads[old_last_succ] : graph.threads[graph.j_in];

        // Update _thread and _parent along the stem nodes (i.e. the nodes
        // between i_in and i_out, whose parent have to be changed)
        int32_t stem = graph.i_in;                    // the current stem node
        int32_t par_stem = graph.j_in;                // the new parent of stem
        int32_t next_stem;                            // the next stem node
        int32_t last = graph.last_succs[graph.i_in];  // the last successor of stem
        int32_t before, after = graph.threads[last];
        graph.threads[graph.j_in] = graph.i_in;
        graph.dirty_revs.clear();
        graph.dirty_revs.push_back(graph.j_in);
        while (stem != graph.i_out) {
            // Insert the next stem node into the thread list
            next_stem = graph.parents[stem];
            graph.threads[last] = next_stem;
            graph.dirty_revs.push_back(last);

            // Remove the subtree of stem from the thread list
            before = graph.rev_threads[stem];
            graph.threads[before] = after;
            graph.rev_threads[after] = before;

            // Change the parent node and shift stem nodes
            graph.parents[stem] = par_stem;
            par_stem = stem;
            stem = next_stem;

            // Update last and after
            last = graph.last_succs[stem] == graph.last_succs[par_stem] ?
                graph.rev_threads[par_stem] : graph.last_succs[stem];
            after = graph.threads[last];
        }
        graph.parents[graph.i_out] = par_stem;
        graph.threads[last] = thread_continue;
        graph.rev_threads[thread_continue] = last;
        graph.last_succs[graph.i_out] = last;

        // Remove the subtree of i_out from the thread list except for
        // the case when old_rev_thread equals to j_in
        if (old_rev_thread != graph.j_in) {
            graph.threads[old_rev_thread] = after;
            graph.rev_threads[after] = old_rev_thread;
        }

        // Update rev_thread using the new thread values
        for (int32_t i = 0; i != int32_t(graph.dirty_revs.size()); ++i) {
            int32_t u = graph.dirty_revs[i];
            graph.rev_threads[graph.threads[u]] = u;
        }

        // Update pred, dir, last_succ and num_succ for the
        // stem nodes from i_out to i_in
        int32_t tmp_sc = 0, tmp_ls = graph.last_succs[graph.i_out];
        for (int32_t u = graph.i_out, p = graph.parents[u];
                u != graph.i_in;
                u = p, p = graph.parents[u]) {
            graph.preds[u] = graph.preds[p];
            graph.dirs[u] = (Dir)(-graph.dirs[p]);
            tmp_sc += graph.num_succs[u] - graph.num_succs[p];
            graph.num_succs[u] = tmp_sc;
            graph.last_succs[p] = tmp_ls;
        }
        graph.preds[graph.i_in] = graph.a_in;
        graph.dirs[graph.i_in] = graph.i_in == graph.tails[graph.a_in] ?
            DIR_UP : DIR_DOWN;
        graph.num_succs[graph.i_in] = old_succ_num;
    }

    // Update last_succ from j_in towards the root
    int32_t up_limit_out = graph.last_succs[graph.i_join] == graph.j_in ?
        graph.i_join : -1;
    int32_t last_succ_out = graph.last_succs[graph.i_out];
    for (int32_t u = graph.j_in;
            u != -1 && graph.last_succs[u] == graph.j_in;
            u = graph.parents[u]) {
        graph.last_succs[u] = last_succ_out;
    }

    // Update last_succ from j_out towards the root
    if (graph.i_join != old_rev_thread && graph.j_in != old_rev_thread) {
        for (int32_t u = graph.j_out;
                u != up_limit_out && graph.last_succs[u] == old_last_succ;
                u = graph.parents[u]) {
            graph.last_succs[u] = old_rev_thread;
        }
    } else if (last_succ_out != old_last_succ) {
        for (int32_t u = graph.j_out;
                u != up_limit_out && graph.last_succs[u] == old_last_succ;
                u = graph.parents[u]) {
            graph.last_succs[u] = last_succ_out;
        }
    }

    // Update num_succ from j_in to i_join
    for (int32_t u = graph.j_in; u != graph.i_join; u = graph.parents[u]) {
        graph.num_succs[u] += old_succ_num;
    }
    // Update num_succ from j_out to i_join
    for (int32_t u = graph.j_out; u != graph.i_join; u = graph.parents[u]) {
        graph.num_succs[u] -= old_succ_num;
    }

    update_tree_time += std::chrono::steady_clock::now() - beg_time;
}

static void
update_pots(Graph& graph)
{
    auto beg_time = std::chrono::steady_clock::now();

    int32_t sigma = graph.pots[graph.j_in] -
        graph.pots[graph.i_in] -
        graph.dirs[graph.i_in] * graph.costs[graph.a_in];
    int32_t end = graph.threads[graph.last_succs[graph.i_in]];
    for (int32_t i = graph.i_in; i != end; i = graph.threads[i]) {
        graph.pots[i] += sigma;
    }

    update_pots_time += std::chrono::steady_clock::now() - beg_time;
}

static void
usage(const std::string& name)
{
    std::cerr
        << "usage: " << name << " [-k FACTOR] [-f] INPUT\n"
        << "\n"
        << "where\n"
        << "    -k FACTOR\n"
        << "        block size is approximately FACTOR * sqrt(number of arcs)\n"
        << "        and FACTOR is an integer with a default value of 1\n"
        << "    -f\n"
        << "        print arc flow values in the solution output\n"
        << "\n"
        << "and INPUT is a dimacs minimum cost flow file\n"
        << std::endl;
}

int
main(int argc, char* argv[])
{
    int32_t factor = 1;

    bool flows = false;

    std::string inp_name;

    for (int i = 1; i < argc; ++i) {
        std::string opt = argv[i];
        if (opt == "-h") {
            usage(argv[0]);
            return 0;
        }
        if (opt.find("-k") == 0) {
            std::string val;
            opt.erase(0, 2);
            if (opt.empty()) {
                ++i;
                if (i == argc) {
                    std::cerr << "error: expected block size factor" << std::endl;
                    usage(argv[0]);
                    return 1;
                }
                val = argv[i];
            } else {
                val = opt;
            }
            std::istringstream ss(val);
            ss >> factor;
            if (!ss) {
                std::cerr << "error: expected integer as block size factor but found: " << val << std::endl;
                usage(argv[0]);
                return 1;
            }
        } else if (opt.find("-f") == 0) {
            flows = true;
        } else if (opt.front() == '-') {
            std::cerr << "error: unknown option: " << opt << std::endl;
            usage(argv[0]);
            return 1;
        } else {
            if (!inp_name.empty()) {
                std::cerr << "error: unexpected argument: " << argv[i] << std::endl;
                usage(argv[0]);
                return 1;
            }
            inp_name = argv[i];
        }
    }

    if (inp_name.empty()) {
        std::cerr << "error: expected input filename" << std::endl;
        usage(argv[0]);
        return 1;
    }

    std::cout << "c pns v1.0.0" << std::endl;

    auto beg_time = std::chrono::steady_clock::now();

    std::ifstream inp(inp_name);
    auto graph = read_dimacs(inp);
    inp.close();

    // We use a block size of a multiple of 16 if possible to match with our
    // alignment (i.e. 16 x sizeof(int32_t) = 512 bit) and minimize the number
    // of excess elements for AVX2/AVX512 implementations.
    graph.block_size = std::floor(std::sqrt(graph.m));
    graph.block_size = ((graph.block_size + 15) / 16) * 16;
    graph.block_size = factor * graph.block_size;
    graph.block_size = std::max(graph.block_size, 16);
    graph.block_size = std::min(graph.block_size, graph.m);

    auto end_time = std::chrono::steady_clock::now();

    std::cout << "c # nodes             : " << graph.n << std::endl;
    std::cout << "c # arcs              : " << graph.m << std::endl;
    std::cout << "c # arcs in a block   : " << graph.block_size << std::endl;

    std::cout
        << "c Init Time           : "
        << std::chrono::duration_cast<std::chrono::milliseconds>(end_time - beg_time).count()
        << " ms"
        << std::endl;

    beg_time = std::chrono::steady_clock::now();

    long num_iterations = 0;

    while (find_entering_arc(graph)) {
        // std::cout << "c # iterations : " << num_iterations << "\r" << std::flush;
        ++num_iterations;
        find_join_node(graph);
        find_leaving_arc(graph);
        change_flows(graph);
        change_states(graph);
        if (graph.change) {
            update_tree(graph);
            update_pots(graph);
        }
    }

    end_time = std::chrono::steady_clock::now();

    std::cout << "c # iterations        : " << num_iterations << std::endl;

    std::cout
        << "c Time                : "
        << std::chrono::duration_cast<std::chrono::milliseconds>(end_time - beg_time).count()
        << " ms"
        << std::endl;

    auto total_time = end_time - beg_time;

    std::cout
        << "c - find entering arc : "
        << std::chrono::duration_cast<std::chrono::milliseconds>(find_entering_arc_time).count()
        << " ms"
        << " (" << find_entering_arc_time / total_time << ")"
        << std::endl;

    std::cout
        << "c - find join node    : "
        << std::chrono::duration_cast<std::chrono::milliseconds>(find_join_node_time).count()
        << " ms"
        << " (" << find_join_node_time / total_time << ")"
        << std::endl;

    std::cout
        << "c - find leaving arc  : "
        << std::chrono::duration_cast<std::chrono::milliseconds>(find_leaving_arc_time).count()
        << " ms"
        << " (" << find_leaving_arc_time / total_time << ")"
        << std::endl;

    std::cout
        << "c - change flows      : "
        << std::chrono::duration_cast<std::chrono::milliseconds>(change_flows_time).count()
        << " ms"
        << " (" << change_flows_time / total_time << ")"
        << std::endl;

    std::cout
        << "c - change states     : "
        << std::chrono::duration_cast<std::chrono::milliseconds>(change_states_time).count()
        << " ms"
        << " (" << change_states_time / total_time << ")"
        << std::endl;

    std::cout
        << "c - update tree       : "
        << std::chrono::duration_cast<std::chrono::milliseconds>(update_tree_time).count()
        << " ms"
        << " (" << update_tree_time / total_time << ")"
        << std::endl;

    std::cout
        << "c - update pots       : "
        << std::chrono::duration_cast<std::chrono::milliseconds>(update_pots_time).count()
        << " ms"
        << " (" << update_pots_time / total_time << ")"
        << std::endl;

    print_solution(graph, flows);

    return 0;
}


// [[Rcpp::depends(BH)]]
#include <Rcpp.h>
#include <boost/container_hash/hash.hpp>
using namespace Rcpp;

struct Edge {
  std::tuple<long,long,double> edge;

  bool operator==(const Edge& e) const{
    long v1 = std::get<0>(this->edge);
    long v2 = std::get<1>(this->edge);
    long e_v1 = std::get<0>(e.edge);
    long e_v2 = std::get<1>(e.edge);
    return ((v1==e_v1)&&(v2==e_v2))
           ||
           ((v1==e_v2)&&(v2==e_v1));
  }
};

class edgeHashFn{
public:
  std::size_t operator()(const Edge& e) const{
    std::size_t seed = 0;
    if(std::get<0>(e.edge) < std::get<1>(e.edge)){
      boost::hash_combine(seed, std::get<0>(e.edge));
      boost::hash_combine(seed, std::get<1>(e.edge));
    }
    else{
      boost::hash_combine(seed, std::get<1>(e.edge));
      boost::hash_combine(seed, std::get<0>(e.edge));
    }
    return seed;
  }
};

class edgeCompareFn{
public:
  std::size_t operator()(const Edge& e1,const Edge& e2) const{
    return (std::get<0>(e1.edge)==std::get<0>(e2.edge)
              &&
            std::get<1>(e1.edge)==std::get<1>(e2.edge))
           ||
           (std::get<0>(e1.edge)==std::get<1>(e2.edge)
              &&
            std::get<1>(e1.edge)==std::get<0>(e2.edge));
  }
};

//' Remove duplicate links
//'
//' @param links A numeric matrix of network edges
//' @return The matrix of edges with duplicates removed
//' @export
// [[Rcpp::export]]
NumericMatrix dedup_links(NumericMatrix links){
  std::unordered_set<Edge,edgeHashFn,edgeCompareFn> edgeSet;

  for(long i = 0; i<links.nrow();i++){
    if(links(i,0)!=0 && links(i,1)!=0 && links(i,2)!=0.0){
      Edge e = {std::make_tuple(links(i,0),links(i,1),links(i,2))};
      edgeSet.insert(e);
    }
  }

  NumericMatrix deduplicated_edges(edgeSet.size(),3);

  long row_index=0;
  for(Edge e:edgeSet){
    deduplicated_edges(row_index,0)=std::get<0>(e.edge);
    deduplicated_edges(row_index,1)=std::get<1>(e.edge);
    deduplicated_edges(row_index,2)=std::get<2>(e.edge);
    row_index++;
  }
  return deduplicated_edges;
}


#include <Eigen/Core>
#include <boost/python.hpp>
#include <numpy/arrayobject.h>

namespace boopy
{
  namespace bp = boost::python;

  template <typename SCALAR>  struct NumpyEquivalentType {};
  template <> struct NumpyEquivalentType<double>  { enum { type_code = NPY_DOUBLE };};
  template <> struct NumpyEquivalentType<int>     { enum { type_code = NPY_INT    };};
  template <> struct NumpyEquivalentType<float>   { enum { type_code = NPY_FLOAT  };};

  struct EigenMatrix_to_python_matrix
  {
    static PyObject* convert(Eigen::MatrixXd const& mat)
    {
      typedef Eigen::MatrixXd::Scalar T;

      npy_intp shape[2] = { mat.rows(), mat.cols() };
      PyArrayObject* pyArray = (PyArrayObject*)
	PyArray_SimpleNew(2, shape,
			  NumpyEquivalentType<T>::type_code);

      T* pyData = (T*)PyArray_DATA(pyArray);
      for(int i=0;i<mat.rows();++i)
	for(int j=0;j<mat.cols();++j)
	  pyData[i*mat.cols()+j] = mat(i,j);

      return ((PyObject*)pyArray);
    }
  };

  struct EigenMatrix_from_python_array
  {

    EigenMatrix_from_python_array()
    {
      bp::converter::registry
	::push_back(&convertible,
		    &construct,
		    bp::type_id<Eigen::MatrixXd>());
    }

    // Determine if obj_ptr can be converted in a Eigenvec
    static void* convertible(PyObject* obj_ptr)
    {
      typedef Eigen::MatrixXd::Scalar T;

      if (!PyArray_Check(obj_ptr)) {
	return 0;
      }
      if (PyArray_NDIM(obj_ptr) > 2) {
	return 0;
      }
      if (PyArray_ObjectType(obj_ptr, 0) != NumpyEquivalentType<T>::type_code) {
	return 0;
      }
      int flags = PyArray_FLAGS(obj_ptr);
      if (!(flags & NPY_C_CONTIGUOUS)) {
	return 0;
      }
      if (!(flags & NPY_ALIGNED)) {
	return 0;
      }

      return obj_ptr;
    }

    // Convert obj_ptr into a Eigenvec
    static void construct(PyObject* pyObj,
			  bp::converter::rvalue_from_python_stage1_data* memory)
    {
      typedef Eigen::MatrixXd::Scalar T;
      using namespace Eigen;

      PyArrayObject * pyArray = reinterpret_cast<PyArrayObject*>(pyObj);
      int ndims = PyArray_NDIM(pyArray);
      assert(ndims == 2);

      int dtype_size = (PyArray_DESCR(pyArray))->elsize;
      int s1 = PyArray_STRIDE(pyArray, 0);
      assert(s1 % dtype_size == 0);

      int R = MatrixXd::RowsAtCompileTime;
      int C = MatrixXd::ColsAtCompileTime;
      if (R == Eigen::Dynamic) R = PyArray_DIMS(pyArray)[0];
      else	               assert(PyArray_DIMS(pyArray)[0]==R);

      if (C == Eigen::Dynamic) C = PyArray_DIMS(pyArray)[1];
      else	               assert(PyArray_DIMS(pyArray)[1]==C);

      T* pyData = reinterpret_cast<T*>(PyArray_DATA(pyArray));

      void* storage = ((bp::converter::rvalue_from_python_storage<MatrixXd>*)
		       (memory))->storage.bytes;
      MatrixXd & mat = * new (storage) MatrixXd(R,C);
      for(int i=0;i<R;++i)
	for(int j=0;j<C;++j)
	  mat(i,j) = pyData[i*C+j];

      memory->convertible = storage;
    }
  };

}

Eigen::MatrixXd test()
{
  Eigen::MatrixXd mat = Eigen::MatrixXd::Random(5,5);
  std::cout << "EigenMAt = " << mat << std::endl;
  return mat;
}

void test2( Eigen::MatrixXd mat )
{
  std::cout << "test2: dim = " << mat.rows() << " ||| m[0,0] = " << mat(0,0) << std::endl;
}

BOOST_PYTHON_MODULE(libeigenc)
{
  import_array();
  namespace bp = boost::python;
  bp::to_python_converter<Eigen::MatrixXd,
			  boopy::EigenMatrix_to_python_matrix>();
  boopy::EigenMatrix_from_python_array();

  bp::def("test", test);
  bp::def("test2", test2);
}


//=======================================================================
// Copyright 2015 - 2020 Jeff Linahan
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include "graphutil.h"
#include "BFSVisitorData.h"
#include "strutil.h"
#include <boost/graph/make_biconnected_planar.hpp>
#include <boost/graph/make_maximal_planar.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <iostream>
#include <fstream>
using namespace std;
using namespace boost;

int levi_civita(uint i, uint j, uint k)
{
        if( i == j || j == k || k == i ) return 0;
        if( i == 1 && j == 2 && k == 3 ) return 1;
        if( i == 2 && j == 3 && k == 1 ) return 1;
        if( i == 3 && j == 1 && k == 2 ) return 1;
        return -1;
}

void init_vert_propmap(Graph& g)
{
	auto prop_map = get(vertex_index, g); // writing to this property map has side effects in the graph
	VertIter vi, vend;
	uint i = 0;
	for( tie(vi, vend) = vertices(g); vi != vend; ++vi ){
#ifdef GRAPH_TYPE_VEC
                abort();
#else
		prop_map[*vi] = i;
#endif
		cout << "prop map " << *vi << " == " << i << '\n';
		++i;
	}
}

vertex_t get_common_ancestor(vector<vertex_t> const& ancestors_v, vector<vertex_t> const& ancestors_w)
{
        for( uint i = 0; i < ancestors_v.size(); ++i ){
		for( uint j = 0; j < ancestors_w.size(); ++j ){
			if( ancestors_v[i] == ancestors_w[j] ) return ancestors_v[i];
		}
	}
        return Graph::null_vertex();
}

// return set of vertices neighboring v in graph g
set<vertex_t> get_neighbors(vertex_t v, Graph const& g)
{
        set<vertex_t> neighbors;
        OutEdgeIter e_cur, e_end;
        for( tie(e_cur, e_end) = out_edges(v, g); e_cur != e_end; ++e_cur ){
		auto n = target(*e_cur, g);
		neighbors.insert(n);
		//cout << "      vertex " << vmap.vert2uint[v] << " has neighbor " << vmap.vert2uint[n] << '\n';
	}
        return neighbors;
}

// get set intersection of a and b
set<vertex_t> get_intersection(set<vertex_t> const& a, set<vertex_t> const& b)
{
        set<vertex_t> c;
        set_intersection(STLALL(a), STLALL(b), inserter(c, c.begin()));
        //for( auto& i : c ) cout << "      set intersection: " << vmap.vert2uint[i] << '\n';
        return c;
}

/* Given an edge e and a cycle of vertices, determine whether e is in inside, outside, or on the cycle.
An embedding is needed to establish what is inside and outside.
e may be anywhere on the graph.
common_vert_on_cycle should be a tree vertex that both of e's incident vertices share as an ancestor */
InsideOutOn is_edge_inside_outside_or_on_cycle(edge_t e, vertex_t common_vert_on_cycle, vector<vertex_t> const& cycle, Graph const& g, Embedding const& em)
{
        cout << "------------------ is edge inside outside or on cycle -----------------\n";
        cout << "is edge inside outside or on cycle? " << to_string(e, g) << '\n';
	//cout << "edge: " << vmap.vert2uint[source(e, g)] << ' ' << vmap.vert2uint[target(e, g)] << '\n';
        //cout << "cycle: ";
        //for( uint i = 0; i < cycle.size(); ++i ) cout << vmap.vert2uint[cycle[i]] << ' ';
        //cout << '\n';

        auto src = source(e, g);
        auto tar = target(e, g);
        if( on_cycle(e, cycle, g) ){
		cout << "edge is on cycle\n";
		return ON;
	}
        //cout << "      testing if edge " << vmap.vert2uint[src] << ", " << vmap.vert2uint[tar] << " is inside or outside the cycle\n";
        //cout << "      common_vert_on_cycle:    " << vmap.vert2uint[common_vert_on_cycle]   << '\n';
        auto it = find(STLALL(cycle), common_vert_on_cycle);
        if( it == cycle.end() ){ cout << "      common_vert_on_cycle needs to appear in cycle\n"; BOOST_ASSERT(0); }
        BOOST_ASSERT(*it == common_vert_on_cycle);
        auto before_common = it   == cycle.begin() ?  cycle.end  ()-1   : it-1;
        auto after_common        = it+1 == cycle.end  () ?  cycle.begin()     : it+1;
        auto other_end_of_common = source(e, g) == common_vert_on_cycle ? target(e, g) : source(e, g);

        /*cout << '\n';
        cout << "      it:     " << vmap.vert2uint[*it]                  << '\n';
        cout << "      v:      " << vmap.vert2uint[common_vert_on_cycle] << '\n';
        cout << "      before: " << vmap.vert2uint[*before_common]       << '\n';
        cout << "      after:  " << vmap.vert2uint[*after_common]        << '\n';
        cout << "      other:  " << vmap.vert2uint[other_end_of_common]  << '\n';*/

        vector<uint> permu;
        set<vertex_t> seenbefore;
        for( auto& edge : em[*it] ){ // why does this contain duplicates?
                auto src = source(edge, g);
                auto tar = target(edge, g);
                if( src != common_vert_on_cycle ) swap(src, tar);
                BOOST_ASSERT(src == common_vert_on_cycle);

                //cout << "    tar: " << vmap.vert2uint[tar] << '\n';

                if( seenbefore.find(tar) != seenbefore.end() ) continue;
                seenbefore.insert(tar);

                // this can't be an if-else ladder because sometimes other, before, and after equal each other and we wouldn't have 3 permutations
                if( tar == other_end_of_common ) permu.push_back(1);
                if( tar == *before_common      ) permu.push_back(2);
                if( tar == *after_common       ) permu.push_back(3);
        }
        BOOST_ASSERT(permu.size() == 3);

	return levi_civita(permu[0], permu[1], permu[2]) == 1 ?
	       INSIDE					      :
	       OUTSIDE;
}

// scan edges around all vertices of cycle and add up edge costs
CycleCost compute_cycle_cost(vector<vertex_t> const& cycle, Graph const& g, BFSVisitorData const& vis_data, EmbedStruct const& em)
{
        CycleCost cc;
        for( auto& v : cycle ){
                cout << "   scanning cycle vert " << v << '\n';
                for( auto e = out_edges(v, g); e.first != e.second; ++e.first ){
                        if( vis_data.is_tree_edge(*e.first) ){
                                if( !on_cycle(*e.first, cycle, g) ){
                                        uint cost = vis_data.edge_cost(*e.first, cycle, g);
                                        //uint vert_id = vmap.vert2uint[v];
                                        //cout << "      scanning incident tree edge " << vert_id << "   cost: " << cost << '\n';
                                        auto insideout = is_edge_inside_outside_or_on_cycle(*e.first, v, cycle, g, em.em);
                                        BOOST_ASSERT(insideout != ON);
                                        bool is_inside = (insideout == INSIDE);
                                        (is_inside ? cc.inside : cc.outside) += cost;
                                }
                        }
                }
        }
        return cc;
}

// Make all faces of graph G into triangles by adding scanning the boundary and adding (nontree) edges as necessary
EmbedStruct make_max_planar(Graph& g)
{
        EdgeIndex i = reset_edge_index(g);
        EmbedStruct em(&g);
        BOOST_ASSERT(em.test_planar());
        make_biconnected_planar(g, em.em, i);

        reset_edge_index(g);
        BOOST_ASSERT(em.test_planar());

        make_maximal_planar(g, em.em);

        reset_edge_index(g);
        BOOST_ASSERT(em.test_planar());
        return em;
}

void reset_vertex_indices(Graph& g)
{
        VertIter vi, vend;
        uint i = 0;
        for( tie(vi, vend) = vertices(g); vi != vend; ++vi, ++i ){
		put(vertex_index, g, *vi, i);
	}
}
EdgeIndex reset_edge_index(Graph const& g)
{
        EdgeIndex edgedesc_to_uint;
        EdgesSizeType edge_num = 0;
        EdgeIter ei, ei_end;
        for( tie(ei, ei_end) = edges(g); ei != ei_end; ++ei ){
	       	edgedesc_to_uint[*ei] = edge_num++;
	}
        return edgedesc_to_uint;
}

struct CantContractVertexWithSelf
{
};

// replace a and b with a single vertex that has edges to all of their neighbors
vertex_t contract_vertices(vertex_t a, vertex_t b, Graph& g)
{
        if( a == b ) throw CantContractVertexWithSelf();
        auto be = adjacent_vertices(b, g);
        for( auto beit = be.first; beit != be.second; ++beit ) if( *beit != a ) add_edge(a, *beit, g);
        remove_vertex(b, g);
        return a;
}

void kill_vertex(vertex_t v, Graph& g)
{
        //cout << "killing vertex " << i << '\n';
        clear_vertex(v, g);
        remove_vertex(v, g);
}

struct FileNotFound {};

Graph load_graph(string const& fname)
{
        ifstream in(fname);
        if( !in ){
                cerr << "file \"in\" not found!\n";
		throw FileNotFound();
        }

        string str;
        vector<pair<uint, uint>> file_edges;
        int max_v = -1;
        while( getline(in, str) ){
                uint   colon = str.find(",");
                string stra  = str.substr(0, colon); trim(stra);
                string strb  = str.substr(colon+1 ); trim(strb);
                int   a     = lexical_cast<int>(stra);
                int   b     = lexical_cast<int>(strb);
                max_v = max(max(max_v, a), b);
                file_edges.push_back({a, b});
        }
        Graph g(max_v+1);

	map<uint, vertex_t> uint_to_vert;

	VertIter vi, vi_end;
	uint i = 0;
	for( tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi ){
		//put(vertex_index, g, *vi, i);
		uint_to_vert[i] = *vi;
		++i;
	}

        for( pair<uint, uint>& e : file_edges ){
                auto src = uint_to_vert[e.first];
                auto tar = uint_to_vert[e.second];
                add_edge(src, tar, g);
        }

        return g;
}

bool vertex_exists(vertex_t x, Graph const& g)
{
        VertIter vi, vi_end;
        for( tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi) {
                if( *vi == x ) return true;
        }
        return false;
}

bool edge_exists(edge_t e, Graph const& g)
{
        EdgeIter ei, ei_end;
        for( tie(ei, ei_end) = edges(g); ei != ei_end; ++ei ){
                if( *ei == e ) return true;
        }
        return false;
}

bool assert_verts(GraphCR g, BFSVisitorData const& vis_data)
{
        VertIter vei, vend;
        for( tie(vei, vend) = vertices(g); vei != vend; ++vei ){
                vertex_t v = *vei;
                if( !vis_data.verts.contains(v) ){
                        cout << "graphutils.cpp: ignoring bad vertex : " << v << '\n';
                        return false;
                }
        }
        return true;
}


#include <netlistDB/hw_type/aint.h>
#include <boost/multiprecision/cpp_int.hpp>

namespace netlistDB {
namespace hw_type {

boost::multiprecision::cpp_int & as_cpp_int(aint_t::cpp_int_placehoder * v) {
	return *reinterpret_cast<boost::multiprecision::cpp_int*>(v);
}

aint_t::aint_t() {
	_M =
			reinterpret_cast<cpp_int_placehoder *>(new boost::multiprecision::cpp_int);
}
aint_t::aint_t(const aint_t & v) {
	_M =
			reinterpret_cast<cpp_int_placehoder *>(new boost::multiprecision::cpp_int(
					as_cpp_int(v._M)));
}
aint_t::aint_t(uint64_t v) {
	_M =
			reinterpret_cast<cpp_int_placehoder *>(new boost::multiprecision::cpp_int(
					v));
}
aint_t::aint_t(int64_t v) {
	_M =
			reinterpret_cast<cpp_int_placehoder *>(new boost::multiprecision::cpp_int(
					v));
}
aint_t::aint_t(int v) {
	_M =
			reinterpret_cast<cpp_int_placehoder *>(new boost::multiprecision::cpp_int(
					v));
}
aint_t::aint_t(unsigned v) {
	_M =
			reinterpret_cast<cpp_int_placehoder *>(new boost::multiprecision::cpp_int(
					v));
}

#define def_bin_op(ret_t, op)                             \
		ret_t aint_t::operator op(const aint_t & other) const {      \
	aint_t res;                                               \
	as_cpp_int(res._M) = as_cpp_int(_M) op as_cpp_int(other._M);   \
	return res;                                    \
}                                                  \
ret_t aint_t::operator op(uint64_t other) const { \
	aint_t res;                                    \
	as_cpp_int(res._M) = as_cpp_int(_M) op other;  \
	return res;                                    \
}                                                  \
ret_t aint_t::operator op(int64_t other) const {  \
	aint_t res;                                    \
	as_cpp_int(res._M) = as_cpp_int(_M) op other;  \
	return res;                                    \
}                                                  \
ret_t aint_t::operator op(int other) const {      \
	aint_t res;                                    \
	as_cpp_int(res._M) = as_cpp_int(_M) op other;  \
	return res;                                    \
}                                                  \
ret_t aint_t::operator op(unsigned other) const { \
	aint_t res;                                    \
	as_cpp_int(res._M) = as_cpp_int(_M) op other;  \
	return res;                                    \
}

def_bin_op(aint_t, -)
def_bin_op(aint_t, +)
def_bin_op(aint_t, *)
def_bin_op(aint_t, /)
def_bin_op(aint_t, %)
def_bin_op(aint_t, ^)
def_bin_op(aint_t, &)
def_bin_op(aint_t, |)
def_bin_op(bool, <)
def_bin_op(bool, >)
def_bin_op(bool, <=)
def_bin_op(bool, >=)
aint_t aint_t::operator <<(size_t other) const {
	aint_t res;
	as_cpp_int(res._M) = as_cpp_int(_M) << other;
	return res;
}
aint_t aint_t::operator >>(size_t other) const {
	aint_t res;
	as_cpp_int(res._M) = as_cpp_int(_M) >> other;
	return res;
}
def_bin_op(bool, ==)
def_bin_op(bool, !=)
def_bin_op(bool, &&)
def_bin_op(bool, ||)

#define def_inplace_op(op)                             \
void aint_t::operator op(const aint_t & other) { \
	as_cpp_int(_M) op as_cpp_int(other._M);            \
}                                                      \
void aint_t::operator op(uint64_t other) {       \
	as_cpp_int(_M) op other;                           \
}                                                      \
void aint_t::operator op(int64_t other) {        \
	as_cpp_int(_M) op other;                           \
}                                                      \
void aint_t::operator op(int other) {            \
	as_cpp_int(_M) op other;                           \
}                                                      \
void aint_t::operator op(unsigned other) {       \
	as_cpp_int(_M) op other;                           \
}

def_inplace_op(+=)
def_inplace_op(-=)
def_inplace_op(/=)
def_inplace_op(%=)
def_inplace_op(^=)
def_inplace_op(&=)
def_inplace_op(|=)
def_inplace_op(*=)

void aint_t::operator <<=(uint64_t other) {
	as_cpp_int(_M) <<= other;
}
void aint_t::operator >>=(uint64_t other) {
	as_cpp_int(_M) >>= other;
}

aint_t aint_t::operator~() {
	aint_t res;
	as_cpp_int(res._M) = ~as_cpp_int(_M);
	return res;
}
aint_t aint_t::operator-() {
	aint_t res;
	as_cpp_int(res._M) = -as_cpp_int(_M);
	return res;
}
aint_t aint_t::operator!() {
	aint_t res;
	as_cpp_int(res._M) = !as_cpp_int(_M);
	return res;
}

aint_t aint_t::operator++() {
	as_cpp_int(_M)++;
	return aint_t(*this);
}

aint_t aint_t::operator--() {
	as_cpp_int(_M)--;
	return aint_t(*this);
}

aint_t::operator int64_t() const {
	return int64_t(as_cpp_int(_M));
}
aint_t::operator uint64_t() const {
	return uint64_t(as_cpp_int(_M));
}
aint_t::operator unsigned() const {
	return unsigned(as_cpp_int(_M));
}
aint_t::operator int() const {
	return int(as_cpp_int(_M));
}
aint_t::operator bool() const {
	return bool(as_cpp_int(_M));
}

std::ostream & operator<<(std::ostream & stream, const aint_t & t) {
	return stream << as_cpp_int(t._M);
}

aint_t::~aint_t() {
	delete &as_cpp_int(_M);
}

}
}


#ifndef RENDERER_HPP
#define RENDERER_HPP

#include "Screen.hpp"
#include <vector>
#include <Eigen/Dense>

class wxPaintDC;

class renderer {
public:
    // constructors:
    renderer(int width, int height);
    renderer();

    // setter and getters
    void set_screen_width_and_height(int w, int h);
    void set_projection_matrix(const Eigen::Matrix4d& mat);
    void get_projection_matrix(Eigen::Matrix4d& mat) const;
    void get_pure_projection_matrix(Eigen::Matrix4d& mat) const;

    // rendering functions:
    void render(const double* data, const unsigned int num_verts, wxPaintDC& dc);
    void render(const std::vector<Eigen::Vector3d>& data, wxPaintDC& dc);
    void render_test(const std::vector<Eigen::Vector3d>& data,
                     int& glx, int& gly,
                     int& scx, int& scy,
                     double& depth, double& xndc, double& yndc,
                     double& zndc, double& xclip, double& yclip,
                     double& zclip, double& wclip);

    // similar to OpenGL functions
    void glFrustrum(double left, double right, double bottom, double top, double near, double far);
    void gluPerspective(double fovy, double near, double far);
    void gluPerspective_via_glFrustrum(double fovy, double near, double far);
    void glOrtho(double left, double right, double bottom, double top, double near, double far);
    void glViewport(unsigned int x, unsigned int y, unsigned int width, unsigned int height);
    void glDepthRange(double nearVal, double farVal);

    // test and print fucntions
    void print_projection_matrix();
    void print_inverse_projection_matrix();
    void get_inverse_projection_matrix(double fovy, double near, double far, Eigen::Matrix4d& mat);
    void get_inverse_projection_matrix(double left, double right, double bottom, double top, double near, double far, Eigen::Matrix4d& mat);
    void print_converted_parameters(double fovy, double near, double far);
    void print_converted_parameters(double left, double right,
                                  double bottom, double top,
                                  double near, double far);
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
    screen m_screen;
    Eigen::Matrix4d m_projection;
    Eigen::Matrix4d m_pure_projection;
    void project(const double* data, const unsigned int num_verts, wxPaintDC& dc);
    void project(const std::vector<Eigen::Vector3d>& data, wxPaintDC& dc);
    inline void project(const Eigen::MatrixXd& vertices, wxPaintDC& dc);
    void calculate_pure_projection_matrix(double near, double far);
};

#endif // RENDERER_HPP


#ifndef MATHTOOLBOX_ACQUISITION_FUNCTIONS_HPP
#define MATHTOOLBOX_ACQUISITION_FUNCTIONS_HPP

#include <Eigen/Core>
#include <functional>

namespace mathtoolbox
{
    double GetExpectedImprovement(const Eigen::VectorXd&                               x,
                                  const std::function<double(const Eigen::VectorXd&)>& mu,
                                  const std::function<double(const Eigen::VectorXd&)>& sigma,
                                  const Eigen::VectorXd&                               x_best);

    Eigen::VectorXd
    GetExpectedImprovementDerivative(const Eigen::VectorXd&                                        x,
                                     const std::function<double(const Eigen::VectorXd&)>&          mu,
                                     const std::function<double(const Eigen::VectorXd&)>&          sigma,
                                     const Eigen::VectorXd&                                        x_best,
                                     const std::function<Eigen::VectorXd(const Eigen::VectorXd&)>& mu_derivative,
                                     const std::function<Eigen::VectorXd(const Eigen::VectorXd&)>& sigma_derivative);
} // namespace mathtoolbox

#endif // MATHTOOLBOX_ACQUISITION_FUNCTIONS_HPP


#include <boost/algorithm/string.hpp>

#include <set>
#include <string>
#include <vector>
#include <iostream>
#include <fstream>
#include <sstream>

const std::string kProcDirectory{"/proc/"};
const std::string kStatFilename{"/stat"};

std::string Pad(std::string s, char c){
  s.insert(s.begin(), 2 - s.size(), c);
  return s;
}

std::string ElapsedTime(long seconds) {
  int h = seconds / 3600;
  int m = (seconds / 60) % 60;
  int s = seconds % 60;
  return std::string(Pad(std::to_string(h), '0') + ":" +
                     Pad(std::to_string(m), '0') + ":" +
                     Pad(std::to_string(s), '0'));
}

std::vector<std::string> ReadProcessStat(int pid) {
  std::vector<std::string> stat{};
  std::ifstream filestream(kProcDirectory + std::to_string(pid) + kStatFilename);
  std::string line, token;
  if (filestream.is_open()) {
    std::getline(filestream, line);
    std::istringstream ss(line);
    while (ss >> token) {
      stat.push_back(token);
    }
  }
  return stat;
}

long ActiveJiffies(std::vector<std::string> stat_) {
  long active{0};
  active += stol(stat_[13]);
  active += stol(stat_[14]);
  active += stol(stat_[15]);
  active += stol(stat_[16l]);
  return active;
}

int main (){
    std::vector<std::string> stat = ReadProcessStat(9);
    long int elapsed = stol(stat[23])/sysconf(_SC_CLK_TCK);
    std::string formatted = ElapsedTime(elapsed);
    std::cout << formatted <<"\n";

    // for (int i=0; i<stat.size(); i++){
    //   std::cout << i << " : " << stat[i] << "\n";
    // }
    std::cout << ActiveJiffies(stat) << "\n";

}