#include <iostream>
#include <thread>
#include <chrono>
#include <iostream>
#include <string>
#include <boost/program_options.hpp>
#include "mqtt/async_client.h"

using namespace std;

const string SERVER_ADDRESS { "tcp://localhost:1883" };
const string CLIENT_ID      { "async_consume" };
const string TOPIC          { "adc/1" };

const int  QOS = 1;

class pid_controller {
    public:
    double kp,ki,kd,intcap,outcap_pos,outcap_neg,setpoint;
    double iterm, pterm, dterm;
    double last_error;

    pid_controller(){
        kp = 1;
        ki = 0;
        kd = 0;
        intcap = 0;
        outcap_pos = 1.0;
        outcap_neg = 0;
        setpoint = 1;
        iterm = 0;
        last_error = 0;
    }

    double operator()(double input){
        double rval;
        double error = input - setpoint;
        pterm = -error * kp;
        iterm += error * ki;
        dterm = kd * (last_error - error);
        last_error = error;
        if(iterm > intcap)
            iterm = intcap;
        if(-iterm > intcap)
            iterm = -intcap;
        rval = pterm + iterm + dterm;
        if(rval > outcap_pos)
            rval = outcap_pos;
        if(rval < outcap_neg)
            rval = outcap_neg;
        return rval;
    }
};

/////////////////////////////////////////////////////////////////////////////
int main(int ac, char **av)
{
    namespace po = boost::program_options;
    po::options_description desc("Allowed options");
    desc.add_options()
        ("help", "produce help message")
        ("server",  po::value<std::string>()->default_value("localhost"), "mqtt server")
        ("port",    po::value<int>()->default_value(1883),"mqtt port")

        ("pwmtopic",   po::value<std::string>()->default_value("pwm1"), "mqtt publish topic for pwm output")
        ("pwmtopictopic",   po::value<std::string>()->default_value("pid/pwmtopic"), "mqtt subscribe topic to dynamically change pwm output topic")

        ("sensor", po::value<std::string>()->default_value("adc/1"), "mqtt default subscribe topic for temperature sensor")
        ("sensortopic", po::value<std::string>()->default_value("sensortopic"), "mqtt (sensor topic) (subscribe topic) for temperature sensor e.g. dynamic control of temperature source data")
        ("setpointtopic", po::value<std::string>()->default_value("pid/set"), "mqtt setpoint topic")

        ("topic_kp", po::value<std::string>()->default_value("pid/kp"), "mqtt subscribe topic for online kp updates")
        ("topic_ki", po::value<std::string>()->default_value("pid/ki"), "mqtt subscribe topic for online ki updates")
        ("topic_kd", po::value<std::string>()->default_value("pid/kd"), "mqtt subscribe topic for online kd updates")

        ("td",  po::value<int>()->default_value(500), "time between iterations (integer milliseconds)")
        ("kp",   po::value<double>()->default_value(0.01) , "kp")
        ("ki",   po::value<double>()->default_value(-.0001), "ki")
        ("kd",   po::value<double>()->default_value(0.1) , "kd")
        ("intcap",   po::value<double>()->default_value(0.1) , "integral term cap (+-)")

        ("verbose", "report to stdout")
        ("mqtt_log", "send debug log to mqtt")
        ("csv", "csv readout to stdout")

    ;

    po::variables_map vm;
    po::store(po::parse_command_line(ac, av, desc), vm);
    po::notify(vm);

    if (vm.count("help")) {
        std::cout << desc << "\n";
        return 1;
    }

    bool verbose=false;
    if (vm.count("verbose")){
        std::cout << "verbose set\n";
        verbose = true;
    }

    bool mqtt_log=false;
    if (vm.count("mqtt_log")){
        std::cout << "mqtt_log set\n";
        mqtt_log = true;
    }

    bool csv=false;
    if (vm.count("verbose")){
        std::cout << "verbose set\n";
        verbose = true;
    }

    int port = vm["port"].as<int>();
    std::string server = vm["server"].as<std::string>();

    int td = vm["td"].as<int>();
    double kp = vm["kp"].as<double>();
    double ki = vm["ki"].as<double>();
    double kd = vm["kd"].as<double>();
    double intcap = vm["intcap"].as<double>();

    std::string pwmtopic = vm["pwmtopic"].as<std::string>();

    std::string sensortopic = vm["sensor"].as<std::string>();
    std::string sensortopictopic = vm["sensortopic"].as<std::string>();
    std::string pwmtopictopic = vm["pwmtopictopic"].as<std::string>();
    std::string setpointtopic = vm["setpointtopic"].as<std::string>();
    std::string topic_kp = vm["topic_kp"].as<std::string>();
    std::string topic_ki = vm["topic_ki"].as<std::string>();
    std::string topic_kd = vm["topic_kd"].as<std::string>();

    std::cout << "done parsing command line\n" << std::flush;
    std::cout << "\ntp= " << kp;
    std::cout << "\nti= " << ki;
    std::cout << "\ntd= " << kd;
    std::cout << "\nintcap= " << intcap;
    std::cout << "\npwmtopic= " << pwmtopic;
    std::cout << "\nsetpointtopic= " << setpointtopic;
    std::cout << "\nsensortopic= " << sensortopic;
    std::cout << "\nsensortopictopic= " << sensortopictopic;
    std::cout << "\npwmtopictopic= " << pwmtopictopic;
    std::cout << "\ntopic_kp= " << topic_kp;
    std::cout << "\ntopic_ki= " << topic_ki;
    std::cout << "\ntopic_kd= " << topic_kd;

    pid_controller pid{};
    pid.kp = kp;
    pid.ki = ki;
    pid.kd = kd;
    pid.intcap = intcap;
    pid.outcap_neg = 0;
    pid.outcap_pos = 1.0;

    // mqtt
    const int    QOS = 1;
    const auto PERIOD = std::chrono::seconds(5);
    const int MAX_BUFFERED_MSGS = 120;  // 120 * 5sec => 10min off-line buffering
    const std::string PERSIST_DIR { "data-persist" };

    std::string address = std::string("tcp://") + server + std::string(":") + std::to_string(port);
    mqtt::async_client cli(address, "", MAX_BUFFERED_MSGS, PERSIST_DIR);

    mqtt::connect_options connOpts;
    connOpts.set_keep_alive_interval(MAX_BUFFERED_MSGS * PERIOD);
    connOpts.set_clean_session(true);
    connOpts.set_automatic_reconnect(true);

    try {
        std::cout << "Connecting to server '" << address << "'..." << std::flush;
        cli.connect(connOpts)->wait();
        cli.start_consuming();
        std::cout << "OK\n" << std::endl;
    }
    catch (const mqtt::exception& exc) {
        cerr << exc.what() << endl;
        return 1;
    }

    // mqtt publish topics
    shared_ptr<mqtt::topic> pwm;
    pwm = make_shared<mqtt::topic>(cli, pwmtopic, QOS, true);

    // mqtt_log
    shared_ptr<mqtt::topic> mqtt_log_sender;
    mqtt_log_sender = make_shared<mqtt::topic>(cli, "log/pwm", QOS, true);

    // mqtt consume topics
    cli.start_consuming();
    std::cout << "Subscribing to topic[s]\n";
    cli.subscribe(sensortopic, QOS)->wait();
    cli.subscribe(setpointtopic, QOS)->wait();
    cli.subscribe(sensortopictopic, QOS)->wait();
    cli.subscribe(pwmtopictopic, QOS)->wait();
    cli.subscribe(topic_kp, QOS)->wait();
    cli.subscribe(topic_ki, QOS)->wait();
    cli.subscribe(topic_kd, QOS)->wait();

    mqtt::const_message_ptr mqtt_msg;

    // Consume messages

    while (true) {
        double input_val = 0;
        double outval = 0;
        while(cli.try_consume_message(&mqtt_msg)){
            std::string topic = mqtt_msg->get_topic();
            std::string payload = mqtt_msg->to_string();
            //std::cout << "\n" << topic << ": " << payload;
            if(topic == sensortopic){
                input_val = std::stod(payload);
                continue;
            }
            if(topic == sensortopictopic){
                std::cout << "\nSwitching to sensor topic: " << payload;
                cli.unsubscribe(sensortopic);
                sensortopic = payload;
                cli.subscribe(sensortopic, QOS)->wait();
                continue;
            }
            if(topic == pwmtopictopic){
                std::cout << "\nSwitching to PWM output topic: " << payload;
                pwm = make_shared<mqtt::topic>(cli, payload, QOS, true);
                continue;
            }
            if(topic == topic_kp){
                std::cout << "\nReceived kp update: " << payload;
                pid.kp = std::stod(payload);
                continue;
            }
            if(topic == setpointtopic){
                std::cout << "\nReceived setpointtopic update: " << payload;
                pid.setpoint = std::stod(payload);
                continue;
            }
            if(topic == topic_ki){
                std::cout << "\nReceived ki update: " << payload;
                pid.ki = std::stod(payload);
                pid.iterm = 0;
                continue;
            }
            if(topic == topic_kd){
                std::cout << "\nReceived kd update: " << payload;
                pid.kd = std::stod(payload);
                continue;
            }
        }
        outval = pid(input_val);
        pwm->publish(std::to_string(outval));
        mqtt_log_sender->publish(std::string("pid.pterm ") + std::to_string(pid.pterm));
        mqtt_log_sender->publish(std::string("pid.iterm ") + std::to_string(pid.iterm));
        mqtt_log_sender->publish(std::string("pid.dterm ") + std::to_string(pid.dterm));
        //
        this_thread::sleep_for(chrono::milliseconds(td));
    }

    // Disconnect

    cout << "\nShutting down and disconnecting from the MQTT server..." << flush;
    cli.unsubscribe(TOPIC)->wait();
    cli.stop_consuming();
    cli.disconnect()->wait();
    cout << "OK" << endl;

    return 0;
}


#ifndef GRAPH_HPP
#define GRAPH_HPP

#include "graph_int.hpp"

#include "units.hpp"

#include <list>
#include <map>
#include <set>
#include <string>
#include <tuple>
#include <utility>
#include <vector>

#include <boost/graph/adjacency_list.hpp>

namespace boost {
  // Describes the set of available units on an edge.
  enum edge_su_t {edge_su};

  BOOST_INSTALL_PROPERTY(edge, su);

  // Describes the number of units on an edge.
  enum edge_nou_t {edge_nou};

  BOOST_INSTALL_PROPERTY(edge, nou);
}

// The type of cost of reaching a vertex.
typedef double COST;

/**
 * The type of the graph we use.  The edge_su_t property describes the
 * units available, and not already taken.
 */
typedef
boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
		      boost::property<boost::vertex_name_t,
                                      std::string>,
                      boost::property<boost::edge_weight_t, COST,
                      boost::property<boost::edge_nou_t, unsigned,
                      boost::property<boost::edge_su_t, SU> > > >
graph;

typedef graph::edge_descriptor edge;
typedef graph::vertex_descriptor vertex;

// The path.
typedef std::list<edge> path;

// The list of paths.
typedef std::list<path> plist;

// The CU path.
typedef std::pair<CU, path> cupath;

// The CU path pair.
typedef std::pair<cupath, cupath> cupp;

// Node pair.
typedef std::pair<vertex, vertex> npair;

// The demand type: npair and the number of contiguous units (ncu).
typedef std::pair<npair, int> demand;

template <typename Graph>
using Vertex = typename Graph::vertex_descriptor;

template <typename Graph>
using Edge = typename Graph::edge_descriptor;

#endif /* GRAPH_HPP */


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

#define BOOST_TEST_MODULE floating_point

#define BOOST_SPIRIT_NO_PREDEFINED_TERMINALS

#include <boost/coerce.hpp>
#include <boost/limits.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/mpl/for_each.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include <string>

