#include <tas_ctrl.h>
#include "coor_trans.h"
#include <qmath.h>
#include"memory.h"
#include <QVector>
using namespace std;







TAS_Ctrl::TAS_Ctrl()
{
    memset(tas_target_queue,0,TAS_QUEUE_LENGTH*sizeof(Tracking_Target));

    tas_target_num=0;
}

void TAS_Ctrl::tas_ctrl_process(QVector<Trust_Track>            *trust_track,
                                struct TrackingBeam             *Tracking_beam,
                                struct Track                    Trust_Track_Output[][10],
                                int                             *Trust_track_num_Output,
                                struct  RadarPara               Work_Parameter)
{
    tas_target_add(trust_track, Trust_Track_Output, Trust_track_num_Output,Work_Parameter);
    tas_target_del(trust_track, Trust_Track_Output, Trust_track_num_Output,Work_Parameter);
    tas_beam_output(trust_track,Tracking_beam);

    //跟踪队列移位
    struct Tracking_Target tas_target_tmp;
    memcpy(&tas_target_tmp, &tas_target_queue[TAS_QUEUE_LENGTH-1], sizeof(Tracking_Target));
    for (int i=TAS_QUEUE_LENGTH-1;i>0;i--)
    {
        memcpy(&tas_target_queue[i],&tas_target_queue[i-1],sizeof(Tracking_Target));
    }
    memcpy(&tas_target_queue[0], &tas_target_tmp, sizeof(Tracking_Target));



};


void TAS_Ctrl::tas_target_add(QVector<Trust_Track>          *trust_track,
                              struct Track                  Trust_Track_Output[][10],
                              int                           *Trust_track_num_Output,
                              struct  RadarPara             Work_Parameter)
{

    for (int i=0;i<trust_track->size();i++)
        {
            double r=sqrt(pow((*trust_track)[i].X[0],2)+pow((*trust_track)[i].X[3],2));
            double v=sqrt(pow((*trust_track)[i].X[1],2)+pow((*trust_track)[i].X[4],2));
            double h=(*trust_track)[i].Height+Work_Parameter.Height;
            double azi = atan2((*trust_track)[i].X[3],(*trust_track)[i].X[0]);
            if(azi<0)
                azi=azi+2*PI;
            azi=azi/PI*180;

            //进入跟踪的条件：  1.手动跟踪的目标   或  满足速度、距离、高度条件满足 关联点数大于2    2.不在禁止跟踪区域
            if( ( (*trust_track)[i].manual_tracking_flag == 1)//|| (tas_auto_start( v,  r,  h, Work_Parameter)==1 && (*trust_track)[i][k].associate_point_number>=2)
                && tas_target_num < MAX_TAS_NUM
                && (*trust_track)[i].Track_Mode == 0
                && tas_prohibite_area( r, azi,  v,  h, Work_Parameter) == 0)
            {
                for(int j=0;j<TAS_QUEUE_LENGTH;j++)
                {
                    if(tas_target_queue[j].empty_flag==0 && j%2==0)
                    {
                        //插入跟踪队列
                        tas_target_queue[j].Index=(*trust_track)[i].Track_Index;
                        tas_target_queue[j].empty_flag=1;

                        //跟踪目标数目加一
                        tas_target_num=tas_target_num+1;

                        //TAS跟踪标志置1
                        (*trust_track)[i].Track_Mode=1;

                        //目标跟踪状态发生改变 对外输出航迹更新信息
                        *Trust_track_num_Output= *Trust_track_num_Output+1;
                        double r_output,azmi_output;
                        coor_trans Coor_trans;
                        Coor_trans.cart2polar((*trust_track)[i].X[0],(*trust_track)[i].X[3],&r_output,&azmi_output);
                        Trust_Track_Output[*Trust_track_num_Output-1][0].Point_Sum=1;
                        Trust_Track_Output[*Trust_track_num_Output-1][0].Range=r_output;
                        Trust_Track_Output[*Trust_track_num_Output-1][0].Azimuth=azmi_output/PI*180;
                        Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Index=(*trust_track)[i].Track_Index;	//航迹号
                        Trust_Track_Output[*Trust_track_num_Output-1][0].Range_V=
                                sqrt((*trust_track)[i].X[1]*(*trust_track)[i].X[1]+(*trust_track)[i].X[4]*(*trust_track)[i].X[4]);
                        Trust_Track_Output[*Trust_track_num_Output-1][0].Amplitude=(*trust_track)[i].Amplitude;
                        Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Point=(*trust_track)[i].point_flag;
                        Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Mode=(*trust_track)[i].Track_Mode;
                        Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation=asin((*trust_track)[i].Height/r_output)/PI*180;
                        Trust_Track_Output[*Trust_track_num_Output-1][0].z=(*trust_track)[i].Height+Work_Parameter.Height;
                        Trust_Track_Output[*Trust_track_num_Output-1][0].Target_Type=(*trust_track)[i].Target_Type;
                        Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = (*trust_track)[i].snr_point;
                        break;

                    }
                }
            }
    }



};



