//
// Created by zx on 2019/12/5.
//

#include "Lidar.h"
#include "LogFiles.h"

pcl::PointCloud<pcl::PointXYZ>::Ptr  laserScan2Cloud(const sensor_msgs::LaserScan& msg,
                                                     Eigen::MatrixXf transform2d, float dist_limit)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZ>);

    float angle = msg.angle_min;
    for (size_t i = 0; i < msg.ranges.size(); ++i)
    {
        const float first_echo = msg.ranges[i];
        // within the border
        if (msg.range_min <= first_echo && first_echo <= msg.range_max && dist_limit > first_echo)
        {
            const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());
            Eigen::MatrixXf  mat=rotation * (first_echo * Eigen::Vector3f::UnitX());
            /*Eigen::Matrix<float,4,1>  e_transform;
            e_transform<<mat,1.0;
            Eigen::MatrixXf  new_pos=tf_btol*e_transform;*/
            float x = mat(0, 0);
            float y = mat(1, 0);
            float newx = x * transform2d(0, 0) + y * transform2d(0, 1) + transform2d(0, 2);
            float newy = x * transform2d(1, 0) + y * transform2d(1, 1) + transform2d(1, 2);
            pcl::PointXYZ point(newx, newy, 0);
            point_cloud->points.push_back(point);
        }
        angle += msg.angle_increment;
    }
    return point_cloud;
}
void SaveTxtCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,std::string strFile)
{
    CLogFile log;
    log.open(strFile.c_str());
    std::cout<<log.is_open()<<std::endl;
    if(log.is_open())
    {
        for(int i=0;i<cloud->size();++i)
        {
            pcl::PointXYZ point=cloud->points[i];
            char buf[255]={0};
            sprintf(buf,"%.3f %.3f %.3f\n",point.x,point.y,point.z);
            log.write(buf,strlen(buf));
        }
        log.close();
    }
}

Lidar::Lidar(EleFence::lidarParam lidar_param, ros::NodeHandle handle):
b_save_cloud(0)
{
    m_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
    m_lidar_param=lidar_param;
    m_handle=handle;
//    std::cout<<"topic: "<<lidar_param.topic()<<std::endl;
    m_scan_sub=m_handle.subscribe<sensor_msgs::LaserScan>(lidar_param.topic(), 1,
                                                          boost::bind(cloud_callback,_1,this));
}
Lidar::~Lidar()
{

}

void Lidar::cloud_callback(const sensor_msgs::LaserScan::ConstPtr& msg,Lidar* pLidar)
{
//    ROS_INFO("cloud callbacked");
    if(pLidar==0) return ;
    EleFence::Transform2d T=pLidar->m_lidar_param.transform();
    float dist_limit = pLidar->m_lidar_param.dist_limit();
    Eigen::Matrix<float,2,3> transform;
    transform(0,0)=T.m00();
    transform(0,1)=T.m01();
    transform(0,2)=T.m02();
    transform(1,0)=T.m10();
    transform(1,1)=T.m11();
    transform(1,2)=T.m12();
    pLidar->m_cloud=laserScan2Cloud(*msg,transform,dist_limit);
    pLidar->m_cloud_time = ros::Time::now();

    if(pLidar->b_save_cloud==false)
    {
        char buf[255] = {0};
        if (pLidar->m_lidar_param.topic().size() > 0 && pLidar->m_lidar_param.topic().at(0) == '/')
        {
            sprintf(buf, "/home/youchen/data/wj_wheels/cloud_%s.txt", pLidar->m_lidar_param.topic().c_str() + 1);
        }
        else
        {
            sprintf(buf, "/home/youchen/data/wj_wheels/cloud_%s.txt", pLidar->m_lidar_param.topic().c_str());
        }
        SaveTxtCloud(pLidar->m_cloud, buf);
        std::cout<<" save cloud   "<<buf<<std::endl;
        pLidar->b_save_cloud=true;
    }
}

bool Lidar::GetCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, int timeout_milli)
{
    cloud=m_cloud;
//    std::cout<<cloud->size()<<std::endl;
//    return true;
    ros::Duration duration = ros::Time::now() - m_cloud_time;
    if(duration.toSec()*1000 > timeout_milli)
        return false;
    else
        return true;
}