server/classdef.cpp

#include "stdafx.h"
#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 "./log/log_module.h"

#include <math.h>

#pragma warning(disable: 4244)

unsigned int g_nAlgoFailedCounts[ALGO_LOC_TOTAL] = {0};
unsigned int g_nAlgoFailedCycleCounts[ALGO_LOC_TOTAL] = {0};
ULONGLONG g_ullCurTime = 0;

Card::Card( string cardid, int cardtype, double z_offset, double offset_x /*= 12.0*/, double offset_y /*=12.0*/ )
{
	InitializeCriticalSectionAndSpinCount(&m_csCard, MAXCRITICALSECTIONSPINCOUNT);

	card_type = cardtype;
	card_id = cardid;
	coor_offset_x = offset_x;
	coor_offset_y = offset_y;
	this->z_offset = z_offset;

	::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_low_power = time(NULL);
	last_locate_time = time(NULL);

	//坐标初始值为0
	//x =	y = z = stored_x = stored_y = stored_z = 0;
	x =	y = z = stored_x = stored_y = stored_z = INVALID_COORDINATE;

	output_x = output_y = 0;
	xx = yy = zz = 0;
	x1 = y1 = z1 = x2 = y2 = z2 = x3 = y3 = z3 = x4 = y4 = z4 = 0;
	a = 0;
	t = 0;
	init_postion = false;
	is_first_location = true;
	right_x = right_y = right_z = left_x = left_y = left_z = 0;
	m_nMoveDirection = 0;

	map_id = map_id_old = area_id = reader_id = 0;
	map_scale = 1.0;
	state = 0;
	state_moving = 0;
	state_biz = 0;
	pos_state = pos_state_old = PDT_INIT;
	dept_id = group_id = 0;
	power_state = power_state_last = 0;

	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_pos_changed = is_hist = is_need_cal = false;
	is_area_over_time = is_mine_over_time = false;

	id = name = number = department = group = worktype = "";
	driver_id = "";

	reader_tickcount = time_stamp = time_stamp_last = 0;

	p_dists_locate = new _coordinate*[DIST_COUNT];
	time_stamp_max = 0;
	for(int i = 0; i < DIST_COUNT; i++){
		p_dists_locate[i] = NULL;
	}

	is_deal_by_algo = false;
	pReaderPathMap = NULL;
	pTdoaReaderPathMap = NULL;
	last_s_locate_reader[0] = last_s_locate_reader[1] = -1;
	m_syncNumList.clear();
	m_dFirstDiff = m_dSecondDiff = 0.0;

	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 = last_locate.y = last_locate.z = INVALID_COORDINATE;
	last_x = last_y = last_z = INVALID_COORDINATE;
	
	locate = nullptr;

	m_bUseFilter = false;
	m_nFilterType = NO_FILTER;
	m_pKalmanFilter = 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;
	ins_weight = INS_WEIGHT;
	uwb_weight = UWB_WEIGHT;
	acce_cur_state = 0;
	acce_last_state = 0;
	b_long_interval = false;
	accelerate_state = accelerate_state_last = ACCELERATE_INIT_STATE;	//默认初始的为-10
	ins_direction = 0;
	direction = 0;
	antenna_angle = antenna_angle_last = 0;

	time_stamp_last = 0;
	reader_id_last  = 0;

	m_nLightGroupId = -1;	//默认为没控制灯组
	b_enter_intersection = false;
	is_red_light = false;
	light_id = 0;
	time_red_light = time(NULL);
	m_nOutputPosState = 0;
	m_nIsRailroad = 0;
	mileage = 0;
	diff_direction_counts = 0;
	m_nStream = 0;

	for(int i = 0; i < CARD_EVENT_COUNT; i++){
		m_event_list[i] = 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 = 0;
	vt_deal_call_reader_id.resize(0);
	recv_call_time = time(NULL);

	landmark_id = 0;
	landmark_dis = 0;
	directory = NODIRECTORY;
}

Card::Card( void )
{

}

void Card::reset()
{
	//以下参数与算法相关
	//坐标初始值为0
	TRACE(_T("reset card info \r\n"));
	m_nLastLocateT = 0;
	time_stamp_last = 0;
	b_long_interval = false;
	is_first_location = true;
	last_locate.sync_num = 0;

	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);
	}
}