struct source_test {
    template <typename T>
    void
    operator()(T) {
        using namespace boost;
        BOOST_CHECK_EQUAL(coerce::as<std::string>(static_cast<T>(0)), "0.0");

        BOOST_CHECK_EQUAL(coerce::as<std::string>(static_cast<T>(23)), "23.0");
        BOOST_CHECK_EQUAL(
            coerce::as<std::string>(static_cast<T>(-23)), "-23.0");

        BOOST_CHECK_EQUAL(
            coerce::as<std::string>(static_cast<T>(1.0182818L)), "1.0182818");

        BOOST_CHECK_EQUAL(
            coerce::as<std::string>(static_cast<T>(1.0e18L)), "1.0e18");
        BOOST_CHECK_EQUAL(
            coerce::as<std::string>(static_cast<T>(1.0e-18L)), "1.0e-18");

        BOOST_CHECK_EQUAL(
            coerce::as<std::string>(std::numeric_limits<T>::quiet_NaN()),
            "nan");
        BOOST_CHECK_EQUAL(
            coerce::as<std::string>(-std::numeric_limits<T>::quiet_NaN()),
            "-nan");

        BOOST_CHECK_EQUAL(
            coerce::as<std::string>(std::numeric_limits<T>::infinity()), "inf");
        BOOST_CHECK_EQUAL(
            coerce::as<std::string>(-std::numeric_limits<T>::infinity()),
            "-inf");
    }
};

BOOST_AUTO_TEST_CASE(source) {
    source_test test;

    typedef boost::mpl::vector<
        float
      , double
      , long double
    > integral_types;

    boost::mpl::for_each<integral_types>(test);
}

struct target_test {
    template <typename T>
    void
    operator()(T) {
        using namespace boost;

        BOOST_CHECK_EQUAL(coerce::as<T>(".0"), 0);
        BOOST_CHECK_EQUAL(coerce::as<T>("0."), 0);
        BOOST_CHECK_EQUAL(coerce::as<T>("0.0"), 0);

        BOOST_CHECK_EQUAL(coerce::as<T>("23.0"), 23);
        BOOST_CHECK_EQUAL(coerce::as<T>("+23.0"), 23);
        BOOST_CHECK_EQUAL(coerce::as<T>("-23.0"), -23);

        BOOST_CHECK_CLOSE_FRACTION(
            coerce::as<T>("1.0182818"),
            static_cast<T>(1.0182818L),
            std::numeric_limits<T>::epsilon());

        BOOST_CHECK_CLOSE_FRACTION(
            coerce::as<T>("1.0e18"),
            static_cast<T>(1.0e18L),
            std::numeric_limits<T>::epsilon());
        BOOST_CHECK_CLOSE_FRACTION(
            coerce::as<T>("1.0E18"),
            static_cast<T>(1.0e18L),
            std::numeric_limits<T>::epsilon());
        BOOST_CHECK_CLOSE_FRACTION(
            coerce::as<T>("1.0e+18"),
            static_cast<T>(1.0e18L),
            std::numeric_limits<T>::epsilon());
        BOOST_CHECK_CLOSE_FRACTION(
            coerce::as<T>("1.0e-18"),
            static_cast<T>(1.0e-18L),
            std::numeric_limits<T>::epsilon());

        BOOST_CHECK_EQUAL(math::fpclassify(coerce::as<T>("NaN")), FP_NAN);
        BOOST_CHECK_EQUAL(math::fpclassify(coerce::as<T>("-NaN")), FP_NAN);

        BOOST_CHECK_EQUAL(math::fpclassify(coerce::as<T>("Inf")), FP_INFINITE);
        BOOST_CHECK_EQUAL(
            math::fpclassify(coerce::as<T>("Infinity")), FP_INFINITE);
        BOOST_CHECK_EQUAL(
            math::fpclassify(coerce::as<T>("-Inf")), FP_INFINITE);
    }
};

BOOST_AUTO_TEST_CASE(target) {
    target_test test;

    typedef boost::mpl::vector<
        float
      , double
      , long double
    > integral_types;

    boost::mpl::for_each<integral_types>(test);
}


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

/* Author: Ioan Sucan */

#include "ompl/util/RandomNumbers.h"
#include "ompl/util/Exception.h"
#include "ompl/util/Console.h"
#include <boost/random/lagged_fibonacci.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/thread/once.hpp>
#include <boost/scoped_ptr.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/math/constants/constants.hpp>

/// @cond IGNORE
namespace
{
    /// We use a different random number generator for the seeds of the
    /// other random generators. The root seed is from the number of
    /// nano-seconds in the current time, or given by the user.
    class RNGSeedGenerator
    {
    public:
        RNGSeedGenerator() :
            someSeedsGenerated_(false),
            firstSeed_((boost::uint32_t)(boost::posix_time::microsec_clock::universal_time() -
                boost::posix_time::ptime(boost::date_time::min_date_time)).total_microseconds()),
            sGen_(firstSeed_),
            sDist_(1, 1000000000),
            s_(sGen_, sDist_)
        {
        }

        boost::uint32_t firstSeed()
        {
            boost::mutex::scoped_lock slock(rngMutex_);
            return firstSeed_;
        }

        void setSeed(boost::uint32_t seed)
        {
            boost::mutex::scoped_lock slock(rngMutex_);
            if (seed > 0)
            {
                if (someSeedsGenerated_)
                {
                    OMPL_ERROR("Random number generation already started. Changing seed now will not lead to deterministic sampling.");
                }
                else
                {
                    // In this case, since no seeds have been generated yet, so we remember this seed as the first one.
                    firstSeed_ = seed;
                }
            }
            else
            {
                if (someSeedsGenerated_)
                {
                    OMPL_WARN("Random generator seed cannot be 0. Ignoring seed.");
                    return;
                }
                else
                {
                    OMPL_WARN("Random generator seed cannot be 0. Using 1 instead.");
                    seed = 1;
                }
            }
            sGen_.seed(seed);
        }

        boost::uint32_t nextSeed()
        {
            boost::mutex::scoped_lock slock(rngMutex_);
            someSeedsGenerated_ = true;
            return s_();
        }

    private:
        bool                       someSeedsGenerated_;
        boost::uint32_t            firstSeed_;
        boost::mutex               rngMutex_;
        boost::lagged_fibonacci607 sGen_;
        boost::uniform_int<>       sDist_;
        boost::variate_generator<boost::lagged_fibonacci607&, boost::uniform_int<> > s_;
    };

    static boost::once_flag g_once = BOOST_ONCE_INIT;
    static boost::scoped_ptr<RNGSeedGenerator> g_RNGSeedGenerator;

    void initRNGSeedGenerator()
    {
        g_RNGSeedGenerator.reset(new RNGSeedGenerator());
    }

    RNGSeedGenerator& getRNGSeedGenerator()
    {
        boost::call_once(&initRNGSeedGenerator, g_once);
        return *g_RNGSeedGenerator;
    }
}  // namespace
/// @endcond

boost::uint32_t ompl::RNG::getSeed()
{
    return getRNGSeedGenerator().firstSeed();
}

void ompl::RNG::setSeed(boost::uint32_t seed)
{
    getRNGSeedGenerator().setSeed(seed);
}

ompl::RNG::RNG() :
    generator_(getRNGSeedGenerator().nextSeed()),
    uniDist_(0, 1),
    normalDist_(0, 1),
    uni_(generator_, uniDist_),
    normal_(generator_, normalDist_)
{
}

double ompl::RNG::halfNormalReal(double r_min, double r_max, double focus)
{
    assert(r_min <= r_max);

    const double mean = r_max - r_min;
    double       v    = gaussian(mean, mean/focus);

    if (v > mean) v = 2.0 * mean - v;
    double r = v >= 0.0 ? v + r_min : r_min;
    return r > r_max ? r_max : r;
}

int ompl::RNG::halfNormalInt(int r_min, int r_max, double focus)
{
    int r = (int)floor(halfNormalReal((double)r_min, (double)(r_max) + 1.0, focus));
    return (r > r_max) ? r_max : r;
}

// From: "Uniform Random Rotations", Ken Shoemake, Graphics Gems III,
//       pg. 124-132
void ompl::RNG::quaternion(double value[4])
{
    double x0 = uni_();
    double r1 = sqrt(1.0 - x0), r2 = sqrt(x0);
    double t1 = 2.0 * boost::math::constants::pi<double>() * uni_(), t2 = 2.0 * boost::math::constants::pi<double>() * uni_();
    double c1 = cos(t1), s1 = sin(t1);
    double c2 = cos(t2), s2 = sin(t2);
    value[0] = s1 * r1;
    value[1] = c1 * r1;
    value[2] = s2 * r2;
    value[3] = c2 * r2;
}

// From Effective Sampling and Distance Metrics for 3D Rigid Body Path Planning, by James Kuffner, ICRA 2004
void ompl::RNG::eulerRPY(double value[3])
{
    value[0] = boost::math::constants::pi<double>() * (-2.0 * uni_() + 1.0);
    value[1] = acos(1.0 - 2.0 * uni_()) - boost::math::constants::pi<double>() / 2.0;
    value[2] = boost::math::constants::pi<double>() * (-2.0 * uni_() + 1.0);
}


#pragma once

#include <Eigen/Core>

#include <ipc/broad_phase/collision_candidate.hpp>
#include <ipc/broad_phase/hash_grid.hpp>

#include <ccd/ccd.hpp>
#include <interval/interval.hpp>
#include <physics/rigid_body_assembler.hpp>

namespace ipc::rigid {

inline void
sort_body_pair(const RigidBodyAssembler& bodies, int& bodyA_id, int& bodyB_id)
{
    if (bodies[bodyA_id].bvh_size() > bodies[bodyB_id].bvh_size()) {
        std::swap(bodyA_id, bodyB_id);
    }
}

inline AABB vertex_aabb(const VectorMax3d& v, double inflation_radius = 0)
{
    return AABB(v.array() - inflation_radius, v.array() + inflation_radius);
}

inline AABB vertex_aabb(const VectorMax3I& v, double inflation_radius = 0)
{
    assert(v.rows() == 1 || v.cols() == 1);
    VectorMax3d min(v.size());
    VectorMax3d max(v.size());
    for (int i = 0; i < v.size(); i++) {
        if (empty(v(i))) {
            throw "interval is empty";
        }
        min(i) = (v(i) - inflation_radius).lower();
        max(i) = (v(i) + inflation_radius).upper();
    }
    return AABB(min, max);
}

template <typename T>
inline std::vector<AABB>
vertex_aabbs(const MatrixX<T>& V, double inflation_radius = 0)
{
    std::vector<AABB> aabbs;
    aabbs.reserve(V.rows());
    for (size_t i = 0; i < V.rows(); i++) {
        aabbs.push_back(vertex_aabb(VectorMax3<T>(V.row(i)), inflation_radius));
    }
    return aabbs;
}

void detect_body_pair_collision_candidates_from_aabbs(
    const RigidBodyAssembler& bodies,
    const std::vector<AABB>& bodyA_vertex_aabbs,
    const int bodyA_id,
    const int bodyB_id,
    const int collision_types,
    Candidates& candidates,
    const double inflation_radius = 0.0);

template <typename T>
inline void detect_body_pair_collision_candidates_bvh(
    const RigidBodyAssembler& bodies,
    const Poses<T>& poses,
    int bodyA_id,
    int bodyB_id,
    const int collision_types,
    Candidates& candidates,
    const double inflation_radius = 0.0)
{
    sort_body_pair(bodies, bodyA_id, bodyB_id);

    // Compute the smaller body's vertices in the larger body's local
    // coordinates.
    // PROFILE_POINT(fmt::format(
    //     "detect_body_pair_collision_candidates_bvh<{}>:compute_vertices",
    //     get_type_name<T>()));
    // PROFILE_START();
    const auto RA = poses[bodyA_id].construct_rotation_matrix();
    const auto RB = poses[bodyB_id].construct_rotation_matrix();
    const auto& pA = poses[bodyA_id].position;
    const auto& pB = poses[bodyB_id].position;
    const MatrixX<T> VA =
        ((bodies[bodyA_id].vertices * RA.transpose()).rowwise()
         + (pA - pB).transpose())
        * RB;
    // PROFILE_END();

    detect_body_pair_collision_candidates_from_aabbs(
        bodies, vertex_aabbs(VA, inflation_radius), bodyA_id, bodyB_id,
        collision_types, candidates, inflation_radius);
}

void detect_body_pair_intersection_candidates_from_aabbs(
    const RigidBodyAssembler& bodies,
    const std::vector<AABB>& bodyA_vertex_aabbs,
    const int bodyA_id,
    const int bodyB_id,
    std::vector<EdgeFaceCandidate>& candidates,
    const double inflation_radius = 0.0);

} // namespace ipc::rigid


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

