#pragma once

#include <rai/lib/blocks.hpp>
#include <rai/node/utility.hpp>

#include <boost/property_tree/ptree.hpp>

#include <unordered_map>

#include <blake2/blake2.h>

namespace boost
{
template <>
struct hash<rai::uint256_union>
{
	size_t operator() (rai::uint256_union const & value_a) const
	{
		std::hash<rai::uint256_union> hash;
		return hash (value_a);
	}
};
}
namespace rai
{
class block_store;
/**
 * Determine the balance as of this block
 */
class balance_visitor : public rai::block_visitor
{
public:
	balance_visitor (MDB_txn *, rai::block_store &);
	virtual ~balance_visitor () = default;
	void compute (rai::block_hash const &);
	void send_block (rai::send_block const &) override;
	void receive_block (rai::receive_block const &) override;
	void open_block (rai::open_block const &) override;
	void change_block (rai::change_block const &) override;
	MDB_txn * transaction;
	rai::block_store & store;
	rai::block_hash current;
	rai::uint128_t result;
};

/**
 * Determine the amount delta resultant from this block
 */
class amount_visitor : public rai::block_visitor
{
public:
	amount_visitor (MDB_txn *, rai::block_store &);
	virtual ~amount_visitor () = default;
	void compute (rai::block_hash const &);
	void send_block (rai::send_block const &) override;
	void receive_block (rai::receive_block const &) override;
	void open_block (rai::open_block const &) override;
	void change_block (rai::change_block const &) override;
	void from_send (rai::block_hash const &);
	MDB_txn * transaction;
	rai::block_store & store;
	rai::uint128_t result;
};

/**
 * Determine the representative for this block
 */
class representative_visitor : public rai::block_visitor
{
public:
	representative_visitor (MDB_txn * transaction_a, rai::block_store & store_a);
	virtual ~representative_visitor () = default;
	void compute (rai::block_hash const & hash_a);
	void send_block (rai::send_block const & block_a) override;
	void receive_block (rai::receive_block const & block_a) override;
	void open_block (rai::open_block const & block_a) override;
	void change_block (rai::change_block const & block_a) override;
	MDB_txn * transaction;
	rai::block_store & store;
	rai::block_hash current;
	rai::block_hash result;
};

/**
 * A key pair. The private key is generated from the random pool, or passed in
 * as a hex string. The public key is derived using ed25519.
 */
class keypair
{
public:
	keypair ();
	keypair (std::string const &);
	rai::public_key pub;
	rai::raw_key prv;
};

std::unique_ptr<rai::block> deserialize_block (MDB_val const &);

/**
 * Latest information about an account
 */
class account_info
{
public:
	account_info ();
	account_info (MDB_val const &);
	account_info (rai::account_info const &) = default;
	account_info (rai::block_hash const &, rai::block_hash const &, rai::block_hash const &, rai::amount const &, uint64_t, uint64_t);
	void serialize (rai::stream &) const;
	bool deserialize (rai::stream &);
	bool operator== (rai::account_info const &) const;
	bool operator!= (rai::account_info const &) const;
	rai::mdb_val val () const;
	rai::block_hash head;
	rai::block_hash rep_block;
	rai::block_hash open_block;
	rai::amount balance;
	/** Seconds since posix epoch */
	uint64_t modified;
	uint64_t block_count;
};

/**
 * Information on an uncollected send, source account, amount, target account.
 */
class pending_info
{
public:
	pending_info ();
	pending_info (MDB_val const &);
	pending_info (rai::account const &, rai::amount const &);
	void serialize (rai::stream &) const;
	bool deserialize (rai::stream &);
	bool operator== (rai::pending_info const &) const;
	rai::mdb_val val () const;
	rai::account source;
	rai::amount amount;
};
class pending_key
{
public:
	pending_key (rai::account const &, rai::block_hash const &);
	pending_key (MDB_val const &);
	void serialize (rai::stream &) const;
	bool deserialize (rai::stream &);
	bool operator== (rai::pending_key const &) const;
	rai::mdb_val val () const;
	rai::account account;
	rai::block_hash hash;
};
class block_info
{
public:
	block_info ();
	block_info (MDB_val const &);
	block_info (rai::account const &, rai::amount const &);
	void serialize (rai::stream &) const;
	bool deserialize (rai::stream &);
	bool operator== (rai::block_info const &) const;
	rai::mdb_val val () const;
	rai::account account;
	rai::amount balance;
};
class block_counts
{
public:
	block_counts ();
	size_t sum ();
	size_t send;
	size_t receive;
	size_t open;
	size_t change;
};
class vote
{
public:
	vote () = default;
	vote (rai::vote const &);
	vote (bool &, rai::stream &);
	vote (bool &, rai::stream &, rai::block_type);
	vote (rai::account const &, rai::raw_key const &, uint64_t, std::shared_ptr<rai::block>);
	vote (MDB_val const &);
	rai::uint256_union hash () const;
	bool operator== (rai::vote const &) const;
	bool operator!= (rai::vote const &) const;
	void serialize (rai::stream &, rai::block_type);
	void serialize (rai::stream &);
	std::string to_json () const;
	// Vote round sequence number
	uint64_t sequence;
	std::shared_ptr<rai::block> block;
	// Account that's voting
	rai::account account;
	// Signature of sequence + block hash
	rai::signature signature;
};
enum class vote_code
{
	invalid, // Vote is not signed correctly
	replay, // Vote does not have the highest sequence number, it's a replay
	vote // Vote has the highest sequence number
};
class vote_result
{
public:
	rai::vote_code code;
	std::shared_ptr<rai::vote> vote;
};

enum class process_result
{
	progress, // Hasn't been seen before, signed correctly
	bad_signature, // Signature was bad, forged or transmission error
	old, // Already seen and was valid
	negative_spend, // Malicious attempt to spend a negative amount
	fork, // Malicious fork based on previous
	unreceivable, // Source block doesn't exist or has already been received
	gap_previous, // Block marked as previous is unknown
	gap_source, // Block marked as source is unknown
	not_receive_from_send, // Receive does not have a send source
	account_mismatch, // Account number in open block doesn't match send destination
	opened_burn_account // The impossible happened, someone found the private key associated with the public key '0'.
};
class process_return
{
public:
	rai::process_result code;
	rai::account account;
	rai::amount amount;
	rai::account pending_account;
};
enum class tally_result
{
	vote,
	changed,
	confirm
};
class votes
{
public:
	votes (std::shared_ptr<rai::block>);
	rai::tally_result vote (std::shared_ptr<rai::vote>);
	// Root block of fork
	rai::block_hash id;
	// All votes received by account
	std::unordered_map<rai::account, std::shared_ptr<rai::block>> rep_votes;
};
extern rai::keypair const & zero_key;
extern rai::keypair const & test_genesis_key;
extern rai::account const & rai_test_account;
extern rai::account const & rai_beta_account;
extern rai::account const & rai_live_account;
extern std::string const & rai_test_genesis;
extern std::string const & rai_beta_genesis;
extern std::string const & rai_live_genesis;
extern std::string const & genesis_block;
extern rai::account const & genesis_account;
extern rai::account const & burn_account;
extern rai::uint128_t const & genesis_amount;
// A block hash that compares inequal to any real block hash
extern rai::block_hash const & not_a_block;
// An account number that compares inequal to any real account number
extern rai::block_hash const & not_an_account;
class genesis
{
public:
	explicit genesis ();
	void initialize (MDB_txn *, rai::block_store &) const;
	rai::block_hash hash () const;
	std::unique_ptr<rai::open_block> open;
};
}


// Copyright (c) 2020-2021 FRC Team 3512. All Rights Reserved.

#pragma once

#include <Eigen/Core>

namespace frc3512 {

/**
 * A base class for subsystem controllers.
 *
 * State, Inputs, and Outputs indices should be specified what they represent in
 * the derived class.
 *
 * @tparam States the number of state estimates in the state vector
 * @tparam Inputs the number of control inputs in the input vector
 * @tparam Outputs the number of local outputs in the output vector
 */
template <int States, int Inputs, int Outputs>
class ControllerBase {
public:
    ControllerBase() = default;

    /**
     * Move constructor.
     */
    ControllerBase(ControllerBase&&) = default;

    /**
     * Move assignment operator.
     */
    ControllerBase& operator=(ControllerBase&&) = default;

    virtual ~ControllerBase() = default;

    /**
     * Returns the current references.
     *
     * See the State class in the derived class for what each element
     * corresponds to.
     */
    const Eigen::Matrix<double, States, 1>& GetReferences() const {
        return m_r;
    }

    /**
     * Returns the control inputs.
     *
     * See the Input class in the derived class for what each element
     * corresponds to.
     */
    const Eigen::Matrix<double, Inputs, 1>& GetInputs() const { return m_u; }

    /**
     * Returns the next output of the controller.
     *
     * @param x The current state x.
     */
    virtual Eigen::Matrix<double, Inputs, 1> Calculate(
        const Eigen::Matrix<double, States, 1>& x) = 0;

