#include "mpc_two_vehicle_controller_node.h"

mpc_two_vehicle_controller::mpc_two_vehicle_controller(ros::NodeHandle nh) : nh_(nh)
{
  omega = 0;
  velocity = 0;
  m_ref_vel = 2.0;
  m_path_publish_count = 0;
  last_pos = pcl::PointXYZ(0, 0, 0);
  curr_pos = pcl::PointXYZ(0, 0, 0);

  //发布与订阅
  vel_publisher_ = nh_.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
  global_path_publisher_ = nh_.advertise<nav_msgs::Path>("global_path", 2);
  local_path_publisher_ = nh_.advertise<nav_msgs::Path>("local_path", 2);
  pose_sub_ = nh_.subscribe<turtlesim::Pose>("/turtle1/pose", 1, &mpc_two_vehicle_controller::poseHandler, this);
  target_sub_ = nh_.subscribe<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1, &mpc_two_vehicle_controller::targetHandler, this);
}

mpc_two_vehicle_controller::~mpc_two_vehicle_controller()
{
}

// 目标设置回调
void mpc_two_vehicle_controller::targetHandler(const geometry_msgs::PoseStamped::ConstPtr &target_pose_msg)
{
  m_edge_mutex.lock();
  tf::poseMsgToTF(target_pose_msg->pose, m_target_pose);
  Eigen::Matrix2d base_rotation;
  Eigen::Matrix2d target_rotation;

  base_rotation << m_current_pose.getBasis()[0][0], m_current_pose.getBasis()[0][1], m_current_pose.getBasis()[1][0], m_current_pose.getBasis()[1][1];
  target_rotation << m_target_pose.getBasis()[0][0], m_target_pose.getBasis()[0][1], m_target_pose.getBasis()[1][0], m_target_pose.getBasis()[1][1];

  m_base.setIdentity();
  m_base.rotate(base_rotation);
  m_base.pretranslate(Eigen::Vector2d(m_current_pose.getOrigin().getX(), m_current_pose.getOrigin().getY()));

  m_target.setIdentity();
  m_target.rotate(target_rotation);
  m_target.pretranslate(Eigen::Vector2d(m_target_pose.getOrigin().getX(), m_target_pose.getOrigin().getY()));
  bool create_trajectory_result = m_traj_tool.create_trajectory(m_base, m_target, m_trajectory, 10);
  m_edge_mutex.unlock();

  // 发布全局路径
  if (create_trajectory_result)
  {
    m_global_path.header.frame_id = "map";
    m_global_path.header.seq = ++m_path_publish_count;
    m_global_path.header.stamp = ros::Time::now();
    m_global_path.poses.clear();
    for (size_t i = 0; i < m_trajectory.size(); i++)
    {
      geometry_msgs::PoseStamped t_pose;
      t_pose.header = m_global_path.header;
      t_pose.pose.position.x = m_trajectory[i].translation().x();
      t_pose.pose.position.y = m_trajectory[i].translation().y();
      double theta = atan2(m_trajectory[i].rotation()(1, 0), m_trajectory[i].rotation()(0, 0));
      t_pose.pose.orientation.w = cos(theta / 2.0);
      t_pose.pose.orientation.z = sin(theta / 2.0);
      m_global_path.poses.push_back(t_pose);
    }
    global_path_publisher_.publish(m_global_path);
  }
}

