#include "data_process.h"
#include "rdp_log.h"
#include "rdp_record.h"
#include "rdp_record_entity.h"
#include <cstring>
#include <vector>
#include <cmath>
#include <iostream>

using namespace std;
/*******************************************************************************/
/*******************************************************************************/
/************************************用户接口函数*******************************/
/*******************************************************************************/
/*******************************************************************************/

//数据预处理
int Data_Process::data_preprocess(struct DataRev    Data_Input[150])
{
	// 更新当前系统时间
	latest_timestamp = Data_Input[0].CPI_time;
	frame_id++;

	//数据存入缓存区域 Data_buffer
	if(Data_Input[0].point_type == 0)        //tws数据
	{
		data_num=min(Data_Input[0].Point_Sum, 150);
		//std::cout << "TARGET azi beam idx:" <<Data_Input[0].Beam_index_aiz;
		for (int i=0; i<data_num;i++)
		{
			PointRecv Data_buffer_temp;
			Data_buffer_temp.point_id = point_id++;
			Data_buffer_temp.frame_id = frame_id;
			Data_buffer_temp.amplitude=Data_Input[i].Amplitude;
			Data_buffer_temp.azimuth=Data_Input[i].Azimuth/180*PI;
			Data_buffer_temp.SNR = Data_Input[i].Snr;
			Data_buffer_temp.RCS = Data_Input[i].RCS;
			Data_buffer_temp.CPI_time = Data_Input[i].CPI_time;
			Data_buffer_temp.GNSS_time = Data_Input[i].GNSS_time;
			Data_buffer_temp.freq_index = Data_Input[i].Freq_index;
			Data_buffer_temp.PRF_index = Data_Input[i].PRI;
			Data_buffer_temp.range = Data_Input[i].Range;
			Data_buffer_temp.height= Data_Input[i].Range*sin(Data_Input[i].Elevation/180*PI);
			Data_buffer_temp.use_flag = 0;
			Data_buffer_temp.velocity = Data_Input[i].Velocity;
			Data_buffer_temp.encoder_value = Data_Input[i].Encoder_value;
			Data_buffer_temp.pitch_beam_index = Data_Input[i].pitch_num;
			Data_buffer_temp.azimuth_beam_index = Data_Input[i].Beam_index_aiz;
			Data_buffer_temp.point_type = Data_Input[i].point_type;
			Data_buffer_temp.amp_asv_point = Data_Input[i].amp_asv;
			Data_buffer_temp.prn_fftn_point = Data_Input[i].prn_fftn;

			std::memcpy(Data_buffer_temp.speed_dim, Data_Input[i].speed_dim, sizeof(Data_buffer_temp.speed_dim));
			std::memcpy(Data_buffer_temp.range_dim, Data_Input[i].range_dim, sizeof(Data_buffer_temp.range_dim));

			//数据记录示例①：把本次接收的输入点迹写入表 RdpRecPointTws（字段为示例，可按需增删）。
			//记录功能关闭时该语句展开为空操作（实参不求值），交付构建不产生任何代码，
			//用法与新增记录点配方见 rdp_record.h 与 docs/data_record_debug.md。
			RDP_RECORD(RdpRecPoint(Data_buffer_temp));

			Data_buffer.push_back(Data_buffer_temp);
		}

		// 修改一周扫描完成的判断条件，原来采用判断波位号变小为一周扫描完成，但机扫模式下，可能存在转台反转的情况，导致波位号变小，但并未扫描一周，因此改为判断波位号为0时，表示扫描一周完成
		if(Data_Input[0].Beam_index_aiz == 0)              //TWS
		{
			last_beam_num = Data_Input[0].Beam_index_aiz;

			return 1;
		}
		else
		{
			last_beam_num = Data_Input[0].Beam_index_aiz;
			return 2;
		}
	}

	else if(Data_Input[0].point_type == 1)          //tas数据
	{
		//        std::cout << "TAS TARGET :" <<Data_Input[0].TAS_track_index;
		//std::cout << "TAS time :" <<Data_Input[0].CPI_time;

		data_num = min(max(Data_Input[0].Point_Sum, 0), 150);
		TAS_track_idx = Data_Input[0].TAS_track_index;
		for (int i=0; i<data_num;i++)
		{
			PointRecv Data_buffer_temp;
			Data_buffer_temp.point_id = point_id++;
			Data_buffer_temp.frame_id = frame_id;
			Data_buffer_temp.amplitude=Data_Input[i].Amplitude;
			Data_buffer_temp.azimuth=Data_Input[i].Azimuth/180*PI;
			Data_buffer_temp.SNR = Data_Input[i].Snr;
			Data_buffer_temp.RCS = Data_Input[i].RCS;
			Data_buffer_temp.CPI_time = Data_Input[i].CPI_time;
			Data_buffer_temp.GNSS_time = Data_Input[i].GNSS_time;
			Data_buffer_temp.freq_index = Data_Input[i].Freq_index;
			Data_buffer_temp.PRF_index = Data_Input[i].PRI;
			Data_buffer_temp.range = Data_Input[i].Range;
			Data_buffer_temp.height= Data_Input[i].Range*sin(Data_Input[i].Elevation/180*PI);
			Data_buffer_temp.use_flag = 0;
			Data_buffer_temp.velocity = Data_Input[i].Velocity;
			Data_buffer_temp.pitch_beam_index = Data_Input[i].pitch_num;
			Data_buffer_temp.encoder_value = Data_Input[i].Encoder_value;
			Data_buffer_temp.azimuth_beam_index = Data_Input[i].Beam_index_aiz;
			Data_buffer_temp.point_type = Data_Input[i].point_type;
			Data_buffer_temp.amp_asv_point = Data_Input[i].amp_asv;
			Data_buffer_temp.prn_fftn_point = Data_Input[i].prn_fftn;

			std::memcpy(Data_buffer_temp.speed_dim, Data_Input[i].speed_dim, sizeof(Data_buffer_temp.speed_dim));
			std::memcpy(Data_buffer_temp.range_dim, Data_Input[i].range_dim, sizeof(Data_buffer_temp.range_dim));

			RDP_RECORD(RdpRecPoint(Data_buffer_temp));

			Data_buffer_tas.push_back(Data_buffer_temp);

			//            std::cout << "Azimuth:" << Data_Input[i].Azimuth
			//                     << "Range:" << Data_Input[i].Range
			//                      << "EL:" << Data_Input[i].Elevation
			//                      << "CPI_Time"<< Data_Input[i].CPI_time
			//                      << "data_num:" << data_num;

		}

		return 3;
	}

	else
	{
		return 0;
	}

}