//int TAS_Ctrl:: tas_auto_start(double v, double r, double h, struct  RadarPara		Work_Parameter)
//{

////    if(r>Work_Parameter.R_TAS_min && r<Work_Parameter.R_TAS_max && v<Work_Parameter.V_TAS_max && v>Work_Parameter.V_TAS_min
////        && ( (r<=1000 && h>=Work_Parameter.TAS_Height_1) || (r<=2000 && r>1000 && h>=Work_Parameter.TAS_Height_2) || (r<=3000 && r>2000 && h>=Work_Parameter.TAS_Height_3)  || (r<=4000 && r>3000 && h>=Work_Parameter.TAS_Height_4) || (r>4000 && h>=Work_Parameter.TAS_Height_5)  )
////       )
//    if( r>Work_Parameter.R_TAS_min  && r<Work_Parameter.R_TAS_max && v<Work_Parameter.V_TAS_max && v>Work_Parameter.V_TAS_min
//        && ( (r<=1000 && h>=Work_Parameter.TAS_Height_1) || (r<=2000 && r>1000 && h>=Work_Parameter.TAS_Height_2) || (r<=3000 && r>2000 && h>=Work_Parameter.TAS_Height_3)  || (r<=4000 && r>3000 && h>=Work_Parameter.TAS_Height_4) || (r>4000 && h>=Work_Parameter.TAS_Height_5)  ) )
//    {
//        return 1;
//    }
//    else
//    {
//        return 0;
//    }

////    return 0;

//};


int TAS_Ctrl::tas_prohibite_area(double r,double azi, double v, double h,struct  RadarPara		Work_Parameter)
{

    for (int i=0;i<Work_Parameter.TAS_prohibite_area_num;i++)
    {
        if(r<Work_Parameter.R_max_TAS_prohibited[i] && r>Work_Parameter.R_min_TAS_prohibited[i] && azi<Work_Parameter.Azimuth_max_TAS_prohibited[i] && azi>Work_Parameter.Azimuth_min_TAS_prohibited[i])
        {
            return 1;
        }
    }

    return 0;


}

void TAS_Ctrl::tas_target_del(QVector<Trust_Track>          *trust_track,
                            struct Track                     Trust_Track_Output[][10],
                            int                             *Trust_track_num_Output,
                            struct  RadarPara               Work_Parameter)
{

    //查找TAS目标是否已经消批  若消批则移出TAS队列
    for (int i=0;i<TAS_QUEUE_LENGTH;i++)
    {
        if(tas_target_queue[i].empty_flag == 1)
        {
            //查找TAS队列里的目标是否存在
            int flag=0;
            for (int j=0;j<trust_track->size();j++)
                {
                    if(tas_target_queue[i].Index==(*trust_track)[j].Track_Index && (*trust_track)[j].Track_Mode == 1)
                    {
                        flag=1;
                    }
                }
            //目标不存在 说明已消批  从队列里删除
            if(flag == 0)
            {
                memset(&tas_target_queue[i],0,sizeof(Tracking_Target));
                tas_target_num=tas_target_num-1;
            }
        }
    }

