
#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>
#include <csm/csm_all.h>
#undef max()
#undef min()
#include <chrono>

#include "pclomp/ndt_omp.h"
#include "kalmanFilter.h"

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

double param[3]={0};


KalmanFilter klmFilter;

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

void laserScanToLDP(const sensor_msgs::LaserScan::ConstPtr& scan_msg,LDP& ldp)
{
  unsigned int n = scan_msg->ranges.size();
  ldp = ld_alloc_new(n);

  for (unsigned int i = 0; i < n; i++)
  {
    // calculate position in laser frame

    double r = scan_msg->ranges[i];

    if (r > scan_msg->range_min && r < scan_msg->range_max)
    {
      // fill in laser scan data

      ldp->valid[i] = 1;
      ldp->readings[i] = r;
    }
    else
    {
      ldp->valid[i] = 0;
      ldp->readings[i] = -1;  // for invalid range
    }

    ldp->theta[i]    = scan_msg->angle_min + i * scan_msg->angle_increment;

    ldp->cluster[i]  = -1;
  }

  ldp->min_theta = ldp->theta[0];
  ldp->max_theta = ldp->theta[n-1];

  ldp->odometry[0] = 0.0;
  ldp->odometry[1] = 0.0;
  ldp->odometry[2] = 0.0;

  ldp->true_pose[0] = 0.0;
  ldp->true_pose[1] = 0.0;
  ldp->true_pose[2] = 0.0;
}

void PointCloudToLDP(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,LDP& ldp)
{
  double max_d2 = 0.01*0.01;

  pcl::PointCloud<pcl::PointXYZ> cloud_f;

  cloud_f.points.push_back(cloud->points[0]);

  for (unsigned int i = 1; i < cloud->points.size(); ++i)
  {
    const pcl::PointXYZ& pa = cloud_f.points[cloud_f.points.size() - 1];
    const pcl::PointXYZ& pb = cloud->points[i];
    double theta_pa=atan2(pa.y,pa.x);
    double theta_pb=atan2(pb.y,pb.x);
    double dx = pa.x - pb.x;
    double dy = pa.y - pb.y;
    double d2 = dx*dx + dy*dy;

    cloud_f.points.push_back(pb);
    
  }

  unsigned int n = cloud_f.points.size();

  ldp = ld_alloc_new(n);

  for (unsigned int i = 0; i < n; i++)
  {
    // calculate position in laser frame
    if (is_nan(cloud_f.points[i].x) || is_nan(cloud_f.points[i].y))
    {
      ROS_WARN("Laser Scan Matcher: Cloud input contains NaN values. \
                Please use a filtered cloud input.");
    }
    else
    {
      double r = sqrt(cloud_f.points[i].x * cloud_f.points[i].x +
                      cloud_f.points[i].y * cloud_f.points[i].y);

      if (1/* r > -135.0/180.0*M_PI && r < 35.0/180.0*M_PI*/)
      {
        ldp->valid[i] = 1;
        ldp->readings[i] = r;
      }
      else
      {
        ldp->valid[i] = 0;
        ldp->readings[i] = -1;  // for invalid range
      }
    }

    ldp->theta[i] = atan2(cloud_f.points[i].y, cloud_f.points[i].x);
    ldp->cluster[i]  = -1;
  }

  ldp->min_theta = ldp->theta[0];
  ldp->max_theta = ldp->theta[n-1];

  //std::cout<<" scan theta :"<<ldp->min_theta<<","<<ldp->max_theta<<"."<<n<<std::endl;

  ldp->odometry[0] = 0.0;
  ldp->odometry[1] = 0.0;
  ldp->odometry[2] = 0.0;

  ldp->true_pose[0] = 0.0;
  ldp->true_pose[1] = 0.0;
  ldp->true_pose[2] = 0.0;
}