    /**
     * Returns the next output of the controller.
     *
     * @param x The current state x.
     * @param r The next reference r.
     */
    Eigen::Matrix<double, Inputs, 1> Calculate(
        const Eigen::Matrix<double, States, 1>& x,
        const Eigen::Matrix<double, States, 1>& r) {
        m_nextR = r;
        Eigen::Matrix<double, Inputs, 1> u = Calculate(x);
        m_r = m_nextR;
        return u;
    }

protected:
    /**
     * Controller reference for current timestep.
     */
    Eigen::Matrix<double, States, 1> m_r =
        Eigen::Matrix<double, States, 1>::Zero();

    /**
     * Controller reference for next timestep.
     */
    Eigen::Matrix<double, States, 1> m_nextR =
        Eigen::Matrix<double, States, 1>::Zero();

    /**
     * Controller output.
     */
    Eigen::Matrix<double, Inputs, 1> m_u =
        Eigen::Matrix<double, Inputs, 1>::Zero();
};

}  // namespace frc3512


// Copyright (c) 2020 fortiss GmbH
//
// Authors: Julian Bernhard, Klemens Esterle, Patrick Hart and
// Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#include "bark/world/map/map_interface.hpp"
#include <math.h>
#include <boost/tokenizer.hpp>
#include <memory>
#include <random>

