/**
* 3D NDT-MCL Node.
* This application runs the ndt-mcl localization based on a map and laser scanner and odometry.
*
* The initialization is based now on ground truth pose information (should be replaced with manual input).
*
* The visualization depends on mrpt-gui
*
* More details about the algorithm:
* Jari Saarinen, Henrik Andreasson, Todor Stoyanov and Achim J. Lilienthal, Normal Distributions Transform Monte-Carlo Localization (NDT-MCL)
* IEEE/RSJ International Conference on Intelligent Robots and Systems November 3-8, 2013, Tokyo Big Sight, Japan
*
* @author Jari Saarinen (jari.p.saarinen@gmail.com), mods by Todor Stoyanov
* @NOTE: experimental 3D version in progress
* @TODO Global initialization possibility
* Known issues: in launch file (or hard coded parameters) you have to set the same resolution for NDT map as is saved -- otherwise it wont work
*/

#include <ndt_visualisation/ndt_viz.h>

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <boost/foreach.hpp>
#include <sensor_msgs/LaserScan.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/OccupancyGrid.h>
#include <visualization_msgs/MarkerArray.h>
#include <tf/transform_broadcaster.h>
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "geometry_msgs/Pose.h"
#include <pcl_conversions/pcl_conversions.h>

#include "message_filters/subscriber.h"
#include "tf/message_filter.h"
#include <tf/transform_broadcaster.h>
#include <tf_conversions/tf_eigen.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>

#include "sensor_msgs/PointCloud2.h"

//#include "ndt_mcl/impl/ndt_mcl.hpp"
#include "ndt_mcl/3d_ndt_mcl.h"
#include <ndt_map/ndt_map.h>

#define SYNC_FRAMES 20

typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, nav_msgs::Odometry> PointsOdomSync;
//TODO typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, geometry_msgs::PoseStamped> PointsPoseSync;

class NDTMCL3DNode {

    private:
	ros::NodeHandle nh_;
	NDTMCL3D *ndtmcl;
        boost::mutex mcl_m,message_m;

        message_filters::Subscriber<sensor_msgs::PointCloud2> *points2_sub_;
        message_filters::Subscriber<nav_msgs::Odometry> *odom_sub_;

	///Laser sensor offset
	Eigen::Affine3d sensorPoseT; //<<Sensor offset with respect to odometry frame
	Eigen::Affine3d Told,Todo,Todo_old,Tcum; //<<old and current odometry transformations
	Eigen::Affine3d initPoseT; //<<Sensor offset with respect to odometry frame

	bool hasSensorPose, hasInitialPose;
	bool isFirstLoad;
	bool forceSIR, do_visualize;
	bool saveMap;						///< indicates if we want to save the map in a regular intervals
	std::string mapName; ///<name and the path to the map
	std::string output_map_name;
	double resolution;
	double subsample_level;
	int pcounter;
	NDTViz ndt_viz;

	ros::Publisher mcl_pub; ///< The output of MCL is published with this!
        message_filters::Synchronizer< PointsOdomSync > *sync_po_;

	std::string tf_base_link, tf_sensor_link, points_topic, odometry_topic;
    public:
	NDTMCL3DNode(ros::NodeHandle param_nh) {

	    //////////////////////////////////////////////////////////
	    /// Prepare Pose offsets
	    //////////////////////////////////////////////////////////
	    bool use_sensor_pose, use_initial_pose;
	    double pose_init_x,pose_init_y,pose_init_z,
		   pose_init_r,pose_init_p,pose_init_t;
	    double sensor_pose_x,sensor_pose_y,sensor_pose_z,
		   sensor_pose_r,sensor_pose_p,sensor_pose_t;

	    param_nh.param<bool>("set_sensor_pose", use_sensor_pose, true);
	    param_nh.param<bool>("set_initial_pose", use_initial_pose, false);

	    if(use_initial_pose) {
		///initial pose of the vehicle with respect to the map
		param_nh.param("pose_init_x",pose_init_x,0.);
		param_nh.param("pose_init_y",pose_init_y,0.);
		param_nh.param("pose_init_z",pose_init_z,0.);
		param_nh.param("pose_init_r",pose_init_r,0.);
		param_nh.param("pose_init_p",pose_init_p,0.);
		param_nh.param("pose_init_t",pose_init_t,0.);
		initPoseT =  Eigen::Translation<double,3>(pose_init_x,pose_init_y,pose_init_z)*
		    Eigen::AngleAxis<double>(pose_init_r,Eigen::Vector3d::UnitX()) *
		    Eigen::AngleAxis<double>(pose_init_p,Eigen::Vector3d::UnitY()) *
		    Eigen::AngleAxis<double>(pose_init_t,Eigen::Vector3d::UnitZ()) ;

		hasInitialPose=true;
	    } else {
		hasInitialPose=false;
	    }

	    if(use_sensor_pose) {
		///pose of the sensor with respect to the vehicle odometry frame
		param_nh.param("sensor_pose_x",sensor_pose_x,0.);
		param_nh.param("sensor_pose_y",sensor_pose_y,0.);
		param_nh.param("sensor_pose_z",sensor_pose_z,0.);
		param_nh.param("sensor_pose_r",sensor_pose_r,0.);
		param_nh.param("sensor_pose_p",sensor_pose_p,0.);
		param_nh.param("sensor_pose_t",sensor_pose_t,0.);
		hasSensorPose = true;
		sensorPoseT =  Eigen::Translation<double,3>(sensor_pose_x,sensor_pose_y,sensor_pose_z)*
		    Eigen::AngleAxis<double>(sensor_pose_r,Eigen::Vector3d::UnitX()) *
		    Eigen::AngleAxis<double>(sensor_pose_p,Eigen::Vector3d::UnitY()) *
		    Eigen::AngleAxis<double>(sensor_pose_t,Eigen::Vector3d::UnitZ()) ;
	    } else {
		hasSensorPose = false;
	    }

	    //////////////////////////////////////////////////////////
	    /// Prepare the map
	    //////////////////////////////////////////////////////////
	    param_nh.param<std::string>("map_file_name", mapName, std::string("basement.ndmap"));
	    param_nh.param<bool>("save_output_map", saveMap, true);
	    param_nh.param<bool>("do_visualize", do_visualize, true);
	    param_nh.param<std::string>("output_map_file_name", output_map_name, std::string("ndt_mapper_output.ndmap"));
	    param_nh.param<double>("map_resolution", resolution , 0.2);
	    param_nh.param<double>("subsample_level", subsample_level , 1);

	    fprintf(stderr,"USING RESOLUTION %lf\n",resolution);

	    perception_oru::NDTMap ndmap(new perception_oru::LazyGrid(resolution));
	    ndmap.loadFromJFF(mapName.c_str());

	    //////////////////////////////////////////////////////////
	    /// Prepare MCL object
	    //////////////////////////////////////////////////////////

	    ndtmcl = new NDTMCL3D(resolution,ndmap,-5);
	    param_nh.param<bool>("forceSIR", forceSIR, false);
	    if(forceSIR) ndtmcl->forceSIR=true;

	    fprintf(stderr,"*** FORCE SIR = %d****",forceSIR);
	    mcl_pub = nh_.advertise<nav_msgs::Odometry>("ndt_mcl",10);

	    //////////////////////////////////////////////////////////
	    /// Prepare the callbacks and message filters
	    //////////////////////////////////////////////////////////
	    //the name of the TF link associated to the base frame / odometry frame
	    param_nh.param<std::string>("tf_base_link", tf_base_link, std::string("/base_link"));
	    //the name of the tf link associated to the 3d laser scanner
	    param_nh.param<std::string>("tf_laser_link", tf_sensor_link, std::string("/velodyne_link"));
	    ///topic to wait for point clouds
	    param_nh.param<std::string>("points_topic",points_topic,"points");
	    ///topic to wait for odometry messages
	    param_nh.param<std::string>("odometry_topic",odometry_topic,"odometry");

	    points2_sub_ = new message_filters::Subscriber<sensor_msgs::PointCloud2>(nh_,points_topic,1);
	    odom_sub_ = new message_filters::Subscriber<nav_msgs::Odometry>(nh_,odometry_topic,10);
	    sync_po_ = new message_filters::Synchronizer< PointsOdomSync >(PointsOdomSync(SYNC_FRAMES), *points2_sub_, *odom_sub_);
	    sync_po_->registerCallback(boost::bind(&NDTMCL3DNode::callback, this, _1, _2));

	    isFirstLoad=true;
	    pcounter =0;
	}
	~NDTMCL3DNode() {
	    delete points2_sub_;
	    delete odom_sub_;
	    delete sync_po_;
	    delete ndtmcl;
	}
	/////////////////////////////////////////////////////////////////////////////////////////////////////////
	//new callback: looks up transforms on TF
	void callback(const sensor_msgs::PointCloud2::ConstPtr& cloud_in,
		const nav_msgs::Odometry::ConstPtr& odo_in)
	{
	    mcl_m.lock();
	    //compute new odo and point cloud
	    Eigen::Quaterniond qd;
	    Eigen::Affine3d Tm;
	    pcl::PointCloud<pcl::PointXYZ> cloud, cloudT, cloudTO;

	    qd.x() = odo_in->pose.pose.orientation.x;
	    qd.y() = odo_in->pose.pose.orientation.y;
	    qd.z() = odo_in->pose.pose.orientation.z;
	    qd.w() = odo_in->pose.pose.orientation.w;

	    Todo = Eigen::Translation3d (odo_in->pose.pose.position.x,
		    odo_in->pose.pose.position.y,odo_in->pose.pose.position.z) * qd;

	    pcl::fromROSMsg (*cloud_in, cloud);

	    fprintf(stderr,"cloud has %zu points\n",cloud.points.size());
	    //check if sensor pose is known, if not, look it up on TF
	    if(!hasSensorPose) {

	    }
	    //check if we have done iterations
	    if(isFirstLoad) {
		//if not, check if initial robot pose has been set
		if(!hasInitialPose) {
		    //can't do anything, wait for pose message...
		    mcl_m.unlock();
		    return;
		}
		//initialize filter
		Eigen::Vector3d tr = initPoseT.translation();
		Eigen::Vector3d rot = initPoseT.rotation().eulerAngles(0,1,2);

		Todo_old=Todo;
		Tcum = initPoseT;

    ndtmcl->initializeFilter(tr[0], tr[1],tr[2],rot[0],rot[1],rot[2],0.5, 0.5, 0.1, 2.0*M_PI/180.0, 2.0*M_PI/180.0 ,2.0*M_PI/180.0, 100);
		//ndt_viz.plotNDTMap(&ndtmcl->map,0,1.0,1.0,true, false);
                ndt_viz.plotNDTSAccordingToOccupancy(-1,&ndtmcl->map);
		isFirstLoad = false;
		mcl_m.unlock();
		return;
	    }

	    Tm = Todo_old.inverse()*Todo;
	    if(Tm.translation().norm()<0.01 && fabs(Tm.rotation().eulerAngles(0,1,2)[2])<(0.5*M_PI/180.0)) {
		mcl_m.unlock();
		return;
	    }

	    Tcum = Tcum*Tm;
	    Todo_old=Todo;

	    //cut off user defined slice form the sensor data
	    //transform point cloud from sensor pose into base pose

	    ///Set the cloud to sensor frame with respect to base
	    perception_oru::transformPointCloudInPlace(sensorPoseT, cloud);
	    //cloudT = lslgeneric::transformPointCloud(Tcum, cloud);

	    //update filter -> + add parameter to subsample ndt map in filter step
	    ndtmcl->updateAndPredictEff(Tm, cloud, subsample_level);

	    //update visualization
	    ///DRAW STUFF///
	    if(do_visualize) {
		pcounter++;
		if(pcounter%500==0){
		    ndt_viz.clear();
        ndt_viz.plotNDTSAccordingToOccupancy(-1,&ndtmcl->map);

		}

		//if(pcounter%10==0){

		ndt_viz.clearParticles();
		for(int i=0;i<ndtmcl->pf.size();i++){
		    double x,y,z;
		    ndtmcl->pf.pcloud[i].getXYZ(x,y,z);
		    ndt_viz.addParticle(x, y,z+0.5, 1.0, 1.0, 1.0);
		}

		Eigen::Affine3d mean = ndtmcl->pf.getMean();
		ndt_viz.addTrajectoryPoint(mean.translation()[0],mean.translation()[1],mean.translation()[2]+0.5,1.0,0,0);
		Eigen::Vector3d tr = Tcum.translation();
		ndt_viz.addTrajectoryPoint(tr[0],tr[1],tr[2]+0.5,0.0,1.0,0);

		//ndt_viz.addPointCloud(cloudT,1,0,0);
		ndt_viz.displayParticles();
		ndt_viz.displayTrajectory();
		ndt_viz.win3D->setCameraPointingToPoint(mean.translation()[0],mean.translation()[1],3.0);
		ndt_viz.repaint();
		//}
	    }
	    //publish pose
	    sendROSOdoMessage(ndtmcl->pf.getMean(),odo_in->header.stamp);
	    mcl_m.unlock();

	}
	/////////////////////////////////////////////////////////////////////////////////////////////////////////
	//FIXME: this should be in 3D
	/////////////////////////////////////////////////////////////////////////////////////////////////////////
	bool sendROSOdoMessage(Eigen::Affine3d mean,ros::Time ts){
	    nav_msgs::Odometry O;
	    static int seq = 0;
	    O.header.stamp = ts;
	    O.header.seq = seq;
	    O.header.frame_id = "/world";
	    O.child_frame_id = "/mcl_pose";

	    O.pose.pose.position.x = mean.translation()[0];
	    O.pose.pose.position.y = mean.translation()[1];
	    O.pose.pose.position.z = mean.translation()[2];
	    Eigen::Quaterniond q (mean.rotation());
	    tf::Quaternion qtf;
	    tf::quaternionEigenToTF (q, qtf);
	    O.pose.pose.orientation.x = q.x();
	    O.pose.pose.orientation.y = q.y();
	    O.pose.pose.orientation.z = q.z();
	    O.pose.pose.orientation.w = q.w();

	    seq++;
	    mcl_pub.publish(O);

	    static tf::TransformBroadcaster br;
	    tf::Transform transform;
	    transform.setOrigin( tf::Vector3(mean.translation()[0],mean.translation()[1], mean.translation()[2]) );

	    transform.setRotation( qtf );
	    br.sendTransform(tf::StampedTransform(transform, ts, "world", "mcl_pose"));

	    return true;
	}
/////////////////////////////////////////////////////////////////////////////////////////////////////////
};

