server/classdef.cpp

#include "stdafx.h"
#include <math.h>
#include <algorithm>

#include "classdef.h"
#include "constdef.h"
#include "structdef.h"
#include "locate_algorithm.h"
#include "./system_basic_info/SystemAnalysis.h"
#include "ProcessRemodule.h"
#include "log_process_module.h"
#include "Functions/Functions.h"
#include "algorithm/Fit.h"
#include "./mylog/log_module.h"
#include <boost/make_shared.hpp>
#include "../Common/matrix/_Matrix.h"
#include "algorithm/base/const_data.h"

#pragma warning(disable: 4244)

using namespace std;

unsigned int g_nAlgoFailedCounts[ALGO_LOC_TOTAL] = {0};
unsigned int g_nAlgoFailedCycleCounts[ALGO_LOC_TOTAL] = {0};
DWORD g_ullCurTime = 0;
unsigned long long m_Start_time = time(NULL)*1000;

Card::Card( string cardid, int cardtype, double z_offset)
	:m_bigCarFlag(false),m_overSpeed(0),m_overAreaSpeed(0),m_isWarning(false),m_minDistanceToWarningpoint(DBL_MAX),m_warning_threshold(DBL_MAX),m_CardHasBeenInDrivingfaceAlarm(false),
	m_DrivingfaceAlarmFlag(-1),m_drivingface_index(0),m_drivingface_index_del(0),m_warning_point_alarm(false),m_disToWarningPoint(0),m_wp_id(0),m_workface_index(0),m_workface_index_del(0),shift_id(0),m_indexForRcvOneReader(0),m_indexCardSpeed(0),need_display(1),speed_limit_after_reset(0),final_v(0)
	,m_area_changed(false),m_oldv(0),power_alarm_display(1),power_change_flag(false),status_help_cancel(0),m_ct_time(0)
{
	time_center_stamp = 0;
	m_warning_point_id = -1;
	BOOL bRet = InitializeCriticalSectionAndSpinCount(&m_csCard, MAXCRITICALSECTIONSPINCOUNT);
	is_drivingface_Initialization=false;
	card_type = cardtype;
	card_id = cardid;
	this->z_offset = z_offset;
	x =	y = z = stored_x = stored_y = stored_z = INVALID_COORDINATE;

	last_correct_position_info_time = time(NULL);
	att_start_time_latest = att_end_time_latest = time(NULL);
	time_low_power = time(NULL);

	map_scale = 1.0;
	dept_id = group_id = occ_id = level_id = worktype_id = vehice_type_id = 0;
	power_state = power_state_last = 0;

	id = 0;
	driver_id = "";
	shift_type_id = 1; 

	is_registered = true;
	is_deal_by_algo = false;
	pTofReaderPathMap = NULL;
	pTdoaReaderPathMap = NULL;
	m_syncNumList.clear();

	this->driving_face_cur_shift_start_x = this->driving_face_cur_shift_start_y = this->driving_face_cur_shift_start_z= this->driving_face_cur_shift_x= this->driving_face_cur_shift_y=this->driving_face_cur_shift_z = 0;

	m_bUseFilter = false;
	m_nFilterType = NO_FILTER;
	m_pKalmanFilter = nullptr;

	ins_weight = INS_WEIGHT;
	uwb_weight = UWB_WEIGHT;

	m_nLightGroupId = -1;	//默认为没控制灯组
	light_id = 0;
	m_nOutputPosState = OUTPUT_POS::RIGHT_POS;

	m_nStream = INIT_STREAM;

	for(int i = 0; i < CARD_EVENT_COUNT; i++){
		m_event_list[i] = 0;
	}

	is_fit_pos = false;
	is_over_interval = false;

	call_type = 0;
	rec_time = time(NULL);
	last_correct_position_info_time = time(NULL);

	cur_ins_pos = std::shared_ptr<POS>();
	m_curshift_finish_length = 0;
	m_cur_total_finish_length = 0;
	ins_request_check = YES_INS_REQUEST;
	ins_gyp_angle = -1;
	ins_gyp_pos = nullptr;
	ins_cal_type = -1;
	ins_request_check = -1;

	cal_location3 = new CalLocation3(KALMAN_OFFSET_COOR_TRI, KALMAN_OFFSET_MOVE_TRI, KALMAN_OFFSET_RANGING_TRI, KALMAN_INTERVAL_TRI);
	p3_anchors = new Point3[ANCHOR_COUNT];
	cal_location = new CalLocation(KALMAN_OFFSET_COOR, KALMAN_OFFSET_MOVE, KALMAN_OFFSET_RANGING, KALMAN_INTERVAL);
	p2_anchors = new Point2[3];
	is_anchor_changed = false;
	is_init_kalman = false;

	ins_req_status_algo = -1;
	is_send_ins_check = false;
	is_del_data = false;

	this->resetKalmanInfo();
	pTaylorWls = std::make_shared<Tdoa::TaylorWls>();

	pSensorManager = nullptr;
	pAcceSensor = nullptr;
	pGyroscopeSensor = nullptr;

	pSensorManager = std::make_shared<algorithm::base::SensorManager>();
	if (pSensorManager!= nullptr)
	{
		pAcceSensor = pSensorManager->get_sensor(algorithm::base::SensorType::Sensor_Accelerometer);
		pGyroscopeSensor = pSensorManager->get_sensor(algorithm::base::SensorType::Sensor_Gyroscope);
	}

	is_use_acc_filter = false;

	this->resetAllBaseInfo();
	this->reset();

	mp_person_in_vechicle_list = std::make_shared<CardMap>();
	recevie_by_moving_reader = 0;
	person_on_card = 0;				//默认为下车状态
	min_distance = 99999.9;
	possible_vehicle_id = "";
	relative_speed = 0.0;
	satisfy_off_vehicle_counts = satisfy_on_vehicle_counts = 0;
	package_is_over = false;
	no_need_add_dist = false;
	recv_signal_power = 0.0;
	pos_state_count = 0;
	pos_state_confirm_times = 1;
	m_cb_state.set_capacity(10); // 设置10个开关机标志位，10个中有7个为1则为开机，有7个为0则为关机
	for (int index = 0; index < 10; index++)// 循环队列初始化
	{
		m_cb_state.push_back(0);
	}
	m_cb_position.resize(0);	//设置60个
	m_b_first_record = true;	
	m_startup_time = 0;
	m_bOpen = false;
	m_nCardId = 0; 
	m_b_first_calc = true;
	m_base_position.set(0,0);
	m_lastrecv_time = 0;	
	m_record_cycle_start_time = time(NULL);
	m_record_avg_dist = 0;
	m_record_max_dist = 0;
	m_record_min_dist = 999999.9;
	m_record_totals = 0;
	m_fScheduleStartupTime = 0.0;
	m_dbTimeInterval = 0;
	m_indexCardSpeed=m_indexCardSpeed_normal=m_indexMineOverSpeed=m_indexMineOverSpeed_normal=0;
	is_mine_over_speed = is_area_over_speed = false;
	m_arg=0x12345678;m_last_time=0;
}

void Card::resetAllBaseInfo()
{
	m_dbTimeInterval = 0;
	::GetLocalTime(&deal_time);
	::GetLocalTime(&enter_area_time);
	down_time = up_time  = enter_reader_time = rec_time = time(NULL);
	time_over_time = time_area_over_time = time_area_forbidden = time_over_speed = time(NULL);
	last_locate_time = time(NULL);
	//坐标初始值为0
	x =	y = z = stored_x = stored_y = stored_z = INVALID_COORDINATE;
	m_indexCardSpeed=m_indexCardSpeed_normal=m_indexMineOverSpeed=m_indexMineOverSpeed_normal=0;
	output_x = output_y = 0;
	a = 0;
	t = 0;
	init_postion = false;
	b_pos_change = false;
	n_pos_no_change_times = 0;
	is_idling = false;
	idle_count = 0;
	is_driving_face_start = 0;
	right_x = right_y = right_z = left_x = left_y = left_z = 0;
	m_nMoveDirection = 0;
	is_mine_over_speed = is_area_over_speed = false;
	map_id = map_id_old = area_id = reader_id = 0;
	state = 0;
	state_moving = 0;
	state_meeting = 0;
	bInSpecailArea = false;
	state_biz = 0;
	pos_state = pos_state_old = PDT_INIT;
	pos_state_count = 0;
	pos_state_confirm_times = 1;

	status_help = status_area_over_time = status_area_forbidden = status_area_over_speed = 0;
	status_over_speed = status_over_time = status_power = status_lost = status_call = 0;

	sync_num = 0;

	isdealed = isreceive = is_hist = false;
	is_area_over_time = is_mine_over_time = false;

	reader_tickcount = time_stamp;
	time_stamp_last = 0;

	time_stamp_max = 0;
	is_deal_by_algo = false;
	m_syncNumList.clear();

	m_nCalcSyncNum = 0;
	m_nSyncNumInList = 0;
	last_locate.tt = 0;
	last_locate.a = last_locate.antenna_id = last_locate.reader_id = last_locate.t = 0;
	last_locate.d = last_locate.d_offset = last_locate.v = 0.0;
	last_locate.x = INVALID_COORDINATE;
	last_locate.y = INVALID_COORDINATE;
	last_locate.z = INVALID_COORDINATE;
	error_locate.x = error_locate.y = error_locate.z = 0;
	last_x = last_y = last_z = INVALID_COORDINATE;

	locate = nullptr;

	m_nLastLocateT = 0;
	origin_locate.x = origin_locate.y = origin_locate.z = origin_locate.v = 0.0;
	last_vx = last_vy = 0.0;
	v = 0.0;
	acce_cur_state = 0;
	acce_last_state = 0;
	b_long_interval = false;
	accelerate_state = accelerate_state_last = ACCELERATE_INIT_STATE;	//默认初始的为-10
	ins_direction = 0;
	antenna_angle = 0;

	b_enter_intersection = false;
	is_red_light = false;
	time_red_light = time(NULL);
	m_nIsRailroad = 0;
	mileage = 0;
	diff_direction_counts = 0;

	p_reader = nullptr;

	_dists.swap(DistQueMap());
	vt_his_speed.resize(0);
	his_pos.resize(0);
	is_ref_pos = false;
	cur_fit_nums = 0;
	have_fit_pos = false;
	have_long_fit_pos = false;
	count_idle = 0;
	count_change_direction = 0;

	call_type_id = 0;
	vt_deal_call_reader_id.resize(0);
	recv_call_time = time(NULL);
	final_v = 0;

	landmark_id = 0;
	landmark_dis = 0;
	landmark_direction = NODIRECTORY;

	drive_face_location_write_time = 0;
	idle_pos_list.resize(0);
	time_last_tof = time(NULL);
	time_cur_algo = time(NULL);

	m_drivingface_start_x = 0;
	m_drivingface_start_y = 0;
	m_drivingface_start_z = 0;
	m_drivingface_start_time.clear();
	m_arg=0x12345678;m_last_time=0;

}
void Card::make_arg()
{
	NAMESPACE_POINT::point p1(x,y);NAMESPACE_POINT::point p2(last_x,last_y);
	if (p1.invalid() || p2.invalid())
		return;
	if (p1.dist(p2)<0.5)
		return;

	if ( m_ct_time-m_last_time<5*1000)
	{
		m_arg = std::arg(std::complex<double>(x,y)-std::complex<double>(last_x,last_y));
		NAMESPACE_POINT::line_v l(NAMESPACE_POINT::point(last_x,last_y),NAMESPACE_POINT::point(x,y));
		m_line=l;
		debug_print_syslog(0,"[vehicle_alarm_arg ] %s,arg:%f (%.2f,%.2f)--(%.2f,%.2f)",card_id.c_str(),m_arg,last_x,last_y,x,y);
	}

}
/*
* 重置算法参数：当信号消失，重置算法相关的参数
*
* param
*		无
*
* return
*		无
*
*/
void Card::reset()
{
	//以下参数与算法相关
	//坐标初始值为0
	m_nLastLocateT = 0;
	time_stamp_cal_last = 0;
	b_long_interval = false;
	is_first_location = true;
	last_locate.sync_num = 0;
	is_algo_first_location = true;

	while (_dists.size()>0)
	{
		//TRACE(_T("33333"));
		_dists.pop_front();
	}

	if (m_syncNumList.size() > 0)
	{
		m_syncNumList.erase(m_syncNumList.begin(),m_syncNumList.end());
		m_syncNumList.resize(0);
	}

	if (vt_his_speed.size()>0)
	{
		vt_his_speed.erase(vt_his_speed.begin(),vt_his_speed.end());
		vt_his_speed.resize(0);
	}

	if (his_pos.size() > 0)
	{
		his_pos.erase(his_pos.begin(),his_pos.end());
		his_pos.resize(0);
		have_fit_pos = false;
	}

	if (long_his_pos.size() > 0)
	{
		long_his_pos.erase(long_his_pos.begin(),long_his_pos.end());
		long_his_pos.resize(0);
		have_long_fit_pos = false;
		count_idle = 0;
	}

	cur_fit_nums = 0;
	count_idle = 0;

	locate = nullptr;

	//卡尔曼参数重置
	if (m_pKalmanFilter!=nullptr)
	{
		m_pKalmanFilter->m_nCounts = 0;
		m_pKalmanFilter->Initial(0.2);
	}

	//tof相关数据重置
	time_last_tof = time(NULL);

	this->pGyroscopeSensor->SetValue(0.0);
	this->pAcceSensor->SetValue(0.0);

	pos_state = PDT_INIT;
	pos_state_old = PDT_DOWN;
	pos_state_count = 0;
	pos_state_confirm_times = 1;
}

void Card::set_reader(std::shared_ptr<Reader> preader) // 设置卡时间
{	
	this->rec_time = time(NULL);

	if(preader->reader_id != this->reader_id){ // 所在分站没有发生变化
		this->p_reader = preader.get();
		this->reader_id = preader->reader_id;
		this->enter_reader_time = this->rec_time;

		this->map_id_old = this->map_id;
		this->map_id = preader->map_id;
		this->map_scale = preader->map_scale;
	}
} 

/*
* 设置地图集覆盖范围，适用TOF
*
* param
*		trpm		tdoa地图集
*
* return
*		无
*
*/
void Card::set_reader_path_tof(std::shared_ptr<ReaderPathMap> rpm)
{
	this->pTofReaderPathMap = rpm;
}

/*
* 设置地图集覆盖范围，适用TDOA
*
* param
*		trpm		tdoa地图集
*
* return
*		无
*
*/
void Card::set_reader_path_tdoa(std::shared_ptr<TDOAReaderPathMap> trpm)
{
	this->pTdoaReaderPathMap = trpm;
}

bool operator==(sync_data& a,sync_data&b){
	return a.sync_num == b.sync_num;
}

/*
* 此函数主要判断此坐标是否需要形成json输出给webserver
*
* param
*		acce_state		算法中保存的当前加速度计状态
*		acce_state_last	算法中保存的上一次加速度计状态
*
* return
*		无
*/
int Card::inspect_coordinate(int acce_state)
{
	this->isoutput = false;

	char log[200] = {0};

	//如果是静止或者怠速状态，每次的定位坐标不变化，此时要求采集不再将此数据送往webserver
	if (STATE_ACCE_STATIC == acce_state || STATE_ACCE_IDLING == acce_state)
	{
		this->isoutput = false;
	}
	else
	{
		if (this->x == this->output_x && this->y == this->output_y) //last_x,last_y
		{
			count_idle++;
			this->isoutput = false;
		}else{
			//如果运动状态时，
			int nSign = 0;
			int nState = 0;

			if (CheckStreamUniformity(this->output_x,this->output_y,this->x,this->y,this->m_nStream))
			{
				double distance = sqrt(pow(this->x - this->output_x,2) + pow(this->y - this->output_y,2)) * this->map_scale;
				if(distance > this->map_scale) //this->map_scale)
				{
					//两点之间的距离是否大于一个0.5米，满足条件，输出json
					this->isoutput = true;
					this->output_x = this->x;
					this->output_y = this->y;
					count_idle = 0;
				}else{
					count_idle++;
				}				
			}  
			else
			{
				if( count_change_direction >= MAX_REBOUND_COUNTS){
					// 认为掉头，输出给web
					this->isoutput = true;
					this->output_x = this->x;
					this->output_y = this->y;
					count_idle = 0;
					count_change_direction = 0;
				}else{
					// 认为是抖动，不输出给web
					count_idle++;
				}
			}
		}
	}	

	return 0;
}

/*
* 检查当前定位坐标和nStream方向是否一致
*
* param
*		x1,y1		起点的坐标
*		x2,y2		终点的坐标
*		nStream		上一次的方向
*
* return
*		如果算出的方向和nStream一致，则返回true；否则返回false
*
*/
bool Card::CheckStreamUniformity(double x1,double y1,double x2,double y2,int nStream)
{
	return nStream == GetStream(x1,y1,x2,y2);
}

/*
* TOF定位算法
*
* param
*		cnt		数据条数
*
* return
*		无
*
*/
void Card::algo_tof(int cnt)
{
}

/*
* TOF一维定位算法
* 
* param
*		cnt		数据条数
*
* return
*		成功返回0，否则返回非零值
*
*/
int Card::algo_tof_1d(int cnt)
{
	if (LOCATEDATATYPE::LDT_TOF == ranging_type)
	{
		if (cnt < 1)
		{
			return 1;
		}
	} 
	else
	{
		if (cnt < 2)
		{
			return 2;
		}
	}

	is_del_data = true;
	int nRet = 0;
	//组装数据部分
	map<unsigned long long, std::shared_ptr<_coordinate>> tmp_dists_locate;
	nRet = AssembleDistData(tmp_dists_locate);
	if (nRet)
	{
		return 3;
	}

	map<unsigned long long, std::shared_ptr<_coordinate>>::iterator it = tmp_dists_locate.begin();
	time_stamp_cal = it->second->t;

	double dMaxDistance = 1000.0;
	int key = 0;
	//修正由于高度所引入的误差
	for (map<unsigned long long, std::shared_ptr<_coordinate>>::iterator it_tmp = tmp_dists_locate.begin();it_tmp != tmp_dists_locate.end();++it_tmp)
	{
		double distance = 0.0;
		/*if(it_tmp->second->d < TOF_HEIGHT_ERROR)
		{
		continue;
		}*/
#ifdef _DEBUG
		distance = sqrt(pow(it_tmp->second->d,2) - pow(TOF_HEIGHT_ERROR,2));//TOF_HEIGHT_ERROR
#else
		distance = sqrt(pow(it_tmp->second->d,2) - pow(TOF_HEIGHT_ERROR,2));
#endif
		it_tmp->second->d = distance;
		if (distance < dMaxDistance)
		{
			key = it_tmp->first;
			dMaxDistance = distance;
		}
	}

	//获取进算法的时间参与运算
	TOF_REFER_DATA refer_data;
	get_ref_data_from_dists(tmp_dists_locate,key,refer_data);
	std::shared_ptr<POS> p = nullptr;
	if (tmp_dists_locate.size() < 2)
	{
		if (tmp_dists_locate.size() == 1)
		{
			if (last_locate.x == INVALID_COORDINATE && last_locate.y == INVALID_COORDINATE)
			{
				return 4;
			}
			//如果当前计算的数据是单条数据，且卡的ct序号相差15以上，则此单条数据不参与计算
			if (time_stamp_cal - time_stamp_cal_last > 15)
			{
				return 5;
			}
			//根据一条距离来进行测试
			if (p == nullptr)
			{
				p = std::make_shared<POS>();
			}

			int nRet = 0;
			nRet = get_position_by_one_distance(pTofReaderPathMap,p,refer_data);
			if (nRet)
			{
				return 6;
			}
		}else{
			return 7;
		}
	}else{
		//如果数据条数大于2，走如下逻辑
		//数据条数大于2，会存在如下异常：即t时刻，天线距离 a<b,t+1时刻，天线距离 a>b 导致无法定位
		std::shared_ptr<POS> tmpPos = std::make_shared<POS>();

		bool isExist = false;
		int nReaderId = 0;			//分站id
		int nAntennaIndex = 0;		//天线索引
		double dist = 0.0;			//距离

		//保存所有定位结果
		std::vector<std::shared_ptr<POS>> udm_pos;
		udm_pos.resize(0);

		for(it = tmp_dists_locate.begin();it != tmp_dists_locate.end();++it){
			nReaderId = it->second->reader_id;
			nAntennaIndex = it->second->antenna_id;
			dist = it->second->d;

			//如果为参考分站，则不计算，直接寻找下一个数据
			if (nReaderId == refer_data.nReaderId && nAntennaIndex == refer_data.nAntennaIndex && abs(dist - refer_data.dDistance) < ZERO_PRECISION)
			{
				continue;
			}

			//计算定位结果
			p = LocateAlgorithm::tof_locate_1d(pTofReaderPathMap,nReaderId,nAntennaIndex,dist,refer_data);

			if(p == nullptr || !CheckPositionValid(p))
			{
				continue;
			}
			isExist = true;
			udm_pos.push_back(p);
		}
		//如果不存在解，直接返回
		if (!isExist)
		{
			return 8;
		}else{
			double distance = 0.0;
			//求平均之前，对所有解过滤一次
			for (std::vector<std::shared_ptr<POS>>::iterator it = udm_pos.begin();it != udm_pos.end();)
			{
				//每个点都计算与参考点的距离，如果过大则抛弃
				distance = sqrt(pow((*it)->posx - refer_data.x*map_scale,2) + pow((*it)->posy - refer_data.y*map_scale,2));
				if (fabs(distance - refer_data.dDistance) > 10)
				{
					it = udm_pos.erase(it);
				}else{
					++it;
				}
			}

			//对多解求平均
			double posx = 0;
			double posy = 0;
			double posz = 0;

			//选取误差最小的解
			double range = 999999.0;
			for (std::vector<std::shared_ptr<POS>>::iterator it_pos = udm_pos.begin();it_pos != udm_pos.end();++it_pos)
			{
				if((*it_pos)->pos_radius < range){
					range = (*it_pos)->pos_radius;
					posx = (*it_pos)->posx;
					posy = (*it_pos)->posy;
					posz = (*it_pos)->posz;
				}
			}

			p->posx = posx;
			p->posy = posy;
			p->posz = posz;

			if (!is_algo_first_location)
			{
				double dt = 0.0;
				if (fabs(GetDeltaT() - 0) < ZERO_PRECISION)
				{
					dt = 2;
				}else{
					dt = GetDeltaT();
				}
				p->cvx = fabs(p->posx - last_locate.x*map_scale)/dt;
				p->cvy = fabs(p->posx - last_locate.x*map_scale)/dt;
			}

			p->cx = p->posx/map_scale;
			p->cy = p->posy/map_scale;
			p->cz = p->posz/map_scale;
		}
	}

	//通过将定位结果与某个分站两天线的测距方向进行比对，如果有异常，则抛弃，都符合则保留
	if(position_data_filter_by_direction(p,tmp_dists_locate)){
		return 9;
	}

	//求完解，对解做加速度检验处理
	//if (!CheckSolutionTof(p))
	{
		//通过加速度被抛弃，利用参考数据来做圆
		//return 10;
	}
	//optimization_pos(p);

	//最后保存
	if (CheckPositionValid(p))
	{
		SaveCardAlgoData(p);
	}

	return 0;
}

int Card::algo_tof_2d(int cnt)
{
	//二维定位
	//map<unsigned long long, std::shared_ptr<_coordinate>> tmp_dists_locate;
	//AssembleDistData(tmp_dists_locate);

	//for (map<unsigned long long, std::shared_ptr<_coordinate>>::iterator it = tmp_dists_locate.begin();it != tmp_dists_locate.end();++it)
	//{
	//	x(i) = it->second->x / map_scale;
	//	y(i) = it->second->y / map_scale;
	//	d(i) = it->second->d;

	//	i++;
	//	if(i > 2)
	//		break;
	//}
	//
	//h = 0;
	//for (int i = 0;i < 3;i++)
	//{
	//	pos(i,0) = x(i);
	//	pos(i,1) = y(i);
	//	d(i) = sqrt(pow(d(0),2) - pow(h,2));
	//}

	//Eigen::Matrix2d A(2,2);
	//Eigen::Matrix2d B(2,2);
	//
	//for (int i = 0; i < 2;i++)
	//{
	//	for (int j = 0; j < 2;j++)
	//	{
	//		temp = pos(i+1,j) - pos(i,j);
	//		A(i,j) = temp;
	//	}
	//}

	//for (int i = 0;i < 2;i++)
	//{
	//	temp = pow(pos(i+1,0),2);
	//	temp += pow(pos(i+1,1),2);
	//	temp -= pow(pos(i,0),2);
	//	temp -= pow(pos(i,1),2);
	//	temp -= (pow(d(i+1,i+1),2) - pow(d(i,1),2));
	//	temp /=2;
	//	B(i,0) = temp;
	//}

	////行列式
	////double det = A.determinant();
	////Eigen::Vector2d C;
	//Eigen::Matrix2d C;
	//C = A.inverse()*B;
	//this->x = C(0);
	//this->y = C(1);
	//逆矩阵
	//A.inverse();
	//转置
	//A.transpose();

	map<unsigned long long, std::shared_ptr<_coordinate>> tmp_dists_locate;
	AssembleDistData(tmp_dists_locate);

	map<unsigned long long, std::shared_ptr<_coordinate>>::iterator it = tmp_dists_locate.begin();
	time_stamp_cal = it->second->t;

	std::shared_ptr<ReceiveDataTofVector> pRdtv = std::make_shared<ReceiveDataTofVector>();
	for (auto it = tmp_dists_locate.begin();it != tmp_dists_locate.end();++it)
	{
		std::shared_ptr<ReceiveDataTof> rdt = std::make_shared<ReceiveDataTof>();
		rdt->x = it->second->x;
		rdt->y = it->second->y;
		rdt->z = it->second->z;
		rdt->reader_id = it->second->reader_id;
		rdt->distance = it->second->d;
		//rdt->distance = sqrt(pow(it->second->d,2)-pow(1.5,2));
		rdt->antenna_id = it->second->antenna_id;

		pRdtv->push_back(rdt);
	}

	std::shared_ptr<algorithm::POS> sp_pos = LocateAlgorithm::tof_locate_2d(pRdtv);

	sp_pos->cx = sp_pos->posx*map_scale;
	sp_pos->cy = sp_pos->posy*map_scale;

	//求完解，对解做加速度检验处理
	//if (!CheckSolutionTof(sp_pos))
	//{
	//	//通过加速度被抛弃
	//	return 10;
	//}

	//最后保存
	if (CheckPositionValid(sp_pos))
	{
		SaveCardAlgoData(sp_pos);
	}

	return 0;
}

int Card::algo_tof_3d(int cnt)
{
	//三维定位
	/*TRACE(_T("data counts : %d \r\n"),cnt);
	if (cnt < 3)
	{
	return 1;
	}
	double distances[4];

	map<unsigned long long, std::shared_ptr<_coordinate>> tmp_dists_locate;
	AssembleDistData(tmp_dists_locate);

	int i = 0;
	for (map<unsigned long long, std::shared_ptr<_coordinate>>::iterator it = tmp_dists_locate.begin();it != tmp_dists_locate.end();++it)
	{
	distances[i] = it->second->d;
	i++;
	}
	Point3 pt3,pt3_1;
	Mat mat(4,1,distances);
	set_anchors(tmp_dists_locate);
	if (is_anchor_changed)
	{
	pt3 = cal_location3->initPos(mat);
	} 
	else
	{
	pt3 = cal_location3->estiPos(mat);
	}
	pt3_1 = cal_location3->getError();

	x = pt3.x;
	y = pt3.y;
	z = pt3.z;*/

	return 0;
}

/*
* TDOA算法
*
* param
*		cnt		数据条数
*
* return
*		无
*
*/
void Card::algo_tdoa(int cnt)
{	
	LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_140);
	GetLocalTime(&m_afmData.st);

	if(cnt < 2){
		LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_141);
		m_afmData.bStatus = true;
		m_afmData.strCardId = this->card_id;
		m_afmData.nCardStamp = _dists.front().cardstamp;
		m_afmData.nType = ALGO_FAILED_CONDITION_15;
		ALGORITHM_FAILED(ALGO_FAILED_CONDITION_15);
		return;
	}

	//主要处理当相同卡的时间戳的数据中存在同步序号大于5的情况，如果有大于5的数据则丢弃此数据
	int k = 0;
	int dst = 0;
	int st = 0;
	bool bRet = false;

	mp_dists_locate.clear();
	for(DistMap::iterator it = _dists.front().distmap.begin(); it != _dists.front().distmap.end(); ++it,k++){	
		if(k==0){
			st = it->second->sync_num;
		}else{
			dst = abs(st - it->second->sync_num);
			if(dst >= 5){
				LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_142);
				bRet = true;
				//此处需要输出计数
				break;
			}
		}
		mp_dists_locate.insert(make_pair(it->second->tt,it->second));
	}

	if(bRet){
		LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_143);
		this->x = this->last_locate.x;
		this->y = this->last_locate.y;
		this->z = 0;
		//this->z = INVALID_COORDINATE;
		m_afmData.bStatus = true;
		m_afmData.strCardId = this->card_id;
		m_afmData.nCardStamp = _dists.front().cardstamp;
		m_afmData.nType = ALGO_FAILED_CONDITION_2;
		ALGORITHM_FAILED(ALGO_FAILED_CONDITION_2);
		return;
	}

	std::shared_ptr<ReceiveDataMap> pRdm(new ReceiveDataMap);
	pRdm->clear();

	bRet = false;

	int maxSyncTimes = 0;
	//保存加速度当前状态和上次状态
	int acce_state = 0;
	int acce_state_last = 0;
	int ins_direction = 0;
	int card_time_stamp = 0;

	map<unsigned long long,std::shared_ptr<_coordinate>>::iterator it_mpdl = mp_dists_locate.begin();
	int i = 0;
	for(;it_mpdl!=mp_dists_locate.end();++it_mpdl){
		if(i==0){
			card_time_stamp = it_mpdl->second->t;
			maxSyncTimes = it_mpdl->second->sync_num;
			acce_state = it_mpdl->second->acce_state;
			acce_state_last = it_mpdl->second->acce_state_last;
			ins_direction = it_mpdl->second->ins_direction;
		}
		else{
			if(maxSyncTimes < it_mpdl->second->sync_num){
				maxSyncTimes = it_mpdl->second->sync_num;
				acce_state = it_mpdl->second->acce_state;
				acce_state_last = it_mpdl->second->acce_state_last;
				ins_direction = it_mpdl->second->ins_direction;
			}
		}
		ReceiveDataMap::iterator prdm_it = pRdm->find(it_mpdl->second->tt);
		if(prdm_it == pRdm->end()){
			if(it_mpdl->second->tt == LLONG_MAX ){
				//如果同步时间戳存在异常值，则不走算法定位，直接返回上一次结果值
				bRet = true;
				//此处需要输出计数
				LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_144);
				break;
			}
			//保存信息用于定位
			std::shared_ptr<ReceiveData> prd(new ReceiveData());
			prd->reader_id = it_mpdl->second->reader_id;
			prd->antenna_id = it_mpdl->second->antenna_id;
			prd->rec_time_stamp = it_mpdl->second->tt;
			prd->x = it_mpdl->second->x*this->map_scale;
			prd->y = it_mpdl->second->y*this->map_scale;
			prd->z = it_mpdl->second->z*this->map_scale;
			prd->special = it_mpdl->second->special;
			pRdm->insert(make_pair(prd->rec_time_stamp,prd));
		}	
		i++;
	}

	//存在异常值，直接返回上一次结果值
	if(bRet){
		LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_145);
		pRdm->clear();
		this->x = this->last_locate.x;
		this->y = this->last_locate.y;
		this->z = 0;
		//this->z = INVALID_COORDINATE;
		m_afmData.bStatus = true;
		m_afmData.strCardId = this->card_id;
		m_afmData.nCardStamp = it_mpdl->first;
		m_afmData.nType = ALGO_FAILED_CONDITION_3;
		ALGORITHM_FAILED(ALGO_FAILED_CONDITION_3);
		return;
	}

	//当卡需要新定位的计数序列号大于上一次成功定位的序列号，则此次不定位，避免定位结果的回退
	if (this->time_stamp_cal_last > this->time_stamp_cal)
	{
		if ((this->time_stamp_cal - this->time_stamp_last) < MAX_SYNCTIME_DELAY_NUM)//120
		{
			//满足此条件，表名标示卡的计数序号走完了65535的周期
		}
		else{
			this->x = this->last_locate.x;
			this->y = this->last_locate.y;
			//this->z = INVALID_COORDINATE;
			this->z = 0;
			m_afmData.bStatus = true;
			m_afmData.strCardId = this->card_id;
			m_afmData.nCardStamp = this->time_stamp_cal;
			m_afmData.nType = ALGO_FAILED_CONDITION_4;
			ALGORITHM_FAILED(ALGO_FAILED_CONDITION_4);
			//此处需要输出计数
			return;
		}
	}

	this->time_stamp_cal_last = this->time_stamp_cal;

	int nCount = 0;
	//std::unique_ptr<POS> p = nullptr;
	std::shared_ptr<POS> p = nullptr;

	bool bOutput = false;
	this->z = 0;
	i = 0;

	if(pTdoaReaderPathMap->size() > 0 && pRdm->size() > 1){
		LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_147);
		this->m_nCalcSyncNum = maxSyncTimes;
		p = LocateAlgorithm::Pos(pRdm,pTdoaReaderPathMap);

		this->origin_locate.x = p->posx / (this->map_scale*1.0);
		this->origin_locate.y = p->posy / (this->map_scale*1.0);
		this->origin_locate.z = p->posz / (this->map_scale*1.0);

		//校验机会3次，3次内跳回去就丢弃此次计算结果
		if (p->posx != INVALID_COORDINATE && p->posy != INVALID_COORDINATE)
		{
			int nDirection = 0;
			if (!algo_is_same_direction(this->origin_locate.x,this->origin_locate.y,0))
			{
				this->x = this->last_locate.x;
				this->y = this->last_locate.y;

				return;
			}
		}

		double interval_time = this->p_reader->reader_interval_time;

		double deltaT = 0.0;
		double cvx = 0;
		double cvy = 0;
		double cv  = 0;

		double cx = 0;
		double cy = 0;
		double cz = 0;

		int nSign = 1;

		sync_data sdNew;
		sync_data sd;

		cx = p->posx / (this->map_scale*1.0);
		cy = p->posy / (this->map_scale*1.0);
		cz = p->posz / (this->map_scale*1.0);