namespace bark {
namespace world {
namespace map {

using LineSegment = boost::geometry::model::segment<Point2d>;
bool MapInterface::interface_from_opendrive(
    const OpenDriveMapPtr& open_drive_map) {
  open_drive_map_ = open_drive_map;

  RoadgraphPtr roadgraph(new Roadgraph());
  roadgraph->Generate(open_drive_map);
  roadgraph_ = roadgraph;

  rtree_lane_.clear();
  for (auto& road : open_drive_map_->GetRoads()) {
    for (auto& lane_section : road.second->GetLaneSections()) {
      for (auto& lane : lane_section->GetLanes()) {
        if (lane.second->GetLanePosition() == 0) continue;
        LineSegment lane_segment(*lane.second->GetLine().begin(),
                                 *(lane.second->GetLine().end() - 1));
        rtree_lane_.insert(std::make_pair(lane_segment, lane.second));
      }
    }
  }

  bounding_box_ = open_drive_map_->BoundingBox();
  return true;
}

bool MapInterface::interface_from_csvtable(
  const std::string csvfile, double x_offset, double y_offset) {
  // Read map data
  std::ifstream file(csvfile);
  if (!file.is_open()) {
    LOG(ERROR) << "Error reading mapfile " << csvfile;
    return false;
  }
  std::vector<double> cx, cy, lx, ly, rx, ry;
  typedef boost::tokenizer<boost::escaped_list_separator<char>> Tokenizer;
  std::string line;
  std::vector<std::string> row;
  bool toprow = true;

  while (getline(file, line)) {
    if (!toprow) {
      Tokenizer tok(line);
      row.assign(tok.begin(), tok.end());
      cx.push_back(stod(row[1]) - x_offset);
      cy.push_back(stod(row[2]) - y_offset);
      rx.push_back(stod(row[3]) - x_offset);
      ry.push_back(stod(row[4]) - y_offset);
      lx.push_back(stod(row[5]) - x_offset);
      ly.push_back(stod(row[6]) - y_offset);
    } else {
      toprow = false;
    }
  }
  const int nr_points = cx.size();

  // Generate centerline
  using bark::geometry::Line;
  using bark::geometry::Point2d;
  Line centerline;
  for (int idx = 0; idx < nr_points; ++idx) {
    centerline.AddPoint(Point2d(cx[idx], cy[idx]));
  }

  // Generate road polygon
  using bark::geometry::Polygon;
  Polygon lanepoly;
  for (int idx = 0; idx < nr_points; ++idx) {
    lanepoly.AddPoint(Point2d(lx[idx], ly[idx]));
  }
  for (int idx = nr_points-1; idx >= 0; --idx) {  // reverse
    lanepoly.AddPoint(Point2d(rx[idx], ry[idx]));
  }
  // lanepoly.correct(); //@todo do we need this?

  // Generate Lane
  double speed = 30 / 3.6;
  int laneid = 0;
  XodrLanePtr xodrlane = std::make_shared<XodrLane>();
  xodrlane->SetId(laneid);
  xodrlane->SetLanePosition(-1);  //@todo how to assign? Type: XodrLanePosition
  // xodrlane->link_; //not needed
  xodrlane->SetLine(centerline);
  // xodrlane->junction_id_ //not needed
  xodrlane->SetIsInJunction(false);
  xodrlane->SetLaneType(XodrLaneType::DRIVING);
  xodrlane->SetDrivingDirection(XodrDrivingDirection::FORWARD);
  // xodrlane->road_mark_ // @todo do we need this to be assigned= Type:
  // XodrRoadMark
  xodrlane->SetSpeed(speed);
  // xodrlane->lane_count//@todo how to assign?

  LanePtr lane = std::make_shared<Lane>(xodrlane);
  // lane->left_lane_ = nullptr; //@todo do we have to assign?
  // lane->right_lane_ = nullptr; //@todo do we have to assign?
  // lane->next_lane_ = nullptr; //@todo do we have to assign?
  lane->center_line_ = centerline;
  lane->polygon_ = lanepoly;
  // Boundary left_boundary_; //@todo do we need this?
  // Boundary right_boundary_; //@todo do we need this?

  // Generate LaneCorridorPtr!
  LaneCorridorPtr lanecorridor = std::make_shared<LaneCorridor>();
  lanecorridor->lanes_[0] = lane;  // s_end: @todo what is the key? is zero ok??
  lanecorridor->center_line_ = centerline;
  lanecorridor->fine_center_line_ = centerline;
  lanecorridor->merged_polygon_ = lanepoly;
  // Line left_boundary_; //@todo do we need this?
  // Line right_boundary_; //@todo do we need this?

  // Generate PlanView
  // @todo if we need the planview make a constructor from line!
  PlanViewPtr xodrplanview = std::make_shared<PlanView>();

  // Generate XodrLaneSection
  double s = 0;
  XodrLaneSectionPtr xodrlanesection = std::make_shared<XodrLaneSection>(s);
  xodrlanesection->AddLane(lane);

  // Generate XodrRoad
  const int roadid = 0;
  XodrRoadPtr xodrroad = std::make_shared<XodrRoad>();
  xodrroad->SetId(roadid);
  xodrroad->SetName("dummy_name");
  // xodrroad->SetLink(); //not needed
  xodrroad->SetPlanView(xodrplanview);
  xodrroad->AddLaneSection({xodrlanesection});

  // Generate Road
  RoadPtr road = std::make_shared<Road>(xodrroad);
  road->next_road_ = nullptr;
  road->lanes_[laneid] = lane;

  // Generate road corridor
  RoadCorridorPtr rc = std::make_shared<RoadCorridor>();
  rc->roads_[roadid] = road;
  rc->road_polygon_ = lanepoly;
  rc->unique_lane_corridors_ = {lanecorridor};
  rc->road_ids_ = {roadid};
  rc->driving_direction_ = XodrDrivingDirection::FORWARD;
  rc->lane_corridors_[laneid] = lanecorridor;

  // Generate roadgraph
  RoadgraphPtr roadgraph(new Roadgraph());
  roadgraph->AddLane(roadid, xodrlane);
  bark::world::map::PolygonPtr lanepolyptr =
      std::make_shared<Polygon>(lanepoly);
  bool lanepoly_set = roadgraph->SetPolygonForVertexFromId(laneid, lanepolyptr);
  if (!lanepoly_set) {
    return false;
  }

  // Generate lane rtree
  rtree_lane_.clear();
  using LineSegment = boost::geometry::model::segment<Point2d>;
  LineSegment lane_segment(
      *centerline.begin(),
      *(centerline.end() - 1));  //@todo ERROR! should be the boundary here, but
                                 // which? I did not model the planview
  rtree_lane_.insert(std::make_pair(lane_segment, lane));

  // Generate bounding box
  boost::geometry::model::box<Point2d> box;
  boost::geometry::envelope(lanepoly.obj_, box);
  boost::geometry::correct(box);
  bounding_box_ = std::make_pair(
      Point2d(boost::geometry::get<boost::geometry::min_corner, 0>(box),
              bg::get<bg::min_corner, 1>(box)),
      Point2d(boost::geometry::get<boost::geometry::max_corner, 0>(box),
              bg::get<bg::max_corner, 1>(box)));

  // Generate (dummy) Open Drive Map
  OpenDriveMapPtr open_drive_map = std::make_shared<OpenDriveMap>();
  open_drive_map->AddRoad(xodrroad);

  // Generate map interface
  open_drive_map_ = open_drive_map;
  road_from_csvtable_ = true;
  roadgraph_ = roadgraph;
  // rtree_lane_ assigned above

  // TODO THIS IS A HACK!!!
  std::vector<XodrRoadId> road_ids = {static_cast<XodrRoadId>(roadid)};
  XodrDrivingDirection driving_direction = XodrDrivingDirection::FORWARD;
  std::size_t road_corridor_hash = RoadCorridor::GetHash(driving_direction, road_ids);

  road_corridors_[road_corridor_hash] = rc;
  // bounding_box_ assigned above
  return true;
}

bool MapInterface::FindNearestXodrLanes(const Point2d& point,
                                        const unsigned& num_lanes,
                                        std::vector<XodrLanePtr>& lanes,
                                        bool type_driving_only) const {
  std::vector<rtree_lane_value> results_n;
  if (type_driving_only) {
    rtree_lane_.query(
        boost::geometry::index::nearest(point, num_lanes) &&
            boost::geometry::index::satisfies(IsLaneType),  // NOLINT
        std::back_inserter(results_n));
  } else {
    rtree_lane_.query(boost::geometry::index::nearest(point, num_lanes),
                      std::back_inserter(results_n));
  }
  if (results_n.empty()) {
    return false;
  }
  lanes.clear();
  for (auto& result : results_n) {
    lanes.push_back(result.second);
  }
  return true;
}

XodrLanePtr MapInterface::FindXodrLane(const Point2d& point) const {
  XodrLanePtr lane;
  std::vector<XodrLanePtr> nearest_lanes;
  if (!FindNearestXodrLanes(point, num_points_nearest_lane_, nearest_lanes,
                            false)) {
    return nullptr;
  }
  for (auto& close_lane : nearest_lanes) {
    if (IsInXodrLane(point, close_lane->GetId())) {
      lane = close_lane;
      return lane;
    }
  }
  return nullptr;
}

bool MapInterface::IsInXodrLane(const Point2d& point, XodrLaneId id) const {
  std::pair<vertex_t, bool> v = roadgraph_->GetVertexByLaneId(id);
  if (v.second) {
    auto polygon = roadgraph_->GetLaneGraph()[v.first].polygon;
    if (!polygon) {
      // found vertex has no polygon
      return false;
    } else {
      // found vertex has a polygon
      bool point_in_polygon = bark::geometry::Collide(*polygon, point);
      if (point_in_polygon) {
        return true;
      } else {
        return false;
      }
    }
  } else {
    // no vertex found
    return false;
  }
}

std::vector<PathBoundaries> MapInterface::ComputeAllPathBoundaries(
    const std::vector<XodrLaneId>& lane_ids) const {
  std::vector<XodrLaneEdgeType> LANE_SUCCESSOR_EDGEs = {
      XodrLaneEdgeType::LANE_SUCCESSOR_EDGE};
  std::vector<std::vector<XodrLaneId>> all_paths =
      roadgraph_->FindAllPathsInSubgraph(LANE_SUCCESSOR_EDGEs, lane_ids);

  std::vector<PathBoundaries> all_path_boundaries;
  for (auto const& path : all_paths) {
    std::vector<std::pair<XodrLanePtr, XodrLanePtr>> path_boundaries;
    for (auto const& path_segment : path) {
      std::pair<XodrLanePtr, XodrLanePtr> lane_boundaries =
          roadgraph_->ComputeXodrLaneBoundaries(path_segment);
      path_boundaries.push_back(lane_boundaries);
    }
    all_path_boundaries.push_back(path_boundaries);
  }
  return all_path_boundaries;
}

std::pair<XodrLanePtr, bool> MapInterface::GetInnerNeighbor(
    const XodrLaneId lane_id) const {
  std::pair<XodrLaneId, bool> inner_neighbor =
      roadgraph_->GetInnerNeighbor(lane_id);
  if (inner_neighbor.second)
    return std::make_pair(roadgraph_->GetLanePtr(inner_neighbor.first), true);
  return std::make_pair(nullptr, false);
}

std::pair<XodrLanePtr, bool> MapInterface::GetOuterNeighbor(
    const XodrLaneId lane_id) const {
  std::pair<XodrLaneId, bool> outer_neighbor =
      roadgraph_->GetOuterNeighbor(lane_id);
  if (outer_neighbor.second)
    return std::make_pair(roadgraph_->GetLanePtr(outer_neighbor.first), true);
  return std::make_pair(nullptr, false);
}

std::vector<XodrLaneId> MapInterface::GetSuccessorLanes(
    const XodrLaneId lane_id) const {
  return roadgraph_->GetSuccessorLanes(lane_id);
}

void MapInterface::CalculateLaneCorridors(RoadCorridorPtr& road_corridor,
                                          const RoadPtr& road) {
  Lanes lanes = road->GetLanes();

  for (auto& lane : lanes) {
    // only add lane if it has not been added already
    if (road_corridor->GetLaneCorridor(lane.first) ||
        lane.second->GetLanePosition() == 0)
      continue;
    // only add if type is drivable
    if (lane.second->GetLaneType() != XodrLaneType::DRIVING) continue;

    LaneCorridorPtr lane_corridor = std::make_shared<LaneCorridor>();
    LanePtr current_lane = lane.second;
    double total_s = current_lane->GetCenterLine().Length();
    lane_corridor->SetCenterLine(current_lane->GetCenterLine());
    lane_corridor->SetFineCenterLine(current_lane->GetCenterLine());
    // lane_corridor->SetMergedPolygon(current_lane->GetPolygon());
    lane_corridor->SetLeftBoundary(current_lane->GetLeftBoundary().line_);
    lane_corridor->SetRightBoundary(current_lane->GetRightBoundary().line_);
    lane_corridor->SetLane(total_s, current_lane);

    // add initial lane
    road_corridor->SetLaneCorridor(current_lane->GetId(), lane_corridor);
    // std::cout << "Current Poly" << std::endl;
    // std::cout << current_lane->GetPolygon().ToArray() << std::endl;
    LanePtr next_lane = current_lane;
    for (;;) {
      next_lane = next_lane->GetNextLane();
      if (next_lane == nullptr) break;
      Line new_center = bark::geometry::ConcatenateLinestring(
          lane_corridor->GetCenterLine(), next_lane->GetCenterLine());
      lane_corridor->SetCenterLine(new_center);
      lane_corridor->SetFineCenterLine(new_center);

      Line new_left = bark::geometry::ConcatenateLinestring(
          lane_corridor->GetLeftBoundary(), next_lane->GetLeftBoundary().line_);
      lane_corridor->SetLeftBoundary(new_left);

      Line new_right = bark::geometry::ConcatenateLinestring(
          lane_corridor->GetRightBoundary(),
          next_lane->GetRightBoundary().line_);
      lane_corridor->SetRightBoundary(new_right);
      // std::cout << "New Poly" << std::endl;
      // std::cout << next_lane->GetPolygon().ToArray() << std::endl;
      // lane_corridor->GetMergedPolygon().ConcatenatePolygons(
      //   next_lane->GetPolygon());

      total_s = lane_corridor->GetCenterLine().Length();
      lane_corridor->SetLane(total_s, next_lane);
      // all following lanes should point to the same LaneCorridor
      road_corridor->SetLaneCorridor(next_lane->GetId(), lane_corridor);
    }

    // \note \kessler: setting max_simplification_dist_ too small can yield
    // self-intersecting road polygons. why? in curves on the inner radius due
    // to sampling inaccuracy the boundaries of two segments can overlap. The
    // code below just copies each point to the lane polygon without checking
    // this.

    Line simplf_center =
        Simplify(lane_corridor->GetCenterLine(), max_simplification_dist_);
    lane_corridor->SetCenterLine(simplf_center);

    Line simplf_fine_center =
        Simplify(lane_corridor->GetFineCenterLine(), 0.001);
    lane_corridor->SetFineCenterLine(simplf_fine_center);

    Line simplf_right =
        Simplify(lane_corridor->GetRightBoundary(), max_simplification_dist_);
    lane_corridor->SetRightBoundary(simplf_right);

    Line simplf_left =
        Simplify(lane_corridor->GetLeftBoundary(), max_simplification_dist_);
    lane_corridor->SetLeftBoundary(simplf_left);

    // TODO hier ist der self intersection bug: magic number 0.5 durch sampling
    // distance ersetzen!!!

    // merged polygons
    PolygonPtr polygon = std::make_shared<bark::geometry::Polygon>();

    // \todo (\kessler): before adding a point, check if this produces a
    // self-intersection.
    for (auto const& p : lane_corridor->GetLeftBoundary()) {
      polygon->AddPoint(p);
    }
    auto reversed_outer = lane_corridor->GetRightBoundary();
    reversed_outer.Reverse();
    for (auto const& p : reversed_outer) {
      polygon->AddPoint(p);
    }
    // Polygons need to be closed!
    polygon->AddPoint(*(lane_corridor->GetLeftBoundary().begin()));
    boost::geometry::correct(polygon->obj_);

    if (!polygon->Valid()) {
      LOG(ERROR) << "Producing a non-valid lane corridor for lane id = "
                 << lane.first;
    }

    lane_corridor->SetMergedPolygon(*polygon);
  }
}

void MapInterface::CalculateLaneCorridors(RoadCorridorPtr& road_corridor,
                                          const XodrRoadId& road_id) {
  RoadPtr first_road = road_corridor->GetRoads()[road_id];
  CalculateLaneCorridors(road_corridor, first_road);
  for (auto& road : road_corridor->GetRoads()) {
    CalculateLaneCorridors(road_corridor, road.second);
  }
}

LanePtr MapInterface::GenerateRoadCorridorLane(const XodrLanePtr& xodr_lane) {
  LanePtr lane = std::make_shared<Lane>(xodr_lane);
  // polygons
  if (xodr_lane->GetLanePosition() != 0) {
    std::pair<PolygonPtr, bool> polygon_success =
        roadgraph_->ComputeXodrLanePolygon(xodr_lane->GetId());
    // Cannot compute polygon for planview!!
    if (polygon_success.second) lane->SetPolygon(*polygon_success.first);
  }
  return lane;
}

RoadPtr MapInterface::GenerateRoadCorridorRoad(const XodrRoadId& road_id) {
  XodrRoadPtr xodr_road = open_drive_map_->GetRoad(road_id);
  RoadPtr road = std::make_shared<Road>(xodr_road);
  Lanes lanes;
  for (auto& lane_section : xodr_road->GetLaneSections()) {
    for (auto& lane : lane_section->GetLanes()) {
      // TODO(@hart): only add driving lanes
      // if (lane.second->GetLaneType() == XodrLaneType::DRIVING)
      lanes[lane.first] = GenerateRoadCorridorLane(lane.second);
    }
  }
  road->SetLanes(lanes);
  return road;
}

void MapInterface::GenerateRoadCorridor(
    const std::vector<XodrRoadId>& road_ids,
    const XodrDrivingDirection& driving_direction) {
  std::size_t road_corridor_hash =
      RoadCorridor::GetHash(driving_direction, road_ids);

  // only compute if it has not been computed yet
  if (road_corridors_.count(road_corridor_hash) > 0) return;

  Roads roads;
  for (auto& road_id : road_ids)
    roads[road_id] = GenerateRoadCorridorRoad(road_id);

  // links can only be set once all roads have been calculated
  int count = 0;
  for (auto& road_id : road_ids) {
    // road successor
    RoadPtr next_road;
    if (count < road_ids.size() - 1) next_road = roads[road_ids[++count]];
    roads[road_id]->SetNextRoad(next_road);
    for (auto& lane : roads[road_id]->GetLanes()) {
      // lane successor
      // auto local_driving_direction = driving_direction;
      // if (lane.second->GetDrivingDirection() != driving_direction)
      //   local_driving_direction = lane.second->GetDrivingDirection();

      if (count + 1 <= road_ids.size()) {
        std::vector<XodrRoadId> vec;
        std::copy(road_ids.begin() + count, road_ids.begin() + count + 1,
                  std::back_inserter(vec));

        // for (auto v: vec)
        //   std::cout << v << std::endl;

        std::pair<XodrLaneId, bool> next_lane =
            roadgraph_->GetNextLane(vec, lane.first);
        if (next_lane.second && next_road)
          lane.second->SetNextLane(next_road->GetLane(next_lane.first));
      }

      // left and right lanes
      LanePtr left_lane, right_lane;
      std::pair<XodrLaneId, bool> left_lane_id = roadgraph_->GetLeftLane(
          lane.first, lane.second->GetDrivingDirection());
      if (left_lane_id.second) {
        left_lane = roads[road_id]->GetLane(left_lane_id.first);
        lane.second->SetLeftLane(left_lane);
      }

      std::pair<XodrLaneId, bool> right_lane_id = roadgraph_->GetRightLane(
          lane.first, lane.second->GetDrivingDirection());
      if (right_lane_id.second) {
        right_lane = roads[road_id]->GetLane(right_lane_id.first);
        lane.second->SetRightLane(right_lane);
      }

      // set boundaries for lane
      std::pair<XodrLaneId, bool> left_boundary_lane_id =
          roadgraph_->GetLeftBoundary(lane.first,
                                      lane.second->GetDrivingDirection());
      if (left_boundary_lane_id.second) {
        LanePtr left_lane_boundary =
            roads[road_id]->GetLane(left_boundary_lane_id.first);
        Boundary left_bound;
        left_bound.SetLine(left_lane_boundary->GetLine());
        left_bound.SetType(left_lane_boundary->GetRoad_mark());
        lane.second->SetLeftBoundary(left_bound);
      }
      std::pair<XodrLaneId, bool> right_boundary_lane_id =
          roadgraph_->GetRightBoundary(lane.first,
                                       lane.second->GetDrivingDirection());
      if (right_boundary_lane_id.second) {
        LanePtr right_lane_boundary =
            roads[road_id]->GetLane(right_boundary_lane_id.first);
        Boundary right_bound;
        right_bound.SetLine(right_lane_boundary->GetLine());
        right_bound.SetType(right_lane_boundary->GetRoad_mark());
        lane.second->SetRightBoundary(right_bound);
      }

      // compute center line
      if (left_boundary_lane_id.second && right_boundary_lane_id.second)
        lane.second->SetCenterLine(
            ComputeCenterLine(lane.second->GetLeftBoundary().line_,
                              lane.second->GetRightBoundary().line_));
    }
  }

  if (roads.size() == 0) return;
  RoadCorridorPtr road_corridor = std::make_shared<RoadCorridor>();
  road_corridor->SetRoads(roads);
  CalculateLaneCorridors(road_corridor, road_ids[0]);
  road_corridor->ComputeRoadPolygon();
  if (full_junction_area_) {
    for (auto& junction : road_corridor->GetJunctionIds()) {
      const Polygon poly = ComputeJunctionArea(junction);
      road_corridor->AddPolygonToRoadCorridor(std::move(poly));
    }
  }
  road_corridor->SetRoadIds(road_ids);
  road_corridor->SetDrivingDirection(driving_direction);
  road_corridors_[road_corridor_hash] = road_corridor;
}

RoadCorridorPtr MapInterface::GenerateRoadCorridor(
    const bark::geometry::Point2d& start_point,
    const bark::geometry::Polygon& goal_region) {
  std::vector<XodrLanePtr> lanes;
  XodrLaneId goal_lane_id;
  bool nearest_start_lane_found = FindNearestXodrLanes(start_point, 1, lanes);
  bool nearest_goal_lane_found = XodrLaneIdAtPolygon(goal_region, goal_lane_id);
  if (!nearest_start_lane_found || !nearest_goal_lane_found) {
    LOG(INFO) << "Could not generate road corridor based on geometric start "
                 "and goal definitions.";  // NOLINT
    return nullptr;
  }

  const auto start_lane_id = lanes.at(0)->GetId();
  const XodrDrivingDirection driving_direction =
      lanes.at(0)->GetDrivingDirection();

  std::vector<XodrRoadId> road_ids;
  std::vector<XodrLaneId> lane_ids =
      roadgraph_->FindDrivableLanePath(start_lane_id, goal_lane_id);
  for (auto lid : lane_ids) {
    std::pair<vertex_t, bool> v_des = roadgraph_->GetVertexByLaneId(lid);
    XodrLaneVertex lv = roadgraph_->GetVertex(v_des.first);
    road_ids.push_back(lv.road_id);
    // std::cout << "lane_id: " << lid << ", road_id: ";
    // std::cout << lv.road_id << std::endl;
  }
  GenerateRoadCorridor(road_ids, driving_direction);
  return GetRoadCorridor(road_ids, driving_direction);
}

RoadCorridorPtr MapInterface::GenerateRoadCorridor(
    const XodrRoadId& start_road_id, const XodrRoadId& end_road_id) {
  std::vector<XodrRoadId> road_ids =
      roadgraph_->FindRoadPath(start_road_id, end_road_id);

  std::pair<std::vector<XodrDrivingDirection>, bool> directions =
      roadgraph_->GetDrivingDirectionsForRoadId(start_road_id);
  if (!directions.second) {
    LOG(ERROR) << "No lanes for start road id " << start_road_id << " found.";
    return nullptr;
  }
  XodrDrivingDirection driving_direction = directions.first.at(0);
  GenerateRoadCorridor(road_ids, driving_direction);
  return GetRoadCorridor(road_ids, driving_direction);
}

bool MapInterface::XodrLaneIdAtPolygon(const bark::geometry::Polygon& polygon,
                                       XodrLaneId& found_lane_id) const {
  bark::geometry::Point2d goal_center(polygon.center_(0), polygon.center_(1));
  std::vector<opendrive::XodrLanePtr> nearest_lanes;
  if (FindNearestXodrLanes(goal_center, 1, nearest_lanes)) {
    found_lane_id = nearest_lanes[0]->GetId();
    return true;
  }
  LOG(INFO) << "No matching lane for goal definition found";
  return false;
}

RoadPtr MapInterface::GetNextRoad(
    const XodrRoadId& current_road_id, const Roads& roads,
    const std::vector<XodrRoadId>& road_ids) const {
  auto it = std::find(road_ids.begin(), road_ids.end(), current_road_id);
  if (road_ids.back() == current_road_id) return nullptr;
  return roads.at(*std::next(it, 1));
}

bark::geometry::Polygon MapInterface::ComputeJunctionArea(
    uint32_t junction_id) {
  PolygonPtr poly = roadgraph_->ComputeJunctionArea(junction_id);
  return *poly.get();
}

}  // namespace map
}  // namespace world
}  // namespace bark