/**
 * @file lexicographic.hpp
 *
 * @brief Find lexicographically smallest and largest solutions
 *
 * Based on Knuth TAOCP Exercise 7.2.2.2-109.
 *
 * @author Mathias Soeken
 * @since  2.3
 */

#ifndef LEXICOGRAPHIC_SAT_HPP
#define LEXICOGRAPHIC_SAT_HPP

#include <exception>
#include <iostream>
#include <vector>

#include <boost/dynamic_bitset.hpp>

#include <core/utils/range_utils.hpp>
#include <classical/sat/sat_solver.hpp>
#include <classical/sat/utils/visit_solutions.hpp>

namespace cirkit
{

class unsat_exception : public std::exception
{
  virtual const char * what() const throw()
  {
    return "Solution is UNSAT";
  }
};

template <typename Solver>
boost::dynamic_bitset<>
lexicographic_solution( Solver& solver, const std::vector<int>& vars,
                        const std::vector<int>& assumptions, bool smallest,
                        const properties::ptr& settings,
                        const properties::ptr& statistics )
{
  auto runtime   = 0.0;
  auto sat_calls = 0u;
  auto clauses   = 0u;

  /* F1. [Initialize.] */
  solver_execution_statistics stats;
  auto result = solve( solver, stats, assumptions );
  runtime += stats.runtime;
  clauses = stats.num_clauses;
  ++sat_calls;

  if ( result == boost::none )
  {
    throw unsat_exception();
  }

  auto ys = extract_solution( result, vars );

  auto d_iter = std::string::npos;

  while ( true )
  {
    /* F2. [Advance d.] */
    std::string s;
    to_string( ys, s );
    if ( smallest )
    {
      d_iter = s.find( "1", d_iter + 1 );
    }
    else
    {
      d_iter = s.find( "0", d_iter + 1 );
    }

    /* F3. [Done?] */
    if ( d_iter == std::string::npos )
    {
      set( statistics, "runtime", runtime );
      set( statistics, "sat_calls", sat_calls );
      set( statistics, "num_clauses", clauses );
      return ys;
    }

    unsigned d = s.size() - d_iter - 1;

    /* F4. [Try for smaller/larger.] */
    auto local_assumptions = assumptions;
    for ( auto j = vars.size() - 1; j > d; --j )
    {
      local_assumptions.push_back( ys[j] ? vars[j] : -vars[j] );
    }
    local_assumptions.push_back( smallest ? -vars[d] : vars[d] );

    result = solve( solver, stats, local_assumptions );
    runtime += stats.runtime;
    ++sat_calls;
    if ( result != boost::none )
    {
      ys = extract_solution( result, vars );
    }
  }
}

template<typename Solver>
boost::dynamic_bitset<> lexicographic_smallest_solution( Solver& solver, const std::vector<int>& vars,
                                                         const std::vector<int>& assumptions = std::vector<int>(),
                                                         const properties::ptr& settings = properties::ptr(),
                                                         const properties::ptr& statistics = properties::ptr() )
{
  return lexicographic_solution( solver, vars, assumptions, true, settings, statistics );
}

template<typename Solver>
boost::dynamic_bitset<> lexicographic_largest_solution( Solver& solver, const std::vector<int>& vars,
                                                        const std::vector<int>& assumptions = std::vector<int>(),
                                                        const properties::ptr& settings = properties::ptr(),
                                                        const properties::ptr& statistics = properties::ptr() )
{
  return lexicographic_solution( solver, vars, assumptions, false, settings, statistics );
}

}

#endif

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2015 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.

// This file was modified by Oracle on 2017-2021.
// Modifications copyright (c) 2017-2021 Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_TEST_DISJOINT_HPP
#define BOOST_GEOMETRY_TEST_DISJOINT_HPP

#include <iostream>
#include <string>
#include <boost/variant/variant.hpp>

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/disjoint.hpp>
#include <boost/geometry/geometries/geometry_collection.hpp>
#include <boost/geometry/io/wkt/read.hpp>

template <typename Geometry1, typename Geometry2, typename Strategy>
bool call_disjoint(Geometry1 const& geometry1,
                   Geometry2 const& geometry2,
                   Strategy const& strategy)
{
    return bg::disjoint(geometry1, geometry2, strategy);
}

template <typename Geometry1, typename Geometry2>
bool call_disjoint(Geometry1 const& geometry1,
                   Geometry2 const& geometry2,
                   bg::default_strategy const&)
{
    return bg::disjoint(geometry1, geometry2);
}

template <typename G1, typename G2, typename Strategy>
void check_disjoint(std::string const& id,
                    std::string const& wkt1,
                    std::string const& wkt2,
                    G1 const& g1,
                    G2 const& g2,
                    bool expected,
                    Strategy const& strategy)
{
    bool detected = call_disjoint(g1, g2, strategy);
    if (id.empty())
    {
        BOOST_CHECK_MESSAGE(detected == expected,
            "disjoint: " << wkt1 << " and " << wkt2
            << " -> Expected: " << expected
            << " detected: " << detected);
    }
    else
    {
        BOOST_CHECK_MESSAGE(detected == expected,
            "disjoint: " << id
            << " -> Expected: " << expected
            << " detected: " << detected);
    }
}

template <typename G1, typename G2>
void test_disjoint(std::string const& id,
                   std::string const& wkt1,
                   std::string const& wkt2,
                   bool expected)
{
    G1 g1;
    bg::read_wkt(wkt1, g1);

    G2 g2;
    bg::read_wkt(wkt2, g2);

    boost::variant<G1> v1(g1);
    boost::variant<G2> v2(g2);

    bg::model::geometry_collection<boost::variant<G1>> gc1{v1};
    bg::model::geometry_collection<boost::variant<G2>> gc2{v2};

    using strategy_t = typename bg::strategy::disjoint::services::default_strategy<G1, G2>::type;
    using strategies_t = typename bg::strategies::relate::services::default_strategy<G1, G2>::type;

    check_disjoint(id, wkt1, wkt2, g1, g2, expected, bg::default_strategy());
    check_disjoint(id, wkt1, wkt2, g1, g2, expected, strategy_t());
    check_disjoint(id, wkt1, wkt2, g1, g2, expected, strategies_t());

    check_disjoint(id, wkt1, wkt2, v1, v2, expected, bg::default_strategy());
    check_disjoint(id, wkt1, wkt2, v1, v2, expected, strategy_t());
    check_disjoint(id, wkt1, wkt2, v1, v2, expected, strategies_t());

    check_disjoint(id, wkt1, wkt2, gc1, gc2, expected, bg::default_strategy());
    check_disjoint(id, wkt1, wkt2, gc1, gc2, expected, strategy_t());
    check_disjoint(id, wkt1, wkt2, gc1, gc2, expected, strategies_t());

    check_disjoint(id, wkt1, wkt2, v1, g2, expected, bg::default_strategy());
    check_disjoint(id, wkt1, wkt2, v1, g2, expected, strategy_t());
    check_disjoint(id, wkt1, wkt2, v1, g2, expected, strategies_t());
    check_disjoint(id, wkt1, wkt2, g1, v2, expected, bg::default_strategy());
    check_disjoint(id, wkt1, wkt2, g1, v2, expected, strategy_t());
    check_disjoint(id, wkt1, wkt2, g1, v2, expected, strategies_t());

    check_disjoint(id, wkt1, wkt2, gc1, g2, expected, bg::default_strategy());
    check_disjoint(id, wkt1, wkt2, gc1, g2, expected, strategy_t());
    check_disjoint(id, wkt1, wkt2, gc1, g2, expected, strategies_t());
    check_disjoint(id, wkt1, wkt2, g1, gc2, expected, bg::default_strategy());
    check_disjoint(id, wkt1, wkt2, g1, gc2, expected, strategy_t());
    check_disjoint(id, wkt1, wkt2, g1, gc2, expected, strategies_t());

    check_disjoint(id, wkt1, wkt2, gc1, v2, expected, bg::default_strategy());
    check_disjoint(id, wkt1, wkt2, gc1, v2, expected, strategy_t());
    check_disjoint(id, wkt1, wkt2, gc1, v2, expected, strategies_t());
    check_disjoint(id, wkt1, wkt2, v1, gc2, expected, bg::default_strategy());
    check_disjoint(id, wkt1, wkt2, v1, gc2, expected, strategy_t());
    check_disjoint(id, wkt1, wkt2, v1, gc2, expected, strategies_t());
}

template <typename G1, typename G2>
void test_geometry(std::string const& wkt1,
                   std::string const& wkt2,
                   bool expected)
{
    test_disjoint<G1, G2>("", wkt1, wkt2, expected);
}

#endif // BOOST_GEOMETRY_TEST_DISJOINT_HPP


#include <stan/math/prim/mat.hpp>
#include <stan/math/prim/scal.hpp>
#include <test/unit/math/prim/prob/vector_rng_test_helper.hpp>
#include <test/unit/math/prim/prob/NegativeBinomial2LogTestRig.hpp>
#include <test/unit/math/prim/prob/VectorIntRNGTestRig.hpp>
#include <gtest/gtest.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/math/distributions.hpp>
#include <limits>
#include <vector>
#include <string>

class NegativeBinomial2TestRig : public VectorIntRNGTestRig {
 public:
  NegativeBinomial2TestRig()
      : VectorIntRNGTestRig(10000, 10, {0, 1, 2, 3, 4, 5, 6}, {0.1, 1.7, 3.99},
                            {1, 2, 3}, {-2.1, -0.5, 0.0}, {-3, -1, 0},
                            {0.1, 1.1, 4.99}, {1, 2, 3}, {-3.0, -2.0, 0.0},
                            {-3, -1, 0}) {}

  template <typename T1, typename T2, typename T3, typename T_rng>
  auto generate_samples(const T1& mu, const T2& phi, const T3&,
                        T_rng& rng) const {
    return stan::math::neg_binomial_2_rng(mu, phi, rng);
  }

  template <typename T1>
  double pmf(int y, T1 mu, double phi, double) const {
    return std::exp(stan::math::neg_binomial_2_lpmf(y, mu, phi));
  }
};

TEST(ProbDistributionsNegativeBinomial2, errorCheck) {
  check_dist_throws_all_types(NegativeBinomial2TestRig());
}

TEST(ProbDistributionsNegativeBinomial2, distributionCheck) {
  check_counts_real_real(NegativeBinomial2TestRig());
}

