//
// Created by zx on 2021/5/20.
//

#include "ground_region.h"

#include <pcl/common/transforms.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/segmentation/extract_clusters.h>
#include <fcntl.h>
#include "publisher.h"
#include "point_tool.h"
//欧式聚类*******************************************************
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> Ground_region::segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr sor_cloud)
{
    std::vector<pcl::PointIndices> ece_inlier;
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ece;
    ece.setInputCloud(sor_cloud);
    ece.setClusterTolerance(0.07);
    ece.setMinClusterSize(20);
    ece.setMaxClusterSize(20000);
    ece.setSearchMethod(tree);
    ece.extract(ece_inlier);

    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>  segmentation_clouds;
    for (int i = 0; i < ece_inlier.size(); i++)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_copy(new pcl::PointCloud<pcl::PointXYZ>);
        std::vector<int> ece_inlier_ext = ece_inlier[i].indices;
        copyPointCloud(*sor_cloud, ece_inlier_ext, *cloud_copy);//按照索引提取点云数据
        segmentation_clouds.push_back(cloud_copy);
    }
    return segmentation_clouds;
}

double Ground_region::distance(cv::Point2f p1, cv::Point2f p2)
{
    return sqrt(pow(p1.x - p2.x, 2.0) + pow(p1.y - p2.y, 2.0));
}