    //判断TAS目标是否满足自动跟踪条件  若不满足移除TAS队列   通知界面改变目标状态
//    for ( int i=0;i<trust_track->size();i++)
//        {
//            double r=sqrt(pow((*trust_track)[i].X[0],2)+pow((*trust_track)[i].X[3],2));
//            double v=sqrt(pow((*trust_track)[i].X[1],2)+pow((*trust_track)[i].X[4],2));
//            double h=(*trust_track)[i].Height+Work_Parameter.Height;

//            if((*trust_track)[i].manual_tracking_flag == 0 && (*trust_track)[i].Track_Mode == 1 &&  tas_auto_end(v,r,h,Work_Parameter) == 1)  //满足结束跟踪条件
//            {

//            int track_index=(*trust_track)[i].Track_Index;

//                for (int ii=0;ii<TAS_QUEUE_LENGTH;ii++)
//                {
//                    if(tas_target_queue[ii].Index == track_index)
//                    {
//                        //从队列中删除
//                        memset(&tas_target_queue[ii],0,sizeof(Tracking_Target));
//                        tas_target_num=tas_target_num-1;

//                        //TAS跟踪标志置零
//                        (*trust_track)[i].Track_Mode=0;

//                        //目标类型设为地面目标
//                        (*trust_track)[i].Target_Type = 0;

//                        //航迹状态发生改变 对外输出航迹更新信息
//                        *Trust_track_num_Output= *Trust_track_num_Output+1;
//                        double r_output,azmi_output;
//                        coor_trans Coor_trans;
//                        Coor_trans.cart2polar((*trust_track)[i].X[0],(*trust_track)[i].X[3],&r_output,&azmi_output);
//                        Trust_Track_Output[*Trust_track_num_Output-1][0].Point_Sum=1;
//                        Trust_Track_Output[*Trust_track_num_Output-1][0].Range=r_output;
//                        Trust_Track_Output[*Trust_track_num_Output-1][0].Azimuth=azmi_output/PI*180;
//                        Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Index=(*trust_track)[i].Track_Index;	//航迹号
//                        Trust_Track_Output[*Trust_track_num_Output-1][0].Range_V=sqrt((*trust_track)[i].X[1]*(*trust_track)[i].X[1]+(*trust_track)[i].X[4]*(*trust_track)[i].X[4]);
//                        Trust_Track_Output[*Trust_track_num_Output-1][0].Amplitude=(*trust_track)[i].Amplitude;
//                        Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Point=(*trust_track)[i].point_flag;
//                        Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Mode=(*trust_track)[i].Track_Mode;
//                        Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation=asin((*trust_track)[i].Height/r_output)/PI*180;
//                        Trust_Track_Output[*Trust_track_num_Output-1][0].z=(*trust_track)[i].Height+Work_Parameter.Height;
//                        Trust_Track_Output[*Trust_track_num_Output-1][0].Target_Type=(*trust_track)[i].Target_Type;
//                        Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = (*trust_track)[i].snr_point;
//                    }
//                }
//            }

//        }


};

//int TAS_Ctrl::tas_auto_end(double v, double r, double h, struct  RadarPara		Work_Parameter)
//{
//    if((r<=1000 && h<Work_Parameter.TAS_Height_1) || (r<=2000 && r>1000 && h<Work_Parameter.TAS_Height_2) || (r<=3000 && r>2000 && h<Work_Parameter.TAS_Height_3)  || (r<=4000 && r>3000 && h<Work_Parameter.TAS_Height_4) || (r>4000 && h<Work_Parameter.TAS_Height_5)
//        ||  v<Work_Parameter.V_TAS_min  || v>Work_Parameter.V_TAS_max
//        )
//    {
//        return 1;

//    }
//    else
//    {

//        return 0;
//    }

//    return 0;

//};



void  TAS_Ctrl::tas_beam_output(QVector<Trust_Track>            *trust_track,
                                struct TrackingBeam             *Tracking_beam)
{
    if(tas_target_queue[0].empty_flag==1)
    {

        double H_track;
        double X_now[6];
        for (int i=0;i<trust_track->size();i++ )
            {
                if((*trust_track)[i].Track_Index == tas_target_queue[0].Index)
                {
                    H_track = (*trust_track)[i].Height;
                    for(int ii=0;ii<6;ii++)
                        X_now[ii]=(*trust_track)[i].X[ii];
                }
            }


        //预测目标位置 计算跟踪波束波位号 俯仰角

        double x_track=X_now[0]+X_now[1]*T_TAS_PRED;
        double y_track=X_now[3]+X_now[4]*T_TAS_PRED;
        double amzi,range;
        coor_trans Coor_trans;
        Coor_trans.cart2polar(x_track,y_track,&range,&amzi);


        //目标距离
        Tracking_beam->Range=range;
        //目标方位
        Tracking_beam->Azi=amzi/PI*180;
        //目标俯仰角
        double elev=asin(H_track/range)/PI*180;


        if(elev<=0)
            elev=0;
        else if(elev>=40)
            elev=40;
        else
            elev=elev;


        Tracking_beam->Elev=elev;

        //跟踪波束类型
        Tracking_beam->type=1;

        //跟踪目标批号
        Tracking_beam->TAS_track_index = tas_target_queue[0].Index;

        //跟踪波束开关开启
        Tracking_beam->open_flag=1;
    }
    else
    {
        //跟踪波束开关关闭
        Tracking_beam->open_flag=0;
    }



}