#ifdef ALGORITHM_TYPE_INS
		int acce_direction = 0;		//加速度计速度方向	0-静止，1-前进，-1-后退
		int uwb_direction = 0;		//uwb速度方向，同上
		bool bUseKalman = false;
		bool bOriginLocate = false;	//原始定位是否成功
		bool bDirectReturn = false;	//是否直接返回本次定位结果
#endif

		if(p->posx != INVALID_COORDINATE && p->posy != INVALID_COORDINATE){
			LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_148);
#ifdef ALGORITHM_TYPE_INS
			bOriginLocate = true;	//原始定位成功
#endif
			//定位成功
			if(this->m_nLastLocateT == 0){
				LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_149);
				sdNew.sync_num = maxSyncTimes;
				sdNew.x = cx;
				sdNew.y = cy;
				sdNew.vx = 0;
				sdNew.vy = 0;
				sdNew.update = false;
				m_syncNumList.push_back(sdNew);
#ifdef ALGORITHM_TYPE_INS
				uwb_direction = 1;
#endif
			}
			else{
				LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_150);
				//现在的关于同步序号的处理是这样的：
				//如果定位成功，就把这次定位成功的同步数据：同步序号，坐标；x,y方向的速度，扔到一个队列里，
				//后来定位成功的就会先根据同步序号差用加速度抛一次；
				//抛不掉，就用队列里的同步数据（从后往前找），找到第一个与当前同步序号相差大于5的同步数据来进行第二次计算速度以及加速度，
				//如果加速度大于5，就不要此次的定位数据，
				//如果通过加速度判断就将队列中从头开始到此同步数据的所有元素都丢弃，并插入新的此次同步数据

				if (this->b_long_interval)
				{   
					LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_151);
					//此段代码用于将上一次定位是根据两个时间差是个很大值而定位出的结果
					//当后续定位时就和最近的定位结果进行比较
					//例如：当上一次同步序号是14321，它定位时比较的同步序号是14200，时间差大于20多秒
					//这时我们就将b_long_interval置为true
					//当本次定位，同步序号是14326，,这时就需要根据最近的14321进行判断
					list<sync_data>::reverse_iterator it = m_syncNumList.rbegin();
					sync_data sdl = *it;

					//以下计算deltaT还需要考虑卡的同步序号轮回的情况。
					if (maxSyncTimes > it->sync_num)
					{
						LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_152);
						deltaT = (maxSyncTimes - sdl.sync_num)*interval_time;
					} 
					else
					{
						deltaT = (maxSyncTimes + 65536 - sdl.sync_num)*interval_time;
					}
					if (deltaT < 10 && deltaT > 0)
					{
						LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_155);
						this->b_long_interval = false;
					}
					//避免同一个同步序号下存在多个不同卡序号
					if (deltaT < 1E-2)
					{
						LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_156);
						deltaT = 0.2;
					}
					cvx = (cx - sdl.x)*this->map_scale/deltaT;
					cvy = (cy - sdl.y)*this->map_scale/deltaT;

					double avx = (cvx - sdl.vx) / deltaT;
					double avy = (cvy - sdl.vy) / deltaT;
					double av = sqrt(pow(avx,2) + pow(avy,2));
					//车卡的加速度
					switch(this->card_type){
					case CT_PERSON:
						LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_157);
						if(av > PERSON_ACCELERATE_THRESHOLD){
							LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_158);
							this->x = this->last_locate.x;
							this->y = this->last_locate.y;
							this->b_long_interval = false;

							m_afmData.bStatus = true;
							m_afmData.strCardId = this->card_id;
							m_afmData.nType = ALGO_FAILED_CONDITION_5;
							ALGORITHM_FAILED(ALGO_FAILED_CONDITION_5);
							return;
						}
						break;
					case CT_VEHICLE:
						LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_159);
						if(av > VECHILE_ACCELERATE_THRESHOLD){
							LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_160);
							//保留上次结果
							this->x = this->last_locate.x;
							this->y = this->last_locate.y;
							this->b_long_interval = false;

							m_afmData.bStatus = true;
							m_afmData.strCardId = this->card_id;
							m_afmData.nType = ALGO_FAILED_CONDITION_6;
							ALGORITHM_FAILED(ALGO_FAILED_CONDITION_6);
							return;
						}
						break;
					}
					deltaT = 0;
					cvx = cvy = 0;
				}

				//从队列尾部开始查找，找到第一个同步序号与当前计算卡的同步序号相差5个以上的数据
				list<sync_data>::reverse_iterator it;
				bool bOverflow = false;
				for(it = m_syncNumList.rbegin();it!=m_syncNumList.rend();it++){
					if(maxSyncTimes - it->sync_num >= 5){
						LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_161);
						sd = *it;
						break;
					}
					else{
						if(maxSyncTimes - it->sync_num < 0 && maxSyncTimes < 100){
							LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_162);
							//如果最新同步号小于列表中的同步号则
							if(maxSyncTimes + 65536 - it->sync_num >=5 ){//5
								LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_163);
								bOverflow = true;
								sd = *it;
							}
						}else{
							continue;
						}
					}
				}
				//根据溢出条件来计算deltaT
				if(bOverflow){
					LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_164);
					deltaT = (maxSyncTimes + 65536 - sd.sync_num)*interval_time;
				}else{
					LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_165);
					deltaT = (maxSyncTimes - sd.sync_num)*interval_time;
				}

				//速度正负的判断：以x轴，y轴正向运动为正
				//如果x相等，则y2 - y1 > 0为正
				//其他情况，则x2 - x1 > 0 为正
				if(cx == sd.x){
					if(cy > sd.y){
						nSign = 1;
					}else{
						nSign = -1;
					}
				}else{
					if(cx > sd.x){
						nSign = 1;
					}else{
						nSign = -1;
					}
				}
#ifdef ALGORITHM_TYPE_INS
				uwb_direction = (nSign == 1)?1:-1;		//uwb形式判断不出静止或者怠速
#endif

				//使用间隔来修正速度
				if(deltaT - 1.0 >= 0){
					LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_166);
					if (deltaT > 10)
					{
						LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_167);
						this->b_long_interval = true;
					}
					//转为m/s
					cvx = (cx - sd.x)*this->map_scale/deltaT;
					cvy = (cy - sd.y)*this->map_scale/deltaT;

					cv = sqrt(pow(cvx,2) + pow(cvy,2));
					cv = cv*nSign;

					double avx = (cvx - sd.vx) / deltaT;
					double avy = (cvy - sd.vy) / deltaT;
					double av = sqrt(pow(avx,2) + pow(avy,2));
					//车卡的加速度
					switch(this->card_type){
					case CT_PERSON:
						LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_168);
						if(av > PERSON_ACCELERATE_THRESHOLD){
							LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_169);
							this->x = this->last_locate.x;
							this->y = this->last_locate.y;

							m_afmData.bStatus = true;
							m_afmData.strCardId = this->card_id;
							m_afmData.nType = ALGO_FAILED_CONDITION_5;
							ALGORITHM_FAILED(ALGO_FAILED_CONDITION_5);
							return;
						}
						break;
					case CT_VEHICLE:
						LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_170);
						if(av > VECHILE_ACCELERATE_THRESHOLD){
							LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_171);
							//保留上次结果
							this->x = this->last_locate.x;
							this->y = this->last_locate.y;

							m_afmData.bStatus = true;
							m_afmData.strCardId = this->card_id;
							m_afmData.nType = ALGO_FAILED_CONDITION_6;
							ALGORITHM_FAILED(ALGO_FAILED_CONDITION_6);
							return;
						}
						break;
					}
					this->last_locate.acceleration = av;
					this->last_vx = cvx;
					this->last_vy = cvy;	
					cv = cv*3.6;
					LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_172);
					//速度的限制
					if(fabs(cv) > MAX_VECHILE_SPEED){
						LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_173);
						this->x = this->last_locate.x;
						this->y = this->last_locate.y;

						m_afmData.bStatus = true;
						m_afmData.strCardId = this->card_id;
						m_afmData.nType = ALGO_FAILED_CONDITION_7;
						ALGORITHM_FAILED(ALGO_FAILED_CONDITION_7);
						return;
					}

					//删除第一个元素到tmp（含）之间的所有元素
					bool bStartDel = false;
					for(list<sync_data>::reverse_iterator tmp = m_syncNumList.rbegin();tmp != m_syncNumList.rend();)
					{
						if(bStartDel){
							tmp = list<sync_data>::reverse_iterator(m_syncNumList.erase((++tmp).base()));		
						}else{
							if(*tmp == sd){
								bStartDel = true;
							}
							++tmp;
						}					
					}

					//更新值为当前值并插入队列
					sdNew.sync_num = maxSyncTimes;
					this->m_nSyncNumInList = sd.sync_num;
					sdNew.vx = cvx;
					sdNew.vy = cvy;
					sdNew.update = true;
				}else{
					cv = this->origin_locate.v;	
				}
			}	
		}	
#ifdef ALGORITHM_TYPE_INS
		else{
			LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_174);
			uwb_direction = ins_direction;
		}
#endif

#ifdef ALGORITHM_TYPE_INS
		bRet = false;
		//0,1,2分别表示静止(含怠速)，运动，运动中刹车
		switch (acce_state)
		{
		case 0:
			LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_175);
			acce_direction = 0;
			if(acce_state_last!=0){
				LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_176);
				//说明从其他状态变为静止（怠速）状态
				//返回这次定位结果
				if (bOriginLocate)
				{
					LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_177);
					bDirectReturn = true;	//直接返回本次定位结果
				}			
			}else{
				//返回上一次定位结果
				LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_178);
				bDirectReturn = false;
			}
			bUseKalman = false;
			//bRet = true;
			break;
		case 1://需要判断状态是否发生了变化
			LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_179);
			if (acce_state_last == ACCELERATE_INIT_STATE)
			{
				LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_180);
				//第一次获得卡的状态，需要确定首次方向
				acce_direction = uwb_direction;	//将UWB定出的速度方向赋值给它
			}else{
				LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_181);
				acce_direction = acce_state_last;
			}
			//bRet = false;
			break;
		case 2:
		case 3:
			LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_182);
			acce_direction = 0;	//此处将速度方向设为静止，也就是0
			//bRet = false;
			break;
		}

		if (acce_state!=acce_last_state)
		{
			//	如果状态发生了改变，则加速度计的权重需要重置
			LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_183);
			this->ins_weight = INS_WEIGHT;
		}

		double cweight = 0;
		if(!bRet){
			LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_184);
			cweight = this->ins_weight * acce_direction + this->uwb_weight*uwb_direction;
			//如果计算出的权重在合适范围内，就逐渐降低惯导的权限
			//如果计算出的权重超过范围，则重置惯导的权重为90%
			if(cweight>=INS_WEIGHT*-1&&cweight<=INS_WEIGHT){
				LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_185);
				cweight = abs(cweight);
			}else{
				LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_186);
				cweight = INS_WEIGHT;
			}

			if(cweight*uwb_direction < 0){
				//惯导和uwb定位方向不一致，定位失败
				//增加一次判断，与上一次的进行比对，如果和上一次的一致，则送进卡尔曼滤波
				LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_187);
				if(cweight*acce_state_last > 0){
					LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_188);
					bUseKalman = true;
				}else{
					LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_189);
					bUseKalman = false;
				}
			}else{
				//惯导和uwb定位方向一致，定位成功
				//送进卡尔曼滤波
				LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_190);
				bUseKalman = true;
			}
		}
		if (abs(cweight) > 0)
		{
			LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_191);
			ins_direction = cweight/abs(cweight) > 0?1:-1;
		}else{
			//如果加速度计的状态权重为零
			LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_192);
			ins_direction = uwb_direction;
		}

		this->ins_weight = cweight;
		this->ins_direction = ins_direction;
		this->acce_last_state = acce_last_state;
		//this->accelerate_state_last  = this->acce_cur_state = acce_state;
#endif
		this->accelerate_state_last  = this->acce_cur_state = acce_state;
		if(m_nFilterType == FILTER_KALMAN){
			LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_193);
#ifdef ALGORITHM_TYPE_INS
			if(bUseKalman){
#endif           
				LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_194);
				//也需要考虑轮回
				//double kalman_detal_t = (maxSyncTimes - this->last_locate.st)*interval_time;
				double kalman_detal_t = 0;
				if (maxSyncTimes - this->last_locate.sync_num < 0 && maxSyncTimes < 100)
				{
					kalman_detal_t = (maxSyncTimes + 65535 - this->last_locate.sync_num)*interval_time;
				}else{
					kalman_detal_t = (maxSyncTimes - this->last_locate.sync_num)*interval_time;
					LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_195);
				}

				//通过卡尔曼滤波处理
				if(p->posx == INVALID_COORDINATE && p->posy == INVALID_COORDINATE){
					LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_196);
					if(this->m_pKalmanFilter->m_nCounts < 3 || this->m_pKalmanFilter->m_pCar->P(0,0) > 2 || kalman_detal_t > 3){
						//P(0,0):连续时间(大于2s)都定位失败
						//deltaT>3:距离上次成功定位时间间隔为3s
						LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_197);
						this->x = this->last_locate.x;
						this->y = this->last_locate.y;
						this->z = 0;

						m_afmData.bStatus = true;
						m_afmData.strCardId = this->card_id;
						this->m_pKalmanFilter->m_bFlag = false;	
						m_afmData.nType = ALGO_FAILED_CONDITION_8;
						ALGORITHM_FAILED(ALGO_FAILED_CONDITION_8);
						return;
					}

					if(this->m_pKalmanFilter->m_nCounts >= 3){
						//只有三次以上才允许使用kalman滤波以下的函数
						LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_198);
						this->m_pKalmanFilter->Predict(kalman_detal_t);
						this->z = 0;
						sdNew.update = true;
					}		
				}else{
					LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_199);
					this->m_pKalmanFilter->m_bFlag = true;
					this->m_pKalmanFilter->m_nCounts++;

					this->m_pKalmanFilter->m_pCar->z(0,0) = cx * this->map_scale;
					this->m_pKalmanFilter->m_pCar->z(1,0) = cvx;
					this->m_pKalmanFilter->m_pCar->z(2,0) = cy * this->map_scale;
					this->m_pKalmanFilter->m_pCar->z(3,0) = cvy;

					if(this->m_pKalmanFilter->m_nCounts == 1){
						//第一次直接赋值
						LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_200);
						this->m_pKalmanFilter->m_pCar->x = this->m_pKalmanFilter->m_pCar->z;
					}
					if(this->m_pKalmanFilter->m_nCounts == 2){
						//两次处理
						LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_201);
						this->m_pKalmanFilter->m_pCar->z(1, 0) = (this->m_pKalmanFilter->m_pCar->z(0, 0) - this->m_pKalmanFilter->m_pCar->x(0, 0))/deltaT;
						this->m_pKalmanFilter->m_pCar->z(3, 0) = (this->m_pKalmanFilter->m_pCar->z(2, 0) - this->m_pKalmanFilter->m_pCar->x(2, 0))/deltaT;
						this->m_pKalmanFilter->m_pCar->x = this->m_pKalmanFilter->m_pCar->z;
					}
					if(this->m_pKalmanFilter->m_nCounts >= 3){
						LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_202);
						//只有三次以上才允许使用kalman滤波以下的函数
						//this->m_pKalmanFilter->Predict_Correct(deltaT);
						this->m_pKalmanFilter->Predict_Correct(kalman_detal_t);
						sdNew.update = true;

						if(deltaT!=0){
							LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_203);
							this->m_pKalmanFilter->m_pCar->x(1,0) = (this->m_pKalmanFilter->m_pCar->x(0,0) - sd.x*this->map_scale)/deltaT;
							this->m_pKalmanFilter->m_pCar->x(3,0) = (this->m_pKalmanFilter->m_pCar->x(2,0) - sd.y*this->map_scale)/deltaT;
						}	
					}
				}

				if(p->nFirstReader == -1 && p->nSecondReader == -1){
					LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_204);
				}

				//增加地图集的判定，判断定位结果是否在地图集上
				//如果不在地图集上，需要再次定位
				//需要带出定位结果的分站信息，
				//利用地图集中分站信息再次定位
				//std::shared_ptr<POS> kalman_p(new POS());
				std::shared_ptr<POS> kalman_p = std::make_shared<POS>();
				kalman_p->nFirstReader = p->nFirstReader;
				kalman_p->nSecondReader = p->nSecondReader;
				kalman_p->posx = this->m_pKalmanFilter->m_pCar->x(0,0);//* this->map_scale
				kalman_p->posy = this->m_pKalmanFilter->m_pCar->x(2,0);//* this->map_scale
				kalman_p->pos_radius = p->pos_radius;

				if(!LocateAlgorithm::IsOnMap(kalman_p,pTdoaReaderPathMap)){
					//再一次定位到地图集上
					LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_205);
					std::shared_ptr<POS> cp = LocateAlgorithm::Pos(kalman_p,pTdoaReaderPathMap);

					if(cp != nullptr){
						LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_206);
						this->m_pKalmanFilter->m_pCar->x(0,0) = cp->posx;
						this->m_pKalmanFilter->m_pCar->x(2,0) = cp->posy;
					}

					cp.reset();
				}		

				//车辆上下行确定
				//3个条件：起点（x1,y1）,终点（x2,y2）
				//1.x1==x2的情况下，y2>y1为下行
				//2.y1==y2的情况下，x1>x2为下行
				//3.x1>x2且y2>y1为下行
				//其他情况为上行
				if ((this->last_locate.x == this->x && this->y > this->last_locate.y)
					||(this->last_locate.x > this->x && this->y == this->last_locate.y)
					||(this->last_locate.x > this->x && this->y > this->last_locate.y))
				{
					this->m_nStream = DOWN_STREAM;
				} 
				else
				{
					this->m_nStream = UP_STREAM;
				}

				this->x = this->m_pKalmanFilter->m_pCar->x(0,0) / this->map_scale;
				this->y = this->m_pKalmanFilter->m_pCar->x(2,0) / this->map_scale;

				nSign = 1;
				if(this->m_pKalmanFilter->m_pCar->x(1,0) == 0){
					if(this->m_pKalmanFilter->m_pCar->x(3,0)>0){
						nSign = 1;
					}
				}else{
					if(this->m_pKalmanFilter->m_pCar->x(1,0) > 0){
						nSign = 1;
					}else{
						nSign = -1;
					}
				}

				this->m_nMoveDirection = nSign;

				algo_calc_offset();

				//求里程
				if (!this->is_first_location)
				{
					this->mileage = sqrt(pow(this->x - this->last_locate.x,2) + pow(this->y - this->last_locate.y,2))*this->map_scale/1000.0;
				}
				else{
					this->is_first_location = false;
				}

				this->last_locate.x = this->x = this->m_pKalmanFilter->m_pCar->x(0,0) / this->map_scale;
				this->last_locate.y = this->y = this->m_pKalmanFilter->m_pCar->x(2,0) / this->map_scale;
				this->last_locate.z = this->z;
				this->last_locate.sync_num = maxSyncTimes;


				this->m_pKalmanFilter->m_pCar->t = this->m_nLastLocateT = maxSyncTimes;

				//速度的计算采用求平均的方式
				double speed = sqrt(pow(this->m_pKalmanFilter->m_pCar->x(1,0),2) + pow(this->m_pKalmanFilter->m_pCar->x(3,0),2));
				speed *=3.6;	//转为km/h
				double av = (fabs(this->v) + fabs(speed))/2.0;
				this->v = av*nSign;
				//this->v = sqrt(pow(this->m_pKalmanFilter->m_pCar->x(1,0),2) + pow(this->m_pKalmanFilter->m_pCar->x(3,0),2)); //*this->map_scale
				//this->v*=3.6;	//转为km/h

				cvx = this->m_pKalmanFilter->m_pCar->x(1,0);
				cvy = this->m_pKalmanFilter->m_pCar->x(3,0);

				this->last_locate.v = this->v;
				this->origin_locate.v = cv;

				ALGORITHM_FAILED(ALGO_LOC_SUCCESSED);
				LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_207);
				if(sdNew.update){
					sdNew.sync_num = maxSyncTimes;
					sdNew.x = this->x;
					sdNew.y = this->y;
					sdNew.vx = cvx;
					sdNew.vy = cvy;
					m_syncNumList.push_back(sdNew);
					LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_208);
				}
#ifdef ALGORITHM_TYPE_INS
			}else{
				//这组数据的处理方法是：
				//如果第一次成功定位，但状态是静止，就取第一次成功定位值(这是为了处理当第一次成功定位，但状态是静止的，此时取上一次定位值为零的问题)，
				//后续如果不管定位成功还是失败，只要状态是静止的，就输出上一次成功定位值，并更新同步序号。
				//取上次结果
				LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_209);
				if (bDirectReturn)
				{
					LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_210);
					if(this->map_scale > 0){
						LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_211);
						this->last_locate.x = this->x = p->posx / (this->map_scale*1.0);
						this->last_locate.y = this->y = p->posy / (this->map_scale*1.0);
						this->last_locate.z = this->z = p->posz / (this->map_scale*1.0);
						this->last_locate.v = this->v = cv;
					}
				}else{
					LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_212);
					this->x = this->last_locate.x;
					this->y = this->last_locate.y;
					this->z = -5;//this->last_locate.z
					this->v = this->last_locate.v;
				}
				this->m_nLastLocateT = this->m_nCalcSyncNum = this->last_locate.st = this->sync_num = maxSyncTimes;

				if(sdNew.update){
					LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_213);
					sdNew.sync_num = maxSyncTimes;
					sdNew.x = this->x;
					sdNew.y = this->y;
					sdNew.vx = cvx;
					sdNew.vy = cvy;
					m_syncNumList.push_back(sdNew);	

					this->m_nSyncNumInList = maxSyncTimes;
				}

				this->is_deal_by_algo = true;
			}
#endif
		}else{
			//TRACE(_T("no kalman . \n"));
			//最新通过算法算出的结果
			LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_214);
			if(p->posx == INVALID_COORDINATE || p->posy == INVALID_COORDINATE){
				LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_215);
				this->x = this->last_locate.x;
				this->y = this->last_locate.y;
				this->z = this->last_locate.z;
			}else{
				LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_216);
				if(this->map_scale > 0){
					LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_216);
					this->x = p->posx / (this->map_scale*1.0);
					this->y = p->posy / (this->map_scale*1.0);
					this->z = p->posz / (this->map_scale*1.0);
				}

				double speed = (fabs(this->v) + fabs(cv))/2;
				this->v = speed*nSign;

				this->last_locate.x = this->x;
				this->last_locate.y = this->y;
				this->last_locate.z = this->z;
				this->last_locate.v = this->v;
				this->m_nLastLocateT = this->last_locate.sync_num = maxSyncTimes;
			}
			this->a = 0;
		}
	}
	LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_217);
	pRdm->clear();
}

/*
* tdoa一维定位
*
* param
*		cnt		数据条数
* 
* return
*		成功输出定位结果返回0，否则返回非零值
*
*/
int Card::algo_tdoa_1d(int cnt)
{
	int ret = 0;

	//1.数据有效性判断
	ret = CheckDistData(cnt);
	if(ret){
		return ret;
	}

	//2.组装数据
	std::shared_ptr<ReceiveDataMap> pRdm = std::make_shared<ReceiveDataMap>();
	pRdm->clear();
	ret = AssembleDistData(pRdm);
	if(ret){
		return ret;
	}

	//3.算法定位	
	//保存结果解
	std::shared_ptr<POS> pos = std::make_shared<POS>();
	//保存所有可能解
	std::vector<std::shared_ptr<POS>> udm_pos;
	udm_pos.resize(0);
	//根据分站数据计算所有可能解，并将所有可能解保存到udm_pos中
	ret = LocateAlgorithm::CalcTdoaPosition(pRdm,pTdoaReaderPathMap,udm_pos);

	bool bIsBack = false;

	if (ret == 0)
	{
		//4.从多解中筛选出一个解,存在两种可能：
		//a.可能无解返回非0，
		//b.可能有解，但解不正确，比如解的位置在4727,-100,但选出的解是4727，-200
		ret = ChooseOneSolution(pRdm, udm_pos, pos);
		//无论正确与否，保存原始值
		SaveOriginDataBeforeFilter(pos);

		if (ret == 0)
		{
			//calibration_pos_data(pos);
			//5.如果有解，则对唯一解做合法性验证
			//主要验证条件为：加速度和速度
			ret = CheckSolution(pos);	

			if (ret == 0)
			{
				//回退的判断应该建立在非怠速的情况下，如果怠速则不走回退的逻辑
				//if (!IsIdleStatus(pos))
				//{
				//	//对通过加速度验证的解进行是否回退的判断
				//	if (IsRebound(pos))
				//	{
				//		if (count_change_direction < MAX_REBOUND_COUNTS)
				//		{
				//			CalcPositionBySpeed(pos,last_locate.v);
				//			//if (pos->posx != INVALID_COORDINATE && pos->posy != INVALID_COORDINATE)
				//			if(CheckPositionValid(pos))
				//			{
				//				pos->cx = pos->posx / (map_scale*1.0);
				//				pos->cy = pos->posy / (map_scale*1.0);
				//				pos->update = true;
				//				pos->is_fit = true;

				//				count_change_direction++;
				//			}else{
				//				//这里可能需要加逻辑处理，避免一次回跳太多
				//				count_change_direction = 0;
				//				bIsBack = true;
				//			}
				//		} 
				//		else
				//		{
				//			count_change_direction = 0;
				//		}
				//	}
				//}
			}else{
				//加速度校验失败
				//加速度或速度抛弃的结果
				if (!IsRebound(pos))
				{
					//如果不是回退，说明了是往前大跳
					//加速追
				}else{
					//说明是往回大跳
					//减速走
					bIsBack = true;
				}
			}
		}else{
			//选解失败
			return 1;
		}
	}

	if (acce_cur_state == ACCELERATE_STATE::STATE_ACCE_MOTION && !bIsBack)
	{
		//6.卡尔曼滤波,两种情况：
		//a.解通过了合法性验证，b.无解,但在2s内
		//则使用卡尔曼滤波进行滤波处理
		if (FILTER_KALMAN == m_nFilterType)
		{
			ret = KalmanFilterProcess(pos);
		}
	}

	bool bRet = LocateAlgorithm::IsOnMap(pos,pTdoaReaderPathMap);

	if (bRet)
	{
		//7.数据保存
		ret = SaveCardAlgoData(pos);
	}

	return ret;
}

/*
* tdoa二维定位
*
* param
*		cnt		数据记录条数
*
* return
*		成功返回0，失败返回非零值
*
*/
int Card::algo_tdoa_2d(int cnt)
{
	int ret = 0;
	//1.数据有效性判断
	ret = CheckDistData(cnt);
	if(ret) return ret;

	//2.组装数据
	std::shared_ptr<ReceiveDataMap> pRdm = std::make_shared<ReceiveDataMap>();
	pRdm->clear();
	ret = AssembleDistData(pRdm);
	if(ret) return ret;

	char chlog[MAX_LOG_LENGTH] = {0};
	string log = "";

	sprintf_s(chlog,"card_id: %s , ct: %d, size: %d,",card_id.c_str(),time_stamp_cal,pRdm->size());
	log+=chlog;
	ReceiveDataMap::iterator it_test = pRdm->begin();
	ReceiveDataMap::iterator it_test_2 = it_test;
	std::advance(it_test_2,1);
	for (it_test_2;it_test_2 != pRdm->end();++it_test_2)
	{
		memset(chlog,0,20*sizeof(char));
		unsigned long long dt = 0;
		dt = it_test_2->second->rec_time_stamp - it_test->second->rec_time_stamp;
		sprintf_s(chlog,"%d-%d: %ld,",it_test_2->second->reader_id,it_test->second->reader_id,dt);
		log+=chlog;
	}
	if (log != "")
	{
		Log::write_log(FILE_TYPE::SYS_S,log,true);
	}

	//3.算法定位
	std::shared_ptr<POS> pos = std::make_shared<POS>();
	//pos = LocateAlgorithm::TdoaLocate2d(pRdm);
	//pos = LocateAlgorithm::tdoa_locate_2d_by_fang(pRdm);

	//多个定位结果的输出
	//无序3个元素的输入，将会已第一个元素作为基准进行计算
	std::shared_ptr<ReceiveDataUnorderedMap> pUnOrderedRdm = std::make_shared<ReceiveDataUnorderedMap>();
	pUnOrderedRdm->clear();
	//多个解保存到vector数组内
	std::vector<std::shared_ptr<POS>> vt_pos;
	vt_pos.resize(0);

	//2017/11/28 add by zhuyf
	/*if (is_algo_first_location)
	{
	pos = LocateAlgorithm::tdoa_locate_2d_by_fang(pRdm);

	if (!CheckPositionValid(pos))
	{
	return 1;
	}
	Position p;
	p.x = pos->posx;
	p.y = pos->posy;
	pTaylorWls->init_position(p);
	}else{
	std::shared_ptr<algorithm::base::ReceiveDataUnorderedMap> pTdoaUnOrderedRdm = std::make_shared<algorithm::base::ReceiveDataUnorderedMap>();
	pTdoaUnOrderedRdm->clear();

	for (auto it_item = pRdm->begin();it_item != pRdm->end();++it_item)
	{
	std::shared_ptr<algorithm::base::ReceiveData> pTdoaReceiveData = std::make_shared<algorithm::base::ReceiveData>();
	pTdoaReceiveData->antenna_id = it_item->second->antenna_id;
	pTdoaReceiveData->reader_id  = it_item->second->reader_id;
	pTdoaReceiveData->rec_time_stamp = it_item->second->rec_time_stamp;
	pTdoaReceiveData->special = it_item->second->special;
	pTdoaReceiveData->x = it_item->second->x;
	pTdoaReceiveData->y = it_item->second->y;
	pTdoaReceiveData->z = it_item->second->z;
	pTdoaUnOrderedRdm->insert(std::make_pair(it_item->first,pTdoaReceiveData));
	}

	TRACE(_T("ct: %d\r\n"),time_stamp_cal);

	Position p;
	p = pTaylorWls->calc_position(pTdoaUnOrderedRdm);
	pos->posx = p.x;
	pos->posy = p.y;
	}*/

	//方法二
	//如果元素为4个元素，则只取其中三个进行计算即可
	//if (pRdm->size() > 3)
	//{
	//	for (ReceiveDataMap::iterator it_first = pRdm->begin();it_first != pRdm->end();++it_first)
	//	{
	//		for (ReceiveDataMap::iterator it_second = pRdm->begin();it_second != pRdm->end();++it_second)
	//		{
	//			if (it_second->first != it_first->first)
	//			{
	//				pUnOrderedRdm->insert(std::make_pair(it_second->first,it_second->second));
	//			}
	//		}
	//		//构建完数据后，进行定位计算
	//		std::vector<std::shared_ptr<POS>> vt_tmp_pos = LocateAlgorithm::tdoa_locate_2d_by_fang(pUnOrderedRdm);
	//		
	//		memset(chlog,0,MAX_LOG_LENGTH*sizeof(char));
	//		std::string output_log = "";
	//		sprintf_s(chlog,"ct: %d, The number of solutions: %d,",time_stamp_cal,vt_tmp_pos.size());
	//		output_log+=chlog;
	//		if (vt_tmp_pos.size() > 0)
	//		{
	//			int i = 0;
	//			output_log += "reader number: ";
	//			for (ReceiveDataUnorderedMap::iterator it_reader = pUnOrderedRdm->begin();it_reader != pUnOrderedRdm->end();++it_reader)
	//			{
	//				memset(chlog,0,MAX_LOG_LENGTH*sizeof(char));
	//				sprintf_s(chlog,"%d,",it_reader->second->reader_id);
	//				output_log+=chlog;
	//			}
	//			output_log += ", positions: ";
	//			for (std::vector<std::shared_ptr<POS>>::iterator tmp_pos = vt_tmp_pos.begin();tmp_pos != vt_tmp_pos.end();++tmp_pos)
	//			{
	//				memset(chlog,0,MAX_LOG_LENGTH*sizeof(char));
	//				sprintf_s(chlog,"x%d : %.3f,y%d : %.3f",i,(*tmp_pos)->posx,i,(*tmp_pos)->posy);
	//				output_log+=chlog;
	//				i++;
	//			}
	//		}
	//		Log::write_log(FILE_TYPE::SYS_S,output_log,true);
	//		
	//		for (std::vector<std::shared_ptr<POS>>::iterator tmp_pos = vt_tmp_pos.begin();tmp_pos != vt_tmp_pos.end();++tmp_pos)
	//		{
	//			vt_pos.push_back(*tmp_pos);
	//		}
	//		pUnOrderedRdm->clear();
	//	}

	//	if (vt_pos.size() > 1)
	//	{
	//		double x = 0.0 ,y = 0.0,z = 0.0;
	//		for (std::vector<std::shared_ptr<POS>>::iterator tmp_pos = vt_pos.begin();tmp_pos != vt_pos.end();++tmp_pos)
	//		{
	//			x+=(*tmp_pos)->posx;
	//			y+=(*tmp_pos)->posy;
	//			z+=(*tmp_pos)->posz;
	//		}
	//		pos->posx = x/vt_pos.size();
	//		pos->posy = y/vt_pos.size();
	//		pos->posz = z/vt_pos.size();
	//	}else if(vt_pos.size() == 1){
	//		pos->cx = vt_pos.at(0)->posx / (map_scale*1.0);
	//		pos->cy = vt_pos.at(0)->posx / (map_scale*1.0);
	//		pos->cz = vt_pos.at(0)->posx / (map_scale*1.0);
	//	}
	//}else{
	//	pos = LocateAlgorithm::tdoa_locate_2d_by_fang(pRdm);
	//}
	//原始方法
	for (ReceiveDataMap::iterator it_first = pRdm->begin();it_first != pRdm->end();++it_first)
	{
		pUnOrderedRdm->insert(std::make_pair(it_first->first,it_first->second));
		std::string reader_list = "";
		char log[MAX_LOG_LENGTH] = {0};

		sprintf_s(log,"card_id : %s, ct : %d, reader_list: %d",card_id.c_str(),time_stamp_cal,it_first->second->reader_id);
		reader_list = log;

		for (ReceiveDataMap::iterator it_second = pRdm->begin();it_second != pRdm->end();++it_second)
		{
			memset(log,0,MAX_LOG_LENGTH*sizeof(char));
			std::string r_log = "";
			//ReceiveDataUnorderedMap::iterator it_first_element = pUnOrderedRdm->begin();
			if (it_second->first != it_first->first)
			{
				pUnOrderedRdm->insert(std::make_pair(it_second->first,it_second->second));
				sprintf_s(log,"-%d",it_second->second->reader_id);
				r_log = log;
			}

			reader_list += r_log;
		}
		//构建完数据后，进行定位计算
		//std::shared_ptr<POS> p = std::make_shared<POS>();
		std::vector<std::shared_ptr<POS>> calc_vt_pos;
		calc_vt_pos.resize(0);
		calc_vt_pos = LocateAlgorithm::tdoa_locate_2d_by_fang(pUnOrderedRdm);

		std::string pos_list = "";
		for (auto it_pos = calc_vt_pos.begin();it_pos != calc_vt_pos.end();++it_pos)
		{
			memset(log,0,MAX_LOG_LENGTH*sizeof(char));
			std::string p_log = "";
			sprintf_s(log,"x: %.2f, y %.2f",(*it_pos)->posx,(*it_pos)->posy);
			p_log = log;
			pos_list+=p_log;
			vt_pos.push_back(*it_pos);	
		}

		std::string str_log = "";
		str_log = reader_list + " : " + pos_list;

		Log::write_log(FILE_TYPE::KALMAN_S,str_log,true);
		//vt_pos.push_back(p);
		pUnOrderedRdm->clear();
	}
	unsigned int size = vt_pos.size();
	switch(size){
	case 0:
		break;
	default:
		double sum_x = 0.0,sum_y = 0.0,sum_z = 0.0;;
		for (std::vector<std::shared_ptr<POS>>::iterator it = vt_pos.begin();it != vt_pos.end();++it)
		{
			sum_x += (*it)->posx;
			sum_y += (*it)->posy;
			sum_z += (*it)->posz;
		}
		pos->posx = sum_x/size;
		pos->posy = sum_y/size;
		pos->posz = sum_z/size;
		break;
	}

	//无论多少条数据，二维定位只以最小的三条进行定位
	//pos = LocateAlgorithm::tdoa_locate_2d_by_fang(pRdm);

	if (pos == nullptr)
	{
		return 0;
	}else{
		pos->cx = pos->posx / (map_scale*1.0);
		pos->cy = pos->posy / (map_scale*1.0);
		pos->cz = pos->posz / (map_scale*1.0);
		isoutput = true;
	}

	//保存原始值
	SaveOriginDataBeforeFilter(pos);

	if (!CheckPositionValid(pos))
	{
		return 1;
	}

	//5.唯一解合法性验证
	//ret = CheckSolution(pos);
	//ret = check_solution_tdoa_2d(pos);
	if(ret) {
		char log[MAX_LOG_LENGTH] = {0};
		sprintf_s(log,"cardid: %s, ct: %d, x: %.2f, y: %.2f",card_id.c_str(),time_stamp_cal,pos->cx,pos->cy);
		Log::write_log(KALMAN_S,log,true);
		return ret; 
	}	

	//6.滤波
	if (FILTER_KALMAN == m_nFilterType)
	{
		//ret = KalmanFilterProcess(pos);
	}

	if (ret)
	{
		return ret;
	}
	pos->dimension = _2D;
	//7.数据保存
	ret = SaveCardAlgoData(pos);

	if (ret)
	{
		return ret;
	}

	return ret;
}