int main(int argc, char **argv){
	ros::init(argc, argv, "NDT-MCL");
	ros::NodeHandle paramHandle ("~");
	NDTMCL3DNode mclnode(paramHandle);
	ros::spin();
	return 0;
}


#include <cctbx/boost_python/flex_fwd.h>

#include <cctbx/translation_search/fast_nv1995.h>
#include <boost/python/class.hpp>
#include <boost/python/args.hpp>

namespace cctbx { namespace translation_search { namespace boost_python {

namespace {

  struct fast_nv1995_wrappers
  {
    typedef fast_nv1995<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("fast_nv1995", no_init)
        .def(init<af::int3 const&,
                  sgtbx::space_group const&,
                  bool,
                  af::const_ref<miller::index<> > const&,
                  af::const_ref<double> const&,
                  af::const_ref<std::complex<double> > const&,
                  af::const_ref<miller::index<> > const&,
                  af::const_ref<std::complex<double> > >(
          (arg("gridding"),
           arg("space_group"),
           arg("anomalous_flag"),
           arg("miller_indices_f_obs"),
           arg("f_obs"),
           arg("f_part"),
           arg("miller_indices_p1_f_calc"),
           arg("p1_f_calc"))))
        .def("target_map", &w_t::target_map)
      ;
    }
  };

} // namespace <anoymous>

  void wrap_fast_nv1995()
  {
    fast_nv1995_wrappers::wrap();
  }

}}} // namespace cctbx::translation_search::boost_python


// Copyright 2018 Ulf Adams
//
// The contents of this file may be used under the terms of the Apache License,
// Version 2.0.
//
//    (See accompanying file LICENSE-Apache or copy at
//     http://www.apache.org/licenses/LICENSE-2.0)
//
// Alternatively, the contents of this file may be used under the terms of
// the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE-Boost or copy at
//     https://www.boost.org/LICENSE_1_0.txt)
//
// Unless required by applicable law or agreed to in writing, this software
// is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
// KIND, either express or implied.

/*
    This is a derivative work
*/

#ifndef BOOST_JSON_DETAIL_RYU_DETAIL_COMMON_HPP
#define BOOST_JSON_DETAIL_RYU_DETAIL_COMMON_HPP

#include <boost/json/detail/config.hpp>
#include <string.h>

BOOST_JSON_NS_BEGIN
namespace detail {

namespace ryu {
namespace detail {

constexpr int DOUBLE_MANTISSA_BITS = 52;
constexpr int DOUBLE_EXPONENT_BITS = 11;
constexpr int DOUBLE_BIAS = 1023;

#if defined(_M_IX86) || defined(_M_ARM)
#define BOOST_JSON_RYU_32_BIT_PLATFORM
#endif

inline uint32_t decimalLength9(const uint32_t v) {
  // Function precondition: v is not a 10-digit number.
  // (f2s: 9 digits are sufficient for round-tripping.)
  // (d2fixed: We print 9-digit blocks.)
  BOOST_ASSERT(v < 1000000000);
  if (v >= 100000000) { return 9; }
  if (v >= 10000000) { return 8; }
  if (v >= 1000000) { return 7; }
  if (v >= 100000) { return 6; }
  if (v >= 10000) { return 5; }
  if (v >= 1000) { return 4; }
  if (v >= 100) { return 3; }
  if (v >= 10) { return 2; }
  return 1;
}

// Returns e == 0 ? 1 : ceil(log_2(5^e)).
inline int32_t pow5bits(const int32_t e) {
  // This approximation works up to the point that the multiplication overflows at e = 3529.
  // If the multiplication were done in 64 bits, it would fail at 5^4004 which is just greater
  // than 2^9297.
  BOOST_ASSERT(e >= 0);
  BOOST_ASSERT(e <= 3528);
  return (int32_t) (((((uint32_t) e) * 1217359) >> 19) + 1);
}

// Returns floor(log_10(2^e)).
inline uint32_t log10Pow2(const int32_t e) {
  // The first value this approximation fails for is 2^1651 which is just greater than 10^297.
  BOOST_ASSERT(e >= 0);
  BOOST_ASSERT(e <= 1650);
  return (((uint32_t) e) * 78913) >> 18;
}

// Returns floor(log_10(5^e)).
inline uint32_t log10Pow5(const int32_t e) {
  // The first value this approximation fails for is 5^2621 which is just greater than 10^1832.
  BOOST_ASSERT(e >= 0);
  BOOST_ASSERT(e <= 2620);
  return (((uint32_t) e) * 732923) >> 20;
}

inline int copy_special_str(char * const result, const bool sign, const bool exponent, const bool mantissa) {
  if (mantissa) {
    memcpy(result, "NaN", 3);
    return 3;
  }
  if (sign) {
    result[0] = '-';
  }
  if (exponent) {
    memcpy(result + sign, "Infinity", 8);
    return sign + 8;
  }
  memcpy(result + sign, "0E0", 3);
  return sign + 3;
}

inline uint32_t float_to_bits(const float f) {
  uint32_t bits = 0;
  memcpy(&bits, &f, sizeof(float));
  return bits;
}

inline uint64_t double_to_bits(const double d) {
  uint64_t bits = 0;
  memcpy(&bits, &d, sizeof(double));
  return bits;
}

} // detail
} // ryu

} // detail
BOOST_JSON_NS_END

#endif


/*
  Copyright (C) 2020 Skandinaviska Enskilda Banken AB (publ)
  All rights reserved.
  This file is part of ORE, a free-software/open-source library
  for transparent pricing and risk analysis - http://opensourcerisk.org
  ORE is free software: you can redistribute it and/or modify it
  under the terms of the Modified BSD License.  You should have received a
  copy of the license along with this program.
  The license is also available online at <http://opensourcerisk.org>
  This program is distributed on the basis that it will form a useful
  contribution to risk analytics and model standardisation, but WITHOUT
  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
 */

 #include <ored/portfolio/commodityasianoption.hpp>

 #include <boost/make_shared.hpp>

 #include <qle/indexes/commodityindex.hpp>
 #include <qle/termstructures/pricetermstructure.hpp>
 #include <ql/errors.hpp>

 #include <ored/portfolio/enginefactory.hpp>
 #include <ored/utilities/log.hpp>
 #include <ored/utilities/to_string.hpp>

 namespace ore {
 namespace data {

 void CommodityAsianOption::build(const boost::shared_ptr<EngineFactory>& engineFactory) {
     // Checks
     QL_REQUIRE(quantity_ > 0, "Commodity Asian option requires a positive quatity");
     QL_REQUIRE(strike_ >= 0, "Commodity Asian option requires a strike >= 0");

     // Get the price curve for the commodity.
     const boost::shared_ptr<Market>& market = engineFactory->market();
     Handle<QuantExt::PriceTermStructure> priceCurve =
         market->commodityPriceCurve(assetName_, engineFactory->configuration(MarketContext::pricing));

     // Populate the index_ in case the option is automatic exercise.
     // Intentionally use null calendar because we will ask for index value on the expiry date without adjustment.
     if (!isFuturePrice_ || *isFuturePrice_) {
         // Assume future price if isFuturePrice_ is not explicitly set or if it is and true.

         // If we are given an explicit future contract expiry date, use it, otherwise use option's expiry.
         Date expiryDate;
         if (futureExpiryDate_ != Date()) {
             expiryDate = futureExpiryDate_;
         } else {
             // Get the expiry date of the option. This is the expiry date of the commodity future index.
             const vector<string>& expiryDates = option_.exerciseDates();
             QL_REQUIRE(expiryDates.size() == 1, "Expected exactly one expiry date for CommodityAsianOption but got "
                                                     << expiryDates.size() << ".");
             expiryDate = parseDate(expiryDates[0]);
         }

         index_ = boost::make_shared<QuantExt::CommodityFuturesIndex>(assetName_, expiryDate, NullCalendar(), priceCurve);
     } else {
         // If the underlying is a commodity spot, create a spot index.
         index_ = boost::make_shared<QuantExt::CommoditySpotIndex>(assetName_, NullCalendar(), priceCurve);
     }

     AsianOptionTrade::build(engineFactory);
 }

 std::map<AssetClass, std::set<std::string>> CommodityAsianOption::underlyingIndices(const boost::shared_ptr<ReferenceDataManager>& referenceDataManager) const {
     return {{AssetClass::COM, std::set<std::string>({assetName_})}};
 }

 void CommodityAsianOption::fromXML(XMLNode* node) {
     Trade::fromXML(node);

     XMLNode* commodityNode = XMLUtils::getChildNode(node, "CommodityAsianOptionData");
     QL_REQUIRE(commodityNode, "A commodity Asian option needs a 'CommodityAsianOptionData' node");

     option_.fromXML(XMLUtils::getChildNode(commodityNode, "OptionData"));
     QL_REQUIRE(option_.payoffType() == "Asian", "Expected PayoffType Asian for CommodityAsianOption.");
     XMLNode* asianNode = XMLUtils::getChildNode(commodityNode, "AsianData");
     if (asianNode)
         asianData_.fromXML(asianNode);

     XMLNode* scheduleDataNode = XMLUtils::getChildNode(commodityNode, "ObservationDates");
     observationDates_.fromXML(scheduleDataNode);

     XMLNode* tmp = XMLUtils::getChildNode(commodityNode, "Underlying");
     if (!tmp)
         tmp = XMLUtils::getChildNode(commodityNode, "Name");
     commodityUnderlying_.fromXML(tmp);
     assetName_ = commodityUnderlying_.name();

     currency_ = XMLUtils::getChildValue(commodityNode, "Currency", true);
     // Require explicit Strike
     strike_ = XMLUtils::getChildValueAsDouble(commodityNode, "Strike", true);
     quantity_ = XMLUtils::getChildValueAsDouble(commodityNode, "Quantity", true);

     isFuturePrice_ = boost::none;
     if (XMLNode* n = XMLUtils::getChildNode(commodityNode, "IsFuturePrice"))
         isFuturePrice_ = parseBool(XMLUtils::getNodeValue(n));

     futureExpiryDate_ = Date();
     if (XMLNode* n = XMLUtils::getChildNode(commodityNode, "FutureExpiryDate"))
         futureExpiryDate_ = parseDate(XMLUtils::getNodeValue(n));
 }

 XMLNode* CommodityAsianOption::toXML(XMLDocument& doc) {
     XMLNode* node = Trade::toXML(doc);

     XMLNode* comNode = doc.allocNode("CommodityAsianOptionData");
     XMLUtils::appendNode(node, comNode);

     XMLUtils::appendNode(comNode, option_.toXML(doc));
     XMLUtils::appendNode(comNode, asianData_.toXML(doc));
     auto tmp = observationDates_.toXML(doc);
     XMLUtils::setNodeName(doc, tmp, "ObservationDates");
     XMLUtils::appendNode(comNode, tmp);

     XMLUtils::appendNode(comNode, commodityUnderlying_.toXML(doc));
     XMLUtils::addChild(doc, comNode, "Currency", currency_);
     XMLUtils::addChild(doc, comNode, "Strike", strike_);
     XMLUtils::addChild(doc, comNode, "Quantity", quantity_);

     if (isFuturePrice_)
         XMLUtils::addChild(doc, comNode, "IsFuturePrice", *isFuturePrice_);

     if (futureExpiryDate_ != Date())
         XMLUtils::addChild(doc, comNode, "FutureExpiryDate", to_string(futureExpiryDate_));

     return node;
 }

 } // namespace data
 } // namespace ore


