/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, 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: Stuart Glaser

#include <robot_mechanism_controllers/jt_cartesian_controller.h>

#include <Eigen/LU>

#include <ros/ros.h>

#include <angles/angles.h>

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(controller::JTCartesianController, pr2_controller_interface::Controller)

namespace controller {

JTCartesianController::JTCartesianController()
  : robot_state_(NULL), use_posture_(false)
{}

JTCartesianController::~JTCartesianController()
{
  sub_gains_.shutdown();
  sub_posture_.shutdown();
  sub_pose_.shutdown();
}

bool JTCartesianController::init(pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle &n)
{
  node_ = n;

  // get name of root and tip from the parameter server
  std::string tip_name;
  if (!node_.getParam("root_name", root_name_)){
    ROS_ERROR("JTCartesianController: No root name found on parameter server (namespace: %s)",
              node_.getNamespace().c_str());
    return false;
  }
  if (!node_.getParam("tip_name", tip_name)){
    ROS_ERROR("JTCartesianController: No tip name found on parameter server (namespace: %s)",
              node_.getNamespace().c_str());
    return false;
  }

  // test if we got robot pointer
  assert(robot_state);
  robot_state_ = robot_state;

  // Chain of joints
  if (!chain_.init(robot_state_, root_name_, tip_name))
    return false;
  if (!chain_.allCalibrated())
  {
    ROS_ERROR("Not all joints in the chain are calibrated (namespace: %s)", node_.getNamespace().c_str());
    return false;
  }
  if (chain_.size() != Joints)
  {
    ROS_ERROR("The JTCartesianController works with %d joints, but the chain from %s to %s has %d joints.",
              Joints, root_name_.c_str(), tip_name.c_str(), chain_.size());
    return false;
  }

  // Kinematics
  KDL::Chain kdl_chain;
  chain_.toKDL(kdl_chain);
  kin_.reset(new Kin<Joints>(kdl_chain));

  // Cartesian gains
  double kp_trans, kd_trans, kp_rot, kd_rot;
  if (!node_.getParam("cart_gains/trans/p", kp_trans) ||
      !node_.getParam("cart_gains/trans/d", kd_trans))
  {
    ROS_ERROR("P and D translational gains not specified (namespace: %s)", node_.getNamespace().c_str());
    return false;
  }
  if (!node_.getParam("cart_gains/rot/p", kp_rot) ||
      !node_.getParam("cart_gains/rot/d", kd_rot))
  {
    ROS_ERROR("P and D rotational gains not specified (namespace: %s)", node_.getNamespace().c_str());
    return false;
  }
  Kp << kp_trans, kp_trans, kp_trans,  kp_rot, kp_rot, kp_rot;
  Kd << kd_trans, kd_trans, kd_trans,  kd_rot, kd_rot, kd_rot;

  node_.param("pose_command_filter", pose_command_filter_, 1.0);

  // Velocity saturation
  node_.param("vel_saturation_trans", vel_saturation_trans_, 0.0);
  node_.param("vel_saturation_rot", vel_saturation_rot_, 0.0);

  node_.param("jacobian_inverse_damping", jacobian_inverse_damping_, 0.0);
  node_.param("joint_vel_filter", joint_vel_filter_, 1.0);

  // Joint gains
  for (int i = 0; i < Joints; ++i)
    node_.param("joint_feedforward/" + chain_.getJoint(i)->joint_->name, joint_dd_ff_[i], 0.0);
  for (int i = 0; i < Joints; ++i)
    node_.param("saturation/" + chain_.getJoint(i)->joint_->name, saturation_[i], 0.0);

  // Posture gains
  node_.param("k_posture", k_posture_, 1.0);

  node_.param("resolution/force", res_force_, 0.01);
  node_.param("resolution/position", res_position_, 0.001);
  node_.param("resolution/torque", res_torque_, 0.01);
  node_.param("resolution/orientation", res_orientation_, 0.001);

  sub_gains_ = node_.subscribe("gains", 5, &JTCartesianController::setGains, this);
  sub_posture_ = node_.subscribe("command_posture", 5, &JTCartesianController::commandPosture, this);
  sub_pose_ = node_.subscribe("command_pose", 1, &JTCartesianController::commandPose, this);

  StateMsg state_template;
  state_template.header.frame_id = root_name_;
  state_template.x.header.frame_id = root_name_;
  state_template.x_desi.header.frame_id = root_name_;
  state_template.x_desi_filtered.header.frame_id = root_name_;
  state_template.tau_pose.resize(Joints);
  state_template.tau_posture.resize(Joints);
  state_template.tau.resize(Joints);
  state_template.J.layout.dim.resize(2);
  state_template.J.data.resize(6*Joints);
  state_template.N.layout.dim.resize(2);
  state_template.N.data.resize(Joints*Joints);
  pub_state_.init(node_, "state", 10);
  pub_state_.lock();
  pub_state_.msg_ = state_template;
  pub_state_.unlock();

  pub_x_desi_.init(node_, "state/x_desi", 10);
  pub_x_desi_.lock();
  pub_x_desi_.msg_.header.frame_id = root_name_;
  pub_x_desi_.unlock();

  return true;
}

void JTCartesianController::starting()
{
  //Kp << 800.0, 800.0, 800.0,   80.0, 80.0, 80.0;
  //Kd << 12.0, 12.0, 12.0,   0.0, 0.0, 0.0;

  JointVec q;
  chain_.getPositions(q);
  kin_->fk(q, x_desi_);
  x_desi_filtered_ = x_desi_;
  last_pose_ = x_desi_;
  q_posture_ = q;
  qdot_filtered_.setZero();
  last_wrench_.setZero();

  loop_count_ = 0;
}

static void computePoseError(const Eigen::Affine3d &xact, const Eigen::Affine3d &xdes, Eigen::Matrix<double,6,1> &err)
{
  err.head<3>() = xact.translation() - xdes.translation();
  err.tail<3>()   = 0.5 * (xdes.linear().col(0).cross(xact.linear().col(0)) +
                          xdes.linear().col(1).cross(xact.linear().col(1)) +
                          xdes.linear().col(2).cross(xact.linear().col(2)));
}

void JTCartesianController::update()
{
  // get time
  ros::Time time = robot_state_->getTime();
  ros::Duration dt = time - last_time_;
  last_time_ = time;
  ++loop_count_;

  // ======== Measures current arm state

  JointVec q;
  chain_.getPositions(q);

  Eigen::Affine3d x;
  kin_->fk(q, x);

  Jacobian J;
  kin_->jac(q, J);

  JointVec qdot_raw;
  chain_.getVelocities(qdot_raw);
  for (int i = 0; i < Joints; ++i)
    qdot_filtered_[i] += joint_vel_filter_ * (qdot_raw[i] - qdot_filtered_[i]);
  JointVec qdot = qdot_filtered_;
  CartVec xdot = J * qdot;

  // ======== Controls to the current pose setpoint

  {
    Eigen::Vector3d p0(x_desi_filtered_.translation());
    Eigen::Vector3d p1(x_desi_.translation());
    Eigen::Quaterniond q0(x_desi_filtered_.linear());
    Eigen::Quaterniond q1(x_desi_.linear());
    q0.normalize();
    q1.normalize();

    tf::Quaternion tf_q0(q0.x(), q0.y(), q0.z(), q0.w());
    tf::Quaternion tf_q1(q1.x(), q1.y(), q1.z(), q1.w());
    tf::Quaternion tf_q = tf_q0.slerp(tf_q1, pose_command_filter_);

    Eigen::Vector3d p = p0 + pose_command_filter_ * (p1 - p0);
    //Eigen::Quaterniond q = q0.slerp(pose_command_filter_, q1);
    Eigen::Quaterniond q(tf_q.w(), tf_q.x(), tf_q.y(), tf_q.z());
    //x_desi_filtered_ = q * Eigen::Translation3d(p);
    x_desi_filtered_ = Eigen::Translation3d(p) * q;
  }
  CartVec x_err;
  //computePoseError(x, x_desi_, x_err);
  computePoseError(x, x_desi_filtered_, x_err);

  CartVec xdot_desi = (Kp.array() / Kd.array()) * x_err.array() * -1.0;

  // Caps the cartesian velocity
  if (vel_saturation_trans_ > 0.0)
  {
    if (fabs(xdot_desi.head<3>().norm()) > vel_saturation_trans_)
      xdot_desi.head<3>() *= (vel_saturation_trans_ / xdot_desi.head<3>().norm());
  }
  if (vel_saturation_rot_ > 0.0)
  {
    if (fabs(xdot_desi.tail<3>().norm()) > vel_saturation_rot_)
      xdot_desi.tail<3>() *= (vel_saturation_rot_ / xdot_desi.tail<3>().norm());
  }

  CartVec F = Kd.array() * (xdot_desi - xdot).array();

  JointVec tau_pose = J.transpose() * F;

  // ======== J psuedo-inverse and Nullspace computation

  // Computes pseudo-inverse of J
  Eigen::Matrix<double,6,6> I6; I6.setIdentity();
  //Eigen::Matrix<double,6,6> JJt = J * J.transpose();
  //Eigen::Matrix<double,6,6> JJt_inv = JJt.inverse();
  Eigen::Matrix<double,6,6> JJt_damped = J * J.transpose() + jacobian_inverse_damping_ * I6;
  Eigen::Matrix<double,6,6> JJt_inv_damped = JJt_damped.inverse();
  Eigen::Matrix<double,Joints,6> J_pinv = J.transpose() * JJt_inv_damped;

  // Computes the nullspace of J
  Eigen::Matrix<double,Joints,Joints> I;
  I.setIdentity();
  Eigen::Matrix<double,Joints,Joints> N = I - J_pinv * J;

  // ======== Posture control

  // Computes the desired joint torques for achieving the posture
  JointVec tau_posture;
  tau_posture.setZero();
  if (use_posture_)
  {
    JointVec posture_err = q_posture_ - q;
    for (size_t j = 0; j < Joints; ++j)
    {
      if (chain_.getJoint(j)->joint_->type == urdf::Joint::CONTINUOUS)
        posture_err[j] = angles::normalize_angle(posture_err[j]);
    }

    for (size_t j = 0; j < Joints; ++j) {
      if (fabs(q_posture_[j] - 9999) < 1e-5)
        posture_err[j] = 0.0;
    }

    JointVec qdd_posture = k_posture_ * posture_err;
    tau_posture = joint_dd_ff_.array() * (N * qdd_posture).array();
  }

  JointVec tau = tau_pose + tau_posture;

  // ======== Torque Saturation
  double sat_scaling = 1.0;
  for (int i = 0; i < Joints; ++i) {
    if (saturation_[i] > 0.0)
      sat_scaling = std::min(sat_scaling, fabs(saturation_[i] / tau[i]));
  }
  JointVec tau_sat = sat_scaling * tau;

  chain_.addEfforts(tau_sat);

  if (loop_count_ % 10 == 0)
  {
    if (pub_x_desi_.trylock()) {
      pub_x_desi_.msg_.header.stamp = time;
      tf::poseEigenToMsg(x_desi_, pub_x_desi_.msg_.pose);
      pub_x_desi_.unlockAndPublish();
    }

    if (pub_state_.trylock()) {
      pub_state_.msg_.header.stamp = time;
      pub_state_.msg_.x.header.stamp = time;
      tf::poseEigenToMsg(x, pub_state_.msg_.x.pose);
      pub_state_.msg_.x_desi.header.stamp = time;
      tf::poseEigenToMsg(x_desi_, pub_state_.msg_.x_desi.pose);
      pub_state_.msg_.x_desi_filtered.header.stamp = time;
      tf::poseEigenToMsg(x_desi_filtered_, pub_state_.msg_.x_desi_filtered.pose);
      tf::twistEigenToMsg(x_err, pub_state_.msg_.x_err);
      tf::twistEigenToMsg(xdot, pub_state_.msg_.xd);
      tf::twistEigenToMsg(xdot_desi, pub_state_.msg_.xd_desi);
      tf::wrenchEigenToMsg(F, pub_state_.msg_.F);
      tf::matrixEigenToMsg(J, pub_state_.msg_.J);
      tf::matrixEigenToMsg(N, pub_state_.msg_.N);
      for (size_t j = 0; j < Joints; ++j) {
        pub_state_.msg_.tau_pose[j] = tau_pose[j];
        pub_state_.msg_.tau_posture[j] = tau_posture[j];
        pub_state_.msg_.tau[j] = tau[j];
      }
      pub_state_.unlockAndPublish();
    }
  }
}

} //namespace