TEST(ProbDistributionsNegBinomial2, error_check) {
  boost::random::mt19937 rng;
  EXPECT_NO_THROW(stan::math::neg_binomial_2_rng(6, 2, rng));
  EXPECT_NO_THROW(stan::math::neg_binomial_2_rng(0.5, 1, rng));
  EXPECT_NO_THROW(stan::math::neg_binomial_2_rng(1e8, 1, rng));

  EXPECT_THROW(stan::math::neg_binomial_2_rng(0, -2, rng), std::domain_error);
  EXPECT_THROW(stan::math::neg_binomial_2_rng(6, -2, rng), std::domain_error);
  EXPECT_THROW(stan::math::neg_binomial_2_rng(-6, -0.1, rng),
               std::domain_error);
  EXPECT_THROW(
      stan::math::neg_binomial_2_rng(stan::math::positive_infinity(), 2, rng),
      std::domain_error);
  EXPECT_THROW(
      stan::math::neg_binomial_2_rng(stan::math::positive_infinity(), 6, rng),
      std::domain_error);
  EXPECT_THROW(
      stan::math::neg_binomial_2_rng(2, stan::math::positive_infinity(), rng),
      std::domain_error);

  std::string error_msg;

  error_msg
      = "neg_binomial_2_rng: Location parameter "
        "divided by the precision parameter is "
        "inf, but must be finite!";
  try {
    stan::math::neg_binomial_2_rng(1e300, 1e-300, rng);
    FAIL() << "neg_binomial_2_rng should have thrown" << std::endl;
  } catch (const std::exception& e) {
    if (std::string(e.what()).find(error_msg) == std::string::npos)
      FAIL() << "Error message is different than expected" << std::endl
             << "EXPECTED: " << error_msg << std::endl
             << "FOUND: " << e.what() << std::endl;
    SUCCEED();
  }

  error_msg
      = "neg_binomial_2_rng: Random number that "
        "came from gamma distribution is";
  try {
    stan::math::neg_binomial_2_rng(1e10, 1e20, rng);
    FAIL() << "neg_binomial_2_rng should have thrown" << std::endl;
  } catch (const std::exception& e) {
    if (std::string(e.what()).find(error_msg) == std::string::npos)
      FAIL() << "Error message is different than expected" << std::endl
             << "EXPECTED: " << error_msg << std::endl
             << "FOUND: " << e.what() << std::endl;
    SUCCEED();
  }
}

TEST(ProbDistributionsNegBinomial2, chiSquareGoodnessFitTest) {
  boost::random::mt19937 rng;
  int N = 1000;
  int K = stan::math::round(2 * std::pow(N, 0.4));
  boost::math::negative_binomial_distribution<> dist(1.1, 1.1 / (1.1 + 2.4));
  boost::math::chi_squared mydist(K - 1);

  int loc[K - 1];
  for (int i = 1; i < K; i++)
    loc[i - 1] = i - 1;

  int count = 0;
  double bin[K];
  double expect[K];
  for (int i = 0; i < K; i++) {
    bin[i] = 0;
    expect[i] = N * pdf(dist, i);
  }
  expect[K - 1] = N * (1 - cdf(dist, K - 1));

  while (count < N) {
    int a = stan::math::neg_binomial_2_rng(2.4, 1.1, rng);
    int i = 0;
    while (i < K - 1 && a > loc[i])
      ++i;
    ++bin[i];
    count++;
  }

  double chi = 0;

  for (int j = 0; j < K; j++)
    chi += ((bin[j] - expect[j]) * (bin[j] - expect[j]) / expect[j]);

  EXPECT_TRUE(chi < quantile(complement(mydist, 1e-6)));
}

TEST(ProbDistributionsNegBinomial2, chiSquareGoodnessFitTest2) {
  boost::random::mt19937 rng;
  int N = 1000;
  int K = stan::math::round(2 * std::pow(N, 0.4));
  boost::math::negative_binomial_distribution<> dist(0.6, 0.6 / (0.6 + 2.4));
  boost::math::chi_squared mydist(K - 1);

  int loc[K - 1];
  for (int i = 1; i < K; i++)
    loc[i - 1] = i - 1;

  int count = 0;
  double bin[K];
  double expect[K];
  for (int i = 0; i < K; i++) {
    bin[i] = 0;
    expect[i] = N * pdf(dist, i);
  }
  expect[K - 1] = N * (1 - cdf(dist, K - 1));

  while (count < N) {
    int a = stan::math::neg_binomial_2_rng(2.4, 0.6, rng);
    int i = 0;
    while (i < K - 1 && a > loc[i])
      ++i;
    ++bin[i];
    count++;
  }

  double chi = 0;

  for (int j = 0; j < K; j++)
    chi += ((bin[j] - expect[j]) * (bin[j] - expect[j]) / expect[j]);

  EXPECT_TRUE(chi < quantile(complement(mydist, 1e-6)));
}

TEST(ProbDistributionsNegBinomial2, chiSquareGoodnessFitTest3) {
  boost::random::mt19937 rng;
  int N = 1000;
  int K = stan::math::round(2 * std::pow(N, 0.4));
  boost::math::negative_binomial_distribution<> dist(30, 30 / (30 + 60.4));
  boost::math::chi_squared mydist(K - 1);

  int loc[K - 1];
  for (int i = 1; i < K; i++)
    loc[i - 1] = i - 1;

  int count = 0;
  double bin[K];
  double expect[K];
  for (int i = 0; i < K; i++) {
    bin[i] = 0;
    expect[i] = N * pdf(dist, i);
  }
  expect[K - 1] = N * (1 - cdf(dist, K - 1));

  while (count < N) {
    int a = stan::math::neg_binomial_2_rng(60.4, 30, rng);
    int i = 0;
    while (i < K - 1 && a > loc[i])
      ++i;
    ++bin[i];
    count++;
  }

  double chi = 0;

  for (int j = 0; j < K; j++)
    chi += ((bin[j] - expect[j]) * (bin[j] - expect[j]) / expect[j]);

  EXPECT_TRUE(chi < quantile(complement(mydist, 1e-6)));
}

TEST(ProbDistributionsNegBinomial, chiSquareGoodnessFitTest4) {
  boost::random::mt19937 rng;
  int N = 1000;
  int K = stan::math::round(2 * std::pow(N, 0.4));
  boost::math::negative_binomial_distribution<> dist(80, 80 / (80 + 30.4));
  boost::math::chi_squared mydist(K - 1);

  int loc[K - 1];
  for (int i = 1; i < K; i++)
    loc[i - 1] = i - 1;

  int count = 0;
  double bin[K];
  double expect[K];
  for (int i = 0; i < K; i++) {
    bin[i] = 0;
    expect[i] = N * pdf(dist, i);
  }
  expect[K - 1] = N * (1 - cdf(dist, K - 1));

  while (count < N) {
    int a = stan::math::neg_binomial_2_rng(30.4, 80, rng);
    int i = 0;
    while (i < K - 1 && a > loc[i])
      ++i;
    ++bin[i];
    count++;
  }

  double chi = 0;

  for (int j = 0; j < K; j++)
    chi += ((bin[j] - expect[j]) * (bin[j] - expect[j]) / expect[j]);

  EXPECT_TRUE(chi < quantile(complement(mydist, 1e-6)));
}

TEST(ProbDistributionsNegBinomial, extreme_values) {
  int N = 100;
  double mu = 8;
  double phi = 1e12;
  for (int n = 0; n < 10; ++n) {
    phi *= 10;
    double logp = stan::math::neg_binomial_2_log<false>(N, mu, phi);
    EXPECT_LT(logp, 0);
  }
}

TEST(ProbDistributionsNegativeBinomial2Log, distributionCheck) {
  check_counts_real_real(NegativeBinomial2LogTestRig());
}


/*
 Copyright (C) 2017 Sascha Meiers
 Distributed under the MIT software license, see the accompanying
 file LICENSE.md or http://www.opensource.org/licenses/mit-license.php.
 */

#ifndef simulate_hpp
#define simulate_hpp

#include <iostream>
#include <vector>
#include <random>
#include <utility>
#include <chrono>
#include <unordered_map>

//#include <boost/math/distributions/negative_binomial.hpp>
#include <boost/multiprecision/random.hpp>
#include <boost/program_options/cmdline.hpp>
#include <boost/program_options/options_description.hpp>
#include <boost/program_options/parsers.hpp>
#include <boost/program_options/variables_map.hpp>
#include <boost/filesystem.hpp>

#include "version.hpp"
#include "program_options.hpp"
#include "intervals.hpp"
#include "counter.hpp"
#include "iocounts.hpp"

/**
 * @file
 * @defgroup simulation Simulation of Strand-seq data
 *
 * Summary of how Strand-seq data simulation works
 *
 * ## Strand-seq simulation
 *
 * The simulation of a single cell contains of three steps:
 *
 *   1. Sample basic counts for both haplotypes from a negative binomial distribution
 *   2. Insert SV according to the user specificaitons onto one of the haplotypes.
 *   3. Render cell as WW,WC or CC, potentially including SCEs
 *
 * ### 1: Basic counts for a cell
 * First, we randomly select a mean coverage (\f$c\f$) between `minCoverage` and
 * `maxCoverage` (user-specified) from a uniform distribution.
 *
 * Internally, a simulated cell is represented by two haplotypes (`h1`, `h2`),
 * which both have reads on the main strand (I call it *plus*), and a few
 * abnormal reads that are flipped in orientation (i.e. on the *minus* strand).
 *
 *   * *plus* reads are sampled from a negative binomial with parameters \f$p\f$
 *     (user-specified) and \f$n = c/2 * p/(1-p)\f$
 *   * *minus* reads are sampled from a zero-inflated geometric distribution:
 *     with probability \f$1-\alpha\f$ the bin gets 0 reads, otherwise the read
 *     number is sampled from a geometric distribution
 *
 * ### 2: Insertion of SVs
 * SVs are specified in a config file and their start and end positions do **not
 * need to align with bins**.
 *
 * SVs are introduced by changing the *plus* and *minus* counts according to
 * what you would expect from an SV. This can be either done for a single
 * haplotype (heterozygous) or both haplotypes (homozygous). Below the changes
 * to the strands are listed in detail
 *
 * SV type         | Haplotype *plus* counts | Haplotype *minus* counts
 * ---             | ---                     | ---
 * deletion  *HET* / *HOM*)     | set to 0                | no change
 * duplication (*HET* / *HOM*)  | multiply by 2           | no change
 * inversion (*HET* / *HOM*)    | switch with *minus*     | switch with *plus*
 * inverted duplication (*HET*) | no change               | set to *plus*
 * false_del (*HOM*)            | divide by 2             | no change
 *
 * > For now, heterozygous SV are **always introduced on haplotype 1**. In *HET*
 * > SVs the aforementioned changes are applied only to haplotype 1, in *HOM*
 * > SVs to both haplotypes.
 *
 * The changes are applied to all bins involved in the SV. However, if a bin is
 * only involved partially, the rule is applied only to a fraction of the
 * counts.
 *
 *
 * ### 3: Render cells
 *
 * At the last step, haplotypes are "inherited" as either Watson or Crick
 * strands. This is randomly chosen for each chromosome so that on average we
 * will obtain a 25:50:25 ratio of WW:WC:CC chromosomes.
 */

