//
// Created by huli on 2021/3/24.
//

#ifndef NNXX_TESTS_POINT3D_TOOL_H
#define NNXX_TESTS_POINT3D_TOOL_H

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <vector>
#include <opencv4/opencv2/opencv.hpp>
#include "defines.hpp"

class Point3D_tool
{
public:
	//坐标
	struct Point3D
	{
		float x=0;				//x轴
		float y=0;				//y轴
		float z=0;				//z轴
	};

	//平面坐标的限定范围
	struct Point3D_box
	{
		float x_min=0;				//x轴最小值
		float x_max=0;				//x轴最大值
		float y_min=0;				//y轴最小值
		float y_max=0;				//y轴最大值
		float z_min=0;				//z轴最小值
		float z_max=0;				//z轴最大值
	};

	template <typename PclPoint>
	static int getMinRect(cv::RotatedRect &rect, pcl::shared_ptr<pcl::PointCloud<PclPoint>> &clouds) {
		if (clouds->points.size() < 4) {
			DLOG(ERROR) << "points to little.";
			return false;
		}
		std::vector<cv::Point2f> cv_clouds;
		for (auto &point: clouds->points) {
			cv_clouds.emplace_back(point.x, point.y);
		}
		rect = cv::minAreaRect(cv_clouds);
		return true;
	}

	// template <typename PclPoint>
	// static int getMinRect(cv::RotatedRect &rect, pcl::shared_ptr<pcl::PointCloud<PclPoint>> &points) {
	static int getRectCloud(cv::RotatedRect &rect, pcl::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> &points, float z_value = 0, int numbers = 100) {
		cv::Point2f cv_points[4];
		rect.points(cv_points);
		// PclPoint point;
		pcl::PointXYZ point;
		// 左侧点
		double step_x = (cv_points[1].x - cv_points[0].x) / numbers;
		double step_y = (cv_points[1].y - cv_points[0].y) / numbers;
		for (int i = 0; i < numbers; i++) {
			point.x = cv_points[0].x + i * step_x;
			point.y = cv_points[0].y + i * step_y;
			point.z = z_value;
			points->points.emplace_back(point);
		}

		// 上侧点
		step_x = (cv_points[2].x - cv_points[1].x) / numbers;
		step_y = (cv_points[2].y - cv_points[1].y) / numbers;
		for (int i = 0; i < numbers; i++) {
			point.x = cv_points[1].x + i * step_x;
			point.y = cv_points[1].y + i * step_y;
			point.z = z_value;
			points->points.emplace_back(point);
		}

		// 右侧点
		step_x = (cv_points[3].x - cv_points[2].x) / numbers;
		step_y = (cv_points[3].y - cv_points[2].y) / numbers;
		for (int i = 0; i < numbers; i++) {
			point.x = cv_points[2].x + i * step_x;
			point.y = cv_points[2].y + i * step_y;
			point.z = z_value;
			points->points.emplace_back(point);
		}


		// 下侧点
		step_x = (cv_points[0].x - cv_points[3].x) / numbers;
		step_y = (cv_points[0].y - cv_points[3].y) / numbers;
		for (int i = 0; i < numbers; i++) {
			point.x = cv_points[3].x + i * step_x;
			point.y = cv_points[3].y + i * step_y;
			point.z = z_value;
			points->points.emplace_back(point);
		}

		return points->size();
	}

	template <typename PclPoint>
	static int getRectCloud(pcl::shared_ptr<pcl::PointCloud<PclPoint>> &clouds, pcl::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> &points, float z_value = 0, int numbers = 100) {
		cv::RotatedRect rect = getMinRect(clouds);
        getRectCloud(rect, points, z_value, numbers);
		return points->size();
	}

	static void getRectWidthlength(cv::RotatedRect &rect, double &width, double &length) {
		cv::Point2f points[4];
		rect.points(points);

		double a = sqrt((points[1].x - points[0].x) * (points[1].x - points[0].x) + (points[1].y - points[0].y) * (points[1].y - points[0].y));
		double b = sqrt((points[2].x - points[1].x) * (points[2].x - points[1].x) + (points[2].y - points[1].y) * (points[2].y - points[1].y));

		width = MIN(a, b);
		length = MAX(a, b);
	}
};


#endif //NNXX_TESTS_POINT3D_TOOL_H