#pragma once

#include <iostream>
#include <iomanip>　　　
#include <string>
#include <functional>      
#include <algorithm>
#include <pcl/io/openni2_grabber.h>         //OpenNI采集头文件

#include <pcl/console/parse.h>
#include <boost/thread/thread.hpp>.
#include <pcl/common/common_headers.h>

#include <pcl/features/normal_3d.h>       // 法线
#include <pcl/features/boundary.h>        // 特征提取，边界
#include <pcl/keypoints/harris_3d.h>      // 关键点
#include <pcl/surface/mls.h>

#include <pcl/visualization/pcl_visualizer.h>

#include <pcl/filters/passthrough.h>                    //直通滤波器头文件
#include <pcl/filters/voxel_grid.h>                     //体素滤波器头文件
#include <pcl/filters/statistical_outlier_removal.h>    //统计滤波头文件
#include <pcl/filters/extract_indices.h>                //索引提取滤波器头文件
#include <pcl/filters/project_inliers.h>                //映射相关头文件
#include <pcl/segmentation/region_growing.h>            //区域生长算法

#include <pcl/segmentation/sac_segmentation.h>  //基于采样一致性分割的类的头文件
#include <pcl/ModelCoefficients.h>          //采样一致性模型相关类头文件

#include <pcl/sample_consensus/method_types.h>  //随机参数估计方法头文件
#include <pcl/sample_consensus/model_types.h>   //模型定义头文件

#include <pcl/surface/concave_hull.h>           //提取凸（凹）多边形的头文件.

#include <opencv2/opencv.hpp>
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"

using namespace std;
using namespace cv;


//// 计算法线
//void EstimateNormal(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
//  pcl::PointCloud<pcl::Normal>::Ptr normals);
//
//// 计算边界
//void EstimateBoundary(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
//  pcl::PointCloud<pcl::Normal>::Ptr normals,
//  pcl::PointCloud<pcl::Boundary>& boundaries);
//
//// 检查边界
//void CheckBoundary(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_boundary,         // 保存输入的点云的边界点云
//  pcl::PointCloud<pcl::PointXYZ>::Ptr target_projected,       // 目标投影点云
//  pcl::PointCloud<pcl::Boundary>& boundaries);                 // 边界


class VolumDetectViewer
{
    typedef pcl::visualization::PCLVisualizer PCLVisualizer;
    typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
    typedef const pcl::PointCloud<pcl::PointXYZ>::ConstPtr ConstPointCloudConstPtr;
    typedef pcl::PointCloud<pcl::PointXYZ>::Ptr PointCloudPtr;                            // 点云指针变量 
    typedef const pcl::PointCloud<pcl::PointXYZ>::Ptr ConstPointCloudPtr;                 // 点云指针常量
    typedef pcl::search::KdTree<pcl::PointXYZ> KdTree;                               // kdtree变量
    typedef pcl::search::KdTree<pcl::PointXYZ>::Ptr KdTreePtr;                       // kdtree指针变量
    typedef pcl::PointCloud<pcl::Normal> Normal;
    typedef pcl::PointCloud<pcl::Normal>::Ptr NormalPtr;                            // 点云曲面指针变量
    typedef pcl::PointCloud<pcl::Boundary> Boundary;                                  // 曲面边界变量
    typedef pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointXYZ> MovingLeastSquares;
    typedef pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> NormalEstimation;     // 曲面法向估算器
    typedef pcl::BoundaryEstimation<pcl::PointXYZ, pcl::Normal, pcl::Boundary> BoundaryEstimation;  // 曲面边界估算器
    typedef pcl::PassThrough<pcl::PointXYZ> PassThrough;                                 // 直通滤波器
    //typedef pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal> MovingLeastSquares;      // 平滑点云滤波器
    typedef pcl::VoxelGrid<pcl::PointXYZ> VoxelGrid;                                  // 下采样 VoxelGrid 滤波器
    typedef pcl::StatisticalOutlierRemoval<pcl::PointXYZ> StatisticalOutlierRemoval;                 // 统计滤波器
    typedef pcl::SACSegmentation<pcl::PointXYZ> SACSegmentation;                      // 分割滤波器
    typedef pcl::ExtractIndices<pcl::PointXYZ> ExtractIndices;                            // 提取索引滤波器
    typedef pcl::ProjectInliers<pcl::PointXYZ> ProjectInliers;                          // 点云投影滤波器
    typedef pcl::ModelCoefficients ModelCoefficients;
    typedef pcl::ModelCoefficients::Ptr ModelCoefficientsPtr;
    typedef pcl::PointIndices PointIndices;
    typedef pcl::PointIndices::Ptr PointIndicesPtr;
    typedef pcl::ConcaveHull<pcl::PointXYZ> ConcaveHull;
    typedef pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>  PointCloudColorHandlerCustom;

public:
    boost::mutex cloud_mutex;
    PCLVisualizer viewer;