void Card::set_reader(std::shared_ptr<Reader> preader) // 设置卡时间
{	
	this->rec_time = time(NULL);
	if(preader->reader_id == this->reader_id){ // 所在分站没有发生变化

		this->pos_state_count++;
	}else{ // 分站变化
		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
*
*/
void Card::set_reader_path_tof(std::shared_ptr<ReaderPathMap> rpm)
{
	this->pReaderPathMap = rpm;
}

/*
* 设置地图集覆盖范围，适用TDOA
*
*/
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
 *		无
*/
void Card::inspect_coordinate(int acce_state)
{
	this->isoutput = false;
	
	char log[200] = {0};
	//如果是静止或者怠速状态，每次的定位坐标不变化，此时要求采集不再将此数据送往webserver
	if (acce_state == STATE_ACCE_STATIC)
	{
		this->isoutput = false;
	}
	else
	{
		if (this->x == this->output_x && this->y == this->output_y) //last_x,last_y
		{
			count_idle++;
			this->isoutput = false;
#ifdef _DEBUG
			CString cstrLog = _T("");
			cstrLog = this->card_id.c_str();
			TRACE(_T("1. card_id : %s, idle counts ++ : %d, sync_num : %d \r\n"),cstrLog,count_idle,sync_num);
			sprintf_s(log,"1. card_id : %s, idle counts ++ : %d, sync_num : %d",this->card_id.c_str(),count_idle,sync_num);
			std::string strLog = log;
			Log::write_log(FILE_TYPE::SYS_S,strLog,true);
#endif
		}else{
			//如果运动状态时，
			int nSign = 0;
			int nState = 0;

			if (this->x == this->output_x)
			{
				if (this->y > this->output_y)
				{
					nSign = 1;
				}else{
					nSign = -1;
				}
			}else{
				if (this->x > this->output_x)
				{
					nSign = 1;
				}else{
					nSign = -1;
				}
			}

			if (nSign == this->m_nMoveDirection)
			{
				count_change_direction = 0; // 未连续掉头状态，认为是抖动，不改变方向，重置计数

				double distance = sqrt(pow(this->x - this->output_x,2) + pow(this->y - this->output_y,2)) * this->map_scale;
				if(distance > 0.5) //this->map_scale)
				{
					//两点之间的距离是否大于一个0.5米，满足条件，输出json
					this->isoutput = true;
					this->output_x = this->x;
					this->output_y = this->y;
					count_idle = 0;
					TRACE(_T("1. send pos json \r\n"));
				}else{
					count_idle++;
					#ifdef _DEBUG
					CString cstrLog = _T("");
					cstrLog = this->card_id.c_str();
					//TRACE(_T("2. card_id : %s, idle counts ++ : %d \r\n"),cstrLog,count_idle);
					//sprintf_s(log,"2. card_id : %s, idle counts ++ : %d",this->card_id.c_str(),count_idle);
					TRACE(_T("2. card_id : %s, idle counts ++ : %d, sync_num : %d \r\n"),cstrLog,count_idle,sync_num);
					sprintf_s(log,"2. card_id : %s, idle counts ++ : %d, sync_num : %d",this->card_id.c_str(),count_idle,sync_num);
					std::string strLog = log;
					Log::write_log(FILE_TYPE::SYS_S,strLog,true);
					#endif
				}				
			} 
			else
			{
				if( count_change_direction > 3){
					// 认为掉头，输出给web
					this->m_nMoveDirection = nSign;
					this->isoutput = true;
					this->output_x = this->x;
					this->output_y = this->y;
					count_idle = 0;
					count_change_direction = 0;
					TRACE(_T("2. send pos json \r\n"));
				}else{
					// 认为是抖动，不输出给web
					count_change_direction++;
					count_idle++;
					#ifdef _DEBUG
					CString cstrLog = _T("");
					cstrLog = this->card_id.c_str();
					//TRACE(_T("3. card_id : %s, idle counts ++ : %d \r\n"),cstrLog,count_idle);
					//sprintf_s(log,"3. card_id : %s, idle counts ++ : %d",this->card_id.c_str(),count_idle);
					TRACE(_T("3. card_id : %s, idle counts ++ : %d, sync_num : %d \r\n"),cstrLog,count_idle,sync_num);
					sprintf_s(log,"3. card_id : %s, idle counts ++ : %d, sync_num : %d",this->card_id.c_str(),count_idle,sync_num);
					std::string strLog = log;
					Log::write_log(FILE_TYPE::SYS_S,strLog,true);
					#endif
				}
			}
		}
	}	
}

/*
 * TOF定位算法
 *
 * param
 *		cnt		数据条数
 *
 * return
 *		无
 *
*/
void Card::algo_tof(int cnt)
{
	//int i = 0, nCount = 0;
	//POS *p = NULL;

	//POS tmpPos;
	//tmpPos.posx = 0;
	//tmpPos.posy = 0;
	//tmpPos.pos_radius = 0;
	//int difT = 1;
	//bool bfound = false;
	//DistMap::iterator it = _dists.front().distmap.begin();
	//if(cnt > 2){
	//	for(; it != _dists.front().distmap.end(); ++it){
	//		bfound = false;
	//		// 判断是否已经加入到列表
	//		for(int j=0; j < i; j++){
	//			if(p_dists_locate[j]->reader_id == it->second->reader_id){
	//				bfound = true;
	//				break;
	//			}
	//		}
	//		if(i < DIST_COUNT && !bfound){
	//			p_dists_locate[i] = it->second;
	//			i++;
	//		}
	//	}
	//	nCount = i;
	//}else{
	//	for(; it != _dists.front().distmap.end(); ++it,i++){
	//		p_dists_locate[i] = it->second;
	//	}
	//	nCount = cnt;
	//}


	//if(!init_postion){
	//	this->x = p_dists_locate[0]->x;
	//	this->y = p_dists_locate[0]->y;
	//	this->z = p_dists_locate[0]->z;
	//	this->a = p_dists_locate[0]->a;
	//	this->distance = p_dists_locate[0]->d;
	//	this->t = p_dists_locate[0]->t;

	//	this->last_x = p_dists_locate[0]->x;
	//	this->last_y = p_dists_locate[0]->y;
	//	this->last_z = p_dists_locate[0]->z;

	//	init_postion = true;
	//}	

	//int sta_num = 0;
	//int ant = 0;
	//double dist = 0.0;

	//INFO_PRE info_pre;
	//info_pre.t = p_dists_locate[0]->t;
	//info_pre.detaT = 0;
	//info_pre.sta_num = p_dists_locate[0]->reader_id - 1;
	//info_pre.ant = p_dists_locate[0]->antenna_id;
	//info_pre.dist = p_dists_locate[0]->d;

	//for(int i = 1;i < nCount;i++){
	//	sta_num = p_dists_locate[i]->reader_id - 1;
	//	ant = p_dists_locate[i]->antenna_id;
	//	dist = p_dists_locate[i]->d;
	//	
	//	p = LocateAlgorithm::Pos(*(this->pReaderPathMap),sta_num,ant,dist,info_pre);

	//	if(p == NULL)
	//	{
	//		continue;
	//	}
	//	if(p->pos_radius < 2.0){
	//		if(i == 1){
	//			tmpPos.posx = p->posx;
	//			tmpPos.posy = p->posy;
	//			tmpPos.pos_radius = p->pos_radius;
	//		}else{
	//			double posx = (tmpPos.posx + p->posx)/2;
	//			double posy = (tmpPos.posy + p->posy)/2;
	//			double radius = (tmpPos.pos_radius + p->pos_radius)/2;

	//			tmpPos.posx = posx;
	//			tmpPos.posy = posy;
	//			tmpPos.pos_radius = radius;	
	//		}
	//	}

	//	if(p){
	//		delete p;
	//		p = NULL;
	//	}
	//}

	//if(tmpPos.posx != 0 && tmpPos.posy != 0){
	//	int difT = info_pre.t - this->t;
	//	double newPosX = tmpPos.posx / (this->map_scale*1.0);
	//	double newPosY = tmpPos.posy / (this->map_scale*1.0);
	//	double d = sqrt(pow(newPosX - this->last_locate.x,2)+pow(newPosY - this->y,2))*this->map_scale;
	//	if(difT==0){
	//		this->last_locate.v = 0;
	//	}else{
	//		this->last_locate.v = d/difT;
	//	}

	//	this->last_locate.x = tmpPos.posx / (this->map_scale*1.0);
	//	this->last_locate.y = tmpPos.posy / (this->map_scale*1.0);
	//	this->last_locate.t = info_pre.t;
	//}

	//this->x = this->last_locate.x;
	//this->y = this->last_locate.y;
	//this->v = ((int)(this->last_locate.v*1000))/1000.0;
	//this->z = 0;
	//this->a = 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.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;
		m_afmData.bStatus = true;
		m_afmData.strCardId = this->card_id;
		m_afmData.nType = ALGO_FAILED_CONDITION_2;
		ALGORITHM_FAILED(ALGO_FAILED_CONDITION_2);
		return;
	}

	std::shared_ptr<ReceiveDataMap> pRdm = std::make_shared<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 = 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++;
	}

	//存在异常值，直接返回上一次结果值
	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;
		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;
	}

	//time_stamp_last特指卡的计数序号，非分站的
	//当卡需要新定位的计数序列号小于上一次成功定位的序列号，则此次不定位，避免定位结果的回退
	if (this->time_stamp_last != 0 && this->time_stamp_last > this->time_stamp_cal)
	{
		string str_last = "";
		string str_cur  = "";

		str_last = CFunctions::int2string(this->time_stamp_last);
		str_cur  = CFunctions::int2string(this->time_stamp_cal);

		if (str_last.length() == 5 && str_cur.length() <= 2)
		{
			//满足此条件，表示标示卡的计数序号走完了65535的周期
		}
		else{
			pRdm->clear();
			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;
		}
	}

	this->time_stamp_last = this->time_stamp_cal;

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

	bool bOutput = false;
	this->z = 0;
	i = 0;
	
	if(pRdm->size() > 1){
		LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_147);
		this->m_nCalcSyncNum = maxSyncTimes;
		//p = LocateAlgorithm::Pos(pRdm, pTdoaReaderPathMap);

		//算法逻辑修改：
		//1.当开始两分站之间有地图集时，走原来的算法逻辑
		//2.当开始两分站之间无地图集时，走两两遍历的算法逻辑
		ReceiveDataMap::iterator first = pRdm->begin();
		ReceiveDataMap::iterator second = first;
		//偏移到第二个元素
		std::advance(second,1);
		bool bFind = false;
		bool bExitPath = false;

		//如果两级都能找到才运行继续后续操作，否则，表明没有此路径地图集
		TDOAReaderPathMap::iterator rdm_it = pTdoaReaderPathMap->find(first->second->reader_id);
		if(rdm_it != pTdoaReaderPathMap->end()){
			//表示地图集中存在第一个分站的路径集
			bFind = true;
		}

		if (bFind)
		{
			//确认第一个和第二个分站之间是否有地图集
			ReaderPathMap::iterator rpm_it = pTdoaReaderPathMap->find(first->second->reader_id)->second->find(second->second->reader_id);
			if(rpm_it == pTdoaReaderPathMap->find(first->second->reader_id)->second->end()){
				bExitPath = false;
			}else{
				bExitPath = true;
			}
		}
		
		if (bExitPath)
		{
			//以第一个基站作为基准分站进行坐标计算
			p = LocateAlgorithm::Pos(pRdm, pTdoaReaderPathMap);
		}else{
			//pRdm内的数据双重遍历进行坐标计算
			p = LocateAlgorithm::LocatePos(pRdm, pTdoaReaderPathMap);

			if(fabs(p->posx) < 1E-4 && fabs(p->posy) < 1E-4 ){
				p->posx = INVALID_COORDINATE;
				p->posy = INVALID_COORDINATE;
				p->posz = INVALID_COORDINATE;
			}
		}

		//std::shared_ptr<POS> p3 = LocateAlgorithm::Pos(pRdm);
		//p = LocateAlgorithm::Pos(pRdm);
		if (p==nullptr)
		{
			//p = std::make_shared<POS>();
			//p->posx = INVALID_COORDINATE;
			//p->posy = INVALID_COORDINATE;
			//p->posz = INVALID_COORDINATE;
		}else{
			//debug_print_syslog(0, "CalcCardPosition cardid position by 3D, cx: %f, cy: %f, cz: %f",
			//	p->posx, p->posy, p->posz);
		}
	
		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 = 0.2; // 当ReadFileTest时，没有set_reader，给默认值
		if(this->p_reader){
			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);
				if (m_syncNumList.size() > 0)
				{
					m_syncNumList.erase(m_syncNumList.begin(),m_syncNumList.end());
					m_syncNumList.resize(0);
				}
				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，就不要此次的定位数据，
				//如果通过加速度判断就将队列中从头开始到此同步数据的所有元素都丢弃，并插入新的此次同步数据
				this->m_dFirstDiff = p->dDiff[0];
				this->m_dSecondDiff = p->dDiff[1];

				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;
							pRdm->clear();

							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;
							pRdm->clear();

							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;
							pRdm->clear();

							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;
							pRdm->clear();

							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;
						pRdm->clear();

						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){
					m_afmData.nCardStamp = m_nCalcSyncNum;
					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;
						pRdm->clear();

						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);
					p->nFirstReader = last_s_locate_reader[0];
					p->nSecondReader = last_s_locate_reader[1];
				}

				//增加地图集的判定，判断定位结果是否在地图集上
				//如果不在地图集上，需要再次定位
				//需要带出定位结果的分站信息，
				//利用地图集中分站信息再次定位
				//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){
						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();
				}		

				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;

				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为下行
					//其他情况为上行
					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))
					{
						//TRACE(_T("down stream \r\n"));
						this->m_nStream = DOWN_STREAM;
					} 
					else
					{
						//TRACE(_T("up stream \r\n"));
						this->m_nStream = UP_STREAM;
					}
				}

				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();

				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;

				last_s_locate_reader[0] = p->nFirstReader;
				last_s_locate_reader[1] = p->nSecondReader;

				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

				if (vt_his_speed.size()==3)//10
				{
					vt_his_speed.pop_front();
				}
				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 (this->acce_cur_state == STATE_ACCE_STATIC)
				{
					this->v = 0;
				} 
				else
				{
					this->v = av*nSign;
				}

				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);
				}

				//校验机会3次，3次内跳回去就丢弃此次计算结果
				if (this->x != INVALID_COORDINATE && this->y != INVALID_COORDINATE)
				{
					int nDirection = 0;
					if (!algo_is_same_direction(this->x,this->y,0))
					{
						this->x = this->last_locate.x;
						this->y = this->last_locate.y;
					}
				}
