#include <obstacle/obstacle.h>
#include <geometry_msgs/Point32.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <glog/logging.h>

namespace obs 
{

static Json::Value __readLaserConfig() 
{
    std::string path = space::home() + "configure/obstacle_config.json";

    Json::Value c = space::readConfig(path);
    
    return c["sensor"]["laser"];
}

static Json::Value __readCameraConfig() 
{
    std::string path = space::home() + "configure/obstacle_config.json";

    Json::Value c = space::readConfig(path);

    return c["sensor"]["camera"];
}

template <typename T>
static bool __interval(const T& value, const T& top, const T& bot)
{
    if (value < top && value > bot) return true;

    return false;
} 

static sensor_msgs::PointCloud __filter3D(sensor_msgs::PointCloud& in, float resolution)
{
    sensor_msgs::PointCloud out;
    std::map<int, std::map<int, std::map<int, sensor_msgs::PointCloud>>> filter;

    for(geometry_msgs::Point32& point : in.points) {
        if (fabs(point.x) > 3 || fabs(point.y) > 3 || fabs(point.z) > 3) {
            continue;
        }
        int x = std::ceil(point.x / resolution);
        int y = std::ceil(point.y / resolution);
        int z = std::ceil(point.z / resolution);

        if (filter[x][y][z].points.size() < 6) {
            filter[x][y][z].points.push_back(point);
        }
        else if (filter[x][y][z].points.size() == 6) {
            filter[x][y][z].points.push_back(point);
            out.points.push_back(point);
        }
    }
    return out;
}

static sensor_msgs::PointCloud __extract3D(sensor_msgs::PointCloud& in, int extract_step)
{
    sensor_msgs::PointCloud out;
    int step = 0;

    for(geometry_msgs::Point32& point : in.points) {
        if (step == 0) {
            out.points.push_back(point);
        }
        step++;
        if (step > extract_step) {
            step = 0;
        }
    }
    return out;
}

/**
 * 说明: 对平面点云压缩
 * is_gather：是否需要消除孤立点
 */ 
static std::vector<Pose2D> __filter2D(std::vector<Pose2D>& in, float resolution, bool is_gather)
{
    std::vector<Pose2D> out;
    std::map<int, std::map<int, std::vector<Pose2D>>> filter;

    for (Pose2D& pose2d : in) {
        int x = std::ceil(pose2d.x / resolution);
        int y = std::ceil(pose2d.y / resolution);

        if (is_gather) {
            if (filter[x][y].size() < 2) {
                filter[x][y].push_back(pose2d);
            }
            else if (filter[x][y].size() == 2) {
                filter[x][y].push_back(pose2d);
                out.push_back(pose2d);
            }
        }
        else {
            if (filter[x][y].empty()) {
                filter[x][y].push_back(pose2d);
                out.push_back(pose2d);
            }
        }
    }
    return out;
}

ObstacleIdentify::ObstacleIdentify() : debug_(true), debug_3d_(false), turn_off_(false), on_(true)
{
}

ObstacleIdentify::~ObstacleIdentify()
{
}

void ObstacleIdentify::start()
{
    stop();

    __readConfig();
    __getPrintFoot();
    __registerLaser();
    __registerCamera();

    point_cloud3D_pub_ = nh_.advertise<sensor_msgs::PointCloud>("/sensor_cloud3D", 10);

debug_ = true;
    if (debug_) {
        point_cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud>("/sensor_cloud", 10);
        dynamic_polygon_pub_ = nh_.advertise<geometry_msgs::PolygonStamped>("/dynamic_polygon", 1, true);
        static_polygon_pub_ = nh_.advertise<geometry_msgs::PolygonStamped>("/static_polygon", 1, true);
    
        std::thread pub_thread = std::thread(
            [this](){
                uint delay = 1000 * 20;

                while (ros::ok() && on_) {
                    std::vector<Pose2D> obstacle = this->__getParseObs();
                    usleep(delay);
                    if (obstacle.empty()) {
                        continue;
                    }
                    
                    sensor_msgs::PointCloud laser_point_cloud;
                    
                    for (const Pose2D& point : obstacle) {
                        geometry_msgs::Point32 tmp;
                        tmp.x = point.x;
                        tmp.y = point.y;
                        tmp.z = 0;
                        laser_point_cloud.points.push_back(tmp);
                    }
                    laser_point_cloud.header.stamp = ros::Time::now();
                    laser_point_cloud.header.frame_id = "base_link";
                    this->point_cloud_pub_.publish(laser_point_cloud);
                }
            }
        );
        pub_thread.detach();
    }

    on_ = true;
}

bool ObstacleIdentify::isClose(common::Point pp)
{
    for (CloseClose& cll : close_closes_) {
        float d = hypot(cll.x - pp.x, cll.y - pp.y);
        
        if (d < cll.close_dist) {
            return true;
        }
    }
    return false;
}

void ObstacleIdentify::__laserCallback(const sensor_msgs::LaserScanConstPtr& msg)
{
    std::map<std::string, LaserParam>::iterator la_it = laser_params_.find(msg->header.frame_id);

    if (la_it == laser_params_.end()) {
        return;
    }

    LaserParam laser_config = la_it->second;

    std::vector<Pose2D> gather; // 数据聚类
    float max_detection_range = 5.f;
    std::map<std::string, Eigen::Matrix<float, 4, 4>>::iterator it = 
        transforms_.find(msg->header.frame_id);
    
    if (it == transforms_.end()) {
        return;
    }
    Eigen::Matrix<float, 4, 4> transform = it->second;

    float theta = msg->angle_max;
    float offset = msg->angle_max + msg->angle_min;
    float sm_theta = 0.0;

    for (const float& range : msg->ranges) {
        theta -= msg->angle_increment;
        sm_theta += msg->angle_increment;

        if (sm_theta < (laser_config.angle_increment - 1e-10)) {
            continue;
        }
        else {
            sm_theta = 0.0;
        }
        if (range > msg->range_max) continue;
        if (range < msg->range_min) continue;
        if (range < laser_config.min_range) continue;
        if (range > laser_config.max_range) continue;
        if (theta < msg->angle_min) continue;
        if (theta > msg->angle_max) continue;
        if (range > max_detection_range) continue;
        if (std::isnan(range)) continue;

        const Eigen::Matrix<float, 4, 1> point(range * cos(theta - offset),
                                                range * sin(theta - offset),
                                                1.0,
                                                0.0);
       
        // //将基于传感器坐标系的数据转到基于base_link坐标系
        Eigen::Matrix<float, 3, 1> base_link_coord = (transform * point).head<3>();
        //激光转置
        base_link_coord = transform::Rigid3f::Rotation(Eigen::AngleAxisf(
            M_PI, Eigen::Vector3f::UnitX())) * base_link_coord;

        Pose2D base_link_point;

        base_link_point.x = base_link_coord(0);
        base_link_point.y = base_link_coord(1);

        if (__isIgnore(base_link_point.x, base_link_point.y, theta, laser_config.ignores)) {
            continue;
        }

        gather.push_back(base_link_point);
    }
    gather = __filter2D(gather, 0.05, true);
    __addObs(gather, msg->header.frame_id, ros::Time::now());
}

void ObstacleIdentify::__cameraCallback(const sensor_msgs::PointCloud2ConstPtr& msg)
{
    std::map<std::string, CameraParam>::iterator cam_it = camera_params_.find(msg->header.frame_id);

    if (cam_it == camera_params_.end()) {
        return;
    }

    CameraParam caram_config = cam_it->second;
    sensor_msgs::PointCloud cloud;

    sensor_msgs::convertPointCloud2ToPointCloud(*msg, cloud);

    cloud = __extract3D(cloud, caram_config.filter_step);
    if (cloud.points.empty()) {
        return;
    }
    
    std::map<std::string, Eigen::Matrix<float, 4, 4>>::iterator it = 
    transforms_.find(msg->header.frame_id);

    if (it == transforms_.end()) {
        return;
    }

    Eigen::Matrix<float, 4, 4> transform = it->second;
    std::vector<Pose2D> obstacle_pose;
    sensor_msgs::PointCloud laser_point_cloud;

    for(geometry_msgs::Point32& point : cloud.points) {
        if(std::isnan(point.x) || std::isnan(point.y) || std::isnan(point.z)) {
            continue;
        }
        Eigen::Matrix<float, 4, 1> pose;
        
        pose << point.z, point.x, point.y, 1.0;
        
        pose = transform * pose;

        if (__interval(pose(0), caram_config.x_head, caram_config.x_tail) && __interval(pose(1), caram_config.y_right, caram_config.y_left) && 
            __interval(pose(2), caram_config.z_ceil, caram_config.z_floor)) {
                Pose2D pose2d;
                
                pose2d.x = pose(0);
                pose2d.y = pose(1);

                if (__isIgnore(pose2d.x, pose2d.y, 1e10, caram_config.ignores)) {
                    continue;
                }
                obstacle_pose.push_back(pose2d);
        }
        if (debug_3d_) {
            geometry_msgs::Point32 tmp;
            tmp.x = pose(0);
            tmp.y = pose(1);
            tmp.z = pose(2);
            laser_point_cloud.points.push_back(tmp);
        }
    }

    if (debug_3d_) {
        laser_point_cloud.header.stamp = ros::Time::now();
        laser_point_cloud.header.frame_id = "base_link";
        point_cloud3D_pub_.publish(laser_point_cloud);
    }
    if (!obstacle_pose.empty()) {
        __addObs(obstacle_pose, msg->header.frame_id, ros::Time::now());
    }
}

void ObstacleIdentify::__addObs(std::vector<Pose2D>& obs, const std::string sensor_name, ros::Time time) 
{
    mutex_.lock();
    
    Obstacle obstacle;

    obstacle.obstacle_pose = obs;
    obstacle.stamp = time;

    obs_[sensor_name] = obstacle;

    mutex_.unlock();
}

std::vector<Pose2D> ObstacleIdentify::__getParseObs() 
{
    mutex_.lock();
    
    std::vector<Pose2D> obstacle;

    for (auto& obs : obs_) {
        if ((ros::Time::now() - obs.second.stamp).toSec() < 0.1) {
            obstacle.insert(obstacle.begin(), obs.second.obstacle_pose.begin(), obs.second.obstacle_pose.end());
        }
    }
    obstacle = __filter2D(obstacle, 0.05, false);

    mutex_.unlock();

    return obstacle;
}

void ObstacleIdentify::__getPrintFoot()
{
    std::string path = space::home() + "configure/obstacle_config.json";

    Json::Value c = space::readConfig(path);
    Json::Value copy = c;
    
    foot_.poses.clear();
    if (c["foot"]["x"].size() == c["foot"]["y"].size()) {
        for (int i = 0;i < c["foot"]["x"].size();i++) {
            Pose2D pose_2d;

            space::jsonValueTo(c["foot"]["x"][i], pose_2d.x);
            space::jsonValueTo(c["foot"]["y"][i], pose_2d.y);
            LOG(INFO)<<pose_2d.x<<"  "<<pose_2d.y;
            foot_.poses.push_back(pose_2d);
        }
    }
    if (c["foot"]["x"].empty()) {
        c["foot"]["x"][0] = 0.0;
        c["foot"]["y"][0] = 0.0;

        if(copy != c) {
            space::writeConfig(path, c);
        }
    }
}

void ObstacleIdentify::updateFootPrint()
{
    __getPrintFoot();
}

bool ObstacleIdentify::isObstacleInArea(const float& x,
                                        const float& y,
                                        const float& theta,
                                        float& distance)
{
    std::vector<Pose2D> obstacle = __getParseObs();

    if (obstacle.empty() || foot_.poses.empty() || turn_off_) {
        return false;
    }

    Area new_foots;
    std::vector<Pose2D> points;

    if(foot_.poses.size() < 3 && !foot_.poses.empty()) {
        float radius = hypot(foot_.poses[0].x, foot_.poses[0].y);
        int num = 16;
        Pose2D pt;

        for (int i = 0; i < num; ++i) {
            float angle = i * 2 * M_PI / num;
            pt.x = cos(angle) * radius;
            pt.y = sin(angle) * radius;
            points.push_back(pt);
        }
        foot_.poses =points;
    }
    Pose2D pose;

    pose.x = x;
    pose.y = y;
    pose.theta = theta;
    for(int i = 0;i < foot_.poses.size();i++) {
        Pose2D foot = __localTransGlobal(foot_.poses[i], pose);
        new_foots.poses.push_back(foot);
    }

    debug_ = true;
    if (debug_) {
        geometry_msgs::PolygonStamped static_polygon;

        static_polygon.header.frame_id ="base_link";
        static_polygon.polygon.points.resize(new_foots.poses.size());
        for(int i = 0;i < foot_.poses.size();i++) {
            static_polygon.polygon.points[i].x =foot_.poses[i].x;
            static_polygon.polygon.points[i].y =foot_.poses[i].y;
        }
        static_polygon_pub_.publish(static_polygon);
        LOG(INFO)<<"pub foot";

        geometry_msgs::PolygonStamped dynamic_polygon;

        dynamic_polygon.header.frame_id ="base_link";
        dynamic_polygon.polygon.points.resize(new_foots.poses.size());
        for(int i=0;i<new_foots.poses.size();i++) {
            dynamic_polygon.polygon.points[i].x =new_foots.poses[i].x;
            dynamic_polygon.polygon.points[i].y =new_foots.poses[i].y;
        }
        dynamic_polygon_pub_.publish(dynamic_polygon);
    }

    for(int i = 0;i < obstacle.size();i++) {
        if(__poseInArea(obstacle[i], new_foots)) {
            distance = hypot(obstacle[i].x, obstacle[i].y);

            return true; 
        }
    }

    return false;
}

bool ObstacleIdentify::__poseInArea(Pose2D point, Area area) 
{
    if(area.poses.empty()) {
        printf("Foot Error!\n");
        return true;
    }

    std::vector<Pose2D> points;
    points = area.poses;

    bool flag = false;
    float px = point.x;
    float py = point.y;
    
    points.push_back(points[0]);
    for(int i = 0;i < points.size() - 1;i++) {
        float x1 = points[i].x;
        float y1 = points[i].y;
        float x2 = points[i+1].x;
        float y2 = points[i+1].y;

        if((px == x1 && py == y1) || (px == x2 && py == y2)) {
            return true;
        }

        if((y1 < py && y2 >= py) || (y1 >= py && y2 < py)) {
            float x = x1 + (py - y1) * (x2 -x1) / (y2 -y1);

            if( x == px) {
                return true;
            }

            if(x > px) {
                flag = !flag;
            }
        }
    }

    return flag;
}

bool ObstacleIdentify::__isIgnore(float x, float y, float angle, std::vector<IgnoreParam> ignores)
{
    for (IgnoreParam& param : ignores) {
        if (x < param.x_upper && x > param.x_lower) {
            return true;
        }
        if (y < param.y_upper && y > param.y_lower) {
            return true;
        }
        if (angle < param.angle_upper && angle > param.angle_lower) {
            return true;
        }
    }
    return false;
}

Pose2D ObstacleIdentify::__trasformBaseLink(Pose2D pose, const Eigen::Matrix<float, 3, 3>& transform) 
{
    const Eigen::Matrix<float, 3, 1> point(pose.x,pose.y,1.0);
    Eigen::Matrix<float, 3, 1> base_link_coord =  transform * point;

    Pose2D trans_pose;

    trans_pose.x = base_link_coord(0);
    trans_pose.y = base_link_coord(1);
    trans_pose.theta = 0;

    return trans_pose;
}

Pose2D ObstacleIdentify::__localTransGlobal(Pose2D a, Pose2D b) 
{
    Pose2D c;

    c.x = b.x + a.x * cos(b.theta) - a.y * sin(b.theta);
    c.y = b.y + a.x * sin(b.theta) + a.y * cos(b.theta);

    return c;
}

void ObstacleIdentify::__readConfig()
{
    std::string path = space::home() + "configure/obstacle_config.json";
    Json::Value c = space::readConfig(path);
    Json::Value copy = c;
    
    laser_params_.clear();
    camera_params_.clear();
    transforms_.clear();

    for (int i = 0;i < c["sensor"]["laser"].size() || i == 0;i++) {
        float tf_x, tf_y, tf_yaw;
        std::string name, topic;

        bool ok1 = space::jsonValueTo(c["sensor"]["laser"][i]["base"]["topic"], topic);
        bool ok2 = space::jsonValueTo(c["sensor"]["laser"][i]["tf"]["tf_x"], tf_x);
        bool ok3 = space::jsonValueTo(c["sensor"]["laser"][i]["tf"]["tf_y"], tf_y);
        bool ok4 = space::jsonValueTo(c["sensor"]["laser"][i]["tf"]["tf_yaw"], tf_yaw);
        bool ok5 = space::jsonValueTo(c["sensor"]["laser"][i]["base"]["name"], name);
        LaserParam laser_param;

        space::jsonValueTo(c["sensor"]["laser"][i]["base"]["min_range"], laser_param.min_range) ? false : laser_param.min_range = 0.0;
        space::jsonValueTo(c["sensor"]["laser"][i]["base"]["max_range"], laser_param.max_range) ? false : laser_param.max_range = 0.0;
        space::jsonValueTo(c["sensor"]["laser"][i]["base"]["angle_increment"], laser_param.angle_increment) ? false : laser_param.angle_increment = 0.0;

        if (ok1 && ok2 && ok3 && ok4 && ok5) {
            Eigen::Matrix<float, 2, 1> translation(tf_x, -tf_y);
            Eigen::Rotation2D<float> rotation(-tf_yaw); //yaw 轴旋转矩阵
            Eigen::Matrix<float, 2, 2> rotation_matrix = rotation.toRotationMatrix();
            Eigen::Matrix<float, 3, 3> transform3;
            Eigen::Matrix<float, 4, 4> transform4;

            transform3 << rotation_matrix, translation, 0., 0., 1.;
            transform4.rightCols<1>() << 0, 0, 0, 0;
            transform4.block<3, 3>(0, 0, 3, 3) = transform3;
            transform4.bottomRows<1>() << 0, 0, 0, 0;
            transforms_.emplace(std::make_pair(name, transform4));
            subs_.push_back(nh_.subscribe(topic, 1, &ObstacleIdentify::__laserCallback, this));
        }
        else
        {
            LOG(INFO)<<"read lasr base error";
        }
        
        Json::Value& ignores = c["sensor"]["laser"][i]["ignore"];

        for (int n = 0;n < ignores.size() || n == 0;n++) {
            IgnoreParam ignore;

            space::jsonValueTo(ignores[n]["x_upper"], ignore.x_upper) ? false : 
                (ignores[n]["x_upper"] = ignore.x_upper = 0.0);
            space::jsonValueTo(ignores[n]["x_lower"], ignore.x_lower) ? false : 
                (ignores[n]["x_lower"] = ignore.x_lower = 0.0);
            space::jsonValueTo(ignores[n]["y_upper"], ignore.y_upper) ? false : 
                (ignores[n]["y_upper"] = ignore.y_upper = 0.0);
            space::jsonValueTo(ignores[n]["y_lower"], ignore.y_lower) ? false : 
                (ignores[n]["y_lower"] = ignore.y_lower = 0.0);
            space::jsonValueTo(ignores[n]["angle_upper"], ignore.angle_upper) ? false : 
                (ignores[n]["angle_upper"] = ignore.angle_upper = 0.0);
            space::jsonValueTo(ignores[n]["angle_lower"], ignore.angle_lower) ? false : 
                (ignores[n]["angle_lower"] = ignore.angle_lower = 0.0);
                
            if (ignores.size() > 0) {
                laser_param.ignores.push_back(ignore);
            }
        }
        laser_params_.emplace(std::make_pair(name, laser_param));
    }
    // for (int i = 0;i < c["sensor"]["camera"].size() || i == 0;i++) {
    //     float tf_x, tf_y, tf_z, tf_roll, tf_pitch, tf_yaw;
    //     std::string name, topic;

    //     bool ok1 = space::jsonValueTo(c["sensor"]["camera"][i]["base"]["topic"], topic);
    //     bool ok2 = space::jsonValueTo(c["sensor"]["camera"][i]["tf"]["tf_x"], tf_x);
    //     bool ok3 = space::jsonValueTo(c["sensor"]["camera"][i]["tf"]["tf_y"], tf_y);
    //     bool ok4 = space::jsonValueTo(c["sensor"]["camera"][i]["tf"]["tf_z"], tf_z);
    //     bool ok5 = space::jsonValueTo(c["sensor"]["camera"][i]["tf"]["tf_roll"], tf_roll);
    //     bool ok6 = space::jsonValueTo(c["sensor"]["camera"][i]["tf"]["tf_pitch"], tf_pitch);
    //     bool ok7 = space::jsonValueTo(c["sensor"]["camera"][i]["tf"]["tf_yaw"], tf_yaw);
    //     bool ok8 = space::jsonValueTo(c["sensor"]["camera"][i]["base"]["name"], name);

    //     if (ok1 && ok2 && ok3 && ok4 && ok5 && ok6 && ok7 && ok8) {
    //         Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(tf_roll, Eigen::Vector3d::UnitX()));
    //         Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(tf_pitch, Eigen::Vector3d::UnitY()));
    //         Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(tf_yaw, Eigen::Vector3d::UnitZ()));
            
    //         Eigen::Matrix3f rotation_matrix;
    //         Eigen::Matrix<float, 4, 4> transform;

    //         rotation_matrix = (yawAngle * pitchAngle * rollAngle).cast<float>();
    //         transform.rightCols<1>() << tf_x, tf_y, tf_z, 1.0;
    //         transform.block<3, 3>(0, 0, 3, 3) = rotation_matrix;
    //         transform.bottomRows<1>() << 0, 0, 0, 1.0;

    //         transforms_.emplace(std::make_pair(name, transform));
    //         subs_.push_back(nh_.subscribe(topic, 1, &ObstacleIdentify::__cameraCallback, this));
    //     }
    //     CameraParam camera_param;
    //     Json::Value& ignores = c["sensor"]["camera"][i]["ignore"];

    //     space::jsonValueTo(c["sensor"]["camera"][i]["filter"]["z_ceil"], camera_param.z_ceil) ? false : camera_param.z_ceil = 1e10;
    //     space::jsonValueTo(c["sensor"]["camera"][i]["filter"]["z_floor"], camera_param.z_floor) ? false : camera_param.z_floor = -1e10;
    //     space::jsonValueTo(c["sensor"]["camera"][i]["filter"]["x_head"], camera_param.x_head) ? false : camera_param.x_head = 1e10;
    //     space::jsonValueTo(c["sensor"]["camera"][i]["filter"]["x_tail"], camera_param.x_tail) ? false : camera_param.x_tail = -1e10;
    //     space::jsonValueTo(c["sensor"]["camera"][i]["filter"]["y_left"], camera_param.y_left) ? false : camera_param.y_left = 1e10;
    //     space::jsonValueTo(c["sensor"]["camera"][i]["filter"]["y_right"], camera_param.y_right) ? false : camera_param.y_right = -1e10;
    //     space::jsonValueTo(c["sensor"]["camera"][i]["filter"]["pit_top"], camera_param.pit_top) ? false : camera_param.pit_top = -1e10;
    //     space::jsonValueTo(c["sensor"]["camera"][i]["filter"]["pit_bot"], camera_param.pit_bot) ? false : camera_param.pit_bot = -1e10;
    //     space::jsonValueTo(c["sensor"]["camera"][i]["filter"]["filter_step"], camera_param.filter_step) ? false : camera_param.filter_step = 3;

    //     for (int n = 0;n < ignores.size() || n == 0;n++) {
    //         IgnoreParam ignore;

    //         space::jsonValueTo(ignores[n]["x_upper"], ignore.x_upper) ? false : 
    //             (ignores[n]["x_upper"] = ignore.x_upper = 0.0);
    //         space::jsonValueTo(ignores[n]["x_lower"], ignore.x_lower) ? false : 
    //             (ignores[n]["x_lower"] = ignore.x_lower = 0.0);
    //         space::jsonValueTo(ignores[n]["y_upper"], ignore.y_upper) ? false : 
    //             (ignores[n]["y_upper"] = ignore.y_upper = 0.0);
    //         space::jsonValueTo(ignores[n]["y_lower"], ignore.y_lower) ? false : 
    //             (ignores[n]["y_lower"] = ignore.y_lower = 0.0);
    //         space::jsonValueTo(ignores[n]["angle_upper"], ignore.angle_upper) ? false : 
    //             (ignores[n]["angle_upper"] = ignore.angle_upper = 0.0);
    //         space::jsonValueTo(ignores[n]["angle_lower"], ignore.angle_lower) ? false : 
    //             (ignores[n]["angle_lower"] = ignore.angle_lower = 0.0);
                
    //         if (ignores.size() > 0) {
    //             camera_param.ignores.push_back(ignore);
    //         }
    //     }
    //     camera_params_.emplace(std::make_pair(name, camera_param));
    // }
    // for (int i = 0;i < c["close"]["close_close"].size() || i == 0;i++) {
    //     CloseClose cc;

    //     space::jsonValueTo(c["close"]["close_close"][i]["x"], cc.x) ? false : 
    //         (c["close"]["close_close"][i]["x"] = cc.x = 0.0);
    //     space::jsonValueTo(c["close"]["close_close"][i]["y"], cc.y) ? false : 
    //         (c["close"]["close_close"][i]["y"] = cc.y = 0.0);
    //     space::jsonValueTo(c["close"]["close_close"][i]["close_dist"], cc.close_dist) ? false : 
    //         (c["close"]["close_close"][i]["close_dist"] = cc.close_dist = 0.0);
        
    //     if (c["close"]["close_close"].size() > 0) {
    //         close_closes_.push_back(cc);
    //     }
    // }
    // space::jsonValueTo(c["debug"], debug_) ? false : 
    //         (c["debug"] = debug_ = false);
    // space::jsonValueTo(c["debug_3d"], debug_3d_) ? false : 
    //         (c["debug_3d"] = debug_3d_ = false);
    // space::jsonValueTo(c["turn_off"], turn_off_) ? false : 
    //         (c["turn_off"] = turn_off_ = false);
    // debug_ = false;
    debug_3d_ = false;
    turn_off_ = false;
    if(copy != c) {
        space::writeConfig(path, c);
    }
}

void ObstacleIdentify::__registerLaser()
{
    Json::Value lasers = __readLaserConfig();

    for (int i = 0;i < lasers.size();i++) {
        float tf_x, tf_y, tf_yaw;
        std::string name, topic;

        bool ok = space::jsonValueTo(lasers[i]["topic"], topic) &&
                    space::jsonValueTo(lasers[i]["tf_x"], tf_x) &&
                    space::jsonValueTo(lasers[i]["tf_y"], tf_y) &&
                    space::jsonValueTo(lasers[i]["tf_yaw"], tf_yaw) &&
                    space::jsonValueTo(lasers[i]["name"], name);
        if (ok) {
            Eigen::Matrix<float, 2, 1> translation(tf_x, -tf_y);
            Eigen::Rotation2D<float> rotation(-tf_yaw); //yaw 轴旋转矩阵
            Eigen::Matrix<float, 2, 2> rotation_matrix = rotation.toRotationMatrix();
            Eigen::Matrix<float, 3, 3> transform3;
            Eigen::Matrix<float, 4, 4> transform4;

            transform3 << rotation_matrix, translation, 0., 0., 1.;
            transform4.rightCols<1>() << 0, 0, 0, 0;
            transform4.block<3, 3>(0, 0, 3, 3) = transform3;
            transform4.bottomRows<1>() << 0, 0, 0, 0;
            transforms_.emplace(std::make_pair(name, transform4));
            subs_.push_back(nh_.subscribe(topic, 1, &ObstacleIdentify::__laserCallback, this));
        }
    }
}

void ObstacleIdentify::__registerCamera()
{
    Json::Value camera = __readCameraConfig();
    
    for (int i = 0;i < camera.size();i++) {
        float tf_x, tf_y, tf_z, tf_roll, tf_pitch, tf_yaw;
        std::string name, topic;

        bool ok =   space::jsonValueTo(camera[i]["topic"], topic) &&
                    space::jsonValueTo(camera[i]["tf_x"], tf_x) &&
                    space::jsonValueTo(camera[i]["tf_y"], tf_y) &&
                    space::jsonValueTo(camera[i]["tf_z"], tf_z) &&
                    space::jsonValueTo(camera[i]["tf_roll"], tf_roll) &&
                    space::jsonValueTo(camera[i]["tf_pitch"], tf_pitch) &&
                    space::jsonValueTo(camera[i]["tf_yaw"], tf_yaw) &&
                    space::jsonValueTo(camera[i]["name"], name);
        if (ok) {
            Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(tf_roll, Eigen::Vector3d::UnitX()));
            Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(tf_pitch, Eigen::Vector3d::UnitY()));
            Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(tf_yaw, Eigen::Vector3d::UnitZ()));
            
            Eigen::Matrix3f rotation_matrix;
            Eigen::Matrix<float, 4, 4> transform;

            rotation_matrix = (yawAngle * pitchAngle * rollAngle).cast<float>();
            transform.rightCols<1>() << tf_x, tf_y, tf_z, 1.0;
            transform.block<3, 3>(0, 0, 3, 3) = rotation_matrix;
            transform.bottomRows<1>() << 0, 0, 0, 1.0;

            transforms_.emplace(std::make_pair(name, transform));
            subs_.push_back(nh_.subscribe(topic, 1, &ObstacleIdentify::__cameraCallback, this));
        }
    }
}

}