#include <array>
#include <cassert>
#include <iostream>
#include <iterator>
#include <numeric>
#include <type_traits>

#include <boost/mpl/vector.hpp>

#define png_infopp_NULL nullptr
#define int_p_NULL      nullptr
#include <boost/gil/gil_all.hpp>
#include <boost/gil/extension/io/png_dynamic_io.hpp>
#include <boost/gil/extension/dynamic_image/dynamic_image_all.hpp>

namespace gil = boost::gil;
namespace mpl = boost::mpl;

// return the average color from all pixels in the view
template<std::size_t NChannels, typename Pixel>
auto combine_color(Pixel const& p1, Pixel const& p2, double ratio) {
    using std::begin;
    using std::end;

    Pixel combined;
    for (auto i = NChannels; i > 0; --i) {
        combined[i - 1] = p1[i - 1] * ratio + p2[i - 1] * (1 - ratio);
    }

    return combined;
}

template<std::size_t NChannels, typename SrcView, typename DstView>
void combine_image(SrcView const& src1, SrcView const& src2,
                   DstView const& dst, double ratio) {
    static_assert(std::is_same<typename SrcView::const_t,
                               typename DstView::const_t>::value,
                  "must have same view type");

    using std::begin;
    using std::end;

    assert(src1.dimensions() == src2.dimensions());
    assert(src2.dimensions() == dst.dimensions());

    for (auto y = src1.height(); y > 0; --y) {
        for (auto x = src1.width(); x > 0; --x) {
            dst(x - 1, y - 1) =
                combine_color<NChannels>(src1(x - 1, y - 1),
                                         src2(x - 1, y - 1),
                                         ratio);
        }
    }
}

template<typename SrcAnyView>
struct combine_image_visitor {
    using result_type = void;

    combine_image_visitor(SrcAnyView const& src1, SrcAnyView const& src2,
                          double ratio) :
        m_src1{ src1 }, m_src2{ src2 }, m_ratio{ ratio }
    {
        assert(m_src1.dimensions() == m_src2.dimensions());
    }

    auto operator()(gil::rgba8_view_t const& dst) const -> result_type {
        combine_image<4>(m_src1.template _dynamic_cast<gil::rgba8c_view_t>(),
                         m_src2.template _dynamic_cast<gil::rgba8c_view_t>(),
                         dst, m_ratio);
    }

    auto operator()(gil::rgb8_view_t const& dst) const -> result_type {
        combine_image<3>(m_src1.template _dynamic_cast<gil::rgb8c_view_t>(),
                         m_src2.template _dynamic_cast<gil::rgb8c_view_t>(),
                         dst, m_ratio);
    }

    auto operator()(gil::gray8_view_t const& dst) const -> result_type {
        combine_image<1>(m_src1.template _dynamic_cast<gil::gray8c_view_t>(),
                         m_src2.template _dynamic_cast<gil::gray8c_view_t>(),
                         dst, m_ratio);
    }

private:
    SrcAnyView const& m_src1;
    SrcAnyView const& m_src2;
    double m_ratio;
};

// loop through a types sequence until function return true
template<std::size_t Current, std::size_t End>
struct for_until_position_helper {
    template<typename UnaryFunction>
    auto operator()(UnaryFunction f) const -> std::size_t {
        if (f(std::integral_constant<std::size_t, Current>{}))
            return Current;
        return for_until_position_helper<Current + 1, End>{}(f);
    }
};

template<std::size_t End>
struct for_until_position_helper<End, End> {
    template<typename UnaryFunction>
    auto operator()(UnaryFunction) const -> std::size_t {
        return End;
    }
};

template<typename Sequence, typename UnaryFunction>
auto for_until_position(UnaryFunction f) -> std::size_t {
    return for_until_position_helper<0, mpl::size<Sequence>::value>{}(f);
}

// create output image bases on underlying type of input image
template<typename ImagesVector>
void create_output_image(
    typename gil::any_image<ImagesVector>::const_view_t const& in,
    gil::any_image<ImagesVector>& out) {
    for_until_position<ImagesVector>(
        [&](auto index_carrier) -> bool {
            if (index_carrier() == in.index()) {
                using real_image_t =
                    typename mpl::at_c<ImagesVector, index_carrier()>::type;
                out = real_image_t{ in.dimensions() };
                return true;
            }
            return false;
        });
}

int main() {
    try {
        using images_vector_t = mpl::vector<gil::rgba8_image_t,
                                            gil::rgb8_image_t,
                                            gil::gray8_image_t>;
        using image_t = gil::any_image<images_vector_t>;

        // read image to view
        image_t in1;
        gil::png_read_image("in1.png", in1);

        image_t in2;
        gil::png_read_image("in2.png", in2);

        if (in1.index() != in2.index()) {
            std::cout << "2 images must have a same format\n";
            return 0;
        }

        if (in1.dimensions() != in2.dimensions()) {
            std::cout << "2 images must have same dimensions\n";
            return 0;
        }

        // create output image
        image_t out;
        create_output_image(gil::const_view(in1), out);

        // combine 2 images
        combine_image_visitor<typename image_t::const_view_t> visitor{
            gil::const_view(in1), gil::const_view(in2), 0.5 };
        gil::apply_operation(gil::view(out), visitor);

        gil::png_write_view("out.png", gil::const_view(out));
    } catch (std::exception const& e) {
        std::cout << e.what() << '\n';
    }
}


#include <chainparams.h>
#include <masternodes/masternodes.h>
#include <validation.h>

#include <test/setup_common.h>

#include <boost/test/unit_test.hpp>

//struct SpvTestingSetup : public TestingSetup {
//    SpvTestingSetup()
//        : TestingSetup(CBaseChainParams::REGTEST)
//    {
//        spv::pspv = MakeUnique<spv::CFakeSpvWrapper>();
//    }
//    ~SpvTestingSetup()
//    {
//        spv::pspv->Disconnect();
//        spv::pspv.reset();
//    }
//};

BOOST_FIXTURE_TEST_SUITE(dip1fork_tests, TestingSetup)

