#include "track_init.h"
#include "kalman.h"
#include "coor_trans.h"
#include <qmath.h>
#include "memory.h"
#include <QVector>
#include <iostream>
#include<iomanip>
using namespace std;


int Track_Init::track_init_process_logic(	QVector <PointRecv>               *point_recv,                            //输入点迹
                                            QVector <QVector<Trust_Track>>    *trust_track,                           //可靠航迹
                                            QVector <QVector<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++)
        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);

        //删除关联上的点迹
        QVector <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++;
            }
        }

        //剩余点重新存入点迹
        QVector<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.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.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( QVector <Temp_track> ());
            int n=temp_track->size();
            (*temp_track)[n-1].push_back(temp_track_tmp);

        }

        //清空点迹
        QVector<PointRecv>().swap(point_process);
        QVector<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);


    return 0;

}



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


    //关联信息
    struct Asso_info
    {
        int track_idx;
        int point_idx;
    };
    QVector <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)
            {

                //点迹信息
//                float Z[2]={ point_process[i].Range*cos(point_process[i].Azimuth),point_process[i].Range*sin(point_process[i].Azimuth)} ;
                float Z[3]={point_process[i].Range,
                            point_process[i].Azimuth,
                            point_process[i].Velocity};
                float vr_point=point_process[i].Velocity;
                float r_point=point_process[i].Range;
                float prt = point_process[i].PRF_index;
                float freq_ind = point_process[i].Freq_index;
                float h_point = point_process[i].Height;
                float T_point = point_process[i].CPI_Time;

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


                float delta_T = (T_point - T_track_head)/1000.0;

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


                    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.push_back(asso_info_tmp);
                        (*temp_track)[j][L-1].asso_flag = 1;
                        point_process[i].Use_Flag = 1;
                    }
                }
            }
        }
    }



    // temp_track中加入新关联上的临时航迹
    for (int i = 0 ; i<asso_info.size();i++ )
    {
        (*temp_track).push_back(QVector <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.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.Temp_track_section_idx = floor(point_process[asso_info[i].point_idx-1].beam_index/BEAM_NUM_DOT_SECTION)+1;
        float 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)};
        float 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)};
        float X[4];
        float P[4][4];
        kalman Kalman;
        float 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(float));
        memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(float));
        (*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp);
    }


    //temp_track中删除关联上的临时航迹
    QVector <QVector<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( QVector <QVector<Temp_track>>     *temp_track,
                                        struct  RadarPara                 Work_Parameter)
{

