//
// Created by zx on 2020/7/1.
//

#ifndef CERES_TEST_DETECT_WHEEL_CERES_H
#define CERES_TEST_DETECT_WHEEL_CERES_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 "boost/format.hpp"

class detect_wheel_ceres {
public:
    struct Detect_result
    {
        public: 
        double cx;
        double cy;
        double theta;
        double wheel_base;
        double width;
        double front_theta; 
        bool success; 
        Detect_result& operator=(const Detect_result detect_result)
        {
            this->cx = detect_result.cx;
            this->cy = detect_result.cy;
            this->theta = detect_result.theta;
            this->wheel_base = detect_result.wheel_base;
            this->width = detect_result.width;
            this->front_theta = detect_result.front_theta;
            this->success = detect_result.success;
            return *this;
        }
    };
    struct Loss_result
    {
        public: 
        double lf_loss;
        double rf_loss;
        double lb_loss;
        double rb_loss;
        double total_avg_loss;
        Loss_result& operator=(const Loss_result& loss_result)
        {
            this->lf_loss = loss_result.lf_loss;
            this->rf_loss = loss_result.rf_loss;
            this->lb_loss = loss_result.lb_loss;
            this->rb_loss = loss_result.rb_loss;
            this->total_avg_loss = loss_result.total_avg_loss;
            return *this;
        }
    };
    detect_wheel_ceres();
    ~detect_wheel_ceres();
    bool detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> cloud_vec, Detect_result &detect_result);

protected:
    void transform_src(Detect_result detect_result);
    bool update_mat(int rows, int cols);
    bool Solve(Detect_result &detect_result, double &avg_loss);
    bool Solve(Detect_result &detect_result, Loss_result &loss_result, bool save_img=false);
    void debug_img(Detect_result detect_result, Loss_result loss_result, bool save_img=false, std::string out_img_path="");

    void create_mark(double x,double y,double theta,pcl::PointCloud<pcl::PointXYZ>& cloud);
public:
    pcl::PointCloud<pcl::PointXYZ>          m_left_front_cloud_out;     //左前点云
    pcl::PointCloud<pcl::PointXYZ>          m_right_front_cloud_out;    //右前点云
    pcl::PointCloud<pcl::PointXYZ>          m_left_rear_cloud_out;      //左后点云
    pcl::PointCloud<pcl::PointXYZ>          m_right_rear_cloud_out;     //右后点云

protected:
    pcl::PointCloud<pcl::PointXYZ>          m_left_front_cloud;     //左前点云
    pcl::PointCloud<pcl::PointXYZ>          m_right_front_cloud;    //右前点云
    pcl::PointCloud<pcl::PointXYZ>          m_left_rear_cloud;      //左后点云
    pcl::PointCloud<pcl::PointXYZ>          m_right_rear_cloud;     //右后点云
    cv::Mat                                 m_map;                  //保存一份地图用作匹配


};


#endif //CERES_TEST_DETECT_WHEEL_CERES_H