#include "MapSystem.h"

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

#include "../utility/Logging.h"
#include "../constants/MapConstants.h"
#include "../constants/RenderConstants.h"

using namespace std;

MapSystem::MapSystem(Input& input, Camera& camera, Map& map, Log& log)
  : input_{input}
  , camera_{camera}
  , map_{map}
  , log_{log}
{
}

void
MapSystem::init()
{
  cout << "MapSystem initializing" << endl;

  srand(MAP_SEED);

  generate_map();
}

void
MapSystem::update()
{
  if (map_.floor_changed) {
    map_.floor_changed = false;
  }

  if (input_.lclick) {
    input_.lclick = false;
    calculate_selected_tile();
  }

  update_floor();
}

void
MapSystem::generate_map()
{
  for (auto floor{1}; floor <= NUM_FLOORS; floor++) {
    define_blocks(floor);
    layout_main_floor(floor);
    seed_rooms(floor);
    expand_rooms(floor);
    build_rooms(floor);
    place_doors(floor);
    integrate_walls(floor);
  }
}

void
MapSystem::update_floor()
{
  if (input_.descend) {
    input_.descend = false;

    if (map_.cur_floor > 1) {
      map_.floor_changed = true;
      map_.cur_floor--;
    }
  }

  if (input_.ascend) {
    input_.ascend = false;

    if (map_.cur_floor < NUM_FLOORS) {
      map_.floor_changed = true;
      map_.cur_floor++;
    }
  }
}

void
MapSystem::define_blocks(i32 floor)
{
  // left edge
  map_.blocks[floor].push_back({
    0, 0, OUTER_PATH, TILES_PER_LAYER - 1
  });
  // right edge
  map_.blocks[floor].push_back({
    TILES_PER_LAYER - OUTER_PATH - 1, 0, OUTER_PATH, TILES_PER_LAYER - 1
  });
  // top edge
  map_.blocks[floor].push_back({
    0, 0, TILES_PER_LAYER - 1, OUTER_PATH
  });
  // bottom edge
  map_.blocks[floor].push_back({
    0, TILES_PER_LAYER - OUTER_PATH - 1, TILES_PER_LAYER - 1, OUTER_PATH
  });
  // middle horizontal
  map_.blocks[floor].push_back({
    0, TILES_PER_LAYER / 2 - CENTRAL_PATH / 2 - 1,
    TILES_PER_LAYER - 1, CENTRAL_PATH + 1
  });
  // middle vertical
  map_.blocks[floor].push_back({
    TILES_PER_LAYER / 2 - CENTRAL_PATH / 2 - 1, 0,
    CENTRAL_PATH + 1, TILES_PER_LAYER - 1
  });
}

void
MapSystem::layout_main_floor(i32 floor)
{
  string floor_type;

  switch (get_section(floor)) {
  case 1: floor_type = "dark_concrete"; break;
  case 2: floor_type = "smooth_dark_concrete"; break;
  case 3: floor_type = "bright_dark_concrete"; break;
  }

  for (auto x{0}; x < TILES_PER_LAYER; x++) {
    for (auto y{0}; y < TILES_PER_LAYER; y++) {
      set_tile("flr", x, y, floor, floor_type);
    }
  }
}

void
MapSystem::seed_rooms(i32 floor)
{
  for (auto i{0}; i < NUM_ROOMS; i++) {
    Room test_room;
    bool collision{true};

    switch (get_section(floor)) {
    case 1:
      test_room.wall_type = "wall1";
      test_room.floor_type = "light_concrete";
      break;
    case 2:
      test_room.wall_type = "wall2";
      test_room.floor_type = "smooth_light_concrete";
      break;
    case 3:
      test_room.wall_type = "wall3";
      test_room.floor_type = "bright_light_concrete";
      break;
    }

    do {
      test_room.rect.x = rand() % (TILES_PER_LAYER - 1);
      test_room.rect.y = rand() % (TILES_PER_LAYER - 1);
      test_room.rect.w = 2;
      test_room.rect.h = 2;
    }
    while (room_collision(floor, test_room));

    map_.rooms[floor].push_back(test_room);
  }
}

void
MapSystem::expand_rooms(i32 floor)
{
  if (RANDOM_ROOMS) srand(time(nullptr));

  for (auto i{0}; i < EXPANSION_ITERATIONS; i++) {
    bool found{false};
    vector<Dir> dirs{Dir::UP, Dir::DOWN, Dir::LEFT, Dir::RIGHT};

    u64 random_room_index{rand() % map_.rooms[floor].size()};

    Room& room{map_.rooms[floor][random_room_index]};

    while (!found && !dirs.empty()) {
      const Dir dir{rand_dir()};
      dirs.erase(remove(dirs.begin(), dirs.end(), dir), dirs.end());

      switch (dir) {
      case Dir::UP:    room.rect.y--; break;
      case Dir::DOWN:  room.rect.h++; break;
      case Dir::LEFT:  room.rect.x--; break;
      case Dir::RIGHT: room.rect.w++; break;
      }

      if (room_collision(floor, room)) {
        switch (dir) {
        case Dir::UP:    room.rect.y++; break;
        case Dir::DOWN:  room.rect.h--; break;
        case Dir::LEFT:  room.rect.x++; break;
        case Dir::RIGHT: room.rect.w--; break;
        }
      } else {
        found = true;
      }
    }
  }
}

void
MapSystem::build_rooms(i32 floor)
{
  for (const auto& room : map_.rooms[floor]) {
    for (auto x{room.l()}; x <= room.r(); x++) {
      for (auto y{room.t()}; y <= room.b(); y++) {
        set_tile("flr", x, y, floor, room.floor_type);
      }
    }

    for (auto x{room.l()}; x <= room.r(); x++) {
      set_solid(x, room.t(), floor);
      set_solid(x, room.b(), floor);
      set_tile("wal", x, room.t(), floor, room.wall_type + "-str");
      set_tile("wal", x, room.b(), floor, room.wall_type + "-str");
    }

    for (auto y{room.t() + 1}; y <= room.b(); y++) {
      set_solid(room.l(), y, floor);
      set_solid(room.r(), y, floor);
      set_tile("wal", room.l(), y, floor, room.wall_type + "-str", 90);
      set_tile("wal", room.r(), y, floor, room.wall_type + "-str", 90);
    }
  }

  cout << " Floor " << floor << " rooms built" << endl;
}

void
MapSystem::integrate_walls(i32 floor)
{
  const auto& tiles{map_.floors[floor].layers["wal"].tiles};

  for (auto x{OUTER_PATH}; x < TILES_PER_LAYER - OUTER_PATH; x++) {
    for (auto y{OUTER_PATH}; y < TILES_PER_LAYER - OUTER_PATH; y++) {
      const Tile& tile{tiles[x][y]};

      if (tile.category == "walls") {
        const Tile& utile{tiles[x + 0][y - 1]};
        const Tile& dtile{tiles[x + 0][y + 1]};
        const Tile& ltile{tiles[x - 1][y + 0]};
        const Tile& rtile{tiles[x + 1][y + 0]};

        const bool umatch{tile.type == utile.type};
        const bool rmatch{tile.type == rtile.type};
        const bool dmatch{tile.type == dtile.type};
        const bool lmatch{tile.type == ltile.type};

        if (umatch && lmatch && dmatch && rmatch) {
          set_tile("wal", x, y, floor, tile.type + "-int");
        } else if (umatch && rmatch && dmatch) {
          set_tile("wal", x, y, floor, tile.type + "-tee");
        } else if (rmatch && dmatch && lmatch) {
          set_tile("wal", x, y, floor, tile.type + "-tee", 90);
        } else if (dmatch && lmatch && umatch) {
          set_tile("wal", x, y, floor, tile.type + "-tee", 180);
        } else if (lmatch && umatch && rmatch) {
          set_tile("wal", x, y, floor, tile.type + "-tee", 270);
        } else if (umatch && rmatch) {
          set_tile("wal", x, y, floor, tile.type + "-cor");
        } else if (rmatch && dmatch) {
          set_tile("wal", x, y, floor, tile.type + "-cor", 90);
        } else if (dmatch && lmatch) {
          set_tile("wal", x, y, floor, tile.type + "-cor", 180);
        } else if (lmatch && umatch) {
          set_tile("wal", x, y, floor, tile.type + "-cor", 270);
        } else if (lmatch && rmatch) {
          set_tile("wal", x, y, floor, tile.type + "-str");
        } else if (umatch && dmatch) {
          set_tile("wal", x, y, floor, tile.type + "-str", 90);
        } else if (umatch) {
          set_tile("wal", x, y, floor, tile.type + "-end");
        } else if (rmatch) {
          set_tile("wal", x, y, floor, tile.type + "-end", 90);
        } else if (dmatch) {
          set_tile("wal", x, y, floor, tile.type + "-end", 180);
        } else if (lmatch) {
          set_tile("wal", x, y, floor, tile.type + "-end", 270);
        } else {
          set_tile("wal", x, y, floor, tile.type + "-one");
        }
      }
    }
  }

  cout << " Floor " << floor << " rooms integrated" << endl;
}

