#include "track_init.h"
#include "kalman.h"
#include "coor_trans.h"
#include <cmath>
#include <cstring>
#include <vector>
#include <iostream>
#include<iomanip>
#include "rdp_record.h"
using namespace std;

int Track_Init::track_init_process_logic(   std::vector <PointRecv>               *point_recv,                            //输入点迹
											std::vector<Trust_Track>              *trust_track,                           //可靠航迹
											std::vector <std::vector<Temp_track>>     *temp_track,
											struct  Track                     Trust_Track_Output[MAX_TRACK_NUM][10],
											int                               *Trust_track_num_Output
										)
{

	//取出待起航的点迹数据
	for (size_t i=0;i<(*point_recv).size();i++){
		point_process.push_back((*point_recv)[i]);
	}

	//临时航迹的buff_round+1
	for (size_t i=0;i<temp_track->size();i++)
	{
		for (size_t j=0;j<(*temp_track)[i].size();j++)
		{
			(*temp_track)[i][j].buff_round = (*temp_track)[i][j].buff_round+1;
		}
	}

	if(point_process.size()>0)
	{
		//点迹与临时航迹关联
		point_temp_track_asso(temp_track);

		//点迹与航迹头关联
		point_track_head_asso(temp_track);

		//删除关联上的点迹
		std::vector <PointRecv>::iterator Iter;
		for (Iter=point_process.begin(); Iter!=point_process.end();)
		{
			if((*Iter).use_flag==1)
			{
				point_process.erase(Iter);
				Iter=point_process.begin();
			}
			else
			{
				Iter++;
			}
		}

		//剩余点重新存入点迹
		std::vector<PointRecv>().swap((*point_recv));
		for (size_t i=0;i<point_process.size();i++)
			(*point_recv).push_back(point_process[i]);

		//剩余点转为航迹头
		for (unsigned int i=0;i<point_process.size();i++)
		{
			Temp_track temp_track_tmp = {};
			temp_track_tmp.temp_track_id = temp_track_id++;
			temp_track_tmp.point_id = point_process[i].point_id;
			temp_track_tmp.circle_id = point_process[i].circle_id;
			temp_track_tmp.r = point_process[i].range;
			temp_track_tmp.azi = point_process[i].azimuth;
			temp_track_tmp.height = point_process[i].height;
			temp_track_tmp.amplitude = point_process[i].amplitude;
			temp_track_tmp.vr =  point_process[i].velocity;
			temp_track_tmp.SNR = point_process[i].SNR;
			temp_track_tmp.RCS = point_process[i].RCS;
			temp_track_tmp.pitch_num = point_process[i].pitch_beam_index;
			std::memcpy(temp_track_tmp.speed_dim, point_process[i].speed_dim, sizeof(temp_track_tmp.speed_dim));
			std::memcpy(temp_track_tmp.range_dim, point_process[i].range_dim, sizeof(temp_track_tmp.range_dim));
			temp_track_tmp.asso_flag = 0;
			temp_track_tmp.X[0]=point_process[i].range*cos(point_process[i].azimuth);
			temp_track_tmp.X[1]=0;
			temp_track_tmp.X[2]=point_process[i].range*sin(point_process[i].azimuth);
			temp_track_tmp.X[3]=0;
			temp_track_tmp.CPI_time = point_process[i].CPI_time;
			temp_track_tmp.GNSS_time = point_process[i].GNSS_time;
			temp_track_tmp.buff_round = 1;
//            temp_track_tmp.Temp_track_section_idx = floor(point_process[i].beam_index/BEAM_NUM_DOT_SECTION)+1;
			temp_track->push_back( std::vector <Temp_track> ());
			int n=static_cast<int>(temp_track->size());
			(*temp_track)[n-1].push_back(temp_track_tmp);

//            if(point_process[i].CPI_Time == 30675948 || point_process[i].CPI_Time  == 30682320)
//            {
//                std::cout << "point_process_azi=" << point_process[i].Azimuth
//                         << "point_process_dis=" << point_process[i].Range
//                         << "point_process_h=" << point_process[i].Height
//                         << "point_process_v=" << point_process[i].Velocity
//                         << "point_process_wave=" << point_process[i].beam_index
//                         << "point_process_time=" << point_process[i].CPI_Time;
//            }

		}

		//清空点迹
		std::vector<PointRecv>().swap(point_process);
		std::vector<PointRecv>().swap((*point_recv));

#ifdef RDP_RECORD_ENABLE
		//记录临时航迹
		std::vector<Temp_track> rec_temp;
		for(auto& vec_t : *temp_track){
			for(auto& t : vec_t){
				rec_temp.push_back(t);
			}
		}
		RDP_RECORD_TEMP_TRACKS(rec_temp);
#endif	

		//临时航迹满足起始长度 转为可靠航迹
		tmp_track_to_trust_track(trust_track,temp_track,Trust_Track_Output, Trust_track_num_Output);

	}

	//消亡临时航迹
	tmp_track_die(temp_track);

	return 0;

}

