//
// Created by zx on 2021/9/17.
//

#ifndef FIND_CHASSIS__CHASSIS_CERES_SOLVER_H_
#define FIND_CHASSIS__CHASSIS_CERES_SOLVER_H_
#include <ceres/ceres.h>
#include <glog/logging.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/centroid.h>
#include <opencv2/opencv.hpp>
#include <iostream>
#include <string>
#include <chrono>

#include "../error_code/error_code.h"

constexpr float kMinProbability = 0.01f;
constexpr float kMaxProbability = 1.f - kMinProbability;
constexpr float kMaxCorrespondenceCost = 1.f - kMinProbability;
constexpr int kPadding = INT_MAX / 4;
constexpr float resolutionx = 0.01;
constexpr float resolutiony = 0.005;
constexpr float inner_width = 1.2;
constexpr float chassis_height = 0.14;
constexpr float startx = -2.0;
constexpr float starty = -0.1;
constexpr float endx = 2.0;
constexpr float endy = 0.3;

class chassis_ceres_solver
{
 public:
    chassis_ceres_solver();
    ~chassis_ceres_solver();

    Error_manager solve(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,double& x,double& z,double& w,double& h);

    // 利用图像优化，计算速度提升
    Error_manager solve_mat(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,double& x,double& z,double& w,double& h, bool update_debug_img=false);

    // 创建优化地图
    static cv::Mat create_mat();
    // 返回更新投影地图
    cv::Mat get_projected_mat() { return m_projected_mat; }
    // 返回投影地图转换的点云, 便于投影到pcl视野中
    pcl::PointCloud<pcl::PointXYZ>::Ptr get_projected_cloud()
    {
       pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
       if (m_projected_mat.empty())
          return t_cloud;
       for (size_t i = 0; i < m_projected_mat.rows; i++)
       {
          for (size_t j = 0; j < m_projected_mat.cols; j++)
          {
             double x0 = (i * resolutionx + startx);
             t_cloud->push_back(pcl::PointXYZ());
          }
       }
       return t_cloud;
    }

 private:
    static cv::Mat m_model;
    cv::Mat m_projected_mat;
};

#endif //FIND_CHASSIS__CHASSIS_CERES_SOLVER_H_