namespace simulator {

using interval::Interval;
using count::TGenomeCounts;
using count::Counter;

/**
 * Save read counts of a single bin for 2 haplotypes, separated by correct
 * strand (*plus*) and flipped strand (*minus*).
 *
 * @ingroup simulation
 */
struct HaploCount {
    unsigned h1_plus, h1_minus, h2_plus, h2_minus;
    HaploCount() : h1_plus(0), h1_minus(0), h2_plus(0), h2_minus(0)
    {}
};

/**
 * @ingroup simulation
 */
enum SV_type {
    het_inv,
    hom_inv,
    het_del,
    hom_del,
    het_dup,
    hom_dup,
    inv_dup,
    false_del
};
    std::string SV_type_to_string(SV_type x) {
        switch(x) {
            case het_inv: return ("het_inv");
            case hom_inv: return ("hom_inv");
            case het_del: return ("het_del");
            case hom_del: return ("hom_del");
            case het_dup: return ("het_dup");
            case hom_dup: return ("hom_dup");
            case inv_dup: return ("inv_dup");
            case false_del: return ("false_del");
        }
        return ("?");
    }

/**
 * @ingroup simulation
 */
struct SV {
    Interval where;
    SV_type type;
    float vaf;
    SV(Interval const & intvl, SV_type const & type) :
        where(intvl), type(type), vaf(1)
    {}
    SV(Interval const & intvl, SV_type const & type, float vaf) :
        where(intvl), type(type), vaf(vaf)
    {}
};

typedef std::vector<HaploCount> THapCount;
typedef std::vector<std::string> THapType;

struct phased_counts {
    uint8_t h1_w;
    uint8_t h1_c;
    uint8_t h2_w;
    uint8_t h2_c;
};

/**
 * Turn the list of HaploCount information into Strand-seq data.
 * @ingroup simulation
 *
 * Initially we decide for each haplotype on which strand (W or C) it is going
 * to be inherited. Then, while traversing along the chromosome, there is a
 * small chance in every bin that these states change --> this is an SCE.
 *
 * **Update**: Now this function also simulates phased reads for both
 * haplotypes, which are drawn from a binomial distribution using `phased_frac`
 * as a probability.
 *
 * @param hapls Vector of haplotypes (THapl) for each cell, which shall be written as W/C counts.
 * @param chrom_map Chromosome boarders
 * @param sce_prob Probabiliy per bin to change strands
 * @param strand_states Vector of inherited strand states. Note that `Interval`s
 *        get mis-used by inputting bin numbers instead of chromosomal positions
 * @param phases Empty vector of `phased_counts` which will be filled to the
 *        same size as the returned `TGenomeCounts` with counts of haplotypes
 *        H1 and H2 on Watson and Crick strands. This simulates phase data.
 * @param phased_frac Fraction of reads that can be phased (used in binomial
 *        distribution).
 * @return Final Watson/Crick counts that can be plotted.
 */
template <typename TRandomDev>
TGenomeCounts render_cell(THapCount const & hapls,
                          std::vector<int32_t> const & chrom_map,
                          float sce_prob,
                          std::vector<std::pair<Interval, std::string>> & strand_states,
                          std::vector<phased_counts> & phases,
                          TRandomDev & rd_gen,
                          float phased_frac = 0.1)
{
    std::uniform_real_distribution<> rd_unif(0,1);

    // Final counts to be written
    TGenomeCounts counts(chrom_map.back());
    phases.resize(chrom_map.back());

    // Go through all chromosomes
    for (int32_t chrom = 0; chrom<chrom_map.size()-1; ++chrom)
    {
        if (chrom_map[chrom+1] - chrom_map[chrom] < 1) continue;

        // strand states for both haplotypes: true = Watson, false = Crick
        // Initially, choose states with equal prob.
        bool W_h1 = rd_unif(rd_gen) < 0.5;
        bool W_h2 = rd_unif(rd_gen) < 0.5;
        std::string state = std::string(W_h1?"W":"C") + std::string(W_h2?"W":"C");

        // Iterate over bins
        unsigned start_bin = chrom_map[chrom];
        for(unsigned bin = chrom_map[chrom]; bin < chrom_map[chrom+1]; ++bin)
        {
            // Small chance of an SCE:
            if(bin > chrom_map[chrom] && rd_unif(rd_gen) < sce_prob)
            {
                // Write down interval
                strand_states.push_back(std::make_pair(Interval(chrom, start_bin, bin-1), state));
                start_bin = bin;

                // change the state of one haplotype
                if (rd_unif(rd_gen) < 0.5) W_h1 = !W_h1;
                else                       W_h2 = !W_h2;
                state = std::string(W_h1?"W":"C") + std::string(W_h2?"W":"C");
            }

            // Fill counts
            counts[bin].watson_count = (W_h1  ? hapls[bin].h1_plus : hapls[bin].h1_minus) +
                                       (W_h2  ? hapls[bin].h2_plus : hapls[bin].h2_minus);
            counts[bin].crick_count  = (!W_h1 ? hapls[bin].h1_plus : hapls[bin].h1_minus) +
                                       (!W_h2 ? hapls[bin].h2_plus : hapls[bin].h2_minus);

            // Simulate phased reads (for each read there is a small chance to be phased)
            if (W_h1) {
                phases[bin].h1_w = std::binomial_distribution<>(hapls[bin].h1_plus, phased_frac)(rd_gen);
                phases[bin].h1_c = std::binomial_distribution<>(hapls[bin].h1_minus, phased_frac)(rd_gen);
            } else {
                phases[bin].h1_c = std::binomial_distribution<>(hapls[bin].h1_plus, phased_frac)(rd_gen);
                phases[bin].h1_w = std::binomial_distribution<>(hapls[bin].h1_minus, phased_frac)(rd_gen);
            }
            if (W_h2) {
                phases[bin].h2_w = std::binomial_distribution<>(hapls[bin].h2_plus, phased_frac)(rd_gen);
                phases[bin].h2_c = std::binomial_distribution<>(hapls[bin].h2_minus, phased_frac)(rd_gen);
            } else {
                phases[bin].h2_c = std::binomial_distribution<>(hapls[bin].h2_plus, phased_frac)(rd_gen);
                phases[bin].h2_w = std::binomial_distribution<>(hapls[bin].h2_minus, phased_frac)(rd_gen);
            }
        }

        // write down interval
        strand_states.push_back(std::make_pair(Interval(chrom, start_bin, chrom_map[chrom+1]-1), state));
    }
    return counts;
}

/** (Partially) flip haplotype counts according to a certail SV type
 *
 * For a het inversion for example, plus and minus on h1 are exchanged.
 * If a breakpoint of the SV does not perfectly align with bin boundaries,
 * you can specify an additional fraction f [0,1] to say that the flip should
 * only occur in f% of the bin.
 *
 * @param h Haplotype of a single bin (containing 4 counts).
 * @param sv_type Type of SV, only a few are possible.
 * @param f Apply the flip only to a portion of this bin.
 */
inline void flip_strand(HaploCount & h, SV_type sv_type, float f = 1)
{
    HaploCount x = h;
    switch(sv_type) {
        case het_inv:
            h.h1_plus  = (1-f) * h.h1_plus  + f * x.h1_minus;
            h.h1_minus = (1-f) * h.h1_minus + f * x.h1_plus;
            break;
        case hom_inv:
            h.h1_plus  = (1-f) * h.h1_plus  + f * x.h1_minus;
            h.h1_minus = (1-f) * h.h1_minus + f * x.h1_plus;
            h.h2_plus  = (1-f) * h.h2_plus  + f * x.h2_minus;
            h.h2_minus = (1-f) * h.h2_minus + f * x.h2_plus;
            break;
        case het_del:
            h.h1_plus  = (1-f) * h.h1_plus  + f * 0;
            h.h1_minus = (1-f) * h.h1_minus + f * 0;
            break;
        case hom_del:
            h.h1_plus  = (1-f) * h.h1_plus  + f * 0;
            h.h1_minus = (1-f) * h.h1_minus + f * 0;
            h.h2_plus  = (1-f) * h.h2_plus  + f * 0;
            h.h2_minus = (1-f) * h.h2_minus + f * 0;
            break;
        case het_dup:
            h.h1_plus  = (1-f) * h.h1_plus  + f * 2 * h.h1_plus;
            h.h1_minus = (1-f) * h.h1_minus + f * 2 * h.h1_minus; // should be 0
            break;
        case hom_dup:
            h.h1_plus  = (1-f) * h.h1_plus  + f * 2 * h.h1_plus;
            h.h1_minus = (1-f) * h.h1_minus + f * 2 * h.h1_minus; // should be 0
            h.h2_plus  = (1-f) * h.h2_plus  + f * 2 * h.h2_plus;
            h.h2_minus = (1-f) * h.h2_minus + f * 2 * h.h2_minus; // should be 0
            break;
        case inv_dup:
            h.h1_plus  = (1-f) * h.h1_plus  + f * (x.h1_minus + x.h1_plus);
            h.h1_minus = (1-f) * h.h1_minus + f * (x.h1_minus + x.h1_plus);
            break;
        case false_del:
            h.h1_plus  = (1-f) * h.h1_plus  + f * x.h1_plus/2;
            h.h1_minus = (1-f) * h.h1_minus + f * x.h1_minus/2;
            h.h2_plus  = (1-f) * h.h2_plus  + f * x.h2_plus/2;
            h.h2_minus = (1-f) * h.h2_minus + f * x.h2_minus/2;
    }
}

/** Read the SV config file (5 columns)
 *
 * this function also checks that the SV definition is valid and within the
 * chromosome boarders. Note that this is important because this check will not
 * be done during `locate_partial_bins`.
 *
 * @param filename file name.
 * @param chrom_names vector of names of the chromosomes.
 * @param chrom_size  vector of chromosome sizes. Must match `chrom_names` in size.
 * @param sv_list List of SVs to be written (push_back is used)
 */
bool read_SV_config_file(std::string const & filename,
                         std::vector<std::string> const & chrom_names,
                         std::vector<int32_t> const & chrom_size,
                         std::vector<SV> & sv_list)
{
    std::ifstream interval_file(filename.c_str(), std::ifstream::in);
    if (interval_file.is_open()) {
        while (interval_file.good()) {
            std::string line;
            getline(interval_file, line);
            typedef boost::tokenizer< boost::char_separator<char> > Tokenizer;
            boost::char_separator<char> sep(" \t,;");
            Tokenizer tokens(line, sep);
            Tokenizer::iterator tokIter = tokens.begin();
            if (tokIter!=tokens.end())
            {
                std::string chrName = *tokIter++;

                // get chromosome id
                //int32_t tid = bam_name2id(hdr, chrName.c_str());
                int32_t tid = -1;
                for (int32_t i = 0; i < chrom_names.size(); ++i) {
                    if (chrom_names[i] == chrName) {
                        tid = i;
                        break;
                    }
                }
                if (tid >= 0 && tid < chrom_names.size())
                {
                    Interval ivl;
                    ivl.chr = tid;

                    if (tokIter == tokens.end()) {
                        std::cerr << "Warning: Invalid line: " << line << std::endl;
                        continue;
                    }
                    ivl.start = boost::lexical_cast<int32_t>(*tokIter++);
                    if (tokIter == tokens.end()) {
                        std::cerr << "Warning: Invalid line: " << line << std::endl;
                        continue;
                    }
                    ivl.end   = boost::lexical_cast<int32_t>(*tokIter++);

                    // check whether interval makes sens
                    if (ivl.start < 0 || ivl.start > ivl.end || ivl.end > chrom_size[tid]) {
                        std::cerr << "[Warning] Interval out of bounds " << line << std::endl;
                        continue;
                    }

                    // Get SV type and VAF
                    if (tokIter == tokens.end()) {
                        std::cerr << "Warning: Invalid line: " << line << std::endl;
                        continue;
                    }
                    std::string type_in = *tokIter++;
                    SV_type type_out;
                    if      (type_in == "het_inv")
                        type_out = het_inv;
                    else if (type_in == "hom_inv")
                        type_out = hom_inv;
                    else if (type_in == "het_del")
                        type_out = het_del;
                    else if (type_in == "hom_del")
                        type_out = hom_del;
                    else if (type_in == "het_dup")
                        type_out = het_dup;
                    else if (type_in == "hom_dup")
                        type_out = hom_dup;
                    else if (type_in == "inv_dup")
                        type_out = inv_dup;
                    else if (type_in == "false_del")
                        type_out = false_del;
                    else {
                        std::cerr << "[Warning] Unknown SV type. Ignored: " << line << std::endl;
                    }

                    // Variant allele frequency
                    if (tokIter == tokens.end()) {
                        std::cerr << "Warning: Invalid line: " << line << std::endl;
                        continue;
                    }
                    float sv_vaf = boost::lexical_cast<float>(*tokIter++);
                    assert(tokIter == tokens.end());

                    sv_list.push_back(SV(ivl, type_out, sv_vaf));
                } else {
                    std::cerr << "Warning: Chromosome not found: " << chrName << " in \"" << line << "\"" << std::endl;
                }
            }
        }
        interval_file.close();
    } else {
        std::cerr << "[Error] SV config file cannot be read: " << filename << std::endl;
        return false;
    }
    return true;
}

/**
 * Insert SVs onto haplotype counts.
 * @ingroup simulator
 *
 * This function does a couple of steps at the same time **for each SV**:
 *    * smaple which cells are supposed to carry the SV with a probability
 *      of *vaf* for each cell (this is probabilistic, so the actual number can
 *      differ from the expected vaf).
 *    * Insert SV onto haplotypes (currently just on h1), including
 *      *fractional bins* at the ends.
 *    * Note down ids of cell carrying the SV into `inserted_SVs` (refers to
 *      `cells`).
 *    * Calculate `optimal_breakpoints`, which are **always the right end of the
 *      bins**. The bin adapts to the left or right if the real SV breakpoint
 *      is < or > 50% of the bin.
 *
 * @param haplotypes Haplotype Counts that will be edited according to `flip_strands`.
 * @param inserted_SVs List of SVs and carriers that are inserted into haplotypes (initially empty).
 * @param cells List of `CellInfo`, which include cell and sample names etc.
 * @param optimal_breakpoints Set of optimal bin positions (e.g. optimal segmentation).
 * @param sv_list List of SVs to be inserted.
 * @param bins List of Intervals (bins) across the genome.
 * @param rd_gen Just pass the random generator here to flip a coin for each carrier.
 */
template <typename TRandomGenerator>
void simulate_SVs(std::vector<THapCount> & haplotypes,
                  std::vector<std::vector<unsigned>> & inserted_SVs,
                  std::set<unsigned> & optimal_breakpoints,
                  std::vector<SV> const & sv_list,
                  std::vector<Interval> const & bins,
                  std::vector<int32_t> const & chrom_map,
                  TRandomGenerator & rd_gen)
{
    // Need a uniform dist. to sample carriers of the SV.
    std::uniform_real_distribution<> rd_unif(0,1);

    // Prepare entries in inserted_SVs for each SV
    inserted_SVs.resize(sv_list.size());

    for (unsigned j = 0; j < sv_list.size(); ++j)
    {
        // SV position in bin coordinates
        SV const & sv = sv_list[j];
        auto   sv_bins = interval::locate_partial_bins(sv.where, bins, chrom_map);
        int32_t   binl = sv_bins.first.first;
        int32_t   binr = sv_bins.first.second;
        float       fl = sv_bins.second.first;
        float       fr = sv_bins.second.second;

        // Try for each cell
        for (unsigned i = 0; i < haplotypes.size(); ++i)
        {
            if (rd_unif(rd_gen) < sv.vaf)
            {
                // List which cells got which SV.
                inserted_SVs[j].push_back(i);

                // There is only one bin --> fraction is fl - (1-fr)
                if (binl == binr)
                {
                    flip_strand(haplotypes[i][binl], sv.type, fl - (1-fr));
                    optimal_breakpoints.insert(binl); // right
                    if (binl > chrom_map[sv.where.chr])
                        optimal_breakpoints.insert(binl-1); // left

                } else {

                    // Partially flip first bin
                    flip_strand(haplotypes[i][binl], sv.type, fl);
                    if (fl > 0.5 && binl > chrom_map[sv.where.chr]) // left
                        optimal_breakpoints.insert(binl-1);
                    else
                        optimal_breakpoints.insert(binl);

                    // Partially flip last bin
                    flip_strand(haplotypes[i][binr], sv.type, fr);
                    if (fr < 0.5 && binr > chrom_map[sv.where.chr]) // right
                        optimal_breakpoints.insert(binr-1);
                    else
                        optimal_breakpoints.insert(binr);

                    // Completely flip all bins in between
                    for (unsigned bin = binl+1; bin < binr; ++bin) {
                        flip_strand(haplotypes[i][bin], sv.type);
                    }
                }
            }
        }
    }
}

} /* namespace */