#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();
}

int Card::algo_tdoa_1d(int cnt)
{
	int ret = 0;
	std::shared_ptr<POS> pos = std::make_shared<POS>();

	//1.数据有效性判断
	bool b_is_single = false;
	ret = CheckDistData(cnt);
	if(ret){
		b_is_single = true;
		OutputCmdLog(1);	
	}

	//2.组装数据
	std::shared_ptr<ReceiveDataMap> pRdm = std::make_shared<ReceiveDataMap>();
	pRdm->clear();
	ret = AssembleDistData(pRdm);
	if(ret){
		OutputCmdLog(2);
		return ret;
	}
	
	// 获取两次定位的时间差数据
	GetDeltaT(mp_dists_locate);

	//保证每个ct都输出一个坐标:
	//特殊情况一：单条记录也输出
	if (b_is_single)
	{
		int nRet = GetPosFromFittingData(pos);
		if (nRet)
		{
			if (last_locate.x == INVALID_COORDINATE && last_locate.y == INVALID_COORDINATE)
			{
				//这种情况只针对第一次运行时，无法计算结果的情况
				last_locate.x = 0;
				last_locate.y = 0;
				return 1;
			}
			//表示无拟合数据可以输出
			std::shared_ptr<POS> fit_p = GetPosFromFittingData();
			//在所有退出路径都补坐标，保证每个ct都有输出
			pos->posx = last_locate.x * map_scale;
			pos->posy = last_locate.y * map_scale;
			pos->cx = last_locate.x;
			pos->cy = last_locate.y;
			pos->cvx = last_vx;
			pos->cvy = last_vy;
		}		
	}else{
		//3.算法定位	
		std::vector<std::shared_ptr<POS>> udm_pos;
		udm_pos.resize(0);
		//time_stamp_cal == 46163	
		//time_stamp_cal == 3388
		ret = LocateAlgorithm::CalcTdoaPosition(pRdm,pTdoaReaderPathMap,udm_pos);

		if (ret == 0)
		{
			//4.从多解中筛选出一个解,
			//存在两种可能：
			//a.可能无解返回非0，
			//b.可能有解，但解不正确，比如解的位置在4727,-100,但选出的解是4727，-200
			ret = ChooseOneSolution(pRdm, udm_pos, pos);
			if (ret == 0)
			{
				//5.如果有解，则对唯一解做合法性验证
				//主要验证条件为：加速度和速度
				ret = CheckSolution(pos);	
			}
		}

		//无论正确与否，保存原始值
		SaveOriginDataBeforeFilter(pos);

		ret = CheckSolutionByFit(ret,pos);
		//std::shared_ptr<POS> p = GetPosFromFittingData();
		if (ret)
		{
			//ret = GetPosFromFittingData(pos);
			//if (ret)
			//{
			if (last_locate.x == INVALID_COORDINATE && last_locate.y == INVALID_COORDINATE)
			{
				//这种情况只针对第一次运行时，无法计算结果的情况
				last_locate.x = 0;
				last_locate.y = 0;
				return 1;
			}
			//在所有退出路径都补坐标，保证每个ct都有输出
			pos->posx = last_locate.x * map_scale;
			pos->posy = last_locate.y * map_scale;
			pos->cx = last_locate.x;
			pos->cy = last_locate.y;
			pos->cvx = last_vx;
			pos->cvy = last_vy;
			//return 1;
			//}
		}	

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

	ret = CheckSolutionBySpeed(pos);
	if (ret)
	{
		//TRACE(_T("exit by speed \r\n"));
		//在所有退出路径都补坐标，保证每个ct都有输出
		pos->posx = last_locate.x * map_scale;
		pos->posy = last_locate.y * map_scale;
		pos->cx = last_locate.x;
		pos->cy = last_locate.y;
		pos->cvx = last_vx;
		pos->cvy = last_vy;
		//return 1;
	}else{
		if (pos->is_fit)
		{
			this->rec_time = time(NULL);
		}	
	}
	bool bRet = LocateAlgorithm::IsOnMap(pos,pTdoaReaderPathMap);
	if (bRet)
	{
		//7.数据保存
		TRACE(_T("x: %.4f , y :%.4f \r\n"),pos->cx,pos->cy);
		ret = SaveCardAlgoData(pos);
	}

	return ret;
}

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;

	//3.算法定位
	std::shared_ptr<POS> pos = std::make_shared<POS>();
	pos = LocateAlgorithm::TdoaLocate2d(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;
}

int Card::algo_tdoa_3d(int cnt)
{
	if (cnt < 3)
	{
		return 1;
	}

	int sync_num = 0;
	int k = 0;
	bool ret = false;

	mp_dists_locate.clear();

	for (DistMap::iterator it = _dists.front().distmap.begin();it!=_dists.front().distmap.end();++it,k++)
	{
		if (k==0)
		{
			sync_num = it->second->sync_num;
		} 
		else
		{
			int diff = abs(sync_num - it->second->sync_num);
			if (diff >= 5)
			{
				ret = true;
				break;
			}
		}
		if (it->second->tt == LLONG_MAX)
		{
			ret = true;
			break;
		}
		mp_dists_locate.insert(make_pair(it->second->tt,it->second));
	}

	if (ret)
	{
		return 1;
	}

	//构造数据
	int i = 0;
	int max_sync_time = 0;

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

	for (map<unsigned long long ,std::shared_ptr<_coordinate>>::iterator it = mp_dists_locate.begin();it!=mp_dists_locate.end();++it)
	{
		int card_time_stamp = 0;

		if (i==0)
		{
			max_sync_time = it->second->sync_num;
			card_time_stamp = it->second->t;
		}else{
			if (max_sync_time < it->second->sync_num)
			{
				max_sync_time = it->second->sync_num;
				card_time_stamp = it->second->t;
			}
		}

		ReceiveDataMap::iterator rdm_it = pRdm->find(it->second->tt);
		if (rdm_it == pRdm->end())
		{
			//保存信息用于定位
			std::shared_ptr<ReceiveData> prd = std::make_shared<ReceiveData>();
			prd->reader_id = it->second->reader_id;
			prd->antenna_id = it->second->antenna_id;
			prd->rec_time_stamp = it->second->tt;
			prd->x = it->second->x*this->map_scale;
			prd->y = it->second->y*this->map_scale;
			prd->z = it->second->z*this->map_scale;
			prd->special = it->second->special;
			if (prd->rec_time_stamp > 0)
			{
				pRdm->insert(make_pair(prd->rec_time_stamp,prd));
			}
		}
		i++;
	}
	
	//if (time_stamp_last !=0 && time_stamp_last > time_stamp_cal)
	//{
	//	if ((time_stamp_last + time_stamp_cal)%65535 < 2)
	//	{
	//		return;
	//	}
	//}

	if (pRdm->size() >2)
	{
		m_nCalcSyncNum = max_sync_time;
		std::shared_ptr<POS> p = nullptr;
		//p = LocateAlgorithm::Pos(pRdm);
	}
	return 0;
}

/*
 * 根据算法计算新位置的坐标以及卡的上一次坐标，计算车辆偏移坐标
 *
 * param
 *		无
 *
 * return
 *		无locatepos
 *
*/
void Card::algo_calc_offset()
{
	double offset = CHAMBER_WIDTH/4;
	if (this->last_locate.x == this->x)
	{
		//判断车的方向
		switch (this->m_nMoveDirection)
		{
		case 1:
			right_y = left_y = this->y;
			right_x = this->x - offset;
			left_x  = this->x + offset;
			break;
		case -1:
			right_y = left_y = this->y;
			right_x = this->x + offset;
			left_x  = this->x - offset;
			break;
		default:
			break;
		}
	}else if(this->last_locate.y == this->y){
		//这个分支单独拆出来是因为斜率为零，求0的倒数是错误
		//判断车的方向
		switch (this->m_nMoveDirection)
		{
		case 1:
			right_x = left_x = this->x;
			right_y = this->y + offset;
			left_y  = this->y - offset;
			break;
		case -1:
			right_x = left_x = this->x;
			right_y = this->y - offset;
			left_y  = this->y + offset;
			break;
		default:
			break;
		}
	}else{
		double k = (this->y - this->last_locate.y)/(this->x - this->last_locate.x);
		double b1 = this->y - k*this->x;
		double b2 = this->y - 1*this->x/k;

		double right_d = 0;
		double left_d  = 0;
		right_d = offset*sqrt(pow(k,2) + 1) + b2 - b1;
		left_d  = -1*offset*sqrt(pow(k,2) + 1) + b2 - b1;

		switch (this->m_nMoveDirection)
		{
		case 1:
			this->right_x = k*right_d/(pow(k,2) - 1);
			this->right_y = this->right_x*1/k + b2;

			this->left_x = k*left_d/(pow(k,2) - 1);
			this->left_y = this->left_x*1/k + b2;
			break;
		case -1:
			this->right_x = k*left_d/(pow(k,2) - 1);
			this->right_y = this->right_x*1/k + b2;

			this->left_x = k*right_d/(pow(k,2) - 1);
			this->left_y = this->left_x*1/k + b2;
			break;
		default:
			break;
		}
	}
}

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( int nval )  // 考勤
{
	if(this->pos_state == this->pos_state_old){
		if(this->pos_state_count < nval){
			this->pos_state_count++;
		}
		return false;
	}
	//if(this->pos_state_count < nval ){ // 未达到确认次数
	//	this->pos_state_count++;
	//	return false;
	//}
	this->pos_state_old = this->pos_state;
	this->pos_state_count = 1;
	return true;
	//if(last_area_type_id != cur_area_type_id){
	//	if(0 ==last_area_type_id || 0==cur_area_type_id){
	//		return true;
	//	}
	//}
	//return false;
	//return (this->last_area_type_id != this->cur_area_type_id && 0 == (this->cur_area_type_id & this->last_area_type_id));
}

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

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

	if(_dists.size() >= MAX_DIST_CACHE){ // 超过缓存数量限制
		// 计算并删除第一个
		get_coordinate();
		remove_dist_head();
	}

	if (this->x != this->last_x || this->y != this->last_y)
	{
		this->b_pos_change = true;
	}else{
		this->b_pos_change = false;
	}
	LeaveCriticalSection(&m_csCard);
}

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

	if(_dists.size() >= MAX_DIST_CACHE){ // 超过缓存数量限制
		// 计算并删除第一个
		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);
		this->b_pos_change = true;
	}else{
	    LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_139);
		this->b_pos_change = false;
	}
	LeaveCriticalSection(&m_csCard);
}