#ifdef RDP_RECORD_ENABLE
//数据记录示例②：把本圈输出的航迹批量写入表 RdpRecTrackOut（TWS/TAS 共用一张表，用 track_mode 区分）。
//该辅助函数只在记录功能开启时编译（RDP_RECORD_ENABLE 由构建系统定义），交付构建完全不参与编译
static void rdp_record_output_tracks(struct Track Trust_Track_Output[][10], int track_num)
{
	if (track_num <= 0)
	{
		return;
	}
	if (track_num > MAX_TRACK_NUM)      //防御：输出行数理论上不应超过航迹容量（见 README 11.2 BUG-08）
	{
		track_num = MAX_TRACK_NUM;
	}

	std::vector<RdpRecTrackOut> recs;
	recs.reserve(static_cast<size_t>(track_num));
	for (int i = 0; i < track_num; ++i)
	{
		const struct Track &t = Trust_Track_Output[i][0];   //每行第 0 列为该航迹的主记录
		recs.push_back(RdpRecTrackOut{0, t.Track_Index, t.Track_Mode, t.Point_Sum, t.Flag_Point,
		                              t.Flag_Direction, t.GNSS_time, t.track_time,
		                              t.x, t.y, t.z, t.v_x, t.v_y, t.v_z,
		                              t.Range, t.Azimuth, t.Elevation, t.Range_V,
		                              t.track_snr, t.track_rcs});
	}
	RDP_RECORD_BATCH(recs);
}
#endif

//数据处理
int Data_Process::track_process(struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
int         *Trust_track_num_Output,
int         Track_die_Index_Output[],
int         *Track_die_num_Output,
int         model)
{

	//TWS处理
	if(model==2)
	{

		dot_coh.dot_coh_process_buff(&Data_buffer,&point_recv,Work_Parameter);
		std::vector<PointRecv>().swap(Data_buffer);
	}

	if(model==1)
	{
		*Trust_track_num_Output=0;      //航迹更新数
		*Track_die_num_Output=0;        //航迹消亡数目

		track_asso.track_asso_process(&point_recv,
		                              &trust_track,
		                              Trust_Track_Output,
		                              Trust_track_num_Output,
		                              Work_Parameter
		                              );

		track_die.track_die_process( &trust_track,
		                             Track_die_Index_Output,
		                             Track_die_num_Output);

		track_init.track_init_process_logic( &point_recv,
		                                     &trust_track,
		                                     &temp_track,
		                                     Trust_Track_Output,
		                                     Trust_track_num_Output,
		                                     Work_Parameter);

	}

	// TAS处理
	if(model==3)
	{
		*Trust_track_num_Output=0;      //航迹更新数
		*Track_die_num_Output=0;        //航迹消亡数目

		dot_coh_tas.dot_coh_tas_process(&Data_buffer_tas,&point_recv_tas);  //点迹凝聚

		std::vector<PointRecv>().swap(Data_buffer_tas);//凝聚完毕 清除Data_buffer

		track_asso_tas.track_asso_process_tas(&point_recv_tas,                          //点迹文件
		                                      &trust_track,                             //航迹文件
		                                      Trust_Track_Output,                       //更新航迹信息
		                                      Trust_track_num_Output,                   //更新航迹数
		                                      Work_Parameter,                           //工作参数
		                                      TAS_track_idx,
		                                      latest_timestamp		//最新时间戳
		                                      );

		std::vector<PointRecv>().swap(point_recv_tas); //清除 point_recv_tas

		track_die_tas.track_die_process_tas( &trust_track,
		                                     Track_die_Index_Output,
		                                     Track_die_num_Output);

		//std::cout << "Extrapolate_round :" << trust_track[TAS_track_idx].Extrapolate_round;
	}

	//数据记录示例②：本圈输出的航迹（仅 TWS/TAS 处理帧有输出；记录功能关闭时不产生任何代码）
#ifdef RDP_RECORD_ENABLE
	if (model == 1 || model == 3)
	{
		rdp_record_output_tracks(Trust_Track_Output, *Trust_track_num_Output);
	}
#endif

	return 1;
}