void
MapSystem::place_doors(i32 floor)
{
  for (auto& room : map_.rooms[floor]) {
    u8 count{0};
    bool found{false};

    while (!found && count++ < 40) {
      const auto horz_range{room.w() - 1};
      const auto horz_start{room.l() + 1};
      const auto vert_range{room.h() - 1};
      const auto vert_start{room.t() + 1};

      i32 rot, x, y;
      const Dir dir{rand_dir()};

      if (dir == Dir::UP) {
        rot = 0;
        x = horz_start + rand() % horz_range;
        y = room.t();
      } else if (dir == Dir::DOWN) {
        rot = 90;
        x = room.r();
        y = vert_start + rand() % vert_range;
      } else if (dir == Dir::RIGHT) {
        rot = 180;
        x = horz_start + rand() % horz_range;
        y = room.b();
      } else if (dir == Dir::LEFT) {
        rot = 270;
        x = room.l();
        y = vert_start + rand() % vert_range;
      }

      if (has_clearance("door", x, y, floor, dir)) {
        found = true;

        if (rand() % 2 == 0) {
          set_solid(x, y, floor);
          set_tile("wal", x, y, floor, "door1-cls", rot);
        } else {
          set_solid(x, y, floor, false);
          set_tile("wal", x, y, floor, "door1-opn", rot);
        }
      }
    }
  }

  cout << " Floor " << floor << " doors placed" << endl;
}

void
MapSystem::calculate_selected_tile()
{
  f32 screenx{(input_.mx - HALF_SCREEN_SIZE_X) / (f32)TILE_SIZE / camera_.zoom};
  f32 screeny{(input_.my - HALF_SCREEN_SIZE_Y) / (f32)TILE_SIZE / camera_.zoom};

  i32 targetx{static_cast<i32>(floor(screenx + camera_.pos.x))};
  i32 targety{static_cast<i32>(floor(screeny + camera_.pos.y))};

  if (select_tile(targetx, targety)) {
    string msg{"Selected: ["};
    msg += to_string(input_.selectx) + "," + to_string(input_.selecty) + ",";
    msg += to_string(input_.selectfloor) + "]";

    ::msg(log_, msg);
  } else {
    ::msg(log_, "Selected: [invalid]");
  }
}

const bool
MapSystem::select_tile(i32 x, i32 y)
{
  const auto x_in_bounds{x >= 0 && x <= TILES_PER_LAYER - 1};
  const auto y_in_bounds{y >= 0 && y <= TILES_PER_LAYER - 1};

  if (x_in_bounds && y_in_bounds) {
    clear_selection();

    input_.selectx = x;
    input_.selecty = y;
    input_.selectfloor = map_.cur_floor;

    set_tile("ovr", x, y, map_.cur_floor, "select");

    return true;
  } else {
    return false;
  }
}

void
MapSystem::clear_selection()
{
  if (input_.selectx == -1 && input_.selecty == -1) {
    return;
  }

  set_active(
    "ovr", input_.selectx, input_.selecty, input_.selectfloor, false
  );
}

const bool
MapSystem::room_collision(i32 floor, const Room& test_room)
const
{
  for (const auto& room : map_.blocks[floor]) {
    if (SDL_HasIntersection(&room.rect, &test_room.rect)) {
      return true;
    }
  }

  for (const auto& room : map_.rooms[floor]) {
    const auto intersection{SDL_HasIntersection(&room.rect, &test_room.rect)};

    if (&room.rect != &test_room.rect && intersection) {
      return true;
    }
  }

  return false;
}

const bool
MapSystem::has_clearance(
  const string& category, i32 x, i32 y, i32 floor, Dir dir
)
const
{
  i8 dx1, dy1;
  i8 dx2, dy2;
  i8 dx3, dy3;

  if (dir == Dir::UP) {
    dx1 =  0; dy1 = -1;
    dx2 = -1; dy2 =  0;
    dx3 =  1; dy3 =  0;
  } else if (dir == Dir::RIGHT) {
    dx1 =  1; dy1 =  0;
    dx2 =  0; dy2 = -1;
    dx3 =  0; dy3 =  1;
  } else if (dir == Dir::DOWN) {
    dx1 =  0; dy1 =  1;
    dx2 =  1; dy2 =  0;
    dx3 = -1; dy3 =  0;
  } else if (dir == Dir::LEFT) {
    dx1 = -1; dy1 =  0;
    dx2 =  0; dy2 =  1;
    dx3 =  0; dy3 = -1;
  }

  const auto& tiles{map_.floors[floor].layers["wal"].tiles};

  const auto front_clear{tiles[x + dx1][y + dy1].type == ""};
  const auto left_clear{tiles[x + dx2][y + dy2].category != "door"};
  const auto right_clear{tiles[x + dx3][y + dy3].category != "door"};

  return front_clear && left_clear && right_clear;
}

void
MapSystem::set_tile(
  const string& layer, i32 x, i32 y, i32 floor, const string& type,
  f32 rot, SDL_RendererFlip flip
) {
  Tile& tile{map_.floors[floor].layers[layer].tiles[x][y]};

  tile.active = true;
  tile.rot = rot;
  tile.flip = flip;

  if (map_.tile_data.find(type) != map_.tile_data.end()) {
    vector<string> type_vector;
    boost::split(type_vector, type, boost::is_any_of("-"));

    tile.type = type_vector[0];
    tile.subtype = type_vector.size() <= 1 ? "" : type_vector[1];
    tile.category = map_.tile_data[type].category;
    tile.src.x = map_.tile_data[type].uv.x * TILE_SIZE;
    tile.src.y = map_.tile_data[type].uv.y * TILE_SIZE;
  } else {
    tile.src.x = 0;
    tile.src.y = 0;

    cerr << "Tile(" << x << "," << y << ") has invalid type: ";
    cerr << tile.type << endl;
  }
}

void
MapSystem::set_solid(i32 x, i32 y, i32 floor, bool solid)
{
  Tile& tile{map_.floors[floor].layers["wal"].tiles[x][y]};
  tile.solid = solid;
}

void
MapSystem::set_active(
  const std::string& layer, i32 x, i32 y, i32 floor, bool active
) {
  Tile& tile{map_.floors[floor].layers[layer].tiles[x][y]};
  tile.active = active;
}

const i32
MapSystem::get_section(i32 floor)
const
{
  if (floor > 2 * NUM_FLOORS / 3 && floor <= NUM_FLOORS) {
    return 3;
  } else if (floor > 1 * NUM_FLOORS / 3) {
    return 2;
  } else if (floor > 0 * NUM_FLOORS / 3) {
    return 1;
  } else {
    return -1;
  }
}


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

#ifndef cancer_caller_hpp
#define cancer_caller_hpp

#include <vector>
#include <unordered_map>
#include <memory>
#include <functional>
#include <typeindex>

#include <boost/optional.hpp>

#include "config/common.hpp"
#include "core/types/haplotype.hpp"
#include "core/types/cancer_genotype.hpp"
#include "core/models/genotype/cancer_genotype_prior_model.hpp"
#include "core/models/genotype/genotype_prior_model.hpp"
#include "core/models/mutation/coalescent_model.hpp"
#include "core/models/mutation/somatic_mutation_model.hpp"
#include "core/models/genotype/individual_model.hpp"
#include "core/models/genotype/cnv_model.hpp"
#include "core/models/genotype/tumour_model.hpp"
#include "basics/phred.hpp"
#include "caller.hpp"

namespace octopus {

class GenomicRegion;
class ReadPipe;
class Variant;
class VariantCall;

class CancerCaller : public Caller
{
public:
    using Caller::CallTypeSet;

    struct Parameters
    {
        enum class NormalContaminationRisk { high, low };
        Phred<double> min_variant_posterior, min_somatic_posterior, min_refcall_posterior;
        unsigned ploidy;
        boost::optional<SampleName> normal_sample;
        boost::optional<CoalescentModel::Parameters> germline_prior_model_params;
        SomaticMutationModel::Parameters somatic_mutation_model_params;
        double min_expected_somatic_frequency, credible_mass, min_credible_somatic_frequency;
        std::size_t max_genotypes = 20000;
        NormalContaminationRisk normal_contamination_risk = NormalContaminationRisk::low;
        double cnv_normal_alpha = 50.0, cnv_tumour_alpha = 0.5;
        double somatic_normal_germline_alpha = 50.0, somatic_normal_somatic_alpha = 0.05;
        double somatic_tumour_germline_alpha = 1.5, somatic_tumour_somatic_alpha = 1.0;
    };

    CancerCaller() = delete;

    CancerCaller(Caller::Components&& components,
                 Caller::Parameters general_parameters,
                 Parameters specific_parameters);

    CancerCaller(const CancerCaller&)            = delete;
    CancerCaller& operator=(const CancerCaller&) = delete;
    CancerCaller(CancerCaller&&)                 = delete;
    CancerCaller& operator=(CancerCaller&&)      = delete;

    ~CancerCaller() = default;

private:
    using GermlineModel = model::IndividualModel;
    using CNVModel      = model::CNVModel;
    using TumourModel   = model::TumourModel;

    class Latents;
    friend Latents;

    struct ModelProbabilities
    {
        double germline, cnv, somatic;
    };

    using ModelPriors     = ModelProbabilities;
    using ModelPosteriors = ModelProbabilities;

    Parameters parameters_;

    // overrides

    std::string do_name() const override;
    CallTypeSet do_call_types() const override;

    std::unique_ptr<Caller::Latents>
    infer_latents(const std::vector<Haplotype>& haplotypes,
                  const HaplotypeLikelihoodCache& haplotype_likelihoods) const override;

    boost::optional<double>
    calculate_model_posterior(const std::vector<Haplotype>& haplotypes,
                              const HaplotypeLikelihoodCache& haplotype_likelihoods,
                              const Caller::Latents& latents) const override;

    boost::optional<double>
    calculate_model_posterior(const std::vector<Haplotype>& haplotypes,
                              const HaplotypeLikelihoodCache& haplotype_likelihoods,
                              const Latents& latents) const;

    std::vector<std::unique_ptr<VariantCall>>
    call_variants(const std::vector<Variant>& candidates, const Caller::Latents& latents) const override;