time_t Card::get_working_time()
{
	//return (card_type == CT_VEHICLE) ? leave_park_time : down_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)
{
	if(init_postion){
		for(int i = DIST_COUNT - 1;i >= 0;i--){
			if(p_dists_locate[i] != NULL){
				delete p_dists_locate[i];
				p_dists_locate[i] = NULL;
			}
		}
		if(p_dists_locate){
			delete[] p_dists_locate;
			p_dists_locate = NULL;
		}
	}

	_dists.clear();

	DeleteCriticalSection(&m_csCard);
}

double Card::get_speed()
{
	return fabs(v);
}

/*
 * 采用TOF或者TDOA算法进行定位计算
 *
 * param
 *		cnt		------	_dists数据条数
 *
 * return
 *		无返回值
 *
*/
void Card::get_coordinate( int cnt )
{
#ifdef ALGORITHM_TOF
	algo_tof(cnt);
#elif defined ALGORITHM_TYPE_TDOA
	//algo_tdoa(cnt);
	int ret = 0;
	ret = algo_tdoa_1d(cnt);
	//algo_tdoa_2d(cnt);
#else
	
#endif
	if (ret == 0)
	{
		inspect_coordinate(this->acce_cur_state);
	}	

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

void Card::get_coordinate()
{
	get_coordinate(get_effictive_dist_count());
}

/*
 * 根据状态值获得状态文本描述
 *
 * param
 *		无
 *
 * return
 *		状态文本描述
 *
*/
std::string Card::get_state_text()
{
	string ret = "";
	state = 0;
	state_biz = 0;

	if(status_help == STATUS_ERROR){
		state += STATUS_HELP;
		state_biz += STATUS_HELP;
		ret += "呼救,";
	}
	if(status_area_over_time == STATUS_ERROR){
		state += STATUS_AREA_OVER_TIME;
		state_biz += STATUS_AREA_OVER_TIME;
		ret += "区域超时,";
	}else if(status_area_over_time == STATUS_ERROR){
		state += STATUS_OVER_TIME;
		state_biz += STATUS_OVER_TIME;
		ret += "超时,";
	}
	if(status_area_over_speed == STATUS_ERROR){
		state += STATUS_AREA_OVER_SPEED;
		state_biz += STATUS_AREA_OVER_SPEED;
		ret += "区域超速,";
	}else if(status_over_speed == STATUS_ERROR){
		state += STATUS_OVER_SPEED;
		state_biz += STATUS_OVER_SPEED;
		ret += "超速,";
	}
	if(status_area_forbidden == STATUS_ERROR){
		state += STATUS_AREA_FORBIDDEN;
		state_biz += STATUS_AREA_FORBIDDEN;
		ret += "进入限制区域,";
	}
	if(status_call == STATUS_ERROR){
		state += STATUS_CALL;
		state_biz += STATUS_CALL;
		ret += "呼叫,";
	}
	if(status_lost == STATUS_ERROR){
		state += STATUS_LOST;
		state_biz += STATUS_LOST;
		ret += "进入盲区,";
	}

	if(power_state == STATUS_ERROR){
		state += STATUS_POWER_LOWER;
		ret += "电量低,";
	}else if(power_state == STATUS_ERROR_SERIOUS){
		state += STATUS_POWER_LOWER_SERIOUS;
		ret += "电量极低,";
	}

	if(ret.length() > 0){
		ret = ret.substr(0, ret.length() - 1);
	}else{
		ret = "正常";
	}

	return ret;
}

std::string Card::get_acc_text()
{
	string ret = "";
	state_moving = (accelerate_state & 0x01)? STATUS_ERROR : STATUS_NORMAL;

	if(state_moving == STATUS_NORMAL){
		ret += "静止";
	}else if(state_moving == STATUS_ERROR){
		ret += "运动";
	}
	return ret;
}

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 == NULL){
		m_pKalmanFilter = new CKalmanFilter();
		m_pKalmanFilter->Initial(0.2);
		m_pKalmanFilter->m_bFlag = false;
		}*/
		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::~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>();

	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)
{
	//std::vector<std::string> vec = split(sz_path, ",");
	//if(vec.size() >= 4){
	//	rect_left = atoi(vec[0].c_str());
	//	rect_top = atoi(vec[1].c_str());
	//	rect_right = atoi(vec[2].c_str());		
	//	rect_bottom = atoi(vec[3].c_str());
	//}else{
	//	rect_left = rect_right = rect_top = rect_bottom = 0;
	//}
	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){
		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;
	}
	//TRACE(_T("counter : %d \n"),counter);
	return (counter % 2 == 0) ? false : true;
}

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