struct Conf_simul {
    bool verbose;
    unsigned n_cells;
    unsigned window;
    boost::filesystem::path f_sv;
    boost::filesystem::path f_out;
    boost::filesystem::path f_phases;
    boost::filesystem::path f_sce;
    boost::filesystem::path f_fai;
    boost::filesystem::path f_svs;
    boost::filesystem::path f_segment;
    boost::filesystem::path f_info;
    unsigned seed;
    double p, min_cov, max_cov, alpha, phased_frac;
    unsigned sce_num;
    std::string sample_name;
};

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

    using interval::Interval;
    using count::TGenomeCounts;
    using count::Counter;
    using simulator::SV;
    using simulator::HaploCount;
    using simulator::SV_type;
    using simulator::THapCount;
    using simulator::THapType;

    // Command line options
    Conf_simul conf;
    boost::program_options::options_description po_generic("Generic options");
    po_generic.add_options()
    ("help,?", "show help message")
    ("verbose,v", "tell me more")
    ("seed", boost::program_options::value<unsigned>(&conf.seed), "Random generator seed")
    ("window,w", boost::program_options::value<unsigned>(&conf.window)->default_value(100000)->notifier(in_range(1000,10000000,"window")), "window size of fixed windows")
    ("numcells,n", boost::program_options::value<unsigned>(&conf.n_cells)->default_value(10)->notifier(in_range(0,500,"numcells")), "number of cells to simulate")
    ("genome,g", boost::program_options::value<boost::filesystem::path>(&conf.f_fai), "Chrom names & length file. Default: GRch38")
    ;

    boost::program_options::options_description po_out("Output options");
    po_out.add_options()
    ("out,o",         boost::program_options::value<boost::filesystem::path>(&conf.f_out)->default_value("out.txt.gz"), "output count file")
    ("phases,P",      boost::program_options::value<boost::filesystem::path>(&conf.f_phases), "output phased reads into a file")
    ("sceFile,S",     boost::program_options::value<boost::filesystem::path>(&conf.f_sce), "output the positions of SCEs")
    ("variantFile,V", boost::program_options::value<boost::filesystem::path>(&conf.f_svs), "output SVs and which cells they were simulated in")
    ("segmentFile,U", boost::program_options::value<boost::filesystem::path>(&conf.f_segment), "output optimal segmentation according to SVs and SCEs.")
    ("info,i",        boost::program_options::value<boost::filesystem::path>(&conf.f_info), "Write info about samples")
    ("sample-name",   boost::program_options::value<std::string>(&conf.sample_name)->default_value("simulated"), "Use this sample name in the output")
    ;

    boost::program_options::options_description po_rand("Radnomization parameters");
    po_rand.add_options()
    ("nbinom_p,p",    boost::program_options::value<double>(&conf.p)->default_value(0.8,"0.8")->notifier(in_range(0.01,0.99,"nbinom")), "p parameter of the NB distirbution")
    ("minCoverage,c", boost::program_options::value<double>(&conf.min_cov)->default_value(10)->notifier(in_range(1,500,"minCoverage")), "min. read coverage per bin")
    ("maxCoverage,C", boost::program_options::value<double>(&conf.max_cov)->default_value(60)->notifier(in_range(1,500,"maxCoverage")), "max. read coverage per bin")
    ("alpha,a",       boost::program_options::value<double>(&conf.alpha)->default_value(0.1,"0.1")->notifier(in_range(0,1,"alpha")), "noise added to all bins: mostly 0, but for a fraction alpha drawn from geometrix distribution")
    ("scesPerCell,s", boost::program_options::value<unsigned>(&conf.sce_num)->default_value(4)->notifier(in_range(0,200,"scesPerCell")), "Average number of SCEs per cell")
    ("phasedFraction,z", boost::program_options::value<double>(&conf.phased_frac)->default_value(0.1)->notifier(in_range(0,1,"scesPerCell")), "Average number of SCEs per cell")
    ;

    boost::program_options::options_description po_hidden("Hidden options");
    po_hidden.add_options()
    ("sv_config_file", boost::program_options::value<boost::filesystem::path>(&conf.f_sv), "Config file for SVs (see details)")
    ;

    boost::program_options::positional_options_description po_positional;
    po_positional.add("sv_config_file", -1);

    boost::program_options::options_description po_cmdline_options;
    po_cmdline_options.add(po_generic).add(po_out).add(po_rand).add(po_hidden);
    boost::program_options::options_description po_visible_options;
    po_visible_options.add(po_generic).add(po_out).add(po_rand);
    boost::program_options::variables_map vm;
    boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(po_cmdline_options).positional(po_positional).run(), vm);
    boost::program_options::notify(vm);

    conf.verbose = vm.count("verbose");

    if (vm.count("help") || !vm.count("sv_config_file") || !file_exists(conf.f_sv.string()))
    {
        std::cout << std::endl;
        std::cout << "Mosaicatcher " << STRINGIFYMACRO(MOSAIC_VERSION_MAJOR);
        std::cout << "." << STRINGIFYMACRO(MOSAIC_VERSION_MINOR) << std::endl;
        std::cout << "> Simulate binned Strand-seq data." << std::endl;
        std::cout << std::endl;
        std::cout << "Usage:   " << argv[0] << " [OPTIONS] SV-conf-file" << std::endl << std::endl;
        std::cout << po_visible_options << std::endl;

        if (vm.count("help")) {
            std::cout << "Simulate binned Strand-seq cells including structural variants (SVs)" << std::endl;
            std::cout << "and sister chromatid exchange events (SCEs). Type, size, position and" << std::endl;
            std::cout << "frequency of SVs are specified by a config file. To not include SVs" << std::endl;
            std::cout << "specify an empty file. The SV config file is a tab-separated file with" << std::endl;
            std::cout << "5 columns (chrom, start, end, SV type, avg. freuqency)." << std::endl;
            std::cout << "The allowed SV types are" << std::endl;
            std::cout << "  - het_del, hom_del" << std::endl;
            std::cout << "  - het_dup, hom_dup" << std::endl;
            std::cout << "  - het_inv, hom_inv" << std::endl;
            std::cout << "  - inv_dup" << std::endl;
            std::cout << "  - false_del (to simulate lower mappability region)" << std::endl;
            std::cout << "SV breakpoints do not need to align with bin boundaries." << std::endl;
            std::cout << std::endl;
            std::cout << "Note: The frequency (5-th colum) is interpreted as an expected" << std::endl;
            std::cout << "      fraction of cells carrying the SV - the exact number of cells" << std::endl;
            std::cout << "      can eventually differ form that expectation." << std::endl;
        }
        return vm.count("help") ? 0 : 1;
    }

    // Error when coverage & p are not applicable to boost negative_binomial
    double nb_n_param = conf.min_cov/2 * conf.p / (1-conf.p);
    if (nb_n_param < 1) {
        std::cerr << "[Error] The specified values for p and minCov will cause a problem" << std::endl;
        std::cerr << "        for the negative binomial distribution. This is currently" << std::endl;
        std::cerr << "        a limitation of boost::random::negative_binomial_distribution," << std::endl;
        std::cerr << "        which only allows integer values > 0 as a value for n" << std::endl;
        std::cerr << "        This behaviour is different in boost::math::negative_binomial" << std::endl;
        std::cerr << "        and again different in the implementation used in R." << std::endl;
        std::cerr << "        std::negative_binomial is not giving an error but produces" << std::endl;
        std::cerr << "        wrong numbers!" << std::endl;
        std::cerr << "        This limitation is also the reason why background reads cannot be" << std::endl;
        std::cerr << "        modelled by an NB for now." << std::endl;
        std::cerr << "Please choose a higher p or minCov!" << std::endl;
        return 0;
    }

    std::chrono::steady_clock::time_point t1, t2;

    // global vars
    std::vector<Interval>       bins;
    std::vector<int32_t>        chrom_map;
    std::vector<int32_t>        chrom_sizes;
    std::vector<std::string>    chrom_names;
    std::vector<THapCount>      haplotypes;
    std::vector<THapType>       chrom_states;
    std::vector<CellInfo>       cells;

    // Read genome or use GRch38 by default
    if (vm.count("genome")) {
        std::cerr << "[Error]: reading a genome file is not implemented. Leave out this flag to use GRch38" << std::endl;
        return 2;
    } else {
        chrom_sizes = { \
            248956422, 242193529, 198295559, 190214555,
            181538259, 170805979, 159345973, 145138636,
            138394717, 133797422, 135086622, 133275309,
            114364328, 107043718, 101991189,  90338345,
            83257441,  80373285,  58617616,  64444167,
            46709983,  50818468, 156040895,  57227415 };     // GRCh38
        chrom_names = { \
            "chr1",  "chr2",  "chr3",   "chr4",  "chr5",  "chr6",
            "chr7",  "chr8",  "chr9",  "chr10", "chr11", "chr12",
            "chr13", "chr14", "chr15", "chr16", "chr17", "chr18",
            "chr19", "chr20", "chr21", "chr22",  "chrX",  "chrY" };
    }

    // Generage bins & chrom_map
    chrom_map.resize(chrom_sizes.size());
    create_fixed_bins(bins, chrom_map, conf.window, std::vector<Interval>(), (int32_t)chrom_sizes.size(), chrom_sizes);
    chrom_map.push_back((int32_t)bins.size());

    // Random generator
    std::random_device rd;
    std::mt19937 rd_gen(rd());
    boost::random::mt19937 rd_gen_boost;
    if (vm.count("seed")) {
        rd_gen_boost.seed(conf.seed);
        rd_gen      .seed(conf.seed);
        std::cout << "[Info] Using random seed " << conf.seed << std::endl;
    }

    // Generate basic haplotype counts of each cells, including random noise
    t1 = std::chrono::steady_clock::now();
    std::uniform_real_distribution<> rd_cov(conf.min_cov, conf.max_cov);
    std::uniform_real_distribution<> rd_unif(0,1);
    double p = conf.p;

    if (conf.verbose) std::cout << "Simulating  " << conf.n_cells << " cells" << std::endl;
    for (unsigned i = 0; i < conf.n_cells; ++i)
    {
        double cov_per_bin = rd_cov(rd_gen);
        std::geometric_distribution<>         rd_geom(5/(5+log2(cov_per_bin)));
        //std::negative_binomial_distribution<> rd_nb(cov_per_bin/2 * p/(1-p), p); // gives wrong results for small r !!
        boost::random::negative_binomial_distribution<> rd_nb_boost(cov_per_bin/2 * p/(1-p), p);
        boost::random::variate_generator<boost::mt19937&, boost::random::negative_binomial_distribution<> > rd_nb(rd_gen_boost, rd_nb_boost);

        CellInfo cell;
        cell.median_bin_count = static_cast<unsigned>(cov_per_bin);
        cell.sample_name = conf.sample_name;
        cell.cell_name   = std::string("cell_") + std::to_string(i);
        cell.bam_file    = "no_file";
        cell.nb_p        = p;
        cell.nb_a        = 0;
        cell.nb_r        = cov_per_bin * p / (1-p);

        THapCount count(bins.size());
        for (unsigned bin = 0; bin < bins.size(); ++bin)
        {
            count[bin].h1_plus = rd_nb();
            count[bin].h2_plus = rd_nb();
            count[bin].h1_minus = (rd_unif(rd_gen)<conf.alpha) ? rd_geom(rd_gen) : 0;
            count[bin].h2_minus = (rd_unif(rd_gen)<conf.alpha) ? rd_geom(rd_gen) : 0;
        }
        haplotypes.push_back(std::move(count));
        cells.push_back(std::move(cell));
    }

    // SV part
    std::vector<SV> sv_list;
    read_SV_config_file(conf.f_sv.string(), chrom_names, chrom_sizes, sv_list);

    // Insert SVs
    std::vector<std::vector<unsigned>> inserted_SVs;
    std::set<unsigned> optimal_breakpoints;
    simulate_SVs(haplotypes,
                 inserted_SVs,
                 optimal_breakpoints,
                 sv_list,
                 bins,
                 chrom_map,
                 rd_gen);

    t2 = std::chrono::steady_clock::now();
    auto time = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
    if (conf.verbose) std::cout << "[Info] Simulation took " << time.count() << " sec." << std::endl;

    // Write SV information (especially which cells contain the SVs)
    if (vm.count("variantFile"))
    {
        std::cout << "[Write] Variant summary: " << conf.f_svs.string() << std::endl;
        std::ofstream out(conf.f_svs.string());
        if (out.is_open())
        {
            out << "chrom\tstart\tend\tSV_type\tsample\tcell" << std::endl;
            for (unsigned j = 0; j < sv_list.size(); ++j)
            {
                SV const & sv = sv_list[j];
                for (unsigned carrier_id : inserted_SVs[j])
                {
                    out << chrom_names[sv.where.chr] << "\t";
                    out << sv.where.start << "\t" << sv.where.end << "\t";
                    out << SV_type_to_string(sv.type) << "\t";
                    out << cells[carrier_id].sample_name << "\t";
                    out << cells[carrier_id].cell_name << std::endl;
                }
            }
        } else {
            std::cerr << "[Warning] Cannot write to " << conf.f_svs.string() << std::endl;
        }
    }

    // Update optimal_breakpoints by one breakpoint at the end of each chrom.
    for (int32_t chrom = 1; chrom < chrom_map.size(); ++chrom)
        optimal_breakpoints.insert(chrom_map[chrom]-1);

    // Write optimal segmentation file, which includes SV breakpoints and SCE breakpoints.
    if (vm.count("segmentFile"))
    {
        std::cout << "[Write] Segmentation file: " << conf.f_segment.string() << std::endl;
        std::ofstream out(conf.f_segment.string());
        if (out.is_open())
        {
            out << "k\tchrom\tbps" << std::endl;
            for (auto const & bin : optimal_breakpoints)
            {
                int32_t chrom = std::upper_bound(chrom_map.begin(), chrom_map.end(), bin) - chrom_map.begin() -1;
                out << 0 << "\t" << chrom_names[chrom] << "\t";
                out << bin - chrom_map[chrom] << std::endl;
            }
        } else {
            std::cerr << "[Warning] Cannot write to " << conf.f_segment.string() << std::endl;
        }
    }

    // Turn haplotypes into TGenomeCounts and simulate SCEs
    t1 = std::chrono::steady_clock::now();
    std::vector<TGenomeCounts> final_counts;
    std::vector<std::pair<Interval,std::string>> strand_states;
    std::vector<unsigned> str_states_cells;
    std::vector<std::vector<simulator::phased_counts>> phases;

    for (unsigned i = 0; i < conf.n_cells; ++i)
    {
        unsigned cell_pos = strand_states.size();
        std::vector<simulator::phased_counts> phase;
        final_counts.push_back(render_cell(haplotypes[i],   // simulated counts
                                           chrom_map,       // chromosome boundaries
                                           (float)conf.sce_num / bins.size(), // sce_probability
                                           strand_states, // Intervals with inherited strand states
                                           phase,
                                           rd_gen,
                                           conf.phased_frac));

        // note down which cells belong to these intervals
        for (; cell_pos < strand_states.size(); ++cell_pos)
            str_states_cells.push_back(i);

        // save phased reads as additional output
        phases.push_back(phase);
    }
    t2 = std::chrono::steady_clock::now();
    time = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
    if (conf.verbose) std::cout << "[Info] Rendering cells took " << time.count() << " sec." << std::endl;

    // write down SCEs
    if (vm.count("sceFile"))
    {
        std::cout << "[Write] Inherited strand states (SCEs) to file " << conf.f_sce.string() << std::endl;
        std::ofstream out(conf.f_sce.string());
        if (out.is_open()) {
            out << "sample\tcell\tchrom\tstart\tend\tclass" << std::endl;
            for (unsigned i = 0; i < strand_states.size(); ++i) {
                out << conf.sample_name << "\t";
                out << "cell_" << std::to_string(str_states_cells[i]) << "\t";
                out << chrom_names[strand_states[i].first.chr] << "\t";
                out << bins[(strand_states[i].first).start].start << "\t";
                out << bins[(strand_states[i].first).end].end << "\t";
                out << strand_states[i].second << std::endl;
            }
        } else {
            std::cerr << "[Warning] Cannot write to " << conf.f_sce.string() << std::endl;
        }
    }

    // Get total number of reads per cell and print cell information:
    for (unsigned i = 0; i < final_counts.size(); ++i) {
        for (unsigned bin = 0; bin < bins.size(); ++bin) {
            cells[i].n_mapped += final_counts[i][bin].watson_count + final_counts[i][bin].crick_count;
        }
    }

    //
    // Chapter: Filter cells and bins and run HMM

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

    // median per cell
    count::set_median_per_cell(final_counts, cells);

    // filter cells with low counts and set pass_qc = false for bad cells;
    std::vector<unsigned> good_cells;
    good_cells = count::get_good_cells(final_counts, cells);
    for (auto c : cells) c.pass_qc = false;
    for (unsigned cid : good_cells) cells[cid].pass_qc = true;
    if (cells.size() > good_cells.size())
        std::cout << "[Info] " << cells.size() - good_cells.size() << "/" << cells.size()
                  << " cells were deemed QC fail by HMM (which is purely based on coverage for now)" << std::endl;

    // filter bins with abnormal counts (not happening here)
    std::vector<unsigned> good_bins(bins.size());
    std::iota(good_bins.begin(), good_bins.end(), 0); // fill with 0,1,2,...

    // calculate cell means and cell variances, grouped by sample (not cell)
    std::unordered_map<std::string, SampleInfo> samples;
    calculate_new_cell_mean(samples, cells, final_counts, good_cells, good_bins);

    // Estimation of parameter p per sample
    for (auto it = samples.begin(); it != samples.end(); ++it) {
        SampleInfo & s = it->second;
        s.p = std::inner_product(s.means.begin(), s.means.end(), s.means.begin(), 0.0f) \
        / std::inner_product(s.means.begin(), s.means.end(), s.vars.begin(), 0.0f);
    }

    // Chapter: Run HMM
    run_standard_HMM(final_counts,
                     good_cells,
                     cells,
                     good_bins,
                     chrom_map,
                     samples,
                     10.0f / bins.size());

    t2 = std::chrono::steady_clock::now();
    time = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
    if (conf.verbose) std::cout << "[Info] Running HMM took " << time.count() << " sec." << std::endl;

    // Write info files
    if (vm.count("info")) {
        std::cout << "[Write] Cell info to " << conf.f_info.string() << ". Note that NB parameters are estimated before SVs are introduced." << std::endl;
        write_cell_info(conf.f_info.string(), cells);
    }

    // write down phases
    if (vm.count("phases"))
    {
        std::cout << "[Write] Phases to " << conf.f_phases.string() << std::endl;
        std::ofstream out(conf.f_phases.string());
        if (out.is_open())
        {
            out << "chrom\tstart\tend\tsample\tcell\th1_w\th1_c\th2_w\th2_c" << std::endl;
            for (unsigned i = 0; i < phases.size(); ++i)
            {
                for (unsigned j = 0; j < phases[i].size(); ++j)
                {
                    out << chrom_names[bins[j].chr] << "\t";
                    out << bins[j].start << "\t";
                    out << bins[j].end << "\t";
                    out << cells[i].sample_name << "\t";
                    out << cells[i].cell_name << "\t";
                    out << static_cast<int>(phases[i][j].h1_w) << "\t";
                    out << static_cast<int>(phases[i][j].h1_c) << "\t";
                    out << static_cast<int>(phases[i][j].h2_w) << "\t";
                    out << static_cast<int>(phases[i][j].h2_c) << std::endl;
                }
            }
        } else {
            std::cerr << "[Warning] Cannot write to " << conf.f_phases.string() << std::endl;
        }
    }

    // write down counts
    std::cout << "[Write] Count table " << conf.f_out.string() << std::endl;
    std::vector<std::pair<std::string, std::string>> sample_cell_names;
    for (unsigned i=0; i<conf.n_cells; ++i) {
        sample_cell_names.push_back(std::make_pair(conf.sample_name, "cell_" + std::to_string(i)));
    }
    io::write_counts_gzip(conf.f_out.string(), final_counts, bins, chrom_names, sample_cell_names);

    return 0;
}