//波束控制
void Data_Process::Beam_Ctrl(struct TrackingBeam    *Tracking_beam,
                            struct Track           Trust_Track_Output[][10],
                            int *Trust_track_num_Output)
{

	tas_ctrl.tas_ctrl_process(&trust_track,Tracking_beam, Trust_Track_Output, Trust_track_num_Output,Work_Parameter, latest_timestamp);
}

//数据处理参数初始化
int Data_Process::track_process_parameters_initial(struct RadarPara Radar_Parameter)
{

	//工作参数设置
	if (Radar_Parameter.track_start_point_num < 3 || Radar_Parameter.track_start_point_num > 9)
		return -1;
	memcpy(&Work_Parameter, &Radar_Parameter, sizeof(RadarPara));

	return 0;
}

//数据处理参数修改
int Data_Process::track_process_parameters_modify(struct RadarPara Radar_Parameter)
{
	if (Radar_Parameter.track_start_point_num < 3 || Radar_Parameter.track_start_point_num > 9)
		return -1;
	memcpy(&Work_Parameter, &Radar_Parameter, sizeof(RadarPara));
	return 0;
}

//航迹清空函数
int Data_Process::track_clear_all(void)
{
	//清空可靠航迹
	Data_buffer.clear();
	Data_buffer_tas.clear();
	point_recv.clear();
	point_recv_tas.clear();
	trust_track.clear();
	temp_track.clear();

	// 同步清理内部缓存和状态，避免重连后输出幽灵航迹/幽灵波束
	dot_coh.reset();
	track_asso.reset();
	track_asso_tas.reset();
	track_init.reset();
	tas_ctrl.reset();
	last_beam_num = INT_MAX;

	return 0;
}

//手动航迹删除函数
int Data_Process:: track_delete(int delete_track_num,                   //手动删除的航迹数目
                                int delete_track_index[])               //手动删除的航迹号
{
	for (int i=0;i<delete_track_num ;i++)
	{
		for (size_t j=0;j<trust_track.size();j++)
		{
			if(trust_track[j].Track_Index == delete_track_index[i])
			{

				trust_track[j].manual_delete_flag=1;
				// RDP_LOG << "Delete :" << trust_track[j].Track_Index << std::endl;
			}
		}

	}

	return 0;
}

//手动添加重点目标
int Data_Process::primary_target_add(int track_index)
{

	for ( size_t i=0; i<trust_track.size();i++){
		if(trust_track[i].Track_Index==track_index)
		{
			trust_track[i].primary_target_flag = 1;
			//std::cout<< "Add primary target :" << trust_track[i].Track_Index << std::endl;
			break;
		}
	}

	return 0;
}

//手动取消重点目标
int Data_Process::primary_target_cancel(int track_index)
{

	for ( size_t i=0; i<trust_track.size();i++){
		if(trust_track[i].Track_Index==track_index)
		{
			trust_track[i].primary_target_flag = 0;
			//std::cout<< "Cancel primary target :" << trust_track[i].Track_Index << std::endl;
			break;
		}
	}

	return 0;
}

//手动转TAS跟踪函数
int Data_Process:: tracking_start(int track_index)              //需要手动转入TAS跟踪的航迹号
{

	for ( size_t i=0; i<trust_track.size();i++){
		if(trust_track[i].Track_Index==track_index)
		{
			trust_track[i].manual_tracking_flag = 1;
			break;
		}
	}

	return 0;
}

//手动取消TAS跟踪函数
int Data_Process:: tracking_stop(int track_index)              //需要手动转入TAS跟踪的航迹号
{

	for ( size_t i=0; i<trust_track.size();i++){
		if(trust_track[i].Track_Index==track_index)
		{
			trust_track[i].manual_tracking_flag = 0;
			trust_track[i].Track_Mode = 0;
			break;
		}
	}

	return 0;
}

//手动打跟踪波束
int  Data_Process::tracking_point(float Azimuth)//方位角（当前接口未启用，保留空实现）
{
	return 0;
}