int Card::KalmanFilterProcess(std::shared_ptr<POS>& pos)
{
	double kalman_detal_t = 0;
	double interval_time = 0.2;

	//存在分站在计数序号为1000掉电，但重新上电会导致此时的计算值偏大
	if (m_nCalcSyncNum - this->last_locate.sync_num < 0 && m_nCalcSyncNum < 100)
	{
		kalman_detal_t = (m_nCalcSyncNum + 65535 - this->last_locate.sync_num)*interval_time;
	}else{
		kalman_detal_t = (m_nCalcSyncNum - this->last_locate.sync_num)*interval_time;
	}
	/*if (CheckCrossCycle())
	{
		kalman_detal_t = (m_nCalcSyncNum + 65535 - this->last_locate.sync_num)*interval_time;
	} 
	else
	{
		kalman_detal_t = (m_nCalcSyncNum - this->last_locate.sync_num)*interval_time;
	}*/

	//如果定位失败，通过卡尔曼滤波处理
	if(pos->posx == INVALID_COORDINATE && pos->posy == INVALID_COORDINATE){
		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){
			//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{
		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 (card_id == "0020000001047")
		{
			CString tmp = _T("");
			tmp.Format(_T("x = %.4f , y = %.4f , cvx = %.4f ,cvy = %.4f ,dt = %.4f \r\n"),pos->posx,pos->posy,pos->cvx,pos->cvy,pos->diff_reader_sync_num);
			TRACE(tmp);
		}*/

		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) > 1E-5){
				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)
	{
		//如果经过卡尔曼处理之后
		if(pos->nFirstReader == 0 && pos->nSecondReader == 0){
			pos->nFirstReader = last_s_locate_reader[0];
			pos->nSecondReader = last_s_locate_reader[1];
		}

		//二维定位不需要如下内容
		//增加地图集的判定，判断定位结果是否在地图集上
		//如果不在地图集上，需要再次定位
		//需要带出定位结果的分站信息，
		//利用地图集中分站信息再次定位
		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::Pos(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::InitAlgoParam()
{
	m_nLastLocateT = 0;

	last_s_locate_reader[0] = last_s_locate_reader[1] = 0;	

	ins_weight = 1;	//加速度计状态权重
	uwb_weight = 9;	//uwb状态权重

	acce_cur_state  = 0;		//在算法中保存当前加速度状态，保存这两个值的原因是在多线程情况下accelerate_state和accelerate_state_last状态可能变化
	acce_last_state = 0;	//在算法中保存上一次加速度状态

	is_first_location = true;

	if (vt_his_speed.size() > 0)
	{
		vt_his_speed.resize(0);
	}

	return 0;
}

bool Card::CheckCrossCycle()
{
	//time_stamp_last特指卡的计数序号，非分站的
	//当卡需要新定位的计数序列号小于上一次成功定位的序列号，则此次不定位，避免定位结果的回退
	if (this->time_stamp_last != 0 && this->time_stamp_last > this->time_stamp_cal)
	{
		string str_last = "";
		string str_cur  = "";

		str_last = CFunctions::int2string(this->time_stamp_last);
		str_cur  = CFunctions::int2string(this->time_stamp_cal);

		if (str_last.length() == 5 && str_cur.length() <= 2)
		{
			//满足此条件，表示标示卡的计数序号走完了65535的周期
			return true;
		}
		else{
			return false;
		}
	}

	return true;
}

/*
 * 组装拟合数据，主要是前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()
{
	/*if (cur_ref_totals != REF_POSITION_NUM - 1)
	{
		if (Fitting::VAR(fitting_v) < 25)
		{
			double fitting_coe_x[REF_POSITION_NUM] = {0};
			double fitting_coe_y[REF_POSITION_NUM] = {0};

			Fitting::EMatrix(fitting_ct,fitting_x,REF_POSITION_NUM,3,fitting_coe_x);
			Fitting::EMatrix(fitting_ct,fitting_y,REF_POSITION_NUM,3,fitting_coe_y);
			for (size_t i=0;i<FIT_POSITION_NUM;i++)
			{
				fit_new_x[i] = Fitting::polyval(fitting_coe_x,fitting_ct[cur_ref_totals] + i);
				fit_new_y[i] = Fitting::polyval(fitting_coe_y,fitting_ct[cur_ref_totals] + i);
			}
		}
	}*/

	//如果滤波失败或者其他条件失败，
	//则通过直线回归算法拟合出三个解
	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 = 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;
	if (have_fit_pos)
	{
		TRACE(_T("have fit pos \r\n"));
	}

	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;
	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);

		if (i>=2)
		{
			diff_x += abs(long_fit_new_x[i-1] - long_fit_new_x[i-2]);
			diff_y += abs(long_fit_new_y[i-1] - long_fit_new_y[i-2]);
		}
	}

	double diff = 0.0;
	int interval = 0;
	double d_val = 0.0;
	if (diff_x < 1 && diff_y > 10)
	{
		diff = 0.0;
		diff += abs(long_fit_new_y[FIT_POSITION_NUM - 1] - long_fit_new_y[0]);
		interval = nums - FIT_POSITION_NUM;
		d_val = diff / interval;

		for (int i = FIT_POSITION_NUM;i<nums;i++)
		{
			long_fit_new_y[i] = long_fit_new_y[FIT_POSITION_NUM - 1] + (i - FIT_POSITION_NUM + 1)*d_val;
		}
	}else if(diff_x > 1 && diff_y < 1){
		diff = 0.0;
		diff += abs(long_fit_new_x[FIT_POSITION_NUM - 1] + long_fit_new_x[0]);
		interval = nums - FIT_POSITION_NUM;
		d_val = diff / interval;

		for (int i = FIT_POSITION_NUM;i<nums;i++)
		{
			long_fit_new_x[i] = long_fit_new_x[FIT_POSITION_NUM - 1] + (i - FIT_POSITION_NUM + 1)*d_val;
		}
	}

	have_long_fit_pos = true;

	return 0;
}

