#pragma once

#include <opencv2/opencv.hpp>
#include <opencv2/calib3d.hpp>

#include <cstdlib>
#include <vector>
#include "sort_test/detection.h"
#include <unistd.h>


using namespace cv;

class Ranging
{
private:
    // rknn_handle hdx;
    cv::VideoCapture vcapture;
    Detection yolov5s;
/*  2-stereo
    Mat cam_matrix_left = (Mat_<double>(3, 3) << 
     4.791153893499601e+02, 0, 0,
     0,4.798785696902847e+02, 0,
     3.195422157433843e+02, 2.355081129251360e+02, 1);
    Mat cam_matrix_right = (Mat_<double>(3, 3) << 
     4.851946576927952e+02, 0, 0,
        0,4.848412081674973e+02, 0,
       3.139237321746737e+02, 2.316610217516937e+02, 1);
    Mat distortion_l = (Mat_<double>(1, 5) <<0.091215339806518,-0.088601421082219, 0,
        0, 0);

    Mat distortion_r = (Mat_<double>(1, 5) <<0.086266675232625,-0.070869167707634, 0,
        0, 0);
    Mat rotate = (Mat_<double>(3, 3) << 
    0.999999240684794, -5.243648990073547e-05, -0.001231210888060,
      5.272610271745578e-05, 0.999999970951700, -0.002653487630467,
       0.001231198519510, -2.352594617066257e-04,0.999999214401287);
    Mat trans = (Mat_<double>(3, 1) << 
      -61.636981845981540, -1.107000282904877, -1.084419989523733);
*/
/*
    Mat cam_matrix_left = (Mat_<double>(3, 3) << 
    4.869084743604942e+02, 0, 0,
    0,4.859921620798602e+02, 0,
    2.813183643903652e+02, 2.267657091677993e+02, 1);
    Mat cam_matrix_right = (Mat_<double>(3, 3) << 
    4.859133331805883e+02, 0, 0,
    0,4.850356748771951e+02, 0,
    2.970046483040089e+02, 2.324763397214774e+02, 1);
    Mat distortion_l = (Mat_<double>(1, 5) <<0.121235284781974,-0.161097849662596, 0,
        0, 0);

    Mat distortion_r = (Mat_<double>(1, 5) <<0.105479235005840,-0.120347246815955, 0,
        0, 0);
    Mat rotate = (Mat_<double>(3, 3) << 
    0.999921984818601, 3.753847738839353e-04, -0.012485325894835,
    -4.085449515452996e-04, 0.999996396040715, -0.002653487630467,
    0.012484284819374, 0.002658381435011,0.999918534502034);
    Mat trans = (Mat_<double>(3, 1) << 
       -60.319997608188590, -0.019664800882533, -0.638993708428792);
*/
// 3_stereo:
    Mat cam_matrix_left = (Mat_<double>(3, 3) << 
        725.8008655, 0, 606.83544339,
        0, 725.71074058, 355.60009442,
        0, 0, 1);
    Mat cam_matrix_right = (Mat_<double>(3, 3) << 
        720.47514477, 0, 611.91679085,
        0, 720.93965216, 328.55158146,
        0, 0, 1);
    Mat distortion_l = (Mat_<double>(1, 5) << 
        0.0794014, -0.07158048, -0.00233537, -0.00538731, -0.07631366);

    Mat distortion_r = (Mat_<double>(1, 5) << 
        0.10602397, -0.12121708, -0.0039814, -0.00316623, -0.06940564);
    Mat rotate = (Mat_<double>(3, 3) << 
        9.99952249e-01, -3.99415019e-04, -9.76426160e-03,
        4.05975519e-04, 9.99999693e-01, 6.69916194e-04,
        9.76399103e-03, -6.73848256e-04, 9.99952104e-01);
    Mat trans = (Mat_<double>(3, 1) << 
        -45.23755445, -0.59995078, -3.05666663);
    cv::Mat mapX1, mapX2, mapY1, mapY2, q, Z;

    int imgw, imgh;
    detect_result_group_t detect_result_group;
    cv::Mat info;
    cv::Mat empty;

  

public:
    Ranging(){};
    Ranging(int imgw, int imgh);
    void rectifyImage(cv::Mat &oriImgL, cv::Mat &oriImgR);
    void getInfo(cv::Mat &imgl, cv::Mat &imgr, cv::Mat &detBoxes, cv::Mat &info);
    std::vector<float> pic2cam(int u, int v);
    std::vector<int> muban(cv::Mat &left_image, cv::Mat &right_image, const int *coordinates);
    std::vector<cv::Mat> get_range(int frame_num, int detect_interval);
    void horizon_estimate(cv::Mat& img, cv::Mat& bboxs,int k);
    
};