void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>>     *temp_track)
{

	//关联信息
	struct Asso_info
	{
		int track_idx;
		int point_idx;
		double d;
	};
	std::vector <Asso_info> asso_info;

	for ( size_t i=0;i<point_process.size();i++)
	{
		for ( size_t j=0;j<(*temp_track).size();j++)
		{
			int L = static_cast<int>((*temp_track)[j].size());

			if( (*temp_track)[j].size() > 1 && (*temp_track)[j][L-1].buff_round >= 2)
			{

				//点迹信息
//                double Z[2]={ point_process[i].Range*cos(point_process[i].Azimuth),point_process[i].Range*sin(point_process[i].Azimuth)} ;
				double Z[3]={point_process[i].range,
							point_process[i].azimuth,
							point_process[i].velocity};
				double vr_point=point_process[i].velocity;
				double r_point=point_process[i].range;
				double prt = point_process[i].PRF_index;
				double freq_ind = point_process[i].freq_index;
				double h_point = point_process[i].height;
				double T_point = static_cast<double>(point_process[i].CPI_time);

				//航迹信息
				double X[4];
				double P[4][4];
				memcpy(X,(*temp_track)[j][L-1].X,4*sizeof(double));
				memcpy(P,(*temp_track)[j][L-1].P,4*4*sizeof(double));
				double v_temp_track=(*temp_track)[j][L-1].vr;
				double r_temp_track=(*temp_track)[j][L-1].r;
				double h_temp_track=(*temp_track)[j][L-1].height;
				double T_track_head = static_cast<double>((*temp_track)[j][L-1].CPI_time);

				double delta_T = (T_point - T_track_head)/1000.0;

				if( fabs(r_point-r_temp_track)>=5 )
				{
					//计算d
					kalman Kalman;
					double d = Kalman.d_cal_track_init_EKF(Z,X,P,delta_T,prt,freq_ind);
					//double d = Kalman.d_cal_track_init_with_doppler(Z,X,P,delta_T,vr_point,prt,freq_ind);
					//double d = Kalman.d_cal_track_init(Z,X,P,delta_T);
					//计算夹角
					double x0=(*temp_track)[j][L-2].X[0];
					double y0=(*temp_track)[j][L-2].X[2];
					double x1=(*temp_track)[j][L-1].X[0];
					double y1=(*temp_track)[j][L-1].X[2];
					double x2=Z[0]*cos(Z[1]);
					double y2=Z[0]*sin(Z[1]);
					double alpha = alpha_cal_track_init(x0,y0,x1,y1,x2,y2);

					if(d*d<=rdp_parameter.track_init_threshold*rdp_parameter.track_init_threshold && alpha<rdp_parameter.track_init_angle && vr_point*v_temp_track>0)//  &&  fabs(h_point-h_temp_track )<= r_point*rdp_parameter.sigma_pitch&& abs(vr_point-v_temp_track)/abs(v_temp_track)<0.8
					{
						struct Asso_info asso_info_tmp;
						asso_info_tmp.point_idx = static_cast<int>(i)+1;
						asso_info_tmp.track_idx = static_cast<int>(j)+1;
						asso_info_tmp.d = d;
						asso_info.push_back(asso_info_tmp);
						(*temp_track)[j][L-1].asso_flag = 1;
						point_process[i].use_flag = 1;
						//std::cout << "v_temp_track : " << v_temp_track << "vr_point : "<< vr_point;
					}
				}
			}
		}
	}

	// temp_track中加入新关联上的临时航迹
	for (size_t i = 0 ; i<asso_info.size();i++ )
	{
		(*temp_track).push_back(std::vector <Temp_track> ());

		//前L个点
		int L = static_cast<int>((*temp_track)[asso_info[i].track_idx-1].size());
		for (int j=0;j < L;j++ )
		{
			(*temp_track)[(*temp_track).size()-1].push_back((*temp_track)[asso_info[i].track_idx-1][j]);
			(*temp_track)[(*temp_track).size()-1][j].asso_flag = 0;
			(*temp_track)[(*temp_track).size()-1][j].temp_track_id = temp_track_id;
		}

		//关联上的点
		Temp_track asso_track_info_tmp = {};
		asso_track_info_tmp.temp_track_id = temp_track_id++;
		asso_track_info_tmp.point_id = point_process[asso_info[i].point_idx-1].point_id;
		asso_track_info_tmp.circle_id = point_process[asso_info[i].point_idx-1].circle_id;
		asso_track_info_tmp.r = point_process[asso_info[i].point_idx-1].range;
		asso_track_info_tmp.azi = point_process[asso_info[i].point_idx-1].azimuth;
		asso_track_info_tmp.height = point_process[asso_info[i].point_idx-1].height;
		asso_track_info_tmp.amplitude = point_process[asso_info[i].point_idx-1].amplitude;
		asso_track_info_tmp.CPI_time = point_process[asso_info[i].point_idx-1].CPI_time;
		asso_track_info_tmp.GNSS_time = point_process[asso_info[i].point_idx-1].GNSS_time;
		asso_track_info_tmp.vr = point_process[asso_info[i].point_idx-1].velocity;
		asso_track_info_tmp.SNR = point_process[asso_info[i].point_idx-1].SNR;
		asso_track_info_tmp.RCS = point_process[asso_info[i].point_idx-1].RCS;
		asso_track_info_tmp.asso_flag=0;
		asso_track_info_tmp.buff_round = 1;
		asso_track_info_tmp.pitch_num = point_process[asso_info[i].point_idx-1].pitch_beam_index;
		std::memcpy(asso_track_info_tmp.speed_dim, point_process[asso_info[i].point_idx-1].speed_dim, sizeof(asso_track_info_tmp.speed_dim));
		std::memcpy(asso_track_info_tmp.range_dim, point_process[asso_info[i].point_idx-1].range_dim, sizeof(asso_track_info_tmp.range_dim));
		asso_track_info_tmp.d = asso_info[i].d;
//        asso_track_info_tmp.Temp_track_section_idx = floor(point_process[asso_info[i].point_idx-1].beam_index/BEAM_NUM_DOT_SECTION)+1;
		double Z0[2]={(*temp_track)[asso_info[i].track_idx-1][L-1].r*cos((*temp_track)[asso_info[i].track_idx-1][L-1].azi),
					 (*temp_track)[asso_info[i].track_idx-1][L-1].r*sin((*temp_track)[asso_info[i].track_idx-1][L-1].azi)};
		double Z1[2]={point_process[asso_info[i].point_idx-1].range*cos(point_process[asso_info[i].point_idx-1].azimuth),
					 point_process[asso_info[i].point_idx-1].range*sin(point_process[asso_info[i].point_idx-1].azimuth)};
		double X[4];
		double P[4][4];
		kalman Kalman;
		double T = ( point_process[asso_info[i].point_idx-1].CPI_time - (*temp_track)[asso_info[i].track_idx-1][L-1].CPI_time)/1000.0;

		Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P);
		memcpy(asso_track_info_tmp.X, X, 4*sizeof(double));
		memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(double));
		(*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp);

		//计算航迹质量
		auto& temp_track_ref = (*temp_track)[(*temp_track).size()-1];
		L = static_cast<int>(temp_track_ref.size());
		
		double snr_tmp = 0;
		double d_tmp = 0;
		double quality_tmp = 0;

		d_tmp = exp(-temp_track_ref[L-1].d);

		if(L == 3){
			double average_snr = 0;
			for(int i=0;i<L;i++)
			{
				average_snr += temp_track_ref[i].SNR;
			}
			average_snr /= L;
			snr_tmp = 1/(1+exp(-(average_snr - 16) / 1.2));
		}else{
			snr_tmp = 1/(1+exp(-(temp_track_ref.back().SNR - 16) / 1.2));
		}

		quality_tmp = d_tmp * rdp_parameter.track_quality_d_weight + snr_tmp * rdp_parameter.track_quality_snr_weight;
		quality_tmp /= (rdp_parameter.track_quality_d_weight + rdp_parameter.track_quality_snr_weight);

		if(L == 3){
			for(int i=0;i<L;i++)
			{
				temp_track_ref[i].quality = quality_tmp;
			}
		}else{
			temp_track_ref[L-1].quality = temp_track_ref[L-2].quality * rdp_parameter.track_quality_iter_coeff + quality_tmp * (1 - rdp_parameter.track_quality_iter_coeff);
		}
	}

	//temp_track中删除关联上的临时航迹
	std::vector <std::vector<Temp_track>>::iterator Iter;
	for (Iter=temp_track->begin(); Iter!=temp_track->end();)
	{
		if((*Iter).size()>=2)
		{
			if((*Iter)[(*Iter).size()-1].asso_flag==1)
			{
				temp_track->erase(Iter);
				Iter=temp_track->begin();
			}
			else
			{
				Iter++;
			}
		}
		else
		{
			Iter++;
		}
	}
}