//   Copyright 2015 Patrick Putnam
//
//   Licensed under the Apache License, Version 2.0 (the "License");
//   you may not use this file except in compliance with the License.
//   You may obtain a copy of the License at
//
//       http://www.apache.org/licenses/LICENSE-2.0
//
//   Unless required by applicable law or agreed to in writing, software
//   distributed under the License is distributed on an "AS IS" BASIS,
//   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//   See the License for the specific language governing permissions and
//   limitations under the License.
#ifndef CLOTHO_SIM_ENGINE_MT_HPP_
#define CLOTHO_SIM_ENGINE_MT_HPP_

#ifdef DEBUG_MODE
#define DEBUGGING 0
#endif  // DEBUG_MODE

#include "qtlsim_logger.hpp"

#include <boost/property_tree/ptree.hpp>

#include "clotho/genetics/population_growth_toolkit.hpp"

#include "clotho/data_spaces/allele_space/allele_space_vector.hpp"
#include "clotho/data_spaces/allele_space/allele_generator_vector.hpp"

#include "clotho/data_spaces/phenotype_evaluator/trait_space_vector.hpp"
#include "clotho/data_spaces/phenotype_evaluator/trait_space_generator.hpp"

#include "clotho/data_spaces/phenotype_evaluator/trait_accumulator.hpp"
#include "clotho/data_spaces/free_space/free_space_mts.hpp"