bool Ground_region::isRect(std::vector<cv::Point2f>& points)
{
    if (points.size() == 4)
    {
        double L[3] = {0.0};
        L[0] = distance(points[0], points[1]);
        L[1] = distance(points[1], points[2]);
        L[2] = distance(points[0], points[2]);
        double max_l = L[0];
        double l1 = L[1];
        double l2 = L[2];
        cv::Point2f ps = points[0], pt = points[1];
        cv::Point2f pc = points[2];
        for (int i = 1; i < 3; ++i) {
            if (L[i] > max_l)
            {
                max_l = L[i];
                l1 = L[abs(i + 1) % 3];
                l2 = L[abs(i + 2) % 3];
                ps = points[i % 3];
                pt = points[(i + 1) % 3];
                pc = points[(i + 2) % 3];
            }
        }

        //直角边与坐标轴的夹角 <20°
        float thresh=20.0*M_PI/180.0;
        cv::Point2f vct(pt.x-pc.x,pt.y-pc.y);
        float angle=atan2(vct.y,vct.x);
        if(!(fabs(angle)<thresh || (M_PI_2-fabs(angle)<thresh)))
        {
            //std::cout<<" 4 wheel axis angle : "<<angle<<std::endl;
            return false;
        }

        double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
        if (fabs(cosa) >= 0.15) {
            /*char description[255]={0};
            sprintf(description,"angle cos value(%.2f) >0.13 ",cosa);
            std::cout<<description<<std::endl;*/
            return false;
        }

        float width=std::min(l1,l2);
        float length=std::max(l1,l2);
        if(width<1.400 || width >1.900 || length >3.300 ||length < 2.200)
        {
            /*char description[255]={0};
            sprintf(description,"width<1400 || width >1900 || length >3300 ||length < 2200 l:%.1f,w:%.1f",length,width);
            std::cout<<description<<std::endl;*/
            return false;
        }

        double d = distance(pc, points[3]);
        cv::Point2f center1 = (ps + pt) * 0.5;
        cv::Point2f center2 = (pc + points[3]) * 0.5;
        if (fabs(d - max_l) > max_l * 0.1 || distance(center1, center2) > 0.150) {
            /*std::cout << "d:" << d << " maxl:" << max_l << "  center1:" << center1 << "  center2:" << center2<<std::endl;
            char description[255]={0};
            sprintf(description,"Verify failed-4  fabs(d - max_l) > max_l * 0.1 || distance(center1, center2) > 0.150 ");
            std::cout<<description<<std::endl;*/
            return false;
        }
        //std::cout << "d:" << d << " maxl:" << max_l << "  center1:" << center1 << "  center2:" << center2<<std::endl;


        return true;
    }
    else if(points.size()==3)
    {
        double L[3] = {0.0};
        L[0] = distance(points[0], points[1]);
        L[1] = distance(points[1], points[2]);
        L[2] = distance(points[0], points[2]);
        double max_l = L[0];
        double l1 = L[1];
        double l2 = L[2];
        int max_index = 0;
        cv::Point2f ps = points[0], pt = points[1];
        cv::Point2f pc = points[2];
        for (int i = 1; i < 3; ++i) {
            if (L[i] > max_l) {
                max_index = i;
                max_l = L[i];
                l1 = L[abs(i + 1) % 3];
                l2 = L[abs(i + 2) % 3];
                ps = points[i % 3];
                pt = points[(i + 1) % 3];
                pc = points[(i + 2) % 3];
            }
        }

        //直角边与坐标轴的夹角 <20°
        float thresh=20.0*M_PI/180.0;
        cv::Point2f vct(pt.x-pc.x,pt.y-pc.y);
        float angle=atan2(vct.y,vct.x);
        if(!(fabs(angle)<thresh || (M_PI_2-fabs(angle)<thresh)))
        {
            //std::cout<<" 4 wheel axis angle : "<<angle<<std::endl;
            return false;
        }

        double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
        if (fabs(cosa) >= 0.15) {
            /*char description[255]={0};
            sprintf(description,"3 wheels angle cos value(%.2f) >0.13 ",cosa);
            std::cout<<description<<std::endl;*/
            return false;
        }

        double l=std::max(l1,l2);
        double w=std::min(l1,l2);
        if(l>2.100 && l<3.300 && w>1.400 && w<2.100)
        {
            //生成第四个点
            cv::Point2f vec1=ps-pc;
            cv::Point2f vec2=pt-pc;
            cv::Point2f point4=(vec1+vec2)+pc;
            points.push_back(point4);

            /*char description[255]={0};
            sprintf(description,"3 wheels rectangle cos angle=%.2f,L=%.1f, w=%.1f",cosa,l,w);
            std::cout<<description<<std::endl;*/
            return true;
        }
        else
        {

            /*char description[255]={0};
            sprintf(description,"3 wheels rectangle verify Failed  cos angle=%.2f,L=%.1f, w=%.1f",cosa,l,w);
            std::cout<<description<<std::endl;*/
            return false;
        }

    }
    //std::cout<<" default false"<<std::endl;
    return false;

}
bool Ground_region::classify_ceres_detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,double thresh_z,
        detect_wheel_ceres3d::Detect_result& result)
{
    if(m_detector== nullptr)
        return false;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    for(int i=0;i<cloud->size();++i)
    {
        pcl::PointXYZ pt=cloud->points[i];
        if(pt.z< thresh_z)
        {
            cloud_filtered->push_back(pt);
        }
    }
    //下采样
    pcl::VoxelGrid<pcl::PointXYZ> vox;  //创建滤波对象
    vox.setInputCloud(cloud_filtered);                 //设置需要过滤的点云给滤波对象
    vox.setLeafSize(0.02f, 0.02f, 0.2f);     //设置滤波时创建的体素体积为1cm的立方体
    vox.filter(*cloud_filtered);         //执行滤波处理，存储输出
    if(cloud_filtered->size()==0)
    {
        return false;
    }

    if(cloud_filtered->size()==0)
        return false;
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;   //创建滤波器对象
    sor.setInputCloud (cloud_filtered);                           //设置待滤波的点云
    sor.setMeanK (5);                               //设置在进行统计时考虑的临近点个数
    sor.setStddevMulThresh (3.0);                      //设置判断是否为离群点的阀值，用来倍乘标准差，也就是上面的std_mul
    sor.filter (*cloud_filtered);                    //滤波结果存储到cloud_filtered

    if(cloud_filtered->size()==0)
    {
        return false;
    }

    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> seg_clouds;
    seg_clouds=segmentation(cloud_filtered);

    if(!(seg_clouds.size()==4 || seg_clouds.size()==3))
    {
        return false;
    }
    std::vector<cv::Point2f> centers;
    for(int i=0;i<seg_clouds.size();++i)
    {
        Eigen::Vector4f centroid;
        pcl::compute3DCentroid(*seg_clouds[i], centroid);
        centers.push_back(cv::Point2f(centroid[0],centroid[1]));

    }
    bool ret= isRect(centers);
    if(ret)
    {

        std::string error_str;
        if(m_detector->detect(seg_clouds,result,error_str))
        {
            return true;
        }
        else
        {
            return false;
        }
    }
    return ret;
}