    // 直通滤波器对象
    PassThrough XpassFilter, YpassFilter, ZpassFilter;

    // 平滑点云对象
    MovingLeastSquares smoothedFilter;

    // 下采样 VoxelGrid 滤波对象
    VoxelGrid voxelFilter;

    // 统计滤波器
    StatisticalOutlierRemoval statisFilter;

    // 分割对象
    SACSegmentation segPlane;

    // 提取索引滤波器
    ExtractIndices extractPlane;

    // 点云投影滤波对象
    ProjectInliers project;

    VolumDetectViewer();

    // void cloud_cb_(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& Prev_cloud, bool* new_cloud_available);
    void cloud_cb_(ConstPointCloudConstPtr& cloud, PointCloudPtr& Prev_cloud, bool* new_cloud_available);

    void run();

    static PointCloudPtr smooth_cloud(PointCloudPtr& CloudPtr);

    static PointCloudPtr extract_targets(PointCloudPtr& CloudPtr, PointIndicesPtr inliersPlane, ExtractIndices& extractPlane);

    static bool extract_pointcloud(PointCloudPtr& CloudPtr, SACSegmentation& segPlane, ModelCoefficientsPtr coefficients, PointIndicesPtr inliers);

    static void EstimateNormal(ConstPointCloudPtr cloud, NormalPtr normals);

    static void EstimateBoundary(PointCloudPtr cloud, NormalPtr normals, Boundary& boundaries);

    static void CheckBoundary(PointCloudPtr cloud_boundary, PointCloudPtr target_projected, Boundary& boundaries);
};

#include "volDetectVisualizer.h"

// 计算法线
void VolumDetectViewer::EstimateNormal(ConstPointCloudPtr cloud, NormalPtr normals)
{
    NormalEstimation normal_est;
    normal_est.setInputCloud(cloud);
    //normal_est.setRadiusSearch(0.05); // 设置为分辨率的10倍时效果较好，邻域半径太小，噪声较大，估计的法线容易出错，而太大则估计速度较慢
    normal_est.setKSearch(100); // 点云法向计算时，需要搜索的近邻点数目
    pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
    normal_est.setSearchMethod(kdtree); // 建立kdtree来进行近邻点集搜索
    normal_est.compute(*normals);
}

// 计算边界
void VolumDetectViewer::EstimateBoundary(PointCloudPtr cloud, NormalPtr normals, Boundary& boundaries) {
    BoundaryEstimation boundary_est;
    boundary_est.setInputCloud(cloud);
    boundary_est.setInputNormals(normals); // 边界估计依赖于法线
    boundary_est.setKSearch(100); // 一般这里的数值越高，最终边界识别的精度越好  10
//  boundary_est.setRadiusSearch(0.1); // 设置为分辨率的10倍时效果较好，太小则内部的很多点就都当成边界点了
    boundary_est.setAngleThreshold(M_PI / 2); // 边界估计时的角度阈值，默认值为PI/2，可根据需要进行更改。
    pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
    boundary_est.setSearchMethod(kdtree);
    boundary_est.compute(boundaries);
}