int Card::algo_tdoa_3d(int cnt)
{
	int ret = 0;
	//1.数据有效性判断
	ret = CheckDistData(cnt);
	if(ret) return ret;

	//2.组装数据
	std::shared_ptr<ReceiveDataMap> pRdm = std::make_shared<ReceiveDataMap>();
	pRdm->clear();
	ret = AssembleDistData(pRdm);
	if(ret) return ret;

	//3.算法定位
	std::shared_ptr<POS> pos = std::make_shared<POS>();
	pos = LocateAlgorithm::TdoaLocate3d(pRdm);

	if (pos == nullptr)
	{
		return 0;
	}else{
		pos->cx = pos->posx / (map_scale*1.0);
		pos->cy = pos->posy / (map_scale*1.0);
		pos->cz = pos->posz / (map_scale*1.0);
		isoutput = true;
	}

	//5.唯一解合法性验证
	ret = CheckSolution(pos);
	if(ret) {
		return ret; 
	}	
	//保存原始值
	SaveOriginDataBeforeFilter(pos);

	//6.滤波
	if (FILTER_KALMAN == m_nFilterType)
	{
		ret = KalmanFilterProcess(pos);
	}

	if (ret)
	{
		return ret;
	}
	//7.数据保存
	ret = SaveCardAlgoData(pos);

	if (ret)
	{
		return ret;
	}

	return ret;
}

/*
* 根据算法计算新位置的坐标以及卡的上一次坐标，计算车辆偏移坐标
*
* param
*		无
*
* return
*		无locatepos
*
*/
void Card::algo_calc_offset()
{
	if(is_first_location)
	{
		right_x = left_x = this->x, right_y = right_y = this->y, right_z = left_z = this->z;
		return ;
	}
	double offset = CHAMBER_WIDTH / 4;
	// 根据方向判断，如何靠右行驶
	if(abs(this->x - this->last_locate.x) <= 1E-4)
	{
		this->right_y = this->left_y = this->y;
		if(this->y >= this->last_locate.y)
		{
			this->right_x = this->x - offset;
			this->left_x  = this->x + offset;
		}
		else
		{
			this->right_x = this->x + offset;
			this->left_x  = this->x - offset;
		}
	}
	else
	{
		if(abs(this->y - this->last_locate.y) <= 1E-4)
		{
			this->right_x = this->left_x = this->x;
			if(this->x > this->last_locate.x)
			{
				this->right_y = this->y + offset;
				this->left_y = this->y - offset;
			}
			else
			{
				this->right_y = this->y - offset;
				this->left_y = this->y + offset;
			}
		}
		else
		{
			double k = -1.0 / (this->y - this->last_locate.y)/(this->x - this->last_locate.x);
			double m = this->y - k * this->x;

			double a = 1 + pow(k, 2);
			double b = 2 * (k * m - this->y * k - this->x); 
			double c = pow(m, 2) + pow(this->x, 2) + pow(this->y, 2) - pow(offset, 2) - 2 * m * this->y;

			if(this->last_locate.x < this->x && this->last_locate.y < this->y)
			{
				this->right_x = -0.5 * (b + sqrt(pow(b, 2) - 4 * a * c)) / a;
				this->left_x = - 0.5 * (b - sqrt(pow(b, 2) - 4 * a * c)) / a;
			}
			else if(this->last_locate.x < this->x && this->last_locate.y > this->y)
			{
				this->right_x = -0.5 * (b - sqrt(pow(b, 2) - 4 * a * c)) / a;
				this->left_x = - 0.5 * (b + sqrt(pow(b, 2) - 4 * a * c)) / a;
			}
			else if(this->last_locate.x > this->x && this->last_locate.y < this->y)
			{
				this->right_x = -0.5 * (b + sqrt(pow(b, 2) - 4 * a * c)) / a;
				this->left_x = - 0.5 * (b - sqrt(pow(b, 2) - 4 * a * c)) / a;
			}
			else if(this->last_locate.x > this->x && this->last_locate.y > this->y)
			{
				this->right_x = -0.5 * (b - sqrt(pow(b, 2) - 4 * a * c)) / a;
				this->left_x = - 0.5 * (b + sqrt(pow(b, 2) - 4 * a * c)) / a;
			}

			this->right_y = this->right_x * k + m;
			this->left_y = this->left_x * k + m;
		}
	}
}

double Card::x_offset_after()
{
	//if (!m_GetOnOffMonkeyCar)
	{
		return this->x;
	}
	//else
	//	{
	//	return m_modeling_x;
	//}
	//return this->x;
	//if(this->is_hist)
	//{
	//	return this->x;
	//}
	//return (OUTPUT_POS::LEFT_POS == m_nOutputPosState) ? this->left_x : this->right_x;
}

double Card::y_offset_after()
{
	//if (!m_GetOnOffMonkeyCar)
	{
		return this->y;
	}
	//else
	//{
	//	return this->m_modeling_y;
	//}
	//if(this->is_hist)
	//{
	//	return this->y;
	//}
	//return (OUTPUT_POS::LEFT_POS == m_nOutputPosState) ? this->left_y : this->right_y;
}

double Card::z_offset_after()
{
	return this->z;
}
bool Card::algo_is_same_direction(double x,double y,double z)
{
	int sign = 0;

	if(x == last_locate.x){
		if(y > last_locate.y){
			sign = 1;
		}else{
			sign = -1;
		}
	}else{
		if(x > last_locate.x){
			sign = 1;
		}else{
			sign = -1;
		}
	}

	if (sign!=m_nMoveDirection)
	{
		diff_direction_counts++;
	}else{
		diff_direction_counts = 0;
	}

	if (diff_direction_counts > 0 && diff_direction_counts <3)
	{
		return true;
	}else{
		diff_direction_counts = 0;
		return false;
	}
}

bool Card::is_pos_state_changed()  // 考勤
{
	bool ret = false;
	debug_print_syslog(0,"[is_pos_state_changed]%s,old:%d,%d",card_id.c_str(),pos_state_old,pos_state);
	if(pos_state != pos_state_old)
	{
		if((PDT_INIT == pos_state_old && PDT_UP == pos_state ) || (PDT_INIT == pos_state && PDT_UP == pos_state_old) )
		{
			// lihongzhen  2017/8/18 无信号状态变为有信号状态，记录时间
			if(PDT_INIT == pos_state_old)
			{
				att_start_time_latest = time(NULL);
			}
			pos_state = pos_state_old = PDT_UP;
			pos_state_count = pos_state_confirm_times;
			ret = false;
		}
		else if(pos_state_count >= pos_state_confirm_times)
		{
			if(PDT_DOWN == pos_state_old)
			{
				att_end_time_latest = time(NULL);
			}
			pos_state_old = pos_state;
			ret = true;
		}
	}

	return ret;
}

/*
* 将数据加入到队列中，但相同卡的接受时间戳的数据满足一定条件时，将相应的数据丢入算法进行定位计算，
* 计算完后删除相应的数据，释放内存
*
* param
*		dist		定位数据
*
* return
*		无
*/
void Card::add_dist(std::shared_ptr<_coordinate> dist)
{
	LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_134);
	EnterCriticalSection(&m_csCard);
	string s = concat(dist->reader_id, dist->antenna_id);

	int idx = FindDistMap(dist->t);

	if(-1 == idx){
		LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_135);
		DistQueMapItem dq;
		dq.cardstamp = dist->t;
		dq.distmap[s] = dist;
		_dists.push_back(dq);
	}else{
		LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_136);
		_dists[idx].distmap[s] = dist;
	}

	int ct = dist->t;
	//add by zhuyf 2017/09/29
	int cache_dists = 0;		//缓存数据条数
	switch (ranging_type)
	{
	case LOCATEDATATYPE::LDT_TOF:
		cache_dists = LOCATE_DATA_NUMS::FOUR;
		break;
	case LOCATEDATATYPE::LDT_TDOA:
		cache_dists = LOCATE_DATA_NUMS::THREE;
		break;
	case LOCATEDATATYPE::LDT_TOF_INS:
		//由于惯导的tof数据不同ct是间隔10s发送的，如果要设置为2，那么下发校准坐标就会延迟10s，
		//所以为了保证实时性，即只需要相同ct的两条tof数据即可定位
		cache_dists = LOCATE_DATA_NUMS::ONE;	
		break;
	default:
		cache_dists = MAX_DIST_CACHE;
		break;
	}
	TRACE(_T("ct: %d,size: %d.\n"),ct,_dists.size());
	if (_dists.size() >= cache_dists)// || package_is_over
	{
		get_coordinate();
		if (is_del_data)
		{
#if ENABLE_DEBUG_2D_TEST_LOG
			char log[MAX_LOG_LENGTH];
			memset(log,0,MAX_LOG_LENGTH*sizeof(char));
			sprintf_s(log,"pop data: card_id: %s, ct: %d",card_id.c_str(),_dists.front().distmap.begin()->second->t);
			Log::write_log(FILE_TYPE::SYS_S,log,true);
#endif
			_dists.pop_front();
			distmap.erase(distmap.begin(),distmap.end());
			is_del_data = false;
			package_is_over = false;
		}

		if (LOCATEDATATYPE::LDT_TOF_INS == ranging_type && _dists.size() > 1)
		{
			_dists.pop_front();
			is_del_data = false;
			get_coordinate();
		}
	}

	if (this->x != this->last_x || this->y != this->last_y)
	{
		LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_138);
		algo_calc_offset();
		this->n_pos_no_change_times = 0;
		this->b_pos_change = true;
	}else{
		LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_139);
		this->n_pos_no_change_times++;
		this->b_pos_change = false;
	}
	LeaveCriticalSection(&m_csCard);
}

time_t Card::get_working_time()
{
	return down_time;
}

int Card::get_effictive_dist_count( int offset /*= 0*/ )
{
	if(0 == _dists.size()) return 0;
	time_stamp_cal = _dists.front().cardstamp;
	return (_dists.front().distmap.size());
}

/*
* Card析构类
*/
Card::~Card(void)
{
	_dists.clear();
	DeleteCriticalSection(&m_csCard);
}

double Card::get_speed()
{
	//if(this->idle_count >= IDLE_CONFIRM_TIME || STATUS_LOST == this->status_lost || this->n_pos_no_change_times >= POS_NO_CHANGE_TIMES)
	//{
	//	return 0;
	//}

	if (fabs(final_v) < ZERO_PRECISION)
	{
		return 0;
	}

	double ret_v = ceil(fabs(final_v));
	//if (ret_v >= 50) // TODO: 为了演示暂时修正输出值
	//{
	//    ret_v = 40;
	//}

	return ret_v;
}



/*
* 采用TOF或者TDOA算法进行定位计算
*
* param
*		cnt		------	_dists数据条数
*
* return
*		无返回值
*
*/
void Card::get_coordinate(int cnt)
{
	int ret = 0;

	switch(ranging_type){
	case LOCATEDATATYPE::LDT_TDOA:
		algo_tdoa_1d(cnt);
		break;
	case LOCATEDATATYPE::LDT_TOF_INS:
		ret = algo_tof_1d(cnt);
		if (ret != 0)
		{
			reader_id_algo = -1;
		}else{
			need_display = false;
		}
		break;
	case LOCATEDATATYPE::LDT_TOF:
		//ret = algo_tof_1d(cnt);
		ret = new_algo_tof_1d(cnt);
		//ret = algo_tof_raw_1d(distmap);

		/*char log[MAX_LOG_LENGTH];
		memset(log,0,MAX_LOG_LENGTH*sizeof(char));
		sprintf_s(log,"card_id: %s,ct: %d, error: %d,algo_time: %s",card_id.c_str(),time_stamp_cal,ret,CFunctions::time_t2string(time_cur_algo).c_str());
		Log::write_log(FILE_TYPE::ALGO_S,log,true);*/

		break;
	default:
		;
	}
	if (ret == 0)
	{
		m_indexForRcvOneReader = 0;
		inspect_coordinate(this->acce_cur_state);

		/*char log[MAX_LOG_LENGTH];
		memset(log,0,MAX_LOG_LENGTH*sizeof(char));
		sprintf_s(log,"card_id: %s, x: %.3f, y: %.3f,ct: %d, size: %d",card_id.c_str(),origin_locate.x,origin_locate.y,time_stamp_last,distmap.size());
		Log::write_log(FILE_TYPE::SYS_S,log,true);*/
	}
	else if(DIST_COUNT_LESS_THAN_TWO == ret)
	{
		m_indexForRcvOneReader++;
	}
	else
	{
		m_indexForRcvOneReader=0;
	}
	if (m_indexForRcvOneReader >= INDEXFORRECONEREADER)
	{
		this->v = 0;
	}

	if(_isnan(this->x) || _isnan(this->y) || _isnan(this->z)){
		this->x = this->last_x;
		this->y = this->last_y;
		this->z = this->last_z;

		this->output_x = this->x;
		this->output_y = this->y;
	}
}

/*
* 选择算法调用种类
*
* param
*		无
*
* return
*		无
*
*/
void Card::get_coordinate()
{
	//识别定位是采用一维定位还是二维或三维定位
	int dim = 0;
	dim = get_dimension();			//检查数据，看采用定位方法
	int cnt = 0;
	cnt = distmap.size();
	switch(dim){
	case _1D:
		if (LOCATEDATATYPE::LDT_TOF == ranging_type || LOCATEDATATYPE::LDT_TDOA == ranging_type)
		{
			get_coordinate(cnt);
		}else if (LOCATEDATATYPE::LDT_TOF_INS == ranging_type ||
			LOCATEDATATYPE::LDT_TDOA_INS == ranging_type)
		{
			get_coordinate(get_effictive_dist_count());
		}
		break;
	case _2D:
		if (LOCATEDATATYPE::LDT_TOF == ranging_type)
		{
			//get_coordinate_2d(cnt);
			algo_tof_2d(cnt);
		}else if (LOCATEDATATYPE::LDT_TDOA == ranging_type)
		{
			algo_tdoa_2d(cnt);
		}
		break;
	case _3D:
		if (LOCATEDATATYPE::LDT_TOF == ranging_type)
		{
			get_coordinate_3d(cnt);
		}else if (LOCATEDATATYPE::LDT_TDOA == ranging_type)
		{
			algo_tdoa_3d(cnt);
		}
		break;
	}

	is_del_data = true;
}

/*
* 根据状态值获得状态文本描述
*
* param
*		无
*
* return
*		状态文本描述
*
*/
int Card::get_state()
{
	this->state = 0;

	//电量极低
	if(power_state == STATUS_ERROR_SERIOUS){
		state = STATUS_POWER_LOWER_SERIOUS;
	}

	return state;
}

int Card::get_state_moving()
{
	if(STATUS_LOST == this->status_lost)
	{
		state_moving = STATE_ACCE_STATIC;
	}
	return state_moving;
}

std::string Card::concat( int reader_id, int ant_id )
{
	char s[10];
	sprintf_s(s, "%d-%d", reader_id, ant_id);
	return s;
}

/*
* 滤波功能设置
* 
* param
*		nType ------		滤波类型
*
* return
*		无
*/
void Card::EnableFilter(int nType)
{
	//如果无滤波类型直接返回
	if(nType == NO_FILTER){
		return;
	}

	//开启滤波功能，设置滤波类型
	m_bUseFilter = TRUE;
	m_nFilterType = nType;

	switch(nType){
	case FILTER_KALMAN:
		//分配卡尔曼滤波类型变量并初始化参数
		if(m_pKalmanFilter == nullptr){
			std::unique_ptr<CKalmanFilter> p(new CKalmanFilter());
			m_pKalmanFilter = std::move(p);
			m_pKalmanFilter->Initial(0.2);
			m_pKalmanFilter->m_bFlag = false;
		}
		break;
	default:
		break;
	}
}

void Card::remove_dist_head()
{
	DistMap tmp = _dists.front().distmap;
	if(tmp.size() > 0 ){
		DistMap::iterator it_mp_dist = tmp.begin();
		for(it_mp_dist;it_mp_dist != tmp.end();){
			it_mp_dist = tmp.erase(it_mp_dist);
		}
	}
	_dists.pop_front();
}

Reader::Reader(void)
{
	reader_id = device_type_id = pos_state = map_id = area_id = temperature = tick_count= 0;
	sync_level = 0xFF;
	reader_state = reader_state_old = STATUS_DEVICE_NORMAL;
	m_nIsSpecial = -1;

	last_send_time = rec_time = reader_time = lost_time = time(NULL);
	sync_rootId = 0;
	map_scale = 1.0;
	reader_x = reader_y = reader_z = reader_angle = reader_interval_time = -9999.0;

	reader_name = ip = "";

	for(int i = 0;i < ANTENNA_COUNT;i++){
		ant[i] = nullptr;
	}

	for(int i = 0;i < ADHOC_COUNT;i++){
		adhoc[i] = nullptr;
	}

	bIsInitCoverage = false;
	init_ctrl_reader_state = false;

	for(int i = 0; i < READER_EVENT_COUNT; i++){
		m_event_list[i] = 0;
	}

	reader_package_end_pos = 0;
	last_parse_card_id = "";
	pBoundaryArea = nullptr;
	is_simulation_card = b_simulation_status = false;
	reader_dir = -1;
	relay_counts = -1;
	m_start_simulation_time = time(NULL);
	m_vt_his_position.resize(0);
	m_bNeedPowerAlarm = false;
}

Reader::~Reader(void)
{
}

std::string Reader::get_state_text()
{
	string ret = "";
	if(reader_state == STATUS_DEVICE_ERROR){
		ret = "故障";
	}else if(reader_state == STATUS_DEVICE_NORMAL){
		ret = "正常";
	}
	return ret;
}

Antenna::Antenna(void)
{
	antenna_angle = 0;
	antenna_id = 0;
	antenna_x = 0;
	antenna_y = 0;
	antenna_z = 0;
}

Antenna::~Antenna(void)
{
}

Area::Area(void)
{
	is_att = 1;
	polygon_count = 0;
	polygon = NULL;
	map_id = area_id = area_type_id = 0 ;
	area_name = area_type_name = path = "";
	over_count_person = over_time_person = under_count_person = under_time_person = 0;
	over_count_vehicle = over_time_vehicle = under_count_vehicle = under_time_vehicle = 0;

	count_person = count_vehicle = count_card = 0;
	is_area_over_time_person = is_area_over_time_vehicle = false; 
	count_area_over_time_person = count_area_over_time_vehicle = 0;
	time_over_time_person = time_over_time_vehicle = time(NULL);

	is_area_over_count_person = is_area_over_count_vehicle = false;
	count_area_over_count_person = count_area_over_count_vehicle = 0;
	time_over_count_person = time_over_count_vehicle = time(NULL); 

	is_area_forbidden_person = is_area_forbidden_vehicle = false;
	count_area_forbidden_person = count_area_forbidden_vehicle = 0;
	time_forbidden_person = time_forbidden_vehicle = time(NULL);

	over_speed_vehicle = 0;

	area_card_list_person = std::make_shared<CardMap>();
	area_card_list_vehicle  = std::make_shared<CardMap>();
	area_card_list_over_speed = std::make_shared<CardMap>();

	for(int i = 0; i < AREA_EVENT_COUNT; i++){
		m_event_list[i] = 0;
	}
}

Area::~Area(void)
{
	if(polygon){
		delete[] polygon;
		polygon = NULL;
	}
}

void Area::init_border(string sz_path)
{
	if(sz_path == ""){
		return ;
	}

	std::vector<std::string> vec = split(sz_path, " ");
	std::vector<std::string>::iterator it = vec.begin();
	if(polygon){
		delete[] polygon;
		polygon = NULL;
	}
	polygon = new _point[vec.size()];
	polygon_count = 0;
	for(; it != vec.end(); ++it){
		if (*it == "")
		{
			continue;
		}
		std::vector<std::string> subvec = split(it->c_str(), ",");
		_point p;
		p.x = get_vertex(subvec[0]);
		p.y = get_vertex(subvec[1]);
		p.z = 0;
		polygon[polygon_count] = p;
		polygon_count++;
	}
}

std::vector<std::string> Area::split( std::string str,std::string pattern )
{
	std::string::size_type pos;  
	std::vector<std::string> result;  
	str+=pattern;//扩展字符串以方便操作  
	unsigned int size=str.size();  

	for(unsigned int i=0; i<size; i++){  
		pos=str.find(pattern,i);  
		if(pos<size){  
			std::string s=str.substr(i,pos-i);  
			result.push_back(s);  
			i=pos+pattern.size()-1;  
		}  
	}  
	return result; 
}

double Area::get_vertex( std::string src)
{
	std::string dest = "";
	for(unsigned int i = 0; i < src.length(); i++){
		if((src[i] >= '0' && src[i]<='9') || src[i]=='-' || src[i] == '.'){
			dest += src[i];
		}
	}
	return atof(dest.c_str());
}

bool Area::is_in_polygon( _point p )
{
	if(polygon == NULL){
		return false;
	}

	int counter = 0;
	int i;
	double xinters;
	_point p1,p2;

	p1 = polygon[0];
	for (int i=1;i<= polygon_count;i++) {
		p2 = polygon[i % polygon_count];
		if (p.y > MIN(p1.y,p2.y)) {
			if (p.y <= MAX(p1.y,p2.y)) {
				if (p.x <= MAX(p1.x,p2.x)) {
					if (p1.y != p2.y) {
						xinters = (p.y-p1.y)*(p2.x-p1.x)/(p2.y-p1.y)+p1.x;
						if (p1.x == p2.x || p.x <= xinters)
							counter++;
					}
				}
			}
		}
		p1 = p2;
	}
	return (counter % 2 == 0) ? false : true;
}

bool Area::is_special()
{
	return (area_type_id == AREA_TYPE_NO_COVER) ? true : false;
}

int Card::FindDistMap( int cardstamp )
{
	int idx = -1;
	for(int i = _dists.size() - 1; i >= 0; i--){
		if(_dists[i].cardstamp == cardstamp ){
			return i;
		}
	}
	return idx;
}

/*
* 卡尔曼滤波处理模块
*
* param
*		pos		定位结果
*
* return
*		处理成功返回0，否则返回非零值
*
*/
int Card::KalmanFilterProcess(std::shared_ptr<POS>& pos)
{
	int bRet = 0;

	switch (ranging_type)
	{
	case LOCATEDATATYPE::LDT_TDOA:
		bRet = kalman_filter_process_tdoa(pos);
		break;
	case LOCATEDATATYPE::LDT_TOF:
		bRet = kalman_filter_process_tof(pos);
		break;
	default:
		break;
	}

	return bRet;
}

/*
* 组装拟合数据，主要是前REF_POSITION_NUM次的定位数据信息
*
* param
*		pos		定位坐标
*
* return
*		成功返回0
*/
int Card::UpdateFittingData(std::shared_ptr<POS> pos)
{
	if (cur_ref_totals == REF_POSITION_NUM - 1)
	{
		for (int i = 0;i < cur_ref_totals-1;i++)
		{
			fitting_v[i]  = fitting_v[i + 1];
			fitting_ct[i] = fitting_ct[i + 1];
			fitting_x[i]  = fitting_x[i + 1];
			fitting_y[i]  = fitting_y[i + 1];
		}
 
	}

	fitting_v[cur_ref_totals] = v;
	fitting_ct[cur_ref_totals] = m_nCalcSyncNum;
	fitting_x[cur_ref_totals] = pos->posx;
	fitting_y[cur_ref_totals] = pos->posy;

	if (cur_ref_totals < REF_POSITION_NUM - 1)
	{
		cur_ref_totals++;
	}

	return 0;
}

int Card::CalcFittingData(list<std::shared_ptr<POS>> & list_pos)
{
	
	//如果滤波失败或者其他条件失败，
	//则通过直线回归算法拟合出三个解
	vector<double> vx,vy,vt;
	vx.resize(0);
	vy.resize(0);
	vt.resize(0);

	for (list<std::shared_ptr<POS>>::iterator it = list_pos.begin();it!=list_pos.end();++it)
	{
		vt.push_back((*it)->card_count);
		vx.push_back((*it)->posx);
		vy.push_back((*it)->posy);
	}

	//线性回归拟合，从5个历史点中取出3个预测点
	//Fit fit_x;
	fit_x.linearFit(vt,vx);
	//Fit fit_y;
	fit_y.linearFit(vt,vy);

	for (int i = 0;i < FIT_POSITION_NUM;i++)
	{
		fit_new_x[i] = 0;
		fit_new_y[i] = 0;
	}

	for (int i = 1;i <= FIT_POSITION_NUM;i++)
	{
		fit_new_x[i-1] = fit_x.getY(vt[4] + i);
		fit_new_y[i-1] = fit_y.getY(vt[4] + i);
	}

	have_fit_pos = true;

	return 0;
}

/*
* 计算线性拟合数据，
* 当满足如下两个条件：
*		a.之前定位成功，
*		b.参考数据小于5，
*		c.连续取拟合数据等于3次了，
* 则不计算拟合数据
*
* param
*		ret			定位状态
*		pos			定位结果
*
* return
*		成功获得拟合数据返回0，否则返回1
*
*/
int Card::CalcFittingData(int ret,std::shared_ptr<POS>& pos)
{
	if (his_pos.size() < FIT_POSITION_NUM || cur_fit_nums >= 3)
	{
		if (cur_fit_nums == 3)
		{
			have_fit_pos = false;
		}
		return 1;
	}
	if (ret == 0 && pos->reason == 0)
	{
		return 2;
	}

	if (cur_fit_nums == 0)
	{
		//如果滤波失败或者其他条件失败，
		//则通过直线回归算法拟合出三个解
		vector<double> vx,vy,vt;
		vx.resize(0);
		vy.resize(0);
		vt.resize(0);

		for (list<std::shared_ptr<POS>>::iterator it = his_pos.begin();it!=his_pos.end();++it)
		{
			vt.push_back((*it)->card_count);
			vx.push_back((*it)->posx);
			vy.push_back((*it)->posy);
		}

		//线性回归拟合，从5个历史点中取出3个预测点
		Fit fit_x;
		fit_x.linearFit(vt,vx);
		Fit fit_y;
		fit_y.linearFit(vt,vy);

		for (int i = 1;i <= FIT_POSITION_NUM;i++)
		{
			fit_new_x[i-1] = fit_x.getY(vt[4] + i);
			fit_new_y[i-1] = fit_y.getY(vt[4] + i);
		}
	}
	int index = cur_fit_nums;

	//判断
	double kx = fit_new_x[index];
	double ky = fit_new_y[index];

	std::shared_ptr<POS> ks = std::make_shared<POS>();
	ks->posx = fit_new_x[index] ;
	ks->posy = fit_new_y[index] ;
	ks->cx = ks->posx / (1.0*map_scale);
	ks->cy = ks->posy / (1.0*map_scale);

	//判断拟合数据是否在地图集上，如果不在返回1
	if (!LocateAlgorithm::IsOnMap(ks,pTdoaReaderPathMap))
	{
		//不在地图集上，则返回
		return 1;
	}

	pos->posx = ks->posx;
	pos->posy = ks->posy;
	pos->cx = ks->cx;
	pos->cy = ks->cy;

	if (fabs(pos->diff_reader_sync_num) > 1E-4)
	{
		pos->cvx = (pos->cx - last_locate.x)/pos->diff_reader_sync_num;
		pos->cvy = (pos->cy - last_locate.y)/pos->diff_reader_sync_num;
	}

	cur_fit_nums++;
	pos->reason = 0;
	pos->is_fit = true;
	have_fit_pos = true;

	return 0;
}

