#include "dot_coh.h"
#include <cmath>
#include <cstring>
#include <vector>
#include "rdp_record.h"

using namespace std;

int Dot_Coh::dot_coh_process(std::vector <PointRecv>                *data_input,
							 std::vector <PointRecv>                 *point_recv,
							const RDP_parameter& rdp_parameter)
{

	if(data_input->size()>1)
	{

		for (unsigned int loop_of_point=0; loop_of_point<data_input->size()-1;loop_of_point++ )
		{
			if( (*data_input)[loop_of_point].use_flag!=1)
			{
				float point_0_R=static_cast<float>((*data_input)[loop_of_point].range);
				float point_0_V=static_cast<float>((*data_input)[loop_of_point].velocity);
				float point_0_F=static_cast<float>((*data_input)[loop_of_point].azimuth);
				float point_0_A=static_cast<float>((*data_input)[loop_of_point].amplitude);
				for (unsigned int i=loop_of_point+1;i<data_input->size();i++)
				{
					if((*data_input)[loop_of_point].use_flag!=1 && (*data_input)[i].use_flag!=1)
					{
						float point_1_R=static_cast<float>((*data_input)[i].range);
						float point_1_V=static_cast<float>((*data_input)[i].velocity);
						float point_1_F=static_cast<float>((*data_input)[i].azimuth);
						float point_1_A=static_cast<float>((*data_input)[i].amplitude);

						//凝聚条件： 距离、方位接近
						if(rdp_parameter.radar_work_mode == 0)
						{
							float delta_F = fabs(point_0_F-point_1_F)<2*PI-fabs(point_0_F-point_1_F) ? fabs(point_0_F-point_1_F) :2*PI-fabs(point_0_F-point_1_F);
							if( (fabs(point_0_R-point_1_R)<=rdp_parameter.point_cohesion_range && delta_F <=rdp_parameter.point_cohesion_azimuth/180.0*PI && fabs(point_0_V-point_1_V)<=rdp_parameter.point_cohesion_velocity)//
								&&  point_0_A <point_1_A  )//
							{
								point_0_R=static_cast<float>((*data_input)[i].range);
								point_0_V=static_cast<float>((*data_input)[i].velocity);
								point_0_A=static_cast<float>((*data_input)[i].amplitude);
								point_0_F=static_cast<float>((*data_input)[i].azimuth);
								(*data_input)[loop_of_point].use_flag=1;
							}
							else if( (fabs(point_0_R-point_1_R)<=rdp_parameter.point_cohesion_range && delta_F <=rdp_parameter.point_cohesion_azimuth/180.0*PI && fabs(point_0_V-point_1_V)<=rdp_parameter.point_cohesion_velocity)//
								&&  point_0_A >= point_1_A)//
							{
								(*data_input)[i].use_flag=1;
							}
						}
						else
						{
							float delta_F = fabs(point_0_F-point_1_F)<2*PI-fabs(point_0_F-point_1_F) ? fabs(point_0_F-point_1_F) :2*PI-fabs(point_0_F-point_1_F);
							if( (fabs(point_0_R-point_1_R)<=rdp_parameter.point_cohesion_range && delta_F <=rdp_parameter.point_cohesion_azimuth/180.0*PI )//
								&&  point_0_A <point_1_A  )//
							{
								point_0_R=static_cast<float>((*data_input)[i].range);
								point_0_V=static_cast<float>((*data_input)[i].velocity);
								point_0_A=static_cast<float>((*data_input)[i].amplitude);
								point_0_F=static_cast<float>((*data_input)[i].azimuth);
								(*data_input)[loop_of_point].use_flag=1;
							}
							else if( (fabs(point_0_R-point_1_R)<=rdp_parameter.point_cohesion_range && delta_F <=rdp_parameter.point_cohesion_azimuth/180.0*PI )//
								&&  point_0_A >= point_1_A)//
							{
								(*data_input)[i].use_flag=1;
							}

						}
					}
				}
			}
		}
	}

	//删除凝聚点 和 量程范围外点
	std::vector <PointRecv>::iterator Iter;
	for (Iter=data_input->begin(); Iter!=data_input->end();)
	{
		if((*Iter).use_flag==1 || (*Iter).range <rdp_parameter.point_cohesion_range_min  ||  (*Iter).range > rdp_parameter.point_cohesion_range_max   )
		{
			data_input->erase(Iter);
			Iter=data_input->begin();
		}
		else
		{
			Iter++;
		}
	}

	for (size_t i=0;i<data_input->size();i++)      //data_buffer_2 ---> point_recv
	{
			(*point_recv).push_back((*data_input)[i]);
	}

	return 0;
}