// 检查边界
void VolumDetectViewer::CheckBoundary(PointCloudPtr cloud_boundary, PointCloudPtr target_projected, Boundary& boundaries)
{
    // 存储边界点云数据
    //pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_boundary(new pcl::PointCloud<pcl::PointXYZ>);
    std::vector<float> contour_x;
    std::vector<float> contour_y;
    for (size_t i = 0; i < target_projected->points.size(); ++i)
    {
        if (boundaries[i].boundary_point > 0)   // 轮廓点云中有轮廓点
        {
            cloud_boundary->push_back(target_projected->points[i]);
            contour_x.push_back(target_projected->points[i].x);  // 保存所有点云的横坐标
            contour_y.push_back(target_projected->points[i].y);  // 保存所有点云的纵坐标 
        }
    }
    std::sort(contour_x.begin(), contour_x.end());  // 横坐标排序
    size_t n = contour_x.size();
    if (abs(contour_x[n - 1] - 0.18f) <= 0.03f || abs(contour_x[0] + 0.18f) <= 0.03f)
        std::cout << "物体在横向越界" << std::endl;

    std::sort(contour_y.begin(), contour_y.end());  // 纵坐标排序
    n = contour_y.size();
    if (abs(contour_y[n - 1] - 0.15f) <= 0.03f || abs(contour_y[0] + 0.15f) <= 0.03f)
        std::cout << "物体在纵向越界" << std::endl;
}

VolumDetectViewer::VolumDetectViewer() : viewer("PCLVIEW")
{
    // 1. 初始化 new viewer
    /*viewer.setBackgroundColor(0, 0, 0);
    viewer.addCoordinateSystem(1.0f, 0.0f, 0.0f, 0.0f);
    viewer.initCameraParameters();
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud");*/

    // 2. 直通滤波设置
    // 滤波字段设为x轴方向.
    XpassFilter.setFilterFieldName("x");
    // 设定可接受范围，将不在范围内的点过滤掉或者保留（由setFilterLimitsNegative决定）.
    XpassFilter.setFilterLimits(-0.18, 0.18);  // TODO: X方向裁剪阈值 实际是0.36m.
    // 滤波字段设为y轴方向.
    YpassFilter.setFilterFieldName("y");
    // 设定可接受范围，将不在范围内的点过滤掉或者保留（由setFilterLimitsNegative决定）.
    YpassFilter.setFilterLimits(-0.15, 0.15);  // TODO: Y方向裁剪阈值 实际是0.30m.

    ZpassFilter.setFilterFieldName("z");
    // 设定可接受范围，将不在范围内的点过滤掉或者保留（由setFilterLimitsNegative决定）.
    ZpassFilter.setFilterLimits(0.6f, 1.2f);  // TODO: Y方向裁剪阈值 实际是0.6m.

    // 3. 体素滤波设置
    // 设置体素滤波时创建的体素体积为0.5*0.5*0.5cm的立方体
    voxelFilter.setLeafSize(0.008f, 0.008f, 0.008f);

    // 4. 统计滤波初始设置
    statisFilter.setMeanK(50);  // 对每个点分析的临近点个数设为50
    statisFilter.setStddevMulThresh(1.0);   // 将标准差倍数设为1，意味着一个点的距离超出平均距离1个标准差以上，就会被标记为离群点，并被移除。

    // 5. 设置分割对象
    // 可选：设置模型系数需要优化
    segPlane.setOptimizeCoefficients(true);
    // 必选：设置分割的模型类型、所用的随机参数估计方法、距离阈值、迭代次数上限
    segPlane.setModelType(pcl::SACMODEL_PLANE);  // SACMODEL_NORMAL_PLANE
    segPlane.setMethodType(pcl::SAC_RANSAC);
    segPlane.setDistanceThreshold(0.01);    // TODO: 距离阈值需要调试
    segPlane.setMaxIterations(1000);

    // 6. 设置投影模型为SACMODEL_PLANE
    project.setModelType(pcl::SACMODEL_PLANE);
}

void VolumDetectViewer::cloud_cb_(ConstPointCloudConstPtr& cloud, PointCloudPtr& Prev_cloud, bool* new_cloud_available)
{
    cloud_mutex.lock();

    *Prev_cloud = *cloud;

    *new_cloud_available = true;

    cloud_mutex.unlock();
}

VolumDetectViewer::PointCloudPtr VolumDetectViewer::smooth_cloud(PointCloudPtr& cloudPtr)
{
    KdTreePtr treeSampling(new KdTree);  // 创建用于最近邻搜索的KD-Tree
    PointCloud mls_point;                //输出MLS
    MovingLeastSquares mls;              // 定义最小二乘实现的对象mls
    mls.setComputeNormals(false);        //设置在最小二乘计算中是否需要存储计算的法线
    mls.setInputCloud(cloudPtr);         //设置待处理点云
    mls.setPolynomialOrder(2);            // 拟合2阶多项式拟合
    //mls.setPolynomialFit(false);        // 设置为false可以 加速 smooth
    mls.setSearchMethod(treeSampling);         // 设置KD-Tree作为搜索方法
    mls.setSearchRadius(0.01);            // 单位m.设置用于拟合的K近邻半径 0.01
    mls.process(mls_point);                 //输出
    return mls_point.makeShared();    // 输出重采样结果
}