void Track_Init::point_track_head_asso( std::vector <std::vector<Temp_track>>     *temp_track)
{

	//关联上的信息
	struct Asso_info
	{
		int track_idx;
		int point_idx;
	};
	std::vector <Asso_info> asso_info;

	//关联
	for ( size_t i=0;i<point_process.size();i++)
	{

		for ( size_t j=0;j<temp_track->size();j++)
		{

			if((*temp_track)[j].size()==1 && (*temp_track)[j][0].buff_round >= 2)
			{

				//点迹信息
				double x_point, y_point, vr_point;
				coor_trans Coor_trans;
				Coor_trans.polar2cart(&x_point,&y_point,point_process[i].range,point_process[i].azimuth);
				vr_point=point_process[i].velocity;
				double r_point=point_process[i].range;
				double h_point = point_process[i].height;
				double T_point = static_cast<double>(point_process[i].CPI_time);

				//航迹信息
				double x_track_head, y_track_head;
				x_track_head=(*temp_track)[j][0].X[0];
				y_track_head=(*temp_track)[j][0].X[2];
				double v_track_head=(*temp_track)[j][0].vr;
				double h_track_head = (*temp_track)[j][0].height;
				double T_track_head = static_cast<double>((*temp_track)[j][0].CPI_time);

				//距离差
				double dis = sqrt(pow(x_point-x_track_head,2)+pow(y_point-y_track_head,2));

				double vmax;
				if(rdp_parameter.radar_work_mode == 0)  //近程模式 最大速度减小一点
				{
					vmax = rdp_parameter.track_init_velocity_max;
				}
				else                               //中远程模式 最大速度正常用
				{
					vmax = rdp_parameter.track_init_velocity_max;
				}

				double delta_T = (T_point - T_track_head)/1000.0;


				// if(
				// 	(      (rdp_parameter.radar_work_mode == 0&&( (r_point>=1000 && dis<=vmax*delta_T && dis>=rdp_parameter.track_init_velocity_min*delta_T) || (r_point<1000 && dis<=vmax*delta_T/2.0 && dis>=rdp_parameter.track_init_velocity_min*delta_T/2.0)     ))
				// 	||  (rdp_parameter.radar_work_mode != 0&&( (r_point>=2000 && dis<=vmax*delta_T && dis>=rdp_parameter.track_init_velocity_min*delta_T) || (r_point<2000 && dis<=vmax*delta_T/3.0 && dis>=rdp_parameter.track_init_velocity_min*delta_T/2.0)     ))
				// 	)
				// 	&& vr_point*v_track_head>0  )

				//点航关联条件
				if(	dis <= vmax*  delta_T && dis >= rdp_parameter.track_init_velocity_min * delta_T && \
					vr_point*v_track_head>0 && \
					dis >= abs(vr_point) * delta_T && \
					dis >= abs(v_track_head) * delta_T
				)
				{
					struct Asso_info asso_info_tmp;
					asso_info_tmp.point_idx = static_cast<int>(i)+1;
					asso_info_tmp.track_idx = static_cast<int>(j)+1;
					asso_info.push_back(asso_info_tmp);
					(*temp_track)[j][0].asso_flag = 1;
					point_process[i].use_flag = 1;
					//std::cout << "v_track_head: " << v_track_head << "vr_point: " << vr_point;
				}

			}
		}
	}

	// temp_track中加入新关联上的临时航迹
	for (size_t i = 0 ; i<asso_info.size();i++ )
	{
		(*temp_track).push_back(std::vector <Temp_track> ());

		//第一个点
		(*temp_track)[(*temp_track).size()-1].push_back((*temp_track)[asso_info[i].track_idx-1][0]);
		(*temp_track)[(*temp_track).size()-1][0].asso_flag = 0;
		(*temp_track)[(*temp_track).size()-1][0].temp_track_id = temp_track_id;

		//第二个点
		struct Temp_track asso_track_info_tmp = {};
		asso_track_info_tmp.temp_track_id = temp_track_id++;
		asso_track_info_tmp.point_id = point_process[asso_info[i].point_idx-1].point_id;
		asso_track_info_tmp.circle_id = point_process[asso_info[i].point_idx-1].circle_id;
		asso_track_info_tmp.r = point_process[asso_info[i].point_idx-1].range;
		asso_track_info_tmp.azi = point_process[asso_info[i].point_idx-1].azimuth;
		asso_track_info_tmp.height = point_process[asso_info[i].point_idx-1].height;
		asso_track_info_tmp.amplitude = point_process[asso_info[i].point_idx-1].amplitude;
		asso_track_info_tmp.vr = point_process[asso_info[i].point_idx-1].velocity;
		asso_track_info_tmp.SNR = point_process[asso_info[i].point_idx-1].SNR;
		asso_track_info_tmp.RCS = point_process[asso_info[i].point_idx-1].RCS;
		asso_track_info_tmp.CPI_time = point_process[asso_info[i].point_idx-1].CPI_time;
		asso_track_info_tmp.GNSS_time = point_process[asso_info[i].point_idx-1].GNSS_time;
		asso_track_info_tmp.pitch_num = point_process[asso_info[i].point_idx-1].pitch_beam_index;
		std::memcpy(asso_track_info_tmp.speed_dim, point_process[asso_info[i].point_idx-1].speed_dim, sizeof(asso_track_info_tmp.speed_dim));
		std::memcpy(asso_track_info_tmp.range_dim, point_process[asso_info[i].point_idx-1].range_dim, sizeof(asso_track_info_tmp.range_dim));
		asso_track_info_tmp.asso_flag = 0;
		asso_track_info_tmp.buff_round = 1;
//        asso_track_info_tmp.Temp_track_section_idx = floor(point_process[asso_info[i].point_idx-1].beam_index/BEAM_NUM_DOT_SECTION)+1;;
		double Z0[2]={(*temp_track)[asso_info[i].track_idx-1][0].r*cos((*temp_track)[asso_info[i].track_idx-1][0].azi),
					 (*temp_track)[asso_info[i].track_idx-1][0].r*sin((*temp_track)[asso_info[i].track_idx-1][0].azi)};
		double Z1[2]={point_process[asso_info[i].point_idx-1].range*cos(point_process[asso_info[i].point_idx-1].azimuth),
					 point_process[asso_info[i].point_idx-1].range*sin(point_process[asso_info[i].point_idx-1].azimuth)};
		double X[4];
		double P[4][4];
		kalman Kalman;
		double T = (point_process[asso_info[i].point_idx-1].CPI_time - (*temp_track)[asso_info[i].track_idx-1][0].CPI_time)/1000.0;

		Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P);
		memcpy(asso_track_info_tmp.X, X, 4*sizeof(double));
		memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(double));
		(*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp);

	}

	//temp_track中删除关联上的航迹头
	std::vector <std::vector<Temp_track>>::iterator Iter;
	for (Iter=temp_track->begin(); Iter!=temp_track->end();)
	{
		if((*Iter)[0].asso_flag==1)
		{
			temp_track->erase(Iter);
			Iter=temp_track->begin();
		}
		else
		{
			Iter++;
		}

	}

}