/*
* 通过多项式拟合计算数据，
* 当满足如下两个条件：
*		a.之前定位成功，
*		b.参考数据小于5，
*		c.连续取拟合数据等于3次了，
* 则不计算拟合数据
*
* param
*		ret			定位状态
*		pos			定位结果
*
* return
*		成功获得拟合数据返回0，否则返回1
*
*/
int Card::CalcLongFittingData(int ret,std::shared_ptr<POS>& pos)
{
	int nums = FIT_POSITION_NUM*4;
	if (long_his_pos.size() < nums || cur_fit_nums >= nums)
	{
		if (cur_fit_nums == nums)
		{
			have_fit_pos = false;
		}
		return 1;
	}

	if (ret == 0 && pos->reason == 0)
	{
		return 2;
	}

	if (cur_fit_nums <= 3)
	{
		//如果滤波失败或者其他条件失败，
		//则通过多项式拟合算法拟合出20个解
		vector<double> vx,vy,vt;
		vx.resize(0);
		vy.resize(0);
		vt.resize(0);

		for (list<std::shared_ptr<POS>>::iterator it = long_his_pos.begin();it!=long_his_pos.end();++it)
		{
			vt.push_back((*it)->card_count);
			vx.push_back((*it)->posx);
			vy.push_back((*it)->posy);
		}

		//多项式拟合，从20个历史点中拟合出20个预测点
		Fit fit_x;
		fit_x.polyfit(vt,vx,2);
		Fit fit_y;
		fit_y.polyfit(vt,vy,2);

		for (int i = 1;i <= nums;i++)
		{
			long_fit_new_x[i-1] = fit_x.getY(vt[nums - 1] + i);
			long_fit_new_y[i-1] = fit_y.getY(vt[nums - 1] + i);
		}
	}
	int index = cur_fit_nums - 3;

	//判断
	double kx = long_fit_new_x[index];
	double ky = long_fit_new_y[index];

	std::shared_ptr<POS> ks = std::make_shared<POS>();
	ks->posx = long_fit_new_x[index] ;
	ks->posy = long_fit_new_y[index] ;
	ks->cx = ks->posx / (1.0*map_scale);
	ks->cy = ks->posy / (1.0*map_scale);

	//判断拟合数据是否在地图集上，如果不在返回1
	if (!LocateAlgorithm::IsOnMap(ks,pTdoaReaderPathMap))
	{
		//不在地图集上，则返回
		return 1;
	}

	pos->posx = ks->posx;
	pos->posy = ks->posy;
	pos->cx = ks->cx;
	pos->cy = ks->cy;

	if (fabs(pos->diff_reader_sync_num) > 1E-4)
	{
		pos->cvx = (pos->cx - last_locate.x)/pos->diff_reader_sync_num;
		pos->cvy = (pos->cy - last_locate.y)/pos->diff_reader_sync_num;
	}

	cur_fit_nums++;
	pos->reason = 0;
	pos->is_fit = true;
	have_fit_pos = true;

	return 0;
}

int Card::CalcLongFittingData()
{
	int nums = FIT_POSITION_NUM*4;

	//如果滤波失败或者其他条件失败，
	//则通过多项式拟合算法拟合出20个解
	vector<double> vx,vy,vt;
	vx.resize(0);
	vy.resize(0);
	vt.resize(0);

	for (list<std::shared_ptr<POS>>::iterator it = long_his_pos.begin();it!=long_his_pos.end();++it)
	{
		vt.push_back((*it)->card_count);
		vx.push_back((*it)->posx);
		vy.push_back((*it)->posy);
	}

	//多项式拟合，从20个历史点中拟合出20个预测点
	Fit fit_x;
	fit_x.polyfit(vt,vx,2);
	Fit fit_y;
	fit_y.polyfit(vt,vy,2);

	for (int i = 0;i < nums;i++)
	{
		long_fit_new_x[i] = 0;
		long_fit_new_y[i] = 0;
	}

	double diff_x = 0 , diff_y = 0;
	double dx[FIT_POSITION_NUM*4] = {0};
	double dy[FIT_POSITION_NUM*4] = {0};
	for (int i = 1;i <= nums;i++)
	{
		long_fit_new_x[i-1] = fit_x.getY(vt[nums - 1] + i);
		dx[i-1] = long_fit_new_x[i-1];
		long_fit_new_y[i-1] = fit_y.getY(vt[nums - 1] + i);
		dy[i-1] = long_fit_new_y[i-1];
	}

	double start_x = 0.0;
	double end_x = 0.0;
	double start_y = 0.0;
	double end_y = 0.0;

	start_x = long_fit_new_x[4];
	start_y = long_fit_new_y[4];
	end_x = long_fit_new_x[5];
	end_y = long_fit_new_y[5];
	double distance = sqrt(pow(start_x - end_x,2)+pow(start_y - end_y,2));
	double percent = 0.5;

	for (int i = 5;i < nums;i++)
	{
		distance *= percent;
		if (abs(long_fit_new_x[i-1] - long_fit_new_x[i]) < 1E-4)
		{
			//表示x相等，在y轴上
			if (long_fit_new_y[i-1] < long_fit_new_y[i])
			{
				//如果下一次拟合值比当前拟合值大
				dy[i] = dy[i-1] + distance;
			}else{
				dy[i] = dy[i-1] - distance;
			}
		}else{
			//计算斜率
			double k = (long_fit_new_y[i-1] - long_fit_new_y[i])/(long_fit_new_x[i-1] - long_fit_new_x[i]);

			//在有斜率的地方
			double arg = atan(k);
			if (long_fit_new_x[i-1] < long_fit_new_x[i] && long_fit_new_y[i-1] < long_fit_new_y[i])
			{
				dx[i] = dx[i-1] + cos(arg)*distance;
				dy[i] = dy[i-1] + sin(arg)*distance;
			} 
			else
			{
				dx[i] = dx[i-1] - cos(arg)*distance;
				dy[i] = dy[i-1] - sin(arg)*distance;
			}
		}
		//使用下一个点和第六点的距离取50%
		distance = sqrt(pow(dx[i-1] - end_x,2) + pow(dy[i] - end_y,2));
	}

	for (int i = 0;i < nums;i++)
	{
		long_fit_new_x[i] = dx[i];
		long_fit_new_y[i] = dy[i];
	}

	have_long_fit_pos = true;

	return 0;
}

std::shared_ptr<POS> Card::GetPosFromFittingData()
{
	if ((!have_fit_pos&&!have_long_fit_pos) 
		|| (have_fit_pos && cur_fit_nums >= 3&&!have_long_fit_pos)
		|| (have_fit_pos && have_long_fit_pos && cur_fit_nums >= 20))
	{
		//1.如果5点拟合和20点拟合无数据，则无法获取
		//2.如果有5点拟合，但无20点拟合，获取拟合数据超过3，则无法获取
		//3.如果有5点拟合，且有20点拟合，而且获取拟合数据超过20，则无法获取
		return nullptr;
	}
	std::shared_ptr<POS> p = std::make_shared<POS>();
	//没有拟合20个点的数据
	if (!have_long_fit_pos)
	{
		if (have_fit_pos)
		{
			p->posx = fit_new_x[cur_fit_nums];
			p->posy = fit_new_y[cur_fit_nums];
			p->cx = p->posx / (map_scale*1.0);
			p->cy = p->posy / (map_scale*1.0);
			p->is_fit = true;
			p->reason = 0;
			cur_fit_nums++;
		}	
	}else{
		//有拟合20个点的数据
		p->posx = long_fit_new_x[cur_fit_nums];
		p->posy = long_fit_new_y[cur_fit_nums];
		p->cx = p->posx / (map_scale*1.0);
		p->cy = p->posy / (map_scale*1.0);
		p->is_fit = true;
		p->reason = 0;
		cur_fit_nums++;
	}

	if (p->posx == INVALID_COORDINATE && p->posy == INVALID_COORDINATE)
	{
		p = nullptr;
	}

	return p;
}

int Card::GetPosFromFittingData(std::shared_ptr<POS>& pos)
{
	if (have_fit_pos || have_long_fit_pos)
	{
		//如果拟合了数据，则直接取拟合数据
		std::shared_ptr<POS> fit_p = GetPosFromFittingData();

		if (fit_p == nullptr)
		{
			return 1;
		}

		pos->posx = fit_p->posx;
		pos->posy = fit_p->posy;
		pos->cx   = fit_p->cx;
		pos->cy   = fit_p->cy;

		if (m_nCalcSyncNum - this->last_locate.sync_num < 0 && m_nCalcSyncNum < 100)
		{
			pos->diff_reader_sync_num = (m_nCalcSyncNum  + 65536 - this->last_locate.sync_num)*0.2;
		}else{
			pos->diff_reader_sync_num = (m_nCalcSyncNum - this->last_locate.sync_num)*0.2;
		}

		if (!LocateAlgorithm::IsOnMap(pos,pTdoaReaderPathMap))
		{
			pos->reason = 1;
			return 1;
		}else{
			//如果在地图集上，则计算相关参数
			if (fabs(pos->diff_reader_sync_num) > 1E-4)
			{
				pos->cvx = (pos->cx - last_locate.x)/pos->diff_reader_sync_num;
				pos->cvy = (pos->cy - last_locate.y)/pos->diff_reader_sync_num;
			}else{
				pos->cvx = last_vx;
				pos->cvy = last_vy;
			}

			cur_fit_nums++;
			pos->reason = 0;
			pos->is_fit = true;
		}
	}else{
		return 1;
	}

	return 0;
}

/*
* 处理上传的惯导数据的校准功能：含初始坐标和陀螺仪角度校准
*
* param
*
* return
*		成功返回0，失败返回1
*
*/
int Card::deal_ins_cal_data()
{
	int nRet = 0;

	std::shared_ptr<POS> cur_uwb_pos = std::make_shared<POS>();
	cur_uwb_pos->posx = this->x*map_scale;
	cur_uwb_pos->posy = this->y*map_scale;
	if (CheckPositionValid(cur_uwb_pos))
	{
		if (lt_uwb_pos.size() == 0 && lt_ins_pos.size() == 0)
		{
			ins_cal_type = CAL_DATA_POS;
			lt_uwb_pos.push_back(cur_uwb_pos);
			lt_ins_pos.push_back(cur_uwb_pos);
		}else{
			if (lt_uwb_pos.size() >= 2)
			{
				lt_uwb_pos.pop_front();
			}
			lt_uwb_pos.push_back(cur_uwb_pos);
			//为了保证相同时刻uwb定位数据和惯导坐标的一致性
			//数据保存到校准队列中
			if (lt_ins_pos.size() >= 2)
			{
				lt_ins_pos.pop_front();
			}
			lt_ins_pos.push_back(cur_ins_pos);	
		}	
	}

	if (lt_ins_pos.size() == 2 && lt_uwb_pos.size() == 2)
	{
		ins_cal_type = CAL_DATA_ALL;
		double k1 = 0.0,k2 = 0.0;
		int s1 = 0, s2 = 0;
		std::shared_ptr<POS> uwb_start_pos = nullptr,uwb_end_pos = nullptr;
		std::shared_ptr<POS> ins_start_pos = nullptr,ins_end_pos = nullptr;

		uwb_start_pos = *(lt_uwb_pos.begin());
		uwb_end_pos = *(lt_uwb_pos.rbegin());
		s1 = calc_slope(uwb_start_pos,uwb_end_pos,k1);

		ins_start_pos = *(lt_ins_pos.begin());
		ins_end_pos = *(lt_ins_pos.rbegin());
		s2 = calc_slope(ins_start_pos,ins_end_pos,k2);

		//else if 后的为如果某条直线的斜率不存在
		if (s1 == 0 && s2 == 0)
		{
			//如果直线斜率都存在，才可以计算校准角度
			this->ins_gyp_angle = atan((k2 - k1)/(1+k1*k2));
		}else if (s1 == -1 && s2 == 0)
		{
			//如果k1直线斜率不存在，及为平行于y轴的直线
			if (abs(k2) < ZERO_PRECISION)
			{
				//如果k2的斜率为零，则k2直线和k1直线垂直
				this->ins_gyp_angle = 90.0;
			}else{
				this->ins_gyp_angle = atan(k2);
			}
		}else if(s1 == 0 && s2 == -1){
			//如果k2直线斜率不存在，及为平行于y轴的直线
			if (abs(k1) < ZERO_PRECISION)
			{
				//如果k1的斜率为零，则k1直线和k2直线垂直
				this->ins_gyp_angle = 90.0;
			}else{
				this->ins_gyp_angle = atan(k1);
			}
		}else{
			nRet = 1;
		}
	}else{
		//数据不够，无法校准
		nRet = 1;
	}

	return nRet;
}

/*
* 根据起始点和结束点计算所在直线的斜率
*
* param
*		start			起点坐标
*		end				终点坐标
*		slope			计算后的斜率
* return
*		斜率存在返回0，否则返回-1
*
*/
int Card::calc_slope(std::shared_ptr<POS> start,std::shared_ptr<POS> end,double & slope)
{
	int nRet = -1;
	if (abs(end->posx - start->posx) < ZERO_PRECISION)
	{
		//斜率不存在
	}else{
		slope = (end->posy - start->posy)/(end->posx - start->posx);
		nRet = 0;
	}
	return nRet;
}

int Card::CheckSolutionByFit(int ret,std::shared_ptr<POS>& pos)
{
	int fit_ret = 0;
	//拟合预测解
	is_ref_pos = true;
	std::shared_ptr<POS> fit_pos = GetPosFromFittingData();
	if (fit_pos == nullptr)
	{
		return 1;
	}

	//存在如下情况，解是个错误解
	if (ret && fit_ret == 0)
	{
		//如果加速度和速度失败了或选解选不出来，而且有拟合数据了
		//则直接使用拟合解
		pos->posx = fit_pos->posx;
		pos->posy = fit_pos->posy;
		pos->cx   = fit_pos->cx;
		pos->cy   = fit_pos->cy;
		pos->cvx  = fit_pos->cvx;
		pos->cvy  = fit_pos->cvy;
		pos->is_fit = fit_pos->is_fit;
		//如果是拟合出来的点就不能作为参考点
		is_ref_pos = !pos->is_fit;
	}else{
		if (fit_ret == 0)
		{
			//如果有拟合数据
			//如果选出了一个解，需要判断此解和预测值得距离
			double distance = 0.0;
			distance = sqrt(pow(pos->posx - fit_pos->posx,2) + pow(pos->posy - fit_pos->posy,2));
			if (distance > 10)
			{
				//如果定位出的解和拟合解的距离差大于10，则认为拟合解可信
				pos->posx = fit_pos->posx;
				pos->posy = fit_pos->posy;
				pos->cx   = fit_pos->cx;
				pos->cy   = fit_pos->cy;
				pos->cvx  = fit_pos->cvx;
				pos->cvy  = fit_pos->cvy;
				pos->is_fit = fit_pos->is_fit;
				//如果是拟合出来的点就不能作为参考点
				is_ref_pos = !pos->is_fit;
			}
		}
		if (fit_ret && ret == 0)
		{
			//表示没有拟合值，但解是可信的，则直接返回0
			return 0;
		}
	}

	return fit_ret;
}

int Card::CheckSolutionBySpeed(std::shared_ptr<POS>& pos)
{
	if (pos->posx == INVALID_COORDINATE && pos->posy == INVALID_COORDINATE)
	{
		return 1;
	}

	//double speed = sqrt(pow(pos->cvx,2) + pow(pos->cvy,2));
	if (m_nLastLocateT != 0)
	{
		//如果第一次定位不做速度的判别
		double speed = sqrt(pow(pos->cvx,2) + pow(pos->cvy,2));
		speed *= 3.6;	//转为km/h
		if (speed > MAX_VECHILE_SPEED)
		{
			return 1; 
		}
	}
	return 0;
}

bool Card::CheckCardCtValid()
{
	bool bRet = true;
	// 本次序号小于上次序号，存在如下情况
	// 1、跨周期，如本次0，上次为65535，另外进入盲区也可能跨周期后回来
	// 2、卡重置，从0开始,初始状态值为-1
	// 3、序号较连续，新数据先到，旧数据后到，会造成跳动
	if (this->time_stamp_cal < this->time_stamp_cal_last)
	{
		if (this->time_stamp_cal - this->time_stamp_last < MAX_SYNCTIME_DELAY_NUM)
		{
		}
		else
		{
			// 小于5，认为会影响数据
			this->x = this->last_locate.x;
			this->y = this->last_locate.y;
			this->z = 0;
			m_afmData.bStatus = true;
			m_afmData.strCardId = this->card_id;
			m_afmData.nCardStamp = this->time_stamp_cal;
			m_afmData.nType = ALGO_FAILED_CONDITION_4;
			ALGORITHM_FAILED(ALGO_FAILED_CONDITION_4);
			//此处需要输出计数
			return false;
		}
	}

	return bRet;
}

void Card::set_speed( double v )
{
	//this->final_v = v; // lyl added: final_v only can be controled by pull_data()
}

// 检查dist数据有效性
int Card::CheckDistData(int cnt)
{
	LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_140);
	GetLocalTime(&m_afmData.st);

	// dist数据少于两条直接退出
	if(cnt < 2){
		LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_141);
		m_afmData.bStatus = true;
		m_afmData.strCardId = this->card_id;
		m_afmData.nType = ALGO_FAILED_CONDITION_15;
		ALGORITHM_FAILED(ALGO_FAILED_CONDITION_15);

		return DIST_COUNT_LESS_THAN_TWO;
	}

	//主要处理当相同卡的时间戳的数据中存在同步序号大于5的情况，如果有大于5的数据则丢弃此数据
	int k = 0;
	int dst = 0;
	int st = 0;
	bool bRet = false;

	// 获取最大时间同步值
	for(DistMap::iterator it = _dists.front().distmap.begin(); it != _dists.front().distmap.end(); ++it,k++){
		if(k==0){
			st = it->second->sync_num;
		}else{
			if(st < it->second->sync_num){
				//未考虑跨周期
				st = it->second->sync_num;
			}
		}
	}

	map<unsigned long long,std::shared_ptr<_coordinate>> mp_dists_locate_ex;
	mp_dists_locate_ex.clear();

	bool bExist = false;

	// 筛选掉线性插值异常的数据
	for(DistMap::iterator it = _dists.front().distmap.begin(); it != _dists.front().distmap.end(); ++it){
		//如果同步时间戳存在异常值，则不走算法定位，直接返回上一次结果值
		if(LLONG_MAX == it->second->tt){
			bExist = true;
			LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_144);
		}else
		{
			mp_dists_locate_ex.insert(make_pair(it->second->tt,it->second));
		}
	}
	if (bExist)
	{
		//因为存在异常数据，如果抛弃了几条，剩下的数据定位错误会导致后续所有定位成功的都失败
		return DIST_COUNT_LESS_FOR_TIMESTAMP_ERROR;
	}

	if(mp_dists_locate_ex.size() < 2){
		LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_143);
		this->x = this->last_locate.x;
		this->y = this->last_locate.y;
		this->z = 0;

		m_afmData.bStatus = true;
		m_afmData.strCardId = this->card_id;
		m_afmData.nType = ALGO_FAILED_CONDITION_2;
		ALGORITHM_FAILED(ALGO_FAILED_CONDITION_2);

		return DIST_COUNT_LESS_FOR_TIMESTAMP_ERROR;
	}

	// 筛选掉同步序号与最大值差5的数据
	mp_dists_locate.clear();
	map<unsigned long long,std::shared_ptr<_coordinate>>::iterator it_mpdl = mp_dists_locate_ex.begin();
	for(;it_mpdl!=mp_dists_locate_ex.end();++it_mpdl){
		if(st - it_mpdl->second->sync_num < 5){
			mp_dists_locate.insert(make_pair(it_mpdl->second->tt, it_mpdl->second));
		}else{
			LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_142);
		}
	}

	if(mp_dists_locate.size() < 2){
		LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_143);
		this->x = this->last_locate.x;
		this->y = this->last_locate.y;
		this->z = 0;

		m_afmData.bStatus = true;
		m_afmData.strCardId = this->card_id;
		m_afmData.nType = ALGO_FAILED_CONDITION_2;
		ALGORITHM_FAILED(ALGO_FAILED_CONDITION_2);

		return DIST_COUNT_LESS_FOR_SYNC_NUM_DIFFER_FIVE;
	}

	if (!CheckCardCtValid())
	{
		//如果卡的ct存在当前比之前的小，且是未跨周期的情况下，则认为此组数据无效
		return DIST_COUNT_CARD_CUR_CT_LESS_LAST;
	}

	return 0;
}

/*
* 组装数据
*
* param
*		pRdm
*
* return
*		成功返回0，失败返回非零值
*
*/
int Card::AssembleDistData(std::shared_ptr<ReceiveDataMap> pRdm)
{
	int maxSyncTimes = 0;

	//保存加速度当前状态和上次状态
	int acce_state = 0;
	int acce_state_last = 0;
	int ins_direction = 0;
	int card_time_stamp = 0;

	map<unsigned long long,std::shared_ptr<_coordinate>>::iterator it_mpdl = mp_dists_locate.begin();
	int i = 0;
	for(;it_mpdl!=mp_dists_locate.end();++it_mpdl){
		if(i==0){
			card_time_stamp = it_mpdl->second->t;
			maxSyncTimes = it_mpdl->second->sync_num;
			acce_state = it_mpdl->second->acce_state;
			acce_state_last = it_mpdl->second->acce_state_last;
			ins_direction = it_mpdl->second->ins_direction;
		}
		else{
			if(maxSyncTimes < it_mpdl->second->sync_num){
				maxSyncTimes = it_mpdl->second->sync_num;
				acce_state = it_mpdl->second->acce_state;
				acce_state_last = it_mpdl->second->acce_state_last;
				ins_direction = it_mpdl->second->ins_direction;
			}
		}
		ReceiveDataMap::iterator prdm_it = pRdm->find(it_mpdl->second->tt);
		if(prdm_it == pRdm->end()){
			//保存信息用于定位
			std::shared_ptr<ReceiveData> prd = std::make_shared<ReceiveData>();
			prd->reader_id = it_mpdl->second->reader_id;
			prd->antenna_id = it_mpdl->second->antenna_id;
			prd->rec_time_stamp = it_mpdl->second->tt;
			prd->x = it_mpdl->second->x*this->map_scale;
			prd->y = it_mpdl->second->y*this->map_scale;
			prd->z = it_mpdl->second->z*this->map_scale;
			prd->special = it_mpdl->second->special;
			if (prd->rec_time_stamp > 0)
			{
				pRdm->insert(make_pair(prd->rec_time_stamp,prd));
			}
		}	
		i++;
	}

	m_nCalcSyncNum = maxSyncTimes;
	acce_cur_state = acce_state;
	time_stamp_cal = card_time_stamp;

	return 0;
}

/*
* 组装数据
*
* param
*
* return
*
*
*/
int Card::AssembleDistData(map<unsigned long long, std::shared_ptr<_coordinate>>& p_dist_locate)
{
	int i = 0;
	int nCount = 0;
	bool bFound = false;

	unordered_map<int,std::list<double>> check_dist_data;
	for(DistMap::iterator it = _dists.front().distmap.begin(); it != _dists.front().distmap.end(); ++it){
		bFound = false;
		int tct = 0;
		tct = it->second->t;
		// 判断是否已经加入到列表
		for(int j = 0; j < i; j++){
			if(p_dist_locate.find(it->second->reader_id) != p_dist_locate.end()){
				//已存在，则退出查找for循环
				bFound = true;
				break;
			}
		}
		if(i < DIST_COUNT && !bFound){
			if (abs(it->second->d + 1.0) < ZERO_PRECISION)
			{
				//即计算的距离为-1，抛弃此数据
				continue;
			}else{
				p_dist_locate.insert(make_pair(i,(it->second)));
				check_dist_data[it->second->reader_id].push_back(it->second->d);
			}
			i++;
		}
	}

	//二维或三维定位不需要地图路径集概念
	//做一次筛选，如果dist中的分站在地图集中不存在，则删除此数据
	//避免如下情况的出现：608，502分站相同ct值的数据，但502分站无地图集，此时会走两条数据的处理逻辑，这是错误的
	/*for (map<unsigned long long, std::shared_ptr<_coordinate>>::iterator it_dist = p_dist_locate.begin();it_dist != p_dist_locate.end();)
	{
	int nReaderId = 0;
	nReaderId = it_dist->second->reader_id;
	ReaderPathMap::iterator it_tof_path = pTofReaderPathMap->find(nReaderId);
	if (it_tof_path == pTofReaderPathMap->end())
	{
	it_dist = p_dist_locate.erase(it_dist);
	}else{
	++it_dist;
	}
	}*/

	nCount = p_dist_locate.size();

	//如果存在两天线的测距距离是一样的情况，抛弃此次的数据
	if (nCount >= 2)
	{
		bool bFind = false;
		for (map<unsigned long long, std::shared_ptr<_coordinate>>::iterator tmp_dist = p_dist_locate.begin();tmp_dist != p_dist_locate.end();++tmp_dist)
		{
			bFind = false;
			map<unsigned long long, std::shared_ptr<_coordinate>>::iterator comp_dist = tmp_dist;
			std::advance(comp_dist,1);
			for (;comp_dist != p_dist_locate.end();comp_dist++)
			{
				if (abs(comp_dist->second->d - tmp_dist->second->d) < ZERO_PRECISION && comp_dist->second->reader_id == tmp_dist->second->reader_id)
				{
					bFind = true;
					break;
				}
			}
			if (bFind)
			{
				break;
			}
		}
		if (bFind)
		{
			return 1;
		}
	}

	//检查数据，如果相同分站的两天线的距离差大于3米，正常值在1~2米，则此分站的数据被抛弃
	for (unordered_map<int,std::list<double>>::iterator it = check_dist_data.begin();it != check_dist_data.end();++it)
	{
		//如果相同分站有两条距离数据，即表明此分站两天线都有测距信息
		if ((*it).second.size() == 2)
		{
			//获取两天线的距离
			std::list<double>::iterator it_first  = it->second.begin();
			std::list<double>::iterator it_second = it_first;
			std::advance(it_second,1);

			//计算两天线的距离差，如果大于3，则从p_dist_locate中删除此分站的信息
			double distance_diff = 0.0;
			distance_diff = fabs(*it_first - *it_second);
			if (distance_diff > 3)
			{
				//进入此条件，表示两分站的测距有问题
				for (map<unsigned long long, std::shared_ptr<_coordinate>>::iterator tmp_dist = p_dist_locate.begin();tmp_dist != p_dist_locate.end();)
				{
					//从定位数据中删除此分站的数据
					if (tmp_dist->second->reader_id == it->first)
					{
						tmp_dist = p_dist_locate.erase(tmp_dist);
					}else{
						++tmp_dist;
					}
				}
			}
		}
	}

	nCount = p_dist_locate.size();

	if (nCount <= 0)
	{
		return 1;
	}

	return 0;
}

int Card::SaveCardAlgoData(std::shared_ptr<POS>& pos)
{
	int nRet = 0;

	if (ranging_type == LOCATEDATATYPE::LDT_TDOA)
	{
		switch (pos->dimension)
		{
		case _1D:
			nRet = SaveTdoaData(pos);
			break;
		case _2D:
			nRet = save_tdoa_data_2d(pos);
			break;
		case _3D:
			break;
		}
	}else if (ranging_type == LOCATEDATATYPE::LDT_TOF ||ranging_type == LOCATEDATATYPE::LDT_TOF_INS)
	{
		this->is_deal_by_algo = true;
		nRet = SaveTofData(pos);
	}
	if (nRet == 0)
	{
		last_correct_position_info_time = time(NULL);
	}

	return nRet;
}

/*
* 保存tof定位结果数据
*
* param
*		pos		定位结果
*
* return
*		成功返回0，否则返回1
*/
int Card::SaveTofData(const std::shared_ptr<POS> pos)
{
	int _algorithm_path = 0;
	if(!CheckPositionValid(pos)){
		return 1;
	}else{

		//int _ret = m_position_optimizer->push_raw_data(_data_object);

		origin_locate.x = pos->cx;
		origin_locate.y = pos->cy;

		if (is_algo_first_location)
		{
			is_algo_first_location = false;	
		}

		switch(card_type){
		case CT_VEHICLE:
			{
				//如果是车，查看其中是否有已上车的人卡，将已上车的人卡坐标绑定为车的坐标
				/*for (CardMap::iterator it_card = mp_person_in_vechicle_list->begin();it_card != mp_person_in_vechicle_list->end();++it_card)
				{
				if (PERSON_ON_VEHICLE_STATUS::PERSON_ON_VEHICLE == it_card->second->person_on_card)
				{
				it_card->second->x = pos->posx;
				it_card->second->y = pos->posy;
				}
				}*/
			}
			break;
		case CT_PERSON:
			{

			}
			break;
		}

		char chLog[200] = {0};
		sprintf_s(chLog,"card:%s, ct:%d, x:%.2f, y:%.2f, reader:%d "
			, this->card_id.c_str(), this->time_stamp_cal
			, pos->cx, pos->cy
			, p_reader->reader_id
			);
		std::string log = chLog;
		Log::write_log(FILE_TYPE::RAW_LOCATION_DATA,log,true);

		time_last_tof = time_cur_algo;

		x = pos->cx;
		y = pos->cy;

		origin_locate.x = this->last_locate.x = pos->cx;
		origin_locate.y = this->last_locate.y = pos->cy;
		origin_locate.t = time_stamp_cal;	
		this->t = this->last_locate.t = time_stamp_cal;		
		this->time_stamp_cal_last = time_stamp_cal;
	}
	return 0;
}
/*
* 判断正向，反向
*
* param
*		(x1,y1)		起点坐标
*		(x2,y2)		终点坐标
*
* return	
*
*/
// int Card::get_direction(double x1,double y1,double x2,double y2)
// {
// 
// 	int nStream = 0;
// 	//车辆上下行确定
// 	//3个条件：起点（x1,y1）,终点（x2,y2）
// 	//1.x1==x2的情况下，y2>y1为下行
// 	//2.y1==y2的情况下，x2<x1为下行
// 	//3.x2<x1且y2>y1为下行
// 	//其他情况为上行
// 	if (((x1 == x2 || abs(x1 - x2) < 1E-2) && y1 < y2 )
// 		||((y1 == y2 || abs(y1 - y2) < 1E-2) && x2 < x1)
// 		||(x1 > x2 && y1 < y2))
// 	{
// 		nStream = NEGTIVE_DIRECTION;
// 	} 
// 	else
// 	{
// 		nStream = POSTIVE_DIRECTION;
// 	}
// 
// 	return nStream;
// }

int Card::SaveTdoaData(const std::shared_ptr<POS> pos)
{
	//if (IsIdleStatus(pos))
	if(false)
	{
		this->x = this->last_locate.x;
		this->y = this->last_locate.y;
		this->z = this->last_locate.z;
	}else{
		this->x = pos->cx;
		this->y = pos->cy;
		this->z = pos->cz;

		Position p;
		p.x = pos->posx;
		p.y = pos->posy;
		pTaylorWls->set_position(p);

		char chLog[MAX_LOG_LENGTH] = {0};
		std::string log = "";
#ifdef _DEBUG
		sprintf_s(chLog,"card:%s, ct:%d, x:%.2f, y:%.2f, reader:%d"
			, this->card_id.c_str(), this->time_stamp_cal
			, pos->cx, pos->cy
			, 0);	
#else
		sprintf_s(chLog,"card:%s, ct:%d, x:%.2f, y:%.2f, reader:%d"
			, this->card_id.c_str(), this->time_stamp_cal
			, pos->cx, pos->cy
			, p_reader->reader_id);
#endif
		log = chLog;
		Log::write_log(FILE_TYPE::RAW_LOCATION_DATA,log,true);

		//非怠速状态
		if (!is_algo_first_location)
		{
			//第一定位不计算上下行方向
			if (abs(this->last_locate.x) > 1E-5 || abs(this->last_locate.y) > 1E-5)
			{
				//获得上下行方向
				this->m_nStream = GetStream(this->last_locate.x,this->last_locate.y,this->x,this->y);
			}
		}

		this->last_locate.x = this->x;
		this->last_locate.y = this->y;
		this->last_locate.z = this->z;
		this->last_locate.sync_num = this->m_nCalcSyncNum;
		this->last_locate.acceleration = pos->av;
		this->time_stamp_cal_last = this->time_stamp_cal;

		int nSign = 1;
		if(pos->cvx == 0){
			if(pos->cvy > 0){
				nSign = 1;
			}
		}else{
			if(pos->cvx > 0){
				nSign = 1;
			}else{
				nSign = -1;
			}
		}

		this->m_nMoveDirection = nSign;

		if (!is_algo_first_location)
		{
			//第一次定位不计算速度
			//速度的计算采用求平均的方式
			double speed = sqrt(pow(pos->cvx,2) + pow(pos->cvy,2));
			speed *=3.6;	//转为km/h

			if (vt_his_speed.size() == 3)
			{
				vt_his_speed.pop_front();
			}
			if (speed > 1E-4)
			{
				vt_his_speed.push_back(speed);
			}	

			int total = 0;
			double sum_speed = 0;
			for (list<double>::iterator it = vt_his_speed.begin();it != vt_his_speed.end();++it)
			{
				if (*it > 0)
				{
					sum_speed += *it;
					total++;
				}
			}
			double av = 0;
			if (total > 0)
			{
				av = sum_speed / total;
			}
			if (STATE_ACCE_STATIC == this->acce_cur_state || STATE_ACCE_IDLING == this->acce_cur_state)
			{
				this->v = 0;
			} 
			else
			{
				this->v = av*nSign;
			}
		}
	}

	this->last_locate.sync_num = this->m_nCalcSyncNum;
	this->m_pKalmanFilter->m_pCar->t = this->m_nLastLocateT = this->m_nCalcSyncNum;
	this->last_vx = pos->cvx;
	this->last_vy = pos->cvy;

	if (is_algo_first_location)
	{
		is_algo_first_location = false;
	}

	if (FIT_POSITION_NUM*4 == cur_fit_nums)
	{
		this->v = 0;
	}

	this->last_locate.v = this->v;

	if(pos->update){
		sync_data sdNew;
		sdNew.sync_num = this->m_nCalcSyncNum;
		sdNew.x = this->x;
		sdNew.y = this->y;
		sdNew.vx = this->last_vx;
		sdNew.vy = this->last_vy;
		m_syncNumList.push_back(sdNew);
	}

	//保存历史数据
	std::shared_ptr<POS> p = std::make_shared<POS>();
	p->posx = this->x*map_scale;
	p->posy = this->y*map_scale;
	p->card_count = this->time_stamp_cal;

	//拟合点作为参考点
	//拟合数据的计算
	if (his_pos.size() == REF_POSITION_NUM)
	{
		his_pos.pop_front();
	}
	his_pos.push_back(p);

	int nRef = REF_POSITION_NUM * 4;
	if (long_his_pos.size() == nRef)
	{
		long_his_pos.pop_front();
	}
	long_his_pos.push_back(p);

	if(is_ref_pos)
	{
		//保存历史数据
		std::shared_ptr<POS> p = std::make_shared<POS>();
		p->posx = this->x*map_scale;
		p->posy = this->y*map_scale;
		p->card_count = this->time_stamp_cal;

		//拟合数据的计算
		if (REF_POSITION_NUM == his_pos.size())
		{
			his_pos.pop_front();
		}
		his_pos.push_back(p);
		if (REF_POSITION_NUM == his_pos.size())
		{
			if (!have_long_fit_pos)
			{
				CalcFittingData(his_pos);
				cur_fit_nums = 0;
			}	
		}

		int nRef = REF_POSITION_NUM * 4;
		if (long_his_pos.size() == nRef)
		{
			long_his_pos.pop_front();
		}
		long_his_pos.push_back(p);

		if (long_his_pos.size() == nRef)
		{
			CalcLongFittingData();
			cur_fit_nums = 0;
		}
	}

	return 0;
}

