#include <iostream>
#include <opencv2/opencv.hpp>
#include <string>
#include <ctime>

using namespace std;

typedef struct armor_point {
  cv::Point point;
  double dis;
} armor_point;

double distance(cv::Point a, cv::Point b) {
  return sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y));
}

int LeastSquaresCircleFitting(
    vector<cv::Point2d> &m_Points, 
    cv::Point2d &Centroid,
    double &dRadius)  //拟合圆函数(三个参数依次为输入点集，圆心，半径)
{
  if (!m_Points.empty()) {
    int iNum = (int)m_Points.size();
    if (iNum < 3) return 1;
    double X1 = 0.0;
    double Y1 = 0.0;
    double X2 = 0.0;
    double Y2 = 0.0;
    double X3 = 0.0;
    double Y3 = 0.0;
    double X1Y1 = 0.0;
    double X1Y2 = 0.0;
    double X2Y1 = 0.0;
    vector<cv::Point2d>::iterator iter;
    vector<cv::Point2d>::iterator end = m_Points.end();
    for (iter = m_Points.begin(); iter != end; ++iter) {
      X1 = X1 + (*iter).x;
      Y1 = Y1 + (*iter).y;
      X2 = X2 + (*iter).x * (*iter).x;
      Y2 = Y2 + (*iter).y * (*iter).y;
      X3 = X3 + (*iter).x * (*iter).x * (*iter).x;
      Y3 = Y3 + (*iter).y * (*iter).y * (*iter).y;
      X1Y1 = X1Y1 + (*iter).x * (*iter).y;
      X1Y2 = X1Y2 + (*iter).x * (*iter).y * (*iter).y;
      X2Y1 = X2Y1 + (*iter).x * (*iter).x * (*iter).y;
    }
    double C = 0.0;
    double D = 0.0;
    double E = 0.0;
    double G = 0.0;
    double H = 0.0;
    double a = 0.0;
    double b = 0.0;
    double c = 0.0;
    C = iNum * X2 - X1 * X1;
    D = iNum * X1Y1 - X1 * Y1;
    E = iNum * X3 + iNum * X1Y2 - (X2 + Y2) * X1;
    G = iNum * Y2 - Y1 * Y1;
    H = iNum * X2Y1 + iNum * Y3 - (X2 + Y2) * Y1;
    a = (H * D - E * G) / (C * G - D * D);
    b = (H * C - E * D) / (D * D - G * C);
    c = -(a * X1 + b * Y1 + X2 + Y2) / iNum;
    double A = 0.0;
    double B = 0.0;
    double R = 0.0;
    A = a / (-2);
    B = b / (-2);
    R = double(sqrt(a * a + b * b - 4 * c) / 2);
    Centroid.x = A;
    Centroid.y = B;
    dRadius = R;
    return 0;
  } else
    return 1;
  return 0;
}

