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


tf::Pose EigenToTF(Eigen::Matrix4f mat)
{
    Eigen::Matrix3f rotation=mat.block<3,3>(0,0);
    Eigen::Quaterniond eigen_quat(rotation.cast<double>());
    Eigen::Vector3d eigen_trans(mat.block<3,1>(0,3).cast<double>());
    tf::Quaternion tf_quat(eigen_quat.x(), eigen_quat.y(), eigen_quat.z(), eigen_quat.w());
    tf::Vector3 offset(eigen_trans(0),eigen_trans(1),0);
    tf::Pose pose(tf_quat,offset);
    return pose;
}
Eigen::Matrix4f TFToEigen(tf::Pose pose)
{
    Eigen::Matrix4f tf_btol;
    Eigen::Translation3f tl_btol(pose.getOrigin().getX(), pose.getOrigin().getY(), pose.getOrigin().getZ());
    double roll, pitch, yaw;
    tf::Matrix3x3(pose.getRotation()).getEulerYPR(yaw, pitch, roll);
    Eigen::AngleAxisf rot_x_btol(roll, Eigen::Vector3f::UnitX());
    Eigen::AngleAxisf rot_y_btol(pitch, Eigen::Vector3f::UnitY());
    Eigen::AngleAxisf rot_z_btol(yaw, Eigen::Vector3f::UnitZ());
    tf_btol = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix();  

    return tf_btol;
}


void NdtTask::Main()
{
  Eigen::Matrix4f init_matrix=TFToEigen(m_initPose);
  //init_matrix=Eigen::Matrix4f::Identity(); 
  pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr ndt_omp(new pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
  
  int n = 2;
  ndt_omp->setInputTarget(m_target);
  ndt_omp->setInputSource(m_source);
  ndt_omp->setNumThreads(n);
  ndt_omp->setTransformationEpsilon(1e-5);
  ndt_omp->setStepSize(0.05);
  ndt_omp->setResolution(0.3);
  ndt_omp->setMaximumIterations(30);
  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_matrix);
 
  Eigen::Matrix4f last_matrix = ndt_omp->getFinalTransformation();
  //std::cout<<"last "<<std::endl<<last_matrix<<std::endl;
  m_last_pose=EigenToTF(last_matrix);
  m_statu=eSucc;
  
}



#include <csm/csm_all.h>

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;
}

void IcpTask::Main()
{
  double T[3]={0};
  T[0]=m_initPose.getOrigin().getX();
  T[1]=m_initPose.getOrigin().getY();
   double roll, pitch, yaw;
   tf::Matrix3x3(m_initPose.getRotation()).getEulerYPR(yaw, pitch, roll);
  T[2]=yaw;

  LDP ldp_source;
  LDP ldp_target;
  PointCloudToLDP(m_source,ldp_source);
  PointCloudToLDP(m_target,ldp_target);
  

  struct sm_params param;
  param.laser_ref=ldp_target;
  param.laser_sens=ldp_source;
  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();
  ld_free(ldp_source);
  ld_free(ldp_target);
  //std::cout<<std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()<<std::endl;
  if(res.valid!=1)
  {
    m_statu=eFailed;
    std::cout<<"  icp task failed..."<<std::endl;
    return ;
  }
  m_statu=eSucc;
  
  m_last_pose.setIdentity();
  tf::Matrix3x3 tf_matrix;
  tf_matrix.setIdentity();
  tf_matrix.setRPY(0,0,res.x[2]);
  m_last_pose.setBasis(tf_matrix);
  m_last_pose.setOrigin(tf::Vector3(res.x[0],res.x[1],0));
  

}