////////////////////////////////////////////计算三点间的夹角////////////////////////////////////////////////
double Track_Init::alpha_cal_track_init(double x0,double y0,double x1,double y1,double x2,double y2)
{
		double D12[2]={x1-x0,y1-y0};
		double D23[2]={x2-x1,y2-y1};
		double alpha=acos((D12[0]*D23[0]+D12[1]*D23[1])/(sqrt(D12[0]*D12[0]+D12[1]*D12[1])*sqrt(D23[0]*D23[0]+D23[1]*D23[1])))/PI*180;

		return alpha;

}

void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track>    *trust_track,
										  std::vector <std::vector<Temp_track>>     *temp_track,
										  struct Track                      Trust_Track_Output[MAX_TRACK_NUM][10],
										  int                               *Trust_track_num_Output)
{

	std::vector <std::vector<Temp_track>> Track_to_start;

	// 1. 将temp_track中满足条件的航迹取出, 放到Track_to_start中
	std::vector <std::vector<Temp_track>>::iterator Iter;
	for (Iter=temp_track->begin(); Iter!=temp_track->end();)
	{
		int L=static_cast<int>((*Iter).size());
		double range=(*Iter)[L-1].r;
		double azi=(*Iter)[L-1].azi/PI*180;
		double quality = (*Iter)[L-1].quality;

		// //计算航迹初始质量
		// double average_snr = 0;
		// for(int i=0;i<L;i++)
		// {
		// 	average_snr += (*Iter)[i].SNR;
		// }
		// average_snr /= L;
		// double snr_tmp = 1/(1+exp(-(average_snr - 16) / 1.2));
		// double &d = (*Iter)[L-1].d;
		// double d_tmp = exp(-d);
		// double quality_tmp = d_tmp * rdp_parameter.track_quality_d_weight + snr_tmp * rdp_parameter.track_quality_snr_weight;
		// quality_tmp /= (rdp_parameter.track_quality_d_weight + rdp_parameter.track_quality_snr_weight);
		// for(int i=0;i<L;i++)
		// {
		// 	(*Iter)[i].quality = quality_tmp;
		// }

		//航迹起批条件判断
		bool track_init_flag = true;

		//航迹长度大于10点，不起批
		if(L > 10){
			track_init_flag = false;
		}

		//航迹长度小于起始点数，不起批
		if(L < rdp_parameter.track_init_point_num_min){
			track_init_flag = false;
		}
		
		//航迹起批禁止区域判断
		if(track_init_prohibit(range,azi)!=0)
		{
			track_init_flag = false;
		}

		//航迹质量起批条件判断
		switch(L){
			case 3:
				if(quality < rdp_parameter.track_init_quality_L3){
					track_init_flag = false;
				}
				break;
			case 4:
				if(quality < rdp_parameter.track_init_quality_L4){
					track_init_flag = false;
				}
				break;
			case 5:
				if(quality < rdp_parameter.track_init_quality_L5){
					track_init_flag = false;
				}
				break;
			case 6:
				if(quality < rdp_parameter.track_init_quality_L6){
					track_init_flag = false;
				}
				break;
			case 7:
				if(quality < rdp_parameter.track_init_quality_L7){
					track_init_flag = false;
				}
				break;
			default:
				if(quality < rdp_parameter.track_init_quality_L7){
					track_init_flag = false;
				}
				break;
		}

		//TODO: 杂波密度判断

		if(track_init_flag)
		{
			//加入Track_to_start中
			Track_to_start.push_back(std::vector <Temp_track> ());
			for (int i = 0; i<L; i++)
			{
			   Track_to_start[Track_to_start.size()-1].push_back((*Iter)[i]);
			}
			temp_track->erase(Iter);
			Iter=temp_track->begin();
		}
		else
		{
			Iter++;
		}
	}

	if (Track_to_start.empty())
		return;

	//2.两两比较Track_to_start中的航迹信息，删除重复的航迹
	int L = rdp_parameter.track_init_point_num_min;
	for (size_t i=0; i+1<Track_to_start.size();i++)
	{
		if(Track_to_start[i][0].asso_flag!=2)
		{
			for (size_t j=i+1;j<Track_to_start.size();j++)
			{
				if(Track_to_start[j][0].asso_flag!=2)
				{
					if (Track_to_start[i].size() < 3 || Track_to_start[j].size() < 3)
						continue;

					if( (Track_to_start[i][0].CPI_time == Track_to_start[j][0].CPI_time && Track_to_start[i][0].r == Track_to_start[j][0].r)
					   || (Track_to_start[i][1].CPI_time == Track_to_start[j][1].CPI_time && Track_to_start[i][1].r == Track_to_start[j][1].r)
					   || (Track_to_start[i][2].CPI_time == Track_to_start[j][2].CPI_time && Track_to_start[i][2].r == Track_to_start[j][2].r)
							)
					{
						if(Track_to_start[i][L-1].d<Track_to_start[j][L-1].d)
						{
							Track_to_start[j][0].asso_flag=2;
						}
						else
						{
							Track_to_start[i][0].asso_flag=2;
						}

					}

				}
			}
		}
	 }

	std::vector <std::vector<Temp_track>>::iterator Iter1;
	for (Iter1=Track_to_start.begin(); Iter1!=Track_to_start.end();)
	{
		if( (*Iter1)[0].asso_flag == 2 )
		{
		   Track_to_start.erase(Iter1);
		   Iter1=Track_to_start.begin();
		}
		else
		{
			Iter1++;
		}
	}

	//3.Track_to_start中剩余的航迹起始为可靠航迹
	for (size_t i=0; i<Track_to_start.size();i++)
	{
		if (Track_to_start[i].size() < 3) continue;
		if(trust_track->size() < static_cast<std::size_t>(rdp_parameter.track_index_max))
		{
			int index = track_index_mangement.track_ind_get(trust_track);
			if(index>0)
			{

				//加入本地航迹文件
				double Z0[2]={(Track_to_start[i][L-3].r)*cos(Track_to_start[i][L-3].azi),(Track_to_start[i][L-3].r)*sin(Track_to_start[i][L-3].azi) };
				double Z1[2]={(Track_to_start[i][L-2].r)*cos(Track_to_start[i][L-2].azi),(Track_to_start[i][L-2].r)*sin(Track_to_start[i][L-2].azi) };
				double Z2[2]={(Track_to_start[i][L-1].r)*cos(Track_to_start[i][L-1].azi),(Track_to_start[i][L-1].r)*sin(Track_to_start[i][L-1].azi) };
				double X[6];
				double P[6][6];
				double T1=(Track_to_start[i][L-2].CPI_time-Track_to_start[i][L-3].CPI_time)/1000.0;
				double T2=(Track_to_start[i][L-1].CPI_time-Track_to_start[i][L-2].CPI_time)/1000.0;
				kalman Kalman;
				Kalman.kalman_filter_init_3dots(Z0,  Z1, Z2, T1,T2, X ,P);

				Trust_Track trust_track_tmp = {};
				memcpy(trust_track_tmp.X, X, 6*sizeof(double));
				memcpy(trust_track_tmp.P, P, 6*6*sizeof(double));
				
				trust_track_tmp.track_id = Track_to_start[i][L-1].temp_track_id;
				trust_track_tmp.point_id = Track_to_start[i][L-1].point_id;
				trust_track_tmp.circle_id = Track_to_start[i][L-1].circle_id;
				trust_track_tmp.Amplitude=Track_to_start[i][L-1].amplitude;
				trust_track_tmp.snr_point = Track_to_start[i][L-1].SNR;
				trust_track_tmp.RCS = Track_to_start[i][L-1].RCS;
				trust_track_tmp.pitch_num = Track_to_start[i][L-1].pitch_num;
				std::memcpy(trust_track_tmp.speed_dim, Track_to_start[i][L-1].speed_dim, sizeof(trust_track_tmp.speed_dim));
				std::memcpy(trust_track_tmp.range_dim, Track_to_start[i][L-1].range_dim, sizeof(trust_track_tmp.range_dim));
				trust_track_tmp.T_track = Track_to_start[i][L-1].CPI_time;
				trust_track_tmp.GNSS_time = Track_to_start[i][L-1].GNSS_time;
				trust_track_tmp.Unupdated_time = 0;
				trust_track_tmp.point_flag=1;       //实点
				trust_track_tmp.Track_Mode=0;       //跟踪模式 TWS 0
				trust_track_tmp.Target_Type=0;
				trust_track_tmp.Track_Index=index;
				trust_track_tmp.Height=Track_to_start[i][L-1].height;
				trust_track_tmp.Extrapolate_round=0;
				trust_track_tmp.approach_flag = -1;
				trust_track_tmp.manual_delete_flag = 0;
				trust_track_tmp.associate_point_number = 0;
				trust_track_tmp.manual_tracking_flag = 0;
				trust_track_tmp.point_type = 0;
				trust_track_tmp.Quality = Track_to_start[i][L-1].quality;
				trust_track_tmp.Point_sum = L;
				trust_track_tmp.Valid_point_sum = L;
				trust_track_tmp.u[0]=0.3333;
				trust_track_tmp.u[1]=0.3333;
				trust_track_tmp.u[2]=0.3333;
				memcpy(trust_track_tmp.X1, X, 6*sizeof(double));
				memcpy(trust_track_tmp.P1, P, 6*6*sizeof(double));
				memcpy(trust_track_tmp.X2, X, 6*sizeof(double));
				memcpy(trust_track_tmp.P2, P, 6*6*sizeof(double));
				memcpy(trust_track_tmp.X3, X, 6*sizeof(double));
				memcpy(trust_track_tmp.P3, P, 6*6*sizeof(double));

				double r, azi;
				coor_trans Coor_trans;
				Coor_trans.cart2polar(X[0], X[3], &r, &azi);

				(*trust_track).push_back(trust_track_tmp);

				//输出航迹更新信息
				*Trust_track_num_Output=*Trust_track_num_Output+1;
				for (int j=0;j<L;j++)
				{
					Trust_Track_Output[*Trust_track_num_Output-1][j].Track_Index=index;
					Trust_Track_Output[*Trust_track_num_Output-1][j].Point_Sum=L;

					Trust_Track_Output[*Trust_track_num_Output-1][j].Range=static_cast<float>(Track_to_start[i][j].r);
					Trust_Track_Output[*Trust_track_num_Output-1][j].Azimuth=static_cast<float>(Track_to_start[i][j].azi/PI*180);
					{
						double elev_ratio = (Track_to_start[i][j].r > 0.0) ? Track_to_start[i][j].height / Track_to_start[i][j].r : 0.0;
						if (elev_ratio > 1.0) elev_ratio = 1.0;
						if (elev_ratio < -1.0) elev_ratio = -1.0;
						Trust_Track_Output[*Trust_track_num_Output-1][j].Elevation=static_cast<float>(asin(elev_ratio)/PI*180);
					}
					Trust_Track_Output[*Trust_track_num_Output-1][j].Range_V=static_cast<float>(sqrt(pow(Track_to_start[i][j].X[1],2)+pow(Track_to_start[i][j].X[3],2)));
					Trust_Track_Output[*Trust_track_num_Output-1][j].z=static_cast<float>(Track_to_start[i][j].height);
					Trust_Track_Output[*Trust_track_num_Output-1][j].Amplitude=static_cast<float>(Track_to_start[i][j].amplitude);
					Trust_Track_Output[*Trust_track_num_Output-1][j].track_snr = static_cast<float>(Track_to_start[i][j].SNR);
					Trust_Track_Output[*Trust_track_num_Output-1][j].track_rcs = static_cast<float>(Track_to_start[i][j].RCS);
					Trust_Track_Output[*Trust_track_num_Output-1][j].Quality = static_cast<float>(Track_to_start[i][j].quality * rdp_parameter.track_quality_max);

					Trust_Track_Output[*Trust_track_num_Output-1][j].pitch_num = Track_to_start[i][j].pitch_num;

					std::memcpy(Trust_Track_Output[*Trust_track_num_Output-1][j].speed_dim, Track_to_start[i][j].speed_dim, sizeof(Trust_Track_Output[*Trust_track_num_Output-1][j].speed_dim));
					std::memcpy(Trust_Track_Output[*Trust_track_num_Output-1][j].range_dim, Track_to_start[i][j].range_dim, sizeof(Trust_Track_Output[*Trust_track_num_Output-1][j].range_dim));
					double Direction_Angle;
					Direction_Angle=atan2(Track_to_start[i][j].X[3],Track_to_start[i][j].X[1]);
					if(Direction_Angle<0)
						Direction_Angle=Direction_Angle+2*PI;
					Trust_Track_Output[*Trust_track_num_Output-1][j].Direction_Angle=static_cast<float>(Direction_Angle/PI*180);

					Trust_Track_Output[*Trust_track_num_Output-1][j].track_time=static_cast<float>((Track_to_start[i][j].CPI_time)/1000.0);
					Trust_Track_Output[*Trust_track_num_Output-1][j].GNSS_time=Track_to_start[i][j].GNSS_time;

					Trust_Track_Output[*Trust_track_num_Output-1][j].x=static_cast<float>(Track_to_start[i][j].X[0]);
					Trust_Track_Output[*Trust_track_num_Output-1][j].v_x=static_cast<float>(Track_to_start[i][j].X[1]);
					Trust_Track_Output[*Trust_track_num_Output-1][j].y=static_cast<float>(Track_to_start[i][j].X[2]);
					Trust_Track_Output[*Trust_track_num_Output-1][j].v_y=static_cast<float>(Track_to_start[i][j].X[3]);

					Trust_Track_Output[*Trust_track_num_Output-1][j].range_point=static_cast<float>(Track_to_start[i][j].r);
					Trust_Track_Output[*Trust_track_num_Output-1][j].azi_point=static_cast<float>(Track_to_start[i][j].azi/PI*180);
					{
						double elev_ratio = (Track_to_start[i][j].r > 0.0) ? Track_to_start[i][j].height / Track_to_start[i][j].r : 0.0;
						if (elev_ratio > 1.0) elev_ratio = 1.0;
						if (elev_ratio < -1.0) elev_ratio = -1.0;
						Trust_Track_Output[*Trust_track_num_Output-1][j].elev_point=static_cast<float>(asin(elev_ratio)/PI*180);
					}
					Trust_Track_Output[*Trust_track_num_Output-1][j].vr_point=static_cast<float>(Track_to_start[i][j].vr);
					Trust_Track_Output[*Trust_track_num_Output-1][j].point_type=0;
					Trust_Track_Output[*Trust_track_num_Output-1][j].Flag_Point=1;
					Trust_Track_Output[*Trust_track_num_Output-1][j].Track_Mode=0;//跟踪模式 TWS 0

				}
			}
		}
	}

	std::vector <std::vector<Temp_track>>().swap(Track_to_start);

}