std::shared_ptr<POS> Card::GetPosFromFittingData()
{
	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;
		}	
	}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;
	}

	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();

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

		if (CheckCrossCycle())
		{
			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->posx = last_locate.x * map_scale;
			pos->posy = last_locate.y * map_scale;
			pos->cx = last_locate.x;
			pos->cy = last_locate.y;
			pos->cvx = last_vx;
			pos->cvy = last_vy;
		}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;
}

int Card::CheckSolutionByFit(int ret,std::shared_ptr<POS>& pos)
{
	//拟合预测解
	is_ref_pos = true;
	std::shared_ptr<POS> fit_pos = std::make_shared<POS>();
	fit_pos->diff_reader_sync_num = pos->diff_reader_sync_num;
	int fit_ret = 0;
	//fit_ret = CalcFittingData(ret,fit_pos);
	if (cur_fit_nums<3)
	{
		fit_ret = CalcFittingData(ret,fit_pos);
	} 
	else
	{
		fit_ret = CalcLongFittingData(ret,fit_pos);
	}
	
	//存在如下情况，解是个错误解
	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 > 1)
			{
				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)
{
	//double speed = sqrt(pow(pos->cvx,2) + pow(pos->cvy,2));
	if (m_nLastLocateT != 0)
	{
		//如果第一次定位不做速度的判别
		//double speed = sqrt(pow(pos->cx - last_locate.x,2) + pow(pos->cy - last_locate.y,2));
		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;
}

int Card::OutputCmdLog(int n)
{
	if (card_id != "0020000001047")
	{
		return 0;
	}
	CString strOutput = _T("");
	CString tmp = _T("");
	tmp = this->card_id.c_str();
	strOutput += tmp;
	tmp =_T("");
	tmp.Format(_T(",sync num:%d"),this->m_nLastLocateT);
	strOutput += tmp;
	tmp =_T("");
	tmp.Format(_T(",card num:%d"),this->time_stamp_cal);
	strOutput += tmp;
	tmp =_T("");
	tmp.Format(_T(",sync list size :%d \r\n"),m_syncNumList.size());
	strOutput += tmp;
	/*int i = 0;
	for (std::list<sync_data>::iterator it = m_syncNumList.begin();it!= m_syncNumList.end();++it)
	{
		i++;
		tmp = _T("");
		tmp.Format(_T("%d:%d  "),i,it->sync_num);
		strOutput += tmp;
	}*/
	TRACE(strOutput);
	tmp = _T("");
	tmp.Format(_T("exit %d \r\n"),n);
	TRACE(tmp);

	return 0;
}

void Card::set_speed( double v )
{

}

// 检查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);
		
		if (mp_dists_locate.size() > 0)
		{
			m_nCalcSyncNum = mp_dists_locate.begin()->second->sync_num;
		}

		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();
	
	// 筛选掉线性插值异常的数据
	for(DistMap::iterator it = _dists.front().distmap.begin(); it != _dists.front().distmap.end(); ++it){
		//如果同步时间戳存在异常值，则不走算法定位，直接返回上一次结果值
		if(LLONG_MAX == it->second->tt){
			LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_144);
		}else
		{
			mp_dists_locate_ex.insert(make_pair(it->second->tt,it->second));
		}
	}

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

	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;

	return 0;
}