int main() {
//   string video_path = "/home/gy/视频/buffv.mp4";
  cv::VideoCapture cap;
  cap.open("/home/gy/视频/kazam_wrdl4w6n.avi");
  clock_t start, finish;

  // cv::VideoWriter writer;
  // int coder = cv::VideoWriter::fourcc('M','J','P','G');//选择编码格式
  // double fps=50.0;//设置视频帧率
  // string filename = "/home/zjk/Videos/buff_demo.avi"; //保存的视频文件名称
  // writer.open(filename,coder,fps,cv::Size(1350,1080),true);//创建保存视频文件的视频流
  // if(!writer.isOpened()){
  //     cout<<"打开视频文件失败，请确认是否为合法输入"<<endl;
  //     return -1;
  // }

  while (true) {
    cv::Mat src_frame;
    cap >> src_frame;
    start = clock();
    // cv::resize(src_frame, src_frame, cv::Size(480, 360));
    cv::resize(src_frame, src_frame, cv::Size(560, 420));

    cv::Mat input;
    vector<cv::Mat> bgr_images;
    cv::split(src_frame, bgr_images);
    cv::Mat b_src_img = bgr_images[0] - bgr_images[2];
    cv::Mat g_src_img = bgr_images[0] - bgr_images[1];
    cv::Mat gray ;
    cv::cvtColor(src_frame, gray, cv::COLOR_BGR2GRAY);
    // bitwise_and(b_src_img.clone(),g_src_img.clone(),input);

    cv::threshold(b_src_img, b_src_img, 130, 255, cv::THRESH_BINARY);
    cv::threshold(g_src_img, g_src_img, 120, 255, cv::THRESH_BINARY_INV);
    cv::threshold(gray, gray, 100, 255, cv::THRESH_BINARY);
    // cv::imshow("gray", gray);

    cv::Mat element1 =
        cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5));
        //设置内核1
    cv::Mat element2 = cv::getStructuringElement(cv::MORPH_RECT,
                                                 cv::Size(3, 3));
       //设置内核2

    cv::Mat threshold_img;
    cv::bitwise_and(b_src_img, g_src_img, threshold_img);
    cv::bitwise_and(threshold_img, gray, threshold_img);

    // cv::blur(threshold_img, threshold_img, cv::Size(3,3));
    // cv::threshold(b_src_img, threshold_img, 140, 255, cv::THRESH_BINARY);
     cv::dilate(threshold_img, threshold_img, element1);
     cv::erode(threshold_img, threshold_img, element2);
    //  cv::dilate(threshold_img, threshold_img, element1);

    // cv::imshow("b-r", b_src_img);
    // cv::imshow("b-g", g_src_img);   
    cv::imshow("final_th", threshold_img);  

    // cv::blur(b_src_img, b_src_img, cv::Size(3, 3));

    vector<vector<cv::Point>> contours;
    cv::findContours(threshold_img, contours, cv::RETR_EXTERNAL,
                     cv::CHAIN_APPROX_SIMPLE);
    // cv::drawContours(src_frame,contours,-1,cv::Scalar(0,0,255));

    cv::Point2d r_center;                          // R 的中心点
    vector<cv::RotatedRect> contours_min_rects;  //所有轮廓的最小外接矩形
    vector<cv::RotatedRect> armor_min_rects;
    vector<cv::RotatedRect> target_min_rects;
    vector<cv::RotatedRect> r_min_rects;  // R

    double r_index = 0.0;
    int cnt = 0;
    for (unsigned int contour_index = 0; contour_index < contours.size();
         contour_index++) {
      //寻找最小旋转矩形,放进容器，顺便将面积过大的剔除,长宽比悬殊的剔除
      cv::RotatedRect minrect =
          minAreaRect(contours[contour_index]);  //最小旋转矩形
          
      cv::Rect rect = boundingRect(contours[contour_index]);  //获取矩形信息
      if ((minrect.center.x < (src_frame.cols)/2 + 10) && ((minrect.center.x > (src_frame.cols)/2 - 10)) && (minrect.center.y < (src_frame.rows)/2 + 30) && (minrect.center.y > (src_frame.rows)/2 - 30) ) {
          r_center = minrect.center;//center_radian
          cv::circle(src_frame, r_center, 15, cv::Scalar(5, 255, 100));
      }

    //   if (minrect.size.area() <= 50000 && minrect.size.area() > 500) {
         if (minrect.size.area() <= 6000.0 && minrect.size.area() > 190) {
        float width;
        float height;
        //判断长宽比
        if (minrect.size.width > minrect.size.height) {
          width = minrect.size.width;
          height = minrect.size.height;
        } else {
          width = minrect.size.height;
          height = minrect.size.width;
        }
        if (width / height < 4) {
          contours_min_rects.push_back(minrect);
          if (minrect.size.area() > 150 && minrect.size.area() < 350 &&
              minrect.center.y > 200) {  // find R
            if (height / width > 0.1) {
            //   cv::R_minRects.push_back(minrect);
              r_center = minrect.center;
              cv::circle(src_frame, r_center, 15, cv::Scalar(5, 255, 100));
              r_index = contour_index;
              // std::cout<<cnt++<<std::endl;
            }
          } else {
            if (minrect.size.area() > 300 && minrect.size.area() < 4350 &&
                (height / width) < 0.7) {
              armor_min_rects.push_back(minrect);
            }
          }
        }
      }
    }

    bool find_ok = false;
    for (int i = 0; i < armor_min_rects.size() - 1; i++) {
      for (int j = i + 1; j < armor_min_rects.size(); j++) {
        double dis =
            distance(armor_min_rects[i].center, armor_min_rects[j].center);
        if (dis < 120) {
          target_min_rects.push_back(armor_min_rects[i]);
          target_min_rects.push_back(armor_min_rects[j]);
          find_ok = true;
          break;
        }
        // std::cout<<dis<<std::endl;
      }
      if (find_ok) {
        break;
      }
    }
    if (target_min_rects.size() != 2) {
      continue;
    } else {
      cv::RotatedRect rrect_in;   //= target_minRects[0];
      cv::RotatedRect rrect_out;  // = target_minRects[1];
      vector<cv::Point2d> points;
      double dis1 = distance(r_center, target_min_rects[0].center);
      double dis2 = distance(r_center, target_min_rects[1].center);
      if (dis1 > dis2) {
        rrect_in = target_min_rects[1];
        rrect_out = target_min_rects[0];
      } else {
        rrect_in = target_min_rects[0];
        rrect_out = target_min_rects[1];
      }
      
      // drawRotatedRect(src_frame, rrect_in, cv::Scalar(0, 250, 0), 1);
      // drawRotatedRect(src_frame, rrect_out, cv::Scalar(0, 0, 255), 1);

      cv::Point target_center =
          cv::Point((int)((rrect_in.center.x + rrect_out.center.x) / 2),
                    (int)((rrect_in.center.y + rrect_out.center.y) / 2));
      cv::circle(src_frame, target_center, 5, cv::Scalar(0, 0, 255), -1);
      cv::Point2f in_vet_points[4];
      cv::Point2f out_vet_points[4];
      rrect_in.points(in_vet_points);
      rrect_out.points(out_vet_points);
      vector<armor_point> armor_points;
      for (int i = 0; i < 4; i++) {
        armor_point point;
        point.point = in_vet_points[i];
        point.dis = distance(target_center, in_vet_points[i]);
        armor_points.push_back(point);
      }
      for (int i = 0; i < 4; i++) {
        armor_point point;
        point.point = out_vet_points[i];
        point.dis = distance(target_center, out_vet_points[i]);
        armor_points.push_back(point);
      }
      sort(armor_points.begin(), armor_points.end(),
           [](armor_point a, armor_point b) { return a.dis < b.dis; });
      for (int i = 0; i < 4; i++) {
        cv::circle(src_frame, armor_points[i].point, 3, cv::Scalar(255, 0,255),-1);
      }
      int buff_run_radius = (int)distance(r_center, target_center);
      points.push_back(target_center);
      LeastSquaresCircleFitting(points,r_center,r_index);
      cv::circle(src_frame, r_center,120, cv::Scalar(55, 110,255),1);   //拟合圆
      
    }

    finish = clock();
            double abc_Frame =
            1000/(double(finish-start)/CLOCKS_PER_SEC*1000);
            std::string str = std::to_string(abc_Frame);
            string Frame_name = "FPS:";
            Frame_name +=str;
            // cout<<"视觉识别帧率为："<<Frame_name<<endl;
            putText(src_frame,Frame_name,cv::Point(0,50),cv::FONT_HERSHEY_COMPLEX,1,cv::Scalar(0,0,255));
    // // cv::resize(src_frame,src_frame,cv::Size(1350,1080));
    // // writer.write(src_frame);//把图像写入视频流
    cv::imshow("src_img", src_frame);
    // cv::imshow("threshold_img",threshold_img);
     cv::waitKey(50);
    // if (cv::waitKey(0) == 'q') {
    //   break;
    // }
    // cv::imshow("g_src",g_src_img);
    // cv::imshow("b_src",b_src_img);

  }

  // writer.release();
  cap.release();
  return 0;
}