//
// Created by zx on 2021/8/6.
//

#include "shuttler_verifier.h"

shuttler_verifier::shuttler_verifier()
{

}
shuttler_verifier::~shuttler_verifier()
{

}

shuttler_verifier& shuttler_verifier::operator=(const shuttler_verifier& shutter)
{
    m_plane=shutter.m_plane;
    m_maxd=shutter.m_maxd;
    m_mind=shutter.m_mind;

}
void shuttler_verifier::set_condition(Eigen::Vector4f plane, float maxd, float mind)
{
    m_plane=plane;
    m_maxd=maxd;
    m_mind=mind;

}

Error_manager shuttler_verifier::verify(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    for(int i=0;i<cloud->size();++i)
    {
        if(pointInRectangle(cloud->points[i]))
            return FAILED;
    }
    return SUCCESS;
}

bool shuttler_verifier::pointInRectangle(pcl::PointXYZ point)
{

    float a=m_plane[0];
    float b=m_plane[1];
    float c=m_plane[2];
    float d=m_plane[3];
    //计算点到该平面的距离

    float D=(a*point.x+b*point.y+c*point.z+d)/sqrt(a*a+b*b+c*c);
    if(D>m_mind && D<m_maxd)
        return true;

    return false;
}

void shuttler_verifier::create_plane(pcl::visualization::PCLVisualizer& viewer)
{
    static int count=0;
    char name[64]={0};
    sprintf(name,"plane_%d",count++);

    float a=m_plane[0],b=m_plane[1],c=m_plane[2],d=m_plane[3];
    float x=3,z=0.5;
    float y=(-d-a*x-c*z)/b;
    float rz=atan2(a,b);
    float ry=atan2(a,c);
    float rx=atan2(c,b);
    Eigen::Vector3f center(x,y,z);
    float ea[] = {rx,ry,rz};
    Eigen::Quaternionf quaternion = Eigen::AngleAxisf(ea[0], Eigen::Vector3f::UnitX()) *
            Eigen::AngleAxisf(ea[1], Eigen::Vector3f::UnitY()) *
            Eigen::AngleAxisf(ea[2], Eigen::Vector3f::UnitZ());

    viewer.addCube(center, quaternion, 6, 0.04, 1, name);
    viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0.5, 0, name);
    viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.3, name);


}