#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <algorithm> 
 
#include<visualization_msgs/Marker.h>
// 定义结构体point用于存放激光信息
struct Point {
    long index;
    float range;
    float intensity;
    float x;
    float y;
  };
 
// 全局变量：发布器和标记（确保在所有函数中可见）
ros::Publisher pub_marker_;
visualization_msgs::Marker marker_;
//发布标记
void Publish(){
  // 等待被订阅
  while (pub_marker_.getNumSubscribers() < 1) {
      sleep(1);
  }
  marker_.header.stamp = ros::Time::now();
  pub_marker_.publish(marker_);  
};
 
  //设置反光柱标记
const float RADIUS = 0.045f; // 根据实际情况调整
void set_marker_fixed_property(float mx,float my,int n){
  /*决定从哪个视图可以看到标记r*/
  marker_.header.frame_id = "laser";
  marker_.ns = "trilateration";
  marker_.id = n;
  //设置标记类型：圆柱体
  marker_.type = visualization_msgs::Marker::CYLINDER;
 
   //设置标记坐标
  marker_.pose.position.x = mx;
  marker_.pose.position.y = my;
  marker_.pose.position.z = 0;
 
  //设置标记尺寸
  marker_.scale.x = 0.09; //m
  marker_.scale.y = 0.09;
  marker_.scale.z = 0.50;
 
   ///设置标记颜色
  marker_.color.a = 1.0; // Don't forget to set the alpha!
  marker_.color.r = 1.0;
  marker_.color.g = 0.0;
  marker_.color.b = 0.0;
 
  //设置标记动作
  marker_.action = visualization_msgs::Marker::ADD;
  marker_.lifetime = ros::Duration(); //(sec,nsec),0 forever
  Publish(); // 调用发布函数
};
 
//查找最大值
Point findMax(std::vector< Point >  &points ) {
  Point max{};
  for (long i = 0; i < points.size(); i++) {
    double intensity = points[i].intensity;
    if (intensity > max.intensity) {
      max.index = points[i].index;
      max.range = points[i].range;
      max.intensity = points[i].intensity;
      max.x = points[i].x;
      max.y = points[i].y;
    }
  }
  return max;
}
 
//查找反光柱
//查找高强度点组作为反光柱
// 查找反光柱：筛选高强度连续点组作为反光柱
void findHigh(const sensor_msgs::LaserScan& scan) {
  std::vector<Point> high_intensity_points;  // 存储一帧内所有高强度点
  std::vector<Point> current_reflector_points;  // 临时存储当前反光柱的连续点组

  Point current_point{};  // 循环中临时存储单个高强度点
  Point max_intensity_point{};  // 当前反光柱中强度最大的点
  Point end_marker{-1, 0, 0, 0, 0};  // 结束标志（用于触发最后一组点的处理）

  int reflector_id = 1;  // 反光柱编号（从1开始）

  // 1. 筛选当前帧所有高强度点（强度>250）
  for (long i = 0; i < scan.ranges.size(); i++) {
    if (scan.intensities[i] > 250) {  // 强度阈值
      current_point.index = i;
      current_point.range = scan.ranges[i] + RADIUS;  // 补偿反光柱半径
      current_point.intensity = scan.intensities[i];
      // 极坐标转直角坐标
      current_point.x = current_point.range * cos(scan.angle_min + scan.angle_increment * current_point.index);
      current_point.y = current_point.range * sin(scan.angle_min + scan.angle_increment * current_point.index);
      high_intensity_points.push_back(current_point);
    }
  }

  // 2. 若当前帧高强度点数量不足3个，直接退出
  if (high_intensity_points.size() > 2) {
    long last_point_index = high_intensity_points[0].index - 1;  // 上一个点的索引（初始化）
    high_intensity_points.push_back(end_marker);  // 添加结束标志，确保最后一组点被处理

    // 3. 对高强度点进行连续性分离（区分不同反光柱）
    for (long i = 0; i < high_intensity_points.size(); i++) {
      // 判断当前点与上一个点是否连续（索引差为1）
      if ((high_intensity_points[i].index - last_point_index) == 1) {
        current_reflector_points.push_back(high_intensity_points[i]);  // 加入当前反光柱点组
      } else {
        // 点不连续：判断当前点组是否为有效反光柱（至少3个点）
        if (current_reflector_points.size() > 2) {
          
          // 提取当前反光柱中强度最大的点
          max_intensity_point = findMax(current_reflector_points);
          // 输出信息
          std::cout <<"反光柱ID:" <<reflector_id <<" 数量：" << current_reflector_points.size() << std::endl;
          std::cout << "激光最大数据:..." << std::endl;
          // 发布标记
          set_marker_fixed_property(max_intensity_point.x, max_intensity_point.y, reflector_id);
          reflector_id++;  // 反光柱编号自增
        }
        current_reflector_points.clear();  // 清空当前点组，准备下一组
      }
      last_point_index = high_intensity_points[i].index;  // 更新上一个点的索引
    }
  } 
  else {
    std::cout << "无法找到有效数据" << std::endl;
  }
  high_intensity_points.clear();
}
 
 
//处理激光信息
void scan_CB(sensor_msgs::LaserScan scan) {
 
  std::vector< Point > points;
 
  findHigh(scan);
}
 
int main(int argc, char *argv[]) {
    ros::init(argc, argv, "trilateration");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("/scan", 1, scan_CB);
    pub_marker_ = nh.advertise<visualization_msgs::Marker>("visualization_marker", 1);
    ros::spin();
    return 0;
}