    std::vector<std::unique_ptr<VariantCall>>
    call_variants(const std::vector<Variant>& candidates, const Latents& latents) const;

    std::vector<std::unique_ptr<ReferenceCall>>
    call_reference(const std::vector<Allele>& alleles, const Caller::Latents& latents,
                   const ReadMap& reads) const override;

    bool has_normal_sample() const noexcept;
    const SampleName& normal_sample() const;

    using GenotypeVector                 = std::vector<Genotype<Haplotype>>;
    using CancerGenotypeVector           = std::vector<CancerGenotype<Haplotype>>;
    using GermlineGenotypeReference      = Genotype<Haplotype>;
    using GermlineGenotypeProbabilityMap = std::unordered_map<GermlineGenotypeReference, double>;
    using ProbabilityVector              = std::vector<double>;

    void generate_germline_genotypes(Latents& latents, const std::vector<Haplotype>& haplotypes) const;
    void generate_cancer_genotypes(Latents& latents, const HaplotypeLikelihoodCache& haplotype_likelihoods) const;
    void generate_cancer_genotypes_with_clean_normal(Latents& latents, const HaplotypeLikelihoodCache& haplotype_likelihoods) const;
    void generate_cancer_genotypes_with_contaminated_normal(Latents& latents, const HaplotypeLikelihoodCache& haplotype_likelihoods) const;
    void generate_cancer_genotypes_with_no_normal(Latents& latents, const HaplotypeLikelihoodCache& haplotype_likelihoods) const;
    void generate_cancer_genotypes(Latents& latents, const std::vector<Genotype<Haplotype>>& germline_genotypes) const;
    bool has_high_normal_contamination_risk(const Latents& latents) const;

    void evaluate_germline_model(Latents& latents, const HaplotypeLikelihoodCache& haplotype_likelihoods) const;
    void evaluate_cnv_model(Latents& latents, const HaplotypeLikelihoodCache& haplotype_likelihoods) const;
    void evaluate_tumour_model(Latents& latents, const HaplotypeLikelihoodCache& haplotype_likelihoods) const;
    void evaluate_noise_model(Latents& latents, const HaplotypeLikelihoodCache& haplotype_likelihoods) const;
    void set_model_priors(Latents& latents) const;
    void set_model_posteriors(Latents& latents) const;

    std::unique_ptr<GenotypePriorModel> make_germline_prior_model(const std::vector<Haplotype>& haplotypes) const;
    CNVModel::Priors get_cnv_model_priors(const GenotypePriorModel& prior_model) const;
    TumourModel::Priors get_somatic_model_priors(const CancerGenotypePriorModel& prior_model) const;
    TumourModel::Priors get_noise_model_priors(const CancerGenotypePriorModel& prior_model) const;
    CNVModel::Priors get_normal_noise_model_priors(const GenotypePriorModel& prior_model) const;

    GermlineGenotypeProbabilityMap
    calculate_germline_genotype_posteriors(const Latents& latents, const ModelPosteriors& model_posteriors) const;
    ProbabilityVector calculate_probability_samples_not_somatic(const Latents& inferences) const;
    Phred<double> calculate_somatic_probability(const ProbabilityVector& sample_somatic_posteriors,
                                                const ModelPosteriors& model_posteriors) const;
};

class CancerCaller::Latents : public Caller::Latents
{
public:
    using Caller::Latents::HaplotypeProbabilityMap;
    using Caller::Latents::GenotypeProbabilityMap;

    Latents() = delete;

    Latents(const std::vector<Haplotype>& haplotypes,
            const std::vector<SampleName>& samples);

    std::shared_ptr<HaplotypeProbabilityMap> haplotype_posteriors() const override;
    std::shared_ptr<GenotypeProbabilityMap> genotype_posteriors() const override;

private:
    std::reference_wrapper<const std::vector<Haplotype>> haplotypes_;
    std::vector<Genotype<Haplotype>> germline_genotypes_;
    std::vector<CancerGenotype<Haplotype>> cancer_genotypes_;
    boost::optional<std::vector<std::vector<unsigned>>> germline_genotype_indices_ = boost::none;
    boost::optional<std::vector<std::pair<std::vector<unsigned>, unsigned>>> cancer_genotype_indices_ = boost::none;

    std::reference_wrapper<const std::vector<SampleName>> samples_;
    boost::optional<std::reference_wrapper<const SampleName>> normal_sample_ = boost::none;

    CancerCaller::ModelPriors model_priors_;
    std::unique_ptr<GenotypePriorModel> germline_prior_model_ = nullptr;
    boost::optional<CancerGenotypePriorModel> cancer_genotype_prior_model_ = boost::none;
    std::unique_ptr<GermlineModel> germline_model_ = nullptr;
    GermlineModel::InferredLatents germline_model_inferences_;
    CNVModel::InferredLatents cnv_model_inferences_;
    TumourModel::InferredLatents tumour_model_inferences_;
    boost::optional<TumourModel::InferredLatents> noise_model_inferences_ = boost::none;
    boost::optional<GermlineModel::InferredLatents> normal_germline_inferences_ = boost::none;
    CancerCaller::ModelPosteriors model_posteriors_;

    mutable std::shared_ptr<HaplotypeProbabilityMap> haplotype_posteriors_ = nullptr;
    mutable std::shared_ptr<GenotypeProbabilityMap> genotype_posteriors_ = nullptr;

    friend CancerCaller;

    void compute_genotype_posteriors() const;
    void compute_haplotype_posteriors() const;
};

} // namespace octopus

#endif


//receive ros msg:1.image_l,image_r   2.position.
//output:scene struct.
#include <memory>
#include <glog/logging.h>
#include <Eigen/Core>
#include <Eigen/Dense>

#include <opencv2/opencv.hpp>
#include "scene_retrieve.h"
#include <iostream>
#include <thread>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <roseus/StringStamped.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/core/persistence.hpp>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <ros/duration.h>

#include <image_process.h>
#include "global_frame_synchronizer.h" //同步信息用.

using namespace std;

ros::Publisher Pub;
cv::Mat* pQ_mat;
LoopClosingManager* plcm;

template<typename Out>
void split(const std::string &s, char delim, Out result) {
    std::stringstream ss(s);
    std::string item;
    while (std::getline(ss, item, delim)) {
        *(result++) = item;
    }
}
std::vector<std::string> __split(const std::string &s, char delim) {
    std::vector<std::string> elems;
    split(s, delim, std::back_inserter(elems));
    return elems;
}

std::vector<double> __split_comma_(const std::string input_str)
{
    std::vector<double> elems;
    std::vector<string> str_d_ = __split(input_str,',');
    LOG(INFO)<<"in __split_comma_,input:"<<input_str<<endl;
    for(int i = 0;i<str_d_.size();i++)
    {
        elems.push_back(stod(str_d_[i]) );
    }
    return elems;
}

GlobalFrameSyncManager* pGlobalFrameSync;
SceneRetriever* pSceneRetriever;

void StereoImageCallback(const sensor_msgs::ImageConstPtr& msgLeft,const sensor_msgs::ImageConstPtr& msgRight);

void GOG_INFO_CALLBACK(const roseus::StringStamped msg_gog) // here we use roseus::StringStamped.
{
    //auto msg_gog = *pmsg_gog;
    cout<<"in GOG_INFO_CALLBACK():"<<endl;
    LOG(INFO)<<"in GOG_INFO_CALLBACK():"<<endl;
    auto timestamp = msg_gog.header.stamp;
    string content(msg_gog.data);
    LOG(INFO)<<"in GOG_INFO_CALLBACK():original content:"<<content<<endl;
    // content contains:
    // 1.对应stamp的图像在gog_frame中的id.

    // 2.对应stamp的图像在gog中的R,t.//可选.

    //step<1> split content and do syntax analysis.
    std::vector<std::string> parts = __split(content,'|');
    std::string id_ = parts[0];
    LOG(INFO)<<"    id:"<<id_<<endl;
    std::string quat_ = parts[1];
    LOG(INFO)<<"    quat:"<<quat_<<endl;
    std::string translation_ = parts[2];
    LOG(INFO)<<"    translation:"<<translation_<<endl;
    LOG(INFO)<<"Received GOG msg,id:"<<id_<<",quat:"<<quat_<<",translation:"<<translation_<<endl;
    int id = stoi(id_);
    auto q_temp = __split_comma_(quat_);
    Quaterniond quat(q_temp[0],q_temp[1],q_temp[2],q_temp[3]);
    auto t_temp = __split_comma_(translation_);
    Vector3d translation(t_temp[0],t_temp[1],t_temp[2]);
    LOG(INFO)<<"in GOG_INFO_CALLBACK():Calling GOG_msg_callback():"<<endl;
    pGlobalFrameSync->GOG_msg_callback(msg_gog,id,pSceneRetriever->getScene().getCurrentIndex());
}

/*void whole_callback(const sensor_msgs::ImageConstPtr& l, const sensor_msgs::ImageConstPtr& r, const roseus::StringStampedPtr& m)
{
    StereoImageCallback(l,r);
    GOG_INFO_CALLBACK(m);
}*/