#endif /* simulate_hpp */


#include <Configuration.h> // Package Path
#include <avatar_locomanipulation/enable_pinocchio_with_hpp_fcl.h> // Enable HPP FCL
// Multibody
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/multibody/geometry.hpp"
// Parsers
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/srdf.hpp"
// Algorithms
#include "pinocchio/algorithm/geometry.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/model.hpp"
// Spatial
#include "pinocchio/spatial/fcl-pinocchio-conversions.hpp"
// Standard
#include <math.h>
#include <iostream>
#include <iomanip>
#include <vector>
#include <boost/shared_ptr.hpp>

int main(int argc, char ** argv){
  std::string filename = (argc<=1) ? THIS_PACKAGE_PATH"models/valkyrie_simplified.urdf" : argv[1];
  std::string srdf_filename = (argc<=1) ? THIS_PACKAGE_PATH"models/valkyrie_disable_collisions.srdf" : argv[1];
  std::vector < std::string > packageDirs;
  std::string meshDir  = THIS_PACKAGE_PATH"../val_model/";
  packageDirs.push_back(meshDir);

  // build URDF model
  pinocchio::Model model;
  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),model);

  // build Geometry model
  pinocchio::GeometryModel geomModel;
  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geomModel, packageDirs );

  geomModel.addAllCollisionPairs();
  // Removes all collision pairs as specified in the srdf_filename
  pinocchio::srdf::removeCollisionPairs(model, geomModel, srdf_filename, false);

  pinocchio::Data data(model);
  pinocchio::GeometryData geomData(geomModel);
  pinocchio::fcl::CollisionResult result;
  int i, j, k;
  std::vector<pinocchio::fcl::Contact> contacts;

  Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq); // Config

  // floating base joints: x, y, z
  q[0] = 0.0;  q[1] = 0.0;  q[2] = 0.0;

  // floating base quaternion: qx, qy, qz, qw
  double theta = 0; //M_PI/4.0;
  Eigen::AngleAxis<double> aa(theta, Eigen::Vector3d(0.0, 0.0, 1.0)); // yaw pi/4 to the left
  Eigen::Quaternion<double> quat_init; quat_init =  aa;

  q[3] = quat_init.x();// 0.0;
  q[4] = quat_init.y(); //0.0;
  q[5] = quat_init.z(); //sin(theta/2.0);
  q[6] = quat_init.w(); //cos(theta/2.0);

  pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q);

  pinocchio::computeCollisions(model,data,geomModel,geomData,q);

  std::cout << "geomData.collisionResults.size(): " << geomData.collisionResults.size() << std::endl;

  for(j=0; j<geomData.collisionResults.size(); j++)
  {
  	result = geomData.collisionResults[j];
  	pinocchio::CollisionPair idx = geomModel.collisionPairs[j];
  	result.getContacts(contacts);
      	if(contacts.size() != 0)
      	{
      		for(k=0; k<contacts.size(); k++)
      		{
      			std::cout << "Contact Found Between: " << geomModel.getGeometryName(idx.first) << " and " << geomModel.getGeometryName(idx.second) << std::endl;
        		std::cout << "position: " << contacts[k].pos << std::endl;
        		std::cout << "-------------------" << std::endl;
      		}
      	}
  }

}