#ifdef USE_BATCH_JOBS
#include "clotho/data_spaces/crossover/batch_crossover_mt.hpp"
#define CROSSOVER_TYPE clotho::genetics::BatchCrossoverMT

#include "clotho/data_spaces/phenotype_evaluator/batch_phenotype_mts.hpp"
#define PHENOTYPE_TYPE clotho::genetics::BatchPhenotypeMT

#else
#include "clotho/data_spaces/crossover/crossover_mt.hpp"
#define CROSSOVER_TYPE clotho::genetics::CrossoverMT

#include "clotho/data_spaces/phenotype_evaluator/phenotype_mt.hpp"
#define PHENOTYPE_TYPE clotho::genetics::PhenotypeMT
#endif  // USE_BATCH_JOBS

#include "clotho/data_spaces/population_space/population_spaces.hpp"
#include "clotho/data_spaces/selection/selection.hpp"

#include "clotho/data_spaces/mutation/mutation_generators.hpp"
#include "clotho/data_spaces/mutation/mutation_allocator.hpp"

#include "clotho/data_spaces/fitness/general_fitness.hpp"

#include "clotho/utility/state_object.hpp"

#include "clotho/data_spaces/task/thread_pool.hpp"

template < class RNG, class RealType = double, class BlockType = unsigned long long, class SizeType = unsigned int >
class EngineMT2 {
public:
    typedef EngineMT2< RNG >             self_type;

    typedef RealType                    position_type;
    typedef RealType                    weight_type;
    typedef weight_type *               phenotype_type;
    typedef BlockType                   block_type;

    typedef RNG                         random_engine_type;

    typedef SizeType                    size_type;

    typedef clotho::genetics::thread_pool< RNG >                                                    thread_pool_type;
    typedef clotho::genetics::AlleleSpace< position_type, size_type >                               allele_type;
//    typedef clotho::genetics::association_matrix< block_type, __ALIGNMENT_TYPE__ >                  sequence_space_type;
//
    typedef clotho::genetics::population_space< block_type, weight_type >                           sequence_space_type;
    typedef clotho::genetics::trait_space_vector< weight_type >                                     trait_space_type;
    typedef clotho::genetics::FreeSpaceAnalyzerMT< sequence_space_type, size_type >                 free_space_type;

    typedef clotho::genetics::mutation_allocator< random_engine_type, size_type >                 mutation_alloc_type;

    typedef clotho::genetics::MutationGenerator2< random_engine_type, sequence_space_type >       mutation_type;

    typedef clotho::genetics::AlleleGenerator< random_engine_type, allele_type >                  allele_generator_type;
    typedef clotho::genetics::TraitSpaceGenerator< random_engine_type, trait_space_type >         trait_generator_type;

    typedef clotho::genetics::GeneralFitness                                                      fitness_type;
    typedef clotho::genetics::SelectionGenerator< random_engine_type, clotho::genetics::fitness_selection< fitness_type > >          selection_type;
    typedef CROSSOVER_TYPE< random_engine_type, sequence_space_type, allele_type >                crossover_type;

    typedef PHENOTYPE_TYPE< sequence_space_type, trait_space_type >                               phenotype_eval_type;

    typedef std::shared_ptr< ipopulation_growth_generator >                                         population_growth_generator_type;
    typedef std::shared_ptr< ipopulation_growth >                                                   population_growth_type;

    friend struct clotho::utility::state_getter< self_type >;

    EngineMT2( random_engine_type * rng, boost::property_tree::ptree & config ) :
        m_rand( rng )
        , m_parent( &m_pop0 )
        , m_child( &m_pop1 )
        , m_trait_space( config )
        , m_fixed_traits( config )
        , m_thread_pool( rng, config )
        , m_free_space( )
        , select_gen( rng, config )
        , mutate_gen( rng, config )
        , cross_gen( rng, config )
        , m_fit( config )
        , m_generation( 0 )
        , m_pop_growth()
        , m_mut_alloc( rng, config )
        , trait_gen( rng, config )
        , allele_gen( rng, config )
    {
        population_growth_generator_type tmp  = population_growth_toolkit::getInstance()->get_tool( config );
        if( tmp ) {
            m_pop_growth = tmp->generate();
            if( m_pop_growth ) {
                m_pop_growth->log( std::cerr );
                std::cerr << std::endl;
            }
        } else {
            population_growth_toolkit::getInstance()->tool_configurations( config );
        }

//        m_pop0.getSequenceSpace().clear();
//        m_pop1.getSequenceSpace().clear();

//        size_t acount = m_pop0.getAlleleSpace().allele_count();
//        m_pop0.getAlleleSpace().grow(acount, m_pheno.trait_count());
//        m_pop1.getAlleleSpace().grow(acount, m_pheno.trait_count());

        init(0);
    }

    size_t getGeneration() const {
        return m_generation;
    }

    void init( size_t aN ) {
//        size_t pN = m_parent->individual_count();
        size_t pN = 0;
        if( m_pop_growth ) {
            pN = m_pop_growth->operator()( pN, m_generation );
        }

        m_pop1.grow( pN, aN );

#ifdef USE_ROW_VECTOR
        m_pop1.getSequenceSpace().fill_empty();
        m_pop1.getSequenceSpace().finalize();
#endif // USE_ROW_VECTOR
        ++m_generation;
    }

    void simulate( ) {
        std::swap( m_child, m_parent );     // use the current child population as the parent population for the next round

        // at the start of each simulate round, m_fit has already been updated from the previous
        // round with the fitness of the "then child/now parent" popualtions fitness
        //
        size_t pN = select_gen.individual_count();
        if( m_pop_growth ) {
            pN = m_pop_growth->operator()( pN, m_generation );
        }

        size_type pM = m_mut_alloc.allocate( 2 * pN );        // generate the number of new mutations
        size_type free_count = updateFixedAlleles( m_parent );    // update the fixed alleles with those of parent population
        size_t all_size = child_max_alleles( m_allele_space.size(), free_count, pM );   // rescale allele space for child population given free space from parent population and new allele count (pM)

#ifdef DEBUGGING
        BOOST_LOG_TRIVIAL( debug ) << "Generation " << m_generation << ": " << pN << " individuals; " << pM << " new alleles";
        BOOST_LOG_TRIVIAL( debug ) << "Free space: " << free_count << "; alleles: " << m_allele_space.size();
        BOOST_LOG_TRIVIAL( debug ) << "Rescaling child population to be: " << pN << " individuals x " << all_size << " alleles";
        std::cerr << "Rescaling child population to be: " << pN << " individuals x " << all_size << " alleles" << std::endl;
#endif // DEBUGGING

        m_child->grow( pN, all_size );                        // grow the child population accordingly

        select_gen.update( m_fit, pN );

        cross_gen( select_gen, m_parent, m_child, &m_allele_space, m_thread_pool );

        generate_child_mutations( pM );

        if( !m_allele_space.isAllNeutral() ) {
            m_pheno( m_child, &m_trait_space, m_thread_pool );
        } else {
            m_pheno.constant_phenotype( m_child, &m_trait_space );
        }
        m_fit( m_pheno.begin(), m_pheno.end() );

        m_child->finalize();

        ++m_generation;
    }