void StereoImageCallback(const sensor_msgs::ImageConstPtr& msgLeft,const sensor_msgs::ImageConstPtr& msgRight)
{
    LOG(INFO)<<"in StereoImageCallback()"<<endl;
    cout<<"in StereoImageCallback():"<<endl;
    std::shared_ptr<cv::Mat> pCurLeftImage(new cv::Mat), pCurRightImage(new cv::Mat);
    try
    {
        //curLeftImage = cv_bridge::toCvShare(msgLeft)->image;
        //curRightImage = cv_bridge::toCvShare(msgRight)->image;
        //auto l_im& = cv_bridge::toCvShare(msgLeft)->image;

        cv::cvtColor(cv_bridge::toCvShare(msgLeft)->image, *pCurLeftImage, CV_BGR2GRAY);
        cv::cvtColor(cv_bridge::toCvShare(msgRight)->image, *pCurRightImage, CV_BGR2GRAY);
    }
    catch(cv_bridge::Exception &e)
    {
        LOG(ERROR)<<"in StereoImageCallback:cv_bridge exception caught!"<<endl;
        ROS_ERROR("in StereoImageCallback():cv_bridge exception: %s", e.what());
        return;
    }

    if (pCurLeftImage->empty() && pCurRightImage->empty())
    {
        LOG(WARNING)<<"Empty image got in ros_global_optimization StereoImageCallback()."<<endl;
        return;
    }
    pGlobalFrameSync->Image_msg_callback(msgLeft->header.stamp,pCurLeftImage,pCurRightImage);//do loop closing check inside this callback;
    /*
    LOG(INFO)<<"Forming empty rt mat."<<endl;
    cv::Mat r_mat = cv::Mat::eye(3,3,CV_32F);
    cv::Mat t_mat = cv::Mat::zeros(3,1,CV_32F);
    LOG(INFO)<<"Generating frame..."<<endl;
    bool success_gen = false;
    auto current_scene_frame = generateSceneFrameFromStereoImage(curLeftImage,curRightImage,r_mat,t_mat,*pQ_mat,*plcm,success_gen);
    if(!success_gen)
    {
        LOG(INFO)<<"    after generateSceneFrameFromStereoImage():generate failed!"<<endl;
        return;
    }
    LOG(INFO)<<"adding frame into scene..."<<endl;
    pSceneRetriever->addFrameToScene(std::get<0>(current_scene_frame),std::get<1>(current_scene_frame),std::get<2>(current_scene_frame),std::get<3>(current_scene_frame),std::get<4>(current_scene_frame));
    int scene_frame_count = pSceneRetriever->getScene().getImageCount();
    LOG(INFO)<<"In ImageCallback():adding frame to scene by stereo..."<<endl;
    bool match_success;
    cv::Mat RT_mat;
    //int inliers = pSceneRetriever->retrieveSceneFromStereoImage(curLeftImage, curRightImage, *pQ_mat, RT_mat, match_success);
    cv::Mat cam_matrix = (cv::Mat_<float >(3,3) << 376, 0, 376,
            0, 376, 240,
            0, 0, 1);
    int loop_id;
    int inliers = pSceneRetriever->retrieveSceneWithScaleFromMonoImage(curLeftImage,cam_matrix,RT_mat,match_success,&loop_id);

    if(match_success)
    {
        LOG(INFO)<<"In scene_retrieving_ros ImageCallback():Match success! RT mat is: \n"<<RT_mat<<endl;
        //TODO:publish RT mat!
        std_msgs::String str;
        stringstream ss;
        std::string str_content;
        ss<<"Frame id:"<<loop_id<<","<<scene_frame_count<<";RT:"<<RT_mat;
        //ss>>str_content;
        str_content = ss.str();
        str.data = str_content.c_str();
        Pub.publish(str);
    }
    else
    {
        LOG(INFO)<<"Match failed."<<endl;
        return;
    }

*/

/*
    cv::Mat RT_mat, Q_mat;
    bool match_success;
    int inliers = pSceneRetriever->retrieveSceneFromStereoImage(curLeftImage, curRightImage, Q_mat, RT_mat, match_success);
    //step<3> form loop msg and publish to GOG.
    if(match_success)
    {
        cout<<" Match success! RT mat is: \n"<<RT_mat<<endl;
        std_msgs::String str;
        stringstream ss;
        std::string str_content;
        ss<<"Frame id:"<<0<<","<<1<<";RT:"<<RT_mat;
        ss>>str_content;
        str.data = str_content.c_str();
        Pub.publish(str);
    }
    else
    {
        return;
    }
*/
}