/*
* 通过加速度或速度阈值来检查p定位结果的正确性
*
* param
*		p		定位结果
*
* return
*		如果通过检查返回0，否则返回非零值
*/
int Card::CheckSolution(std::shared_ptr<POS>& p)
{
	//定位成功
	double cx = p->cx;
	double cy = p->cy;
	double cz = p->cz;

	double cvx = 0.0,cvy = 0.0,cvz = 0.0;
	double av = 0.0;

	if(is_algo_first_location){
		sync_data sdNew;
		sdNew.sync_num = this->m_nCalcSyncNum;
		sdNew.x = cx;
		sdNew.y = cy;
		sdNew.vx = 0;
		sdNew.vy = 0;
		sdNew.update = false;
		m_syncNumList.push_back(sdNew);
	}else{
		//现在的关于同步序号的处理是这样的：
		//如果定位成功，就把这次定位成功的同步数据：同步序号，坐标；x,y方向的速度，扔到一个队列里，
		//后来定位成功的就会先根据同步序号差用加速度抛一次；
		//抛不掉，就用队列里的同步数据（从后往前找），找到第一个与当前同步序号相差大于5的同步数据来进行第二次计算速度以及加速度，
		//如果加速度大于5，就不要此次的定位数据，
		//如果通过加速度判断就将队列中从头开始到此同步数据的所有元素都丢弃，并插入新的此次同步数据
		double interval_time = algorithm::base::ANCHOR_SYNCHRONIZATION_PERIOD;
		double deltaT = 0;
		sync_data sd;

		if (this->b_long_interval)
		{   
			//此段代码用于将上一次定位是根据两个时间差是个很大值而定位出的结果
			//当后续定位时就和最近的定位结果进行比较
			//例如：当上一次同步序号是14321，它定位时比较的同步序号是14200，时间差大于20多秒
			//这时我们就将b_long_interval置为true
			//当本次定位，同步序号是14326，,这时就需要根据最近的14321进行判断
			list<sync_data>::reverse_iterator it = m_syncNumList.rbegin();
			sync_data sd = *it;

			//以下计算deltaT还需要考虑卡的同步序号轮回的情况。
			if (m_nCalcSyncNum > it->sync_num)
			{
				deltaT = (m_nCalcSyncNum - sd.sync_num)*interval_time;
			} 
			else
			{
				deltaT = (m_nCalcSyncNum + 65536 - sd.sync_num)*interval_time;
			}
			if (deltaT < 10 && deltaT > 0)
			{
				this->b_long_interval = false;
			}
			//避免同一个同步序号下存在多个不同卡序号
			if (deltaT < 1E-2)
			{
				deltaT = 0.2;
			}

			p->diff_reader_sync_num = deltaT;
			p->ref_x = sd.x;
			p->ref_y = sd.y;

			cvx = abs((cx - sd.x)*this->map_scale/deltaT);
			cvy = abs((cy - sd.y)*this->map_scale/deltaT);

			double avx = (cvx - sd.vx) / deltaT;
			double avy = (cvy - sd.vy) / deltaT;
			av = sqrt(pow(avx,2) + pow(avy,2));
			//车卡的加速度
			switch(this->card_type){
			case CT_PERSON:
				if(av > PERSON_ACCELERATE_THRESHOLD){
					p->reason = CHECK_PERSON_ACCE_OVER_SPEED;
					p->posx = INVALID_COORDINATE;
					p->posy = INVALID_COORDINATE;
					p->posz = INVALID_COORDINATE;
					p->cx = p->posx / map_scale;
					p->cy = p->posy / map_scale;
					p->cz = p->posz / map_scale;
					this->b_long_interval = false;

					m_afmData.bStatus = true;
					m_afmData.strCardId = this->card_id;
					m_afmData.nType = ALGO_FAILED_CONDITION_5;
					ALGORITHM_FAILED(ALGO_FAILED_CONDITION_5);
					return CHECK_PERSON_ACCE_OVER_SPEED;
				}
				break;
			case CT_VEHICLE:
				if(av > VECHILE_ACCELERATE_THRESHOLD){
					//保留上次结果
					p->reason = CHECK_VEHICLE_ACCE_OVER_SPEED;
					p->posx = INVALID_COORDINATE;
					p->posy = INVALID_COORDINATE;
					p->posz = INVALID_COORDINATE;
					p->cx = p->posx / map_scale;
					p->cy = p->posy / map_scale;
					p->cz = p->posz / map_scale;
					this->b_long_interval = false;

					m_afmData.bStatus = true;
					m_afmData.strCardId = this->card_id;
					m_afmData.nType = ALGO_FAILED_CONDITION_6;
					ALGORITHM_FAILED(ALGO_FAILED_CONDITION_6);
					return CHECK_VEHICLE_ACCE_OVER_SPEED;
				}
				break;
			}	
		}else{
			//从队列尾部开始查找，找到第一个同步序号与当前计算卡的同步序号相差5个以上的数据
			list<sync_data>::reverse_iterator it;
			bool bOverflow = false;
			bool bfind  =false;

			for(it = m_syncNumList.rbegin();it!=m_syncNumList.rend();it++){
				if(m_nCalcSyncNum - it->sync_num >= 5){
					bfind = true;
					sd = *it;
					break;
				}else{
					if(m_nCalcSyncNum - it->sync_num < 0 && m_nCalcSyncNum < 100){
						//如果最新同步号小于列表中的同步号则
						if(m_nCalcSyncNum + 65536 - it->sync_num >=5 ){//5
							bOverflow = true;
							bfind = true;
							sd = *it;
						}
					}else{
						continue;
					}
				}//end else
			}//end for

			//根据溢出条件来计算deltaT
			if(bOverflow){
				deltaT = (m_nCalcSyncNum + 65536 - sd.sync_num)*interval_time;
			}else{
				deltaT = (m_nCalcSyncNum - sd.sync_num)*interval_time;
			}

			p->diff_reader_sync_num = deltaT;
			p->ref_x = sd.x;
			p->ref_y = sd.y;

			//速度正负的判断：以x轴，y轴正向运动为正
			//如果x相等，则y2 - y1 > 0为正
			//其他情况，则x2 - x1 > 0 为正
			int nSign = 1;
			if(cx == sd.x){
				if(cy > sd.y){
					nSign = 1;
				}else{
					nSign = -1;
				}
			}else{
				if(cx > sd.x){
					nSign = 1;
				}else{
					nSign = -1;
				}
			}

			//转为m/s
			cvx = abs((cx - sd.x)*this->map_scale/deltaT);
			cvy = abs((cy - sd.y)*this->map_scale/deltaT);

			double cv = sqrt(pow(cvx,2) + pow(cvy,2));
			cv = cv*nSign;

			double avx = (cvx - sd.vx) / deltaT;
			double avy = (cvy - sd.vy) / deltaT;
			double av = sqrt(pow(avx,2) + pow(avy,2));
			//车卡的加速度
			switch(this->card_type){
			case CT_PERSON:
				if(av > PERSON_ACCELERATE_THRESHOLD){
					p->reason = CHECK_PERSON_ACCE_OVER_SPEED;
					p->posx = INVALID_COORDINATE;
					p->posy = INVALID_COORDINATE;
					p->posz = INVALID_COORDINATE;
					p->cx = p->posx / map_scale;
					p->cy = p->posy / map_scale;
					p->cz = p->posz / map_scale;

					m_afmData.bStatus = true;
					m_afmData.strCardId = this->card_id;
					m_afmData.nType = ALGO_FAILED_CONDITION_5;
					ALGORITHM_FAILED(ALGO_FAILED_CONDITION_5);
					return CHECK_PERSON_ACCE_OVER_SPEED;
				}
				break;
			case CT_VEHICLE:
				if(av > VECHILE_ACCELERATE_THRESHOLD){
					//保留上次结果
					p->reason = CHECK_VEHICLE_ACCE_OVER_SPEED;
					p->posx = INVALID_COORDINATE;
					p->posy = INVALID_COORDINATE;
					p->posz = INVALID_COORDINATE;
					p->cx = p->posx / map_scale;
					p->cy = p->posy / map_scale;
					p->cz = p->posz / map_scale;

					m_afmData.bStatus = true;
					m_afmData.strCardId = this->card_id;
					m_afmData.nType = ALGO_FAILED_CONDITION_6;
					ALGORITHM_FAILED(ALGO_FAILED_CONDITION_6);
					return CHECK_VEHICLE_ACCE_OVER_SPEED;
				}
				break;
			}

			cv = cv*3.6;
			double max_speed = 0.0;
			switch (this->card_type)
			{
			case CT_PERSON:
				max_speed = MAX_PERSON_SPEED;
				break;
			case CT_VEHICLE:
				max_speed = MAX_VECHILE_SPEED;
				break;
			default:
				break;
			}
			//速度的限制
			if(fabs(cv) > max_speed){
				p->reason = CHECK_VEHICLE_OVER_SPEED;
				p->posx = INVALID_COORDINATE;
				p->posy = INVALID_COORDINATE;
				p->posz = INVALID_COORDINATE;
				p->cx = p->posx / map_scale;
				p->cy = p->posy / map_scale;
				p->cz = p->posz / map_scale;

				m_afmData.bStatus = true;
				m_afmData.strCardId = this->card_id;
				m_afmData.nType = ALGO_FAILED_CONDITION_7;
				ALGORITHM_FAILED(ALGO_FAILED_CONDITION_7);
				return CHECK_VEHICLE_OVER_SPEED;
			}		

			//使用间隔来修正速度
			if(deltaT - 1.0 >= 0){
				if (deltaT > 10 && bfind)
				{
					this->b_long_interval = true;
				}
				//删除第一个元素到tmp（含）之间的所有元素
				bool bStartDel = false;
				for(list<sync_data>::reverse_iterator tmp = m_syncNumList.rbegin();tmp != m_syncNumList.rend();)
				{
					if(bStartDel){
						tmp = list<sync_data>::reverse_iterator(m_syncNumList.erase((++tmp).base()));		
					}else{
						if(*tmp == sd){
							bStartDel = true;
						}
						++tmp;
					}					
				}
			}

			if (!bfind)
			{
				cvx = 0;
				cvy = 0;
				cvz = 0;
				av = 0;
			}
		}
		this->m_nSyncNumInList = sd.sync_num;
		p->update = true;
	}

	p->cvx = cvx;
	p->cvy = cvy;
	p->cvz = cvz;
	p->av = av;

	return 0;
}

/*
* 通过校验后的数据, 保存到 m_syncNumList 容器
*
* param
*		position	定位结果
*/
void Card::saveToSyncNumDataList(int sync_num, double pixelX, double pixelY, double speedX, double speedY, bool updated)
{
	sync_data sdNew;
	sdNew.sync_num = sync_num;
	sdNew.x = pixelX;
	sdNew.y = pixelY;
	sdNew.vx = speedX;
	sdNew.vy = speedY;
	sdNew.update = updated;
	m_syncNumList.push_back(sdNew);
}

/*
* 通过趋向性判断解的可靠性，暂通过上下行判断
*
* param
*		p		当前定位坐标信息
*
* return
*		通过验证返回0，否则返回错误码
*
*/
int Card::CheckSulutionByStream(std::shared_ptr<POS> p)
{
	if (INIT_STREAM == m_nStream)
	{
		return 0;
	}

	if (abs(this->last_locate.x) > 1E-5 || abs(this->last_locate.y) > 1E-5)
	{
		//去除采集刚启动，last_locate的坐标为零而导致判断车辆上下行方向错误的问题
		//车辆上下行确定
		//3个条件：起点（x1,y1）,终点（x2,y2）
		//1.x1==x2的情况下，y2>y1为下行
		//2.y1==y2的情况下，x1>x2为下行
		//3.x1>x2且y2>y1为下行
		//其他情况为上行
		int nStream = 0;
		double cx = p->posx / (map_scale*1.0);
		double cy = p->posy / (map_scale*1.0);
		if ((this->last_locate.x == cx && cy > this->last_locate.y)
			||(this->last_locate.x > cx && cy == this->last_locate.y)
			||(this->last_locate.x > cx && cy > this->last_locate.y))
		{
			nStream = DOWN_STREAM;
		} 
		else
		{
			nStream = UP_STREAM;
		}

		if (nStream != m_nStream)
		{
			return 1;
		}
	}
	return 0;
}

bool Card::IsExistPath(int left,int right)
{
	TDOAReaderPathMap::iterator it = pTdoaReaderPathMap->find(left);
	if (it == pTdoaReaderPathMap->end())
	{
		return false;
	}

	ReaderPathMap::iterator it_s = it->second->find(right);
	if (it_s == it->second->end())
	{
		return false;
	}

	return true;
}

/*
* 判断是否为怠速状态
*
* param
*		pos			定位结果
*
* return
*		如果怠速则返回true，否则返回false
*
*/
bool Card::IsIdleStatus(std::shared_ptr<POS> pos)
{
	// 只根据最后位置比较，位移小于预设值认为是怠速
	is_idling = sqrt(pow(pos->cx - this->x, 2) + pow(pos->cy - this->y, 2)) < MIN_IDLE_THRESHOLD ? true : false;
	if(is_idling)
	{
		if(idle_count < IDLE_CONFIRM_TIME)
		{
			idle_count++;
		}
	}
	else
	{
		idle_count = 0;
	}
	return is_idling;

	bool bRet = false;
	if (idle_pos_list.size() < REF_POSITION_NUM)
	{
		return false;
	}

	double acq = 0.3;	//精度范围
	_point cp;
	cp.x = cp.y = cp.z = 0;

	int i = 0;
	double ax = 0,ay = 0;
	//怠速队列数据够REF_POSITION_NUM个，计算怠速队列中数据的平均值；不够则直接返回
	//if (idle_pos_list.size() == REF_POSITION_NUM)
	{
		for (std::list<std::shared_ptr<POS>>::iterator it = idle_pos_list.begin(); it != idle_pos_list.end();++it)
		{
			ax += (*it)->posx;
			ay += (*it)->posy;
		}

		ax /= REF_POSITION_NUM;
		ay /= REF_POSITION_NUM;
	}

	i = 0;
	//计算怠速队列中数据是否都在以平均值为中心，半径是1米的范围内；如果都在，则为怠速，否则为非怠速
	for (std::list<std::shared_ptr<POS>>::iterator it = idle_pos_list.begin(); it != idle_pos_list.end();++it)
	{
		double distance = 0;

		distance = sqrt(pow((*it)->posx - ax,2) + pow((*it)->posy - ay,2));
		if (distance < 1)
		{
			i++;
		}else{
			break;
		}
	}
	if (i == 5)
	{
		bRet = true;
	}

	return bRet;
}

/*
* 判断定位结果回退，根据本次的上下行和上一次的上下行方向是否一致
*
* param
*		pos		定位结果
*
* return
*		如果回退返回true，否则返回false
*
*/
bool Card::IsRebound(std::shared_ptr<POS> pos)
{
	//如果是第一次定位，则不判断回退
	if (is_algo_first_location)
	{
		return false;
	}

	//如果上一次无方向或者本次定位结果异常，则不判断是否回退
	if (m_nStream == 0 || !CheckPositionValid(pos))
	{
		return false;
	}

	int nStream = 0;

	//上一次定位为起点，本次定位结果为终点，判断上下行方向
	nStream = GetStream(last_locate.x,last_locate.y,pos->cx,pos->cy);

	return nStream == this->m_nStream?false:true;
}

/*
* 检查解的有效性：1.非法值检查；2.指定无效数检查；3.零值检查
*
* param
*		pos			定位结果
*
* return
*		如果pos中的posx，posy有效则返回true；否则返回false
*
*/
bool Card::CheckPositionValid(const std::shared_ptr<POS> pos)
{
	if (pos == nullptr)
	{
		return false;
	}else if (_isnan(pos->posx) || _isnan(pos->posy))
	{
		return false;
	}else if (pos->posx == INVALID_COORDINATE || pos->posy == INVALID_COORDINATE)
	{
		return false;
	}else if (abs(pos->posx) < ZERO_PRECISION && abs(pos->posy) < ZERO_PRECISION)
	{
		//如果以后地图有(0,0)坐标的话，就需要删除此判断条件
		return false;
	}

	return true;
}

bool Card::CheckPositionValid(const double& x,const double& y)
{
	if (_isnan(x) || _isnan(y))
	{
		return false;
	}else if (x == INVALID_COORDINATE || y == INVALID_COORDINATE)
	{
		return false;
	}else if (abs(x) < HIGH_ZERO_PRECISION && abs(y) < HIGH_ZERO_PRECISION)
	{
		//如果以后地图有(0,0)坐标的话，就需要删除此判断条件
		return false;
	}

	return true;
}

/*
* tof定位：根据一条距离及历史定位结果进行定位
*
* param
*		pos			定位结果
*
* return
*		成功定位返回0，否则返回非零值
*
*/
int Card::get_position_by_one_distance(std::shared_ptr<TOFReaderPathMap> trpm,std::shared_ptr<POS>& pos,const TOF_REFER_DATA refer_data)
{
	//如果是第一次定位，则无法计算，必须要依赖历史值
	if (is_algo_first_location)
	{
		return 1;
	}
	//如果上次坐标是无定位结果的，则无法处理
	if (last_locate.x == INVALID_COORDINATE && last_locate.y == INVALID_COORDINATE)
	{
		return 1;
	}

	int nReaderId = 0;
	nReaderId = refer_data.nReaderId;

	int seg_num = 0;
	//如果分站不存在，则返回
	if(trpm->find(nReaderId) == trpm->end())
	{
		return 1;
	}
	seg_num = trpm->find(nReaderId)->second->nRealCalcPoints - 1;
	//判断路径集的点数是否大于两个点，两点才可以构成线段路径
	if(seg_num < 0 ){
		return 1;
	}

	int j = 0;
	int m = 0;
	int nCounts = 0;
	double xcross[MAX_READER_TDOA_PATH_NUMS] = {0};
	double ycross[MAX_READER_TDOA_PATH_NUMS] = {0};
	//对每一个线段所在的直线和以天线为圆心的圆求解，最多为两个解
	for(int i = 0;i < seg_num;i++){
		//如果线段两端点的坐标不存在，则寻找下一个线段
		if(trpm->find(nReaderId)->second->px[i] == -1 || trpm->find(nReaderId)->second->px[i+1] == -1)
		{
			continue;
		}

		//求线段所在直线和分站readerid的antenna_idx天线为圆心，半径为dist的圆的两个交点
		std::shared_ptr<SOLUTION> r = LocateAlgorithm::GetPos(trpm,nReaderId,refer_data.nAntennaIndex,refer_data.dDistance,i);
		//如果解不存在，则继续寻找下一个线段
		if(r == nullptr || r->nCount <= 0){
			continue;
		}
		for (int j = 0;j<r->nCount;j++)
		{
			xcross[m] = r->x[j];
			ycross[m] = r->y[j];
			m++;
		}
	}

	double v[MAX_READER_TDOA_PATH_NUMS] = {0};
	double dt = 0;
	dt = GetDeltaT();

	int nNotOverThre = 0;
	int nOverThre = 0;

	for (int i = 0;i < m;i++)
	{
		double distance = 0;
		distance = sqrt(pow(abs(xcross[i] - last_locate.x*map_scale),2) + pow(abs(ycross[i] - last_locate.y*map_scale),2));
		v[i] = distance/dt;
		v[i] *= 3.6;

		double speed_thre = 0.0;
		switch(card_type){
		case CT_VEHICLE:
			speed_thre = MAX_VECHILE_SPEED;
			break;
		case CT_PERSON:
			speed_thre = MAX_PERSON_SPEED;
			break;
		}	
		if (v[i] > speed_thre)
		{
			nOverThre++;
		}else{
			nNotOverThre++;
		}
	}

	double minV = 0.0;
	int nIndex = -1;
	for (int i = 0;i < m;i++)
	{
		if (i == 0)
		{
			minV = v[i];
			nIndex = i;
			continue;
		}
		if (v[i] < minV)
		{
			minV = v[i];
			nIndex = i;
		}
	}

	if (nOverThre > 0 && nNotOverThre > 0)
	{
		if (nIndex == -1)
		{
			return 1;
		}
		pos->posx = xcross[nIndex];
		pos->posy = ycross[nIndex];
		pos->posz = 0;
		pos->cx = pos->posx/map_scale;
		pos->cy = pos->posy/map_scale;
		pos->cz = pos->posz/map_scale;
		return 0;
	}else{
		if (m == 1)
		{
			pos->posx = xcross[nIndex];
			pos->posy = ycross[nIndex];
			pos->posz = 0;
			pos->cx = pos->posx/map_scale;
			pos->cy = pos->posy/map_scale;
			pos->cz = pos->posz/map_scale;
			return 0;
		}else{
			return 1;
		}
	}

	return 0;
}

/*
*	检查tof定位解的有效性，通过速度和加速度方式校验
*
*	param
*		pos			定位结果
*	
*	return
*		通过检验返回true，否则返回false；
*
*/
bool Card::CheckSolutionTof(std::shared_ptr<POS>& pos)
{
	if (is_algo_first_location)
	{
		return true;
	}

	bool bRet = false;

	double delta = 0.0;
	delta = GetDeltaT();

	if (delta > ZERO_PRECISION){
		double distance = 0;
		distance = sqrt(pow(pos->posx - last_locate.x*map_scale,2) + pow(pos->posy - last_locate.y*map_scale,2));

		//计算速度
		double v = 0;
		v = distance/delta;

		//计算加速度
		double a = 0;
		a = (v - final_v) / delta;

		v *= 3.6;

		double acce_thre = 0.0;
		double speed_thre = 0.0;
		switch(card_type){
		case CT_VEHICLE:
			acce_thre = VECHILE_ACCELERATE_THRESHOLD;
			speed_thre = MAX_VECHILE_SPEED;
			break;
		case CT_PERSON:
			acce_thre = PERSON_ACCELERATE_THRESHOLD;
			speed_thre = MAX_PERSON_SPEED;
			break;
		}	
		if (a > acce_thre)
		{
			bRet = false;
		}else{
			pos->cvx = (pos->posx - last_locate.x*map_scale) / delta;
			pos->cvy = (pos->posy - last_locate.y*map_scale) / delta;
			bRet = true;
		}

		if (v > speed_thre)
		{
			bRet = false;
		}
	}


	if (!bRet)
	{
		double output_v = 0.0;
		if (fabs(final_v) > 100)
		{
			output_v = 100;
		}

		char log[MAX_LOG_LENGTH] = {0};
		sprintf_s(log,"card_id: %s, ct: %d, distance: %.3f, posx: %.3f, posy: %.3f, last_locate.x: %.3f, last_locate.y: %.3f, deltaT: %d, a:%.3f, v: %.3f",/*, algo_time: %s, time_last: %s"*/
			card_id.c_str(),
			time_stamp_cal,
			distance,
			pos->posx/map_scale,
			pos->posy/map_scale,
			last_locate.x,
			last_locate.y,
			delta,
			a,
			output_v
			);

		Log::write_log(FILE_TYPE::ALGO_S,log,true);
	}

	return bRet;
}

/*
* 根据dists获取参考数据
*
* param
*		tmp_dists_locate
*		key
*
* return
*		返回参考数据
*
*/
int Card::get_ref_data_from_dists(map<unsigned long long, std::shared_ptr<_coordinate>> tmp_dists_locate,const int& key,TOF_REFER_DATA& refer_data)
{
	map<unsigned long long, std::shared_ptr<_coordinate>>::iterator it = tmp_dists_locate.find(key);
	if (it != tmp_dists_locate.end())
	{
		time_cur_algo = it->second->cur_time;
		reader_id_algo = it->second->reader_id;

		refer_data.nCardTimeStamp = it->second->t;
		refer_data.llDeltaTime = 0;
		refer_data.nReaderId = it->second->reader_id;
		refer_data.nAntennaIndex = it->second->antenna_id;
		refer_data.dDistance = it->second->d;
		refer_data.x = it->second->x;
		refer_data.y = it->second->y;
		refer_data.z = it->second->z;
		refer_data.a = it->second->a;
	}

	return 0;
}

/*
* 1.先将在同一个分站的两天线的距离以及天线坐标保存
* 2.计算定位结果距离相同分站的两天线的距离差
* 3.根据距离差的方向和原有的天线距离的方向进行判断，如果一致，保留结果，如果不一致，则不保留此结果
*
* param
*		pos				定位结果
*		p_dist_locate	原始定位数据结果
*
* return
*	如果方向一致，返回0，否则返回1
*/
int Card::position_data_filter_by_direction(std::shared_ptr<POS> pos,map<unsigned long long, std::shared_ptr<_coordinate>>& p_dist_locate)
{
	//如果只有一条数据，则不用使用此逻辑处理
	if (p_dist_locate.size() < 2)
	{
		return 0;
	}

	if (last_locate.x == INVALID_COORDINATE && last_locate.y == INVALID_COORDINATE)
	{
		return 0;
	}

	typedef struct Data{
		double distance;
		double x;
		double y;
		double z;
		int antenna_id;

		Data(){
			distance = x = y = z = 0.0;
			antenna_id = 0;
		}

		Data& operator=(Data& d){
			this->antenna_id = d.antenna_id;
			this->distance = d.distance;
			this->x = d.x;
			this->y = d.y;
			this->z = d.z;
			return *this;
		}

	}data;

	//将分站的双天线的距离及坐标信息保存下来
	unordered_map<int,unordered_map<int,data>> check_dist_data;
	for(map<unsigned long long, std::shared_ptr<_coordinate>>::iterator it = p_dist_locate.begin(); it != p_dist_locate.end(); ++it){
		data val;
		val.distance = it->second->d;
		val.x = it->second->x*map_scale;
		val.y = it->second->y*map_scale;
		val.antenna_id = it->second->antenna_id;
		check_dist_data[it->second->reader_id].insert(std::make_pair(it->second->antenna_id,val));
	}

	bool bNotExist = true;
	for (unordered_map<int,unordered_map<int,data>>::iterator it = check_dist_data.begin();it != check_dist_data.end();++it)
	{
		if (it->second.size() > 1)
		{
			int nOldSign = 0;
			int nNewSign = 0;

			//计算原分站双天线的距离差方向
			data d1;
			data d2;

			for (unordered_map<int,data>::iterator it_ant = check_dist_data[it->first].begin();it_ant != check_dist_data[it->first].end();++it_ant)
			{
				switch (it_ant->second.antenna_id)
				{
				case 0:
					d1 = it_ant->second;
					break;
				case 1:
					d2 = it_ant->second;
					break;
				}
			}

			if (d1.distance > d2.distance)
			{
				nOldSign = 1;
			}else{
				nOldSign = -1;
			}

			//计算坐标距离分站双天线的方向
			double distance0 = 0.0;
			double distance1 = 0.0;

			distance0 = sqrt(pow(pos->posx - d1.x,2) + pow(pos->posy - d1.y,2));
			distance1 = sqrt(pow(pos->posx - d2.x,2) + pow(pos->posy - d2.y,2));

			if (distance0 > distance1)
			{
				nNewSign = 1;
			} 
			else
			{
				nNewSign = -1;
			}

			if (nOldSign == nNewSign)
			{
				continue;
			}else{
				//只要对一个分站的两天线不满足，则直接退出
				bNotExist = false;
				break;
			}
		}
	}

	if (!bNotExist)
	{
		return 1;
	}

	return 0;
}

/*
* 判断上下行方向
*
* param
*		(x1,y1)		起点坐标
*		(x2,y2)		终点坐标
*
* return
*		上行为1，下行为2，无方向为0
*
*/
int Card::GetStream(double x1,double y1,double x2,double y2)
{
	int nStream = 0;
	//车辆上下行确定
	//3个条件：起点（x1,y1）,终点（x2,y2）
	//1.x1==x2的情况下，y2>y1为下行
	//2.y1==y2的情况下，x2>x1为下行
	//3.x2>x1且y2>y1为下行
	//x2<x1 y2>y1
	//其他情况为上行
	if (((x1 == x2 || abs(x1 - x2) < 1E-2) && y1 < y2 )
		||((y1 == y2 || abs(y1 - y2) < 1E-2) && x2 > x1)
		||(x1 < x2 && y1 < y2))
	{
		nStream = DOWN_STREAM;
	} 
	else
	{
		nStream = UP_STREAM;
	}

	return nStream;
}

/*
* 根据上一次的速度计算本次的新坐标
*
* param
*		pos		定位坐标
*		v		上一次的运动速度
*
* return
*		成功返回0，并将计算出的新坐标保存到pos结果中
*
*/
int Card::CalcPositionBySpeed(std::shared_ptr<POS>& pos,double v)
{
	double distance = 0;

	distance = v*pos->diff_reader_sync_num/3.6; //转为m/s

	//如果两次定位的卡的ct号相差1
	if (abs(time_stamp_cal - time_stamp_cal_last) == 1)
	{
		//时间按0.2s计算
		//速度如果按30km/h计算，则位移为1.6米左右
		//速度如果按20km/h计算，则位移为1.1米左右
		//速度如果按10km/h计算，则位移为0.5米左右
		distance = 1;
	}

	//x相等
	if (abs(pos->cx - last_locate.x) < ZERO_PRECISION)
	{
		int nSign = 0;
		/*if (last_locate.y > pos->cy)
		{
		nSign = 1;
		}
		if (last_locate.y < pos->cy)
		{
		nSign = -1;
		}*/

		if (this->m_nStream == DOWN_STREAM)
		{
			//下行
			//pos->cy = nSign*(last_locate.y + distance/map_scale);
			pos->cy = last_locate.y + distance/map_scale;
		}else if(this->m_nStream == UP_STREAM){
			//上行
			//pos->cy = nSign*(last_locate.y - distance/map_scale);
			pos->cy = last_locate.y - distance/map_scale;
		}
	}else{
		//在有斜率的地方
		double arg = atan((pos->cy - last_locate.y)/(pos->cx - last_locate.x));
		double calc_x = 0,calc_y = 0;
		if (this->m_nStream == DOWN_STREAM)
		{
			//下行
			calc_x = last_locate.x + cos(arg)*distance/map_scale;
			calc_y = last_locate.y + sin(arg)*distance/map_scale;
		}else if(this->m_nStream == UP_STREAM){
			//上行
			calc_x = last_locate.x - cos(arg)*distance/map_scale;
			calc_y = last_locate.y - sin(arg)*distance/map_scale;
		}
		pos->cx = calc_x;
		pos->cy = calc_y;		
	}

	pos->posx = pos->cx*map_scale;
	pos->posy = pos->cy*map_scale;

	return 0;
}

/*
* 保存原始定位信息
*
* param
*		pos		定位结果
*
* return
*		如果成功则为零
*/
int Card::SaveOriginDataBeforeFilter(std::shared_ptr<POS> pos)
{
	origin_locate.x = pos->cx;
	origin_locate.y = pos->cy;
	origin_locate.z = pos->cz;
	origin_locate.v = sqrt(pow(pos->cvx,2) + pow(pos->cvy,2))*3.6;	//*3.6转为km/h
	origin_locate.sync_num = this->m_nCalcSyncNum;

	if (CheckPositionValid(pos))
	{
		char log[MAX_LOG_LENGTH] = {0};
		sprintf_s(log,"cardid: %s, ct: %d, x: %.2f, y: %.2f",card_id.c_str(),time_stamp_cal,pos->cx,pos->cy);
		Log::write_log(FILE_TYPE::MOVING_READER_PARSE_DATA,log,true);
	}

	return 0;
}