pcl::PointCloud<pcl::PointXYZ>::Ptr NDT(pcl::PointCloud<pcl::PointXYZ>::Ptr ori_ptr,
      pcl::PointCloud<pcl::PointXYZ>::Ptr cur_ptr,Eigen::Matrix4f& init_param)
{
  // Initializing Normal Distributions Transform (NDT).

  pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr ndt_omp(new pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
  
  int n = 2;
  ndt_omp->setInputTarget(ori_ptr);
  ndt_omp->setInputSource(cur_ptr);
  ndt_omp->setNumThreads(n);
  ndt_omp->setTransformationEpsilon(1e-12);
  ndt_omp->setStepSize(0.01);
  ndt_omp->setResolution(0.3);
  ndt_omp->setMaximumIterations(50);
  ndt_omp->setNeighborhoodSearchMethod(pclomp::DIRECT7);

  // Calculating required rigid transform to align the input cloud to the target cloud.
  pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  ndt_omp->align (*output_cloud, init_param);
  /* std::cout << "target size"<<ori_ptr->size()<<", input size:"<<cur_ptr->size()<<"," << ndt_omp->hasConverged ()
            << " score: " << ndt_omp->getFitnessScore () << std::endl;*/

  init_param= ndt_omp->getFinalTransformation();
  
  Eigen::Matrix3f rotation=init_param.block<3,3>(0,0);
  Eigen::Quaterniond eigen_quat(rotation.cast<double>());
  Eigen::Vector3d eigen_trans(init_param.block<3,1>(0,3).cast<double>());


  tf::Quaternion tf_quat(eigen_quat.x(), eigen_quat.y(), eigen_quat.z(), eigen_quat.w());
  
    static  tf::TransformBroadcaster tfBroadcaster;
    tf::StampedTransform laserOdometryTrans;
    laserOdometryTrans.frame_id_ = "/map";
    laserOdometryTrans.child_frame_id_ = "/NDT";

    laserOdometryTrans.stamp_ = ros::Time::now();
    laserOdometryTrans.setRotation(tf_quat);
    laserOdometryTrans.setOrigin(tf::Vector3(eigen_trans(0), eigen_trans(1), 0));
    tfBroadcaster.sendTransform(laserOdometryTrans);

  return output_cloud;

}

bool ICP(LDP& ldp,LDP& ldp_target,double* T)
{

  
  struct sm_params param;
  param.laser_ref=ldp_target;
  param.laser_sens=ldp;
  param.first_guess[0]=T[0];
  param.first_guess[1]=T[1];
  param.first_guess[2]=T[2];
  param.max_angular_correction_deg=45.0;
  param.max_linear_correction=0.5;
  param.max_iterations=50;

  param.epsilon_xy=0.000001;
  param.epsilon_theta=0.000001;

  param.max_correspondence_dist=0.3;
  param.use_corr_tricks=1;
  param.restart=0;
  param.restart_threshold_mean_error=0.01;
  param.restart_dt=1.0;
  param.restart_dtheta=0.1;
  param.outliers_maxPerc=0.9;
  param.outliers_adaptive_order=0.7;
  param.outliers_adaptive_mult=2.0;
  param.outliers_remove_doubles=1;
  param.clustering_threshold=0.25;
  param.orientation_neighbourhood=20;
  param.do_alpha_test=0;
  param.do_alpha_test_thresholdDeg=20.0;
  param.do_visibility_test=0;
  param.use_point_to_line_distance=1;
  param.use_ml_weights=0;
  param.use_sigma_weights=0;
  param.do_compute_covariance=0;
  param.debug_verify_tricks=0;

  param.laser[0]=0;
  param.laser[1]=0;
  param.laser[2]=0;

  param.sigma=0.01;
  param.min_reading=0.02;
  param.max_reading=30.0;

  param.gpm_theta_bin_size_deg=0.0;
  param.gpm_extend_range_deg=0.0;
  param.gpm_interval=280;

  param.hsm.max_norm=0.0;
  param.hsm.linear_cell_size=0.0;
  param.hsm.angular_cell_size_deg=0.0;
  param.hsm.num_angular_hypotheses=4502664;
  param.hsm.angular_hyp_min_distance_deg=0.0;
  param.hsm.xc_ndirections=6668808;
  param.hsm.xc_directions_min_distance_deg=0.0;
  param.hsm.linear_xc_max_npeaks=6673640;

  param.hsm.linear_xc_peaks_min_distance=0.0;
  param.hsm.max_translation=0.0;
  param.hsm.debug_true_x_valid=-531946640;

  param.hsm.debug_true_x[0]=0.0;
  param.hsm.debug_true_x[1]=0.0;
  param.hsm.debug_true_x[2]=0.0;
  
  
  std::chrono::time_point<std::chrono::system_clock> start=std::chrono::system_clock::now();
  struct sm_result res;
  sm_icp(&param,&res);
  std::chrono::time_point<std::chrono::system_clock> end=std::chrono::system_clock::now();

  //std::cout<<std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()<<std::endl;
  if(res.valid!=1)
    return false;
  T[0]=res.x[0];
  T[1]=res.x[1];
  T[2]=res.x[2];


     tf::Quaternion tf_quat(tf::Vector3(0,0,1),T[2]);
  
    static  tf::TransformBroadcaster tfBroadcaster;
    tf::StampedTransform laserOdometryTrans;
    laserOdometryTrans.frame_id_ = "/map";
    laserOdometryTrans.child_frame_id_ = "/ICP";

    laserOdometryTrans.stamp_ = ros::Time::now();
    laserOdometryTrans.setRotation(tf_quat);
    laserOdometryTrans.setOrigin(tf::Vector3(T[0], T[1], 0));
    tfBroadcaster.sendTransform(laserOdometryTrans);
  

  return true;

}



void scanHandler(const sensor_msgs::LaserScan::ConstPtr& msg)
{

  
    static int systemInitCount=0;
    static bool systemInited=false;
    int systemDelay=100;
    if (!systemInited) 
    {//丢弃前20个点云数据
        systemInitCount++;
        if (systemInitCount >= systemDelay) {
        systemInited = true;
        }
        return;
    }

  //当前点云时间
    double timeScanCur = msg->header.stamp.toSec();
    static bool first_frame=true;
    int N = int( (msg->angle_max-msg->angle_min)/msg->angle_increment )+1;
    
    //ICP 算法 参数 
    pcl::PointCloud<pcl::PointXYZ>  laserCloud1;
    static double param_sum[3]={0};

    // ndt 算法参数
    Eigen::AngleAxisf init_rotation (0, Eigen::Vector3f::UnitZ ());
    Eigen::Translation3f init_translation (0, 0, 0);
    static Eigen::Matrix4f init_tf = (init_translation * init_rotation).matrix ();
 

    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  使用所有点，非第一帧只保留前方一半点
        if(first_frame==false)
        {
          if(angle<-M_PI_2 || angle> M_PI_2)
            continue;
        }
        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(first_frame)
          {
            //target_cloud->push_back(point);
          }
        }

        
    }
    if(laserCloud1.size()<10)
        return ;
    
    std::vector<tf::Pose> poses;
    tf::Pose lastPose;
    // ndt 算法 
    NDT(target_cloud,laserCloud1.makeShared(),init_tf);
    Eigen::Matrix3f rotation=init_tf.block<3,3>(0,0);
    Eigen::Quaterniond eigen_quat(rotation.cast<double>());
    Eigen::Vector3d eigen_trans(init_tf.block<3,1>(0,3).cast<double>());
    tf::Quaternion tf_quat_NDT(eigen_quat.x(), eigen_quat.y(), eigen_quat.z(), eigen_quat.w());
    tf::Vector3 offset_NDT(eigen_trans(0),eigen_trans(1),0);
    tf::Pose pose_NDT(tf_quat_NDT,offset_NDT);
    lastPose=pose_NDT;
    poses.push_back(pose_NDT);
    
    //// icp  算法 
    
    /*   LDP source_ldp;
    //PointCloudToLDP(laserCloud1.makeShared(),source_ldp);
    laserScanToLDP(msg,source_ldp);
    static LDP target_ldp;
    
    
    if(first_frame==true)
    {
      //PointCloudToLDP(target_cloud,target_ldp);
      //PointCloudToLDP(laserCloud1.makeShared(),target_ldp);
      laserScanToLDP(msg,target_ldp);
      
    }
    

    
    if(!ICP(source_ldp,target_ldp,param_sum))
    {
        std::cout<<" ICP failed...   "<<std::endl;
        ld_free(source_ldp);
        
    }
    else
    {
      ld_free(source_ldp);
      tf::Quaternion tf_quat_ICP(tf::Vector3(0,0,1),param_sum[2]);
      tf::Vector3 offset_ICP(param_sum[0],param_sum[1],0);
      tf::Pose pose_ICP(tf_quat_ICP,offset_ICP);
      poses.push_back(pose_ICP);
      lastPose=pose_ICP;
    }

    */
    first_frame=false;
    if(poses.size()==2)
      lastPose=klmFilter.filter(poses[0],poses[1]);


    static  tf::TransformBroadcaster tfBroadcaster;
    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);


}

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)
{
    
    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/ws/src/grapher/map.pcd", *target_cloud);
    boost::thread* thread_=new boost::thread(publish);

    ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::LaserScan>("/scan", 2,scanHandler);

    ros::spin();
    

    return 0;
}