
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>

#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>

#include <moveit_visual_tools/moveit_visual_tools.h>

#include <iostream>
#include <fstream>
#include <string>
#include <vector>

using namespace std; 
#define pi 3.1415926

void euler2quat(float phi, float theta, float psi, float quat[4]) {
    float c1 = cos(phi / 2);
    float c2 = cos(theta / 2);
    float c3 = cos(psi / 2);
    float s1 = sin(phi / 2);
    float s2 = sin(theta / 2);
    float s3 = sin(psi / 2);
    quat[0] = c1*c2*c3 + s1*s2*s3;
    quat[1] = s1*c2*c3 - c1*s2*s3;
    quat[2] = c1*s2*c3 + s1*c2*s3;
    quat[3] = c1*c2*s3 - s1*s2*c3;
}

 
int read_points() {
    std::ifstream file("points.txt"); // 打开文件
    if (!file.is_open()) {
        std::cerr << "无法打开文件" << std::endl;
        return 1;
    }
  
    std::string line;
    while (getline(file, line)) { // 逐行读取文件内容
        std::cout << line << std::endl; // 打印到控制台
    }
 
    file.close(); // 关闭文件
    return 0;
}

//@brief: 指定单个或多个分隔符（单个字符）分割字符串
//@param: src 原字符串；delimiter 单个或多个分隔符（单个字符）
vector<string> splitStr(const string& src, const string& delimiter) {
	std::vector<string> vtStr;

	// 入参检查
	// 1.原字符串为空返回空 vector
	if (src == "") {
		return vtStr;
	}
	// 2.分隔符为空返回单个元素为原字符串的 vector
	if (delimiter == "") {
		vtStr.push_back(src);
		return vtStr;
	}

	string::size_type startPos = 0;
	auto index = src.find_first_of(delimiter);
	while (index != string::npos) {
		auto str = src.substr(startPos, index - startPos);
		if (str != "") {
			vtStr.push_back(str);
		}
		startPos = index + 1;
		index = src.find_first_of(delimiter, startPos);
	}
	// 取最后一个子串
	auto str = src.substr(startPos);
	if (str != "") {
		vtStr.push_back(str);
	}

	return vtStr;
}

int main(int argc, char** argv)
{
  float res[4]={0};
  ros::init(argc, argv, "visual_node");//初始化节点
  //ros::NodeHandle node_handle;//“大管家”句柄
  ros::AsyncSpinner spinner(1);//ROS多线程订阅消息，这里开了一个线程？？？
  spinner.start();//启动线程

  namespace rvt = rviz_visual_tools;//导入rviz_visual_tools这个namespace，同时使用别名rvt
  moveit_visual_tools::MoveItVisualTools visual_tools("base_link");//创建MoveItVisualTools对象

  visual_tools.deleteAllMarkers();//清除旧的标记（markers）
  visual_tools.loadRemoteControl();//加载Remote control 主要功能是激活RVIZ里面的“按钮GUI”，操作人员可以通过GUI按钮/键盘来控制程序运行下一步
  Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();//将text_pose初始化为单位阵，再做其他操作。虽然称为3D，实质上为4*4矩阵。
  text_pose.translation().z() = 1.75;//设置坐标
  
  geometry_msgs::Pose target_pose1;///创建一个四元数的位姿
  geometry_msgs::Pose target_pose2;///创建一个四元数的位姿
  std::vector<geometry_msgs::Pose> waypoints[20];//定义焊接的路径点列表（创建一个 vector 容器）
 
  //euler2quat(pi,180*pi/180.0,0,res);//ryp角转四元数 设定好姿态
  // target_pose1.orientation.x=res[0];
  // target_pose1.orientation.y=res[1];
  // target_pose1.orientation.z=res[2];
  // target_pose1.orientation.w = res[3];

  std::string f_path;
  std::string line;
  std::vector<string> l;

  ros::param::get("folder_path", f_path);
  f_path=f_path+"/result.txt";
  std::ifstream file(f_path); // 打开文件
  int num=0;
  if (file.is_open()) 
  {
    while (getline(file, line)) 
    {       
      l=splitStr(line,"sp,/");
      target_pose1.position.x = std::stod(l[0])/1000;
      target_pose1.position.y = std::stod(l[1])/1000;
      target_pose1.position.z = std::stod(l[2])/1000;

      target_pose2.position.x = std::stod(l[3])/1000;
      target_pose2.position.y = std::stod(l[4])/1000;
      target_pose2.position.z = std::stod(l[5])/1000;

      target_pose1.orientation.x= std::stod(l[6]);
      target_pose1.orientation.y= std::stod(l[7]);
      target_pose1.orientation.z= std::stod(l[8]);
      target_pose1.orientation.w = std::stod(l[9]);

      target_pose2.orientation.x= std::stod(l[10]);
      target_pose2.orientation.y= std::stod(l[11]);
      target_pose2.orientation.z= std::stod(l[12]);
      target_pose2.orientation.w = std::stod(l[13]);

      waypoints[num].push_back(target_pose1);//添加到路径点列表
      waypoints[num].push_back(target_pose2);//添加到路径点列表
      num++;

      // for (int i=0 ;i<l.size();i++) 
      // {
        
		  //   std::cout << l[i]<< " ";
	    // }
      // std::cout<<std::endl;
    }

  }
  file.close(); // 关闭文件
  

  visual_tools.deleteAllMarkers();//清除所有标记
  visual_tools.publishText(text_pose, "robot", rvt::WHITE, rvt::XLARGE);//文本标记
  for(int k=0;k<num;k++)
  {
  visual_tools.publishPath(waypoints[k], rvt::PINK , rvt::XXXSMALL);//路径？
  for (std::size_t i = 0; i < waypoints[k].size(); ++i)
    visual_tools.publishAxisLabeled(waypoints[k][i], "point" + std::to_string (i)+ "  "+std::to_string(waypoints[k][i].position.x)+ "  "+ std::to_string(waypoints[k][i].position.y)+ "  "+ std::to_string(waypoints[k][i].position.z), rvt::XXXSMALL);//坐标系
  visual_tools.trigger();
  }
  // while (ros::ok())
  // {
    
  // }
  
  
  ros::shutdown();
  return 0;
}
/*
BLACK 	
BROWN 	
BLUE 	
CYAN 	
GREY 	
DARK_GREY 	
GREEN 	
LIME_GREEN 	
MAGENTA 	
ORANGE 	
PURPLE 	
RED 	
PINK 	
WHITE 	
YELLOW 	
TRANSLUCENT 	
TRANSLUCENT_LIGHT 	
TRANSLUCENT_DARK 	
RAND 	
CLEAR 	
DEFAULT 

XXXXSMALL 	
XXXSMALL 	
XXSMALL 	
XSMALL 	
SMALL 	
MEDIUM 	
LARGE 	
XLARGE 	
XXLARGE 	
XXXLARGE 	
XXXXLARGE 	
REGULAR 

*/