//
// Created by zx on 2023/7/20.
//

#include "wheel_model_3d.h"

Error_manager WheelModel3D::init() {
    return (loadLeftModel() == Error_code::SUCCESS && loadRightModel() == Error_code::SUCCESS) ? Error_code::SUCCESS : Error_code::FAILED;
}

Error_manager WheelModel3D::loadLeftModel() {
    pcl::PointCloud<pcl::PointXYZ>::Ptr left_clouds(new pcl::PointCloud<pcl::PointXYZ>);
    if (ReadTxtCloud(left_file, left_clouds)) {
        mp_left_grid = updateModel(left_clouds);
        if (mp_left_grid == nullptr) {
            LOG(INFO) << "------------>WO KAO， FAILED TO LOAD LEFT MODEL";
            exit(0);
        }
        return Error_code::SUCCESS;
    }
    return Error_code::FAILED;
}

Error_manager WheelModel3D::loadRightModel() {
    pcl::PointCloud<pcl::PointXYZ>::Ptr right_clouds(new pcl::PointCloud<pcl::PointXYZ>);
    if (ReadTxtCloud(right_file, right_clouds)) {
        mp_right_grid = updateModel(right_clouds);
        if (mp_right_grid == nullptr) {
            LOG(INFO) << "------------>WO KAO， FAILED TO LOAD RIGHT MODEL";
            exit(0);
        }
        return Error_code::SUCCESS;
    }

    return Error_code::FAILED;
}

HybridGrid* WheelModel3D::updateModel(pcl::PointCloud<pcl::PointXYZ>::Ptr &clouds, float ratio)
{
    float del_x=0.05;
    float del_y=0.03;
    float del_z=0.05;
    Eigen::Matrix3f delta=Eigen::MatrixXf::Identity(3,3);
    delta(0,0)=del_x*del_x;
    delta(1,1)=del_y*del_y;
    delta(2,2)=del_z*del_z;
    Eigen::Matrix3f delta_inv=delta.inverse();

    Eigen::Vector3f d(0,0,0.02);
    Eigen::MatrixXf ce=-0.5*((d).transpose()*delta_inv*(d));
    float cut_p=exp(ce(0,0));

    float resolution=0.01;

    HybridGrid *grid = new HybridGrid(resolution);

    for (auto &point: clouds->points) {
        point.x *= ratio;
        point.y *= ratio;
        point.z *= ratio;
    }

    for(int i = 0; i<clouds->size(); ++i)
    {

        pcl::PointXYZ pt = clouds->points[i];
        Eigen::Vector3f u(pt.x,pt.y,pt.z);

        for(float x=pt.x-0.1;x<pt.x+0.1;x+=grid->resolution())
        {
            for(float y=pt.y-0.1;y<pt.y+0.1;y+=grid->resolution())
            {
                for(float z=pt.z-0.1;z<pt.z+0.1;z+=grid->resolution())
                {
                    Eigen::Vector3f p(x,y,z);
                    Eigen::MatrixXf B=-0.5*((p-u).transpose()*delta_inv*(p-u));
                    float prob=exp(B(0,0));
                    if(prob>cut_p)
                        prob=cut_p;
                    Eigen::Array3i index=grid->GetCellIndex(p);
                    if(grid->GetProbability(index)<prob){
                        grid->SetProbability(index,prob);
                    }
                }
            }
        }
    }

    return grid;
}