#include "track_init.h"
#include "kalman.h"
#include "coor_trans.h"
#include <cmath>
#include <cstring>
#include <vector>
#include <iostream>
#include<iomanip>
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,
											struct  RadarPara                 Work_Parameter
										)

{

//    for(int i=0;i<(*point_recv).size();i++)
//    {
//        if((*point_recv)[i].CPI_Time == 30675948 || (*point_recv)[i].CPI_Time == 30682320
//                 || (*point_recv)[i].CPI_Time == 30682132 || (*point_recv)[i].CPI_Time == 30688692
//                || (*point_recv)[i].CPI_Time == 30691692 ||  (*point_recv)[i].CPI_Time == 30691880)
//        {
//            std::cout << "azi=" << (*point_recv)[i].Azimuth
//                     << "dis=" << (*point_recv)[i].Range
//                     << "h=" << (*point_recv)[i].Height
//                     << "v=" << (*point_recv)[i].Velocity
//                     << "wave=" << (*point_recv)[i].beam_index
//                     << "time=" << (*point_recv)[i].CPI_Time;
//        }
//    }

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

	//临时航迹的buff_round+1
	for (int i=0;i<temp_track->size();i++)
	{
		for (int 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,Work_Parameter);

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

		//删除关联上的点迹
		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 (int 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.r = point_process[i].Range;
			temp_track_tmp.azi = point_process[i].Azimuth;
			temp_track_tmp.height = point_process[i].Height;
			temp_track_tmp.Amp = 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_num;
			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.T = 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=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));

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

	}

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

//    std::cout << "-----------------------------------------------";
//    for(int i=0;i<temp_track->size();i++)
//    {
//        for(int j=0;j<(*temp_track)[i].size();j++)
//        {
//            std::cout << "temp_track_azi=" << (*temp_track)[i][j].azi
//                     << "temp_track_dis=" << (*temp_track)[i][j].r
//                     << "temp_track_h=" << (*temp_track)[i][j].height
//                     << "temp_track_v=" << (*temp_track)[i][j].vr
//                     << "temp_track_time=" << (*temp_track)[i][j].T;
//        }
//    }

	return 0;

}

void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>>     *temp_track,
									   struct  RadarPara                 Work_Parameter)
{

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