/*
 * playgound_main.cc
 * Copyright (C) 2017 roman <roman.podolski@tum.de>
 *
 * Distributed under terms of the MIT license.
 */

#include <gflags/gflags.h>
#include <glog/logging.h>
#include <boost/format.hpp>
#include <boost/math/constants/constants.hpp>
#include <cmath>
#include <fstream>
#include <iostream>
#include <vector>
#include "planner/io/beamer.h"
#include "planner/map.h"
#include "planner/planner.h"

using boost::math::constants::pi;
using boost::math::constants::two_pi;

int main(int argc, char *argv[]) {
  gflags::ParseCommandLineFlags(&argc, &argv, true);
  google::InitGoogleLogging(argv[0]);
  // planner::Map map("data/simple.csv");

  // std::vector<std::pair<double, double>> waypoints_a{
  //{0, 10},  {10, 10}, {20, 10}, {30, 10}, {40, 10}, {50, 10},
  //{60, 10}, {70, 20}, {80, 30}, {90, 40}, {100, 50}};

  // std::vector<std::pair<double, double>> waypoints{
  //{0, 0},  {10, 0}, {20, 0}, {30, 0}, {40, 0}, {50, 0},
  //{60, 0}, {70, 0}, {80, 0}, {90, 0}, {100, 0}};

  std::ofstream tex("obstacles.tex");
  planner::io::beamer_mapper mapper(tex);

  std::vector<std::pair<double, double>> waypoints{{0, 0},  {10, 0}, {20, 0},
                                                   {30, 0}, {31, 0}, {32, 0}};
  planner::Planner p(waypoints);

  planner::pose pos{{5, 2.0}, pi<double>() / 16};

  // create some obstacles
  std::vector<planner::polygon> obstacles;
  for (int i = 0; i < 40; i += 10) {
    planner::polygon p1;
    for (double j = 0; j < two_pi<double>(); j += pi<double>() / 4.0) {
      double x = i + cos(j);
      double y = 5 + sin(j);
      p1.outer().push_back(planner::point(x, y));
    }

    planner::polygon p2;
    for (double j = 0; j < two_pi<double>(); j += pi<double>() / 4.0) {
      double x = i + cos(j);
      double y = -5 + sin(j);
      p2.outer().push_back(planner::point(x, y));
    }
    obstacles.push_back(p1);
    obstacles.push_back(p2);
  }

  p.set_last_known_s(0.0);

  auto my = p.update(pos.position, pos.theta, 10, 1, obstacles);

  // mapper.map(waypoints);

  // for (auto m : p.maneuvers()) {
  // if (m.collision()) {
  // mapper.map(m, "color=red");
  //} else {
  // mapper.map(m, "color=black");
  //}
  //}
  // mapper.map(my, "color=green");
  // for (const auto &o : obstacles) {
  // mapper.map(planner::bg::return_envelope<planner::box>(o), "color=red");
  //}

  for (const auto &pi : my.path()) {
    mapper.map(p.baseframe());
    mapper.map(my, "color=green");

    for (const auto &o : obstacles) {
      mapper.map(o);
    }

    for (const auto &pipi : my.path()) {
      mapper.map(pipi);
    }
    mapper.map(pi, "color=red");
    mapper.next_frame();
  }
}


//---------------------------------------------------------------------------//
// Copyright (c) 2014 Roshan <thisisroshansmail@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#ifndef BOOST_COMPUTE_ALGORITHM_DETAIL_MERGE_WIH_MERGE_PATH_HPP
#define BOOST_COMPUTE_ALGORITHM_DETAIL_MERGE_WIH_MERGE_PATH_HPP

#include <iterator>

#include <boost/compute/algorithm/detail/merge_path.hpp>
#include <boost/compute/algorithm/fill_n.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/detail/iterator_range_size.hpp>
#include <boost/compute/detail/meta_kernel.hpp>
#include <boost/compute/system.hpp>

namespace boost {
namespace compute {
namespace detail {

///
/// \brief Serial merge kernel class
///
/// Subclass of meta_kernel to perform serial merge after tiling
///
class serial_merge_kernel : meta_kernel
{
public:
    unsigned int tile_size;

    serial_merge_kernel() : meta_kernel("merge")
    {
        tile_size = 4;
    }

    template<class InputIterator1, class InputIterator2,
             class InputIterator3, class InputIterator4,
             class OutputIterator>
    void set_range(InputIterator1 first1,
                    InputIterator2 first2,
                    InputIterator3 tile_first1,
                    InputIterator3 tile_last1,
                    InputIterator4 tile_first2,
                    OutputIterator result)
    {
        m_count = iterator_range_size(tile_first1, tile_last1) - 1;

        *this <<
        "uint i = get_global_id(0);\n" <<
        "uint start1 = " << tile_first1[expr<uint_>("i")] << ";\n" <<
        "uint end1 = " << tile_first1[expr<uint_>("i+1")] << ";\n" <<
        "uint start2 = " << tile_first2[expr<uint_>("i")] << ";\n" <<
        "uint end2 = " << tile_first2[expr<uint_>("i+1")] << ";\n" <<
        "uint index = i*" << tile_size << ";\n" <<
        "while(start1<end1 && start2<end2)\n" <<
        "{\n" <<
        "   if(" << first1[expr<uint_>("start1")] << " <= " <<
                    first2[expr<uint_>("start2")] << ")\n" <<
        "   {\n" <<
                result[expr<uint_>("index")] <<
                    " = " << first1[expr<uint_>("start1")] << ";\n" <<
        "       index++;\n" <<
        "       start1++;\n" <<
        "   }\n" <<
        "   else\n" <<
        "   {\n" <<
                result[expr<uint_>("index")] <<
                    " = " << first2[expr<uint_>("start2")] << ";\n" <<
        "       index++;\n" <<
        "       start2++;\n" <<
        "   }\n" <<
        "}\n" <<
        "while(start1<end1)\n" <<
        "{\n" <<
            result[expr<uint_>("index")] <<
                " = " << first1[expr<uint_>("start1")] << ";\n" <<
        "   index++;\n" <<
        "   start1++;\n" <<
        "}\n" <<
        "while(start2<end2)\n" <<
        "{\n" <<
            result[expr<uint_>("index")] <<
                " = " << first2[expr<uint_>("start2")] << ";\n" <<
        "   index++;\n" <<
        "   start2++;\n" <<
        "}\n";
    }

    event exec(command_queue &queue)
    {
        if(m_count == 0) {
            return event();
        }

        return exec_1d(queue, 0, m_count);
    }

private:
    size_t m_count;
};

///
/// \brief Merge algorithm with merge path
///
/// Merges the sorted values in the range [\p first1, \p last1) with
/// the sorted values in the range [\p first2, last2) and stores the
/// result in the range beginning at \p result
///
/// \param first1 Iterator pointing to start of first set
/// \param last1 Iterator pointing to end of first set
/// \param first2 Iterator pointing to start of second set
/// \param last2 Iterator pointing to end of second set
/// \param result Iterator pointing to start of range in which the result
/// will be stored
/// \param queue Queue on which to execute
///
template<class InputIterator1, class InputIterator2, class OutputIterator>
inline OutputIterator
merge_with_merge_path(InputIterator1 first1,
                        InputIterator1 last1,
                        InputIterator2 first2,
                        InputIterator2 last2,
                        OutputIterator result,
                        command_queue &queue = system::default_queue())
{
    typedef typename std::iterator_traits<InputIterator1>::value_type value_type;

    int tile_size = 1024;

    int count1 = iterator_range_size(first1, last1);
    int count2 = iterator_range_size(first2, last2);

    vector<uint_> tile_a((count1+count2+tile_size-1)/tile_size+1, queue.get_context());
    vector<uint_> tile_b((count1+count2+tile_size-1)/tile_size+1, queue.get_context());

    // Tile the sets
    merge_path_kernel tiling_kernel;
    tiling_kernel.tile_size = 1024;
    tiling_kernel.set_range(first1, last1, first2, last2,
                            tile_a.begin()+1, tile_b.begin()+1);
    fill_n(tile_a.begin(), 1, 0, queue);
    fill_n(tile_b.begin(), 1, 0, queue);
    tiling_kernel.exec(queue);

    fill_n(tile_a.end()-1, 1, count1, queue);
    fill_n(tile_b.end()-1, 1, count2, queue);

    // Merge
    serial_merge_kernel merge_kernel;
    merge_kernel.tile_size = 1024;
    merge_kernel.set_range(first1, first2, tile_a.begin(), tile_a.end(),
                            tile_b.begin(), result);

    merge_kernel.exec(queue);

    return result + count1 + count2;
}

} //end detail namespace
} //end compute namespace
} //end boost namespace

#endif // BOOST_COMPUTE_ALGORITHM_DETAIL_MERGE_WIH_MERGE_PATH_HPP