int main_of_backend_thread()
{
    LOG(INFO) << "Starting backend thread."<<endl;
    /*try
    {
        int *p = (int*)0;
        cout<<"create nullpoint deref error."<<endl;
        cout<<"*p = "<<*p<<endl;
    }
    catch(...)
    {
        cout<<"error caught!"<<endl;
        return -1;
    }*/

    //try
    {
        while(true)
        {
            LOG(INFO) << "In backend thread:loop running."<<endl;
            bool match_success_out = false;
            cv::Mat camera_Q_mat;
            cv::Mat RT_mat_out;
            auto ret_val = pGlobalFrameSync->checkLoopTaskQueue(match_success_out,camera_Q_mat,RT_mat_out);
            if(match_success_out)
            {
                LOG(INFO)<<"in main_of_backend_thread():match success!"<<endl;
                LOG(INFO)<<"Match success in backend thread,id:"<<std::get<0>(ret_val)<<","<<std::get<1>(ret_val)<<".RT_mat:"<<RT_mat_out<<endl;
                //cout<<" Match success! RT mat is: \n"<<RT_mat<<endl;
                std_msgs::String str;
                stringstream ss;
                std::string str_content;
                ss<<"Frame id:"<<0<<","<<1<<";RT:"<<RT_mat_out;
                ss>>str_content;
                str.data = str_content.c_str();
                Pub.publish(str);
            }
            {
                LOG(INFO)<<"Match failed sleep 0.1s."<<endl;
                cout<<"sleep 0.1s."<<endl;
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
    }
    //catch(...)
    //{
    //    LOG(ERROR) << "Exiting concurrent thread."<<endl;
    //}
}

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

    google::InitGoogleLogging(argv[0]);
    if (argc!=3)
    {
        //cout<<"Usage: demo [scene_file_path] [voc_file_path] [l_image_path] [r_image_path] [Q_mat_file_path]"<<endl;
        LOG(INFO)<<"Usage: demo [voc_file_path] [Q_mat_file_path]"<<endl;
    }
    std::string voc_file_path(argv[1]) ,Q_mat_path(argv[2]);
    LoopClosingManager lcm(voc_file_path);
    plcm = &lcm;
    LOG(INFO)<<"voc_file_path: "<<voc_file_path<<endl;
    LOG(INFO)<<"Q_mat_path: "<<Q_mat_path<<endl;

    cv::FileStorage fsSettings(Q_mat_path, cv::FileStorage::READ);
    cv::Mat Q_mat;
    fsSettings["Q_mat"] >> Q_mat;

    if (Q_mat.empty())
    {
        LOG(INFO)<<"Q mat empty, exit."<<endl;
        return -1;
    }

    LOG(INFO)<<"Q_mat: "<<endl<<Q_mat<<endl;
    pQ_mat = &Q_mat;

    cv::Mat RT_mat = (cv::Mat_<float >(4,4) << 1, 0, 0, 0,
            0, 1, 0, 1,
            0, 0, 1, 0,
            0, 0, 0, 1);

    /*cout<<"Usage: ros_global_optimization [CONFIG_FILE_PATH]"<<endl;
    if (argc!=2)
    {
        cout <<"Please check input format."<<endl;
        exit(-1);
    }
    std::string config_file_path(argv[1]);
    cv::FileStorage fSettings(config_file_path,cv::FileStorage::READ);
    cv::Mat Q_mat;
    fSettings["Q_mat"] >> Q_mat;
    if (Q_mat.empty())
    {
        cout<<"Q mat empty, exit."<<endl;
        return -1;
    }
    std::string voc_file_path(fSettings["VOC_FILE_PATH"]);
    std::string scene_path;
    //std::string scene_path(argv[1]), voc_file_path(argv[2]) , l_img_path(argv[3]), r_img_path(argv[4]), Q_mat_path(argv[5]);

    cv::Mat RT_mat = (cv::Mat_<float >(4,4) << 1, 0, 0, 0,
            0, 1, 0, 1,
            0, 0, 1, 0,
            0, 0, 0, 1);*/

    //ros init
    ros::init(argc, argv, "scene_retrieve");
    ros::NodeHandle nh;

    Pub = nh.advertise<std_msgs::String>("/gaas/ros_gog_scene_retrieving",10);
    //std::shared_ptr<SceneRetriever> pSceneRetrieve(new SceneRetriever(voc_file_path));//, scene_path));
    //std::shared_ptr<SceneRetriever> pSceneRetrieve(new DynamicalSceneRetriever(voc_file_path));//先试试修改原来的类,不加入新的.
    //pSceneRetriever = pSceneRetrieve;
    pSceneRetriever = new SceneRetriever(voc_file_path);
    pGlobalFrameSync = new GlobalFrameSyncManager(pSceneRetriever,pQ_mat,plcm);

    ros::Subscriber gog_subscriber = nh.subscribe("/gaas/global_optimization_graph/state", 30,GOG_INFO_CALLBACK);
    message_filters::Subscriber<sensor_msgs::Image> left_sub(nh, "/gi/simulation/left/image_raw", 10);
    message_filters::Subscriber<sensor_msgs::Image> right_sub(nh, "/gi/simulation/right/image_raw", 10);
    //message_filters::Subscriber<roseus::StringStamped> global_optimization_graph_sub(nh,"/gi/global_optimization/optimization_result",30);

    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_pol;
    message_filters::Synchronizer<sync_pol> sync(sync_pol(10), left_sub, right_sub);//,global_optimization_graph_sub);
    sync.setMaxIntervalDuration(ros::Duration(0.01));
    sync.registerCallback(boost::bind(StereoImageCallback, _1, _2));
    //sync.registerCallback(boost::bind(whole_callback,_1,_2,_3));
    //sync.registerCallback(whole_callback);
    std::thread backend_thread(main_of_backend_thread);
    ros::spin();
    return 0;
}


/**
 * Copyright (c) 2017 Melown Technologies SE
 *
 * 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.
 *
 * 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 HOLDER 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.
 */

#include <new>
#include <algorithm>
#include <cstring>

#include <boost/iostreams/stream_buffer.hpp>
#include <boost/iostreams/device/array.hpp>

#include <opencv2/highgui/highgui.hpp>

#include <sqlite3.h>

#include <ogrsf_frmts.h>

#include "imgproc/rastermask/cvmat.hpp"

#include "geo/verticaladjuster.hpp"
#include "geo/csconvertor.hpp"
#include "geo/srs.hpp"

#include "../error.hpp"
#include "../support/geo.hpp"
#include "./operations.hpp"

namespace bio = boost::iostreams;
namespace vr = vtslibs::registry;

namespace {

cv::Mat* allocateMat(ManagedBuffer &mb
                     , const math::Size2 &size, int type)
{
    // calculate sizes
    const auto dataSize(math::area(size) * CV_ELEM_SIZE(type));
    const auto matSize(sizeof(cv::Mat) + dataSize);

    // create raw memory to hold matrix and data
    char *raw(static_cast<char*>(mb.allocate(matSize)));

    // allocate matrix in raw data block
    return new (raw) cv::Mat(size.height, size.width, type
                             , raw + sizeof(cv::Mat));
}

cv::Mat* warpImage(DatasetCache &cache, ManagedBuffer &mb
                   , const std::string &dataset
                   , const geo::SrsDefinition &srs
                   , const math::Extents2 &extents
                   , const math::Size2 &size
                   , geo::GeoDataset::Resampling resampling
                   , const boost::optional<std::string> &maskDataset
                   , bool optimize)
{
    auto &src(cache(dataset));
    auto dst(geo::GeoDataset::deriveInMemory(src, srs, size, extents));
    src.warpInto(dst, resampling);

    if (optimize && dst.cmask().empty()) {
        throw EmptyImage("No valid data.");
    }

    // apply mask set if defined
    if (maskDataset) {
        auto &srcMask(cache(*maskDataset));
        auto dstMask(geo::GeoDataset::deriveInMemory
                     (srcMask, srs, size, extents));
        srcMask.warpInto(dstMask, resampling);
        dst.applyMask(dstMask.cmask());

        if (optimize && dst.cmask().empty()) {
            throw EmptyImage("No valid data.");
        }
    }

    // grab destination
    auto dstMat(dst.cdata());
    auto type(CV_MAKETYPE(CV_8U, dstMat.channels()));

    auto *tile(allocateMat(mb, size, type));
    dstMat.convertTo(*tile, type);
    return tile;
}

cv::Mat* warpMask(DatasetCache &cache, ManagedBuffer &mb
                  , const std::string &dataset
                  , const geo::SrsDefinition &srs
                  , const math::Extents2 &extents
                  , const math::Size2 &size
                  , geo::GeoDataset::Resampling resampling
                  , bool optimize)
{
    auto &src(cache(dataset));
    auto dst(geo::GeoDataset::deriveInMemory(src, srs, size, extents));
    src.warpInto(dst, resampling);

    // fetch mask from dataset (optimized, all valid -> invalid matrix)
    auto m(dst.fetchMask(optimize));
    if (!m.data) {
        // all pixels valid
        throw FullImage("All data valid.");
    }

    if (optimize) {
        auto nonzero(cv::countNonZero(m));
        if (!nonzero) {
            // empty mask -> no valid data
            throw EmptyImage("No valid data.");
        } else if (nonzero == area(size)) {
            // all pixels valid
            throw FullImage("All data valid.");
        }
    }

    auto *mask(allocateMat(mb, size, m.type()));
    m.copyTo(*mask);
    return mask;
}

cv::Mat* warpDetailMask(DatasetCache &cache, ManagedBuffer &mb
                        , const std::string &dataset
                        , const geo::SrsDefinition &srs
                        , const math::Extents2 &extents
                        , const math::Size2 &size)
{
    // generate metatile from mask dataset
    auto &srcMask(cache(dataset));
    auto dstMask(geo::GeoDataset::deriveInMemory
                 (srcMask, srs, size, extents, boost::none
                  , geo::GeoDataset::NodataValue()));

    geo::GeoDataset::WarpOptions wo;
    wo.srcNodataValue = geo::GeoDataset::NodataValue();
    wo.dstNodataValue = geo::GeoDataset::NodataValue();
    srcMask.warpInto(dstMask, geo::GeoDataset::Resampling::average, wo);

    // mask is guaranteed to have single (double) channel
    auto &dstMat(dstMask.cdata());
    auto *tile(allocateMat(mb, size, dstMat.type()));
    dstMat.copyTo(*tile);
    return tile;
}

const auto ForcedNodata(geo::GeoDataset::NodataValue(-1e10f));

cv::Mat* warpValueMinMax(DatasetCache &cache, ManagedBuffer &mb
                         , const std::string &dataset
                         , const geo::SrsDefinition &srs
                         , const math::Extents2 &extents
                         , const math::Size2 &size
                         , geo::GeoDataset::Resampling resampling)
{
    // combined result of warped dataset and result of warpMinMax
    auto &src(cache(dataset));
    auto &minSrc(cache(dataset + ".min"));
    auto &maxSrc(cache(dataset + ".max"));

    auto dst(geo::GeoDataset::deriveInMemory
             (src, srs, size, extents, GDT_Float32, ForcedNodata));
    auto minDst(geo::GeoDataset::deriveInMemory
                (minSrc, srs, size, extents, GDT_Float32, ForcedNodata));
    auto maxDst(geo::GeoDataset::deriveInMemory
                (maxSrc, srs, size, extents, GDT_Float32, ForcedNodata));

    geo::GeoDataset::WarpOptions warpOptions;
#if GDAL_VERSION_NUM >= 2020000
    // choose finer overview on GDAL>=2.2, since there is something rotten there
    warpOptions.overviewBias = -1;
#endif

    auto wri(src.warpInto(dst, resampling, warpOptions));
    minSrc.warpInto(minDst, geo::GeoDataset::Resampling::minimum
                    , warpOptions);
    maxSrc.warpInto(maxDst, geo::GeoDataset::Resampling::maximum
                    , warpOptions);

    // combine data
    auto *tile(allocateMat(mb, size, CV_64FC3));
    *tile = cv::Scalar(*ForcedNodata, *ForcedNodata, *ForcedNodata);

    {
        // TODO: use masks (get them as a byte matrices)
        const auto &d(dst.cdata());
        const auto &dmin(minDst.cdata());
        const auto &dmax(maxDst.cdata());

        auto id(d.begin<double>());
        auto idmin(dmin.begin<double>());
        auto idmax(dmax.begin<double>());

        for (auto itile(tile->begin<cv::Vec3d>())
                 , etile(tile->end<cv::Vec3d>());
             itile != etile; ++itile, ++id, ++idmin, ++idmax)
        {
            // skip invalid value
            auto value(*id);
            if (value == ForcedNodata) { continue; }

            auto &sample(*itile);
            sample[0] = value;

            if ((*idmin == ForcedNodata) || (*idmin > value)) {
                // clone value into minimum if minimum is invalid or above value
                sample[1] = value;
            } else {
                // copy min
                sample[1] = *idmin;
            }

            if ((*idmax == ForcedNodata) || (*idmax < value)) {
                // clone value into maximum if maximum is invalid or below value
                sample[2] = value;
            } else {
                // copy max
                sample[2] = *idmax;
            }
        }
    }

    return tile;
}

cv::Mat* warpDem(DatasetCache &cache, ManagedBuffer &mb
                 , const std::string &dataset
                 , const geo::SrsDefinition &srs
                 , const math::Extents2 &extents
                 , const math::Size2 &requestedSize
                 , bool optimize)
{
    auto &src(cache(dataset));

    // calculate size of dataset
    auto size([&]() -> math::Size2
    {
        if (!optimize) { return requestedSize; }

        auto pxc(tileCircumference(extents, srs, src));
        // 1) divide circumference by 4 to get (average) length of one side
        // 2) use 12 samples per one souce pixel
        // 4) clip result to requesed size and 2
        int samples(std::round(12.0 * pxc / 4.0));
        return math::Size2
            (std::max(std::min(samples, requestedSize.width), 2)
             , std::max(std::min(samples, requestedSize.height), 2));
    }());

    // simulate grid registration
    math::Size2 gridSize(size.width + 1, size.height + 1);
    auto gridExtents(extentsPlusHalfPixel(extents, size));

    // warp in floats
    auto dst(geo::GeoDataset::deriveInMemory
             (src, srs, gridSize, gridExtents, ::GDT_Float32
              , ForcedNodata));

    auto wri(src.warpInto(dst, geo::GeoDataset::Resampling::dem));
    LOG(info1) << "Warp result: scale=" << wri.scale
               << ", resampling=" << wri.resampling << ".";

    // mask is guaranteed to have single (double) channel
    auto &dstMat(dst.cdata());
    auto *tile(allocateMat(mb, gridSize, dstMat.type()));
    dstMat.copyTo(*tile);
    return tile;
}

} // namespace

cv::Mat* warp(DatasetCache &cache, ManagedBuffer &mb
              , const GdalWarper::RasterRequest &req)
{
    typedef GdalWarper::RasterRequest::Operation Operation;

    switch (req.operation) {
    case Operation::image:
    case Operation::imageNoOpt:
        return warpImage
            (cache, mb, req.dataset, req.srs, req.extents, req.size
             , req.resampling, req.mask
             , (req.operation == Operation::image));

    case Operation::mask:
    case Operation::maskNoOpt:
        return warpMask
            (cache, mb, req.dataset, req.srs, req.extents, req.size
             , req.resampling, (req.operation == Operation::mask));

    case Operation::detailMask:
        return warpDetailMask
            (cache, mb, req.dataset, req.srs, req.extents, req.size);

    case Operation::dem:
    case Operation::demOptimal:
        return warpDem
            (cache, mb, req.dataset, req.srs, req.extents, req.size
             , (req.operation == Operation::demOptimal));

    case Operation::valueMinMax:
        return warpValueMinMax
            (cache, mb, req.dataset, req.srs, req.extents, req.size
             , req.resampling);
    }
    throw;
}

namespace {

typedef std::shared_ptr< ::GDALDataset> VectorDataset;

class OptionsWrapper {
public:
    OptionsWrapper() : opts_() {}

    ~OptionsWrapper() { ::CSLDestroy(opts_); }

    operator char**() const { return opts_; }

    OptionsWrapper& operator()(const char *name, const char *value) {
        opts_ = ::CSLSetNameValue(opts_, name, value);
        return *this;
    }

    template <typename T>
    OptionsWrapper& operator()(const char *name, const T &value) {
        return operator()
            (name, boost::lexical_cast<std::string>(value).c_str());
    }

    OptionsWrapper& operator()(const char *name, bool value) {
        return operator()(name, value ? "YES" : "NO");
    }

    OptionsWrapper& operator()(const std::string &pair) {
        opts_ = ::CSLAddString(opts_, pair.c_str());
        return *this;
    }

private:
    char **opts_;
};

VectorDataset openVectorDataset(const std::string &dataset
                                , const OptionsWrapper &openOptions)
{
    auto ds(::GDALOpenEx(dataset.c_str(), (GDAL_OF_VECTOR | GDAL_OF_READONLY)
                         , nullptr, openOptions, nullptr));

    if (!ds) {
        const auto code(::CPLGetLastErrorNo());
        if (code == CPLE_OpenFailed) {
            LOGTHROW(err2, EmptyGeoData)
                << "No file found for " << dataset << ".";
        }

        LOGTHROW(err2, std::runtime_error)
            << "Failed to open dataset " << dataset << " ("
            << ::CPLGetLastErrorNo() << ").";
    }

    return VectorDataset(static_cast< ::GDALDataset*>(ds)
                         , [](::GDALDataset *ds) { delete ds; });
}

VectorDataset openVectorDataset(const std::string &dataset
                                , const geo::heightcoding::Config&
                                , const GdalWarper::OpenOptions &openOptions)
{
    OptionsWrapper ow;
    for (const auto &option : openOptions) { ow(option); }
    return openVectorDataset(dataset, ow);
}

GdalWarper::Heightcoded*
allocateHc(ManagedBuffer &mb
           , const std::string &data
           , const geo::heightcoding::Metadata &metadata)
{
    // create raw memory to hold block and data
    char *raw(static_cast<char*>
              (mb.allocate(sizeof(GdalWarper::Heightcoded) + data.size())));

    // poiter to output data
    auto *dataPtr(raw + sizeof(GdalWarper::Heightcoded));

    // copy data into block
    std::copy(data.begin(), data.end(), dataPtr);

    // allocate block in raw data block
    return new (raw) GdalWarper::Heightcoded
        (dataPtr, data.size(), metadata);
}

struct DbError : public std::runtime_error {
    DbError(const std::string &msg) : std::runtime_error(msg) {}
};

class SQLStatement {
public:
    SQLStatement() : stmt_() {}
    ~SQLStatement() { if (stmt_) { ::sqlite3_finalize(stmt_); } }
    operator ::sqlite3_stmt*() { return stmt_; }
    operator ::sqlite3_stmt**() { return &stmt_; }

private:
    ::sqlite3_stmt *stmt_;
};

typedef geo::FeatureLayers::Features::Properties FeatureProperties;

class EnhanceDatabase {
public:
    EnhanceDatabase(const std::string &path
                    , const std::string &table)
        : path_(path), table_(table), db_()
    {
        check(::sqlite3_open_v2
              (path.c_str(), &db_, SQLITE_OPEN_READONLY, nullptr)
              , "sqlite3_open_v2");

        std::ostringstream os;
        os << "SELECT * FROM `" << table << "` WHERE `id`=?";

        const auto &str(os.str());
        check(::sqlite3_prepare_v2(db_, str.data(), str.size()
                                , select_, nullptr)
              , "sqlite3_prepare");
    }

    ~EnhanceDatabase() { if (db_) { ::sqlite3_close(db_); } }

    void enhance(FeatureProperties &properties, const std::string &id) {
        check(::sqlite3_reset(select_), "sqlite3_reset");
        check(::sqlite3_bind_text(select_, 1, id.data(), id.size(), nullptr)
              , "sqlite3_bind_text");

        switch (auto res = ::sqlite3_step(select_)) {
        case SQLITE_ROW: break;

        case SQLITE_DONE:
            // nothing found
            return;

        default:
            check(res, "sqlite3_step");
        }

        // process all columns
        const int columns(::sqlite3_column_count(select_));
        for (int column(0); column < columns; ++column) {
            const auto name(::sqlite3_column_name(select_, column));
            // skip id itself
            if (!std::strcmp(name, "id")) { continue; }
            const auto *text(reinterpret_cast<const char*>
                             (::sqlite3_column_text(select_, column)));
            const auto size(::sqlite3_column_bytes(select_, column));
            properties.insert(FeatureProperties::value_type
                              (name, std::string(text, size)));
        }
    }

private:
    void check(int status, const char *what) const {
        if (status) {
            const char *msg(::sqlite3_errmsg(db_));
            LOGTHROW(err1, DbError)
                << "Sqlite3 operation " << what << " failed: <"
                << msg << "> (file \"" << path_ << "\").";
        }
    }

    const std::string path_;
    const std::string table_;
    ::sqlite3 *db_;
    SQLStatement select_;
};

void enhanceLayer(const LayerEnhancer &enhancer
                   , geo::FeatureLayers::Layer &layer)
{
    EnhanceDatabase db(enhancer.databasePath, enhancer.table);

    (void) db;

    const auto manipulator([&](FeatureProperties &properties)
    {
        const auto fkey(properties.find(enhancer.key));
        if (fkey == properties.end()) { return; }
        db.enhance(properties, fkey->second);
    });

    layer.features.updateProperties(manipulator);
}

void enhanceLayers(const LayerEnhancer::map &layerEnancers
                   , geo::FeatureLayers &layers)
{
    for (auto &layer : layers.layers) {
        auto flayerEnancers(layerEnancers.find(layer.name));
        if (flayerEnancers == layerEnancers.end()) { continue; }

        const auto &enhancer(flayerEnancers->second);
        enhanceLayer(enhancer, layer);
    }
}

GdalWarper::Heightcoded*
heightcode(ManagedBuffer &mb, const VectorDataset &vds
           , std::vector<const geo::GeoDataset*> rds
           , geo::heightcoding::Config config
           , const boost::optional<std::string> &geoidGrid
           , const boost::optional<std::string> &vectorGeoidGrid
           , const LayerEnhancer::map &layerEnancers)
{
    if (geoidGrid) {
        // apply geoid grid to SRS of rasterDs and set to rasterDsSrs
        config.rasterDsSrs = geo::setGeoid(rds.back()->srs(), *geoidGrid);
    }

    if (vectorGeoidGrid && vds->GetLayerCount()) {
        if (auto ref = vds->GetLayer(0)->GetSpatialRef()) {
            // set vertical srs
            config.vectorDsSrs
                = geo::SrsDefinition::fromReference
                (geo::setGeoid(*ref, *vectorGeoidGrid));
        }
    }

    if (!layerEnancers.empty()) {
        config.postprocess = [&](geo::FeatureLayers &layers) -> void {
            enhanceLayers(layerEnancers, layers);
        };
    }

    std::ostringstream os;
    auto metadata(geo::heightcoding::heightCode(*vds, rds, os, config));

    return allocateHc(mb, os.str(), metadata);
}

} // namespace

GdalWarper::Heightcoded*
heightcode(DatasetCache &cache, ManagedBuffer &mb
           , const std::string &vectorDs
           , const DemDataset::list &rasterDs
           , geo::heightcoding::Config config
           , const boost::optional<std::string> &vectorGeoidGrid
           , const GdalWarper::OpenOptions &openOptions
           , const LayerEnhancer::map &layerEnancers)
{
    std::vector<const geo::GeoDataset*> rasterDsStack;
    for (const auto &ds : rasterDs) {
        rasterDsStack.push_back(&cache(ds.dataset));
    }

    return heightcode(mb, openVectorDataset(vectorDs, config, openOptions)
                      , rasterDsStack
                      , config, rasterDs.back().geoidGrid
                      , vectorGeoidGrid, layerEnancers);
}


#pragma once

#include "generator/feature_builder.hpp"
#include "generator/place_node.hpp"

#include "geometry/point2d.hpp"
#include "geometry/tree4d.hpp"

#include "base/geo_object_id.hpp"

#include <cstddef>
#include <string>
#include <unordered_map>
#include <vector>

#include <boost/geometry.hpp>
#include <boost/optional.hpp>

namespace generator_tests
{
class TestPopularityBuilder;
}  // namespace generator_tests

namespace generator
{
namespace popularity
{
// These are functions for generating a csv file for popularity.
// dataFilename - A path to data file
// dataDir - A path to the directory where the data files are located
// dataFilenames - Paths to data files
// cpuCount - A number of processes
// outFilename - A path where the csv file will be saved

// Csv format:
// Id;Parent id;Lat;Lon;Main type;Name
// 9223372036936022489;;42.996411;41.004747;leisure-park;Сквер им. И. А. Когония
// 9223372037297546235;;43.325002;40.224941;leisure-park;Приморский парк
// 9223372036933918763;;43.005177;41.022295;leisure-park;Сухумский ботанический сад
void BuildPopularitySrcFromData(std::string const & dataFilename, std::string const & outFilename);

void BuildPopularitySrcFromAllData(std::string const & dataDir, std::string const & outFilename,
                                   size_t cpuCount = 1);

void BuildPopularitySrcFromAllData(std::vector<std::string> const & dataFilenames, std::string const & outFilename,
                                   size_t cpuCount = 1);

class PopularityGeomPlace
{
public:
  explicit PopularityGeomPlace(feature::FeatureBuilder const & feature);

  bool Contains(PopularityGeomPlace const & smaller) const;
  bool Contains(m2::PointD const & point) const;
  feature::FeatureBuilder const & GetFeature() const { return m_feature; }
  double GetArea() const { return m_area; }
  base::GeoObjectId GetId() const { return m_id; }

private:
  using BoostPoint = boost::geometry::model::point<double, 2, boost::geometry::cs::cartesian>;
  using BoostPolygon = boost::geometry::model::polygon<BoostPoint>;

  base::GeoObjectId m_id;
  std::reference_wrapper<feature::FeatureBuilder const> m_feature;
  std::unique_ptr<BoostPolygon> m_polygon;
  double m_area;
};

struct PopularityLine
{
  base::GeoObjectId m_id;
  boost::optional<base::GeoObjectId> m_parent;
  m2::PointD m_center;
  std::string m_type;
  std::string m_name;
};

class PopularityBuilder
{
public:
  friend class generator_tests::TestPopularityBuilder;

  explicit PopularityBuilder(std::string const & dataFilename);

  std::vector<PopularityLine> Build() const;

private:
  using Node = PlaceNode<PopularityGeomPlace>;
  using Tree4d = m4::Tree<base::GeoObjectId>;
  using MapIdToNode = std::unordered_map<base::GeoObjectId, Node::Ptr>;

  static std::string GetType(feature::FeatureBuilder const & feature);
  static std::string GetFeatureName(feature::FeatureBuilder const & feature);
  static void FillLinesFromPointObjects(std::vector<feature::FeatureBuilder> const & pointObjs, MapIdToNode const & m,
                                        Tree4d const & tree, std::vector<PopularityLine> & lines);
  static boost::optional<base::GeoObjectId>
  FindPointParent(m2::PointD const & point, MapIdToNode const & m, Tree4d const & tree);
  static boost::optional<Node::Ptr>
  FindPopularityGeomPlaceParent(PopularityGeomPlace const & place, MapIdToNode const & m,
                                Tree4d const & tree);
  static MapIdToNode GetAreaMap(Node::PtrList const & nodes);
  static Tree4d MakeTree4d(Node::PtrList const & nodes);
  static void FillLineFromGeomObjectPtr(PopularityLine & line, Node::Ptr const & node);
  static void FillLinesFromGeomObjectPtrs(Node::PtrList const & nodes,
                                          std::vector<PopularityLine> & lines);
  static void LinkGeomPlaces(MapIdToNode const & m, Tree4d const & tree, Node::PtrList & nodes);
  static Node::PtrList MakeNodes(std::vector<feature::FeatureBuilder> const & features);

  std::string m_dataFilename;
};
}  // namespace popularity
}  // namespace generator


///////////////////////////////////////////////////////////////////////////////
// as_quantifier.hpp
//
//  Copyright 2008 Eric Niebler. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_XPRESSIVE_DETAIL_STATIC_TRANSFORMS_AS_QUANTIFIER_HPP_EAN_04_01_2007
#define BOOST_XPRESSIVE_DETAIL_STATIC_TRANSFORMS_AS_QUANTIFIER_HPP_EAN_04_01_2007

// MS compatible compilers support #pragma once
#if defined(_MSC_VER)
#pragma once
#endif

#include <boost/mpl/assert.hpp>
#include <boost/proto/core.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/xpressive/detail/detail_fwd.hpp>
#include <boost/xpressive/detail/static/static.hpp>

namespace boost
{
namespace xpressive
{
namespace detail
{
///////////////////////////////////////////////////////////////////////////////
// generic_quant_tag
template <uint_t Min, uint_t Max>
struct generic_quant_tag
{
    typedef mpl::integral_c<uint_t, Min> min_type;
    typedef mpl::integral_c<uint_t, Max> max_type;
};
} // namespace detail
} // namespace xpressive
} // namespace boost

namespace boost
{
namespace xpressive
{
namespace grammar_detail
{
using detail::uint_t;

///////////////////////////////////////////////////////////////////////////////
// min_type / max_type
template <typename Tag>
struct min_type : Tag::min_type
{
};

template <>
struct min_type<proto::tag::unary_plus> : mpl::integral_c<uint_t, 1>
{
};

template <>
struct min_type<proto::tag::dereference> : mpl::integral_c<uint_t, 0>
{
};

template <>
struct min_type<proto::tag::logical_not> : mpl::integral_c<uint_t, 0>
{
};

template <typename Tag>
struct max_type : Tag::max_type
{
};

template <>
struct max_type<proto::tag::unary_plus> : mpl::integral_c<uint_t, UINT_MAX - 1>
{
};

template <>
struct max_type<proto::tag::dereference> : mpl::integral_c<uint_t, UINT_MAX - 1>
{
};

template <>
struct max_type<proto::tag::logical_not> : mpl::integral_c<uint_t, 1>
{
};

///////////////////////////////////////////////////////////////////////////////
// as_simple_quantifier
template <typename Grammar, typename Greedy,
          typename Callable = proto::callable>
struct as_simple_quantifier
    : proto::transform<as_simple_quantifier<Grammar, Greedy, Callable>>
{
    template <typename Expr, typename State, typename Data>
    struct impl : proto::transform_impl<Expr, State, Data>
    {
        typedef typename proto::result_of::child<Expr>::type arg_type;

        typedef
            typename Grammar::template impl<arg_type, detail::true_xpression,
                                            Data>::result_type xpr_type;

        typedef detail::simple_repeat_matcher<xpr_type, Greedy> matcher_type;

        typedef typename proto::terminal<matcher_type>::type result_type;

        result_type operator()(typename impl::expr_param expr,
                               typename impl::state_param,
                               typename impl::data_param data) const
        {
            xpr_type xpr =
                typename Grammar::template impl<arg_type,
                                                detail::true_xpression, Data>()(
                    proto::child(expr), detail::true_xpression(), data);

            typedef typename impl::expr expr_type;
            matcher_type matcher(
                xpr, (uint_t)min_type<typename expr_type::proto_tag>::value,
                (uint_t)max_type<typename expr_type::proto_tag>::value,
                xpr.get_width().value());

            return result_type::make(matcher);
        }
    };
};

///////////////////////////////////////////////////////////////////////////////
// add_hidden_mark
struct add_hidden_mark : proto::transform<add_hidden_mark>
{
    template <typename Expr, typename State, typename Data>
    struct impl : proto::transform_impl<Expr, State, Data>
    {
        typedef typename impl::expr expr_type;
        typedef typename shift_right<
            terminal<detail::mark_begin_matcher>::type,
            typename shift_right<
                Expr, terminal<detail::mark_end_matcher>::type>::type>::type
            result_type;

        result_type operator()(typename impl::expr_param expr,
                               typename impl::state_param,
                               typename impl::data_param data) const
        {
            // we're inserting a hidden mark ... so grab the next hidden mark
            // number.
            int mark_nbr = data.get_hidden_mark();
            detail::mark_begin_matcher begin(mark_nbr);
            detail::mark_end_matcher end(mark_nbr);

            result_type that = {{begin}, {expr, {end}}};
            return that;
        }
    };
};

///////////////////////////////////////////////////////////////////////////////
// InsertMark
struct InsertMark : or_<when<proto::assign<detail::basic_mark_tag, _>, _>,
                        otherwise<add_hidden_mark>>
{
};

///////////////////////////////////////////////////////////////////////////////
// as_default_quantifier_impl
template <typename Greedy, uint_t Min, uint_t Max>
struct as_default_quantifier_impl
    : proto::transform<as_default_quantifier_impl<Greedy, Min, Max>>
{
    template <typename Expr, typename State, typename Data>
    struct impl : proto::transform_impl<Expr, State, Data>
    {
        typedef typename proto::result_of::child<Expr>::type xpr_type;

        typedef typename InsertMark::impl<xpr_type, State, Data>::result_type
            marked_sub_type;

        typedef typename shift_right<
            terminal<detail::repeat_begin_matcher>::type,
            typename shift_right<marked_sub_type,
                                 typename terminal<detail::repeat_end_matcher<
                                     Greedy>>::type>::type>::type result_type;

        result_type operator()(typename impl::expr_param expr,
                               typename impl::state_param state,
                               typename impl::data_param data) const
        {
            // Ensure this sub-expression is book-ended with mark matchers
            marked_sub_type marked_sub =
                InsertMark::impl<xpr_type, State, Data>()(proto::child(expr),
                                                          state, data);

            // Get the mark_number from the begin_mark_matcher
            int mark_number =
                proto::value(proto::left(marked_sub)).mark_number_;
            BOOST_ASSERT(0 != mark_number);

            typedef typename impl::expr expr_type;
            uint_t min_ = (uint_t)min_type<typename expr_type::proto_tag>();
            uint_t max_ = (uint_t)max_type<typename expr_type::proto_tag>();

            detail::repeat_begin_matcher begin(mark_number);
            detail::repeat_end_matcher<Greedy> end(mark_number, min_, max_);

            result_type that = {{begin}, {marked_sub, {end}}};
            return that;
        }
    };
};

///////////////////////////////////////////////////////////////////////////////
// optional_tag
template <typename Greedy>
struct optional_tag
{
};

///////////////////////////////////////////////////////////////////////////////
// as_default_optional
template <typename Grammar, typename Greedy,
          typename Callable = proto::callable>
struct as_default_optional
    : proto::transform<as_default_optional<Grammar, Greedy, Callable>>
{
    template <typename Expr, typename State, typename Data>
    struct impl : proto::transform_impl<Expr, State, Data>
    {
        typedef detail::alternate_end_xpression end_xpr;

        typedef detail::optional_matcher<
            typename Grammar::template impl<Expr, end_xpr, Data>::result_type,
            Greedy>
            result_type;

        result_type operator()(typename impl::expr_param expr,
                               typename impl::state_param,
                               typename impl::data_param data) const
        {
            return result_type(
                typename Grammar::template impl<Expr, end_xpr, Data>()(
                    expr, end_xpr(), data));
        }
    };
};

///////////////////////////////////////////////////////////////////////////////
// as_mark_optional
template <typename Grammar, typename Greedy,
          typename Callable = proto::callable>
struct as_mark_optional
    : proto::transform<as_mark_optional<Grammar, Greedy, Callable>>
{
    template <typename Expr, typename State, typename Data>
    struct impl : proto::transform_impl<Expr, State, Data>
    {
        typedef detail::alternate_end_xpression end_xpr;

        typedef detail::optional_mark_matcher<
            typename Grammar::template impl<Expr, end_xpr, Data>::result_type,
            Greedy>
            result_type;

        result_type operator()(typename impl::expr_param expr,
                               typename impl::state_param,
                               typename impl::data_param data) const
        {
            int mark_number = proto::value(proto::left(expr)).mark_number_;

            return result_type(
                typename Grammar::template impl<Expr, end_xpr, Data>()(
                    expr, end_xpr(), data),
                mark_number);
        }
    };
};

///////////////////////////////////////////////////////////////////////////////
// IsMarkerOrRepeater
struct IsMarkerOrRepeater
    : or_<shift_right<terminal<detail::repeat_begin_matcher>, _>,
          assign<terminal<detail::mark_placeholder>, _>>
{
};

///////////////////////////////////////////////////////////////////////////////
// as_optional
template <typename Grammar, typename Greedy>
struct as_optional
    : or_<when<IsMarkerOrRepeater, as_mark_optional<Grammar, Greedy>>,
          otherwise<as_default_optional<Grammar, Greedy>>>
{
};

///////////////////////////////////////////////////////////////////////////////
// make_optional_
template <typename Greedy, typename Callable = proto::callable>
struct make_optional_ : proto::transform<make_optional_<Greedy, Callable>>
{
    template <typename Expr, typename State, typename Data>
    struct impl : proto::transform_impl<Expr, State, Data>
    {
        typedef typename impl::expr expr_type;
        typedef
            typename unary_expr<optional_tag<Greedy>, Expr>::type result_type;

        result_type operator()(typename impl::expr_param expr,
                               typename impl::state_param,
                               typename impl::data_param) const
        {
            result_type that = {expr};
            return that;
        }
    };
};

///////////////////////////////////////////////////////////////////////////////
// as_default_quantifier_impl
template <typename Greedy, uint_t Max>
struct as_default_quantifier_impl<Greedy, 0, Max>
    : call<make_optional_<Greedy>(as_default_quantifier_impl<Greedy, 1, Max>)>
{
};

///////////////////////////////////////////////////////////////////////////////
// as_default_quantifier_impl
template <typename Greedy>
struct as_default_quantifier_impl<Greedy, 0, 1>
    : call<make_optional_<Greedy>(_child)>
{
};

///////////////////////////////////////////////////////////////////////////////
// as_default_quantifier
template <typename Greedy, typename Callable = proto::callable>
struct as_default_quantifier
    : proto::transform<as_default_quantifier<Greedy, Callable>>
{
    template <typename Expr, typename State, typename Data>
    struct impl : proto::transform_impl<Expr, State, Data>
    {
        typedef typename impl::expr expr_type;
        typedef as_default_quantifier_impl<
            Greedy, min_type<typename expr_type::proto_tag>::value,
            max_type<typename expr_type::proto_tag>::value>
            other;

        typedef typename other::template impl<Expr, State, Data>::result_type
            result_type;

        result_type operator()(typename impl::expr_param expr,
                               typename impl::state_param state,
                               typename impl::data_param data) const
        {
            return typename other::template impl<Expr, State, Data>()(
                expr, state, data);
        }
    };
};

} // namespace grammar_detail
} // namespace xpressive
} // namespace boost

#endif