
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/LaserScan.h>
#include <nav_msgs/OccupancyGrid.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <pcl/common/transforms.h>	
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/io/pcd_io.h>
#include <boost/thread.hpp>

#undef max()
#undef min()
#include <chrono>
#include "./TaskQueue/TaskPool.h"
#include "./TaskQueue/TQFactory.h"
#include "locateTask.h"
using namespace tq;

#include "kalmanFilter.h"
#include "cfg/grapher_cfg.pb.h"
#include "cfg/protobuf_io.h"

grapher::grapherCfg   grapher_config;

ros::Publisher pubCloudL;
ros::Publisher pubCloudG;

KalmanFilter klmFilter;

pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ>);

pcl::PointCloud<pcl::PointXYZ>::Ptr transformCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr input,tf::Pose pose)
{
    Eigen::Matrix4f transform=TFToEigen(pose);
    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
    pcl::transformPointCloud (*input, *transformed_cloud, transform);
    return transformed_cloud;

}

void scanHandler(const sensor_msgs::LaserScan::ConstPtr& msg)
{
    if(target_cloud->size()<50)
    {
      std::cout<<" target size (<50)  : "<<target_cloud->size()<<std::endl;
      return ;
    }
    static int systemInitCount=0;
    static bool systemInited=false;
    int systemDelay=20;
    if (!systemInited) 
    {//丢弃前20个点云数据
        systemInitCount++;
        if (systemInitCount >= systemDelay) {
        systemInited = true;
        }
        return;
    }


     //当前点云时间
    double timeScanCur = msg->header.stamp.toSec();
    int N = int( (msg->angle_max-msg->angle_min)/msg->angle_increment )+1;
    pcl::PointCloud<pcl::PointXYZ>  laserCloud1;
    for(int i=0;i<N;++i)
    {
        float distance = msg->ranges[i];
        
        float angle = msg->angle_min+ i*msg->angle_increment;
        if(distance >20.0)
          continue;
        //第一帧 用作 target  使用所有点，非第一帧只保留前方一半点
        pcl::PointXYZ point ;
        point.x= -sin(angle-M_PI_2)*distance;
        point.y= cos(angle-M_PI_2)*distance;
        point.z=0;
        if(fabs(point.x)>1.0 || fabs(point.y)>1.0)
        {   
          laserCloud1.points.push_back(point);
        }
    }
    if(laserCloud1.size()<10)
        return ;


   static tf::Pose init_pose=tf::Pose::getIdentity();
    //线程队列
    IQueue* q = TQFactory::CreateDefaultQueue();
    q->Start(grapher_config.locate_functions_size());
    std::vector<LocateTask*>  pTasks;
    for(int i=0;i<grapher_config.locate_functions_size();++i)
    {
      LocateTask* pTask=0;
      if(grapher_config.locate_functions(i).type()=="ndt")
        pTask=new NdtTask(laserCloud1.makeShared(),target_cloud,init_pose); // ndt 算法 
      else if(grapher_config.locate_functions(i).type()=="icp")
        pTask=new IcpTask(laserCloud1.makeShared(),target_cloud,init_pose); // icp 算法 
      else  continue;
      pTasks.push_back(pTask);
      q->AddTask(pTask);
    }
    q->WaitForFinish();
   
    //获取结果，发布tf
    static tf::TransformBroadcaster tfBroadcaster;
    std::vector<tf::Pose> poses;
    tf::Pose lastPose=tf::Pose::getIdentity();
    for(int i=0;i<grapher_config.locate_functions_size();++i)
    {
      LocateTask* pTask=pTasks[i];
      tf::Pose pose;
      if(pTask->GetResult(pose))
      {
        lastPose=pose;
        poses.push_back(pose);
        std::string header_id=grapher_config.locate_functions(i).tfheaderid();
        std::string frame_id=grapher_config.locate_functions(i).tfid();
        
        tf::StampedTransform laserOdometryTrans;
        laserOdometryTrans.frame_id_ = header_id;
        laserOdometryTrans.child_frame_id_ = frame_id;
        laserOdometryTrans.stamp_ = ros::Time::now();
        laserOdometryTrans.setRotation(pose.getRotation());
        laserOdometryTrans.setOrigin(pose.getOrigin());
        tfBroadcaster.sendTransform(laserOdometryTrans);
      }
      //析构
      delete pTask;
    }
    TQFactory::ReleaseQueue(q);
    //滤波
    if(poses.size()==2)
       lastPose=klmFilter.filter(poses[0],poses[1]);
    

    /* static FILE* file=fopen("./trajectory.txt","w");
    fprintf(file,"%.3f %.3f %.3f\n",lastPose.getOrigin().getX(),lastPose.getOrigin().getY(),lastPose.getOrigin().getZ());*/

    tf::StampedTransform laserOdometryTrans;
    laserOdometryTrans.frame_id_ = "/map";
    laserOdometryTrans.child_frame_id_ = "/laser";
    laserOdometryTrans.stamp_ = ros::Time::now();
    laserOdometryTrans.setRotation(lastPose.getRotation());
    laserOdometryTrans.setOrigin(lastPose.getOrigin());
    tfBroadcaster.sendTransform(laserOdometryTrans);

    ///变换点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_T=transformCloud(laserCloud1.makeShared(),lastPose);
    init_pose=lastPose;

    sensor_msgs::PointCloud2 cloudLocalMsg;
    pcl::toROSMsg(*cloud_T, cloudLocalMsg);
    cloudLocalMsg.header.stamp = msg->header.stamp;
    cloudLocalMsg.header.frame_id = "/map";
    pubCloudL.publish(cloudLocalMsg);

}

void publish()
{
    ros::Rate loop_rate(10);
    
    while(ros::ok())
    {
        if(target_cloud->size()>0)
        {
            sensor_msgs::PointCloud2 cloudLocalMsg;
            pcl::toROSMsg(*target_cloud, cloudLocalMsg);
            cloudLocalMsg.header.stamp = ros::Time::now();
            cloudLocalMsg.header.frame_id = "/map";
            pubCloudG.publish(cloudLocalMsg);
        }
        ros::spinOnce();
        loop_rate.sleep();
    }
}


int main(int argc, char** argv)
{
//    if(false==ReadProtoConfig("/home/zx/zzw/catkin_ws/src/grapher/grapher.cfg",grapher_config))
    if(false==ReadProtoConfig("/home/youchen/Documents/git_ws/src/grapher/grapher.cfg",grapher_config))
    {
      printf(" config file load failed ... \n");
      return -1;
    }
    ros::init(argc, argv, "grapher_node");
    ros::NodeHandle nh;

    
    pubCloudL = nh.advertise<sensor_msgs::PointCloud2>
                                 ("/cloud_local", 2);
    pubCloudG = nh.advertise<sensor_msgs::PointCloud2>
                                 ("/cloud_global", 2);

//    pcl::io::loadPCDFile("/home/zx/zzw/map.pcd", *target_cloud);
    pcl::io::loadPCDFile("/home/youchen/Documents/git_ws/src/grapher/map.pcd", *target_cloud);
    boost::thread* thread_=new boost::thread(publish);
    std::string scanID=grapher_config.scanid();
    ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::LaserScan>(scanID, 2,scanHandler);

    ros::spin();
    

    return 0;
}