int Card::SaveCardAlgoData(std::shared_ptr<POS>& pos)
{
	this->x = pos->cx;
	this->y = pos->cy;
	this->z = pos->cz;

	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;
		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))
		{
			nStream = DOWN_STREAM;
		} 
		else
		{
			nStream = UP_STREAM;
		}
	}

	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;
	this->last_locate.x = this->x = pos->cx;
	this->last_locate.y = this->y = pos->cy;
	this->last_locate.z = this->z = 0;
	this->last_locate.sync_num = this->m_nCalcSyncNum;

	this->last_locate.acceleration = pos->av;
	this->last_vx = pos->cvx;
	this->last_vy = pos->cvy;

	last_s_locate_reader[0] = pos->nFirstReader;
	last_s_locate_reader[1] = pos->nSecondReader;
	
	int nlast = this->m_nLastLocateT;
	this->m_pKalmanFilter->m_pCar->t = this->m_nLastLocateT = this->m_nCalcSyncNum;

	//速度的计算采用求平均的方式
	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 (this->acce_cur_state == STATE_ACCE_STATIC)
	{
		this->v = 0;
	} 
	else
	{
		this->v = av*nSign;
	}

	if (cur_fit_nums == FIT_POSITION_NUM*4)
	{
		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);
	}

	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 (his_pos.size() == REF_POSITION_NUM)
		{
			his_pos.pop_front();
		}
		his_pos.push_back(p);
		if (his_pos.size() == REF_POSITION_NUM)
		{
			if (!have_long_fit_pos)
			{
				CalcFittingData();
				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;
}

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(this->m_nLastLocateT == 0){
		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 = 0.2;
		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)
			//if(!CheckCrossCycle())
			{
				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 = (cx - sd.x)*this->map_scale/deltaT;
			cvy = (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;
					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 CHECK_PERSON_ACCE_OVER_SPEED;
				}
				break;
			case CT_VEHICLE:
				if(av > VECHILE_ACCELERATE_THRESHOLD){
					//保留上次结果
					p->reason = CHECK_VEHICLE_ACCE_OVER_SPEED;
					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 CHECK_VEHICLE_ACCE_OVER_SPEED;
				}
				break;
			}	
		}else{
			//从队列尾部开始查找，找到第一个同步序号与当前计算卡的同步序号相差5个以上的数据
			list<sync_data>::reverse_iterator it;
			bool bOverflow = false;
			//sync_data sd;
			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 (CheckCrossCycle())
					{
						if(m_nCalcSyncNum + 65536 - it->sync_num >=5 ){//5
							bOverflow = true;
							bfind = true;
							sd = *it;
						}
					}else{
						continue;
					}
					//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

			//这里不能对前5次定位的数据做加速度处理，不然会出现抛不掉的
			//需要处理，那么第一次定位的成功很重要
			//处理前5次的
			/*if(!bfind){
				sd = *(m_syncNumList.begin());
			}*/
			//根据溢出条件来计算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 = (cx - sd.x)*this->map_scale/deltaT;
			cvy = (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;
					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 CHECK_PERSON_ACCE_OVER_SPEED;
				}
				break;
			case CT_VEHICLE:
				if(av > VECHILE_ACCELERATE_THRESHOLD){
					//保留上次结果
					p->reason = CHECK_VEHICLE_ACCE_OVER_SPEED;
					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 CHECK_VEHICLE_ACCE_OVER_SPEED;
				}
				break;
			}
	
			cv = cv*3.6;
			//速度的限制
			if(fabs(cv) > MAX_VECHILE_SPEED){
				p->reason = CHECK_VEHICLE_OVER_SPEED;
				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 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;
}

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

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;

	return 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;
			}
		}

		double dist = 0.0;
		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;
		}
		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;
		}

		/*if (CheckSulutionByStream(udm_pos.at(0)))
		{
			return SOLUTION_ERROR_STREAM;
		}*/

		//*pos = *udm_pos.at(0);
		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);

		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;

			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 = (*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 < 10)
			{
				//找到即退出
				//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;
}

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

MapInfo::MapInfo( void )
{

}

MapInfo::~MapInfo( void )
{

}

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

Dept::Dept()
{
}

Dept::~Dept()
{
}

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()
{
}