Ground_region::~Ground_region()
{
    m_exit_condition.set_pass_ever(true);
    // Close Capturte Thread
    if( m_running_thread && m_running_thread->joinable() ){
        m_running_thread->join();
        m_running_thread->~thread();
        delete m_running_thread;
        m_running_thread = nullptr;
    }

    for(int i=0;i<m_lidars.size();++i)
    {
        if(m_lidars[i]!= nullptr)
        {
            m_lidars[i]->close();
            delete m_lidars[i];
        }
    }
    m_lidars.clear();
    if(m_detector)
    {
        delete m_detector;
        m_detector= nullptr;
    }


}
Ground_region::Ground_region()
{
    m_detector= nullptr ;
    m_running_thread=nullptr;
}
Error_manager Ground_region::init(std::string configure_file)
{
    if(read_proto_param(configure_file,m_configure)==false)
    {
        return Error_manager(FAILED,NORMAL,"read proto failed");
    }

    pcl::PointCloud<pcl::PointXYZ>::Ptr left_model=ReadTxtCloud("/media/youchen/block_ntfs/yct/catkin_ws/src/feature_extra/left_model.txt");
    // std::cout << "left model:" << left_model->size() << std::endl;
    pcl::PointCloud<pcl::PointXYZ>::Ptr right_model = ReadTxtCloud("/media/youchen/block_ntfs/yct/catkin_ws/src/feature_extra/right_model.txt");
    // std::cout << "right model:" << right_model->size() << std::endl;
    m_detector=new detect_wheel_ceres3d(left_model,right_model);
    // std::cout << "????" << std::endl;

    return init(m_configure);

}
Error_manager Ground_region::init(ground_region::Configure configure)
{
    //绑定通讯ip
    /*char connect_str[255]={0};
    sprintf(connect_str,"tcp://%s:%d",configure.communication_cfg().ip().c_str(),
            configure.communication_cfg().port());
    Publisher::get_instance_pointer()->communication_bind(connect_str);
    Publisher::get_instance_pointer()->communication_run();

    //创建雷达
    m_lidars.resize(configure.lidar_size());
    for(int i=0;i<configure.lidar_size();++i)
    {
        const boost::asio::ip::address address = boost::asio::ip::address::from_string( configure.lidar(i).ip() );
        const unsigned short port = configure.lidar(i).port();
        m_lidars[i]=new velodyne::VLP16Capture(address,port);
        if( !m_lidars[i]->isOpen() ){
            char description[255]={0};
            sprintf(description,"lidar [%d] open failed  ip:%s ,port:%d",i,configure.lidar(i).ip().c_str(),port);
            return Error_manager(FAILED,NORMAL,description);
        }
    }
*/
    m_configure=configure;
    m_running_thread=new std::thread(std::bind(thread_measure_func,this));
    return SUCCESS;
}

bool Ground_region::read_proto_param(std::string file, ::google::protobuf::Message& parameter)
{
    int fd = open(file.c_str(), O_RDONLY);
    if (fd == -1) return false;
    FileInputStream* input = new FileInputStream(fd);
    bool success = google::protobuf::TextFormat::Parse(input, &parameter);
    delete input;
    close(fd);
    return success;
}

pcl::PointCloud<pcl::PointXYZ>::Ptr Ground_region::scans2cloud(const velodyne::Frame& frame,
        const ground_region::Calib_parameter& calib)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    for(int i=0;i<frame.scans.size();++i)
    {
        pcl::PointXYZ pt;

    }
    // 变换
    Eigen::Matrix3d rotation_matrix3 ;
    rotation_matrix3= Eigen::AngleAxisd(calib.r(), Eigen::Vector3d::UnitZ()) *
            Eigen::AngleAxisd(calib.p(), Eigen::Vector3d::UnitY()) *
            Eigen::AngleAxisd(calib.y(), Eigen::Vector3d::UnitX());

    Eigen::Affine3d transform_2 = Eigen::Affine3d::Identity();
    transform_2.translation() << calib.cx(),calib.cy(),calib.cz();
    transform_2.rotate (rotation_matrix3);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transpose(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::transformPointCloud (*cloud, *cloud_transpose, transform_2);

    return cloud_transpose;
}

Error_manager Ground_region::sync_capture(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& clouds,double timeout_second)
{
    Tick timer;
    bool sync=false;
    clouds.clear();
    std::vector<velodyne::Frame>  sync_frames;
    while((false==m_exit_condition.wait_for_ex(std::chrono::microseconds(1))) && sync==false)
    {
        bool sync_once=true;
        for(int i=0;i<m_lidars.size();++i)
        {
            velodyne::Frame frame;
            m_lidars[i]->get_frame(frame);
            if(frame.tic.tic()>0.2)
            {
                sync_frames.clear();
                sync_once=false;
                break;
            }
            else
            {
                sync_frames.push_back(frame);
            }
        }
        if(timer.tic()>timeout_second)
        {
            return Error_manager(FAILED,NORMAL,"sync capture time out");
        }
        sync=sync_once;
        usleep(1);
    }
    if(sync)
    {
        for(int i=0;i<sync_frames.size();++i)
        {
            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud=(scans2cloud(sync_frames[i],m_configure.lidar(i).calib()));
            clouds.push_back(cloud);
        }
        return SUCCESS;
    }
    return FAILED;

}