/*
* 从多解中选择合适的解，会有两种结果：1.选出一个合适解；2.选不出合适解
*
* param
*		pRdm		分站定位数据集
*		udm_pos		多个可能解
*		pos			保存最终解
*
* return
*		选出合适解，将解保存到pos参数中，并返回0；选不出解，则返回非零错误码
*
*/
int Card::ChooseOneSolution(std::shared_ptr<ReceiveDataMap> pRdm, std::vector<std::shared_ptr<POS>> udm_pos, std::shared_ptr<POS>& pos )
{
	if(0 == udm_pos.size()){ // 没有解
		return SOLUTION_NO_SOLUTION;
	}

	// 只有一个解
	if(1 == udm_pos.size()){
		//判断此位置距离分站的距离是否为4米范围内
		ReceiveDataMap::iterator it_first = pRdm->end();
		ReceiveDataMap::iterator it_second = pRdm->end();

		for (ReceiveDataMap::iterator it = pRdm->begin();it != pRdm->end();++it)
		{
			if (it->second->reader_id == udm_pos.at(0)->nFirstReader)
			{
				it_first = it;
			}
			if (it->second->reader_id == udm_pos.at(0)->nSecondReader)
			{
				it_second = it;
			}
		}

		if(it_first == pRdm->end() || it_second == pRdm->end())
		{
			return SOLUTION_NOT_EXIST_READRE;
		}

		double dist = 0.0;
		if (it_first != pRdm->end())
		{
			dist = sqrt(pow(udm_pos.at(0)->posx - it_first->second->x,2)+pow(udm_pos.at(0)->posy - it_first->second->y,2));
			if (dist < NEAR_READER && it_first->second->special == 0)
			{
				return SOLUTION_NEAR_READER;
			}
		}
		if (it_second != pRdm->end())
		{
			dist = sqrt(pow(udm_pos.at(0)->posx - it_second->second->x,2)+pow(udm_pos.at(0)->posy - it_second->second->y,2));
			if (dist < NEAR_READER && it_second->second->special == 0)
			{
				return SOLUTION_NEAR_READER;
			}

		}

		pos->posx = udm_pos.at(0)->posx;
		pos->posy = udm_pos.at(0)->posy;
		pos->posz = udm_pos.at(0)->posz;
		pos->cx = pos->posx / (this->map_scale*1.0);
		pos->cy = pos->posy / (this->map_scale*1.0);
		pos->cz = pos->posz / (this->map_scale*1.0);

		TRACE(_T("solutions: ct:%d, r1: %d, r2:%d, x:%.4f y:%.4f\r\n"), this->time_stamp_cal, it_first->second->reader_id, it_second->second->reader_id, pos->cx, pos->cy);

		return 0;
	}

	ReceiveDataMap::iterator itf = pRdm->begin();
	int bf = itf->second->reader_id;
	std::advance(itf,1);
	int bs = itf->second->reader_id;
	// 有多个解,含两解或两解以上
	//判断依据：选取无地图集的分站数据来校正所有的解，
	//解可靠的条件如下：1解与无地图集的时间戳距离差最小，2且此距离差不大于10米
	for (ReceiveDataMap::reverse_iterator it_first = pRdm->rbegin();it_first!=pRdm->rend();++it_first)
	{
		ReceiveDataMap::reverse_iterator it_second = it_first;
		std::advance(it_second,1);

		if (it_second == pRdm->rend())
		{
			//找到尾部了还未找到合适解，即认为无解
			pos->reason = 1;
			break;
		}

		//反向查找，还要去掉这两个之间有地图集的数据
		int nfr = it_first->second->reader_id;
		int nfs = it_second->second->reader_id;

		if (bf!=nfs && bs!=nfr)
		{
			if (IsExistPath(nfr,nfs))
			{
				continue;
			}
		}

		int nCounts = 0;
		for (std::vector<std::shared_ptr<POS>>::iterator it_pos = udm_pos.begin();it_pos != udm_pos.end();++it_pos)
		{
			//此点在这两个线段内，才允许使用此判断条件
			_point p,l1,l2;
			p.x = (*it_pos)->posx;
			p.y = (*it_pos)->posy;
			l1.x = it_first->second->x;
			l1.y = it_first->second->y;
			l2.x = it_second->second->x;
			l2.y = it_second->second->y;

			TRACE(_T("solutions: ct:%d, r1: %d, r2:%d, x:%.4f y:%.4f\r\n"), this->time_stamp_cal, it_first->second->reader_id, it_second->second->reader_id, p.x/2.0, p.y/2.0);

			if (!LocateAlgorithm::IsInLine(p,l1,l2))
			{
				nCounts++;
				continue;
			}

			double distance_reader = 0;
			distance_reader = fabs(sqrt(
				pow((*it_pos)->posx - it_first->second->x,2) + pow((*it_pos)->posy - it_first->second->y,2)
				) - 
				sqrt(
				pow((*it_pos)->posx - it_second->second->x,2) + pow((*it_pos)->posy - it_second->second->y,2)
				));

			long long dt = it_first->second->rec_time_stamp - it_second->second->rec_time_stamp;
			double distance_tt = 0;
			distance_tt = fabs(CFunctions::getDistance(dt,CFunctions::TDOA));
			(*it_pos)->dis_diff = fabs(distance_reader - distance_tt);
		}

		if (nCounts == udm_pos.size())
		{
			continue;
		}

		double dMinDiff = 99999.9;
		std::vector<std::shared_ptr<POS>>::iterator it_find = udm_pos.end(); 
		for (std::vector<std::shared_ptr<POS>>::iterator it_pos = udm_pos.begin();it_pos != udm_pos.end();++it_pos)
		{
			if ((*it_pos)->dis_diff < dMinDiff)
			{
				_point p,l1,l2;
				p.x = p.y = p.z = l1.x = l1.y = l1.z = l2.x = l2.y = l2.z = 0;

				p.x = (*it_pos)->posx;
				p.y = (*it_pos)->posy;

				for (ReceiveDataMap::iterator it = pRdm->begin();it != pRdm->end();++it)
				{
					if (it->second->reader_id == (*it_pos)->nFirstReader)
					{
						l1.x = it->second->x;
						l1.y = it->second->y;
					}
					if (it->second->reader_id == (*it_pos)->nSecondReader)
					{
						l2.x = it->second->x;
						l2.y = it->second->y;
					}
				}

				if (!LocateAlgorithm::IsInLine(p,l1,l2))
				{
					continue;
				}else{
					if (((*it_pos)->nFirstReader == nfr &&(*it_pos)->nSecondReader == nfs)||
						((*it_pos)->nFirstReader == nfs &&(*it_pos)->nSecondReader == nfr))
					{
						continue;
					}else{
						dMinDiff = (*it_pos)->dis_diff;
						it_find = it_pos;
					}

				}	
			}
		}
		if (it_find != udm_pos.end())
		{
			if ((*it_find)->dis_diff < MIN_DIFFER_DISTANCE)
			{
				//找到即退出
				//pos = *it_find;
				pos->posx = (*it_find)->posx;
				pos->posy = (*it_find)->posy;
				pos->posz = (*it_find)->posz;
				pos->reason = 0;
				break;
			}	
		}
	}

	if (pos->reason == 0)
	{
		pos->cx = pos->posx / (this->map_scale*1.0);
		pos->cy = pos->posy / (this->map_scale*1.0);
		pos->cz = pos->posz / (this->map_scale*1.0);
	}

	// 未计算出结果
	if(pos->posx == INVALID_COORDINATE || pos->posy == INVALID_COORDINATE){
		LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_148);
		return SOLUTION_NO_SOLUTION;
	}else{
		bool bExistSolution = true;
		//对这唯一解最最后一步筛选，在分站1米内
		//增加这个条件主要是为了解决，当距离最近的分站数据未收到的情况，
		//比如实际数据应该是103,102,101，,但目前收到的数据为102,101，但定位到102附近且在102和101之间
		for (ReceiveDataMap::iterator it = pRdm->begin();it != pRdm->end();++it)
		{
			double distance = 0.0;
			distance = sqrt(pow(it->second->x - pos->posx,2) + pow(it->second->y - pos->posy,2));
			if (distance < 1)
			{
				bExistSolution = false;
				break;
			}
		}
		//无解
		if (!bExistSolution)
		{
			std::shared_ptr<POS> tmp = std::make_shared<POS>();
			pos->posx = tmp->posx;
			pos->posy = tmp->posy;
			pos->cx = 0;
			pos->cy = 0;
			pos->cz = 0;
			return SOLUTION_NO_SOLUTION;
		}
	}

	return 0;
}
string Card::store_data_card( int tag ,double xd, double yd)
{
	char sql[LENGTH_SQL] = {0};
	char _time[STR_LEN_TIME], _time_ex[STR_LEN_TIME];
	//struct tm local_time, local_time_ex; 
	time_t t = time(NULL)+10;
	xd = (xd == 0 ? x_offset_after():xd);
	yd = (yd == 0 ? y_offset_after():yd);
	struct  tm p;
	localtime_s(&p,&t);
	strftime(_time, 30, "%Y-%m-%d %H:%M:%S", &p);
	switch(tag){
	case HIS_AREA_LOCATION_LEAVE:
		//sprintf_s(_time,
		//	STR_LEN_TIME,
		//	"%u-%u-%u %u:%u:%u.%u",
		//	deal_time.wYear,deal_time.wMonth,deal_time.wDay,
		//	deal_time.wHour,deal_time.wMinute,deal_time.wSecond,deal_time.wMilliseconds);
		sprintf_s(_time_ex,
			STR_LEN_TIME,
			"%u-%u-%u %u:%u:%u.%u",
			enter_area_time.wYear,enter_area_time.wMonth,enter_area_time.wDay,
			enter_area_time.wHour,enter_area_time.wMinute,enter_area_time.wSecond,enter_area_time.wMilliseconds);
		sprintf_s(sql, LENGTH_SQL,
			"call add_area_staff(%s, %d, %d, %d, '%s','%s');",
			card_id.c_str(), id, area_id, map_id, _time_ex, _time);
		break;
	case HIS_LOCATION:
		sprintf_s(_time,
			STR_LEN_TIME,
			"%u-%u-%u %u:%u:%u.%u",
			deal_time.wYear,deal_time.wMonth,deal_time.wDay,
			deal_time.wHour,deal_time.wMinute,deal_time.wSecond,deal_time.wMilliseconds);
		sprintf_s(sql, LENGTH_SQL, 
			"INSERT IGNORE INTO his_location_staff(card_id, staff_id, cur_time, x, y, z, map_id, area_id, state,speed,mileage, landmark_id, direction_mapper_id, landmark_dist) VALUES(%s, %d, '%s', %.3f, %.3f, %.3f, %d, %d, %d,%.3f,%.4f, %d, %d, %.4f);",  
			card_id.c_str(), id, _time, xd, yd, z_offset_after(), map_id, area_id, state,get_speed(),mileage, landmark_id, landmark_direction, landmark_dis);
		break;
	default:
		return sql;
	}

	return sql;
}
/*
* 对pos结果进行校验
*
* param
*		pos			定位结果
*
* return
*		正确执行返回0，失败返回非零值
*
*/
int Card::calibration_pos_data(std::shared_ptr<POS> pos)
{
	//5.如果有解，则对唯一解做合法性验证
	//主要验证条件为：加速度和速度
	int nRet = 0;
	nRet = CheckSolution(pos);	

	if (nRet == 0)
	{
		//对通过加速度验证的解进行是否回退的判断
		if (IsRebound(pos))
		{
			if (count_change_direction < MAX_REBOUND_COUNTS)
			{
				CalcPositionBySpeed(pos,last_locate.v);
				if(CheckPositionValid(pos))
				{
					pos->cx = pos->posx / (map_scale*1.0);
					pos->cy = pos->posy / (map_scale*1.0);
					pos->update = true;
					pos->is_fit = true;

					count_change_direction++;
				}else{
					//这里可能需要加逻辑处理，避免一次回跳太多
					count_change_direction = 0;
					bIsBack = true;
				}
			} 
			else
			{
				count_change_direction = 0;
			}
		}
	}else{
		//加速度校验失败
		//加速度或速度抛弃的结果
		if (!IsRebound(pos))
		{
			//如果不是回退，说明了是往前大跳
			//加速追
		}else{
			//说明是往回大跳
			//减速走
			bIsBack = true;
		}
	}

	return 0;
}

int Card::optimization_pos(std::shared_ptr<POS> pos)
{
	int bRet = 0;
	bRet = KalmanFilterProcess(pos);
	return bRet;
}

int Card::kalman_filter_process_tdoa(std::shared_ptr<POS> pos)
{
	//卡尔曼的时间差
	double kalman_detal_t = 0;
	//分站同步序号的间隔时间，单位为秒
	double interval_time = 0.2;

	kalman_detal_t = GetDeltaT();

	if (abs(pos->diff_reader_sync_num) < ZERO_PRECISION)
	{
		pos->diff_reader_sync_num = kalman_detal_t;
	}

	//如果定位失败，通过卡尔曼滤波处理
	//if(pos->posx == INVALID_COORDINATE && pos->posy == INVALID_COORDINATE){
	if(!CheckPositionValid(pos)){
		m_afmData.nCardStamp = m_nCalcSyncNum;
		if(this->m_pKalmanFilter->m_nCounts < 3 || this->m_pKalmanFilter->m_pCar->P(0,0) > 2 || kalman_detal_t > 3){
			//if(this->m_pKalmanFilter->m_nCounts < 3 || kalman_detal_t > 3){
			//P(0,0):连续时间(大于2s)都定位失败
			//deltaT>3:距离上次成功定位时间间隔为3s
			this->x = this->last_locate.x;
			this->y = this->last_locate.y;
			this->z = 0;

			m_afmData.bStatus = true;
			m_afmData.strCardId = this->card_id;
			this->m_pKalmanFilter->m_bFlag = false;	
			m_afmData.nType = ALGO_FAILED_CONDITION_8;
			ALGORITHM_FAILED(ALGO_FAILED_CONDITION_8);
			return KALMAN_FILTER_LONG_INTERVAL;
		}

		if(this->m_pKalmanFilter->m_nCounts >= 3){
			//只有三次以上才允许使用kalman滤波以下的函数
			this->m_pKalmanFilter->Predict(kalman_detal_t);
			this->z = 0;
			pos->update = true;
		}		
	}else{
		if (!is_algo_first_location)
		{
			//避免第一次时加入了两次
			pos->update = true;
		}

		this->m_pKalmanFilter->m_bFlag = true;
		this->m_pKalmanFilter->m_nCounts++;

		this->m_pKalmanFilter->m_pCar->z(0,0) = pos->posx;
		this->m_pKalmanFilter->m_pCar->z(1,0) = pos->cvx;
		this->m_pKalmanFilter->m_pCar->z(2,0) = pos->posy;
		this->m_pKalmanFilter->m_pCar->z(3,0) = pos->cvy;

		if(this->m_pKalmanFilter->m_nCounts == 1){
			//第一次直接赋值
			this->m_pKalmanFilter->m_pCar->x = this->m_pKalmanFilter->m_pCar->z;
		}
		if(this->m_pKalmanFilter->m_nCounts == 2){
			//两次处理
			this->m_pKalmanFilter->m_pCar->z(1, 0) = (this->m_pKalmanFilter->m_pCar->z(0, 0) - this->m_pKalmanFilter->m_pCar->x(0, 0))/pos->diff_reader_sync_num;
			this->m_pKalmanFilter->m_pCar->z(3, 0) = (this->m_pKalmanFilter->m_pCar->z(2, 0) - this->m_pKalmanFilter->m_pCar->x(2, 0))/pos->diff_reader_sync_num;
			this->m_pKalmanFilter->m_pCar->x = this->m_pKalmanFilter->m_pCar->z;
		}
		if(this->m_pKalmanFilter->m_nCounts >= 3){
			//只有三次以上才允许使用kalman滤波以下的函数
			//this->m_pKalmanFilter->Predict_Correct(deltaT);
			this->m_pKalmanFilter->Predict_Correct(kalman_detal_t);

			if(fabs(pos->diff_reader_sync_num) > ZERO_PRECISION){
				if (abs(pos->ref_x) < ZERO_PRECISION && abs(pos->ref_y) < ZERO_PRECISION)
				{
				}else{
					this->m_pKalmanFilter->m_pCar->x(1,0) = (this->m_pKalmanFilter->m_pCar->x(0,0) - pos->ref_x*this->map_scale)/pos->diff_reader_sync_num;
					this->m_pKalmanFilter->m_pCar->x(3,0) = (this->m_pKalmanFilter->m_pCar->x(2,0) - pos->ref_y*this->map_scale)/pos->diff_reader_sync_num;
				}	
			}	
		}
	}

	if (pos->update)
	{
		//二维定位不需要如下内容
		//增加地图集的判定，判断定位结果是否在地图集上
		//如果不在地图集上，需要再次定位
		//需要带出定位结果的分站信息，
		//利用地图集中分站信息再次定位
		std::shared_ptr<POS> kalman_p = std::make_shared<POS>();

		kalman_p->posx = this->m_pKalmanFilter->m_pCar->x(0,0);
		kalman_p->posy = this->m_pKalmanFilter->m_pCar->x(2,0);

		////二维定位不需要再判定在地图集上了
		if(!LocateAlgorithm::IsOnMap(kalman_p,pTdoaReaderPathMap)){
			//再一次定位到地图集上
			LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_205);
			std::shared_ptr<POS> cp = LocateAlgorithm::MappingToPath(kalman_p,pTdoaReaderPathMap);

			if(cp != nullptr){
				if (cp->posx != INVALID_COORDINATE && cp->posy !=INVALID_COORDINATE)
				{
					LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_206);
					this->m_pKalmanFilter->m_pCar->x(0,0) = cp->posx;
					this->m_pKalmanFilter->m_pCar->x(2,0) = cp->posy;
				}	
			}

			cp.reset();
		}		

		pos->posx = this->m_pKalmanFilter->m_pCar->x(0,0);
		pos->posy = this->m_pKalmanFilter->m_pCar->x(2,0);
		pos->cvx  = this->m_pKalmanFilter->m_pCar->x(1,0);
		pos->cvy  = this->m_pKalmanFilter->m_pCar->x(3,0);

		pos->cx = pos->posx / (map_scale*1.0);
		pos->cy = pos->posy / (map_scale*1.0);
	}

	return 0;
}

int Card::kalman_filter_process_tof(std::shared_ptr<POS> pos)
{
	//卡尔曼的时间差
	double kalman_detal_t = 0;
	kalman_detal_t = GetDeltaT();

	//如果定位失败，通过卡尔曼滤波处理
	if(!CheckPositionValid(pos)){
		m_afmData.nCardStamp = m_nCalcSyncNum;
		if(this->m_pKalmanFilter->m_nCounts < 3 || kalman_detal_t > 6){
			//deltaT>6:距离上次成功定位时间间隔为6s
			this->x = this->last_locate.x;
			this->y = this->last_locate.y;
			this->z = 0;

			m_afmData.bStatus = true;
			m_afmData.strCardId = this->card_id;
			this->m_pKalmanFilter->m_bFlag = false;	
			m_afmData.nType = ALGO_FAILED_CONDITION_8;
			ALGORITHM_FAILED(ALGO_FAILED_CONDITION_8);
			return KALMAN_FILTER_LONG_INTERVAL;
		}

		if(this->m_pKalmanFilter->m_nCounts >= 3){
			//只有三次以上才允许使用kalman滤波以下的函数
			this->m_pKalmanFilter->Predict(kalman_detal_t);
			this->z = 0;
			pos->update = true;
		}		
	}else{
		if (!is_algo_first_location)
		{
			//避免第一次时加入了两次
			pos->update = true;
		}

		this->m_pKalmanFilter->m_bFlag = true;
		this->m_pKalmanFilter->m_nCounts++;

		this->m_pKalmanFilter->m_pCar->z(0,0) = pos->posx;
		this->m_pKalmanFilter->m_pCar->z(1,0) = pos->cvx;
		this->m_pKalmanFilter->m_pCar->z(2,0) = pos->posy;
		this->m_pKalmanFilter->m_pCar->z(3,0) = pos->cvy;

		if(this->m_pKalmanFilter->m_nCounts == 1){
			//第一次直接赋值
			this->m_pKalmanFilter->m_pCar->x = this->m_pKalmanFilter->m_pCar->z;
		}
		if(this->m_pKalmanFilter->m_nCounts == 2){
			//两次处理
			this->m_pKalmanFilter->m_pCar->z(1, 0) = (this->m_pKalmanFilter->m_pCar->z(0, 0) - this->m_pKalmanFilter->m_pCar->x(0, 0))/kalman_detal_t;
			this->m_pKalmanFilter->m_pCar->z(3, 0) = (this->m_pKalmanFilter->m_pCar->z(2, 0) - this->m_pKalmanFilter->m_pCar->x(2, 0))/kalman_detal_t;
			this->m_pKalmanFilter->m_pCar->x = this->m_pKalmanFilter->m_pCar->z;
		}
		if(this->m_pKalmanFilter->m_nCounts >= 3){
			//只有三次以上才允许使用kalman滤波以下的函数
			//this->m_pKalmanFilter->Predict_Correct(deltaT);
			this->m_pKalmanFilter->Predict_Correct(kalman_detal_t);
			if(fabs(pos->diff_reader_sync_num) > ZERO_PRECISION){
				if (last_locate.x != INVALID_COORDINATE && last_locate.y != INVALID_COORDINATE)
				{
					this->m_pKalmanFilter->m_pCar->x(1,0) = (this->m_pKalmanFilter->m_pCar->x(0,0) - last_locate.x*this->map_scale)/kalman_detal_t;
					this->m_pKalmanFilter->m_pCar->x(3,0) = (this->m_pKalmanFilter->m_pCar->x(2,0) - last_locate.y*this->map_scale)/kalman_detal_t;
				}	
			}	
		}
	}

	if (pos->update)
	{
		//二维定位不需要如下内容
		//增加地图集的判定，判断定位结果是否在地图集上
		//如果不在地图集上，需要再次定位
		//需要带出定位结果的分站信息，
		//利用地图集中分站信息再次定位
		std::shared_ptr<POS> kalman_p = std::make_shared<POS>();

		kalman_p->posx = this->m_pKalmanFilter->m_pCar->x(0,0);
		kalman_p->posy = this->m_pKalmanFilter->m_pCar->x(2,0);

		////二维定位不需要再判定在地图集上了
		if(!LocateAlgorithm::IsOnMap(kalman_p,pTdoaReaderPathMap)){
			//再一次定位到地图集上
			LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_205);
			std::shared_ptr<POS> cp = LocateAlgorithm::MappingToPath(kalman_p,pTdoaReaderPathMap);

			if(cp != nullptr){
				if (cp->posx != INVALID_COORDINATE && cp->posy !=INVALID_COORDINATE)
				{
					LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_206);
					this->m_pKalmanFilter->m_pCar->x(0,0) = cp->posx;
					this->m_pKalmanFilter->m_pCar->x(2,0) = cp->posy;
				}	
			}
			cp.reset();
		}		

		pos->posx = this->m_pKalmanFilter->m_pCar->x(0,0);
		pos->posy = this->m_pKalmanFilter->m_pCar->x(2,0);
		pos->cvx  = this->m_pKalmanFilter->m_pCar->x(1,0);
		pos->cvy  = this->m_pKalmanFilter->m_pCar->x(3,0);

		pos->cx = pos->posx / (map_scale*1.0);
		pos->cy = pos->posy / (map_scale*1.0);
	}

	return 0;
}

int Card::GetDeltaT( map<unsigned long long,std::shared_ptr<_coordinate>> dl )
{
	unsigned long long revTime = 0;
	int rid = 0, sn = 0, ct = 0;

	map<unsigned long long,std::shared_ptr<_coordinate>>::iterator it_mpdl = dl.begin();
	int i = 0;
	for(; it_mpdl != dl.end(); ++it_mpdl){
		if(0 == it_mpdl->second->tt){
			continue;
		}

		if(this->locate && it_mpdl->second->reader_id == this->locate->ReaderId()){
			revTime = it_mpdl->second->tt;
			rid = it_mpdl->second->reader_id;
			sn = it_mpdl->second->sync_num;
			ct = it_mpdl->second->t;
			break;
		}

		if(0 == revTime){
			revTime = it_mpdl->second->tt;
			rid = it_mpdl->second->reader_id;
			sn = it_mpdl->second->sync_num;
			ct = it_mpdl->second->t;
		}
		else if( revTime < it_mpdl->second->tt){ // 跨周期，可能会取到较大值
			revTime = it_mpdl->second->tt;
			rid = it_mpdl->second->reader_id;
			sn = it_mpdl->second->sync_num;
			ct = it_mpdl->second->t;
		}
	}

	std::shared_ptr<nspLocate::LocateRecord> loc = std::make_shared<nspLocate::LocateRecord>(revTime, rid, sn, ct);
	loc->getDeltaT(this->locate);
	this->locate = loc;
	return 0;
}

/*
* 获得tdoa和tof 卡的时间差
*
* param
*		无
*
* return
*		返回值为计算后的时间差
*
*/
double Card::GetDeltaT()
{
	double dt = 0;

	switch (ranging_type)
	{
	case LOCATEDATATYPE::LDT_TOF:
	case LOCATEDATATYPE::LDT_TOF_INS:
	case LOCATEDATATYPE::LDT_TDOA_INS:
		dt = difftime(time_cur_algo,time_last_tof);

		if(dt < ZERO_PRECISION)
		{
			switch (this->card_type)
			{
			case CT_VEHICLE:
				dt = 1 * (time_stamp_cal - time_stamp_cal_last);
				break;
			case CT_PERSON:
				dt = 2 * (time_stamp_cal - time_stamp_cal_last);
				break;
			default:
				dt = 2 * (time_stamp_cal - time_stamp_cal_last);
				break;
			}
		}

		break;
	case LOCATEDATATYPE::LDT_TDOA:
		//存在分站在计数序号为1000掉电，但重新上电会导致此时的计算值偏大
		if (m_nCalcSyncNum - this->last_locate.sync_num < 0 && m_nCalcSyncNum < 100)
		{
			dt = (m_nCalcSyncNum + 65535 - this->last_locate.sync_num)*READER_SYNC_INTERVAL_TIME;
		}else{
			dt = (m_nCalcSyncNum - this->last_locate.sync_num)*READER_SYNC_INTERVAL_TIME;
		}
		if (abs(dt) < ZERO_PRECISION)
		{
			dt = 0.2;
		}
		break;
	default:
		break;
	}
	return dt;
}

/*
* 获得定位维数，根据条件而定
*
* param
*		无
*
* return
*		成功返回指定定位维数，否则返回失败(0)
*
*/
int Card::get_dimension()
{
	//拆分数据为三维,二维,一维的定位数据
	split_dist_data();

	//如果三维数据可定位，则采用三维定位
	if (dist_3d.size() > _3D)
	{
		distmap = dist_3d;
		return _3D;
	}else if (dist_2d.size() > _2D)
	{
		//如果二维定位可定位，则采用二维定位
		distmap = dist_2d;
		return _2D;
	}else if (dist_1d.size() >= _1D)
	{
		//如果采用一维定位可定位，则采用一维定位
		distmap = dist_1d;
		return _1D;
	}

	return 0;
}

/*
* 拆分dist数据为一维，二维，三维数据
*
* param
*		无
* return
*		成功返回0，失败返回1
*
*/
int Card::split_dist_data()
{
	//将数据按分站维数进行拆分
	if (dist_1d.size() > 0)
	{
		dist_1d.erase(dist_1d.begin(),dist_1d.end());
	}
	if (dist_2d.size() > 0)
	{
		dist_2d.erase(dist_2d.begin(),dist_2d.end());
	}
	if (dist_3d.size() > 0)
	{
		dist_3d.erase(dist_3d.begin(),dist_3d.end());
	}
	for(auto it : _dists.front().distmap)
	{
		switch(it.second->dimension){
		case _1D:
			dist_1d.insert(std::make_pair(it.first,it.second));
			break;
		case _2D:
			dist_2d.insert(std::make_pair(it.first,it.second));
			break;
		case _3D:
			dist_3d.insert(std::make_pair(it.first,it.second));
			break;
		}
	}

	return 0;
}

/*
* 处理点和其定位分站相关边界的关系
*
* param
*		p		定位点坐标
*
* return
*		在边界区域内返回0，否则返回非零值
*
*/
int Card::deal_position_boundary(_point& p)
{
	//1.判断点是否在边界内，在直接返回0，表示成功
	if (position_is_in_boundary(p))
	{
#if ENABLE_DEBUG_2D_TEST_LOG
		char log[MAX_LOG_LENGTH];
		memset(log,0,MAX_LOG_LENGTH*sizeof(char));
		sprintf_s(log,"the p is in boundary. p.x : %.3f, p.y : %.3f",p.x,p,y);
		Log::write_log(FILE_TYPE::SYS_S,log,true);
#endif
		return 0;
	}
	//2.不在的话，判断此点和上一个定位位置直接的距离是否大于指定阈值d，
	double distance = 0.0;

	if (is_first_location){
#if ENABLE_DEBUG_2D_TEST_LOG
		char log[MAX_LOG_LENGTH];
		memset(log,0,MAX_LOG_LENGTH*sizeof(char));
		sprintf_s(log,"2d location : it is first location.");
		Log::write_log(FILE_TYPE::SYS_S,log,true);
#endif

		return 0;
	}else{
		if (last_locate.x == INVALID_COORDINATE && last_locate.y == INVALID_COORDINATE)
		{
#if ENABLE_DEBUG_2D_TEST_LOG
			char log[MAX_LOG_LENGTH];
			memset(log,0,MAX_LOG_LENGTH*sizeof(char));
			sprintf_s(log,"2d location : last_locate pos is invalid.");
			Log::write_log(FILE_TYPE::SYS_S,log,true);
#endif
		}else{
			distance = sqrt(pow(p.x - last_locate.x,2) + pow(p.y - last_locate.y,2));
			if (distance < CHAMBER_WIDTH*map_scale)
			{
				//a.小于的话，将新位置垂直映射到边界上
				_point newp = mapping_position_to_boundary(p);
				if (newp.x != INVALID_COORDINATE && newp.y != INVALID_COORDINATE)
				{
					p.x = newp.x;
					p.y = newp.y;
					p.z = newp.z;
				}else{
#if ENABLE_DEBUG_2D_TEST_LOG
					char log[MAX_LOG_LENGTH];
					memset(log,0,MAX_LOG_LENGTH*sizeof(char));
					sprintf_s(log,"mapping position failed.");
					Log::write_log(FILE_TYPE::SYS_S,log,true);
#endif
				}			
			}else{
#if ENABLE_DEBUG_2D_TEST_LOG
				char log[MAX_LOG_LENGTH];
				memset(log,0,MAX_LOG_LENGTH*sizeof(char));
				sprintf_s(log,"2d location is discard!!!");
				Log::write_log(FILE_TYPE::SYS_S,log,true);
#endif
				//b.大于的话，抛弃此次定位结果
				return 1;
			}
		}
	}

	return 0;
}

/*
* 判断点是否在边界区域内
*
* param
*		p		定位点坐标
*
* return
*		如果在边界区域内，返回true，否则返回false
*
*/
bool Card::position_is_in_boundary(_point p)
{
	if (p_reader == NULL || p_reader->pBoundaryArea == nullptr)
	{
#if ENABLE_DEBUG_2D_TEST_LOG
		std::string error_log = "";
		if (p_reader == NULL)
		{
			error_log = "p_reader is null.";
		}else if(p_reader->pBoundaryArea == nullptr){
			error_log = "p_reader->pBoundaryArea is nullptr.";
		}

		char log[MAX_LOG_LENGTH];
		memset(log,0,MAX_LOG_LENGTH*sizeof(char));
		sprintf_s(log,"2d location : position out of boundary ,error message is %s.",error_log.c_str());
		Log::write_log(FILE_TYPE::SYS_S,log,true);
#endif
		return false;
	}
	_point tp;
	//tp.x = p.x / map_scale;
	//tp.y = p.y / map_scale;
	tp.x = p.x ;
	tp.y = p.y ;

#if ENABLE_DEBUG_2D_TEST_LOG
	char log[MAX_LOG_LENGTH];
	memset(log,0,MAX_LOG_LENGTH*sizeof(char));
	sprintf_s(log,"position_is_in_boundary(): p.x: %.3f,p.y: %.3f,.x : %.3f, tp.y : %.3f, boundary_path: %s.",p.x,p.y,tp.x,tp.y,p_reader->pBoundaryArea->path.c_str());
	Log::write_log(FILE_TYPE::SYS_S,log,true);
#endif


	return p_reader->pBoundaryArea->IsInPolygon(p);
}