void Track_Init::tmp_track_die(std::vector <std::vector<Temp_track>>     *temp_track)
{

	std::vector <std::vector<Temp_track>>::iterator Iter;
	for (Iter=temp_track->begin(); Iter!=temp_track->end();)
	{
		int n=static_cast<int>((*Iter).size());
		if (n <= 0)
		{
			temp_track->erase(Iter);
			Iter=temp_track->begin();
			continue;
		}
		if( (*Iter)[n-1].buff_round >2  || n>=10)
		{

		   temp_track->erase(Iter);
		   Iter=temp_track->begin();
		}
		else
		{
			Iter++;
		}
	}
};

int Track_Init::track_init_prohibit(double r, double azi)
{

	for (int i=0;i<rdp_parameter.track_init_prohibite_area_num;i++)
	{
		if(r<rdp_parameter.track_init_prohibite_area_range_max[i] && r>rdp_parameter.track_init_prohibite_area_range_min[i] && azi<rdp_parameter.track_init_prohibite_area_azimuth_max[i] && azi>rdp_parameter.track_init_prohibite_area_azimuth_min[i])
		{
			return 1;
		}
	}

	return 0;

}

Track_Init::Track_Init()
{

}

void Track_Init::reset()
{
	point_process.clear();
	track_index_mangement.reset();
}