    sequence_space_type * getChildPopulation() const {
        return m_child;
    }

    sequence_space_type * getParentPopulation() const {
        return m_parent;
    }

    allele_type * getAlleleSpace() {
        return &m_allele_space;
    }

    virtual ~EngineMT2() { }

protected:

    void generate_child_mutations( unsigned int N ) {
        std::cerr << "Child population size: " << m_child->haploid_genome_count() << std::endl;
        typename mutation_type::sequence_distribution_type seq_gen( 0, m_child->haploid_genome_count() - 1);

        typename free_space_type::base_type::iterator it = m_free_space.free_begin(), end = m_free_space.free_end();
        while( N && it != end ) {
            typename free_space_type::size_type all_idx = *it++;
            unsigned int seq_idx = seq_gen( *m_rand );

            mutate_gen( m_child, seq_idx, all_idx );
            allele_gen( m_allele_space, all_idx, m_generation );
            trait_gen( m_trait_space, all_idx );
            --N;
        }

        while( N ) {
            typename free_space_type::size_type all_idx = m_allele_space.size();
            unsigned int seq_idx = seq_gen( *m_rand );

            assert( all_idx < m_child->getMaxAlleles() );

            mutate_gen( m_child, seq_idx, all_idx );
            allele_gen( m_allele_space, all_idx, m_generation );
            trait_gen( m_trait_space, all_idx );
            --N;
        }
    }

/**
 * estimate the maximum number of alleles in the child
 *
 * N_parent - number of alleles in the parent population
 * F_parent - number of free alleles in the parent population
 * M_child  - number of new alleles to be added the child population
 */
    size_t child_max_alleles( size_t N_parent, size_t F_parent, size_t M_child ) const {
        BOOST_LOG_TRIVIAL(info) << "Parent alleles: " << N_parent << "; Free: " << F_parent << "; New Alleles: " << M_child;

        if( F_parent >= M_child ) {
            // if there are more free alleles in the parent generation
            // than there are new alleles to be added to the child generation
            // then do not adjust scale of the allele space
            return N_parent;
        } else {
            return N_parent + (M_child - F_parent);
        }
    }

    size_type updateFixedAlleles( sequence_space_type * ss ) {
        m_free_space( ss, m_thread_pool );               // analyze the parent population sequence space

        typedef typename free_space_type::iterator  fixed_iterator;
        typedef typename trait_space_type::iterator trait_iterator;

        std::cerr << "Fixed count: " << m_free_space.fixed_size() << std::endl;

        fixed_iterator  fix_it = m_free_space.fixed_begin();
        fixed_iterator  fix_end = m_free_space.fixed_end();

        while( fix_it != fix_end ) {
            size_type  fixed_index = *fix_it++;

            ss->remove_fixed_allele( fixed_index );

            m_fixed.append( m_allele_space, fixed_index );

            trait_iterator tstart = m_trait_space.begin( fixed_index ), tend = m_trait_space.end( fixed_index );
            m_fixed_traits.append( tstart, tend );
        }

#ifdef DEBUGGING
        typedef typename free_space_type::iterator free_iterator;
        free_iterator fr_it = m_free_space.free_begin();
        free_iterator fr_end = m_free_space.free_end();

        unsigned int j = 0;
        while( fr_it != fr_end ) {
            size_type i = *fr_it++;

            if( !ss->freeColumn( i ) ) {
                assert(false);
            }
            ++j;
        }

        assert( j == m_free_space.free_size() );
#endif // DEBUGGING

        return m_free_space.free_size();
    }

    random_engine_type  * m_rand;

    allele_type             m_allele_space, m_fixed;
    sequence_space_type  m_pop0, m_pop1;
    sequence_space_type  * m_parent, * m_child;

    trait_space_type        m_trait_space, m_fixed_traits;

    thread_pool_type        m_thread_pool;
    phenotype_eval_type     m_pheno;
    free_space_type         m_free_space;

    selection_type          select_gen;
    mutation_type           mutate_gen;
    crossover_type          cross_gen;
    fitness_type            m_fit;

    size_t                  m_generation;

    population_growth_type  m_pop_growth;
    mutation_alloc_type     m_mut_alloc;

    trait_generator_type    trait_gen;
    allele_generator_type   allele_gen;
};

namespace clotho {
namespace utility {

template < class RNG, class RealType, class BlockType, class SizeType >
struct state_getter< EngineMT2< RNG, RealType, BlockType, SizeType > > {
    typedef EngineMT2< RNG, RealType, BlockType, SizeType >           object_type;

    void operator()( boost::property_tree::ptree & s, object_type & obj ) {
        boost::property_tree::ptree tr;
        state_getter< typename object_type::trait_space_type > tr_logger;
        tr_logger( tr, obj.m_trait_space );

        boost::property_tree::ptree ph;
        state_getter< typename object_type::phenotype_eval_type > pheno_logger;
        pheno_logger( ph, obj.m_pheno );

        boost::property_tree::ptree fr;
        state_getter< typename object_type::free_space_type > free_logger;
        free_logger( fr, obj.m_free_space );

        boost::property_tree::ptree fx, alls;
        state_getter< typename object_type::allele_type > all_logger;
        all_logger( fx, obj.m_fixed );
        all_logger( alls, obj.m_allele_space );

//        boost::property_tree::ptree c_pop;
//        state_getter< typename object_type::sequence_space_type > pop_logger;
//        pop_logger( c_pop, *(obj.m_child) );

        s.put_child( "phenotypes", ph );
        s.put_child( "free_space", fr );
        s.put_child( "allele_space", alls );
        s.put_child( "trait_space", tr );
        s.put_child( "fixed_alleles", fx );

//        s.put_child( "child", c_pop );
    }
};

}
}

#endif  // CLOTHO_SIM_ENGINE_MT_HPP_


#include <iostream>
#include <sstream>
#include <sys/resource.h>
#include <thread>
#include <algorithm>
//#include <boost/asio/thread_pool.hpp>
//#include <boost/asio/post.hpp>
//#include <boost/bind.hpp>
#include <mutex>
#include "kmercounter.hpp"
#include "jellyfishreader.hpp"
#include "jellyfishcounter.hpp"
#include "emissionprobabilitycomputer.hpp"
#include "copynumber.hpp"
#include "variantreader.hpp"
#include "uniquekmercomputer.hpp"
#include "hmm.hpp"
#include "commandlineparser.hpp"
#include "timer.hpp"
#include "threadpool.hpp"

using namespace std;

/** version of the main algorithm that uses uniform transition probabilities and genotypes only based on kmer information.**/

struct Results {
	mutex result_mutex;
	map<string, vector<GenotypingResult>> result;
	map<string, double> runtimes;
};

void run_genotyping_kmers(string chromosome, KmerCounter* genomic_kmer_counts, KmerCounter* read_kmer_counts, VariantReader* variant_reader, ProbabilityTable* probs, size_t kmer_abundance_peak, bool only_genotyping, bool only_phasing, Results* results) {
	Timer timer;
	// determine sets of kmers unique to each variant region
	UniqueKmerComputer kmer_computer(genomic_kmer_counts, read_kmer_counts, variant_reader, chromosome, kmer_abundance_peak);
	std::vector<UniqueKmers*> unique_kmers;
	kmer_computer.compute_unique_kmers(&unique_kmers, probs);
	// construct HMM and run genotyping/phasing
	HMM hmm(&unique_kmers, probs, !only_phasing, !only_genotyping, 1.26, true);
	// store the results
	{
		lock_guard<mutex> lock (results->result_mutex);
		results->result.insert(pair<string, vector<GenotypingResult>> (chromosome, move(hmm.get_genotyping_result())));
	}
	// destroy unique kmers
	for (size_t i = 0; i < unique_kmers.size(); ++i) {
		delete unique_kmers[i];
		unique_kmers[i] = nullptr;
	}
	lock_guard<mutex> lock (results->result_mutex);
	results->runtimes.insert(pair<string,double>(chromosome, timer.get_total_time()));
}