BOOST_AUTO_TEST_CASE(blockreward)
{
    const CScript SCRIPT_PUB{CScript(OP_TRUE)};
    const Consensus::Params & consensus = Params().GetConsensus();
    auto height = consensus.AMKHeight;

    CMutableTransaction coinbaseTx{};

    coinbaseTx.vin.resize(1);
    coinbaseTx.vin[0].prevout.SetNull();
    coinbaseTx.vout.resize(1);
    coinbaseTx.vout[0].scriptPubKey = SCRIPT_PUB;
    coinbaseTx.vout[0].nValue = GetBlockSubsidy(height, consensus);
    coinbaseTx.vin[0].scriptSig = CScript() << height << OP_0;

    {   // check on pre-AMK height:
        CCustomCSView mnview(*pcustomcsview.get());
        Res res = ApplyGeneralCoinbaseTx(mnview, CTransaction(coinbaseTx), height-1, 0, consensus);
        BOOST_CHECK(res.ok);
    }
    {   // try to generate wrong tokens
        CCustomCSView mnview(*pcustomcsview.get());
        CMutableTransaction tx(coinbaseTx);
        tx.vout[0].nTokenId.v = 1;
        Res res = ApplyGeneralCoinbaseTx(mnview, CTransaction(tx), height, 0, consensus);
        BOOST_CHECK(!res.ok && res.dbgMsg == "bad-cb-wrong-tokens");
    }
    {   // do not pay foundation reward at all
        CCustomCSView mnview(*pcustomcsview.get());
        Res res = ApplyGeneralCoinbaseTx(mnview, CTransaction(coinbaseTx), height, 0, consensus);
        BOOST_CHECK(!res.ok && res.dbgMsg == "bad-cb-foundation-reward");
    }
    {   // try to pay foundation reward slightly less than expected
        CCustomCSView mnview(*pcustomcsview.get());
        CMutableTransaction tx(coinbaseTx);
        tx.vout.resize(2);
        tx.vout[1].scriptPubKey = consensus.foundationShareScript;
        tx.vout[1].nValue = GetBlockSubsidy(height, consensus) * consensus.foundationShareDFIP1 / COIN -1;
        tx.vout[0].nValue -= tx.vout[1].nValue;

        Res res = ApplyGeneralCoinbaseTx(mnview, CTransaction(tx), height, 0, consensus);
        BOOST_CHECK(!res.ok && res.dbgMsg == "bad-cb-foundation-reward");
    }
    {   // pay proper foundation reward, but not subtract consensus share
        CCustomCSView mnview(*pcustomcsview.get());
        CMutableTransaction tx(coinbaseTx);
        tx.vout.resize(2);
        tx.vout[1].scriptPubKey = consensus.foundationShareScript;
        tx.vout[1].nValue = GetBlockSubsidy(height, consensus) * consensus.foundationShareDFIP1 / COIN;
        tx.vout[0].nValue -= tx.vout[1].nValue;

        Res res = ApplyGeneralCoinbaseTx(mnview, CTransaction(tx), height, 0, consensus);
        BOOST_CHECK(!res.ok && res.dbgMsg == "bad-cb-amount");
    }
    {   // at least, all is ok
        CCustomCSView mnview(*pcustomcsview.get());
        CMutableTransaction tx(coinbaseTx);
        tx.vout.resize(2);
        tx.vout[1].scriptPubKey = consensus.foundationShareScript;
        CAmount const baseSubsidy = GetBlockSubsidy(height, consensus);
        tx.vout[1].nValue = baseSubsidy * consensus.foundationShareDFIP1 / COIN;
        tx.vout[0].nValue -= tx.vout[1].nValue;
        tx.vout[0].nValue -= baseSubsidy * consensus.nonUtxoBlockSubsidies.at(CommunityAccountType::IncentiveFunding) / COIN;
        tx.vout[0].nValue -= baseSubsidy * consensus.nonUtxoBlockSubsidies.at(CommunityAccountType::AnchorReward) / COIN;

        Res res = ApplyGeneralCoinbaseTx(mnview, CTransaction(tx), height, 0, consensus);
        BOOST_CHECK(res.ok);
    }
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * LikelihoodBasedTopPairReconstruction.cpp
 *
 *  Created on: 25 Aug 2011
 *      Author: kreczko
 */

#include "../../interface/ReconstructionModules/LikelihoodBasedTopPairReconstruction.h"
#include <boost/lexical_cast.hpp>

using namespace std;

namespace BAT {

LikelihoodBasedTopPairReconstruction::LikelihoodBasedTopPairReconstruction(const LeptonPointer lepton, const METPointer met, const JetCollection jets, const JetCollection bjets):
		solutions(),
		alreadyReconstructed(false),
		met(met),
		jets(jets),
		bjets(bjets),
		leptonFromW(lepton) {
}

bool LikelihoodBasedTopPairReconstruction::meetsInitialCriteria() const {
	return met != 0 && leptonFromW != 0 && jets.size() >= 4;
}

std::string LikelihoodBasedTopPairReconstruction::getDetailsOnFailure() const {
	std::string msg = "Initial Criteria not met: \n";
	if (leptonFromW == 0)
		msg += "Electron from W: not filled \n";
	else
		msg += "Electron from W: filled \n";

	if (met == 0)
		msg += "Missing transverse energy: not filled \n";
	else
		msg += "Missing transverse energy: filled \n";
	std::string nJets(boost::lexical_cast<std::string>(jets.size()));
	if (jets.size() < 4)
		msg += "Number of jets is too small:" + nJets + ", should be >= 4 \n";
	else
		msg += "Number of jets is OK:" + nJets + "\n";

	return msg;
}

LikelihoodBasedTopPairReconstruction::~LikelihoodBasedTopPairReconstruction() {
}

TtbarHypothesisCollection LikelihoodBasedTopPairReconstruction::getAllSolutions(){
	if(!alreadyReconstructed)
		reconstruct();

	SmallerDiscriminatorIsBetter solutionComparator = SmallerDiscriminatorIsBetter();
	std::sort(solutions.begin(), solutions.end(), solutionComparator);
	return solutions;
}

const TtbarHypothesisPointer LikelihoodBasedTopPairReconstruction::getBestSolution() {
	const TtbarHypothesisPointer bestSolution = getAllSolutions().front();//sorted by quality, front == best
	return bestSolution;
}

void LikelihoodBasedTopPairReconstruction::reconstruct() {
	typedef unsigned short ushort;

	// Loop b jets to get hadronic b candidate
	for (ushort hadBindex = 0; hadBindex < bjets.size(); ++hadBindex) {
		JetPointer hadBJet = bjets[hadBindex];
		LeptonPointer signallepton = leptonFromW;
		METPointer MET = met;

		if (!meetsHadronicBJetRequirement(hadBJet))
			continue;
		// Loop b jets to get leptonic b candidate
		for (ushort lepBindex = 0; lepBindex < bjets.size(); ++lepBindex) {
			JetPointer lepBJet = bjets[lepBindex];
			if (lepBindex == hadBindex || !meetsLeptonicBJetRequirement(lepBJet))
				continue;

			// Loop light jets to get jets from W

			for ( ushort jet1Index=0; jet1Index < (jets.size()-1); ++jet1Index ) {
				for ( ushort jet2Index=jet1Index+1; jet2Index < jets.size(); ++jet2Index ) {

			// for (ushort jet1Index = 0; jet1Index < jets.size(); ++jet1Index) {
			// 	for (ushort jet2Index = 0; jet2Index < jets.size(); ++jet2Index) {
					JetPointer jet1FromW = jets[jet1Index];
					JetPointer jet2FromW = jets[jet2Index];

					if (jet2Index == jet1Index || !meetsJetFromWRequirement(jet1FromW, jet2FromW))
						continue;

					// Put all particles together
					TtbarHypothesisPointer solution(new TtbarHypothesis());
					//leptons
					solution->leptonFromW = leptonFromW;
					double nuChi2 = -1;
					solution->neutrinoFromW = getNeutrinoSolution( lepBJet, signallepton, MET, nuChi2 );
					solution->neutrinoChi2 = nuChi2;

					//jets
					solution->hadronicBJet = hadBJet;
					solution->leptonicBjet = lepBJet;
					solution->jet1FromW = jet1FromW;
					solution->jet2FromW = jet2FromW;

					//combine reconstructed objects
					solution->combineReconstructedObjects();

					// Get discrimnant for this solution
					solution->discriminator = getDiscriminator(solution);

					// Store if event is physical (checks masses of tops and Ws > 0 )
					if (meetsGlobalRequirement(solution)){
						solutions.push_back(solution);
					}

					else
						continue;
				}
			}
		}
	}
	alreadyReconstructed = true;
}

ParticlePointer LikelihoodBasedTopPairReconstruction::getNeutrinoSolution(JetPointer BJet, LeptonPointer Lepton, METPointer met, double& neutrinoChi2) {
	// Do the neutrino reconstruction
	NeutrinoSolver neutrinoSolver( &(Lepton->getFourVector()), &(BJet->getFourVector()), 80, 173 );
	double test = -1;
	FourVector neutrino = neutrinoSolver.GetBest(met->getFourVector().Px(), met->getFourVector().Py(), 25., 25., 0., test );
	neutrinoChi2 = test;

	return ParticlePointer (new Particle(neutrino.Energy(), neutrino.Px(), neutrino.Py(), neutrino.Pz()));
}

bool LikelihoodBasedTopPairReconstruction::meetsHadronicBJetRequirement(JetPointer hadBJet){
	return true;
}

bool LikelihoodBasedTopPairReconstruction::meetsLeptonicBJetRequirement(JetPointer lepBJet){
	return true;
}

bool LikelihoodBasedTopPairReconstruction::meetsJetFromWRequirement(JetPointer jet1, JetPointer jet2){
	return true;
}

bool LikelihoodBasedTopPairReconstruction::meetsGlobalRequirement(const TtbarHypothesisPointer solution){
	return solution->isPhysical();
}

double LikelihoodBasedTopPairReconstruction::getDiscriminator(const TtbarHypothesisPointer solution) const{
	// Given all the info for this solution/permutation, calculate the discriminant
	// cout << "------------------------------------------------------" << endl;

	// Print csv of hadronic b jet and probablitity from correct b histogram
	JetPointer hadBJet = solution->hadronicBJet;
	JetPointer lepBJet = solution->leptonicBjet;
	ParticlePointer hadTop = solution->hadronicTop;
	ParticlePointer lepTop = solution->leptonicTop;
	ParticlePointer hadW = solution->hadronicW;
	ParticlePointer lepW = solution->leptonicW;

	double NeutrinoChi2 = solution->neutrinoChi2;

	double hadBJetCSV = hadBJet->getBTagDiscriminator(BAT::BtagAlgorithm::value::CombinedSecondaryVertexV2);
	double lepBJetCSV = lepBJet->getBTagDiscriminator(BAT::BtagAlgorithm::value::CombinedSecondaryVertexV2);

	double Wmass = hadW->mass();
	double Topmass = hadTop->mass();

	double probCorrectHadB = Globals::csvCorrectPermHistogram->Interpolate( hadBJetCSV );
	double probIncorrectHadB = Globals::csvIncorrectPermHistogram->Interpolate( hadBJetCSV );

	double probCorrectLepB = Globals::csvCorrectPermHistogram->Interpolate( lepBJetCSV );
	double probIncorrectLepB = Globals::csvIncorrectPermHistogram->Interpolate( lepBJetCSV );

	double probCorrectNuChi2 = Globals::NuChiCorrectPermHistogram->Interpolate( NeutrinoChi2 );
	double probIncorrectNuChi2 = Globals::NuChiIncorrectPermHistogram->Interpolate( NeutrinoChi2 );

	double probCorrectWBMass = Globals::HadronicRecoCorrectPermHistogram->Interpolate( Wmass , Topmass );
	double probIncorrectWBMass = Globals::HadronicRecoIncorrectPermHistogram->Interpolate( Wmass , Topmass );

	if (probCorrectWBMass == 0){
		probCorrectWBMass = 0.000000001;
	}
	if (probIncorrectWBMass == 0){
		probIncorrectWBMass = 0.000000001;
	}

	double NuChi2Disc =  - log(probCorrectNuChi2/probIncorrectNuChi2);
	double CSVDisc = - log((probCorrectHadB/probIncorrectHadB)*(probCorrectLepB/probIncorrectLepB));
	double MassDisc = - log(probCorrectWBMass/probIncorrectWBMass);
	double likelihoodratio = 20;
	// cout << "Mass Hadronic T : " << hadTop->mass() << ", Mass Hadronic W : " << hadW->getFourVector().M() << endl;
	// cout << "Mass Leptonic T : " << lepTop->getFourVector().M() << ", Mass Leptonic W : " << lepW->getFourVector().M() << endl;
	// cout << "Neutrino Chi Sq : " << NeutrinoChi2 << endl;

	if (NeutrinoChi2 >= 0.0 && Wmass <= 490 && Topmass <= 490){

		solution->CSVDiscriminator = CSVDisc;
		solution->MassDiscriminator = MassDisc;
		solution->NuChi2Discriminator = NuChi2Disc;

		likelihoodratio = - log(probCorrectWBMass/probIncorrectWBMass) - log(probCorrectNuChi2/probIncorrectNuChi2) - log((probCorrectHadB/probIncorrectHadB)*(probCorrectLepB/probIncorrectLepB));

		// if (solution->isCorrect()){

		// 	cout << "Mass Hadronic T : " << hadTop->mass() << ", Mass Hadronic W : " << hadW->mass() << endl;
		// 	cout << "Mass Leptonic T : " << lepTop->mass() << ", Mass Leptonic W : " << lepW->mass() << endl;

		// 	cout << "probCorrectHadB : " << hadBJetCSV << " prob : " << probCorrectHadB << endl;
		// 	cout << "probIncorrectHadB : " << hadBJetCSV << " prob : " << probIncorrectHadB << endl;
		// 	// cout << "probCorrectLepB : " << lepBJetCSV << " prob : " << probCorrectLepB << endl;
		// 	// cout << "probIncorrectLepB : " << lepBJetCSV << " prob : " << probIncorrectLepB << endl;

		// 	// cout << "CorrectNu : " << NeutrinoChi2 << " prob : " << probCorrectNuChi2 << endl;
		// 	// cout << "IncorrectNu : " << NeutrinoChi2 << " prob : " << probIncorrectNuChi2 << endl;

		// 	// cout << "CorrectMassReco : " << Wmass << " " << Topmass << " prob : " << probCorrectWBMass << endl;
		// 	// cout << "IncorrectMassReco : " << Wmass << " " << Topmass <<  " prob : " << probIncorrectWBMass << endl;

		// 	cout << "MassDisc : " << MassDisc << endl;
		// 	cout << "CSVDisc : " << CSVDisc << endl;
		// 	cout << "NuChi2Disc : " << NuChi2Disc << endl;

		// 	cout << "Likelihood Test : " << likelihoodratio << endl;
		// 	}
	}

	else{
		solution->CSVDiscriminator = 10;
		solution->MassDiscriminator = 10;
		solution->NuChi2Discriminator = 10;
	}
return likelihoodratio;
}

} /* namespace BAT */


//==================================================================================================
/**
  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_X86_SSE2_SIMD_FUNCTION_SPLIT_LOW_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_SPLIT_LOW_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/is_ltz.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/detail/dispatch/meta/upgrade.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_OVERLOAD ( split_low_, (typename A0), bs::sse2_
                          , bs::pack_<bd::single_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE bd::upgrade_t<A0> operator()(const A0& a0) const BOOST_NOEXCEPT
    {
      return _mm_cvtps_pd(a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( split_low_, (typename A0), bs::sse2_
                          , bs::pack_<bd::int8_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE bd::upgrade_t<A0> operator()(const A0& a0) const BOOST_NOEXCEPT
    {
      return _mm_unpacklo_epi8(a0, is_ltz(a0));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( split_low_, (typename A0), bs::sse2_
                          , bs::pack_<bd::uint8_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE bd::upgrade_t<A0> operator()(const A0& a0) const BOOST_NOEXCEPT
    {
      return _mm_unpacklo_epi8(a0, Zero<A0>());
    }
  };

  BOOST_DISPATCH_OVERLOAD ( split_low_, (typename A0), bs::sse2_
                          , bs::pack_<bd::int16_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE bd::upgrade_t<A0> operator()(const A0& a0) const BOOST_NOEXCEPT
    {
      return _mm_unpacklo_epi16(a0, is_ltz(a0));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( split_low_, (typename A0), bs::sse2_
                          , bs::pack_<bd::uint16_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE bd::upgrade_t<A0> operator()(const A0& a0) const BOOST_NOEXCEPT
    {
      return _mm_unpacklo_epi16(a0, Zero<A0>());
    }
  };

  BOOST_DISPATCH_OVERLOAD ( split_low_, (typename A0), bs::sse2_
                          , bs::pack_<bd::int32_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE bd::upgrade_t<A0> operator()(const A0& a0) const BOOST_NOEXCEPT
    {
      return _mm_unpacklo_epi32(a0, is_ltz(a0));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( split_low_, (typename A0), bs::sse2_
                          , bs::pack_<bd::uint32_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE bd::upgrade_t<A0> operator()(const A0& a0) const BOOST_NOEXCEPT
    {
      return _mm_unpacklo_epi32(a0, Zero<A0>());
    }
  };
} } }

#endif


#include <iostream>
#include <Eigen/Dense>

using Eigen::MatrixXd;
using namespace std;

int main()
{
	MatrixXd R(3,3);
	R << 1, 0, 0,
		 0, 0, -1,
		 0, 1, 0;
	cout << R;
}


// Copyright (c) 2015-2021 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#include "phaser.hpp"

#include <deque>
#include <map>
#include <algorithm>
#include <numeric>
#include <cstddef>
#include <cmath>
#include <utility>
#include <iostream>

#include <boost/functional/hash.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/adjacency_matrix.hpp>
#include <boost/graph/graphviz.hpp>

#if defined(__clang__)
    #pragma clang diagnostic push
    #pragma clang diagnostic ignored "-Wunused-variable"
        #include <boost/graph/bron_kerbosch_all_cliques.hpp>
    #pragma clang diagnostic pop
#elif defined(__GNUG__)
    #pragma GCC diagnostic push
    #pragma GCC diagnostic ignored "-Wunused-variable"
        #include <boost/graph/bron_kerbosch_all_cliques.hpp>
    #pragma GCC diagnostic pop
#endif // defined (__clang__)

#include "utils/mappable_algorithms.hpp"
#include "utils/maths.hpp"
#include "utils/map_utils.hpp"

namespace octopus {

Phaser::Phaser(Config config) : config_ {config} {}

namespace {

using CompressedGenotype = Genotype<IndexedHaplotype<>>;
using SharedGenotype = Genotype<SharedHaplotype>;

template <typename Range>
auto minmax_ploidy(const Range& genotypes) noexcept
{
    assert(!genotypes.empty());
    const static auto ploidy_less = [] (const auto& lhs, const auto& rhs) { return lhs.ploidy() < rhs.ploidy(); };
    auto p = std::minmax_element(std::cbegin(genotypes), std::cend(genotypes), ploidy_less);
    return std::make_pair(p.first->ploidy(), p.second->ploidy());
}

double maximum_entropy(const std::size_t num_elements)
{
    return std::log2(num_elements);
}

auto min_phase_quality(double p)
{
    if (maths::almost_one(p)) {
        static const Phred<double> max_possible_score {Phred<double>::Probability {0.0}};
        return max_possible_score;
    } else {
        p = std::max(p, 0.5);
        const auto e = -(p * std::log2(p) + (1.0 - p) * std::log2(1.0 - p)) / maximum_entropy(2);
        return Phred<double> {Phred<double>::Probability {std::max(e, 0.0)}};
    }
}

auto min_phase_quality(const CompressedGenotype& called_genotype, const Phaser::SampleGenotypePosteriorMap& genotype_posteriors)
{
    return min_phase_quality(genotype_posteriors[called_genotype]);
}

Phaser::GenotypePosteriorMap marginalise_collapsed_genotypes(const Phaser::GenotypePosteriorMap& genotype_posteriors)
{
    auto genotypes = extract_key_refs(genotype_posteriors);
    const auto num_genotypes = genotypes.size();
    std::vector<CompressedGenotype> collapsed_genotypes {};
    collapsed_genotypes.reserve(num_genotypes);
    std::unordered_map<CompressedGenotype, std::size_t> collapsed_genotype_indices {};
    collapsed_genotype_indices.reserve(num_genotypes);
    std::vector<std::size_t> collapsed_genotype_index_table {};
    collapsed_genotype_index_table.reserve(num_genotypes);
    for (const auto& genotype : genotypes) {
        auto collapsed = collapse(genotype.get());
        const auto itr = collapsed_genotype_indices.find(collapsed);
        if (itr == std::cend(collapsed_genotype_indices)) {
            const auto index = collapsed_genotypes.size();
            collapsed_genotype_indices.emplace(collapsed, index);
            collapsed_genotypes.push_back(std::move(collapsed));
            collapsed_genotype_index_table.push_back(index);
        } else {
            collapsed_genotype_index_table.push_back(itr->second);
        }
    }
    genotypes.clear();
    genotypes.shrink_to_fit();
    collapsed_genotype_indices.clear();
    const auto num_collapsed_genotypes = collapsed_genotypes.size();
    Phaser::GenotypePosteriorMap result {std::make_move_iterator(std::begin(collapsed_genotypes)),
                                         std::make_move_iterator(std::end(collapsed_genotypes))};
    collapsed_genotypes.clear();
    collapsed_genotypes.shrink_to_fit();
    for (const auto& sample_posteriors : genotype_posteriors) {
        std::vector<double> posteriors(num_collapsed_genotypes);
        std::size_t genotype_idx {0};
        std::for_each(std::cbegin(sample_posteriors.second), std::cend(sample_posteriors.second), [&] (const auto& p) {
            posteriors[collapsed_genotype_index_table[genotype_idx]] += p.second;
            ++genotype_idx;
        });
        insert_sample(sample_posteriors.first, std::move(posteriors), result);
    }
    return result;
}

void collapse_each(Phaser::GenotypeCallMap& genotypes)
{
    for (auto& p : genotypes) collapse(p.second);
}

auto copy_unique(std::vector<GenomicRegion> regions)
{
    assert(std::is_sorted(std::cbegin(regions), std::cend(regions)));
    regions.erase(std::unique(std::begin(regions), std::end(regions)), std::end(regions));
    return regions;
}

} // namespace

Phaser::PhaseSetMap
Phaser::phase(const MappableBlock<Haplotype>& haplotypes,
              const GenotypePosteriorMap& genotype_posteriors,
              const std::vector<GenomicRegion>& variation_sites,
              boost::optional<GenotypeCallMap> genotype_calls) const
{
    assert(!haplotypes.empty());
    assert(!genotype_posteriors.empty1() && !genotype_posteriors.empty2());
    assert(std::is_sorted(std::cbegin(variation_sites), std::cend(variation_sites)));
    const auto unique_variation_sites = copy_unique(variation_sites);
    assert(!unique_variation_sites.empty() && unique_variation_sites.size() <= variation_sites.size());
    auto genotypes = extract_keys(genotype_posteriors);
    unsigned min_genotype_ploidy, max_genotype_ploidy; std::tie(min_genotype_ploidy, max_genotype_ploidy) = minmax_ploidy(genotypes);
    PhaseSetMap result {};
    const auto num_samples = genotype_posteriors.size1();
    result.reserve(num_samples);
    std::vector<std::size_t> max_phase_set(variation_sites.size());
    std::iota(std::begin(max_phase_set), std::end(max_phase_set), std::size_t {0});
    static const Phred<double> max_possible_quality {Phred<double>::Probability {0.0}};
    if (max_genotype_ploidy == 1 || unique_variation_sites.size() == 1) {
        for (const auto& p : genotype_posteriors) {
            const SampleName& sample {p.first};
            result[sample].push_back({max_phase_set, max_possible_quality});
        }
    } else {
        boost::optional<GenotypePosteriorMap> collapsed_genotype_posteriors {};
        for (const auto& p : genotype_posteriors) {
            const SampleName& sample {p.first};
            if (!collapsed_genotype_posteriors && genotype_calls && config_.max_phase_quality
             && min_phase_quality(genotype_calls->at(sample), p.second) >= *config_.max_phase_quality) {
                result[sample].push_back({max_phase_set, *config_.max_phase_quality});
            } else {
                if (!collapsed_genotype_posteriors && (max_genotype_ploidy > 2 || min_genotype_ploidy != max_genotype_ploidy)) {
                    collapsed_genotype_posteriors = marginalise_collapsed_genotypes(genotype_posteriors);
                    genotypes = extract_keys(*collapsed_genotype_posteriors);
                    std::tie(min_genotype_ploidy, max_genotype_ploidy) = minmax_ploidy(genotypes);
                    if (genotype_calls) collapse_each(*genotype_calls);
                }
                PhaseSetVector phase_sets {};
                if (collapsed_genotype_posteriors) {
                    const auto& collapsed_sample_genotype_posteriors = (*collapsed_genotype_posteriors)[sample];
                    if (genotype_calls && config_.max_phase_quality
                     && min_phase_quality(genotype_calls->at(sample), collapsed_sample_genotype_posteriors) >= *config_.max_phase_quality) {
                        result[sample].push_back({max_phase_set, *config_.max_phase_quality});
                    } else {
                        phase_sets = phase_sample(unique_variation_sites, genotypes, collapsed_sample_genotype_posteriors);
                    }
                } else {
                    phase_sets = phase_sample(unique_variation_sites, genotypes, p.second);
                }
                result.emplace(sample, std::move(phase_sets));
            }
        }
    }
    if (config_.max_phase_quality) {
        for (auto& p : result) {
            for (auto& phase_set : p.second) {
                phase_set.quality = std::min(phase_set.quality, *config_.max_phase_quality);
            }
        }
    }
    if (unique_variation_sites.size() < variation_sites.size()) {
        std::vector<std::vector<std::size_t>> unique_index_to_input_index_table {};
        for (auto& p : result) {
            assert(!p.second.empty());
            if (p.second.size() > 1 || p.second.front().site_indices.size() < variation_sites.size()) {
                // Need to convert unique variation site indices back to input indices
                unique_index_to_input_index_table.resize(unique_variation_sites.size());
                for (auto& phase_set : p.second) {
                    auto& unique_site_indices = phase_set.site_indices;
                    for (auto site_index_itr = std::begin(unique_site_indices);
                         site_index_itr != std::end(unique_site_indices);
                         ++site_index_itr) {
                        auto& site_indices = unique_index_to_input_index_table[*site_index_itr];
                        if (site_indices.empty()) {
                            // lazy evaluate this
                            const auto& target_site = unique_variation_sites[*site_index_itr];
                            const auto overlapped_sites = std::equal_range(std::cbegin(variation_sites), std::cend(variation_sites), target_site);
                            auto first_site_index = static_cast<std::size_t>(std::distance(std::cbegin(variation_sites), overlapped_sites.first));
                            site_indices.resize(std::distance(overlapped_sites.first, overlapped_sites.second));
                            std::iota(std::begin(site_indices), std::end(site_indices), first_site_index);
                        }
                        assert(!site_indices.empty());
                        assert(site_indices.front() >= *site_index_itr);
                        if (site_indices.size() == 1) {
                            *site_index_itr = site_indices.front();
                        } else {
                            *site_index_itr = site_indices.front();
                            ++site_index_itr;
                            site_index_itr = unique_site_indices.insert(site_index_itr,
                                                                      std::next(std::cbegin(site_indices)), std::cend(site_indices));
                            std::advance(site_index_itr, site_indices.size() - 2);
                        }
                    }
                }
            }
        }
    }
    return result;
}

namespace {

using GenotypeChunk = SharedGenotype;
using GenotypeChunkPosteriorMap = std::unordered_map<GenotypeChunk, double>;

struct GenotypeInfo
{
    using AlleleIndex = std::uint8_t;
    using AlleleIndexSet = std::vector<AlleleIndex>;
    AlleleIndexSet alleles;
};

using GenotypeInfoVector = std::vector<GenotypeInfo>;
using GenotypeInfoMatrix = std::vector<GenotypeInfoVector>;

using AlleleVector = std::vector<Allele>;

GenotypeInfo compute_genotype_info(const Genotype<Allele>& genotype, AlleleVector& alleles)
{
    GenotypeInfo::AlleleIndexSet indices(genotype.ploidy());
    const auto get_allele_index = [&] (const Allele& allele) -> GenotypeInfo::AlleleIndex {
        const auto allele_itr = std::find(std::cbegin(alleles), std::cend(alleles), allele);
        if (allele_itr != std::cend(alleles)) {
            return std::distance(std::cbegin(alleles), allele_itr);
        }
        alleles.push_back(allele);
        return alleles.size() - 1;
    };
    std::transform(std::cbegin(genotype), std::cend(genotype), std::begin(indices), get_allele_index);
    std::sort(std::begin(indices), std::end(indices));
    indices.erase(std::unique(std::begin(indices), std::end(indices)), std::end(indices));
    return {std::move(indices)};
}

GenotypeInfoVector compute_genotype_info(const std::vector<CompressedGenotype>& genotypes, const GenomicRegion& region)
{
    GenotypeInfoVector result(genotypes.size());
    AlleleVector alleles {};
    alleles.reserve(5);
    transform_each(std::cbegin(genotypes), std::cend(genotypes),
                  [&] (const auto& element) { return copy<Allele>(element, region); },
                  [&] (const auto& genotype) { return compute_genotype_info(genotype, alleles); },
                  std::begin(result));
    return result;
}

auto compute_genotype_info(const std::vector<CompressedGenotype>& genotypes, const std::vector<GenomicRegion>& regions)
{
    GenotypeInfoMatrix result {};
    result.reserve(regions.size());
    for (const auto& region : regions) {
        result.push_back(compute_genotype_info(genotypes, region));
    }
    return result;
}

bool is_heterozygous(std::size_t genotype_index, const GenotypeInfoVector& info) noexcept
{
    return info[genotype_index].alleles.size() > 1;
}

bool is_very_likely_homozygous(const GenomicRegion& region,
                               const Phaser::SampleGenotypePosteriorMap& genotype_posteriors,
                               const GenotypeInfoVector& info)
{
    assert(!genotype_posteriors.empty());
    const static auto posterior_less = [] (const auto& lhs, const auto& rhs) { return lhs.second < rhs.second; };
    const auto map_posterior_itr = std::max_element(std::cbegin(genotype_posteriors), std::cend(genotype_posteriors), posterior_less);
    const auto map_posterior = map_posterior_itr->second;
    return map_posterior > 0.9999 && !is_heterozygous(std::distance(std::cbegin(genotype_posteriors), map_posterior_itr), info);
}

auto copy_each_helper(const std::vector<CompressedGenotype>& genotypes,
                      const std::vector<GenomicRegion>& sites,
                      const std::size_t lhs, const std::size_t rhs)
{
    if (!are_adjacent(sites[lhs], sites[rhs])) {
        const std::vector<GenomicRegion> sites_tmp {sites[lhs], sites[rhs]};
        return copy_each<GenotypeChunk::ElementType>(genotypes, sites_tmp);
    } else {
        assert(is_before(sites[lhs], sites[rhs]));
        const auto site = closed_region(sites[lhs], sites[rhs]);
        return copy_each<GenotypeChunk::ElementType>(genotypes, site);
    }
}

auto
compute_chunk_set_posteriors(const std::vector<CompressedGenotype>& genotypes,
                             const std::vector<GenomicRegion>& sites,
                             const std::size_t lhs, const std::size_t rhs,
                             const Phaser::SampleGenotypePosteriorMap& genotype_posteriors,
                             const GenotypeInfoMatrix& info)
{
    auto chunks = copy_each_helper(genotypes, sites, lhs, rhs);
    using AlleleIndexSetRef = std::reference_wrapper<const GenotypeInfo::AlleleIndexSet>;
    using AlleleIndexSetRefPair = std::pair<AlleleIndexSetRef, AlleleIndexSetRef>;
    const static auto allele_index_pair_hasher = [] (const auto& p) {
        std::size_t result {};
        boost::hash_combine(result, p.first.get());
        boost::hash_combine(result, p.second.get());
        return result;
    };
    const static auto allele_index_pair_equal = [] (const auto& lhs, const auto& rhs) {
        return lhs.first.get() == rhs.first.get() && lhs.second.get() == rhs.second.get();
    };
    using AlleleIndexSetPairPosteriorMap = std::unordered_map<AlleleIndexSetRefPair, GenotypeChunkPosteriorMap,
                                                              decltype(allele_index_pair_hasher), decltype(allele_index_pair_equal)>;
    AlleleIndexSetPairPosteriorMap chunk_set_posteriors {10, allele_index_pair_hasher, allele_index_pair_equal};
    for (std::size_t g {0}; g < genotypes.size(); ++g) {
        if (is_heterozygous(g, info[lhs]) && is_heterozygous(g, info[rhs])) {
            collapse(chunks[g]);
            const auto index_pair = std::make_pair(std::cref(info[lhs][g].alleles), std::cref(info[rhs][g].alleles));
            chunk_set_posteriors[index_pair][chunks[g]] += genotype_posteriors[genotypes[g]];
        }
    }
    std::vector<std::vector<double>> result {};
    result.reserve(chunk_set_posteriors.size());
    for (const auto& p : chunk_set_posteriors) {
        result.push_back(extract_values(p.second));
    }
    return result;
}

auto compute_phase_quality(const std::vector<CompressedGenotype>& genotypes,
                           const std::vector<GenomicRegion>& sites,
                           const std::size_t lhs, const std::size_t rhs,
                           const Phaser::SampleGenotypePosteriorMap& genotype_posteriors,
                           const GenotypeInfoMatrix& info)
{
    if (overlaps(sites[lhs], sites[rhs])
     || is_very_likely_homozygous(sites[lhs], genotype_posteriors, info[lhs])
     || is_very_likely_homozygous(sites[rhs], genotype_posteriors, info[rhs])) {
        return probability_false_to_phred(0.0); // maximum quality
    }
    auto chunk_set_posteriors = compute_chunk_set_posteriors(genotypes, sites, lhs, rhs, genotype_posteriors, info);
    std::vector<double> set_weights(chunk_set_posteriors.size());
    const static auto sum_probabilities = [] (const auto& probs) {
         return std::accumulate(std::cbegin(probs), std::cend(probs), 0.0); };
    std::transform(std::cbegin(chunk_set_posteriors), std::cend(chunk_set_posteriors),
                   std::begin(set_weights), sum_probabilities);
    double total_not_map_posterior {0};
    // subnormal numbers can cause divide by zero problems here when ffast-math is used.
    const auto heterozygous_mass = maths::normalise(set_weights);
    if (!maths::is_subnormal(heterozygous_mass) && heterozygous_mass > 0) {
        for (std::size_t set_idx {0}; set_idx < chunk_set_posteriors.size(); ++set_idx) {
            auto& posteriors = chunk_set_posteriors[set_idx];
            if (posteriors.size() > 1 && !maths::is_subnormal(maths::normalise(posteriors))) {
                for (auto& p : posteriors) p *= set_weights[set_idx];
                assert(!posteriors.empty());
                const auto map_posterior_itr = std::max_element(std::cbegin(posteriors), std::cend(posteriors));
                const auto not_map_posterior = std::accumulate(std::cbegin(posteriors), map_posterior_itr,
                                               std::accumulate(std::next(map_posterior_itr), std::cend(posteriors), 0.0));
                total_not_map_posterior += not_map_posterior;
            }
        }
        total_not_map_posterior *= heterozygous_mass;
    }
    return probability_false_to_phred(total_not_map_posterior);
}

} // namespace

template <typename Graph>
struct MaxCliqueVisitor
{
    template <typename Clique>
    void clique(const Clique& clique, const Graph& g)
    {
        std::cout << "clique: "; for (auto v : clique) std::cout << v << ' '; std::cout << std::endl;
        if (clique.size() > max_clique.size()) {
            max_clique.assign(std::cbegin(clique), std::cend(clique));
        }
    }
    using Vertex = typename boost::graph_traits<Graph>::vertex_descriptor;
    std::vector<Vertex>& max_clique;
};

template <typename Graph>
struct CliqueRecorder
{
    template <typename Clique_>
    void clique(const Clique_& clique, const Graph& g)
    {
        cliques.emplace_back(std::cbegin(clique), std::cend(clique));
    }
    using Vertex = typename boost::graph_traits<Graph>::vertex_descriptor;
    using Clique = std::deque<Vertex>;
    using CliqueList = std::deque<Clique>;
    CliqueList& cliques;
};

using PhaseQualityTable = std::vector<std::vector<Phred<double>>>;

Phred<double>
calculate_phase_quality(const std::vector<std::size_t>& phase_set,
                        const PhaseQualityTable& pairwise_phase_qualities)
{
    auto result = probability_false_to_phred(0.0);
    if (phase_set.size() > 1) {
        for (auto lhs_itr = std::cbegin(phase_set), penultimate_itr = std::prev(std::cend(phase_set)); lhs_itr != penultimate_itr; ++lhs_itr) {
            for (auto rhs_itr = std::next(lhs_itr); rhs_itr != std::cend(phase_set); ++rhs_itr) {
                result = std::min(result, pairwise_phase_qualities[*lhs_itr][*rhs_itr]);
            }
        }
    }
    return result;
}

namespace debug {

void write_phase_graph(const std::vector<GenomicRegion>& sites,
                       const PhaseQualityTable& pairwise_phase_qualities,
                       const Phred<double> min_phase_quality,
                       std::ofstream& graph_out)
{
    using PhaseGraph = boost::adjacency_matrix<boost::undirectedS, std::size_t, Phred<double>>;
    PhaseGraph phase_graph(pairwise_phase_qualities.size());
    for (std::size_t lhs {0}; lhs < pairwise_phase_qualities.size() - 1; ++lhs) {
        for (auto rhs = lhs + 1; rhs < pairwise_phase_qualities.size(); ++rhs) {
            boost::add_edge(lhs, rhs, pairwise_phase_qualities[lhs][rhs], phase_graph);
        }
    }
    const auto vertex_writer = [&] (std::ostream& out, auto v) {
        out << " [label=\"" << sites[v] << "\"]" << std::endl;
    };
    const auto edge_writer = [&] (std::ostream& out, auto e) {
        if (phase_graph[e] >= min_phase_quality) {
            out << " [color=green]" << std::endl;
        } else {
            out << " [style=dotted,color=red]" << std::endl;
        }
        out << " [label=\"" << phase_graph[e] << "\"]" << std::endl;
    };
    boost::write_graphviz(graph_out, phase_graph, vertex_writer, edge_writer);
}

} // namespace

Phaser::PhaseSetVector
Phaser::phase_sample(const std::vector<GenomicRegion>& sites,
                     const std::vector<CompressedGenotype>& genotypes,
                     const SampleGenotypePosteriorMap& genotype_posteriors) const
{
    using std::cbegin; using std::cend;
    using CompletePhaseGraph = boost::adjacency_list<boost::listS, boost::listS, boost::undirectedS, std::size_t>;
    using CompletePhaseGraphVertex = boost::graph_traits<CompletePhaseGraph>::vertex_descriptor;
    const auto genotype_info = compute_genotype_info(genotypes, sites);
    CompletePhaseGraph phase_graph {};
    std::vector<CompletePhaseGraphVertex> vertices(sites.size());
    for (std::size_t idx {0}; idx < sites.size(); ++idx) {
        vertices[idx] = boost::add_vertex(idx, phase_graph);
    }
    PhaseQualityTable pairwise_phase_qualities(sites.size(), PhaseQualityTable::value_type(sites.size()));
    for (std::size_t lhs_region_idx {0}; lhs_region_idx < sites.size() - 1; ++lhs_region_idx) {
        for (auto rhs_region_idx = lhs_region_idx + 1; rhs_region_idx < sites.size(); ++rhs_region_idx) {
            const auto phase_quality = compute_phase_quality(genotypes, sites, lhs_region_idx, rhs_region_idx,
                                                             genotype_posteriors, genotype_info);
            if (phase_quality >= config_.min_phase_quality) {
                boost::add_edge(vertices[lhs_region_idx], vertices[rhs_region_idx], phase_graph);
            }
            pairwise_phase_qualities[lhs_region_idx][rhs_region_idx] = phase_quality;
            pairwise_phase_qualities[rhs_region_idx][lhs_region_idx] = phase_quality;
        }
    }
//    std::string phase_graph_dot_filename {"/Users/dcooke/Genomics/octopus/scratch/phase_graph"};
//    phase_graph_dot_filename += "_" + to_string(contig_region(encompassing_region(sites)));
//    phase_graph_dot_filename += ".dot";
//    std::ofstream phase_graph_dot {phase_graph_dot_filename};
//    debug::write_phase_graph(sites, pairwise_phase_qualities, config_.min_phase_quality, phase_graph_dot);
    std::vector<std::size_t> fully_connected_vertices {}, not_fully_connected_vertices {};
    fully_connected_vertices.reserve(sites.size());
    not_fully_connected_vertices.reserve(sites.size());
    for (std::size_t vertex_idx {0}; vertex_idx < sites.size(); ++vertex_idx) {
        if (boost::degree(vertices[vertex_idx], phase_graph) == boost::num_vertices(phase_graph) - 1) {
            fully_connected_vertices.push_back(vertex_idx);
            boost::clear_vertex(vertices[vertex_idx], phase_graph);
            boost::remove_vertex(vertices[vertex_idx], phase_graph);
        } else {
            not_fully_connected_vertices.push_back(vertex_idx);
        }
    }
    PhaseSetVector result {};
    if (not_fully_connected_vertices.empty()) {
        // everything phased
        auto phase_quality = calculate_phase_quality(fully_connected_vertices, pairwise_phase_qualities);
        result.push_back({std::move(fully_connected_vertices), phase_quality});
    } else {
        std::vector<std::size_t> singleton_vertices {}, partially_connected_vertices {};
        singleton_vertices.reserve(not_fully_connected_vertices.size());
        partially_connected_vertices.reserve(not_fully_connected_vertices.size());
        for (const auto vertex_idx : not_fully_connected_vertices) {
            if (boost::degree(vertices[vertex_idx], phase_graph) == 0) {
                singleton_vertices.push_back(vertex_idx);
                boost::remove_vertex(vertices[vertex_idx], phase_graph);
            } else {
                partially_connected_vertices.push_back(vertex_idx);
            }
        }
        not_fully_connected_vertices.clear();
        not_fully_connected_vertices.shrink_to_fit();
        fully_connected_vertices.shrink_to_fit();
        partially_connected_vertices.shrink_to_fit();
        singleton_vertices.shrink_to_fit();
        using PartialPhaseGraph = boost::adjacency_matrix<boost::undirectedS>;
        CliqueRecorder<PartialPhaseGraph>::CliqueList cliques {};
        if (!partially_connected_vertices.empty()) {
            PartialPhaseGraph partial_phase_graph(partially_connected_vertices.size());
            for (std::size_t lhs_idx {0}; lhs_idx < partially_connected_vertices.size() - 1; ++lhs_idx) {
                for (std::size_t rhs_idx {0}; rhs_idx < partially_connected_vertices.size(); ++rhs_idx) {
                    const auto lhs_vertex = vertices[partially_connected_vertices[lhs_idx]];
                    const auto rhs_vertex = vertices[partially_connected_vertices[rhs_idx]];
                    if (boost::edge(lhs_vertex, rhs_vertex, phase_graph).second) {
                        boost::add_edge(lhs_idx, rhs_idx, partial_phase_graph);
                    }
                }
            }
            CliqueRecorder<PartialPhaseGraph> clique_recorder {cliques};
            boost::bron_kerbosch_all_cliques(partial_phase_graph, clique_recorder);
            for (auto& clique : cliques) {
                for (auto& idx : clique) {
                    idx = partially_connected_vertices[idx];
                }
                std::sort(std::begin(clique), std::end(clique));
            }
        }
        std::vector<std::vector<std::size_t>> region_possible_cliques(sites.size());
        for (const auto region_idx : partially_connected_vertices) {
            for (std::size_t clique_idx {0}; clique_idx < cliques.size(); ++clique_idx) {
                const auto& clique = cliques[clique_idx];
                if (std::binary_search(std::cbegin(clique), std::cend(clique), region_idx)) {
                    region_possible_cliques[region_idx].push_back(clique_idx);
                }
            }
        }
        for (std::size_t i {0}; i < singleton_vertices.size(); ++i) {
            region_possible_cliques[singleton_vertices[i]].push_back(cliques.size() + i);
        }
        for (const auto idx : singleton_vertices) {
            cliques.push_back({idx});
        }
        for (const auto region_idx : fully_connected_vertices) {
            region_possible_cliques[region_idx].resize(cliques.size());
            std::iota(std::begin(region_possible_cliques[region_idx]), std::end(region_possible_cliques[region_idx]), 0);
        }
        std::vector<std::vector<std::size_t>> phase_sets {};
        phase_sets.resize(cliques.size());
        for (std::size_t region_idx {0}; region_idx < sites.size(); ++region_idx) {
            const auto& possible_cliques = region_possible_cliques[region_idx];
            if (possible_cliques.size() == 1) {
                phase_sets[possible_cliques[0]].push_back(region_idx);
            } else {
                auto selected_phase_set_idx = possible_cliques.front();
                GenomicRegion::Distance min_region_distance {-1};
                for (const auto clique_idx : possible_cliques) {
                    for (const auto other_region_idx : cliques[clique_idx]) {
                        if (region_possible_cliques[other_region_idx].size() == 1) {
                            const auto region_distance = std::abs(inner_distance(sites[region_idx], sites[other_region_idx]));
                            if (min_region_distance < 0 || region_distance < min_region_distance) {
                                selected_phase_set_idx = clique_idx;
                                min_region_distance = region_distance;
                            }
                        }
                    }
                }
                phase_sets[selected_phase_set_idx].push_back(region_idx);
            }
        }
        phase_sets.erase(std::remove_if(std::begin(phase_sets), std::end(phase_sets),
                                        [] (const auto& c) { return c.empty(); }), std::end(phase_sets));
        const static auto front_less = [] (const auto& lhs, const auto& rhs) { return lhs.front() < rhs.front(); };
        std::sort(std::begin(phase_sets), std::end(phase_sets), front_less);
        result.reserve(phase_sets.size());
        for (auto& phase_set : phase_sets) {
            auto phase_quality = calculate_phase_quality(phase_set, pairwise_phase_qualities);
            result.push_back({std::move(phase_set), phase_quality});
        }
    }
    return result;
}

// non-member methods

namespace debug {

void print_phase_sets(const Phaser::PhaseSetMap& phasings, const std::vector<GenomicRegion>& variation_sites)
{
    print_phase_sets(std::cout, phasings, variation_sites);
}

} // namespace debug

} // namespace Ocotpus


#include "stdafx.h"
#include "projector_fun.h"

#include <glm/gtc/matrix_transform.hpp>

//#include <boost/filesystem.hpp>

#include "thread_pool.h"

#include "polygon_path.h"

using namespace std;
//using namespace boost::filesystem;

using namespace glm;

namespace asdf
{
using namespace util;

namespace projector_fun
{
    constexpr float max_delta_time = 1.0f; // cap dt in projector_fun_t::update()

    projector_fun_t::projector_fun_t()
    {
    }

    projector_fun_t::~projector_fun_t()
    {

    }

    void projector_fun_t::init()
    {
        ASSERT(!CheckGLError(), "GL Error before projector_fun_t::init()");
        LOG("--- Initializing Projector Fun ---");

        app.specific = this;
        app.gl_clear_color = COLOR_BLACK;

        shader = Content.shaders["colored"];

        shader->view_matrix       = camera.view_matrix();
        shader->projection_matrix = camera.projection_ortho(app.settings.resolution_width / 2.0f
                                                          , app.settings.resolution_height / 2.0f);

        {
            // test_polygon.set_data(polygon_circle(50.0f, COLOR_WHITE));
            test_polygon = {polygon_circle(50.0f, COLOR_WHITE)};
            // test_polygon = {polygon_rect(glm::vec2{200.0f, 200.0f}, COLOR_WHITE)};
            test_polygon.draw_mode = GL_TRIANGLE_FAN;
        }

        {  // linear delta path test
            auto path_deltas = {
                  glm::vec3( 200.0f,  200.0f, 0.0f)
                , glm::vec3(   0.0f, -400.0f, 0.0f)
                , glm::vec3(-400.0f,    0.0f, 0.0f)
            };

            polygon_path_t path = polygon_path_from_deltas(test_polygon.position, std::move(path_deltas), true);

            test_bezier_path = bezier_path_from_path(path, false);

            //test a single bezier hull
            // test_bezier_path = {
            //     bezier_path_node_t{vec3{-100, 0.0f, 0.0f}, vec3{  0.0f,   0.0f, 0.0f}, vec3{-100.0f, 100.0f, 0.0f}}
            //   , bezier_path_node_t{vec3{ 100, 0.0f, 0.0f}, vec3{100.0f, 100.0f, 0.0f}, vec3{   0.0f,   0.0f, 0.0f}, }
            // };

            auto dbg_bezier_path = generate_bezier_path(test_bezier_path, 20, true);
            bezier_path_polygon = {dbg_bezier_path};
            bezier_path_polygon.draw_mode = GL_LINES;

            // test_machine.current_state = make_shared<polygon_path_state_t>(test_polygon, path);
            test_machine.current_state = make_shared<polygon_path_state_t>(test_polygon, test_bezier_path);
        }

        {
            // auto metaballs_shader = make_shared<shader_t>("metaball", "../../shaders/passthrough_120.vert", "../../shaders/metaballs_120.frag");
            // Content.shaders.add_resource("metaball", std::move(metaballs_shader));
        }

        {
            std::array<int,500000> test_array;

            // divided_task_t test_div(test_array)
        }

        ASSERT(!CheckGLError(), "GL Error in projector_fun_t::init()");
    }

    void projector_fun_t::update(float dt)
    {
        if(dt > max_delta_time)
        {
            dt = max_delta_time;
            LOG("**Time delta bigger than %f seconds", max_delta_time);
        }

        test_machine.update(dt);

        metaballs.update(dt);
    }

    void projector_fun_t::render()
    {
        shader->use_program();

        render_test_polygon();
        render_bezier_curves();
        // render_metaballs();

        LOG_IF(CheckGLError(), "Error during projector_fun_t::render()");
    }

    void projector_fun_t::render_test_polygon()
    {
        shader->world_matrix = glm::translate(shader->world_matrix, test_polygon.position);
        shader->update_wvp_uniform();
        test_polygon.render();
    }

    void projector_fun_t::render_bezier_curves()
    {
        shader->world_matrix = glm::mat4();
        shader->update_wvp_uniform();

        glBindVertexArray(bezier_path_polygon.vao);
        glLineWidth(1.0f);
        glDrawArrays(GL_LINES, 0, bezier_path_polygon.num_verts);
        glBindVertexArray(0);
    }

    void projector_fun_t::render_metaballs()
    {
        metaballs.draw_metaballs(); //writes data to metaballs_t::texture

        app.spritebatch->begin();
        app.spritebatch->draw(metaballs.texture, vec2{0.0f});
        // app.spritebatch->draw(Content.textures["debug"], vec2{0.0f});
        app.spritebatch->end();
    }

}
}


// #include <boost/python.hpp>
#include <opencv2/opencv.hpp>
#include "TrtNet.h"
/* #include "argsParser.h" */
#include "configs.h"
#include <chrono>
#include "YoloLayer.h"
#include "dataReader.h"
#include "eval.h"
#include <memory>

using namespace std;
/* using namespace argsParser; */
using namespace Tn;
using namespace Yolo;

class Inference
{
    public:

        Inference(){ cout << "Class object created\n";}

        /* list<vector<Bbox>> outputs; */
        float nmsThresh = 0.45;
        int batchSize = 1;
        int classNum = 9;
        int c = 3;
        int h = 416;
        int w = 416;
        int batchCount = 0;
        /* int outputCount; */
        /* unique_ptr<float[]> outputData; */
        std::unique_ptr<trtNet> net;
        vector<float> inputData;
        /* inputData.reserve(h*w*c*batchSize); */
        vector<cv::Mat> inputImgs;
        vector<float> prepareImage(cv::Mat& img);
        void DoNms(vector<Detection>& detections,int classes ,float nmsThresh);
        vector<Bbox> postProcessImg(cv::Mat& img,vector<Detection>& detections,int classes);
        vector<string> split(const string& str, char delim);
        bool loadTRTModel(string netname);
        std::list<vector<Bbox>> infer(vector<cv::Mat> image);
        vector<Bbox> infer_single_image(cv::Mat image);

};

#if 0
using namespace boost::python;

BOOST_PYTHON_MODULE(inference)
{
    /* std::list<vector<Bbox>>(inference::*in1)(vector<cv::Mat>) = &inference::infer; */
    /* std::list<vector<Bbox>>(inference::*in2)(cv::Mat) = &inference::infer; */

    class_<Inference, Inference*, boost::noncopyable>("Inference", boost::python::no_init)
        .def("doNMS", &Inference::DoNms)
        .def("postprocessimg", &Inference::postProcessImg)
        .def("prepareimg", &Inference::prepareImage)
        .def("split", &Inference::split)
        .def("load_trt", &Inference::loadTRTModel)
        .def("infer", &Inference::infer)
        .def("infer_single_image", &Inference::infer_single_image)
        ;

    /* class_<World>("World", init<std::string>()) */
    /*     .def("print", &World::print) */
    /*     ; */
};

/* vector<float> inference::prepareImage(cv::Mat& img); */
/* void inference::DoNms(vector<Detection>& detections,int classes ,float nmsThresh); */
/* vector<Bbox> inference::postProcessImg(cv::Mat& img,vector<Detection>& detections,int classes); */
/* vector<string> inference::split(const string& str, char delim); */
/* bool inference::loadTRTModel(string netname, int batchSize); */
/* list<vector<Bbox>> inference::infer(vector<cv2::Mat> image) */

/*template<typename T, typename... Args>
std::unique_ptr<T> make_unique(Args&&... args) {
    return std::unique_ptr<T>(new T(std::forward<Args>(args)...));
}*/
#endif


#include "fcl_model.h"
#include "urdf_utils.h"

#include <boost/property_tree/xml_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/filesystem/path.hpp>
#include <algorithm>

#define DEFINE_TEMPLATE_FM(DATATYPE) template class FCLModelTpl<DATATYPE>;

DEFINE_TEMPLATE_FM(double)

DEFINE_TEMPLATE_FM(float)

template<typename DATATYPE>
void FCLModelTpl<DATATYPE>::dfs_parse_tree(urdf::LinkConstSharedPtr const &link, std::string parent_link_name) {
    //const urdf::JointConstSharedPtr joint = urdf::const_pointer_cast<urdf::Joint>(link->parent_joint);
    //const Transform3 joint_placement = pose_to_transform<DATATYPE>(joint->parent_to_joint_origin_transform);

    if (link->collision) {
        for (auto geom: link->collision_array) {
            const std::string &geom_name = geom->name;
            auto geom_model = geom->geometry;
            CollisionGeometry_ptr collision_geometry = nullptr;
            auto pose = Transform3::Identity();
            if (geom_model->type == urdf::Geometry::MESH) {
                const urdf::MeshSharedPtr urdf_mesh = urdf::dynamic_pointer_cast<urdf::Mesh>(geom_model);
                std::string file_name = urdf_mesh->filename;
                if (use_convex)
                    file_name = file_name += ".convex.stl";
                auto mesh_path = (boost::filesystem::path(package_dir) / file_name).string();
                if (mesh_path == "") {
                    std::stringstream ss;
                    ss << "Mesh " << file_name << " could not be found.";
                    throw std::invalid_argument(ss.str());
                }
                if (verbose)
                    std::cout << "File name " << file_name << std::endl;
                Vector3 scale = {(DATATYPE) urdf_mesh->scale.x, (DATATYPE) urdf_mesh->scale.y,
                                 (DATATYPE) urdf_mesh->scale.z};
                if (use_convex)
                    collision_geometry = load_mesh_as_Convex(mesh_path, scale);
                else
                    collision_geometry = load_mesh_as_BVH(mesh_path, scale);
                if (verbose)
                    std::cout << scale << " " << collision_geometry << std::endl;
            } else if (geom_model->type == urdf::Geometry::CYLINDER) {
                const urdf::CylinderSharedPtr cylinder = urdf::dynamic_pointer_cast<urdf::Cylinder>(geom_model);
                collision_geometry = std::make_shared<Capsule>((DATATYPE) cylinder->radius,
                                                               (DATATYPE) cylinder->length);
            } else if (geom_model->type == urdf::Geometry::BOX) {
                const urdf::BoxSharedPtr box = urdf::dynamic_pointer_cast<urdf::Box>(geom_model);
                collision_geometry = std::make_shared<Box>((DATATYPE) box->dim.x, (DATATYPE) box->dim.y,
                                                           (DATATYPE) box->dim.z);
            } else if (geom_model->type == ::urdf::Geometry::SPHERE) {
                const urdf::SphereSharedPtr sphere = urdf::dynamic_pointer_cast<urdf::Sphere>(geom_model);
                collision_geometry = std::make_shared<Sphere>((DATATYPE) sphere->radius);
            } else throw std::invalid_argument("Unknown geometry type :");

            if (!collision_geometry)
                throw std::invalid_argument("The polyhedron retrived is empty");
            CollisionObject_ptr obj(new CollisionObject(collision_geometry, pose));

            collision_objects.push_back(obj);
            //collision_link_index.push_back(frame_id);
            collision_link_names.push_back(link->name);
            parent_link_names.push_back(parent_link_name);
            //collision_joint_index.push_back(model.frames[frame_id].parent);
            /// body_placement * convert_data((*i)->origin);
            collision_origin2link_poses.push_back(pose_to_transform<DATATYPE>(geom->origin));
            //collision_origin2joint_pose.push_back(
            //        model.frames[frame_id].placement * convertFromUrdf<DATATYPE>(geom->origin));

        }
    }
    for (auto child: link->child_links)
        dfs_parse_tree(child, link->name);
}

template<typename DATATYPE>
void FCLModelTpl<DATATYPE>::init(urdf::ModelInterfaceSharedPtr const &urdfTree, std::string const &package_dir_) {
    package_dir = package_dir_;
    urdf_model = urdfTree;
    if (not urdf_model)
        throw std::invalid_argument("The XML stream does not contain a valid URDF model.");
    urdf::LinkConstSharedPtr root_link = urdf_model->getRoot();
    dfs_parse_tree(root_link, "root's parent");
    auto tmp_user_link_names = collision_link_names;
    std::sort(tmp_user_link_names.begin(), tmp_user_link_names.end());
    auto last = std::unique(tmp_user_link_names.begin(), tmp_user_link_names.end());
    tmp_user_link_names.erase(last, tmp_user_link_names.end());
    setLinkOrder(tmp_user_link_names);

    for (size_t i = 0; i < collision_link_names.size(); i++)
        for (size_t j = 0; j < i; j++)
            if (collision_link_names[i] != collision_link_names[j]
                && parent_link_names[i] != collision_link_names[j] && parent_link_names[j] != collision_link_names[i]) {
                // We assume that the collisions between objects append to the same joint can be ignored.
                collision_pairs.push_back(std::make_pair(j, i));
                /*if (verbose)
                    std::cout << collision_link_name[j] << " " << collision_link_name[i] << std::endl;*/
            }
}

template<typename DATATYPE>
FCLModelTpl<DATATYPE>::FCLModelTpl(urdf::ModelInterfaceSharedPtr const &urdfTree, std::string const &package_dir,
                                   bool const &verbose, bool const &convex) : verbose(verbose), use_convex(convex) {
    init(urdfTree, package_dir);
}

template<typename DATATYPE>
FCLModelTpl<DATATYPE>::FCLModelTpl(std::string const &urdf_filename, bool const &verbose, bool const &convex)
        : verbose(verbose), use_convex(convex) {
    //std::cout << "Verbose" << verbose << std::endl;
    auto found = urdf_filename.find_last_of("/\\");
    auto urdf_dir = urdf_filename.substr(0, found);
    urdf::ModelInterfaceSharedPtr urdfTree = urdf::parseURDFFile(urdf_filename);
    init(urdfTree, urdf_dir);
}

template<typename DATATYPE>
void
FCLModelTpl<DATATYPE>::setLinkOrder(const std::vector<std::string> &names) {
    user_link_names = names;
    collision_link_user_indices = {};
    for (size_t i = 0; i < collision_link_names.size(); i++) {
        if (verbose)
            std::cout << collision_link_names[i] << " " << names[i] << std::endl;
        auto iter = std::find(names.begin(), names.end(), collision_link_names[i]);
        if (iter == names.end())
            throw std::invalid_argument("The names does not contain link " + collision_link_names[i]);
        size_t link_i = iter - names.begin();
        collision_link_user_indices.push_back(link_i);
    }
}

template<typename DATATYPE>
void FCLModelTpl<DATATYPE>::removeCollisionPairsFromSrdf(std::string const &srdf_filename) {
    const std::string extension = srdf_filename.substr(srdf_filename.find_last_of('.') + 1);
    if (srdf_filename == "") {
        std::cout << "No SRDF file provided!" << std::endl;
        return ;
    }

    ASSERT(extension == "srdf", srdf_filename + " does not have the right extension.");

    std::ifstream srdf_stream(srdf_filename.c_str());

    ASSERT(srdf_stream.is_open(), "Cannot open " + srdf_filename);

    boost::property_tree::ptree pt;
    boost::property_tree::xml_parser::read_xml(srdf_stream, pt);

    for (auto node: pt.get_child("robot")) {
        if (node.first == "disable_collisions") {
            const std::string link1 = node.second.get<std::string>("<xmlattr>.link1");
            const std::string link2 = node.second.get<std::string>("<xmlattr>.link2");
            /*
            // Check first if the two bodies exist in model
            if (!model.existBodyName(link1) || !model.existBodyName(link2)) {
                if (verbose)
                    std::cout << "It seems that " << link1 << " or " << link2 <<
                        " do not exist in model. Skip." << std::endl;
                continue;
            }

            FrameIndex frame_id1 = model.getBodyId(link1);
            FrameIndex frame_id2 = model.getBodyId(link2);

            if ((frame_id1 == model.nframes || frame_id2 == model.nframes) && logging_level > 90) {
                std::cout << "Links do not exist " << link1 << " " << link2 << std::endl;
                continue;
            }
            // Malformed SRDF
            if (frame_id1 == frame_id2) {
                if (verbose)
                    std::cout << "Cannot disable collision between " << link1 << " and " << link2 << std::endl;
                continue;
            } else if (frame_id1 > frame_id2)
                std::swap(frame_id1, frame_id2);
            */
            if (verbose) {
                std::cout << "Try to Remove collision parts:" << link1 << " " << link2 << std::endl;
            }
            for (auto iter = collision_pairs.begin(); iter != collision_pairs.end();) {
                if (collision_link_names[iter->first] == link1 && collision_link_names[iter->second] == link2 ||
                    collision_link_names[iter->first] == link2 && collision_link_names[iter->second] == link1) {
                    iter = collision_pairs.erase(iter);
                } else
                    iter++;

            }
        }
    }
}

template<typename DATATYPE>
bool FCLModelTpl<DATATYPE>::collide(CollisionRequest const& request) {
    // result will be returned via the collision result structure
    CollisionResult result;
    for (auto col_pair: collision_pairs) {
        fcl::collide(collision_objects[col_pair.first].get(), collision_objects[col_pair.second].get(), request,
                     result);
        if (result.isCollision())
            return true;
    }
    return false;
}

template<typename DATATYPE>
std::vector<fcl::CollisionResult<DATATYPE>> FCLModelTpl<DATATYPE>::collideFull(CollisionRequest const& request) {
    //CollisionRequest request(1, false, 1, false, true, fcl::GJKSolverType::GST_INDEP, 1e-6);
    // result will be returned via the collision result structure
    std::vector<CollisionResult> ret;
    //double cnt = 0;
    //std::cout << collision_pairs.size() << std::endl;
    for (auto col_pair: collision_pairs) {
        CollisionResult result;
        result.clear();

        //auto trans = collision_objects[col_pair.first].get()->getTranslation();
        //cnt += trans[0] + trans[1] + trans[2];
        //std::cout << result.numContacts() << std::endl;

        //std::cout << col_pair.first << ' ' << col_pair.second << std::endl;
        //std::cout << trans << std::endl << std::endl;
        //std::cout << collision_objects[col_pair.first].get()->getTranslation() << std::endl;
        //std::cout << collision_objects[col_pair.second].get()->getTranslation() << std::endl;
        fcl::collide(collision_objects[col_pair.first].get(), collision_objects[col_pair.second].get(), request, result);
        /*if (result.isCollision()) {
            std::vector<Contact> contacts;
            result.getContacts(contacts);
            std::cout << "num: " << contacts.size() << std::endl;
            for (auto contact: contacts)
                std::cout << contact.penetration_depth << " " << contact.pos[0] << " " << contact.pos[1] << " " << contact.pos[2] << std::endl;
            std::cout<< std::endl;
        }*/
        ret.push_back(result);
    }
    //std::cout << cnt << "?" << std::endl;
    return ret;
}

template<typename DATATYPE>
void FCLModelTpl<DATATYPE>::updateCollisionObjects(std::vector<Transform3> const &link_pose) {
    for (size_t i = 0; i < collision_objects.size(); i++) {
        auto link_i = collision_link_user_indices[i];
        Transform3 t_i = link_pose[link_i] * collision_origin2link_poses[i];
        collision_objects[i].get()->setTransform(t_i);
        //auto tmp1 = collision_objects[i].get()->getTranslation();
        //std::cout << collision_objects[i].get()->getTranslation() << std::endl;
    }
}

template<typename DATATYPE>
void FCLModelTpl<DATATYPE>::updateCollisionObjects(std::vector<Vector7> const &link_pose) {
    for (size_t i = 0; i < collision_objects.size(); i++) {
        auto link_i = collision_link_user_indices[i];
        Transform3 tt_i;
        tt_i.linear() = Quaternion(link_pose[link_i][3], link_pose[link_i][4],
                                   link_pose[link_i][5], link_pose[link_i][6]).matrix();
        tt_i.translation() = link_pose[link_i].head(3);
        Transform3 t_i = tt_i * collision_origin2link_poses[i];
        collision_objects[i].get()->setTransform(t_i);
        //auto tmp1 = collision_objects[i].get()->getTranslation();
        //auto tmp2 = collision_objects[i].get()->getRotation();
        //Transform3 tmp = collision_objects[i]->getTransform();
        //std::cout << collision_objects[i].get()->getTranslation() << std::endl;
    }
}

template<typename DATATYPE>
void FCLModelTpl<DATATYPE>::printCollisionPairs(void) {
    for (auto cp: collision_pairs) {
        auto i = cp.first, j = cp.second;
        std::cout << collision_link_names[i] << " " << collision_link_names[j] << std::endl;
    }
}


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

#include <boost/simd/sdk/simd/pack/forward.hpp>
#include <boost/dispatch/meta/downgrade.hpp>
#include <boost/dispatch/meta/primitive_of.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/utility/enable_if.hpp>

namespace boost { namespace dispatch { namespace details
{
    template<class T>
    struct downgrade_is_smallest
         : boost::mpl::bool_< sizeof(T) == 1 >
    {
    };

    template<>
    struct downgrade_is_smallest<float>
         : boost::mpl::true_
    {
    };
} } }

namespace boost { namespace dispatch { namespace ext
{
    template< class Type
            , std::size_t Cardinal
            , class Sign
            >
    struct downgrade< boost::simd::pack<Type, Cardinal>, Sign, typename boost::disable_if< details::downgrade_is_smallest<typename meta::primitive_of<Type>::type> >::type >
    {
        typedef typename downgrade<Type, Sign>::type dT;
        typedef boost::simd::pack<dT, Cardinal*2> type;
    };
} } }

#endif


#include <boost/type_traits/is_arithmetic.hpp>