Error_manager Ground_region::prehandle_cloud(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& clouds,
                                             message::Ground_measure_statu_msg& msg)
{
    if(msg.laser_statu_vector_size()!=m_lidars.size())
        return ERROR;
    //直通滤波
    ground_region::Box box = m_configure.box();
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> filtered_cloud_vector;
    for (int i = 0; i < clouds.size(); ++i)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
        for (int j = 0; j < clouds[i]->size(); ++j)
        {
            pcl::PointXYZ pt = clouds[i]->points[j];
            if (pt.x > box.minx() && pt.x < box.maxx()
                    && pt.y > box.miny() && pt.y < box.maxy()
                    && pt.z > box.minz() && pt.z < box.maxz())
            {
                cloud_filtered->push_back(pt);
            }
        }
        filtered_cloud_vector.push_back(cloud_filtered);
    }
    //根据点云判断雷达状态，若所有雷达滤波后都有点， 则正常,lidar_statu_bit 各个位代表雷达是否有点， 有点为0，无点为1
    int lidar_statu_bit = 0;
    for (int i = 0; i < filtered_cloud_vector.size(); ++i)
    {
        if (filtered_cloud_vector[i]->size() == 0)
            lidar_statu_bit |= (0x01 << i);
        else
            msg.set_laser_statu_vector(i, message::LASER_READY);//雷达正常
    }
    if (lidar_statu_bit == 0)
    {
        clouds = filtered_cloud_vector;
        return SUCCESS;
    }

    //判断是否有雷达故障
    for (int i = 0; i < filtered_cloud_vector.size(); ++i)
    {
        if (filtered_cloud_vector[i]->size() == 0)
        {
            //滤波之前也没有点，雷达故障
            if (clouds[i]->size() == 0)
            {
                clouds = filtered_cloud_vector;
                msg.set_laser_statu_vector(i, message::LASER_DISCONNECT);
                return Error_manager(DISCONNECT, NORMAL, "雷达故障");
            }
        }
    }
    //判断是不是所有雷达在该区域都没有点，表示该区域没有东西
    int temp = 0;
    if (~(((~temp) << (filtered_cloud_vector.size())) | lidar_statu_bit) == 0)
    {
        clouds = filtered_cloud_vector;
        return Error_manager(POINT_EMPTY, NORMAL, "区域无点");
    }
    else
    {
        clouds = filtered_cloud_vector;
        return Error_manager(CLOUD_INCOMPLETED, NORMAL, "点云不完整(雷达正常)");
    }

}


