//
// Created by zx on 23-12-7.
//

#include "fallModel.h"
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/common.h>

FallModel::FallModel(){

}
bool FallModel::fall(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,pcl::PointCloud<pcl::PointXYZ>::Ptr& out,
                                                    pcl::PointXYZ& tp,float& theta,FallDirect direct){

  auto t1=std::chrono::steady_clock::now();
  pcl::PointXYZ minp,maxp;
  pcl::getMinMax3D(*cloud,minp,maxp);
  float plate=minp.x;
  if(direct==ePositive)
    plate=maxp.x;

  Eigen::Vector4f centroid;  //质心
  pcl::compute3DCentroid(*cloud,centroid); // 计算质心
  pcl::PointXYZ src_center=pcl::PointXYZ(centroid[0],centroid[1],centroid[2]);

  pcl::PointXYZ new_center, force_center;
  float dtheta=M_PI,dx=1.0,forceLen=1.0;
  pcl::PointCloud<pcl::PointXYZ>::Ptr t_cl=cloud;
  int iter=0;
  while(  fabs(dx)>1e-5 || fabs(forceLen)>1e-4) {
    if(iter==1000){
      printf(" iter 1000 Failed\n");
      return false;
    }

    float gravy, force;
    computeForce(t_cl, new_center, force_center, gravy, force, plate,direct);

    float dis = plate - new_center.x;
    float r = 1 - exp(-100 * dis * dis);
    if(direct==ePositive)
      dx = r * (gravy-force) / float(t_cl->size());
    else
      dx = r * (force - gravy) / float(t_cl->size());

    dx = 0.02 * (1 / (1. + exp(-81. * dx)) - 0.5);

    if(direct==ePositive)
      forceLen = force_center.y - new_center.y;
    else
      forceLen = -force_center.y + new_center.y;

    dtheta = force * forceLen * M_PI / 180.0 * 0.01;
    float dyaw = 0.05 * (1 / (1. + exp(-81. * dtheta)) - 0.5);
    t_cl = transformCloud(t_cl, new_center, dx, dyaw);

    theta += dyaw;
    /*printf(" gravy:%f  force:%f force_len:%f  dx :%f  dtheta:%f Yaw:%f\n", gravy, force,
           forceLen, dx, dtheta * 180.0 / M_PI, theta * 180.0 / M_PI);*/
    iter++;
  }
  auto t2=std::chrono::steady_clock::now();
  auto duration = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);
  double time=double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;

  printf("iter:%d  time:%f\n",iter,time);
  tp=pcl::PointXYZ(new_center.x-src_center.x,new_center.y-src_center.y,new_center.z-src_center.z);
  out= t_cl;
  return true;
}

void FallModel::computeForce(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,pcl::PointXYZ& center,
                  pcl::PointXYZ& force_center,float& gravy,float& force,float plate ,FallDirect direct){
  Eigen::Vector4f centroid;  //质心
  pcl::compute3DCentroid(*cloud,centroid); // 计算质心
  center=pcl::PointXYZ(centroid[0],centroid[1],centroid[2]);
  gravy=cloud->size()*1.0;
  force=0;
  pcl::PointCloud<pcl::PointXYZ>::Ptr gravy_cl(new pcl::PointCloud<pcl::PointXYZ>);
  for(int i=0;i<cloud->size();++i){
    float deep=cloud->points[i].x-plate;
    if(direct==eNagtive) {
      if (deep < 0) {
        float t = pow(36 * deep, 4) * 10.0;
        if (t > 2) {
          t = 2;
        }
        force += t;
        gravy_cl->push_back(cloud->points[i]);
      }
    }else{
      if (deep > 0) {
        float t = pow(36 * deep, 4) * 10.0;
        if (t > 2) {
          t = 2;
        }
        force += t;
        gravy_cl->push_back(cloud->points[i]);
      }
    }
  }
  pcl::compute3DCentroid(*gravy_cl,centroid); // 计算质心
  force_center=pcl::PointXYZ(centroid[0],centroid[1],centroid[2]);
}

pcl::PointCloud<pcl::PointXYZ>::Ptr FallModel::transformCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
                                             pcl::PointXYZ center,float dx,float theta) {
  Eigen::Rotation2D<float> rotation(theta);
  Eigen::Matrix<float, 2, 2> rotation_matrix = rotation.toRotationMatrix();
  pcl::PointCloud<pcl::PointXYZ>::Ptr r_cl(new pcl::PointCloud<pcl::PointXYZ>);
  for (int i = 0; i < cloud->size(); ++i) {
    const Eigen::Matrix<float, 2, 1> point(cloud->points[i].x, cloud->points[i].y);
    const Eigen::Matrix<float, 2, 1> tp(center.x, center.y);
    //减去经过车辆旋转计算的左前中心
    const Eigen::Matrix<float, 2, 1> tanslate_point = rotation_matrix * (point - tp) +tp;
    r_cl->push_back(pcl::PointXYZ(tanslate_point[0]+dx, tanslate_point[1], 0));
  }
  return r_cl;
}