//
// Created by huli on 2020/9/1.
//

#ifndef POINT2D_TOOL_H
#define POINT2D_TOOL_H



//#define OPEN_PCL_FLAG
#ifdef OPEN_PCL_FLAG
#include <pcl/point_types.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/conversions.h>
#include <pcl/common/common.h>
#endif

#include <stddef.h>
#include <math.h>

class Point2D_tool
{
public:
	//极坐标
	struct Polar_coordinates
	{
		float angle=0;				//弧度
		float distance=0;				//距离
	};

	//极坐标的限定范围
	struct Polar_coordinates_box
	{
		float angle_min=0;			//角度最小值
		float angle_max=0;			//角度最大值
		float distance_min=0;			//距离最小值
		float distance_max=0;			//距离最大值
	};

	//平面坐标
	struct Point2D
	{
		float x=0;				//x轴
		float y=0;				//y轴
	};

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

	//平面坐标的转换矩阵, 可以进行旋转和平移, 不能缩放
	struct Point2D_transform
	{
		float m00=1;
		float m01=0;
		float m02=0;
		float m10=0;
		float m11=1;
		float m12=0;
	};

	//极坐标 -> 平面坐标
	static bool Polar_coordinates_to_point2D(Point2D_tool::Polar_coordinates* p_polar_coordinates, Point2D_tool::Point2D* p_point2D);
	//平面坐标 -> 极坐标
	static bool Point2D_to_polar_coordinates(Point2D_tool::Point2D* p_point2D, Point2D_tool::Polar_coordinates* p_polar_coordinates);


	//判断极坐标点是否在限制范围
	static bool limit_with_polar_coordinates_box(float distance, float angle, Point2D_tool::Polar_coordinates_box* p_polar_coordinates_box);
	//判断平面坐标点是否在限制范围
	static bool limit_with_point2D_box(float x, float y, Point2D_tool::Point2D_box* p_point2D_box);

#ifdef OPEN_PCL_FLAG
	//平面坐标的转换, 可以进行旋转和平移, 不能缩放
	static bool transform_with_translation_rotation(float* p_x_in, float* p_y_in, float* p_x_out, float* p_y_out,
													Point2D_tool::Point2D_transform* p_point2D_transform);
	static bool transform_with_translation_rotation(pcl::PointXYZ* p_point3D_in, pcl::PointXYZ* p_point3D_out,
													Point2D_tool::Point2D_transform* p_point2D_transform);
	static bool transform_with_translation_rotation(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in,
													pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_out,
													Point2D_tool::Point2D_transform* p_point2D_transform);

#endif
};

//极坐标 -> 平面坐标
bool Point2D_tool::Polar_coordinates_to_point2D(Point2D_tool::Polar_coordinates* p_polar_coordinates, Point2D_tool::Point2D* p_point2D)
{
    if ( p_polar_coordinates == NULL || p_point2D == NULL)
    {
        return false;
    }
    else
    {
        p_point2D->x = p_polar_coordinates->distance * cos(p_polar_coordinates->angle);
        p_point2D->y = p_polar_coordinates->distance * sin(p_polar_coordinates->angle);
        return true;
    }
    return true;
}
//平面坐标 -> 极坐标
bool Point2D_tool::Point2D_to_polar_coordinates(Point2D_tool::Point2D* p_point2D, Point2D_tool::Polar_coordinates* p_polar_coordinates)
{
    if ( p_polar_coordinates == NULL || p_point2D == NULL)
    {
        return false;
    }
    else
    {
        p_polar_coordinates->distance = sqrt(p_point2D->x * p_point2D->x + p_point2D->y * p_point2D->y);
        p_polar_coordinates->angle = atan(p_point2D->y / p_point2D->x);
        return true;
    }
    return true;
}

//判断极坐标点是否在限制范围
bool Point2D_tool::limit_with_polar_coordinates_box(float distance, float angle, Point2D_tool::Polar_coordinates_box* p_polar_coordinates_box)
{
    if ( p_polar_coordinates_box == NULL )
    {
        return false;
    }
    else
    {
        if ( angle >= p_polar_coordinates_box->angle_min &&
             angle <= p_polar_coordinates_box->angle_max &&
             distance >= p_polar_coordinates_box->distance_min &&
             distance <= p_polar_coordinates_box->distance_max )
        {
            return true;
        }
        else
        {
            return false;
        }
    }
    return true;
}

//判断平面坐标点是否在限制范围
bool Point2D_tool::limit_with_point2D_box(float x, float y, Point2D_tool::Point2D_box* p_point2D_box)
{
    if ( p_point2D_box == NULL )
    {
        return false;
    }
    else
    {
        if ( x >= p_point2D_box->x_min &&
             x <= p_point2D_box->x_max &&
             y >= p_point2D_box->y_min &&
             y <= p_point2D_box->y_max )
        {
            return true;
        }
        else
        {
            return false;
        }
    }
    return true;
}

#ifdef OPEN_PCL_FLAG
//平面坐标的转换, 可以进行旋转和平移, 不能缩放
bool Point2D_tool::transform_with_translation_rotation(float* p_x_in, float* p_y_in, float* p_x_out, float* p_y_out,
										 Point2D_tool::Point2D_transform* p_point2D_transform)
{
	if ( p_x_in == NULL || p_x_in == NULL || p_x_out == NULL || p_y_out == NULL || p_point2D_transform == NULL)
	{
		return false;
	}
	else
	{
		*p_x_out = *p_x_in * p_point2D_transform->m00 + *p_y_in * p_point2D_transform->m01 + p_point2D_transform->m02;
		*p_y_out = *p_x_in * p_point2D_transform->m10 + *p_y_in * p_point2D_transform->m11 + p_point2D_transform->m12;
		return true;
	}
	return true;
}
bool Point2D_tool::transform_with_translation_rotation(pcl::PointXYZ* p_point3D_in, pcl::PointXYZ* p_point3D_out,
												Point2D_tool::Point2D_transform* p_point2D_transform)
{
	if (p_point3D_in == NULL || p_point3D_out == NULL || p_point2D_transform == NULL)
	{
		return false;
	}
	else
	{
		p_point3D_out->x = p_point3D_in->x * p_point2D_transform->m00 + p_point3D_in->y * p_point2D_transform->m01 +
						   p_point2D_transform->m02;
		p_point3D_out->y = p_point3D_in->x * p_point2D_transform->m10 + p_point3D_in->y * p_point2D_transform->m11 +
						   p_point2D_transform->m12;
		return true;
	}
	return true;
}
bool Point2D_tool::transform_with_translation_rotation(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in,
												pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_out,
												Point2D_tool::Point2D_transform* p_point2D_transform)
{
	if ( p_cloud_in.get() == NULL || p_cloud_out.get() == NULL || p_point2D_transform == NULL)
	{
	    return false;
	}
	else
	{
		pcl::PointXYZ point;
		for (int i = 0; i < p_cloud_in->size(); ++i)
		{
			point.x = p_cloud_in->points[i].x * p_point2D_transform->m00 + p_cloud_in->points[i].y * p_point2D_transform->m01 + p_point2D_transform->m02;
			point.y = p_cloud_in->points[i].x * p_point2D_transform->m10 + p_cloud_in->points[i].y * p_point2D_transform->m11 + p_point2D_transform->m12;
			p_cloud_out->push_back(point);
		}
		return true;
	}
	return true;
}
#endif

#endif //POINT2D_TOOL_H