int main (int argc, char* argv[])
{
	Timer timer;
	double time_preprocessing;
	double time_kmer_counting;
	double time_writing;
	double time_total;

	cerr << endl;
	cerr << "program: PanGenie-kmers - genotyping based on kmer-counting." << endl;
	cerr << "author: Jana Ebler" << endl << endl;
	string readfile = "";
	string reffile = "";
	string vcffile = "";
	size_t kmersize = 31;
	string outname = "result";
	string sample_name = "sample";
	size_t nr_jellyfish_threads = 1;
	size_t nr_core_threads = 1;
	bool only_genotyping = false;
	bool only_phasing = false;
	long double regularization = 0.001L;;
	bool count_only_graph = true;
	bool ignore_imputed = false;

	// parse the command line arguments
	CommandLineParser argument_parser;
	argument_parser.add_command("PanGenie-kmers [options] -i <reads.fa/fq> -r <reference.fa> -v <variants.vcf>");
	argument_parser.add_mandatory_argument('i', "sequencing reads in FASTA/FASTQ format");
	argument_parser.add_mandatory_argument('r', "reference genome in FASTA format");
	argument_parser.add_mandatory_argument('v', "variants in VCF format");
	argument_parser.add_optional_argument('o', "result", "prefix of the output files");
	argument_parser.add_optional_argument('k', "31", "kmer size");
	argument_parser.add_optional_argument('s', "sample", "name of the sample (will be used in the output VCFs)");
	argument_parser.add_optional_argument('j', "1", "number of threads to use for kmer-counting");
	argument_parser.add_optional_argument('t', "1", "number of threads to use for core algorithm. Largest number of threads possible is the number of chromosomes given in the VCF");
	argument_parser.add_flag_argument('g', "only run genotyping (Forward backward algorithm)");
	argument_parser.add_flag_argument('p', "only run phasing (Viterbi algorithm)");
	argument_parser.add_optional_argument('m', "0.001", "regularization constant for copynumber probabilities");
	argument_parser.add_flag_argument('c', "count all kmers instead of only those located in graph..");
	argument_parser.add_flag_argument('u', "output genotype ./. for variants not covered by any unique kmers.");

	try {
		argument_parser.parse(argc, argv);
	} catch (const runtime_error& e) {
		argument_parser.usage();
		cerr << e.what() << endl;
		return 1;
	} catch (const exception& e) {
		return 0;
	}
	readfile = argument_parser.get_argument('i');
	reffile = argument_parser.get_argument('r');
	vcffile = argument_parser.get_argument('v');
	kmersize = stoi(argument_parser.get_argument('k'));
	outname = argument_parser.get_argument('o');
	sample_name = argument_parser.get_argument('s');
	nr_jellyfish_threads = stoi(argument_parser.get_argument('j'));
	nr_core_threads = stoi(argument_parser.get_argument('t'));
	only_genotyping = argument_parser.get_flag('g');
	only_phasing = argument_parser.get_flag('p');
	regularization = stold(argument_parser.get_argument('m'));
	count_only_graph = !argument_parser.get_flag('c');
	ignore_imputed = argument_parser.get_flag('u');

	// print info
	cerr << "Files and parameters used:" << endl;
	argument_parser.info();

	// read allele sequences and unitigs inbetween, write them into file
	cerr << "Determine allele sequences ..." << endl;
	VariantReader variant_reader (vcffile, reffile, kmersize, false, sample_name);
	string segment_file = outname + "_path_segments.fasta";
	cerr << "Write path segments to file: " << segment_file << " ..." << endl;
	variant_reader.write_path_segments(segment_file);

	// determine chromosomes present in VCF
	vector<string> chromosomes;
	variant_reader.get_chromosomes(&chromosomes);
	cerr << "Found " << chromosomes.size() << " chromosome(s) in the VCF." << endl;

	// TODO: only for analysis
	struct rusage r_usage0;
	getrusage(RUSAGE_SELF, &r_usage0);
	cerr << "#### Memory usage until now: " << (r_usage0.ru_maxrss / 1E6) << " GB ####" << endl;

	time_preprocessing = timer.get_interval_time();

	KmerCounter* read_kmer_counts = nullptr;
	// determine kmer copynumbers in reads
	if (readfile.substr(std::max(3, (int) readfile.size())-3) == std::string(".jf")) {
		cerr << "Read pre-computed read kmer counts ..." << endl;
		jellyfish::mer_dna::k(kmersize);
		read_kmer_counts = new JellyfishReader(readfile, kmersize);
	} else {
		cerr << "Count kmers in reads ..." << endl;
		if (count_only_graph) {
			read_kmer_counts = new JellyfishCounter(readfile, segment_file, kmersize, nr_jellyfish_threads);
		} else {
			read_kmer_counts = new JellyfishCounter(readfile, kmersize, nr_jellyfish_threads);
		}
	}

	size_t kmer_abundance_peak = read_kmer_counts->computeHistogram(10000, count_only_graph, outname + "_histogram.histo");
	cerr << "Computed kmer abundance peak: " << kmer_abundance_peak << endl;

	// count kmers in allele + reference sequence
	cerr << "Count kmers in genome ..." << endl;

	JellyfishCounter genomic_kmer_counts (segment_file, kmersize, nr_jellyfish_threads);

	// TODO: only for analysis
	struct rusage r_usage1;
	getrusage(RUSAGE_SELF, &r_usage1);
	cerr << "#### Memory usage until now: " << (r_usage1.ru_maxrss / 1E6) << " GB ####" << endl;

	// prepare output files
	if (! only_phasing) variant_reader.open_genotyping_outfile(outname + "_genotyping.vcf");
	if (! only_genotyping) variant_reader.open_phasing_outfile(outname + "_phasing.vcf");

	time_kmer_counting = timer.get_interval_time();

	cerr << "Construct HMM and run core algorithm ..." << endl;
	ProbabilityTable probabilities (kmer_abundance_peak / 4, kmer_abundance_peak*4, 2*kmer_abundance_peak, regularization);

	// determine max number of available threads (at most one thread per chromosome possible)
	size_t available_threads = min(thread::hardware_concurrency(), (unsigned int) chromosomes.size());
	if (nr_core_threads > available_threads) {
		cerr << "Warning: set nr_core_threads to " << available_threads << "." << endl;
		nr_core_threads = available_threads;
 	}
	Results results;
	{
		// create thread pool
		ThreadPool threadPool (nr_core_threads);
		for (auto chromosome : chromosomes) {
			KmerCounter* genomic = &genomic_kmer_counts;
			VariantReader* variants = &variant_reader;
			Results* r = &results;
			ProbabilityTable* probs = &probabilities;
			function<void()> f_genotyping = bind(run_genotyping_kmers, chromosome, genomic, read_kmer_counts, variants, probs, kmer_abundance_peak, only_genotyping, only_phasing, r);
			threadPool.submit(f_genotyping);
		}
	}

/**
	// create thread pool
	boost::asio::thread_pool threadPool(nr_core_threads);
	for (auto chromosome : chromosomes) {
		boost::asio::post(threadPool, boost::bind(run_genotyping_kmers, chromosome, &genomic_kmer_counts, read_kmer_counts, &variant_reader, kmer_abundance_peak, only_genotyping, only_phasing, &results));
	}
	threadPool.join();
**/

	timer.get_interval_time();

	// output VCF
	cerr << "Write results to VCF ..." << endl;
	assert (results.result.size() == chromosomes.size());
	// write VCF
	for (auto it = results.result.begin(); it != results.result.end(); ++it) {
		if (!only_phasing) {
			// output genotyping results
			variant_reader.write_genotypes_of(it->first, it->second, ignore_imputed);
		}
		if (!only_genotyping) {
			// output phasing results
			variant_reader.write_phasing_of(it->first, it->second, ignore_imputed);
		}
	}

	if (! only_phasing) variant_reader.close_genotyping_outfile();
	if (! only_genotyping) variant_reader.close_phasing_outfile();

	time_writing = timer.get_interval_time();
	time_total = timer.get_total_time();

	cerr << endl << "###### Summary ######" << endl;
	// output times
	cerr << "time spent reading input files:\t" << time_preprocessing << " sec" << endl;
	cerr << "time spent counting kmers:\t" << time_kmer_counting << " sec" << endl;
	// output per chromosome time
	double time_hmm = time_writing;
	for (auto chromosome : chromosomes) {
		double time_chrom = results.runtimes.at(chromosome);
		cerr << "time spent genotyping chromosome " << chromosome << ":\t" << time_chrom << endl;
		time_hmm += time_chrom;
	}
	cerr << "total running time:\t" << time_preprocessing + time_kmer_counting + time_hmm << " sec"<< endl;
	cerr << "total wallclock time: " << time_total  << " sec" << endl;

	// memory usage
	struct rusage r_usage;
	getrusage(RUSAGE_SELF, &r_usage);
	cerr << "Total maximum memory usage: " << (r_usage.ru_maxrss / 1E6) << " GB" << endl;

	delete read_kmer_counts;
	return 0;
}


#ifndef OPENGM_PYTHON_INTERFACE
#define OPENGM_PYTHON_INTERFACE 1
#endif

#include <stdexcept>
#include <stddef.h>
#include <string>
#include <boost/python.hpp>
#include <opengm/graphicalmodel/graphicalmodel.hxx>
#include <opengm/inference/inference.hxx>
#include <opengm/inference/dynamicprogramming.hxx>
#include "nifty_iterator.hxx"
#include "inferencehelpers.hxx"
#include "../export_typedes.hxx"
using namespace boost::python;

// Py Inference Types
namespace pydynp{
   template<class PARAM>
   inline void set(PARAM & p){}
}