	for ( int i=0;i<point_process.size();i++)
	{
		for ( int j=0;j<(*temp_track).size();j++)
		{
			int L = (*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 = 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 = (*temp_track)[j][L-1].T;

				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(T_track_head == 33855816 && T_point == 33859004 )
//                    {
//                        std::cout << "r_point=" << r_point
//                                 << "h_point=" << h_point
//                                 << "T_point=" << T_point
//                                 << "r_temp_track=" << r_temp_track
//                                 << "v_temp_track=" << v_temp_track
//                                 << "h_temp_track=" << h_temp_track
//                                 << "T_track_head=" << T_track_head
//                                 << "d=" << d
//                                 << "alpha=" << alpha;

//                    }

					if(d*d<=TRACK_START_THRESHOLD*TRACK_START_THRESHOLD && alpha<ALPHA_START && vr_point*v_temp_track>0)//  &&  fabs(h_point-h_temp_track )<= r_point*SIGMA_E&& abs(vr_point-v_temp_track)/abs(v_temp_track)<0.8
					{
						struct Asso_info asso_info_tmp;
						asso_info_tmp.point_idx = i+1;
						asso_info_tmp.track_idx = 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 (int i = 0 ; i<asso_info.size();i++ )
	{
		(*temp_track).push_back(std::vector <Temp_track> ());

		//前L个点
		int L = (*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 asso_track_info_tmp = {};
		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.Amp = point_process[asso_info[i].point_idx-1].Amplitude;
		asso_track_info_tmp.T = 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_num;
		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].T)/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).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  RadarPara                 Work_Parameter)
{

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

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

		for ( int 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 = 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 = (*temp_track)[j][0].T;

//                if(T_track_head == 30675948)
//                {
//                    std::cout << "r_point=" << r_point
//                             << "h_point=" << h_point
//                             << "T_point=" << T_point
//                             << "x_track_head=" << x_track_head
//                             << "y_track_head=" << y_track_head
//                             << "v_track_head=" << v_track_head
//                             << "T_track_head=" << T_track_head;
//                }

//                if(T_point == 30682320)
//                {
//                    std::cout <<"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!";
//                    std::cout << "r_point=" << r_point
//                             << "h_point=" << h_point
//                             << "T_point=" << T_point
//                             << "x_track_head=" << x_track_head
//                             << "y_track_head=" << y_track_head
//                             << "v_track_head=" << v_track_head
//                             << "T_track_head=" << T_track_head;
//                }

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

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

				 double delta_T = (T_point - T_track_head)/1000.0;

				 //满足关联条件的点航
//                 if( ( (r_point>=1000 && dis<=vmax*delta_T && dis>=V_MIN*delta_T) || (r_point<1000 && dis<=vmax*delta_T/2.0 && dis>=V_MIN*delta_T/2.0)    )
//                        && vr_point*v_track_head>0  )//&& fabs(vr_point-v_track_head)/fabs(v_track_head)<0.2  &&  fabs(h_point-h_track_head)<=r_point*SIGMA_E
				 if(
					 (      (Work_Parameter.work_mode == 0&&( (r_point>=1000 && dis<=vmax*delta_T && dis>=Work_Parameter.V_MIN*delta_T) || (r_point<1000 && dis<=vmax*delta_T/2.0 && dis>=Work_Parameter.V_MIN*delta_T/2.0)     ))
						||  (Work_Parameter.work_mode != 0&&( (r_point>=2000 && dis<=vmax*delta_T && dis>=Work_Parameter.V_MIN*delta_T) || (r_point<2000 && dis<=vmax*delta_T/3.0 && dis>=Work_Parameter.V_MIN*delta_T/2.0)     ))
					 )
						&& vr_point*v_track_head>0  )
				 {

//                     if(T_track_head == 30675948)
//                     {
//                         std::cout << "r_point=" << r_point
//                                  << "h_point=" << h_point
//                                  << "T_point=" << T_point
//                                  << "x_track_head=" << x_track_head
//                                  << "y_track_head=" << y_track_head
//                                  << "v_track_head=" << v_track_head
//                                  << "T_track_head=" << T_track_head;
//                     }

					struct Asso_info asso_info_tmp;
					asso_info_tmp.point_idx = i+1;
					asso_info_tmp.track_idx = 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 (int 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;

		//第二个点
		struct Temp_track asso_track_info_tmp = {};
		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.Amp = 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.T = 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_num;
		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].T)/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,
										  struct  RadarPara                 Work_Parameter)
{

	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=(*Iter).size();
		double range=(*Iter)[L-1].r;
		double azi=(*Iter)[L-1].azi/PI*180;

		//1km以内，SNR小于20dB的临时航迹不转为可靠航迹
		bool snr_flag = true;

		for(int p_idx = 0; p_idx < L; p_idx++){
			if((*Iter)[p_idx].r < 1000 && (*Iter)[p_idx].snr < 20){
				snr_flag = false;
				break;
			}
		}

		if(  ( L==Work_Parameter.track_start_point_num)&& track_init_prohibit(range,azi,Work_Parameter)==0 && snr_flag) //按长度查找TRUST_TRACK_POINT
		{
			//加入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 = Work_Parameter.track_start_point_num;
	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].T == Track_to_start[j][0].T && Track_to_start[i][0].r == Track_to_start[j][0].r)
					   || (Track_to_start[i][1].T == Track_to_start[j][1].T && Track_to_start[i][1].r == Track_to_start[j][1].r)
					   || (Track_to_start[i][2].T == Track_to_start[j][2].T && 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()<MAX_TRACK_INDEX)
		{
			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].T-Track_to_start[i][L-3].T)/1000.0;
				double T2=(Track_to_start[i][L-1].T-Track_to_start[i][L-2].T)/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.Amplitude=Track_to_start[i][L-1].Amp;
				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].T;
				trust_track_tmp.GNSS_time = Track_to_start[i][L-1].GNSS_time;
				trust_track_tmp.point_flag=1;       //实点
				trust_track_tmp.Track_Mode=0;       //跟踪模式 TWS 0
				trust_track_tmp.Target_Type=UNCONF_TARGET;
				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.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=Track_to_start[i][j].r;
					Trust_Track_Output[*Trust_track_num_Output-1][j].Azimuth=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=asin(elev_ratio)/PI*180;
					}
					Trust_Track_Output[*Trust_track_num_Output-1][j].Range_V=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=Track_to_start[i][j].height;
					Trust_Track_Output[*Trust_track_num_Output-1][j].Amplitude=Track_to_start[i][j].Amp;
					Trust_Track_Output[*Trust_track_num_Output-1][j].track_snr = Track_to_start[i][j].snr;
					Trust_Track_Output[*Trust_track_num_Output-1][j].track_rcs = Track_to_start[i][j].RCS;

					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=Direction_Angle/PI*180;

					Trust_Track_Output[*Trust_track_num_Output-1][j].track_time=(Track_to_start[i][j].T)/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=Track_to_start[i][j].X[0];
					Trust_Track_Output[*Trust_track_num_Output-1][j].v_x=Track_to_start[i][j].X[1];
					Trust_Track_Output[*Trust_track_num_Output-1][j].y=Track_to_start[i][j].X[2];
					Trust_Track_Output[*Trust_track_num_Output-1][j].v_y=Track_to_start[i][j].X[3];

					Trust_Track_Output[*Trust_track_num_Output-1][j].range_point=Track_to_start[i][j].r;
					Trust_Track_Output[*Trust_track_num_Output-1][j].azi_point=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=asin(elev_ratio)/PI*180;
					}
					Trust_Track_Output[*Trust_track_num_Output-1][j].vr_point=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=(*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, struct  RadarPara     Work_Parameter)
{

	for (int i=0;i<Work_Parameter.track_prohibite_area_num;i++)
	{
		if(r<Work_Parameter.R_max_track_prohibited[i] && r>Work_Parameter.R_min_track_prohibited[i] && azi<Work_Parameter.Azimuth_max_track_prohibited[i] && azi>Work_Parameter.Azimuth_min_track_prohibited[i])
		{
			return 1;
		}
	}

	return 0;

}

Track_Init::Track_Init()
{

}

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