/*
* 映射边界区域外的点到边界上
*
* param
*		p		定位点坐标
*
* return
*		成功映射，返回在边界上的点，否则返回坐标为异常数据
*
*/
_point Card::mapping_position_to_boundary(_point p)
{
	_point newp;
	newp.x = newp.y = newp.z = INVALID_COORDINATE;

	if (p_reader == NULL)
	{
		return newp;
	}
	if (p_reader->pBoundaryArea == nullptr)
	{
		return newp;
	}

	double min_x = 99999.0 , min_y = 99999.0, max_x = -99999.0, max_y = -99999.0;

	if (p_reader->pBoundaryArea->m_pPolygon == NULL)
	{
		return newp;
	}

	for (int i = 0;i < p_reader->pBoundaryArea->m_nPolygonCount;i++)
	{
		if (p_reader->pBoundaryArea->m_pPolygon[i].x < min_x)
		{
			min_x = p_reader->pBoundaryArea->m_pPolygon[i].x;
		}
		if (p_reader->pBoundaryArea->m_pPolygon[i].x > max_x)
		{
			max_x = p_reader->pBoundaryArea->m_pPolygon[i].x;
		}
		if (p_reader->pBoundaryArea->m_pPolygon[i].y < min_y)
		{
			min_y = p_reader->pBoundaryArea->m_pPolygon[i].y;
		}
		if (p_reader->pBoundaryArea->m_pPolygon[i].y > max_y)
		{
			max_y = p_reader->pBoundaryArea->m_pPolygon[i].y;
		}
	}

	int orientation = -1;

	//已知矩形四个顶点分别是(xi,yi)(其中i = 1,2,3,4),点(x,y)和此矩形的关系如下
	//1.当x > max(xi)时：
	//a.y>max(y),点在矩形的右上角；
	//b.y<min(y),点在矩形的右下角；
	//c.y>min(y)&&y<max(y),点在矩形的右边；
	//2.当x < min(xi)时：
	//a.y>max(y),点在矩形的左上角；
	//b.y<min(y),点在矩形的左下角；
	//c.y>min(y)&&y<max(y),点在矩形的左边；
	//3.当x>min(xi)且x<max(xi)时：
	//a.y>max(y)，点在矩形的正上边
	//b.y<max(y)，点在矩形的正下边
	if (p.x > max_x)
	{
		if (p.y > max_y)
		{
			orientation = Orientation::RIGHT_UP;
		}else if(p.y < min_y)
		{
			orientation = Orientation::RIGHT_DOWN;
		}else{
			orientation = Orientation::RIGHT;
		}
	} 
	else if(p.x < min_x)
	{
		if (p.y > max_y)
		{
			orientation = Orientation::LEFT_UP;
		}else if (p.y < min_y)
		{
			orientation = Orientation::LEFT_DOWN;
		}else{
			orientation = Orientation::LEFT;
		}
	}else{
		if (p.y > max_y)
		{
			orientation = Orientation::UP;
		}else if(p.y < min_y){
			orientation = Orientation::DOWN;
		}
	}

	if (orientation == -1)
	{
		return newp;
	}

	switch (orientation)
	{
	case Orientation::UP:
		newp.x = p.x;
		newp.y = max_y;
		break;
	case Orientation::DOWN:
		newp.x = p.x;
		newp.y = min_y;
		break;
	case Orientation::LEFT:
		newp.x = min_x;
		newp.y = p.y;
		break;
	case Orientation::RIGHT:
		newp.x = max_x;
		newp.y = p.y;
		break;
	case Orientation::LEFT_UP:
		newp.x = min_x;
		newp.y = max_y;
		break;
	case Orientation::LEFT_DOWN:
		newp.x = min_x;
		newp.y = min_y;
		break;
	case Orientation::RIGHT_UP:
		newp.x = max_x;
		newp.y = max_y;
		break;
	case Orientation::RIGHT_DOWN:
		newp.x = max_x;
		newp.y = min_y;
		break;
	default:
		break;
	}

	return newp;
}

/*
* tdoa二维定位的加速度或速度过滤方法
*
* param
*		p		定位坐标
*
* return
*		成功返回0，否则返回非零值
*
*/
int Card::check_solution_tdoa_2d(std::shared_ptr<POS>& p)
{
	// 如果第一次定位则不需检查
	if (is_algo_first_location)
	{
		return 0;
	}

	double distance = 0.0, speed = 0.0, acc = 0.0,dt = 0;
	if (!is_use_acc_filter)
	{
		if (avg_coordinate.size() >=5)
		{
			HostServer::Position avg_pos;
			for (auto it = avg_coordinate.begin();it != avg_coordinate.end();++it)
			{
				avg_pos.x += (*it)->x;
				avg_pos.y += (*it)->y;
				avg_pos.z += (*it)->z;
			}
			avg_pos.x /=5;
			avg_pos.y /=5;
			avg_pos.z /=5;

			distance = sqrt(pow(p->posx - avg_pos.x*map_scale,2) + pow(p->posy - avg_pos.y*map_scale,2));

			is_use_acc_filter = true;
		}else{
			return 0;
		}
	}else{
		distance = sqrt(pow(p->posx - last_locate.x*map_scale,2) + pow(p->posy - last_locate.y*map_scale,2));
	}

	double base_period = 0.0;
	//base_period = algorithm::base::ANCHOR_SYNCHRONIZATION_PERIOD;
	base_period = algorithm::base::CARD_BROADCAST_PERIOD;
	// 计算时间差，考虑跨周期的处理逻辑
	if (time_stamp_cal > time_stamp_cal_last)
	{
		dt = (time_stamp_cal - time_stamp_cal_last)*base_period;
	}else{
		dt = (65536 - (time_stamp_cal - time_stamp_cal_last))*base_period;
	}

	//如果时间差为零
	if (dt < ZERO_PRECISION)
	{
		dt = base_period;
	}

	// 计算速度
	speed = distance / dt;

	// 检查速度的合理性
	if (speed > algorithm::base::TDOA_MAX_PERSON_SPEED)
	{
		return 1;
	}

	// 如果上一次无定位结果，则不计算加速度
	if (last_locate.x == INVALID_COORDINATE && last_locate.y == INVALID_COORDINATE)
	{
		return 0;
	}

	// 计算加速度
	acc = fabs(speed - v)/dt;

	// 检查加速度的合理性
	if (acc > algorithm::base::TDOA_MAX_ACCELERATION_SPEED)
	{
		return 2;
	}

	return 0;
}

/*
* 保存二维定位的数据
*
* param
*		p		定位结果
*
* return
*		如果成功返回0，否则返回非零值
*
*/
int Card::save_tdoa_data_2d(std::shared_ptr<POS>& p)
{
	//测试，处理，将x,y的坐标做平均处理输出
	//x = (last_locate.x + p->cx)/2;
	//y = (last_locate.y + p->cy)/2;

	x = p->cx;
	y = p->cy;

	last_locate.x = p->cx;
	last_locate.y = p->cy;

	time_stamp_cal_last = time_stamp_cal;

	if (is_algo_first_location)
	{
		is_algo_first_location = false;
	}

	if (avg_coordinate.size() <= 5)
	{
		std::shared_ptr<HostServer::Position> position = std::make_shared<HostServer::Position>();
		position->x = x;
		position->y = y;
		position->z = 0;

		avg_coordinate.push_back(position);
	}

	Position pos;
	pos.x = p->posx;
	pos.y = p->posy;
	pTaylorWls->set_position(pos);

	char chLog[MAX_LOG_LENGTH] = {0};
	std::string log = "";
#ifdef _DEBUG
	sprintf_s(chLog,"card:%s, ct:%d, x:%.2f, y:%.2f, reader:%d"
		, this->card_id.c_str(), this->time_stamp_cal
		, pos.x, pos.y
		, 0);	
#else
	sprintf_s(chLog,"card:%s, ct:%d, x:%.2f, y:%.2f, reader:%d"
		, this->card_id.c_str(), this->time_stamp_cal
		, pos.x, pos.y
		, p_reader->reader_id);
#endif
	log = chLog;
	Log::write_log(FILE_TYPE::RAW_LOCATION_DATA,log,true);

	return 0;
}

void Card::PosState( int val )
{
	pos_state = val;
	pos_state_count = pos_state_confirm_times;
}

int const Card::PosState()
{
	if(pos_state_count >= pos_state_confirm_times )
	{
		return pos_state;
	}
	return pos_state_old;
}

void Card::PosStateOld( int val )
{
	pos_state_old = val;
}

// 只显示优先级最高的状态
int const Card::StateBiz() 
{
	this->state_biz = 0; // 按优先级显示state_biz,优先级顺序：
	// 进入盲区 > 呼救 > 呼叫 > 限制区域 > 区域超时 > 井下超时 > 超速 > 正常

	if(status_area_over_speed == STATUS_ERROR){
		state_biz = STATUS_AREA_OVER_SPEED;
	}

	if(status_area_over_time == STATUS_ERROR){
		state_biz = STATUS_OVER_TIME;
	}

	if(status_area_forbidden == STATUS_ERROR){
		state_biz = STATUS_AREA_FORBIDDEN;
	}

	if(status_area_over_time == STATUS_ERROR){
		state_biz = STATUS_AREA_OVER_TIME;
	}

	if(status_call == STATUS_ERROR){
		state_biz = STATUS_CALL;
	}

	if(status_help == STATUS_ERROR){
		state_biz = STATUS_HELP;
	}

	// lihongzhen  2017/8/28 车辆取消盲区状态显示，只显示人员，用来清除盲区卡
	//if(CT_PERSON == this->card_type)
	//{
	if(STATUS_LOST == status_lost)
	{
		state_biz = STATUS_LOST;
	}
	//}	
	return state_biz;
}

int Card::new_algo_tof_1d(int cnt)
{	
	auto first = _dists.begin();
	//first = get_iterator(first);
	if (first==_dists.end())
	{
		return 1;
	}
	auto second = first;
	std::advance(second,1);
	if (second==_dists.end())
	{
		return 2;
	}
	//second = get_iterator(second);
	/*if (second==_dists.end())
	{
	return 1;
	}*/
	auto third = second;
	std::advance(third,1);
	if (third==_dists.end())
	{
		return 3;
	}
	//third = get_iterator(third);

	if (first == _dists.end()||second==_dists.end()||third==_dists.end())
	{
		return 4;
	}

	AlgoParamData d[3];
	int i = 0;
	for (auto it:_dists)
	{
		for (auto it_dm:it.distmap)
		{
			d[i].ant[it_dm.second->antenna_id].x = it_dm.second->x;
			d[i].ant[it_dm.second->antenna_id].y = it_dm.second->y;
			d[i].ct = it_dm.second->t;
			d[i].distance[it_dm.second->antenna_id] = it_dm.second->d;
			d[i].anchor_id = it_dm.second->reader_id;
		}
		i++;
		if (i>2)
		{
			break;
		}
	}

	int ct = d[1].ct;
	if (d[0].anchor_id!=d[1].anchor_id)
	{
		d[0].ct = 0;
		d[0].distance[0] = d[1].distance[0];
		d[0].distance[1] = d[1].distance[1];
		d[0].ant[0].x = d[1].ant[0].x;
		d[0].ant[0].y = d[1].ant[0].y;
		d[0].ant[1].x = d[1].ant[1].x;
		d[0].ant[1].y = d[1].ant[1].y;
	}else if (d[1].anchor_id != d[2].anchor_id)
	{
		d[2].ct = 0;
		d[2].distance[0] = d[1].distance[0];
		d[2].distance[1] = d[1].distance[1];
		d[2].ant[0].x = d[1].ant[0].x;
		d[2].ant[0].y = d[1].ant[0].y;
		d[2].ant[1].x = d[1].ant[1].x;
		d[2].ant[1].y = d[1].ant[1].y;
	}

	double cx = CalculateX(d[0].ant[0].x*map_scale, d[0].ant[1].x*map_scale, d[0].ct, d[1].ct, last_locate.x*map_scale, 
		d[1].distance[0], d[1].distance[1], d[0].distance[0], d[0].distance[1], d[2].distance[0], d[2].distance[1]);

	if (cx != INVALID_COORDINATE)
	{
		double speed = 0.0;
		//增加加速度的过滤
		if (last_locate.t != d[1].ct)
		{
			double new_x = cx/map_scale;
			double unit = 1.0;
			switch (this->card_type)
			{
			case CT_PERSON:
				unit = 2.0;
			case CT_VEHICLE:
				unit = 1.0;
			default:
				unit = 1.0;
				break;
			}
			speed = abs((new_x - last_locate.x)*map_scale/((last_locate.t - d[1].ct)*unit));
			double acc = abs(speed - last_locate.v)/pow((last_locate.t - d[1].ct)*unit,2);
			bool pass = false;
			switch (card_type)
			{
			case CT_PERSON:
				if (acc < PERSON_ACCELERATE_THRESHOLD)
				{
					pass = true;
				}
				break;
			case CT_VEHICLE:
				if (acc < VECHILE_ACCELERATE_THRESHOLD)
				{
					pass = true;
				}
				break;
			}

			if (pass)
			{
				this->x = cx/map_scale;
				this->y = d[1].ant[0].y;
				this->v = speed;
				last_locate.x = this->x;
				last_locate.y = d[1].ant[0].y;
				last_locate.t = d[1].ct;
				last_locate.v = speed;
				this->t = d[1].ct;

				char log[100] = {0};
				sprintf_s(log,"calc: card_id: %s,ct: %d, x: %.3f, y: %.3f, acc: %.3f.",card_id.c_str(),d[1].ct,x,y,acc);
				Log::write_log(FILE_TYPE::SYS_S,log,true);
			}
		}
	}

	return cx!=INVALID_COORDINATE?0:1;
}

//获得含有双天线的数据
DistQueMap::iterator Card::get_iterator(DistQueMap::iterator it)
{
	if (it==_dists.end())
	{
		return std::move(it);
	}
	for (;it!=_dists.end();)
	{
		//删除其中的单条天线数据的元素
		if (it->distmap.size()<=1)
		{
			//it = _dists.erase(++it);
			++it;
			continue;
		}
		break;
	}
	return std::move(it);
}

/* 
* 函数说明
* 参数
*   L1：天线1的坐标
*   L2：天线2的坐标
*   last_ct：本分站上次收到本卡的数据ct值
*   ct：本分站本次收到本卡的数据ct值
*   last_x：本分站上次定位本卡的坐标
*   dis1：本次本分站天线1对本卡的测距值
*   dis2：本次本分站天线2对本卡的测距值
*   last_dis1：上次本分站天线1对本卡的测距值
*   last_dis2：上次本分站天线2对本卡的测距值
*   next_dis1：下次本分站天线1对本卡的测距值
*   next_dis2：下次本分站天线2对本卡的测距值
* 返回值
*   本次定位坐标
*/
double Card::CalculateX(int L1, int L2, int last_ct, int ct, double last_x0, double dis1, double dis2, double last_dis1, double last_dis2, double next_dis1, double next_dis2)
{
	//double x, x0, xt, xt1, xt2;
	//int ctd;
	//int L;
	//double d1 = dis1;
	//double d2 = dis2;
	//double last_d1 = last_dis1;
	//double last_d2 = last_dis2;
	//double next_d1 = next_dis1;
	//double next_d2 = next_dis2;
	//ctd = ct - last_ct;
	//x0 =  (last_x0 - L1) * Sign(L2 - L1);
	//L = abs(L1 - L2);
	//if (d1 < IGNORE_H_DISTANCE && d2 < IGNORE_H_DISTANCE)//此情况下需考虑卡与天线的高差
	//{
	//	if (d1 < H)
	//		d1 = H;
	//	if (d2 < H)
	//		d2 = H;
	//	d1 = sqrt(d1 * d1 - H * H);
	//	d2 = sqrt(d2 * d2 - H * H);
	//}
	//double dd = d1 - d2;
	//x = L / 2 + Sign(dd) * (d1 + d2) / 2;//计算坐标的公式
	////如果两根天线测距都比H小，说明就在天线下方，且不是NLOS，直接返回0
	//if (abs(d1) < ABSOLUTE0 && abs(d2) < ABSOLUTE0)
	//	return L1;
	//if (last_d1 < IGNORE_H_DISTANCE && last_d2 < IGNORE_H_DISTANCE)//此情况下需考虑卡与天线的高差
	//{
	//	if (last_d1 < H)
	//		last_d1 = H;
	//	if (last_d2 < H)
	//		last_d2 = H;
	//	last_d1 = sqrt(last_d1 * last_d1 - H * H);
	//	last_d2 = sqrt(last_d2 * last_d2 - H * H);
	//}
	//double last_dd = last_d1 - last_d2;
	//double last_x = L / 2 + Sign(last_d1 - last_d2) * (last_d1 + last_d2) / 2;//初步计算上一次的坐标
	//if (next_d1 < IGNORE_H_DISTANCE && next_d2 < IGNORE_H_DISTANCE)//此情况下需考虑卡与天线的高差
	//{
	//	if (next_d1 < H)
	//		next_d1 = H;
	//	if (next_d2 < H)
	//		next_d2 = H;
	//	next_d1 = sqrt(next_d1 * next_d1 - H * H);
	//	next_d2 = sqrt(next_d2 * next_d2 - H * H);
	//}
	//double next_dd = next_d1 - next_d2;
	//double next_x = L / 2 + Sign(next_d1 - next_d2) * (next_d1 + next_d2) / 2;//初步计算下一次的坐标
	////首次定位要确保正确
	//if (ctd > 5)
	//{
	//	if (abs(x - next_x) <  card_type * MAX_SPEED)//本次定位与下次定位符合运动规律，可以定位
	//		return L1 + Sign(L2 - L1) * x;
	//	//locating_err_count++;
	//	return INVALID_COORDINATE;
	//}
	////卡从分站的一边移动到分站的另一边时，需特殊处理
	//if (Sign(x0) != Sign(dd) || Sign(next_x) != Sign(dd)) 
	//	if (Sign(x0) != Sign(next_x))
	//		if (abs(x0) < card_type * MAX_SPEED || abs(next_x) < card_type * MAX_SPEED || abs(x) < card_type * MAX_SPEED)
	//		{
	//			xt = (x0 + next_x) / 2;
	//			return L1 + Sign(L2 - L1) * xt;
	//		}
	////其它情况正常处理
	////测得的2天线距离差在阈值范围内，并且距离差的符号与前一条数据或后一条数据的符号相同，即数据没有“跳”，认为定位无误
	//double dda = abs(d1 - d2);//两个天线测距差的绝对值
	//if ((dda > L - 2 * E) && (dda < L + 2 * E) && (Sign(dd) == Sign(last_dd) || Sign(dd) == Sign(next_dd))) 
	//{
	//	return L1 + Sign(L2 - L1) * x;
	//}
	//xt1 = Sign(x0) * d1;//其它情况下，本次定位方向与上次定位相同
	//xt2 = L + Sign(x0) * d2;
	//xt = (xt1 + xt2) / 2;
	//if ((abs(xt1 - x0) < ctd * card_type * MAX_SPEED) && (abs(xt2 - x0) < ctd * card_type * MAX_SPEED))//如果2个天线的测距值都符合运动规律，就以平均值为准计算坐标
	//{
	//	return L1 + Sign(L2 - L1) * xt;
	//}
	//if (abs(xt1 - x0) < ctd * card_type * MAX_SPEED )//如果第一个天线的测距值符合运动规律，就以第一根天线的测距值为准计算坐标
	//{
	//	return L1 + Sign(L2 - L1) * xt1;
	//}
	//if (abs(d2) > L)//此处判断是确保xt符号与x0符号一致
	//{
	//	if (abs(xt2 - x0) < ctd * card_type * MAX_SPEED)//如果第二个天线的测距值符合运动规律，就以第二根天线的测距值为准计算坐标
	//	{
	//		return L1 + Sign(L2 - L1) * xt2;
	//	}
	//}
	//if (abs(x - last_x) < card_type * MAX_SPEED)//本次定位与上次定位符合运动规律，可以定位
	//	return L1 + Sign(L2 - L1) * x;
	//if (abs(x - next_x) < card_type * MAX_SPEED)//本次定位与下次定位符合运动规律，可以定位
	//	return L1 + Sign(L2 - L1) * x;
	////locating_err_count++;

	double x, x0, xt, xt1, xt2;
	int ctd;
	double L;
	double d1 = dis1;
	double d2 = dis2;
	double last_d1 = last_dis1;
	double last_d2 = last_dis2;
	double next_d1 = next_dis1;
	double next_d2 = next_dis2;
	ctd = ct - last_ct;
	x0 =  (last_x0 - L1) * Sign(L2 - L1);
	L = abs(L1 - L2);
	if (d1 < IGNORE_H_DISTANCE && d2 < IGNORE_H_DISTANCE)//此情况下需考虑卡与天线的高差
	{
		if (d1 < H)
			d1 = H;
		if (d2 < H)
			d2 = H;
		d1 = sqrt(d1 * d1 - H * H);
		d2 = sqrt(d2 * d2 - H * H);
	}
	double dd = d1 - d2;
	double dda = abs(d1 - d2);//两个天线测距差的绝对值
	//cout << "dis1: " << dis1 << ", ";
	//cout << "dis2: " << dis2 << ", ";
	//cout << "dis1 - dis2 = " << dis1 -  dis2 << "      ";
	//cout << "d1: " << d1 << ", ";
	//cout << "d2: " << d2 << ", ";
	//cout << "d1 - d2 = " << dd << ", ";
	x = L / 2 + Sign(dd) * (d1 + d2) / 2;//计算坐标的公式
	//如果两根天线测距都比H小，说明就在天线下方，且不是NLOS，直接返回0
	if (abs(d1) < ABSOLUTE0 && abs(d2) < ABSOLUTE0)
		return L1;
	if (last_d1 < IGNORE_H_DISTANCE && last_d2 < IGNORE_H_DISTANCE)//此情况下需考虑卡与天线的高差
	{
		if (last_d1 < H)
			last_d1 = H;
		if (last_d2 < H)
			last_d2 = H;
		last_d1 = sqrt(last_d1 * last_d1 - H * H);
		last_d2 = sqrt(last_d2 * last_d2 - H * H);
	}
	double last_dd = last_d1 - last_d2;
	double last_x = L / 2 + Sign(last_d1 - last_d2) * (last_d1 + last_d2) / 2;//初步计算上一次的坐标
	if (next_d1 < IGNORE_H_DISTANCE && next_d2 < IGNORE_H_DISTANCE)//此情况下需考虑卡与天线的高差
	{
		if (next_d1 < H)
			next_d1 = H;
		if (next_d2 < H)
			next_d2 = H;
		next_d1 = sqrt(next_d1 * next_d1 - H * H);
		next_d2 = sqrt(next_d2 * next_d2 - H * H);
	}
	double next_dd = next_d1 - next_d2;
	double next_x = L / 2 + Sign(next_d1 - next_d2) * (next_d1 + next_d2) / 2;//初步计算下一次的坐标
	//首次定位要确保正确
	if (ctd > 5 )
	{
		if ((dda > L - 2.5 * E) && (dda < L + 2.5 * E) && (abs(x - next_x) <  card_type * MAX_SPEED))//本次定位与下次定位符合运动规律，可以定位
			return L1 + Sign(L2 - L1) * x;
		//locating_err_count++;
		return INVALID_COORDINATE;
	}
	//卡从分站的一边移动到分站的另一边时，需特殊处理
	if (Sign(x0) != Sign(dd) || Sign(next_x) != Sign(dd)) 
		if (Sign(x0) != Sign(next_x))
			if (abs(x0) < card_type * MAX_SPEED || abs(next_x) < card_type * MAX_SPEED || abs(x) < card_type * MAX_SPEED)
			{
				xt = (x0 + next_x) / 2;
				return L1 + Sign(L2 - L1) * xt;
			}

	//其它情况正常处理
	//测得的2天线距离差在阈值范围内，并且距离差的符号与前一条数据的符号相同，即数据没有“跳”，认为定位无误
	if ((dda > L - 2 * E) && (dda < L + 2 * E) && (Sign(dd) == Sign(last_dd) )) 
	{
		return L1 + Sign(L2 - L1) * x;
	}
	xt1 = Sign(x0) * d1;//其它情况下，本次定位方向与上次定位相同
	xt2 = L + Sign(x0) * d2;
	xt = (xt1 + xt2) / 2;
	if ((abs(xt1 - x0) < ctd * card_type * MAX_SPEED) && (abs(xt2 - x0) < ctd * card_type * MAX_SPEED))//如果2个天线的测距值都符合运动规律，就以平均值为准计算坐标
	{
		return L1 + Sign(L2 - L1) * xt;
	}
	if (abs(xt1 - x0) < ctd * card_type * MAX_SPEED )//如果第一个天线的测距值符合运动规律，就以第一根天线的测距值为准计算坐标
	{
		return L1 + Sign(L2 - L1) * xt1;
	}
	if (abs(d2) > L)//此处判断是确保xt符号与x0符号一致
	{
		if (abs(xt2 - x0) < ctd * card_type * MAX_SPEED)//如果第二个天线的测距值符合运动规律，就以第二根天线的测距值为准计算坐标
		{
			return L1 + Sign(L2 - L1) * xt2;
		}
	}
	if (abs(x - last_x) < card_type * MAX_SPEED)//本次定位与上次定位符合运动规律，可以定位
		return L1 + Sign(L2 - L1) * x;
	if (abs(x - next_x) < card_type * MAX_SPEED)//本次定位与下次定位符合运动规律，可以定位
		return L1 + Sign(L2 - L1) * x;
	//locating_err_count++;
	return INVALID_COORDINATE;//以上条件都不满足，本次定位失败
}

double Card::CalculateX(double L1, double L2, int last_ct, int ct, double last_x0, double dis1, double dis2, double last_dis1, double last_dis2, double next_dis1, double next_dis2)
{
	double x, x0, xt, xt1, xt2;
	int ctd;
	double L;
	double d1 = dis1;
	double d2 = dis2;
	double last_d1 = last_dis1;
	double last_d2 = last_dis2;
	double next_d1 = next_dis1;
	double next_d2 = next_dis2;
	ctd = ct - last_ct;
	if (ctd < 0)//ct跨周期的情况
		ctd += 65536;
	x0 =  (last_x0 - L1) * Sign(L2 - L1);
	L = abs(L1 - L2);
	if (d1 < IGNORE_H_DISTANCE && d2 < IGNORE_H_DISTANCE)//此情况下需考虑卡与天线的高差
	{
		if (d1 < H)
			d1 = H;
		if (d2 < H)
			d2 = H;
		d1 = sqrt(d1 * d1 - H * H);
		d2 = sqrt(d2 * d2 - H * H);
	}
	double dd = d1 - d2;
	double dda = abs(d1 - d2);//两个天线测距差的绝对值
	/*cout << "dis1: " << dis1 << ", ";
	cout << "dis2: " << dis2 << ", ";
	cout << "dis1 - dis2 = " << dis1 -  dis2 << "      ";
	cout << "d1: " << d1 << ", ";
	cout << "d2: " << d2 << ", ";
	cout << "d1 - d2 = " << dd << ", ";*/
	x = L / 2 + Sign(dd) * (d1 + d2) / 2;//计算坐标的公式
	if (abs(dis1) < ABSOLUTE0 )//只有dis2的单天线数据的情况，争取单天线数据也能定位
	{
		if (abs(ctd) > 5)//单天线数据不用于首次定位
			return INVALID_COORDINATE;
		xt2 = L + Sign(x0) * d2;
		if (abs(xt2 - x0) < ctd * card_type * MAX_SPEED)//本次定位与上次定位符合运动规律，可以定位
			return L1 + Sign(L2 - L1) * xt2;
	}
	if (abs(dis2) < ABSOLUTE0 )//只有dis1的单天线数据的情况，争取单天线数据也能定位
	{
		if (abs(ctd) > 5)//单天线数据不用于首次定位
			return INVALID_COORDINATE;
		xt1 = Sign(x0) * d1;;
		if (abs(xt1 - x0) < ctd * card_type * MAX_SPEED)//本次定位与上次定位符合运动规律，可以定位
			return L1 + Sign(L2 - L1) * xt1;
	}
	//如果两根天线测距都比H小，说明就在天线下方，且不是NLOS，直接返回0
	if (abs(d1) < ABSOLUTE0 && abs(d2) < ABSOLUTE0)
		return L1;
	if (last_d1 < IGNORE_H_DISTANCE && last_d2 < IGNORE_H_DISTANCE)//此情况下需考虑卡与天线的高差
	{
		if (last_d1 < H)
			last_d1 = H;
		if (last_d2 < H)
			last_d2 = H;
		last_d1 = sqrt(last_d1 * last_d1 - H * H);
		last_d2 = sqrt(last_d2 * last_d2 - H * H);
	}
	double last_dd = last_d1 - last_d2;
	double last_x = L / 2 + Sign(last_d1 - last_d2) * (last_d1 + last_d2) / 2;//初步计算上一次的坐标
	if (next_d1 < IGNORE_H_DISTANCE && next_d2 < IGNORE_H_DISTANCE)//此情况下需考虑卡与天线的高差
	{
		if (next_d1 < H)
			next_d1 = H;
		if (next_d2 < H)
			next_d2 = H;
		next_d1 = sqrt(next_d1 * next_d1 - H * H);
		next_d2 = sqrt(next_d2 * next_d2 - H * H);
	}
	double next_dd = next_d1 - next_d2;
	double next_x = L / 2 + Sign(next_d1 - next_d2) * (next_d1 + next_d2) / 2;//初步计算下一次的坐标
	//首次定位要确保正确
	if (ctd > 5 )
	{
		if ((dda > L - 2.5 * E) && (dda < L + 2.5 * E) && (abs(x - next_x) <  card_type * MAX_SPEED))//本次定位与下次定位符合运动规律，可以定位
			return L1 + Sign(L2 - L1) * x;
		//locating_err_count++;
		return INVALID_COORDINATE;
	}
	//卡从分站的一边移动到分站的另一边时，需特殊处理
	if (Sign(x0) != Sign(dd) || Sign(next_x) != Sign(dd)) 
		if (Sign(x0) != Sign(next_x))
			if (abs(x0) < card_type * MAX_SPEED || abs(next_x) < card_type * MAX_SPEED || abs(x) < card_type * MAX_SPEED)
			{
				xt = (x0 + next_x) / 2;
				return L1 + Sign(L2 - L1) * xt;
			}
			//其它情况正常处理
			//测得的2天线距离差在阈值范围内，并且距离差的符号与前一条数据的符号相同，即数据没有“跳”，认为定位无误
			if ((dda > L - 2 * E) && (dda < L + 2 * E) && (Sign(dd) == Sign(last_dd) )) 
				return L1 + Sign(L2 - L1) * x;
			xt1 = Sign(x0) * d1;//其它情况下，本次定位方向与上次定位相同
			xt2 = L + Sign(x0) * d2;
			xt = (xt1 + xt2) / 2;
			if ((abs(xt1 - x0) < ctd * card_type * MAX_SPEED) && (abs(xt2 - x0) < ctd * card_type * MAX_SPEED))//如果2个天线的测距值都符合运动规律，就以平均值为准计算坐标
				return L1 + Sign(L2 - L1) * xt;
			if (abs(xt1 - x0) < ctd * card_type * MAX_SPEED )//如果第一个天线的测距值符合运动规律，就以第一根天线的测距值为准计算坐标
				return L1 + Sign(L2 - L1) * xt1;
			if (abs(d2) > L)//此处判断是确保xt符号与x0符号一致
				if (abs(xt2 - x0) < ctd * card_type * MAX_SPEED)//如果第二个天线的测距值符合运动规律，就以第二根天线的测距值为准计算坐标
					return L1 + Sign(L2 - L1) * xt2;
			if (abs(x - last_x) < card_type * MAX_SPEED)//本次定位与上次定位符合运动规律，可以定位
				return L1 + Sign(L2 - L1) * x;
			if (abs(x - next_x) < card_type * MAX_SPEED)//本次定位与下次定位符合运动规律，可以定位
				return L1 + Sign(L2 - L1) * x;
			//locating_err_count++;
			return INVALID_COORDINATE;//以上条件都不满足，本次定位失败
}

int Card::Sign(double x)
{
	if(x == 0)
		return 0;
	if(x > 0)
		return 1;
	return -1;
}

void Card::set_reader_path(std::shared_ptr<ReaderPathMap> rpm, std::shared_ptr<TDOAReaderPathMap> trpm)
{
	this->set_reader_path_tof(rpm);
	this->set_reader_path_tdoa(trpm);
}

void Card::cal_pos_by_cache()
{
	EnterCriticalSection(&m_csCard);
	if(_dists.size() > 0){ 
		// 超过缓存数量限制
		LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_137);
		get_coordinate();
		_dists.pop_front();
	}

	if (this->x != this->last_x || this->y != this->last_y)
	{
		LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_138);
		if (CT_VEHICLE == this->card_type)
		{
			//只有车辆需要进行如下偏移计算
			algo_calc_offset();
		}

		this->n_pos_no_change_times = 0;
		this->b_pos_change = true;
	}else{
		LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_139);
		this->n_pos_no_change_times++;
		this->b_pos_change = false;
	}
	LeaveCriticalSection(&m_csCard);
}

int Card::dist_cache_size()
{
	return _dists.size();
}

MapInfo::MapInfo( void )
{

}

MapInfo::~MapInfo( void )
{

}

Dept::Dept( int id, string name )
{
	dept_id = id;
	dept_name = name;
}

OccLevel::OccLevel(int id )
{
	occlevel_id = id;
}