VolumDetectViewer::PointCloudPtr VolumDetectViewer::extract_targets(PointCloudPtr& CloudPtr, PointIndicesPtr inliersPlane, ExtractIndices& extractPlane)
{
    // 5. 根据索引提取除地面之外的点云
    PointCloud extracted;                   // 保存提取到的点云
    extractPlane.setInputCloud(CloudPtr);  
    extractPlane.setIndices(inliersPlane);  
    extractPlane.setNegative(true); // 设置Negative是获取除了平面的点
    extractPlane.filter(extracted);
    return extracted.makeShared();
}

bool VolumDetectViewer::extract_pointcloud(PointCloudPtr& CloudPtr, SACSegmentation& segPlane, ModelCoefficientsPtr coefficients, PointIndicesPtr inliers)
{
    segPlane.setInputCloud(CloudPtr);  
    segPlane.segment(*inliers, *coefficients);
    return (inliers->indices.size() != 0);
}

void VolumDetectViewer::run()
{
    // 1. 新建OpenNI设备的捕获器 grabber
    pcl::Grabber* interface = new pcl::io::OpenNI2Grabber();

    PointCloudPtr cloud(new PointCloud);

    bool new_cloud_available = false;
    // 建立回调 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr   
    std::function<void(ConstPointCloudConstPtr&)> f =
        boost::bind(&VolumDetectViewer::cloud_cb_, this, boost::placeholders::_1, cloud, &new_cloud_available);

    interface->registerCallback(f);

    interface->start();

    while (!viewer.wasStopped())
    {

        if (new_cloud_available && cloud_mutex.try_lock())
        {
            new_cloud_available = false;

            // 1. 空间裁剪滤波
            PointCloudPtr pass_filtered(new pcl::PointCloud<pcl::PointXYZ>);

            XpassFilter.setInputCloud(cloud);
            XpassFilter.filter(*pass_filtered);
            YpassFilter.setInputCloud(pass_filtered);
            YpassFilter.filter(*pass_filtered);
            ZpassFilter.setInputCloud(pass_filtered);
            ZpassFilter.filter(*pass_filtered);

            // 2. 下采样进行数据压缩 下采样
            PointCloudPtr voxel_filtered(new pcl::PointCloud<pcl::PointXYZ>);
            voxelFilter.setInputCloud(pass_filtered);
            voxelFilter.filter(*voxel_filtered);

            // 3. 统计滤波 去除异常点
            PointCloudPtr statis_Filtered(new pcl::PointCloud<pcl::PointXYZ>);
            statisFilter.setInputCloud(voxel_filtered);
            statisFilter.filter(*statis_Filtered);

            // 对点云重采样 平滑点云构成的曲面
            auto cloud_smoothed = smooth_cloud(statis_Filtered);

            // 4. 平面分割 获取地面
            // 创建分割时所需要的模型系数对象 coefficientsPlane
            // 创建存储模型内点的点索引集合对象 inliersPlane
            ModelCoefficientsPtr coefficientsPlane(new ModelCoefficients());
            PointIndicesPtr inliersPlane(new PointIndices());
            // 分离地面成功
            if (extract_pointcloud(cloud_smoothed, segPlane, coefficientsPlane, inliersPlane))  
            {
                PointCloudPtr PlaneCloud(new PointCloud);
                pcl::copyPointCloud(*cloud_smoothed, inliersPlane->indices, *PlaneCloud);  
                PointCloudColorHandlerCustom Plane_color(PlaneCloud, 139, 139, 0);

                auto extracted = extract_targets(cloud_smoothed, inliersPlane, extractPlane);  // 提取地面之上的点云

                // 5. 平面分割 获取目标
                // 创建分割时所需要的模型系数对象 coefficientsPlane
                // 创建存储模型内点的点索引集合对象 inliersPlane
                ModelCoefficientsPtr coefficientsTarget(new ModelCoefficients());
                PointIndicesPtr inliersTarget(new PointIndices());
                // 提取目标成功
                if (extract_pointcloud(extracted, segPlane, coefficientsTarget, inliersTarget))  
                {
                    // 拷贝一份平面点集,并着色
                    PointCloudPtr TargetCloud(new PointCloud);
                    pcl::copyPointCloud(*extracted, inliersTarget->indices, *TargetCloud);
                    PointCloudColorHandlerCustom Target_color(TargetCloud, 205, 201, 201);

                    // 7. 计算高度差
                    float height = abs(coefficientsPlane->values[3] / coefficientsPlane->values[2] - coefficientsTarget->values[3] / coefficientsPlane->values[2]);

                    // 8. 投影平面模型系数
                    PointCloudPtr target_projected(new PointCloud);

                    project.setIndices(inliersTarget);
                    project.setInputCloud(extracted);
                    project.setModelCoefficients(coefficientsTarget);
                    project.filter(*target_projected);
                    PointCloudColorHandlerCustom target_projected_color(target_projected, 255, 69, 0);

                    // 计算法线
                    NormalPtr normals(new Normal);
                    EstimateNormal(target_projected, normals);

                    // 提取边缘
                    Boundary boundaries;
                    EstimateBoundary(target_projected, normals, boundaries);

                    // 存储边界点云数据
                    PointCloudPtr cloud_boundary(new pcl::PointCloud<pcl::PointXYZ>);
                    CheckBoundary(cloud_boundary, target_projected, boundaries);
                    PointCloudColorHandlerCustom boundary_color(cloud, 0, 255, 0);

                    // 9. 为投影的点创建凹包表征
                    PointCloudPtr cloud_hull(new PointCloud);
                    ConcaveHull chull;
                    //polygons存储一系列顶点集，每组顶点集表示一个多边形 //应该取其中第0个顶点集
                    vector<pcl::Vertices> polygons;
                    chull.setInputCloud(target_projected);
                    chull.setAlpha(0.1);
                    // polygons对应的是前面point cloud_hull的索引
                    chull.reconstruct(*cloud_hull, polygons);
                    PointCloudColorHandlerCustom cloud_hull_color(cloud_hull, 0, 245, 255);

                    // 10. 获取二维轮廓供OpenCV处理
                    // 生成二维轮廓
                    vector<Point2f> hullContour;
                    for (int j = 0; j < polygons[0].vertices.size(); ++j) 
                    {
                        hullContour.push_back(Point2f(cloud_hull->points[polygons[0].vertices[j]].x,
                            cloud_hull->points[polygons[0].vertices[j]].y));
                    }

                    RotatedRect minRect = cv::minAreaRect(hullContour);

                    // TODO：此处的长宽与实际需要进行换算
                    float realWidth  = 1.145 * 100 * std::max(minRect.size.width, minRect.size.height);
                    float realHeight = 1.145 * 100 * std::min(minRect.size.width, minRect.size.height);

                    cout << setiosflags(ios::fixed) << setprecision(2);
                    std::cout << "length:" << realWidth << " width:" << realHeight << " height " << height * 100 << std::endl;

                    //float rectArea = realWidth * realHeight;


                    // 8. 计算凹包面积
                    // 8.1 自带函数计算
                    // 面积的计算方式是没有问题的
                    /*float Hullarea = pcl::calculatePolygonArea(*cloud_hull);

                    float Vol = rectArea * height;*/


                    // 更新点云显示和字符串显示
                    viewer.removeAllPointClouds();
                    viewer.removeAllShapes();

                    viewer.addPointCloud<pcl::PointXYZ>(statis_Filtered, "cloud");   
                    viewer.addPointCloud<pcl::PointXYZ>(cloud_smoothed, "cloud_smoothed");
                    viewer.addPointCloud<pcl::PointXYZ>(PlaneCloud, Plane_color, "Plane");
                    viewer.addPointCloud<pcl::PointXYZ>(TargetCloud, Target_color, "Target");
                    viewer.addPointCloud<pcl::PointXYZ>(target_projected, target_projected_color, "target_projected");
                    viewer.addPointCloud<pcl::PointXYZ>(cloud_boundary, boundary_color, "boundaries");
                    viewer.addPointCloud<pcl::PointXYZ>(cloud_hull, cloud_hull_color, "cloud_hull");
                }
            }

            viewer.spinOnce();

            cloud_mutex.unlock();
        }
    }


    interface->stop();
}


int main()
{
    VolumDetectViewer v;
    v.run();
    return (0);
}