//检测呼啦圈
#include<opencv2\opencv.hpp>
#include <iostream>
#include<fstream>
#include <opencv2/highgui/highgui_c.h>
#include <opencv2/imgproc/types_c.h>

using namespace cv;
using namespace std;

double draw_point_start, draw_point_end, draw_point_interval;
double point_speed, point_speed_old, speed_average;
Point draw_point_old = Point(0, 0);

int main()
{
	Mat frame, frame_background;
	VideoCapture capture;
	capture.open("H://VS/Opencv/program/robocon_2021/video/hulala_1.mp4"); //待检测视频的文件地址
	capture >> frame;
	if (frame.empty())
	{
		cout << "Could not load video";
	}
	Mat frame_draw(540, 960, CV_8UC3, Scalar::all(0));
	Mat frame_draw_point(540, 960, CV_8UC3, Scalar::all(0));
	Mat frame_draw_speed(540, 960, CV_8UC3, Scalar::all(0));
	Mat frame_save(540, 960, CV_8UC3, Scalar::all(0));;

	//设置保存视频
	VideoWriter writer;
	int coder = VideoWriter::fourcc('D', 'I', 'V', 'X');//选择编码格式
	double fps = 30.0;//设置视频帧率
	string filename = "hulala_out.mp4";//保存的视频文件名称
	writer.open(filename, coder, fps, frame_save.size(), 1);//创建保存视频文件的视频流

	while (capture.read(frame))
	{
        double start = getTickCount();
		if (frame.empty())
		{
			break;
			cout << "Could not load video";
		}

		resize(frame, frame, Size(960, 540));

		//取识别敏感区域
		Mat frame_are = frame(cv::Rect(0, 270, 960, 270));
		Mat frame_are_src = frame_are;

		vector<Mat> channels;
		split(frame_are, channels);
		threshold(channels.at(1) - channels.at(2), frame_are, 15, 255, CV_THRESH_BINARY);

		Mat element1 = getStructuringElement(MORPH_RECT, Size(10, 10));//设置内核1
		Mat element2 = getStructuringElement(MORPH_RECT, Size(2, 2));//设置内核2
		//morphologyEx(frame_are, frame_are, MORPH_OPEN, element1);//开运算
		//floodFill(frame_are, Point(30, 30), Scalar(10));//漫水
		morphologyEx(frame_are, frame_are, MORPH_CLOSE, element2);//闭运算
		imshow("frame_are", frame_are);

		vector<vector<Point>> contours;
		vector<Vec4i> hierarchy;
		findContours(frame_are, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE);//找轮廓

		int area[25] = { 0 };
		uint8_t draw_point_flag = 0;

		for (int i = 0; i < hierarchy.size(); i++)
		{
			area[i] = contourArea(contours[i]);//计算轮廓面积
			if (area[i] > 10000)
			{
				Point2f rect[4];
				RotatedRect box1 = minAreaRect(Mat(contours[i]));//获取最小外接矩阵

				if (draw_point_flag == 0)
				{
					circle(frame_draw, Point(box1.center.x, box1.center.y + 270), 1, Scalar(255, 255, 255), -1, 8);  //绘制最小外接矩形的中心点
					circle(frame_draw_point, Point(box1.center.x, box1.center.y + 270), 1, Scalar(0, 0, 255), -1, 8);  //绘制最小外接矩形的中心点

					if (draw_point_old.x != 0)
					{
						//绘制行走轨迹
						line(frame_draw, Point(box1.center.x, box1.center.y + 270), draw_point_old, Scalar(255, 255, 255), 1, 8);
						line(frame_draw_point, Point(box1.center.x, box1.center.y + 270), draw_point_old, Scalar(0, 255, 0), 1, 8);

						//测速度
						draw_point_end = getTickCount();
						point_speed = sqrtf(powf((box1.center.x - draw_point_old.x), 2) + powf((box1.center.y + 270 - draw_point_old.y), 2)) / ((draw_point_end - draw_point_start) / getTickFrequency());
						draw_point_start = getTickCount();
						speed_average = (point_speed + speed_average) / 2;

						//绘制速度曲线
						circle(frame_draw_speed, Point(box1.center.x, 540 - point_speed), 1, Scalar(0, 0, 255), -1, 8);  //绘制瞬时速度点
						circle(frame_draw_speed, Point(box1.center.x, 540 - speed_average), 1, Scalar(255, 0, 255), -1, 8);  //绘制平均速度点
						line(frame_draw_speed, Point(box1.center.x, 540 - point_speed), Point(draw_point_old.x, 540 - point_speed_old), Scalar(255, 255, 255), 1, 8);
					}
					draw_point_old.x = box1.center.x;
					draw_point_old.y = box1.center.y + 270;
					point_speed_old = point_speed;
					draw_point_flag = 1;
				}
				circle(frame, Point(box1.center.x, box1.center.y+270), box1.size.height / 2, Scalar(255, 0, 0), 1, 8);  //绘制外圆
			}
		}

		double end = getTickCount();
		double run_time = (end - start) / getTickFrequency();
		double fps = 1 / run_time;

		putText(frame, format("FPS: %0.2f", fps), Point(20, 20), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 0, 255), 1, 8);
		putText(frame, format("speed_average: %0.2f", speed_average), Point(20, 60), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 0, 255), 1, 8);

		imshow("test", frame_draw_speed);
		imshow("show", frame - frame_draw + frame_draw_point);
		frame_save = frame - frame_draw + frame_draw_point;
		writer.write(frame_save);//把图像写入视频流
		//imshow("draw", frame_draw);
		char c = waitKey(5);
		if (c == 27) //"Esc"
		{
		    break;
		}
	}
	waitKey(0);
	writer.release();
	capture.release();
	return 0;
}