#include <path_searching/global_astar.h>
#include <sstream>
#include <nav_msgs/Path.h>
using namespace std;
using namespace Eigen;

namespace GraphSearch {
Astar::~Astar() {
  for (int i = 0; i < allocate_num_; i++) {
    delete path_node_pool_[i];
  }
}
 void Astar::NodeVis(Eigen::Vector3d state){
    sensor_msgs::PointCloud2 globalMap_pcd;
    pcl::PointCloud<pcl::PointXYZ> cloudMap;
    pcl::PointXYZ pt;
    pt.x = state[0];
    pt.y = state[1];
    pt.z = state[2];
    cloudMap.points.push_back(pt); 
    cloudMap.width = cloudMap.points.size();
    cloudMap.height = 1;
    cloudMap.is_dense = true;
    pcl::toROSMsg(cloudMap, globalMap_pcd);
    globalMap_pcd.header.stamp = ros::Time::now();
    globalMap_pcd.header.frame_id = "world";
    expandNodesVis.publish(globalMap_pcd);

    return;
  }
int Astar::search(Eigen::Vector3d start_pt, Eigen::Vector3d end_pt) {
  /* ---------- initialize ---------- */
  NodePtr cur_node = path_node_pool_[0];
  cur_node->parent = NULL;
  cur_node->index = map_util_->floatToInt(start_pt);
  cur_node->position = map_util_->intToFloat(cur_node->index);
  std::cout << "begin to astar search" << std::endl;
  cur_node->g_score = 0.0;

  Eigen::Vector3i end_index;

  end_index = map_util_->floatToInt(end_pt);
  end_pt = map_util_->intToFloat(end_index);
  // cur_node->f_score = lambda_heu_ * getDiagHeu(cur_node->position, end_pt);
  cur_node->f_score = lambda_heu_ * getEuclHeu(cur_node->position, end_pt);
  cur_node->node_state = IN_OPEN_SET;

  open_set_.push(cur_node);
  use_node_num_ += 1;

  expanded_nodes_.insert(cur_node->index, cur_node);

  NodePtr neighbor = NULL;
  NodePtr terminate_node = NULL;

  
  if(map_util_->isOccupied(start_pt)){
    ROS_ERROR("start_pt is already occupied");
    return NO_PATH;
  }
  if(map_util_->isOccupied(end_pt)){
    ROS_ERROR("end_pt is already occupied");
    return NO_PATH;
  }
  /* ---------- search loop ---------- */
  while (!open_set_.empty()) {
    /* ---------- get lowest f_score node ---------- */
    cur_node = open_set_.top();

    /* ---------- determine termination ---------- */

    bool reach_end = abs(cur_node->index(0) - end_index(0)) <= 1 &&
        abs(cur_node->index(1) - end_index(1)) <= 1 &&
        abs(cur_node->index(2) - end_index(2)) <= 1;

    if (reach_end) {
      cout << "[Astar]:---------------------- " << use_node_num_ << endl;
      cout << "use node num: " << use_node_num_ << endl;
      cout << "iter num: " << iter_num_ << endl;
      terminate_node = cur_node;
      retrievePath(terminate_node);
      has_path_ = true;

      return REACH_END;
    }

    /* ---------- pop node and add to close set ---------- */
    open_set_.pop();
    cur_node->node_state = IN_CLOSE_SET;
    iter_num_ += 1;

    /* ---------- init neighbor expansion ---------- */
    Eigen::Vector3d cur_pos = cur_node->position;
    Eigen::Vector3d pro_pos;
    double pro_t;

    vector<Eigen::Vector3d> inputs;
    Eigen::Vector3d d_pos;
    // NodeVis(cur_pos);
    // ros::Duration(0.001).sleep();
    /* ---------- expansion loop ---------- */
    for (int didx_x = -1; didx_x <= 1; didx_x ++)
      for (int didx_y = -1; didx_y <= 1; didx_y ++)
        for (int didx_z = -1; didx_z <= 1; didx_z++){
        d_pos << resolution_ * didx_x, resolution_ * didx_y, resolution_ * didx_z;
        if (d_pos.norm() < 1e-3) continue;

        pro_pos = cur_pos + d_pos;
        Eigen::Vector3i pro_id = map_util_->floatToInt(pro_pos);

        /* ---------- check if in feasible space ---------- */
        /* inside map range */
        if (map_util_->isOutside(pro_id)) {
            // std::cout << "map dim: "<<map_util_->getDim().transpose() << std::endl;
            // std::cout << "pro_pos: "<<pro_pos.transpose() << " pro_id: "<<pro_id.transpose() << std::endl;
            // ROS_INFO("WTFssssssssssssssssssssssssssss");
          continue;
        }

        /* not in close set */

        NodePtr pro_node = expanded_nodes_.find(pro_id);

        if (pro_node != NULL && pro_node->node_state == IN_CLOSE_SET) {
          // cout << "in closeset" << endl;
          continue;
        }

        /* collision free */
        bool isOccupied = false;
        
        // for(double inflateDx = -0.3; inflateDx <= 0.3 + 1.0e-3; inflateDx+=0.1){
        //   for(double inflateDy = -0.3; inflateDy <= 0.3 + 1.0e-3; inflateDy+=0.1){
        //     Eigen::Vector2d checkpos = pro_pos + Eigen::Vector2d(inflateDx, inflateDy);
        //     if(map_util_->isOccupied(checkpos)){
        //       isOccupied = true;
        //     }
        //   }
        // }

        if(map_util_->getDistance(pro_pos) < 0.4){
            isOccupied = true;
        }
        // if(map_util_->isOccupied(pro_pos)){
        //     isOccupied = true;
        // }
        if(isOccupied){
          continue;
        }


        /* ---------- compute cost ---------- */
        double tmp_g_score, tmp_f_score;
        tmp_g_score = d_pos.norm() + cur_node->g_score;
        // tmp_f_score = tmp_g_score + lambda_heu_ * getDiagHeu(pro_pos, end_pt);
        tmp_f_score = tmp_g_score + 1.5*getEuclHeu(pro_pos, end_pt);

        if (pro_node == NULL) {
          pro_node = path_node_pool_[use_node_num_];
          pro_node->index = pro_id;
          pro_node->position = pro_pos;
          pro_node->f_score = tmp_f_score;
          pro_node->g_score = tmp_g_score;
          pro_node->parent = cur_node;
          pro_node->node_state = IN_OPEN_SET;
          open_set_.push(pro_node);

          expanded_nodes_.insert(pro_id, pro_node);

          use_node_num_ += 1;
          if (use_node_num_ == allocate_num_) {
            cout << "run out of memory." << endl;
            return NO_PATH;
          }
        } else if (pro_node->node_state == IN_OPEN_SET) {
          if (tmp_g_score < pro_node->g_score) {
            pro_node->position = pro_pos;
            pro_node->f_score = tmp_f_score;
            pro_node->g_score = tmp_g_score;
            pro_node->parent = cur_node;
          }
        } else {
          cout << "error type in searching: " << pro_node->node_state << endl;
        }

        /* ----------  ---------- */
      }
  }

  /* ---------- open set empty, no path ---------- */
  cout << "open set empty, no path!" << endl;
  cout << "use node num: " << use_node_num_ << endl;
  cout << "iter num: " << iter_num_ << endl;
  return NO_PATH;
}


void Astar::setParam(ros::NodeHandle& nh) {
    expandNodesVis = nh.advertise<sensor_msgs::PointCloud2>("/vis/expanded_nodes", 1);
    nh.param("grid_map/resolution", resolution_, 0.1);
    tie_breaker_ = 1.0 + 1.0 / 10000;
    this->inv_resolution_ = 1.0 / resolution_;
    path_node_pool_.resize(allocate_num_);
    for (int i = 0; i < allocate_num_; i++) {
        path_node_pool_[i] = new Node;
    }

    use_node_num_ = 0;
    iter_num_ = 0;
    pathPub = nh.advertise<nav_msgs::Path>("/vis/astarpath", 100, true);

}

void Astar::retrievePath(NodePtr end_node) {
  NodePtr cur_node = end_node;
  path_nodes_.push_back(cur_node);

  while (cur_node->parent != NULL) {
    cur_node = cur_node->parent;
    path_nodes_.push_back(cur_node);
  }

  reverse(path_nodes_.begin(), path_nodes_.end());
}

std::vector<Eigen::Vector3d> Astar::getPath() {
  vector<Eigen::Vector3d> path;
  for (int i = 0; i < path_nodes_.size(); ++i) {
    path.push_back(path_nodes_[i]->position);
  }
  return path;
}
void Astar::visPath(){
    nav_msgs::Path pathROS;
    geometry_msgs::PoseStamped poseROS;
    std::vector<Eigen::Vector3d> path = getPath();
    for(int i = 0; i < path.size(); i++){
        poseROS.header.frame_id = "world";
        poseROS.pose.position.x = path[i][0];
        poseROS.pose.position.y = path[i][1];
        poseROS.pose.position.z = path[i][2];
        pathROS.poses.push_back(poseROS);
    }
    pathROS.header.frame_id = "world";
    pathROS.header.stamp = ros::Time::now();
    pathPub.publish(pathROS);
}

double Astar::getEuclHeu(Eigen::Vector3d x1, Eigen::Vector3d x2) {
  // return 0.0;
  return tie_breaker_ * (x2 - x1).norm();
}


void Astar::setEnvironment(map_util::VoxelMapUtil* env) {
  map_util_ = env;
}

void Astar::reset() {
  expanded_nodes_.clear();
  path_nodes_.clear();

  std::priority_queue<NodePtr, std::vector<NodePtr>, NodeComparator0> empty_queue;
  open_set_.swap(empty_queue);

  for (int i = 0; i < use_node_num_; i++) {
    NodePtr node = path_node_pool_[i];
    node->parent = NULL;
    node->node_state = NOT_EXPAND;
  }

  use_node_num_ = 0;
  iter_num_ = 0;
}

std::vector<NodePtr> Astar::getVisitedNodes() {
  vector<NodePtr> visited;
  visited.assign(path_node_pool_.begin(), path_node_pool_.begin() + use_node_num_ - 1);
  return visited;
}


}  // namespace rm_planner