Adhoc::Adhoc()
{
	adhoc_id = 0;
	x = 0;
	y = 0;
	z = 0;
	idx = 0;
}

Adhoc::~Adhoc()
{
}

Light::Light()
{
	m_nID = m_nMapID = m_nAreaID = m_nLightsGroupID = m_nReaderID = m_nSectionID = m_nPort = m_nState = m_nStream = 0;
	device_type_id = 0;
	m_nOldState = 0;
	m_nShape = 0;
	x = y = z = 0.0;
	m_nIsCtrl = LIGHT_CTRL_STATE::CTRL_STATE_AUTO;
	m_strIP = m_strName = m_strLabel = "";

	rec_time = time(NULL);
	alarm_start_time = time(NULL);
	last_send_time = time(NULL);
}

int Light::get_light_state()
{
	return m_nState;
}

Chamber::Chamber()
{
	m_nID = m_nIsUsed = m_nMapID = m_nState = m_nSectionID = 0;
	m_strPath = m_strName = m_strLabel = "";
}

Chamber::~Chamber()
{

}

/*
* 初始化区域边界
*
* param
*		path		边界字符串描述
*
* return
*		成功返回0，失败返回1
*/
int BaseArea::init_border(string path)
{
	if(path == "" || path.find(',') == std::string::npos){
		return 1;
	}

	std::vector<std::string> vec = Split(path, " ");
	std::vector<std::string>::iterator it = vec.begin();
	if(m_pPolygon){
		delete[] m_pPolygon;
		m_pPolygon = NULL;
	}
	m_pPolygon = new _point[vec.size()];
	m_nPolygonCount = 0;
	for(; it != vec.end(); ++it){
		std::vector<std::string> subvec = Split(it->c_str(), ",");
		_point p;
		p.x = GetVertex(subvec[0]);
		p.y = GetVertex(subvec[1]);
		p.z = 0;
		m_pPolygon[m_nPolygonCount] = p;
		m_nPolygonCount++;
	}

	return 0;
}

bool BaseArea::IsInPolygon(_point p)
{
	if(m_pPolygon == NULL){
		return false;
	}

	int counter = 0;
	int i;
	double xinters;
	_point p1,p2;

	p1 = m_pPolygon[0];
	for (int i=1;i<= m_nPolygonCount;i++) {
		p2 = m_pPolygon[i % m_nPolygonCount];
		if (p.y > MIN(p1.y,p2.y)) {
			if (p.y <= MAX(p1.y,p2.y)) {
				if (p.x <= MAX(p1.x,p2.x)) {
					if (p1.y != p2.y) {
						xinters = (p.y-p1.y)*(p2.x-p1.x)/(p2.y-p1.y)+p1.x;
						if (p1.x == p2.x || p.x <= xinters)
							counter++;
					}
				}
			}
		}
		p1 = p2;
	}
	return (counter % 2 == 0) ? false : true;
}

double BaseArea::GetVertex(std::string src)
{
	std::string dest = "";
	for(unsigned int i = 0; i < src.length(); i++){
		if((src[i] >= '0' && src[i]<='9') || src[i]=='-' || src[i] == '.'){
			dest += src[i];
		}
	}
	return atof(dest.c_str());
}

std::vector<std::string> BaseArea::Split(std::string str,std::string pattern)
{
	std::string::size_type pos;  
	std::vector<std::string> result;  
	str+=pattern;//扩展字符串以方便操作  
	unsigned int size=str.size();  

	for(unsigned int i=0; i<size; i++){  
		pos=str.find(pattern,i);  
		if(pos<size){  
			std::string s=str.substr(i,pos-i);  
			result.push_back(s);  
			i=pos+pattern.size()-1;  
		}  
	}  
	return result; 
}

Section::Section()
{
	m_nID = m_nMapId = m_nState = 0;
	m_strName = m_strLabel = m_strPath = "";
	for(int i = 0; i < SECTION_EVENT_COUNT; i++){
		m_event_list[i] = 0;
	}
}

bool Section::is_has_chamber()
{
	bool bIsHasEmptyChamber = false;
	if (mp_champer_list.size()<=0)
	{
		return bIsHasEmptyChamber;
	}

	ChamberMap::iterator it = mp_champer_list.begin();
	for (;it!=mp_champer_list.end();++it)
	{
		if (it->second->m_nIsUsed == false)
		{
			bIsHasEmptyChamber = true;
			break;
		}
	}

	return bIsHasEmptyChamber;
}

int Section::get_section_state()
{
	int nTotals = 0;

	nTotals = mp_vehicle_list.size();

	switch (nTotals)
	{
	case 0:
		m_nState = SECTION_STATE_NORMAL;
		break;
	case 1:
		m_nState = SECTION_STATE_BUSY;
		break;
	case 2:
		m_nState = SECTION_STATE_CONGESTION;
		break;
	default:
		m_nState = SECTION_STATE_CONGESTION;
		break;
	}

	return m_nState;
}

int Section::get_section_vehicle_counts()
{
	return mp_vehicle_list.size();
}

int Section::get_section_staffer_counts()
{
	return mp_staffer_list.size();
}

LightsGroup::LightsGroup()
{
	m_bIsUsed = false;
	m_nID = m_nState = 0;
	m_strLabel = m_strName = m_strVechileId = "";
}

/*
* 判断红绿灯组中是否有此灯
*
* param
*		pLight		灯对象
*
* return
*		存在返回true，不存在返回false
*/
bool LightsGroup::isExist(std::shared_ptr<Light> pLight)
{
	bool bRet = false;

	for (LightMap::iterator it = mp_lights_list.begin();it != mp_lights_list.end();++it)
	{
		//如果红绿灯的id，地图id，路段id，都和参数一致，且灯状态可用，即为找到
		if (it->second->m_nID == pLight->m_nID 
			&& it->second->m_nMapID == pLight->m_nMapID
			&& it->second->m_nSectionID == pLight->m_nSectionID
			&& it->second->m_nState == 0)
		{
			bRet = true;
		}
	}

	return bRet;
}

/*
* 判断灯组内灯的颜色是否一致
*
* param
*		nColor		指定颜色（红色或绿色）
*
* return
*		相同返回true，否则返回false
*
*/
bool LightsGroup::isAllLightColor(int nColor)
{
	bool bRet = true;

	for (LightMap::iterator it = mp_lights_list.begin();it != mp_lights_list.end();++it)
	{
		if (it->second->m_nColor != nColor)
		{
			bRet = false;
		}
	}

	return bRet;
}

PatrolTask::PatrolTask()
{
	patrol_task_id = patrol_path_id = 0;
	card_id = "";
	starffer_id = "";
	start_time = end_time = time(NULL);	
	mpPoint.swap(map<unsigned int, std::shared_ptr<PatrolPoint>>());

	cur_point_idx = 1;
	enter_time = leave_time = time(NULL);
	state = stay_state = duration_stay = 0;
	is_in_cur_point = false;
}

PatrolTask::~PatrolTask()
{
}

PatrolPoint::PatrolPoint()
{
	idx = 0;
	patrol_point_id = 0;
	map_id = 0;
	x = y = z = 0;
	ranging = 0; // 距离范围

	duration_last = duration_stay_min = duration_stay_max = duration_ranging = 0;
}

PatrolPoint::~PatrolPoint()
{
}

BanShift::BanShift()
{

}

BanShift::BanShift( int id, std::string s, std::string e, int tid )
{
	shift_id = id;
	start_time = s;
	end_time = e;
	shift_type_id = tid;
}

BanShift::~BanShift()
{
}

void Card::resetKalmanInfo()
{
	cal_location = new CalLocation(KALMAN_OFFSET_COOR, KALMAN_OFFSET_MOVE, KALMAN_OFFSET_RANGING, KALMAN_INTERVAL);
	p2_anchors = new Point2[ANCHOR_COUNT];
	//cal_location3 = new CalLocation3(KALMAN_OFFSET_COOR_TRI, KALMAN_OFFSET_COOR_TRI, KALMAN_OFFSET_COOR_TRI,
	//	KALMAN_OFFSET_MOVE_TRI, KALMAN_OFFSET_MOVE_TRI, 0, KALMAN_OFFSET_RANGING_TRI, KALMAN_INTERVAL_TRI);
	cal_location3 = new CalLocation3(KALMAN_OFFSET_COOR_TRI, KALMAN_OFFSET_MOVE_TRI, KALMAN_OFFSET_RANGING_TRI, KALMAN_INTERVAL_TRI);
	p3_anchors = new Point3[ANCHOR_COUNT];
	is_init_kalman = false;

	p_dists = new std::shared_ptr<_coordinate>[DIST_COUNT];
	p_dists_locate = new std::shared_ptr<_coordinate>[DIST_COUNT];
	for(int i = 0; i < DIST_COUNT; i++){
		p_dists[i] = NULL;
		p_dists_locate[i] = NULL;
	}
}

void Card::set_anchors(int cnt)
{
	bool bExist = false;
	for(auto it : distmap)
	{
		bExist = false;
		for(int i=0; i<cnt; i++)
		{
			if(cnt > 3)
			{
				if(p3_anchors[i].x == it.second->x && p3_anchors[i].y == it.second->y &&
					p3_anchors[i].z == it.second->z)
				{
					bExist = true;
					break;
				}
			}
			else if(cnt > 2)
			{
				if(p2_anchors[i].x == it.second->x && p2_anchors[i].y == it.second->y)
				{
					bExist = true;
					break;
				}
			}
		}

		if(!bExist)
		{
			is_anchor_changed = true;
			break;
		}
	}

	if(is_anchor_changed)
	{
		int i = 0;
		if(cnt > 3)
		{
			for(auto it: distmap)
			{
				p3_anchors[i].x = it.second->x;
				p3_anchors[i].y = it.second->y;
				p3_anchors[i].z = it.second->z;
				i++;
			}
		}
		else if(cnt > 2)
		{
			//for(auto it: _dists.front().distmap)
			for(auto it: distmap)
			{
				p2_anchors[i].x = it.second->x;
				p2_anchors[i].y = it.second->y;
				i++;
			}
		}
	}
}

void Card::get_coordinate_2d(int cnt)
{
#ifdef ENABLE_DEBUG_2D_TEST_LOG
	char log[MAX_LOG_LENGTH];
	memset(log,0,MAX_LOG_LENGTH*sizeof(char));
	sprintf_s(log,"enter 2d location: card_id: %s, ct: %d",card_id.c_str(),time_stamp);
	Log::write_log(FILE_TYPE::SYS_S,log,true);
#endif
	double d_dist[3];
	Point2 pt2, pt2_1;

	set_anchors(cnt);

	int i = 0;
	for(auto it : distmap)
	{
		for(int j=0; j<cnt; j++)
		{
			if(p2_anchors[j].x == it.second->x && p2_anchors[j].y == it.second->y)
			{
				//d_dist[j] = it.second->d;
				// lihongzhen  2017/10/24 利用投影计算二维空间值
				d_dist[j] = sqrt(pow(it.second->d,2) - pow(it.second->z, 2));			
			}
		}	

		if (i == 0)
		{
			time_stamp_cal = it.second->t;
			time_cur_algo = it.second->cur_time;
		}
		i++;
	}

	Mat mat(3, 1, d_dist);

	if(is_anchor_changed){
		cal_location->setAnchors(p2_anchors);
		pt2 = cal_location->initPos(mat);
		is_anchor_changed = false;
	} else{
		pt2 = cal_location->estiPos(mat);
	}

	pt2_1 = cal_location->getError();

	_point p;
	p.x = pt2.x;
	p.y = pt2.y;


#ifdef ENABLE_DEBUG_2D_TEST_LOG
	memset(log,0,MAX_LOG_LENGTH*sizeof(char));
	sprintf_s(log,"2d location before handling:card_id: %s, ct: %d, x : %.3f, y: %.3f",card_id.c_str(),time_stamp,p.x,p.y);
	Log::write_log(FILE_TYPE::SYS_S,log,true);
#endif

	if(deal_position_boundary(p)){
#ifdef ENABLE_DEBUG_2D_TEST_LOG
		memset(log,0,MAX_LOG_LENGTH*sizeof(char));
		sprintf_s(log,"2d location failed:card_id: %s, ct: %d, x : %.3f, y: %.3f",card_id.c_str(),time_stamp,p.x,p.y);
		Log::write_log(FILE_TYPE::SYS_S,log,true);
#endif
		return;
	}


	//if(deal_position_boundary(p)){
	//	return;
	//}
	error_locate.x = pt2_1.x, error_locate.y = pt2_1.y;
	if (!CheckPositionValid(error_locate.x,error_locate.y))
	{
		error_locate.x = error_locate.y = INVALID_COORDINATE;
	} 

	last_locate.x = x = p.x;
	last_locate.y = y = p.y;
	z = 0;

	time_last_tof = time_cur_algo;
	this->t = this->last_locate.t = time_stamp_cal;	
	this->time_stamp_last = time_stamp_cal;

	is_deal_by_algo = true;
	if (is_first_location)
	{
		is_first_location = false;
	}

#ifdef ENABLE_DEBUG_2D_TEST_LOG
	memset(log,0,MAX_LOG_LENGTH*sizeof(char));
	sprintf_s(log,"2d location output: card_id: %s, ct: %d, x : %.3f, y: %.3f",card_id.c_str(),time_stamp,x,y);
	Log::write_log(FILE_TYPE::SYS_S,log,true);
#endif	
}

void Card::get_coordinate_2d_nofilter(int cnt)
{
	double d[3];
	double solution[2]; 
	_Matrix pi;

	matrix_set(&pi, 3, 2);
	matrix_init(&pi);

	//获得坐标
	int i = 0;
	for(auto it : distmap)
	{
		matrix_write(&pi, i, 0, it.second->x);
		matrix_write(&pi, i, 1, it.second->y);
		d[i] = sqrt(pow(it.second->d,2) - pow(it.second->z,2));

		if (i == 0)
		{
			time_stamp_cal = it.second->t;
		}
		i++;
	}

	_point p;
	if (locate_func(&pi, d, solution) > 0)
	{
		p.x = solution[0];
		p.y = solution[1];

		last_locate.x = x = p.x;
		last_locate.y = y = p.y;
		z = 0;

		this->time_stamp_last = time_stamp_cal;
		is_deal_by_algo = true;
		if (is_first_location)
		{
			is_first_location = false;
		}
	}
}

void Card::get_coordinate_3d(int cnt)
{
	double d_dist[ANCHOR_COUNT];
	Point3 pt3, pt3_1;

	set_anchors(cnt);

	int i = 0;
	for(auto it : distmap)
	{
		for(int j=0; j<cnt; j++)
		{
			if(p3_anchors[j].x == it.second->x && p3_anchors[j].y == it.second->y &&
				p3_anchors[j].z == it.second->z)
			{
				d_dist[j] = it.second->d;
			}
		}
		if (i == 0)
		{
			time_stamp_cal = it.second->t;
		}
		i++;
	}

	Mat mat(ANCHOR_COUNT, 1, d_dist);
	if(is_anchor_changed){
		cal_location3->setAnchors(p3_anchors, ANCHOR_COUNT);
		pt3 = cal_location3->initPos(mat);
		is_anchor_changed = false;
	} else{
		pt3 = cal_location3->estiPos(mat);
	}
	x = pt3.x;
	y = pt3.y;
	z = pt3.z;

	this->time_stamp_cal_last = time_stamp_cal;
	is_deal_by_algo = true;
}

void Card::get_coordinate_old(int cnt)
{
	if(cnt > 3)
	{
		get_coordinate_3d(cnt);
	}
	else if(cnt > 2)
	{
		get_coordinate_2d(cnt);
	}
}

void Card::add_dist_ex(std::shared_ptr<_coordinate> dist)
{
	bool b_exist = false;
	int min_stamp = dist->t, min_stamp_index = -1;
	time_stamp_max = dist->t;
	for(int i = 0; i < DIST_COUNT; i++ ){
		if(p_dists[i] == NULL){
			p_dists[i] = dist;
			is_anchor_changed = true;
			b_exist = true;
			break;
		}
		if(p_dists[i]->x == dist->x && p_dists[i]->y == dist->y 
			&& p_dists[i]->z == dist->z && p_dists[i]->a == dist->a){ // 同一分站，新数据有效
				dist->d_offset = p_dists[i]->d_offset; // 同一分站的偏移
				if(dist->t >= p_dists[i]->t){
					p_dists[i] = dist;
				}else if(abs(dist->t - p_dists[i]->t) > 100){
					p_dists[i] = dist;
				}
				b_exist = true;
				break;
		}
		if(min_stamp >= p_dists[i]->t && abs(min_stamp-p_dists[i]->t) <100){
			min_stamp = p_dists[i]->t;
			min_stamp_index = i;
		}else if(min_stamp < p_dists[i]->t && abs(min_stamp - p_dists[i]->t) >= 100){
			min_stamp = p_dists[i]->t;
			min_stamp_index = i;
		}
	}
	if(!b_exist){
		p_dists[min_stamp_index] = dist;
		is_anchor_changed = true;
	}

	get_coordinate_old(get_effictive_dist_count_ex());
	is_deal_by_algo = true;
	if (this->x != this->last_x || this->y != this->last_y)
	{
		b_pos_change = true;
	}
	else
	{
		b_pos_change = false;
	}
}

int Card::get_effictive_dist_count_ex()
{
	int ret = 0, tmp = -1;
	for(int i = 0; i < DIST_COUNT; i++){
		if(p_dists[i] == NULL) break;
		if(p_dists[i]->t == time_stamp_max){
			ret++;
		}
	}
	return ret;
}

LeaderArrange::LeaderArrange(string cardid)
{
	card_id = cardid;
	starttime = time(NULL);
	endtime = time(NULL);
}

LeaderArrange::~ LeaderArrange()
{
}

VehicleTypeAttRule::VehicleTypeAttRule(int vtid, int pre, int back)
{
	this->vehicle_type_id = vtid;
	this->offset_pre = pre;
	this->offset_back = back;
}

VehicleTypeAttRule::~VehicleTypeAttRule()
{
	this->Clear();
}

void VehicleTypeAttRule::Clear()
{
	this->vecArea.clear();
}

void VehicleTypeAttRule::AddArea(int area_id)
{
	std::vector<int>::iterator itArea = std::find(this->vecArea.begin(), this->vecArea.end(), area_id);
	if(itArea == this->vecArea.end())
	{
		this->vecArea.push_back(area_id);
	}
}
const std::string& Card::cardId()
{
	return card_id;
}
const uint64_t Card::CtTime()
{
	return m_ct_time;
}
const int Card::getX()
{
	return  (int)CFunctions::round(this->x,0);
}
int Card::getAreaId()
{
	return area_id;
}
int Card::getAcc()
{
	return accelerate_state;
}
double Card::getv()
{
	return final_v;
}
const int Card::getY()
{
	return  -(int)CFunctions::round(this->y,0);
}
void Card::do_alarm(const std::size_t ow)
{
	//
}
const uint64_t  Card::get_last_recv_time()
{
	return rec_time;
}

void Card::PrintString(const std::string &&s)
{
	debug_print_syslog(0,"[LemonHash Card%s]%s",card_id.c_str(),s.c_str());
}
int Card::getCardType()
{
	return card_type;
}
bool Card::getBigCarFlag()
{
	return m_bigCarFlag;
}

//人卡，掘进机，采煤机返回false，其他返回true
bool Card::smoothFlag()
{
	return false;
	if (card_type == 0x01 || VT_DIGGINGCOAL_MACHINE == vehice_type_id || VT_DRIVINGFACE_MACHINE == vehice_type_id)
	{
		return false;
	}
	return true;
}

//将定位坐标赋值给卡，以便卡执行业务逻辑
int Card::push_optimized_data(NAMESPACE_POINT::loc_point&lp)
{
	//debug_print_syslog(0,"[lemon push data] %s,x:%f,y:%f,ct:%d",card_id.c_str(),lp.x,lp.y,lp.m_ct);
	//this->x = lp.x;
	//this->y = -lp.y;
	//this->time_center_stamp = lp.m_ct;
	////this->final_v = 0;
	////this->state_moving = 0;
	////this->m_nStream = 1;
	//this->origin_locate.x = this->x;
	//this->origin_locate.y = this->y;

	//不在猴车上的人需要如下判断；
	//掘进机或采煤机需要如下判断；
	if ((CT_PERSON == card_type && state != STATE_ACCE_GETON_MONKEYCAR) || ((VT_DIGGINGCOAL_MACHINE == vehice_type_id || VT_DRIVINGFACE_MACHINE == vehice_type_id)))
	{
		NAMESPACE_POINT::point p;
		p.set(lp.x,-lp.y);

		if (INVALID_COORDINATE != last_locate.x && INVALID_COORDINATE != last_locate.y)
		{
			double speed = 0.0, acc = 0.0, dt = 0.0,dist = 0.0;
			dt = abs(lp.m_time - last_locate.cur_time)/1000.0;
			NAMESPACE_POINT::loc_point llp;
			llp.set(last_locate.x,last_locate.y);
			NAMESPACE_POINT::loc_point cp;
			cp.set(lp.x,-lp.y);
			dist = cp.dist_direct(llp)*map_scale;
			speed = abs(dist)/dt;
			acc = abs(speed - last_locate.v)/dt;
			lp.m_speed = speed;
			switch (card_type)
			{
			case CT_PERSON:
				if (acc > PERSON_ACCELERATE_THRESHOLD||speed*3.6 > MAX_PERSON_SPEED)
				{
					return 0;
				}
				break;
			case CT_COALMINING_MACHINE:
			case CT_TUNNELBORING_MACHINE:
				if (acc > VECHILE_ACCELERATE_THRESHOLD||speed*3.6 > MAX_VECHILE_SPEED*0.06)
				{
					return 0;
				}
				break;
			}
			if (acc > PERSON_ACCELERATE_THRESHOLD||speed*3.6 > MAX_PERSON_SPEED)
			{
				debug_print_syslog(0,"[not push data] %s,x:%f,y:%f,ct:%d",card_id.c_str(),lp.x,lp.y,lp.m_ct);
				return 0;
			}
		}
		last_locate.x = lp.x;
		last_locate.y = -lp.y;
		last_locate.cur_time = lp.m_time;
		last_locate.t = lp.m_ct;
		last_locate.v = lp.m_speed;
	}
	this->x = lp.x;
	this->y = -lp.y;
	this->time_center_stamp = lp.m_ct;
	//this->final_v = 0;
	//this->state_moving = 0;
	//this->m_nStream = 1;
	this->origin_locate.x = this->x;
	this->origin_locate.y = this->y;

	debug_print_syslog(0,"[lemon push data] %s,x:%f,y:%f,ct:%d",card_id.c_str(),lp.x,lp.y,lp.m_ct);
	return 0;
}
int Card::algo_tof_raw_1d(const DistMap& dm)
{
	if (LOCATEDATATYPE::LDT_TOF == ranging_type)
	{
		if (dm.size() < 1)
		{
			return 1;
		}
	} 
	else
	{
		if (dm.size() < 2)
		{
			return 2;
		}
	}

	is_del_data = true;
	int nRet = 0;
	//组装数据部分
	map<unsigned long long, std::shared_ptr<_coordinate>> tmp_dists_locate;
	int i = 0;
	int nCount = 0;
	bool bFound = false;

	unordered_map<int,std::list<double>> check_dist_data;
	for(DistMap::iterator it = distmap.begin(); it != distmap.end(); ++it){
		bFound = false;
		int tct = 0;
		tct = it->second->t;
		// 判断是否已经加入到列表
		for(int j = 0; j < i; j++){
			if(tmp_dists_locate.find(it->second->reader_id) != tmp_dists_locate.end()){
				//已存在，则退出查找for循环
				bFound = true;
				break;
			}
		}
		if(i < DIST_COUNT && !bFound){
			if (abs(it->second->d + 1.0) < ZERO_PRECISION)
			{
				//即计算的距离为-1，抛弃此数据
				continue;
			}else{
				tmp_dists_locate.insert(make_pair(i,(it->second)));
				check_dist_data[it->second->reader_id].push_back(it->second->d);
			}
			i++;
		}
	}

	map<unsigned long long, std::shared_ptr<_coordinate>>::iterator it = tmp_dists_locate.begin();
	time_stamp_cal = it->second->t;

	double dMaxDistance = 1000.0;
	//修正由于高度所引入的误差
	for (map<unsigned long long, std::shared_ptr<_coordinate>>::iterator it_tmp = tmp_dists_locate.begin();it_tmp != tmp_dists_locate.end();++it_tmp)
	{
		double distance = 0.0;
#ifdef _DEBUG
		distance = sqrt(pow(it_tmp->second->d,2) - pow(TOF_HEIGHT_ERROR,2));//TOF_HEIGHT_ERROR
#else
		distance = sqrt(pow(it_tmp->second->d,2) - pow(TOF_HEIGHT_ERROR,2));
#endif
		it_tmp->second->d = distance;
		if (distance < dMaxDistance)
		{
			dMaxDistance = distance;
		}
	}

	//	nRet = m_position_optimizer->push_raw_data(tmp_dists_locate);
	//std::vector<std::shared_ptr<_coordinate>> vt_pos;
	//vt_pos.resize(0);
	//vt_pos = calc_locate_tof_raw_1d(tmp_dists_locate);

	//i = 0;
	//std::string log = "";
	//char chlog[MAX_LOG_LENGTH] = {0};

	//sprintf_s(chlog,"ct:%d , ",time_stamp_cal);
	//log+=chlog;
	//for (std::vector<std::shared_ptr<_coordinate>>::iterator it_pos = vt_pos.begin(); it_pos != vt_pos.end();++it_pos )
	//{
	//	memset(chlog,0,MAX_LOG_LENGTH*sizeof(char));
	//	sprintf_s(chlog,"x%d: %.3f ,y%d: %.3f ,",i,(*it_pos)->x,i,(*it_pos)->y);
	//	log += chlog;
	//	i++;
	//}
	//Log::write_log(FILE_TYPE::SYS_S,log,true);

	return 0;
}


const std::vector<std::shared_ptr<_coordinate>> Card::calc_locate_tof_raw_1d(map<unsigned long long, std::shared_ptr<_coordinate>> p_dists_locate) const
{
	std::vector<std::shared_ptr<_coordinate>> vt_pos;

	for (map<unsigned long long, std::shared_ptr<_coordinate>>::iterator it = p_dists_locate.begin();it != p_dists_locate.end();++it)
	{
		if(pTofReaderPathMap->find(it->second->reader_id) == pTofReaderPathMap->end())
		{
			continue;
		}
		int seg_num = 0;
		seg_num = pTofReaderPathMap->find(reader_id)->second->nRealCalcPoints - 1;
		//判断路径集的点数是否大于两个点，两点才可以构成线段路径
		if(seg_num < 0 ){
			continue;
		}

		double d[MAX_READER_TDOA_PATH_NUMS] = {0};
		double xcross[MAX_READER_TDOA_PATH_NUMS] = {0};
		double ycross[MAX_READER_TDOA_PATH_NUMS] = {0};

		int j = 0;
		int m = 0;
		int nCounts = 0;
		//对每一个线段所在的直线和以天线为圆心的圆求解，最多为两个解
		for(int i = 0;i < seg_num;i++){
			//如果线段两端点的坐标不存在，则寻找下一个线段
			if(pTofReaderPathMap->find(reader_id)->second->px[i] == -1 || pTofReaderPathMap->find(reader_id)->second->px[i+1] == -1)
			{
				continue;
			}

			//求线段所在直线和分站readerid的antenna_idx天线为圆心，半径为dist的圆的两个交点
			std::shared_ptr<SOLUTION> r = LocateAlgorithm::GetPos(pTofReaderPathMap,it->second->reader_id,it->second->antenna_id,it->second->d,i);
			//如果解不存在，则继续寻找下一个线段
			if(r == nullptr || r->nCount <= 0){
				continue;
			}

			//如果解存在，则保存两个解
			for(j = 0;j < r->nCount;j++){
				std::shared_ptr<_coordinate> p = std::make_shared<_coordinate>();
				p->x = r->x[j];
				p->y = r->y[j];
				p->reader_id = it->second->reader_id;
				p->antenna_id = it->second->antenna_id;
				p->d = it->second->d;
				p->t = it->second->t;
				vt_pos.push_back(p);
			}
		}

	}

	return std::move(vt_pos);
}

int Card::get_antenna_xyz(int reader_id, int antenna_id,double& x,double& y, double& z)
{
	x = pTofReaderPathMap->find(reader_id)->second->x[antenna_id];
	y = pTofReaderPathMap->find(reader_id)->second->y[antenna_id];
	z = pTofReaderPathMap->find(reader_id)->second->z[antenna_id];

	return 0;
}

int Card::get_key(double x, double y, double& key)
{
	double distance = 999999999.0;
	double offset = 0.0;
	double reader_x1 = 0.0;
	double reader_y1 = 0.0;
	double reader_x2 = 0.0;
	double reader_y2 = 0.0;

	for(auto it = pTofReaderPathMap->begin(); it != pTofReaderPathMap->end(); it++)
	{
		double x_temp = 0.0;
		double y_temp = 0.0;
		x_temp = (*it).second->x[0];
		y_temp = (*it).second->y[0];
		offset = sqrt(std::pow(x_temp - x, 2) + std::pow(y_temp - y, 2));

		if(offset < distance)
		{
			distance = offset;
			reader_x2 = reader_x1;
			reader_y2 = reader_y1;
			reader_x1 = x_temp;
			reader_y1 = y_temp;
		}
	}
	std::stringstream _ss_log;
	_ss_log << "++++++++++print_key,card:" << card_id 
		<<", reader_x2:"<<reader_x2
		<<", reader_y2:"<<reader_y2
		<<", reader_x1:"<<reader_x1
		<<", reader_y1:"<<reader_y1
		<<", "<<__LINE__<<", "<<__FUNCTION__;
	//Log::write_log(FILE_TYPE::POSITION_ORIGIN, _ss_log.str(), true);

	key = abs((reader_y2 - reader_y1) / (reader_x2 - reader_x1));
	return 0;
}

/*
 * 参数p用于选解，得出的解需要和此解距离远的
*/
void Card::calc_tunnel_boring_machine_pos(const op::point& p)
{
	op::point loc_reader_p(p_reader->ant[antenna_id]->antenna_x*map_scale,p_reader->ant[antenna_id]->antenna_y*map_scale);

	op::point line[2];
	auto it = pTofReaderPathMap->find(reader_id);

	if (it!= pTofReaderPathMap->end())
	{
		for (size_t i=0;i<2;++i)
		{
			line[i].set(it->second->px[i],it->second->py[i]);
		}
	}

	std::vector<op::point> vt_pos_list = LocateAlgorithm::calc_pos(loc_reader_p,distance,line);
	
	op::point pos;
	if (vt_pos_list.size()>=2)
	{
		double t_distance = 0.0;
		size_t index = 0;
		for (size_t i = 0;i<vt_pos_list.size();++i)
		{
			if (_isnan(vt_pos_list[i].x)||_isnan(vt_pos_list[i].y))
			{
				continue;
			}
			double ttd = sqrt(pow(vt_pos_list[i].x - p.x,2)+pow(vt_pos_list[i].y - p.y,2));
			if (ttd > t_distance)
			{
				t_distance = ttd;
				index = i;
			}
		}
		pos = vt_pos_list[index];
	}

	if (CT_SIMULATE_CARD != card_type)
	{
		double speed = 0.0;
		if (INVALID_COORDINATE != last_locate.x && INVALID_COORDINATE != last_locate.y)
		{
			double acc = 0.0, dt = 0.0,dist = 0.0;
			dt = abs(rec_time - last_locate.cur_time);
			NAMESPACE_POINT::loc_point llp;
			llp.set(last_locate.x,last_locate.y);
			NAMESPACE_POINT::loc_point cp;
			cp.set(pos.x,pos.y);
			dist = cp.dist_direct(llp)*map_scale;
			speed = abs(dist)/dt;
			acc = abs(speed - last_locate.v)/dt;
			switch (card_type)
			{
			case CT_TUNNELBORING_MACHINE:
				if (acc > 0.01||speed*3.6 > 1)
				{
					return;
				}
				break;
			}
			if (acc > 0.01||speed*3.6 > MAX_PERSON_SPEED)
			{
				debug_print_syslog(0,"[Drivingface machine] %s,x:%f,y:%f,ct:%d",card_id.c_str(),pos.x,pos.y,sync_num);
				return;
			}
		}
		last_locate.v = speed;
	}
	
	this->x = pos.x/map_scale;
	this->y = pos.y/map_scale;
	this->origin_locate.x = this->x;
	this->origin_locate.y = this->y;
	last_locate.x = pos.x;
	last_locate.y = pos.y;
	last_locate.cur_time = rec_time;
	last_locate.t = sync_num;
	
	debug_print_syslog(0,"[three rates: Drivingface machine] %s,x:%f,y:%f,ct:%d",card_id.c_str(),pos.x,pos.y,sync_num);

#ifdef DEBUG
	char log[MSG_LENGTH_MAX];
	memset(log,0,MSG_LENGTH_MAX*sizeof(char));
	sprintf_s(log,"card_id: %s, ct: %d, x: %.3f, y: %.3f",card_id.c_str(),this->t,this->x,this->y);
	Log::write_log(FILE_TYPE::LOCATE_TOF_S,log,true);
#endif
}