    //关联上的信息
    struct Asso_info
    {
        int track_idx;
        int point_idx;
    };
    QVector <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)
            {


                //点迹信息
                float 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;
                float r_point=point_process[i].Range;
                float h_point = point_process[i].Height;
                float T_point = point_process[i].CPI_Time;

                //航迹信息
                float x_track_head, y_track_head;
                x_track_head=(*temp_track)[j][0].X[0];
                y_track_head=(*temp_track)[j][0].X[2];
                float v_track_head=(*temp_track)[j][0].vr;
                float h_track_head = (*temp_track)[j][0].height;
                float T_track_head = (*temp_track)[j][0].T;


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

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

                 float 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
                 {
                    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;
                 }



            }
        }
    }


    // temp_track中加入新关联上的临时航迹
    for (int i = 0 ; i<asso_info.size();i++ )
    {
        (*temp_track).push_back(QVector <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.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;;
        float 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)};
        float 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)};
        float X[4];
        float P[4][4];
        kalman Kalman;
        float 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(float));
        memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(float));
        (*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp);

    }



    //temp_track中删除关联上的航迹头
    QVector <QVector<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(float x0,float y0,float x1,float y1,float x2,float 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(QVector <QVector<Trust_Track>>    *trust_track,
                                          QVector <QVector<Temp_track>>     *temp_track,
                                          struct Track                      Trust_Track_Output[MAX_TRACK_NUM][10],
                                          int                               *Trust_track_num_Output,
                                          struct  RadarPara                 Work_Parameter)
{

    //temp_track中的临时航迹转为可靠航迹
    QVector <QVector<Temp_track>>::iterator Iter;
    for (Iter=temp_track->begin(); Iter!=temp_track->end();)
    {
        int L=(*Iter).size();
        float range=(*Iter)[L-1].r;
        float azi=(*Iter)[L-1].azi/PI*180;
        if(  ( L==Work_Parameter.track_start_point_num)&& track_init_prohibit(range,azi,Work_Parameter)==0) //按长度查找TRUST_TRACK_POINT
        {
            if(trust_track->size()<MAX_TRACK_INDEX)
            {

                int index = track_index_mangement.track_ind_get(trust_track);
                if(index>0)
                {

                    //加入本地航迹文件
                    float Z0[2]={((*Iter)[L-3].r)*cos((*Iter)[L-3].azi),((*Iter)[L-3].r)*sin((*Iter)[L-3].azi) };
                    float Z1[2]={((*Iter)[L-2].r)*cos((*Iter)[L-2].azi),((*Iter)[L-2].r)*sin((*Iter)[L-2].azi) };
                    float Z2[2]={((*Iter)[L-1].r)*cos((*Iter)[L-1].azi),((*Iter)[L-1].r)*sin((*Iter)[L-1].azi) };
                    float X[6];
                    float P[6][6];
                    float T1=((*Iter)[L-2].T-(*Iter)[L-3].T)/1000.0;
                    float T2=((*Iter)[L-1].T-(*Iter)[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(float));
                    memcpy(trust_track_tmp.P, P, 6*6*sizeof(float));

                    trust_track_tmp.Amplitude=(*Iter)[L-1].Amp;
                    trust_track_tmp.snr_point = (*Iter)[L-1].snr;
                    trust_track_tmp.RCS = (*Iter)[L-1].RCS;
                    trust_track_tmp.T_track = (*Iter)[L-1].T;
                    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=(*Iter)[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(float));
                    memcpy(trust_track_tmp.P1, P, 6*6*sizeof(float));
                    memcpy(trust_track_tmp.X2, X, 6*sizeof(float));
                    memcpy(trust_track_tmp.P2, P, 6*6*sizeof(float));
                    memcpy(trust_track_tmp.X3, X, 6*sizeof(float));
                    memcpy(trust_track_tmp.P3, P, 6*6*sizeof(float));

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


                    if(azi/PI*180>=TRACK_SECTION_1_START && azi/PI*180<TRACK_SECTION_2_START)
                       trust_track_tmp.Track_section_idx = 1;

                    if(azi/PI*180>=TRACK_SECTION_2_START && azi/PI*180<TRACK_SECTION_3_START)
                       trust_track_tmp.Track_section_idx = 2;

                    if(azi/PI*180>=TRACK_SECTION_3_START && azi/PI*180<TRACK_SECTION_4_START)
                       trust_track_tmp.Track_section_idx = 3;

                    if(azi/PI*180>=TRACK_SECTION_4_START && azi/PI*180<TRACK_SECTION_5_START)
                       trust_track_tmp.Track_section_idx = 4;

                    if(azi/PI*180>=TRACK_SECTION_5_START && azi/PI*180<TRACK_SECTION_6_START)
                       trust_track_tmp.Track_section_idx = 5;

                    if(azi/PI*180>=TRACK_SECTION_6_START && azi/PI*180<TRACK_SECTION_7_START)
                       trust_track_tmp.Track_section_idx = 6;

                    if(azi/PI*180>=TRACK_SECTION_7_START && azi/PI*180<TRACK_SECTION_8_START)
                       trust_track_tmp.Track_section_idx = 7;

                    if(azi/PI*180>=TRACK_SECTION_8_START && azi/PI*180<TRACK_SECTION_9_START)
                       trust_track_tmp.Track_section_idx = 8;

                    if(azi/PI*180>=TRACK_SECTION_9_START || azi/PI*180<TRACK_SECTION_1_START)
                       trust_track_tmp.Track_section_idx = 9;

                    (*trust_track)[trust_track_tmp.Track_section_idx-1].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=(*Iter)[j].r;
                        Trust_Track_Output[*Trust_track_num_Output-1][j].Azimuth=(*Iter)[j].azi/PI*180;
                        Trust_Track_Output[*Trust_track_num_Output-1][j].Elevation=asin((*Iter)[j].height/(*Iter)[j].r)/PI*180;
                        Trust_Track_Output[*Trust_track_num_Output-1][j].Range_V=sqrt(pow((*Iter)[j].X[1],2)+pow((*Iter)[j].X[3],2));
                        Trust_Track_Output[*Trust_track_num_Output-1][j].z=(*Iter)[j].height;
                        Trust_Track_Output[*Trust_track_num_Output-1][j].Amplitude=(*Iter)[j].Amp;
                        Trust_Track_Output[*Trust_track_num_Output-1][j].track_snr = (*Iter)[j].snr;
                        Trust_Track_Output[*Trust_track_num_Output-1][j].track_rcs = (*Iter)[j].RCS;
                        float Direction_Angle;
                        Direction_Angle=atan2((*Iter)[j].X[3],(*Iter)[j].X[1]);
                        if(Direction_Angle<0)
                            Direction_Angle=Direction_Angle+2*PI;
                        Trust_Track_Output[*Trust_track_num_Output-1][L-1].Direction_Angle=Direction_Angle/PI*180;



                        Trust_Track_Output[*Trust_track_num_Output-1][j].track_time=((*Iter)[j].T)/1000.0;

                        Trust_Track_Output[*Trust_track_num_Output-1][j].x=(*Iter)[j].X[0];
                        Trust_Track_Output[*Trust_track_num_Output-1][j].v_x=(*Iter)[j].X[1];
                        Trust_Track_Output[*Trust_track_num_Output-1][j].y=(*Iter)[j].X[2];
                        Trust_Track_Output[*Trust_track_num_Output-1][j].v_y=(*Iter)[j].X[3];

                        Trust_Track_Output[*Trust_track_num_Output-1][j].range_point=(*Iter)[j].r;
                        Trust_Track_Output[*Trust_track_num_Output-1][j].azi_point=(*Iter)[j].azi/PI*180;
                        Trust_Track_Output[*Trust_track_num_Output-1][j].elev_point=asin((*Iter)[j].height/(*Iter)[j].r)/PI*180;
                        Trust_Track_Output[*Trust_track_num_Output-1][j].vr_point=(*Iter)[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
                    }

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

        }
        else
        {
            Iter++;
        }
    }

}


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

    QVector <QVector<Temp_track>>::iterator Iter;
    for (Iter=temp_track->begin(); Iter!=temp_track->end();)
    {
        int n=(*Iter).size();
        if( (*Iter)[n-1].buff_round >1  || n>=10)
        {
            temp_track->erase(Iter);
            Iter=temp_track->begin();
        }
        else
        {
            Iter++;
        }
    }
};




int Track_Init::track_init_prohibit(float r, float 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()
{


}