/**
 * @file penalty_impl.hpp
 * @author Leonardo Arcari (leonardo1.arcari@gmail.com)
 * @version 1.0.0
 * @date 2018-10-28
 *
 * @copyright Copyright (c) 2018 Leonardo Arcari
 *
 * MIT License
 *
 * 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.
 *
 */

#ifndef BOOST_PENALTY_BASED_ALTERNATIVE_ROUTING_IMPL_HPP
#define BOOST_PENALTY_BASED_ALTERNATIVE_ROUTING_IMPL_HPP

#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/reverse_graph.hpp>
#include <boost/property_map/function_property_map.hpp>
#include <boost/property_map/property_map.hpp>

#include <arlib/details/arlib_utils.hpp>
#include <arlib/routing_kernels/bidirectional_dijkstra.hpp>
#include <arlib/routing_kernels/types.hpp>
#include <arlib/terminators.hpp>
#include <arlib/type_traits.hpp>

#include <functional>
#include <iostream>
#include <queue>
#include <unordered_map>
#include <unordered_set>
#include <vector>

namespace arlib {
/**
 * Implementations details of kSPwLO algorithms
 */
namespace details {
//===----------------------------------------------------------------------===//
//                      Penalty algorithm types
//===----------------------------------------------------------------------===//
/**
 * A map from edges to the number of time they have been penalized so
 * far.
 *
 * @tparam Edge An edge_descriptor
 */
template <typename Edge>
using PenBoundsMap = std::unordered_map<Edge, int, boost::hash<Edge>>;

/**
 * A map from edges to their weights
 *
 * @tparam Edge An edge_descriptor
 * @tparam Length The edge weight type
 */
template <typename Edge, typename Length>
using WeightMap = std::unordered_map<Edge, Length, boost::hash<Edge>>;

/**
 * A vector for tracking distance of a vertex from the source.
 *
 * The vector must be indexable by the vertex_descriptor, thus its size should
 * be equal to the number of vertices in the graph.
 *
 * @tparam Length The edge weight type.
 */
template <typename Length> using DistanceMap = std::vector<Length>;

//===----------------------------------------------------------------------===//
//                      Penalty algorithm classes
//===----------------------------------------------------------------------===//

/**
 * A functor to return the penalized weight of an edge, to avoid changing
 * the original graph weights.
 *
 * @tparam PMap A Weight Property Map.
 */
template <typename PMap> class penalty_functor {
public:
  using Edge = typename boost::property_traits<PMap>::key_type;
  using Length = double;

  /**
   * Construct a new penalty functor object.
   *
   * @tparam EdgeIterator An iterator of the graph edges.
   * @param weight The weight property map.
   */
  penalty_functor(PMap weight)
      : weight{weight}, penalties{std::make_shared<WeightMap<Edge, Length>>()} {
  }

  /**
   * Copy constructor.
   *
   * @param other The penalty functor to copy from
   */
  penalty_functor(const penalty_functor<PMap> &other)
      : weight{other.weight}, penalties{other.penalties} {}

  /**
   * Returns the penalized weight of an edge.
   *
   * @param e The query edge.
   * @return The penalized weight for @p e.
   */
  const Length &operator()(const Edge &e) const { return get_or_insert(e); }

  /**
   * Returns the penalized weight of an edge.
   *
   * @param e The query edge.
   * @return The penalized weight for @p e.
   */
  Length &operator[](const Edge &e) { return get_or_insert(e); }

  /**
   * Returns the penalized weight of an edge.
   *
   * @param e The query edge.
   * @return The penalized weight for @p e.
   */
  const Length &operator[](const Edge &e) const { return get_or_insert(e); }

  penalty_functor clone() const {
    auto pf = *this;
    pf.penalties = std::make_shared<WeightMap<Edge, Length>>(*penalties);
    return pf;
  }

private:
  PMap weight;
  mutable std::shared_ptr<WeightMap<Edge, Length>> penalties;

  Length &get_or_insert(const Edge &e) const {
    if (auto search = penalties->find(e); search != penalties->end()) {
      return search->second;
    } else {
      auto [it, ok] = penalties->insert({e, weight[e]});
      assert(ok && "[kspwlo::penalty_functor] Could not insert edge weight");
      return it->second;
    }
  }
};

template <typename PMap, typename Graph> class reverse_penalty_functor {
public:
  using Edge = typename boost::graph_traits<
      boost::reverse_graph<Graph>>::edge_descriptor;
  using Length = double;

  reverse_penalty_functor(penalty_functor<PMap> &penalty, const Graph &G,
                          const boost::reverse_graph<Graph> &rev_G)
      : inner_pf{penalty}, G{G}, rev_G{rev_G} {}

  reverse_penalty_functor(reverse_penalty_functor const &other)
      : inner_pf{other.inner_pf}, G{other.G}, rev_G{other.rev_G} {}

  const Length &operator()(const Edge &e) const {
    auto forward_edge = get_forward_edge(e);

    return inner_pf(forward_edge);
  }

  Length &operator[](const Edge &e) {
    auto forward_edge = get_forward_edge(e);

    return inner_pf[forward_edge];
  }

  const Length &operator[](const Edge &e) const {
    auto forward_edge = get_forward_edge(e);

    return inner_pf[forward_edge];
  }

private:
  auto get_forward_edge(const Edge &e) const {
    using namespace boost;
    auto u = source(e, rev_G);
    auto v = target(e, rev_G);

    auto [forward_edge, is_valid] = edge(v, u, G);
    assert(is_valid);
    return forward_edge;
  }

  penalty_functor<PMap> &inner_pf;
  const Graph &G;
  const boost::reverse_graph<Graph> &rev_G;
};

//===----------------------------------------------------------------------===//
//                     Penalty algorithm support routines
//===----------------------------------------------------------------------===//
/**
 * Computes the shortest path between two vertices s and t, first
 * from s to t and then from t to s. The distances of each node in the
 * shortest paths are stored in distance_s and distance_t.
 *
 * @tparam Graph A Boost::PropertyGraph having at least one edge
 *               property with tag boost::edge_weight_t.
 * @tparam Vertex A vertex_descriptor
 * @tparam Length The edge weight type.
 * @param G The graph.
 * @param s The source vertex.
 * @param t The target vertex.
 * @param distance_s A map from Vertex to its distance from @p s
 * @param distance_t A map from Vertex to its distance from @p t
 * @return A vector of the edges of the shortest path from s to t.
 *         An empty optional if t is not reachable from s.
 */
template <typename Graph, typename Vertex = vertex_of_t<Graph>,
          typename Edge = edge_of_t<Graph>,
          typename Length = length_of_t<Graph>>
std::optional<std::vector<Edge>>
dijkstra_shortest_path_two_ways(const Graph &G, Vertex s, Vertex t,
                                DistanceMap<Length> &distance_s,
                                DistanceMap<Length> &distance_t) {
  using namespace boost;

  auto predecessor = std::vector<Vertex>(num_vertices(G), s);
  auto vertex_id = get(vertex_index, G);

  // Forward step
  dijkstra_shortest_paths(G, s,
                          distance_map(&distance_s[0])
                              .predecessor_map(make_iterator_property_map(
                                  std::begin(predecessor), vertex_id, s)));
  auto edge_list = build_edge_list_from_dijkstra(G, s, t, predecessor);

  // Backward step
  auto rev_G = make_reverse_graph(G);
  dijkstra_shortest_paths(rev_G, t, distance_map(&distance_t[0]));

  if (exists_path_to<Length>(t, distance_s)) {
    return std::make_optional(edge_list);
  } else {
    // In case t could not be found from astar_search and target_found is not
    // thrown, return empty optional
    return std::optional<std::vector<Edge>>{};
  }

} // namespace kspwlo_impl

/**
 * Computes the Dijkstra shortest path from s to t using a
 *        penalty_functor to gather edges weight instead of the Graph's weight
 *        property map
 *
 * @tparam Graph A Boost::PropertyGraph having at least one edge
 *               property with tag boost::edge_weight_t.
 * @tparam PMap The graph's weight property map.
 * @tparam Vertex A vertex_descriptor.
 * @tparam Edge An edge_descriptor.
 * @tparam Length The edge weight type.
 * @param G The graph
 * @param s The source vertex
 * @param t The target vertex
 * @param penalty A penalty_functor
 * @return A vector of the edges of the shortest path from s to t.
 *         An empty optional if t is not reachable from s.
 */
template <typename Graph, typename PMap, typename Vertex = vertex_of_t<Graph>,
          typename Edge = edge_of_t<Graph>,
          typename Length = length_of_t<Graph>>
std::optional<std::vector<Edge>>
dijkstra_shortest_path(const Graph &G, Vertex s, Vertex t,
                       penalty_functor<PMap> &penalty) {
  using namespace boost;

  auto predecessor = std::vector<Vertex>(num_vertices(G), s);
  auto vertex_id = get(vertex_index, G);

  auto weight = make_function_property_map<Edge>(penalty);
  try {
    dijkstra_shortest_paths(
        G, s,
        weight_map(weight)
            .predecessor_map(make_iterator_property_map(std::begin(predecessor),
                                                        vertex_id, s))
            .visitor(make_dijkstra_visitor(
                make_target_visitor(t, on_examine_vertex{}))));
  } catch (target_found tf) {
    auto edge_list = build_edge_list_from_dijkstra(G, s, t, predecessor);
    return std::make_optional(edge_list);
  }

  // In case t could not be found from astar_search and target_found is not
  // thrown, return empty optional
  return std::optional<std::vector<Edge>>{};
}

template <typename Graph, typename PMap, typename AStarHeuristic,
          typename Vertex = vertex_of_t<Graph>,
          typename Edge = edge_of_t<Graph>,
          typename Length = length_of_t<Graph>>
std::optional<std::vector<Edge>>
astar_shortest_path(const Graph &G, Vertex s, Vertex t,
                    penalty_functor<PMap> &penalty,
                    const AStarHeuristic &heuristic) {
  using namespace boost;
  auto predecessor = std::vector<Vertex>(num_vertices(G), s);
  auto vertex_id = get(vertex_index, G);

  auto weight = make_function_property_map<Edge>(penalty);
  try {
    astar_search(G, s, heuristic,
                 predecessor_map(make_iterator_property_map(
                                     std::begin(predecessor), vertex_id, s))
                     .visitor(astar_target_visitor{t})
                     .weight_map(weight));
  } catch (target_found &tf) {
    auto edge_list = build_edge_list_from_dijkstra(G, s, t, predecessor);
    return std::make_optional(edge_list);
  }
  // In case t could not be found from astar_search and target_found is not
  // thrown, return empty optional
  return std::optional<std::vector<Edge>>{};
}

template <typename Graph, typename PMap, typename Vertex = vertex_of_t<Graph>,
          typename Edge = edge_of_t<Graph>,
          typename Length = length_of_t<Graph>>
std::optional<std::vector<Edge>>
bidirectional_dijkstra_shortest_path(const Graph &G, Vertex s, Vertex t,
                                     penalty_functor<PMap> &penalty) {
  using namespace boost;

  auto index = get(vertex_index, G);
  auto predecessor_vec = std::vector<Vertex>(num_vertices(G), s);
  auto predecessor = make_iterator_property_map(predecessor_vec.begin(), index);
  auto distance_vec = std::vector<Length>(num_vertices(G));
  auto distance = make_iterator_property_map(distance_vec.begin(), index);
  auto weight = make_function_property_map<Edge>(penalty);

  auto rev_G = make_reverse_graph(G);
  auto rev_weight_ = reverse_penalty_functor(penalty, G, rev_G);
  using RevEdge = typename boost::graph_traits<
      boost::reverse_graph<Graph>>::edge_descriptor;
  auto rev_weight = make_function_property_map<RevEdge>(rev_weight_);
  auto rev_index = get(vertex_index, rev_G);

  try {
    bidirectional_dijkstra(G, s, t, predecessor, distance, weight, rev_G,
                           rev_weight, rev_index);
  } catch (details::target_not_found &) {
    // In case t could not be found return empty optional
    return std::optional<std::vector<Edge>>{};
  }

  auto edge_list = build_edge_list_from_dijkstra(G, s, t, predecessor);
  return std::make_optional(edge_list);
}

template <typename Graph, typename PMap, typename Vertex = vertex_of_t<Graph>,
          typename Edge = edge_of_t<Graph>>
constexpr std::function<std::optional<std::vector<Edge>>(
    const Graph &, Vertex, Vertex, penalty_functor<PMap> &)>
build_shortest_path_fn(routing_kernels algorithm, const Graph &, const PMap &) {
  switch (algorithm) {
  case routing_kernels::dijkstra:
    return [](const auto &G, auto s, auto t, auto &penalty) {
      return dijkstra_shortest_path(G, s, t, penalty);
    };
  case routing_kernels::bidirectional_dijkstra:
    return [](const auto &G, auto s, auto t, auto &penalty) {
      return bidirectional_dijkstra_shortest_path(G, s, t, penalty);
    };
  default:
    throw std::invalid_argument{
        "Invalid algorithm. Only [dijkstra|bidirectional_dijkstra] allowed."};
  }
}

template <typename Graph, typename PMap, typename AStarHeuristic,
          typename Vertex = vertex_of_t<Graph>,
          typename Edge = edge_of_t<Graph>>
constexpr std::function<std::optional<std::vector<Edge>>(
    const Graph &, Vertex, Vertex, penalty_functor<PMap> &)>
build_shortest_path_fn(routing_kernels algorithm, const Graph &, const PMap &,
                       const AStarHeuristic &heuristic) {
  switch (algorithm) {
  case routing_kernels::astar:
    return [&heuristic](const auto &G, auto s, auto t, auto &penalty) {
      return astar_shortest_path(G, s, t, penalty, heuristic);
    };
  default:
    throw std::invalid_argument{"Invalid algorithm. Only [astar] allowed."};
  }
}

/**
 * Apply penalization step to the candidate path.
 *
 * @pre For each vertex @c v in @p candidate @p distance_s contains the shortest
 *      path distance of @c v from @p s
 * @pre For each vertex @c v in @p candidate @p distance_t contains the shortest
 *      path distance of @c v from @p t
 * @post For each edge @c e in @p candidate, if <tt>penalty_bounds[e] <
 *       bound_limit</tt>, @c e is penalized in @p penalty according to the
 *       following formula: <tt>w(e)_new = w(e) + @p p * w(e)</tt>
 * @post For each edge @c e incoming to a vertex @c u in @p candidate, if
 *       <tt>penalty_bounds[e] < bound_limit</tt>, @c e is penalized in @p
 *       penalty according to the following formula:
 *       <tt>w(e)_new = w(e) + w(e) * (0.1 + @p r * @p distance_t[u] / @p
 *       distance_t[s])</tt>
 * @post For each edge @c e outgoing from a vertex @c v in @p candidate, if
 *       <tt>penalty_bounds[e] < bound_limit</tt>, @c e is penalized in @p
 *       penalty according to the following formula:
 *       <tt>w(e)_new = w(e) + w(e) * (0.1 + @p r * @p distance_s[v] / @p
 *       distance_s[t])</tt>
 *
 * @tparam Graph A Boost::PropertyGraph having at least one edge
 *               property with tag boost::edge_weight_t.
 * @tparam DistanceMap A DistanceMap.
 * @tparam PMap The graph's weight property map.
 * @tparam Vertex A vertex_descriptor.
 * @tparam Edge An edge_descriptor.
 * @tparam Length The edge weight type.
 * @param candidate The candidate path.
 * @param G The graph.
 * @param s The source vertex.
 * @param t The target vertex.
 * @param p The penalty factor for edges in the candidate path.
 * @param r The penalty factor for edges incoming and outgoing to/from vertices
 *          of the candidate path.
 * @param penalty A penalty_functor.
 * @param distance_s A DistanceMap from s.
 * @param distance_t A DistanceMap from t.
 * @param penalty_bounds A PenBoundsMap.
 * @param bound_limit The maximum number of times an edge can be penalized.
 */
template <typename Graph, typename DistanceMap, typename PMap,
          typename Vertex = vertex_of_t<Graph>,
          typename Edge = edge_of_t<Graph>,
          typename Length = length_of_t<Graph>>
void penalize_candidate_path(const std::vector<Edge> &candidate, const Graph &G,
                             Vertex s, Vertex t, double p, double r,
                             penalty_functor<PMap> &penalty,
                             const DistanceMap &distance_s,
                             const DistanceMap &distance_t,
                             PenBoundsMap<Edge> &penalty_bounds,
                             int bound_limit) {
  using namespace boost;

  // Keep track of candidate vertices to exclude them from incoming/outgoing
  // edges update
  auto candidate_vertices = std::unordered_set<Vertex, boost::hash<Vertex>>{};
  auto candidate_edges = std::unordered_set<Edge, boost::hash<Edge>>{};

  for (const auto &e : candidate) {
    candidate_edges.insert(e);

    auto u = source(e, G);
    auto v = target(e, G);
    candidate_vertices.insert(u);
    candidate_vertices.insert(v);
  }

  for (auto &e : candidate_edges) {
    auto u = source(e, G);
    auto v = target(e, G);

    // Check if 'e' is already part of the alternative graph
    if (auto search = penalty_bounds.find(e);
        search != std::end(penalty_bounds)) {
      // If so, penalize only if limit isnt reached
      auto n_updates = search->second;
      if (n_updates < bound_limit) {
        penalty[e] += p * penalty[e];
        ++penalty_bounds[e];
      }
    } else { // Penalize and create a nb_updates counter for e
      penalty[e] += p * penalty[e];
      penalty_bounds.insert({e, 1});
    }

    // Update incoming edges
    for (auto [it, end] = in_edges(u, G); it != end; ++it) {
      auto a = source(*it, G);

      // Incoming edge (a, u) is updated only if 'a' is not part of candidate
      // path
      if (candidate_vertices.find(a) == std::end(candidate_vertices)) {
        // Check if '*it' is already part of the alternative graph
        if (auto search = penalty_bounds.find(*it);
            search != std::end(penalty_bounds)) {
          auto n_updates = search->second;
          // penalize only if limit isnt reached
          if (n_updates < bound_limit) {
            auto closeness = distance_t[u] / distance_t[s];
            auto pen_factor = 0.1 + r * closeness;
            penalty[*it] += pen_factor * penalty[*it];
            ++penalty_bounds[*it];
          }
        } else {
          // Else, just update it
          auto closeness = distance_t[u] / distance_t[s];
          auto pen_factor = 0.1 + r * closeness;
          penalty[*it] += pen_factor * penalty[*it];
        }
      }
    }

    // Update outgoing edges
    for (auto [it, end] = out_edges(v, G); it != end; ++it) {
      auto b = target(*it, G);

      // Outgoing edge (v, b) is updated only if 'b' is not part of candidate
      // path
      if (candidate_vertices.find(b) == std::end(candidate_vertices)) {
        // Check if '*it' is already part of the alternative graph
        if (auto search = penalty_bounds.find(*it);
            search != std::end(penalty_bounds)) {
          auto n_updates = search->second;
          // penalize only if limit isnt reached
          if (n_updates < bound_limit) {
            auto closeness = distance_s[v] / distance_s[t];
            auto pen_factor = 0.1 + r * closeness;
            penalty[*it] += pen_factor * penalty[*it];
            ++penalty_bounds[*it];
          }
        } else {
          // Else, just update it
          auto closeness = distance_s[v] / distance_s[t];
          auto pen_factor = 0.1 + r * closeness;
          penalty[*it] += pen_factor * penalty[*it];
        }
      }
    }
  }
}

template <typename Graph, typename WeightMap, typename MultiPredecessorMap,
          typename RoutingKernel, typename Terminator,
          typename Vertex = vertex_of_t<Graph>>
void penalty(const Graph &G, WeightMap const &original_weight,
             MultiPredecessorMap &predecessors, Vertex s, Vertex t, int k,
             double theta, double p, double r, int max_nb_updates,
             int max_nb_steps, RoutingKernel &routing_kernel,
             Terminator &&terminator) {
  using namespace boost;
  using Edge = typename graph_traits<Graph>::edge_descriptor;
  using Length = typename boost::property_traits<typename boost::property_map<
      Graph, boost::edge_weight_t>::type>::value_type;

  BOOST_CONCEPT_ASSERT((VertexAndEdgeListGraphConcept<Graph>));
  BOOST_CONCEPT_ASSERT((LvaluePropertyMapConcept<WeightMap, Edge>));

  // P_LO set of k paths
  auto resPathsEdges = std::vector<std::vector<Edge>>{};
  auto resEdges = std::vector<std::unordered_set<Edge, boost::hash<Edge>>>{};

  // P_LO set of k paths
  auto resPaths = std::vector<Path<Graph>>{};
  // Make a local weight map to avoid modifying existing graph.
  auto pen_fctor = details::penalty_functor{original_weight};

  // Compute shortest path from s to t
  auto distance_s = std::vector<Length>(num_vertices(G));
  auto distance_t = std::vector<Length>(num_vertices(G));
  auto sp = dijkstra_shortest_path_two_ways(G, s, t, distance_s, distance_t);
  if (!sp) {
    auto oss = std::ostringstream{};
    oss << "Vertex " << t << " is unreachable from " << s;
    throw details::target_not_found{oss.str()};
  }

  // P_LO <-- {shortest path p_0(s, t)};
  resPathsEdges.push_back(*sp);
  resEdges.emplace_back(sp->begin(), sp->end());

  // If we need the shortest path only
  if (k == 1) {
    fill_multi_predecessor(resPathsEdges.begin(), resPathsEdges.end(), G,
                           predecessors);
    return;
  }

  // Initialize map for penalty bounds
  auto penalty_bounds = std::unordered_map<Edge, int, boost::hash<Edge>>{};

  // Penalize sp edges
  penalize_candidate_path(*sp, G, s, t, p, r, pen_fctor, distance_s, distance_t,
                          penalty_bounds, max_nb_updates);

  int step = 0;
  using Index = std::size_t;
  while (resPathsEdges.size() < static_cast<Index>(k) && step < max_nb_steps) {
    // The remainder code is the hot part of the algorithm. So we check here
    // if the algorithm should terminate
    if (terminator.should_stop()) {
      throw terminator_stop_error{
          "Penalty terminated before completing due to a Terminator. Please "
          "discard partial output."};
    }

    auto p_tmp = routing_kernel(G, s, t, pen_fctor);

    // Penalize p_tmp edges
    penalize_candidate_path(*p_tmp, G, s, t, p, r, pen_fctor, distance_s,
                            distance_t, penalty_bounds, max_nb_updates);
    ++step;

    // If p_tmp is sufficiently dissimilar to other alternative paths, accept it
    bool is_valid_path = true;
    for (const auto &alt_path : resEdges) {
      if (compute_similarity(*p_tmp, alt_path, original_weight) > theta) {
        is_valid_path = false;
        break;
      }
    }

    if (is_valid_path) {
      resPathsEdges.push_back(*p_tmp);
      resEdges.emplace_back(p_tmp->begin(), p_tmp->end());
    }
  }

  // Beforer returning, populate predecessors map
  fill_multi_predecessor(resPathsEdges.begin(), resPathsEdges.end(), G,
                         predecessors);
}
} // namespace details
} // namespace arlib

#endif


#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;
}


/*********************************************************************
* 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);
}


/* 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:


/*
 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 */