bool computer_var(std::vector<double> data,double& var)
{
    if(data.size()==0)
        return false;
    Eigen::VectorXd  dis_vec(data.size());
    for(int i=0;i<data.size();++i)
    {
        dis_vec[i]=data[i];
    }
    double mean=dis_vec.mean();
    Eigen::VectorXd mean_vec(data.size());
    Eigen::VectorXd mat=dis_vec-(mean_vec.setOnes()*mean);
    Eigen::MatrixXd result=(mat.transpose())*mat;
    var=sqrt(result(0)/double(data.size()));
    return true;

}


Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
                                    const ground_region::Box& box,detect_wheel_ceres3d::Detect_result& last_result)
{
    if(cloud->size()==0)
        return Error_manager(POINT_EMPTY,NORMAL,"no point");

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    for(int i=0;i<cloud->size();++i)
    {
        pcl::PointXYZ pt=cloud->points[i];
        if(pt.x>box.minx()&&pt.x<box.maxx()
        &&pt.y>box.miny()&&pt.y<box.maxy()
        &&pt.z>box.minz()&&pt.z<box.maxz())
        {
            cloud_filtered->push_back(pt);
        }
    }
    if(cloud_filtered->size()==0)
        return Error_manager(POINT_EMPTY,NORMAL,"filtered no point");

    float start_z=box.minz();
    float max_z=0.2;
    float center_z=(start_z+max_z)/2.0;
    float last_center_z=start_z;
    float last_succ_z=-1.0;
    int count=0;
    //二分法 找识别成功的 最高的z
    std::vector<detect_wheel_ceres3d::Detect_result> results;
    do
    {
        detect_wheel_ceres3d::Detect_result result;
        bool ret = classify_ceres_detect(cloud_filtered, center_z,result);
        // std::cout << "z: " << center_z <<", "<<start_z<<"," << max_z << std::endl;
        if(ret)
        {
            results.push_back(result);
            last_succ_z=center_z;
            start_z=center_z;
            last_center_z=center_z;

        }
        else
        {
            max_z=center_z;
            last_center_z=center_z;

        }
        center_z=(start_z+max_z)/2.0;
        count++;
        
    } while (fabs(center_z - last_center_z) > 0.01);

    //
    if(results.size()==0)
    {
        return Error_manager(FAILED,NORMAL,"nor car");
    }
    ///  to be
    float min_mean_loss=1.0;
    for(int i=0;i<results.size();++i)
    {
        detect_wheel_ceres3d::Detect_result result=results[i];
        std::vector<double> loss;
        loss.push_back(result.loss.lf_loss);
        loss.push_back(result.loss.rf_loss);
        loss.push_back(result.loss.lb_loss);
        loss.push_back(result.loss.rb_loss);
        double mean=(result.loss.lf_loss+result.loss.rf_loss+result.loss.lb_loss+result.loss.rb_loss)/4.0;
        double var=-1.;
        computer_var(loss,var);
        if(mean<min_mean_loss)
        {
            last_result=result;
            min_mean_loss=mean;
        }

    }
    printf("z : %.3f  angle : %.3f front : %.3f wheel_base:%.3f,width:%.3f, mean:%.5f\n",
           center_z,last_result.theta,last_result.front_theta,last_result.wheel_base,last_result.width,
           min_mean_loss);
    //m_detector->save_debug_data("/home/zx/zzw/catkin_ws/src/feature_extra/debug");
    return SUCCESS;
}

#include "message/message_base.pb.h"
void Ground_region::thread_measure_func(Ground_region* p)
{
    return ;
    message::Ground_measure_statu_msg msg;
    message::Base_info base_info;
    base_info.set_msg_type(message::eGround_measure_statu_msg);
    base_info.set_sender(message::eEmpty);
    base_info.set_receiver(message::eEmpty);
    msg.mutable_base_info()->CopyFrom(base_info);
    msg.set_ground_statu(message::Nothing);
    const float fps=10.;
    //保持 10 fps
    while(false==p->m_exit_condition.wait_for_millisecond(int(1000./fps)))
    {
        for(int i=0;i<p->m_lidars.size();++i)
        {
            msg.add_laser_statu_vector(
                    p->m_lidars[i]->isRun()?message::LASER_READY:message::LASER_DISCONNECT);
        }

        std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds;
        Error_manager code=p->sync_capture(clouds,0.5);
        if(code!=SUCCESS)
        {
            LOG(WARNING)<<code.get_error_description();
            continue;
        }

        Tick tick;
        code=p->prehandle_cloud(clouds,msg);
        if(code!=SUCCESS)
        {
            LOG(WARNING)<<code.get_error_description();
            continue;
        }
        detect_wheel_ceres3d::Detect_result result;
        pcl::PointCloud<pcl::PointXYZ>::Ptr correct_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        for(int i=0;i<clouds.size();++i)
        {
            *correct_cloud+=(*clouds[i]);
        }
        code=p->detect(correct_cloud,p->m_configure.box(),result);

        if(tick.tic()>1./fps)
        {
            LOG(WARNING)<<" detect fps < capture fps";
        }
        if(code!=SUCCESS)
        {
            LOG_EVERY_N(INFO,20)<<code.get_error_description();
            continue;
        }

        //  to be 发布结果
        msg.set_ground_statu(message::Car_correct);
        message::Locate_information locate_information;
        locate_information.set_locate_x(result.cx);
        locate_information.set_locate_y(result.cy);
        locate_information.set_locate_angle(result.theta);
        locate_information.set_locate_wheel_base(result.wheel_base);
        locate_information.set_locate_length(result.wheel_base);
        locate_information.set_locate_wheel_width(result.width);
        locate_information.set_locate_width(result.body_width);
        locate_information.set_locate_front_theta(result.front_theta);
        locate_information.set_locate_correct(true);

        Communication_message c_msg;
        c_msg.reset(msg.base_info(),msg.SerializeAsString());
        Publisher::get_instance_pointer()->publish_msg(&c_msg);


    }
}