int Dot_Coh::dot_coh_process_buff( std::vector <PointRecv>       *data_input,            //输入的点迹
						  std::vector <PointRecv>                *point_recv,             //输出点迹
						  const RDP_parameter& rdp_parameter                             //工作参数
)
{

	//1. data_input与data_input_buff进行凝聚

	//1.1 data_input、data_input_buff中的数据放在一起
	std::vector <PointRecv> data_tmp;
	for (size_t i=0;i<data_input_buff.size();i++)
	{
			data_tmp.push_back(data_input_buff[i]);
			data_tmp[data_tmp.size()-1].point_section_asso = 1;

	}

	for (size_t i=0;i<data_input->size();i++)
	{
		    data_tmp.push_back((*data_input)[i]);
			data_tmp[data_tmp.size()-1].point_section_asso = 2;
	}

	//1.2 把data_input、data_input_buff清空

	std::vector<PointRecv>().swap(data_input_buff);
	std::vector<PointRecv>().swap(*data_input);

	//1.3 对data_tmp进行凝聚
	if(data_tmp.size()>1)
	{

		for (unsigned int loop_of_point=0; loop_of_point<data_tmp.size()-1;loop_of_point++ )
		{
			if( data_tmp[loop_of_point].use_flag!=1)
			{
				float point_0_R=static_cast<float>(data_tmp[loop_of_point].range);
				float point_0_V=static_cast<float>(data_tmp[loop_of_point].velocity);
				float point_0_F=static_cast<float>(data_tmp[loop_of_point].azimuth);
				float point_0_A=static_cast<float>(data_tmp[loop_of_point].amplitude);
				for (unsigned int i=loop_of_point+1;i<data_tmp.size();i++)
				{
					if(data_tmp[loop_of_point].use_flag!=1 && data_tmp[i].use_flag!=1)
					{
						float point_1_R=static_cast<float>(data_tmp[i].range);
						float point_1_V=static_cast<float>(data_tmp[i].velocity);
						float point_1_F=static_cast<float>(data_tmp[i].azimuth);
						float point_1_A=static_cast<float>(data_tmp[i].amplitude);

						//凝聚条件： 距离、方位接近
						if(rdp_parameter.radar_work_mode == 0)
						{
							float delta_F = fabs(point_0_F-point_1_F)<2*PI-fabs(point_0_F-point_1_F) ? fabs(point_0_F-point_1_F) :2*PI-fabs(point_0_F-point_1_F);
							if( (fabs(point_0_R-point_1_R)<=rdp_parameter.point_cohesion_range && delta_F <=rdp_parameter.point_cohesion_azimuth/180.0*PI && fabs(point_0_V-point_1_V)<=rdp_parameter.point_cohesion_velocity)//
								&&  point_0_A <point_1_A  )//
							{
								point_0_R=static_cast<float>(data_tmp[i].range);
								point_0_V=static_cast<float>(data_tmp[i].velocity);
								point_0_A=static_cast<float>(data_tmp[i].amplitude);
								point_0_F=static_cast<float>(data_tmp[i].azimuth);
								data_tmp[loop_of_point].use_flag=1;
							}
							else if( (fabs(point_0_R-point_1_R)<=rdp_parameter.point_cohesion_range && delta_F <=rdp_parameter.point_cohesion_azimuth/180.0*PI && fabs(point_0_V-point_1_V)<=rdp_parameter.point_cohesion_velocity)//
								&&  point_0_A >= point_1_A)//
							{
								data_tmp[i].use_flag=1;
							}
						}
						else
						{
							float delta_F = fabs(point_0_F-point_1_F)<2*PI-fabs(point_0_F-point_1_F) ? fabs(point_0_F-point_1_F) :2*PI-fabs(point_0_F-point_1_F);
							if( (fabs(point_0_R-point_1_R)<=rdp_parameter.point_cohesion_range && delta_F <=rdp_parameter.point_cohesion_azimuth/180.0*PI )//
								&&  point_0_A <point_1_A  )//
							{
								point_0_R=static_cast<float>(data_tmp[i].range);
								point_0_V=static_cast<float>(data_tmp[i].velocity);
								point_0_A=static_cast<float>(data_tmp[i].amplitude);
								point_0_F=static_cast<float>(data_tmp[i].azimuth);
								data_tmp[loop_of_point].use_flag=1;
							}
							else if( (fabs(point_0_R-point_1_R)<=rdp_parameter.point_cohesion_range && delta_F <=rdp_parameter.point_cohesion_azimuth/180.0*PI )//
								&&  point_0_A >= point_1_A)//
							{
								data_tmp[i].use_flag=1;
							}

						}
					}
				}
			}
		}
	}

	//1.4 删除凝聚点 和 量程范围外点
	std::vector <PointRecv>::iterator Iter;
	for (Iter=data_tmp.begin(); Iter!=data_tmp.end();)
	{
		if((*Iter).use_flag==1 || (*Iter).range <rdp_parameter.point_cohesion_range_min  ||  (*Iter).range > rdp_parameter.point_cohesion_range_max   )
		{
			data_tmp.erase(Iter);
			Iter=data_tmp.begin();
		}
		else
		{
			Iter++;
		}
	}

	//1.5 将data_tmp中凝聚后的点再分到 data_input_buff和data_input中
	for (size_t i=0;i<data_tmp.size();i++)
	{
		if(data_tmp[i].point_section_asso==1)
		{
			data_input_buff.push_back(data_tmp[i]);
		}

		if(data_tmp[i].point_section_asso==2)
		{
		  (*data_input).push_back(data_tmp[i]);
		}
	}
	std::vector<PointRecv>().swap(data_tmp);

	//2.data_input_buff数据输出给point_recv

#ifdef RDP_RECORD_ENABLE
	//记录凝聚后点迹：先收集，输出循环结束后一次入队
	std::vector<PointRecv> rec_coh;
	rec_coh.reserve(data_input_buff.size());
#endif
	for (size_t i=0;i<data_input_buff.size();i++)
	{
		(*point_recv).push_back(data_input_buff[i]);
#ifdef RDP_RECORD_ENABLE
		rec_coh.push_back(data_input_buff[i]);
#endif
	}
#ifdef RDP_RECORD_ENABLE
	RDP_RECORD_COHESION_TWS(rec_coh);
#endif

	std::vector<PointRecv>().swap(data_input_buff);

	//3.data_input数据输出给data_input_buff
	for (size_t i=0;i<data_input->size();i++)
	{
			data_input_buff.push_back((*data_input)[i]);
	}

	return 0;
}

Dot_Coh::Dot_Coh(void)
{

}

void Dot_Coh::reset()
{
	data_input_buff.clear();
}