template<class GM,class ACC>
void export_dynp(){
   import_array();
   typedef opengm::DynamicProgramming<GM, ACC>  PyDynamicProgramming;
   typedef typename PyDynamicProgramming::Parameter PyDynamicProgrammingParameter;
   typedef typename PyDynamicProgramming::VerboseVisitorType PyDynamicProgrammingVerboseVisitor;

   class_<PyDynamicProgrammingParameter > ( "DynamicProgrammingParameter" , init< > ())
   .def("set",&pydynp::set<PyDynamicProgrammingParameter>)
   ;
   OPENGM_PYTHON_VERBOSE_VISITOR_EXPORTER(PyDynamicProgrammingVerboseVisitor,"DynamicProgrammingVerboseVisitor" );
   OPENGM_PYTHON_INFERENCE_NO_RESET_EXPORTER(PyDynamicProgramming,"DynamicProgramming");
}

template void export_dynp<GmAdder,opengm::Minimizer>();
template void export_dynp<GmAdder,opengm::Maximizer>();
template void export_dynp<GmMultiplier,opengm::Minimizer>();
template void export_dynp<GmMultiplier,opengm::Maximizer>();


#include <v4r/features/global_alexnet_cnn_estimator.h>

#include <stdio.h>  // for snprintf
#include <string>
#include <vector>

#include <boost/algorithm/string.hpp>
#include <google/protobuf/text_format.h>

#include <caffe/blob.hpp>
#include <caffe/common.hpp>
//#include <caffe/proto/caffe.pb.h>
#include <caffe/util/db.hpp>
#include <caffe/util/io.hpp>
#include <caffe/layer.hpp>
#include <caffe/layers/memory_data_layer.hpp>
#include <caffe/net.hpp>

#include <v4r/common/img_utils.h>
#include <v4r/common/pcl_opencv.h>

using caffe::Blob;
using caffe::Caffe;
using caffe::Datum;
using caffe::Net;
using std::string;
namespace db = caffe::db;

namespace v4r
{

template<typename PointT, typename Dtype>
void
CNN_Feat_Extractor<PointT, Dtype>::WrapInputLayer(std::vector<cv::Mat>* input_channels)
{
    caffe::Blob<float>* input_layer = net_->input_blobs()[0];

    int width = input_layer->width();
    int height = input_layer->height();
    Dtype* input_data = input_layer->mutable_cpu_data();
    for (int i = 0; i < input_layer->channels(); ++i)
    {
        cv::Mat channel(height, width, CV_32FC1, input_data);
        input_channels->push_back(channel);
        input_data += width * height;
    }
}

template<typename PointT, typename Dtype>
void
CNN_Feat_Extractor<PointT, Dtype>::Preprocess(const cv::Mat& img, std::vector<cv::Mat>* input_channels) {
    /* Convert the input image to the input image format of the network. */
    cv::Mat sample;
    if (img.channels() == 3 && num_channels_ == 1)
        cv::cvtColor(img, sample, cv::COLOR_BGR2GRAY);
    else if (img.channels() == 4 && num_channels_ == 1)
        cv::cvtColor(img, sample, cv::COLOR_BGRA2GRAY);
    else if (img.channels() == 4 && num_channels_ == 3)
        cv::cvtColor(img, sample, cv::COLOR_BGRA2BGR);
    else if (img.channels() == 1 && num_channels_ == 3)
        cv::cvtColor(img, sample, cv::COLOR_GRAY2BGR);
    else
        sample = img;

    cv::Mat sample_resized;
    if (sample.size() != input_geometry_)
        cv::resize(sample, sample_resized, input_geometry_);
    else
        sample_resized = sample;

    cv::Mat sample_float;
    if (num_channels_ == 3)
        sample_resized.convertTo(sample_float, CV_32FC3);
    else
        sample_resized.convertTo(sample_float, CV_32FC1);

    cv::Mat sample_normalized;
    cv::subtract(sample_float, mean_, sample_normalized);

    /* This operation will write the separate BGR planes directly to the
   * input layer of the network because it is wrapped by the cv::Mat
   * objects in input_channels. */
    cv::split(sample_normalized, *input_channels);

    CHECK(reinterpret_cast<Dtype*>(input_channels->at(0).data)
          == net_->input_blobs()[0]->cpu_data())
            << "Input channels are not wrapping the input layer of the network.";
}

/* Load the mean file in binaryproto format. */
template<typename PointT, typename Dtype>
void
CNN_Feat_Extractor<PointT, Dtype>::SetMean(const std::string& mean_file)
{
    caffe::BlobProto blob_proto;
    ReadProtoFromBinaryFileOrDie(mean_file.c_str(), &blob_proto);

    /* Convert from BlobProto to Blob<float> */
    Blob<float> mean_blob;
    mean_blob.FromProto(blob_proto);
    CHECK_EQ(mean_blob.channels(), num_channels_)
            << "Number of channels of mean file doesn't match input layer.";

    /* The format of the mean file is planar 32-bit float BGR or grayscale. */
    std::vector<cv::Mat> channels;
    float* data = mean_blob.mutable_cpu_data();
    for (int i = 0; i < num_channels_; i++)
    {
        /* Extract an individual channel. */
        cv::Mat channel(mean_blob.height(), mean_blob.width(), CV_32FC1, data);
        channels.push_back(channel);
        data += mean_blob.height() * mean_blob.width();
    }

    /* Merge the separate channels into a single image. */
    cv::Mat mean;
    cv::merge(channels, mean);

    /* Compute the global mean pixel value and create a mean image
   * filled with this value. */
    cv::Scalar channel_mean = cv::mean(mean);
    mean_ = cv::Mat(input_geometry_, mean.type(), channel_mean);
}

template<typename PointT, typename Dtype>
int
CNN_Feat_Extractor<PointT, Dtype>::init(){

  if (strcmp(param_.device_name_.c_str(), "GPU") == 0)
  {
    Caffe::SetDevice(param_.device_id_);
    Caffe::set_mode(Caffe::GPU);
  }
  else
    Caffe::set_mode(Caffe::CPU);

  net_.reset(new Net<Dtype>(param_.feature_extraction_proto_, caffe::TEST));
  net_->CopyTrainedLayersFrom(param_.pretrained_binary_proto_);

  caffe::Blob<Dtype>* input_layer = net_->input_blobs()[0];
  num_channels_ = input_layer->channels();
  CHECK(num_channels_ == 3 || num_channels_ == 1) << "Input layer should have 1 or 3 channels.";
  input_geometry_ = cv::Size(input_layer->width(), input_layer->height());

  /* Load the binaryproto mean file. */
  SetMean(param_.input_mean_file_);
  return 0;
}

template<typename PointT, typename Dtype>
bool
CNN_Feat_Extractor<PointT, Dtype>::compute (const cv::Mat &img, Eigen::MatrixXf &signature)
{
    if(!init_)
    {
        init();
        init_ = true;
    }

    Blob<Dtype>* input_layer = net_->input_blobs()[0];
    input_layer->Reshape(1, num_channels_,
                         input_geometry_.height, input_geometry_.width);
    /* Forward dimension change to all layers. */
    net_->Reshape();

    std::vector<cv::Mat> input_channels;
    WrapInputLayer(&input_channels);

    Preprocess(img, &input_channels);

    net_->Forward();

    /* Copy the output layer to a std::vector */
//    Blob<float>* output_layer = net_->output_blobs()[0];
    boost::shared_ptr<Blob<Dtype> > output_layer = net_->blob_by_name(param_.output_layer_name_);
    const float* begin = output_layer->cpu_data();
    const float* end = begin + output_layer->channels();
    std::vector<float> sign_f = std::vector<float>(begin, end);

    signature = Eigen::MatrixXf (1, sign_f.size());

    for(size_t i=0; i<sign_f.size(); i++)
        signature(0,i) = sign_f[i];

    indices_.clear();
    cloud_.reset();
    return true;
}

template<typename PointT, typename Dtype>
bool
CNN_Feat_Extractor<PointT, Dtype>::compute (Eigen::MatrixXf &signature)
{
    CHECK( cloud_ && cloud_->isOrganized() && !cloud_->points.empty() && !indices_.empty());
    PCLOpenCVConverter<PointT> pcl_opencv_converter;
    pcl_opencv_converter.setInputCloud(cloud_);
    pcl_opencv_converter.setIndices(indices_);
    pcl_opencv_converter.setBackgroundColor( param_.background_color_(0), param_.background_color_(1), param_.background_color_(2) );
    pcl_opencv_converter.setRemoveBackground( param_.remove_background_ );
    cv::Mat img = pcl_opencv_converter.getRGBImage();
    cv::Rect roi = pcl_opencv_converter.getROI();
    cv::Mat img_cropped = cropImage(img, roi, param_.margin_, true);

    std::cerr << "Closing operation not implemented right now!" << std::endl;   ///TODO: re-implement closing operation to avoid sharp transitions from noisy measurements

    compute(img_cropped, signature);
    indices_.clear();
    return true;
}
    template class V4R_EXPORTS CNN_Feat_Extractor<pcl::PointXYZRGB>;
}