// 当前位姿消息回调
void mpc_two_vehicle_controller::poseHandler(const turtlesim::Pose::ConstPtr &pose_msg)
{
  std::lock_guard<std::mutex> lck(m_edge_mutex);
  m_current_pose.setOrigin(tf::Vector3(pose_msg->x, pose_msg->y, 0));
  m_current_pose.setRotation(tf::Quaternion(0, 0, sin(pose_msg->theta/2.0), cos(pose_msg->theta/2.0)));

  std::chrono::time_point<std::chrono::system_clock> start = std::chrono::system_clock::now();

  //发布tf
  tf::StampedTransform transform_msg(m_current_pose, ros::Time::now(), "map", "base_link");
  tf_broadcaster_.sendTransform(transform_msg);

  // 空路径
  if (m_trajectory.size() < 1){
    // std::cout<<"空路径"<<std::endl;
    return;
  }

  // 在目标点附近
  if((m_current_pose.getOrigin() - m_target_pose.getOrigin()).length() < 0.04){
    // std::cout<<"目标点附近"<<std::endl;
    return;
  }

  // 筛选前方N个点
  // 路径转换为tf位姿
  std::vector<tf::Pose> t_poses, t_partial_poses, t_pred_poses;
  int count = 0;
  // std::cout<<"current pose:"<<current_pose.getOrigin().getX()<<", "<<current_pose.getOrigin().getY()<<std::endl;
  for (int i = 0; i < m_trajectory.size(); i++)
  {
    t_poses.push_back(tf::Pose(tf::Quaternion(), tf::Vector3(m_trajectory[i].translation().x(), m_trajectory[i].translation().y(), 0)));
  }
  // 计算朝向
  tf::Vector3 orient(cos(pose_msg->theta), sin(pose_msg->theta), 0.0);
  tf::Vector3 target_direction = m_target_pose.getOrigin() - m_current_pose.getOrigin();
  if (filterPath(m_current_pose, t_poses, t_partial_poses, 5, target_direction.dot(orient) > 0))
  {
    // 终点附近限制速度
    if (target_direction.length() < 1)
    {
      m_ref_vel *= 0.9;
      m_ref_vel = fmax(m_ref_vel, 0.04);
      if (target_direction.length() < 0.03)
      {
        m_ref_vel = 0;
      }
    }
    else
    {
      m_ref_vel = 2.0;
    }

    // 调整速度方向
    if (target_direction.dot(orient) > 0)
    {
      m_ref_vel = m_ref_vel < 0 ? -m_ref_vel : m_ref_vel;
    }
    else
    {
      m_ref_vel = m_ref_vel > 0 ? -m_ref_vel : m_ref_vel;
    }
    // std::cout<<"m_ref_vel: "<<m_ref_vel<<std::endl;

    double velo = pose_msg->linear_velocity;
    double velo_angle = pose_msg->angular_velocity;

    // std::cout << "start solve" << std::endl;
    m_mpc_model.set_global_target(m_target_pose);
    if (m_mpc_model.Solve(m_ref_vel, t_partial_poses, m_current_pose, velo, velo_angle, t_pred_poses))
    {
      // std::cout << "solve complete" << std::endl;

      geometry_msgs::Twist velocity;
      velocity.linear.x = velo; //(v_left+v_right)/2.0;
      velocity.linear.y = 0;
      velocity.linear.z = 0;
      velocity.angular.x = 0;
      velocity.angular.y = 0;
      velocity.angular.z = velo_angle; //- (v_right-v_left)/Lf;//turtlesim uses left hand angle, if angle less than 0, it means turn left.
      vel_publisher_.publish(velocity);
    }
    std::chrono::time_point<std::chrono::system_clock> end1 = std::chrono::system_clock::now();
    std::cout << "time consumed: " << std::chrono::duration_cast<std::chrono::milliseconds>(end1 - start).count() << "ms."<< std::endl;

    //发布局部路径
    nav_msgs::Path m_local_path;
    m_local_path.header.seq = m_path_publish_count;
    m_local_path.header.stamp = ros::Time::now();
    m_local_path.header.frame_id = "map";
    for (size_t i = 2; i < t_pred_poses.size(); i++)
    {
      geometry_msgs::PoseStamped temp;
      temp.header = m_local_path.header;
      t_pred_poses[i].setRotation(t_pred_poses[i].getRotation().normalize());
      tf::poseTFToMsg(t_pred_poses[i], temp.pose);
      temp.pose.position.x += m_current_pose.getOrigin().getX();
      temp.pose.position.y += m_current_pose.getOrigin().getY();
      temp.pose.position.z += m_current_pose.getOrigin().getZ();
      m_local_path.poses.push_back(temp);
    }
    local_path_publisher_.publish(m_local_path);

    m_last_theta = pose_msg->theta;
    last_pos = curr_pos;
  }else{
    std::cout<<"filter path failed."<<std::endl;
  }
}

//获取小车坐标系中，x前进方向 num 个点，用于拟合
bool mpc_two_vehicle_controller::filterPath(tf::Pose pose, std::vector<tf::Pose> poses, std::vector<tf::Pose> &out, int numOfPoints, bool forward)
{
  if (poses.size() < numOfPoints)
  {
    std::cout<<"lack of poses."<<std::endl;
    return false;
  }

  out.clear();
  // 平移加反向旋转到小车坐标系
  for (int i = 0; i < poses.size(); i++)
  {
    double x = poses[i].getOrigin().getX() - pose.getOrigin().getX();
    double y = poses[i].getOrigin().getY() - pose.getOrigin().getY();
    double trans_x = x * pose.getBasis()[0][0] + y * pose.getBasis()[1][0];
    double trans_y = x * pose.getBasis()[0][1] + y * pose.getBasis()[1][1];
    bool direction_condition = forward ? (trans_x >= 0) : (trans_x < 0);
    if (direction_condition && out.size() < numOfPoints)
    {
      tf::Pose point;
      float nx = trans_x * pose.getBasis()[0][0] + trans_y * pose.getBasis()[0][1];
      float ny = trans_x * pose.getBasis()[1][0] + trans_y * pose.getBasis()[1][1];
      point.setOrigin(tf::Vector3(nx + pose.getOrigin().getX(),
                                  ny + pose.getOrigin().getY(), 0));
      out.push_back(point);
    }
  }
  if (out.size() <= 0){
    std::cout<<"cannot find forward poses."<<std::endl;
    return false;
  }
  return true;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "mpc_two_vehicle_controller_node");
  ros::NodeHandle nh;
  mpc_two_vehicle_controller controller(nh);
  ros::Rate loop_rate(15);

  // double px = 5.54;
  // double py = 5.54;
  // double psi = 0;
  // double v_l = 0;
  // double v_r = 0;
  // double a_l = 0;
  // double a_r = 0;

  while (ros::ok())
  {

    //controller.processData();

    ros::spinOnce();
    loop_rate.sleep();
  }
  //controller.~mpc_two_vehicle_controller();
  return 0;
}