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 "./mylog/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;
	is_driving_face_start = 0;
	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;
	pos_state_count = 0;
	pos_state_confirm_times = 1;
	dept_id = group_id = occlevel_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;
	is_fit_pos = false;
	cur_fit_nums = 0;
	have_fit_pos = false;
	have_long_fit_pos = false;
	count_idle = 0;
	count_change_direction = 0;
	is_over_interval = false;

	bInitalCellPath = false;
	curCellReaderName = "";
	curCellId = 0;
	originCellId = 0;
	nIncrease = 0;
	change_cell_dir = 0;
	nStartLocateCounts = 0;

	call_type = 0;
	vt_deal_call_reader_id.resize(0);
	recv_call_time = time(NULL);

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

<<<<<<< HEAD
	isIdling = false;
	data_cell_time = 0;
	calc_cell_time = 0;
	last_cell = nullptr;

	::GetLocalTime(&cellDealTime);

	//hEvent = CreateEvent(NULL,TRUE,FALSE,NULL);
	//ResetEvent(hEvent);
=======
	drive_face_location_write_time = 0;
>>>>>>> newprotocal
}

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;

	//while (_dists.size()>0)
	//{
	//	_dists.pop_front();
	//}

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

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

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

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

	cur_fit_nums = 0;
	count_idle = 0;

	locate = nullptr;

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

	//清除历史格子列表
	if (his_cell.size() > 0)
	{
		his_cell.erase(his_cell.begin(),his_cell.end());
	}
}

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

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

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

	if(this->pos_state == this->p_reader->pos_state){
		if(this->pos_state_count <= pos_state_confirm_times)
		{
			this->pos_state_count++;
		}
	}
	else // 井上井下位置变化
	{
		this->pos_state = p_reader->pos_state;
		this->pos_state_count = 0;
	}
}

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

bool Card::checkStream(double x1,double y1,double x2,double y2,int nStream)
{
	//方向一致，返回true，否则返回false
	//去除采集刚启动，last_locate的坐标为零而导致判断车辆上下行方向错误的问题
	//车辆上下行确定
	//3个条件：起点（x1,y1）,终点（x2,y2）
	//1.x1==x2的情况下，y2>y1为下行
	//2.y1==y2的情况下，x1>x2为下行
	//3.x2>x1且y2>y1为下行
	//其他情况为上行
	int stream = 0;
	if (((x1 == x2 || abs(x1 - x2) < 1E-2) && y1 < y2 )
		||((y1 == y2 || abs(y1 - y2) < 1E-2) && x1 > x2)
		||(x1 < x2 && y1 < y2))
	{
		stream = DOWN_STREAM;
	} 
	else
	{
		stream = UP_STREAM;
	}

	return nStream == stream;
}

/*
* 设置地图集覆盖范围，适用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//人卡02
	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;
		}else{
			//如果运动状态时，
			int nSign = 0;
			int nState = 0;

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

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

/*
 * 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;
	unsigned short dst = 0;
	unsigned short 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 = (st - (unsigned short)it->second->sync_num);
			if ((st > (unsigned short)it->second->sync_num && dst > MAX_SYNCTIME_DIFF_NUM) ||
				(st < (unsigned short)it->second->sync_num && dst < MAX_SYNCTIME_DIFF_NUM_CROSS_PERIOD))
			{
				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; // 存在一个异常值就退出算法
				// continue; // 将异常值过滤掉，不参与算法，其他正常值照常参与计算
			}
			//保存信息用于定位
			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(pRdm.size() < 2)
	// {
	// 	bRet = true; // 筛选后，数据不满足定位要求，退出算法
	// }

	//存在异常值，直接返回上一次结果值
	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特指卡的计数序号，非分站的
	//当卡需要新定位的计数序列号小于上一次成功定位的序列号，则此次不定位，避免定位结果的回退
	
	// 本次序号小于上次序号，存在如下情况
	// 1、跨周期，如本次0，上次为65535，另外进入盲区也可能跨周期后回来
	// 2、卡重置，从0开始
	// 3、序号较连续，新数据先到，旧数据后到，会造成跳动

	if (this->time_stamp_last != 0 && this->time_stamp_last > this->time_stamp_cal)
	{
		if((unsigned short)this->time_stamp_last - (unsigned short)this->time_stamp_cal < MAX_SYNCTIME_DELAY_NUM)
		{
			// 小于5，认为会影响数据
			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::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);

		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);
					// 定位结果在地图集外，映射到地图集上
					std::shared_ptr<POS> cp = LocateAlgorithm::MappingToPath(kalman_p,pTdoaReaderPathMap);

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

					cp.reset();
				}		

				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))
					{
						this->m_nStream = DOWN_STREAM;
					} 
					else
					{
						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;

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

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

	//保证每个ct都输出一个坐标:
	//特殊情况一：单条记录也输出

	std::shared_ptr<POS> pos = std::make_shared<POS>();
	//3.算法定位	
	std::vector<std::shared_ptr<POS>> udm_pos;
	udm_pos.resize(0);
	//time_stamp_cal == 46163	
	//time_stamp_cal == 18639
	ret = LocateAlgorithm::CalcTdoaPosition(pRdm,pTdoaReaderPathMap,udm_pos);
	//4.从多解中筛选出一个解,
	//存在两种可能：
	//a.可能无解返回非0，
	//b.可能有解，但解不正确，比如解的位置在4727,-100,但选出的解是4727，-200
	ret = ChooseOneSolution(pRdm, udm_pos, pos);

	if (ret == 0)
	{	
		std::shared_ptr<POS> tmp_pos = std::make_shared<POS>();
		
		*tmp_pos = *pos;
		if (ret == 0)
		{
			//5.如果有解，则对唯一解做合法性验证
			//主要验证条件为：加速度和速度
			ret = CheckSolution(tmp_pos);	
		}else{
			tmp_pos->posx = INVALID_COORDINATE;
			tmp_pos->posy = INVALID_COORDINATE;
			tmp_pos->cx   = INVALID_COORDINATE / (map_scale*1.0);
			tmp_pos->cy   = INVALID_COORDINATE / (map_scale*1.0);
		}
		//无论正确与否，保存原始值
		SaveOriginDataBeforeFilter(tmp_pos);

		//6.卡尔曼滤波,两种情况：
		//a.解通过了合法性验证，
		//b.无解,但在2s内
		//则使用卡尔曼滤波进行滤波处理
		if (FILTER_KALMAN == m_nFilterType)
		{
			ret = KalmanFilterProcess(tmp_pos);
		}
		if (tmp_pos->posx != INVALID_COORDINATE && tmp_pos->posy != INVALID_COORDINATE)
		{
			*pos = *tmp_pos;
		}else{
			if (ret==0)
			{
				//解存在，解可能异常
				//判断是否回退，至少第三次定位才走这，回退了使用上一次的
				TRACE(_T("no sol x: %f,y%f\r\n"),pos->posx,pos->posy);
			}else{
				//解异常
			}
		}
	}

	bool bRet = false;

	//是否在地图集上
	bRet = LocateAlgorithm::IsOnMap(pos,pTdoaReaderPathMap);
	if (bRet)
	{
		//检查是否回退
		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 (pRdm->size() >2)
	{
		m_nCalcSyncNum = max_sync_time;
	}
	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()  // 考勤
{
	bool ret = false;

	if(pos_state != pos_state_old)
	{
		if(pos_state_count >= pos_state_confirm_times)
		{
			pos_state_old = pos_state;
			ret = true;
		}
	}

	return ret ;
	//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.size()的大小始终为1，错误原因可能是卡在数据库中不存在
	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);

	//车辆定位20170705
	if(this->card_type == CT_VEHICLE)
	{
		ret = DiscreteLocate(cnt);//数据正常20170705
	}
	
	else if(this->card_type == CT_PERSON)
	{
		ret = PersonLocation(cnt);
	}

	//algo_tdoa_2d(cnt);
#else
	
#endif
	//0代表成功写了格子，尚未入库
	if (ret == 0)
	{
		//写日记flag
		isNeedWrited = true;
		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;
	}

	if (isNeedWrited || nStartLocateCounts<6)
	{
		std::string flag1 = is_fit_pos?"Y":"N";
		int nSource1 = 0;
		if (last_cell)
		{
			nSource1 = last_cell->dataSource;
		}		
		char chLog1[300] = {0};
		sprintf_s(chLog1,"cardid: %s, sync_num: %d, ct: %d, ox: %f, oy: %f, x: %f,y: %f, readerName: %s, cellId : %d, originId: %d, oReaderName: %s, ov: %f, cv: %f, fit: %s, source: %d",
			card_id.c_str(),
			m_nCalcSyncNum,
			time_stamp_cal,
			origin_locate.x,
			origin_locate.y,
			x, 
			y,
			curCellReaderName.c_str(),
			curCellId,
			originCellId,
			originCellReaderName.c_str(),
			origin_locate.v,
			get_speed(),
			flag1.c_str(),
			nSource1);
			
		std::string strLog1 = "";
		strLog1 = chLog1;
		Log::write_log(FILE_TYPE::KALMAN_S,strLog1,true);
		
		isNeedWrited = false;
	}
}

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_MOVING_MOTION : STATUS_MOVING_MOTIONLESS;

	if(state_moving == STATUS_MOVING_MOTIONLESS){
		ret += "静止";
	}else if(state_moving == STATUS_MOVING_MOTION){
		if(CT_VEHICLE == card_type && 0 == v)
		{
			state_moving = STATUS_MOVING_IDLING;
			ret += "怠速";
		}		
		else
		{
			ret += "运动";
		}
	}else if(state_moving == STATUS_MOVING_IDLING)
	{
		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>();
	area_card_list_over_speed = std::make_shared<CardMap>();

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

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

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

	std::vector<std::string> vec = split(sz_path, " ");
	std::vector<std::string>::iterator it = vec.begin();
	if(polygon){
		delete[] polygon;
		polygon = NULL;
	}
	polygon = new _point[vec.size()];
	polygon_count = 0;
	for(; it != vec.end(); ++it){
		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 Area::is_special()
{
	return (area_type_id == AREA_TYPE_NO_COVER) ? 1: 0;
}

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)
{
	if (abs(pos->posx) < 1E-4 && abs(pos->posy) < 1E-4)
	{
		return 1;
	}

	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(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;
			pos->cx = INVALID_COORDINATE / map_scale;
			pos->cy = INVALID_COORDINATE / map_scale;
			pos->posx = INVALID_COORDINATE;
			pos->posy = INVALID_COORDINATE;
			pos->reason = KALMAN_FILTER_LONG_INTERVAL;
			//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(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){
			//两次处理
			if (abs(pos->diff_reader_sync_num) > 1E-5)
			{
				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);

		//如何解决卡的ct为2000时，卡重启，从0开始计数的问题？
		if (str_last.length() >= 3 && str_cur.length() <= 2)
		{
			//满足此条件，表示标示卡的计数序号走完了65535的周期
			return true;
		}
		else{
			return false;
		}
	}

	return true;
}

/*
 * 判断是否回跳:依据上下行来判断
 *
 * return
 *		如果回跳，则返回true，否则返回false
*/
bool Card::isRebound(std::shared_ptr<POS> pos)
{
	if (m_nStream == 0)
	{
		return false;
	}
	//车辆上下行确定
	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 == pos->cx && pos->cy > this->last_locate.y)
			||(this->last_locate.x > pos->cx && pos->cy == this->last_locate.y)
			||(this->last_locate.x > pos->cx && pos->cy > this->last_locate.y))
		{
			nStream = DOWN_STREAM;
		} 
		else
		{
			nStream = UP_STREAM;
		}
		if (nStream != this->m_nStream)
		{
			return true;
		}
	}

	return false;
}

bool Card::isRebound(std::shared_ptr<Cell> cell)
{
	int nCurIncrease = 0;

	if (last_cell->id > cell->id)
	{
		nCurIncrease = -1;
	}else if(last_cell->id < cell->id){
		nCurIncrease = 1;
	}else{
		nCurIncrease = 0;
	}

	if (nCurIncrease*this->nIncrease < 0)
	{
		//存在反向
		return true;
	}

	return false;
}

/*
 * 组装拟合数据，主要是前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;

	return 0;
}

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

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

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

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

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

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

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

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

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

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

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

	return 0;
}

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

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

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

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

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

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

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

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

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

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

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

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

	return 0;
}

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

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

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

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

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

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

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

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

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

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

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

	have_long_fit_pos = true;

	return 0;
}

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

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

	return p;
}

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

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

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

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

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

int Card::CheckSolutionByFit(int ret,std::shared_ptr<POS>& pos)
{
	//拟合预测解
	is_ref_pos = true;
	//std::shared_ptr<POS> fit_pos = std::make_shared<POS>();
	std::shared_ptr<POS> fit_pos = GetPosFromFittingData();
	if (fit_pos == nullptr)
	{
		return 1;
	}
	fit_pos->diff_reader_sync_num = pos->diff_reader_sync_num;
	int fit_ret = fit_pos->reason;
	/*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 > 10)
			{
				//如果定位出的解和拟合解的距离差大于10，则认为拟合解可信
				pos->posx = fit_pos->posx;
				pos->posy = fit_pos->posy;
				pos->cx   = fit_pos->cx;
				pos->cy   = fit_pos->cy;
				pos->cvx  = fit_pos->cvx;
				pos->cvy  = fit_pos->cvy;
				pos->is_fit = fit_pos->is_fit;
				//如果是拟合出来的点就不能作为参考点
				is_ref_pos = !pos->is_fit;
			}
		}
		if (fit_ret && ret == 0)
		{
			//表示没有拟合值，但解是可信的，则直接返回0
			return 0;
		}
	}

	return fit_ret;
}

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

	//double speed = sqrt(pow(pos->cvx,2) + pow(pos->cvy,2));
	if (m_nLastLocateT != 0)
	{
		//如果第一次定位不做速度的判别
		//double speed = sqrt(pow(pos->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;
}

//根据点的信息获得格子信息
std::shared_ptr<Cell> Card::Position2Cell(std::shared_ptr<POS> pos)
{
	if (mpCellPath == nullptr)
	{
		return nullptr;
	}

	std::shared_ptr<Cell> cell = nullptr;

	//获得格子列表
	std::string name1 = CFunctions::getCellName(pos->nFirstReader,pos->nSecondReader);
	std::string name2 = CFunctions::getCellName(pos->nSecondReader,pos->nFirstReader);

	int nIdx = 0;
	CellPathMap::iterator it = mpCellPath->find(name1);
	if (it == mpCellPath->end())
	{
		it = mpCellPath->find(name2);	
		if (it == mpCellPath->end())
		{
			return nullptr;
		}else{
			nIdx = 2;
		}
	}else{
		nIdx = 1;
	}

	//其他方式，判断点距离某个分站的距离，使用距离除以间隔即可

	std::list<std::shared_ptr<Cell>>::iterator it_cell = it->second.end();
	for (it_cell = it->second.begin();it_cell != it->second.end();++it_cell)
	{
		//获得cell，判断点是否在cell内部
		point p;
		p.x = pos->posx;
		p.y = pos->posy;
		
		if (LocateAlgorithm::PointIsInRect(p,(*it_cell)->top,(*it_cell)->left,(*it_cell)->bottom,(*it_cell)->right))
		{
			cell = (*it_cell);
			switch (nIdx)
			{
			case 1:
				cell->strSubjectReader = name1;
				break;
			case 2:
				cell->strSubjectReader = name2;
				break;
			default:
				break;
			}
			break;
		}
	}

	if (cell)
	{
		if (cell->strSubjectReader == "")
		{
			return nullptr;
		}
	}else{
		return nullptr;
	}

	return cell;
}

//把中心点的坐标赋值给pos
std::shared_ptr<POS> Card::Cell2Position(std::shared_ptr<Cell>& cell)
{
	if (cell == nullptr)
	{
		return nullptr;
	}

	std::shared_ptr<POS> pos = std::make_shared<POS>();
	//把cell的中心点的坐标给cell

	pos->posx = (cell->top.x + cell->bottom.x)/2;
	pos->posy = (cell->top.y + cell->bottom.y)/2;
	pos->cx = pos->posx/map_scale;
	pos->cy = pos->posy/map_scale;
	pos->update = true;
	pos->is_fit = cell->isFit;

	return pos;
}
int Card::PersonLocation(int cnt)

{
	int ret = 0;
	std::shared_ptr<Cell> cell = nullptr;
	std::shared_ptr<Cell> tmp_cell = std::make_shared<Cell>();
	is_fit_pos = false;
	TRACE(_T("+ ct : %d \r\n"),time_stamp_cal);
	//1.数据有效性判断
	ret = CheckDistData(cnt);
	if(ret){
		//time_stamp_cal== 14931 14878
		originCellId = 0;
		originCellReaderName = "";
		return 1;
	}else{
		//2.组装数据
		std::shared_ptr<ReceiveDataMap> pRdm = std::make_shared<ReceiveDataMap>();
		pRdm->clear();
		ret = AssembleDistData(pRdm);
		if(ret){
			return 1;
		}else{
			if (pRdm->size()<=0)
			{
				originCellId = 0;
				originCellReaderName = "";
				return 1;
			}

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

			//time_stamp_cal==12298
			std::shared_ptr<POS> pos = std::make_shared<POS>();
			//3.算法定位	
			std::vector<std::shared_ptr<POS>> udm_pos;
			udm_pos.resize(0);
			ret = LocateAlgorithm::CalcTdoaPosition(pRdm,pTdoaReaderPathMap,udm_pos);
			//4.从多解中筛选出一个解,存在两种可能：
			//a.可能无解返回非0，
			//b.可能有解，但解不正确，比如解的位置在4727,-100,但选出的解是4727，-200
			ret = ChooseOneSolution(pRdm, udm_pos, pos);

			SaveOriginDataBeforeFilter(pos);
			if (ret)
			{
				//根据上一次数据计算格子数，并根据上一次方向进行偏移
				originCellId = 0;
				originCellReaderName = "";
				return 1;
			}else{
				std::shared_ptr<POS> tmp_pos = std::make_shared<POS>();
				//获取分站信息
				bool bRet = false;
				bRet = LocateAlgorithm::IsOnMap(pos,pTdoaReaderPathMap);
				if (!bRet)
				{
					return 1;
				}
				*tmp_pos = *pos;

				if (ret == 0)
				{	

					//人卡数据处理20170705
					//time_stamp_cal== 2635
					cell = Position2Cell(tmp_pos);//哈哈哈哈哈哈哈哈哈
					if (cell && cell->id != 0)
					{
						if(cell->id == 184)
						{
							TRACE(_T("cell->id == 184"));
						
						}
						originCellId = cell->id;
						originCellReaderName = cell->strSubjectReader;
						cell->card_stamp_time = time_stamp_cal;
						::GetLocalTime(&cell->deal_time);
						//保存格子到单元格，用于计算怠速判断
						if (his_cell.size() > 100)
						{
							his_cell.pop_front();
						}
						his_cell.push_back(cell);//his_cell保存车辆怠速

						if (nStartLocateCounts>=1&&his_cell.size() > 1)
						{
							//检查是否超限
							isOverThreshold();//检查大跳
						}

						*tmp_cell = *cell;
						nStartLocateCounts++;//统计原始定位次数 
					}else{
						originCellId = 0;
						originCellReaderName = "";
					}

					if (!CheckStartRule())//检查启动规则
					{
						return 1;
					}
					
					if (last_cell && cell)//cell是本次定位  last_cell 是下次定位
					{
						//20170704只判断怠速
						int nCounts = 0;
						if (cell->strSubjectReader == last_cell->strSubjectReader)
						{
							nCounts = ceil(cell->id - last_cell->id);//cell->id - last_cell->id两次格子的步进 ceil向上取整
						}
						else{
							double distance = sqrt(pow(tmp_pos->cx - this->x,2) + pow(tmp_pos->cy - this->y,2)) * map_scale;
							nCounts = distance / CELL_WIDTH;
						}
						//允许调整按ct号的最大格子数
						int diff_ct = abs(cell->card_stamp_time - last_cell->card_stamp_time);
						int thre_value ;
						if(CT_VEHICLE == this->card_type )
						{
						 thre_value = MAX_CELL_FORWARD_JUMP;//各自固定长度
						}
						else if(CT_PERSON == this->card_type )
						{
							 thre_value = 3;//各自固定长度
						}
						if (diff_ct > 0)
						{
							if(CT_VEHICLE == this->card_type)
							{
							thre_value = MAX_CELL_FORWARD_JUMP*diff_ct;
							}
							else if(CT_PERSON == this->card_type )
							{
								thre_value = 3*diff_ct;
							}
						}

						//怠速保留 0704实现
						//越界，向前大跳或向后大跳//人卡01
						//有对应，越界（）
						if (nCounts >= thre_value)//步进大于阈值 //两次成功定位的数据处理好后竟然大于阈值 //向前/后大跳
						{
							isIdling = false;
							if (this->nIncrease != 0)//方向  =0是没有方向
							{
								int nCalcStep = 0;
								if (last_cell->nStep < 1)//上个格子距离上上个格子的步进
								{
									nCalcStep = 1;
								}else{
									nCalcStep = last_cell->nStep;//匀速  ----大跳按匀速处理
								}
								cell = GetNextCell(last_cell,this->nIncrease,nCalcStep);//本次的格子
								if (cell)
								{
									cell->isFit = true;
								}
							}	
						}else if (!CheckIdleStatus())//检查是否怠速
						{
							isIdling = false;
							//非怠速
						}else{
							//怠速
							isIdling = true;
							acce_cur_state = STATE_ACCE_IDLING;
							//如果怠速了，则直接取上一次的结果
							if (last_cell)
							{
								cell = last_cell;
							}
						}

						//1.越界：定位id相对于上一个实际id超过15格以上，直接根据上一个v来设置实际id
						//2.加速：定位id速度高于上一个实际的id的v，采取lv*a+cv*b
						//3.减速：定位id速度高于上一个实际的id的v，采取lv*a+cv*b
						//4.匀速：匀速前进
						//5.回跳：定位id显示回跳，5次以内，保持匀速前进或减速前进，5次掉头开始掉头
						//6.怠速：
						//7.静止：

						//越界
						//double distance = sqrt(pow(tmp_pos->cx - this->x,2) + pow(tmp_pos->cy - this->y,2)) * map_scale;
						//int nCounts = ceil(distance / CELL_WIDTH);
						//
						////允许调整按ct号的最大格子数
						//int diff_ct = abs(cell->card_stamp_time - last_cell->card_stamp_time);
						//int thre_value = MAX_CELL_FORWARD_JUMP;
						//if (diff_ct > 0)
						//{
						//	thre_value = MAX_CELL_FORWARD_JUMP*diff_ct;
						//}

						////怠速保留 0704实现
						////越界，向前大跳或向后大跳
						//if (nCounts >= thre_value)
						//{
						//	 if (isRebound(tmp_pos))
						//	 {
						//		 //如果回退，则是向后大跳
						//		 //如果5次以内回退
						//		 if (change_cell_dir < MAX_CHANGE_DIR_COUNTS)
						//		 {
						//			 int calcStep = 0;

						//			 if (last_cell)
						//			 {
						//				 //在不知下一时刻如何走的情况下，不做减速，
						//				 nCounts = last_cell->nStep;
						//				 //20170703 17:03 根据本次定位格子id和上一次格子id的格子差值取半减速
						//				 //calcStep = abs((originCellId - last_cell->id)/2);
						//			 }
						//			 if (abs(calcStep) < 1E-2)
						//			 {
						//				 calcStep = 1;
						//			 }
						//			 if (this->nIncrease != 0)
						//			 {
						//				 cell = GetNextCell(last_cell,this->nIncrease,calcStep);
						//				 if (cell)
						//				 {
						//					 cell->isFit = true;
						//				 }
						//			 }	
						//			 change_cell_dir++;
						//		 }else{
						//			 change_cell_dir = 0;
						//		 }
						//	 } 
						//	 else
						//	 {
						//		 //否则是向前大跳
						//		 //按上一次速度做匀速运动
						//		 if (last_cell)
						//		 {
						//			 nCounts = last_cell->nStep;
						//		 }
						//		 if (abs(nCounts) < 1E-2)
						//		 {
						//			 nCounts = 1;
						//		 }
						//		 if (this->nIncrease != 0)
						//		 {
						//			 cell = GetNextCell(last_cell,this->nIncrease,nCounts);
						//			 //cell = GetNextCell(last_cell,this->nIncrease,nCounts,originCellReaderName);
						//			 //cell = GetNextCell(last_cell,this->nIncrease,nCounts,tmp_cell);
						//			 if (cell)
						//			 {
						//				 cell->isFit = true;
						//			 }
						//		 } 
						//	 }
						//}else if (!CheckIdleStatus())
						//{
						//	//如果不是怠速
						//	//if (isRebound(tmp_pos))
						//	if (isRebound(cell))
						//	{
						//		//如果5次以内回退
						//		if (change_cell_dir < MAX_CHANGE_DIR_COUNTS)
						//		{
						//			int nCounts = 0;
						//			if (last_cell)
						//			{
						//				//在不知下一时刻如何走的情况下，不做减速，
						//				nCounts = last_cell->nStep;
						//				//nCounts = ceil(last_cell->nStep/2);	//20170703 16:46 做取半减速
						//			}
						//			if (abs(nCounts) < 1E-2)
						//			{
						//				nCounts = 1;
						//			}
						//			if (this->nIncrease != 0)
						//			{
						//				//cell = GetNextCell(last_cell,this->nIncrease,nCounts,originCellReaderName);
						//				cell = GetNextCell(last_cell,this->nIncrease,nCounts);
						//				if (cell)
						//				{
						//					cell->isFit = true;
						//				}
						//			}
						//			change_cell_dir++;
						//		}else{
						//			change_cell_dir = 0;
						//		}
						//	}else{
						//		//不回退，判断是否大跳
						//		//判断是否加速、减速、匀速
						//		//如果权值相同，以下三个可以使用同一逻辑处理
						//		//int calcStep = 0;

						//		//if (nCounts > last_cell->nStep)
						//		//{
						//		//	//加速,权值可调
						//		//	int step = ceil(THIS_V_POWER * last_cell->nStep + LAST_V_POWER * nCounts);

						//		//	//2017/07/02新增
						//		//	switch (this->nIncrease)
						//		//	{
						//		//	case 1:
						//		//		//如果步进后的格子id会超过原始定位格子id，
						//		//		//则取原始定位格子和上一次定位格子的差
						//		//		if (step + last_cell->id >= originCellId)
						//		//		{
						//		//			calcStep =  abs(last_cell->id - originCellId);
						//		//		} 
						//		//		else
						//		//		{
						//		//			calcStep = step;
						//		//		}
						//		//		break;
						//		//	case -1:
						//		//		if (last_cell->id - step >= originCellId)
						//		//		{
						//		//			calcStep = step;
						//		//		} 
						//		//		else
						//		//		{
						//		//			calcStep = abs(last_cell->id - originCellId);
						//		//		}
						//		//		break;
						//		//	}

						//		//	if (abs(calcStep) < 1E-2)
						//		//	{
						//		//		calcStep = 1;
						//		//	}
						//		//	if (this->nIncrease != 0)
						//		//	{
						//		//		cell = GetNextCell(last_cell,this->nIncrease,calcStep,tmp_cell);
						//		//		if (cell)
						//		//		{
						//		//			cell->isFit = true;
						//		//		}
						//		//	}
						//		//} 
						//		//else if (nCounts < last_cell->nStep)
						//		//{
						//		//	//减速,权值可调
						//		//	//采取向下取整，并考虑方向性
						//		//	int step = 0;

						//		//	if (last_cell)
						//		//	{
						//		//		step = originCellId - last_cell->id;
						//		//	}

						//		//	calcStep = ceil(THIS_V_POWER * last_cell->nStep + LAST_V_POWER * nCounts);

						//		//	if (abs(calcStep) < 1E-2)
						//		//	{
						//		//		calcStep = 1;
						//		//	}
						//		//	if (this->nIncrease != 0)
						//		//	{
						//		//		cell = GetNextCell(last_cell,this->nIncrease,calcStep,tmp_cell);
						//		//		if (cell)
						//		//		{
						//		//			cell->isFit = true;
						//		//		}
						//		//	}
						//		//}else
						//		//{
						//		//	if (last_cell->id > tmp_cell->id && this->nIncrease == -1)
						//		//	{
						//		//		calcStep = ceil(abs(last_cell->id - tmp_cell->id)/2);
						//		//	}
						//		//	//匀速运动,权值可调
						//		//	if (this->nIncrease != 0)
						//		//	{
						//		//		cell = GetNextCell(last_cell,this->nIncrease,calcStep,tmp_cell);
						//		//		if (cell)
						//		//		{
						//		//			cell->isFit = true;
						//		//		}
						//		//	}
						//		//}
						//	}
						//}else{
						//	acce_cur_state = STATE_ACCE_IDLING;
						//	//如果怠速了，则直接取上一次的结果
						//	if (last_cell)
						//	{
						//		cell = last_cell;
						//	}
						//}
					}
				}
			}
		}
	}
	
	if (cell)
	{
		SaveCardAlgoData(cell);
	}	

	return ret;
}
int Card::DiscreteLocate(int cnt)
{
	ULONGLONG nowTickCounts = ::GetTickCount();
	//如果last_cell存在，那说明不是第一次定位，如果不存在，那么继续往下走。
	if (last_cell)
	{
		SYSTEMTIME curSt;
		::GetLocalTime(&curSt);

		int ret = 0;
		ret = CFunctions::CompareSystemTime(curSt,last_cell->interval_time);
		if (ret < 0)
		{
			return 1;
		}
	}

	int ret = 0;
	std::shared_ptr<Cell> cell = nullptr;
	std::shared_ptr<Cell> tmp_cell = std::make_shared<Cell>();
	is_fit_pos = false;

	//1.数据有效性判断
	ret = CheckDistData(cnt);
	if(ret){
		//time_stamp_cal== 14931 14878
		originCellId = 0;
		originCellReaderName = "";
		return 1;
	}else{
		//2.组装数据
		std::shared_ptr<ReceiveDataMap> pRdm = std::make_shared<ReceiveDataMap>();
		pRdm->clear();
		ret = AssembleDistData(pRdm);
		if(ret){
			return 1;
		}else{
			if (pRdm->size()<=0)
			{
				originCellId = 0;
				originCellReaderName = "";
				return 1;
			}

			//time_stamp_cal==12298
			std::shared_ptr<POS> pos = std::make_shared<POS>();
			//3.算法定位	
			std::vector<std::shared_ptr<POS>> udm_pos;
			udm_pos.resize(0);
			ret = LocateAlgorithm::CalcTdoaPosition(pRdm,pTdoaReaderPathMap,udm_pos);
			//4.从多解中筛选出一个解,存在两种可能：
			//a.可能无解返回非0，
			//b.可能有解，但解不正确，比如解的位置在4727,-100,但选出的解是4727，-200
			ret = ChooseOneSolution(pRdm, udm_pos, pos);

			SaveOriginDataBeforeFilter(pos);
			if (ret)
			{
				//根据上一次数据计算格子数，并根据上一次方向进行偏移
				originCellId = 0;
				originCellReaderName = "";
				return 1;
			}else{
				std::shared_ptr<POS> tmp_pos = std::make_shared<POS>();
				//获取分站信息
				bool bRet = false;
				bRet = LocateAlgorithm::IsOnMap(pos,pTdoaReaderPathMap);
				if (!bRet)
				{
					return 1;
				}
				*tmp_pos = *pos;

				if (ret == 0)
				{	
					//车辆数据处理20170705
					//time_stamp_cal== 3954
					cell = Position2Cell(tmp_pos);
					if (cell && cell->id != 0)
					{
						originCellId = cell->id;
						originCellReaderName = cell->strSubjectReader;
						cell->card_stamp_time = time_stamp_cal;
						//获取处理格子的系统时间
						::GetLocalTime(&cell->deal_time);	
						//保存卡的格子处理时间
						::GetLocalTime(&cellDealTime);
						
						//保存格子到单元格，用于计算怠速判断
						if (his_cell.size() > 100)
						{
							his_cell.pop_front();
						}
						his_cell.push_back(cell);

						if (nStartLocateCounts>=1&&his_cell.size() > 1)
						{
							//检查是否超限
							isOverThreshold();
							if (card_id == "0020000001043" && nStartLocateCounts ==5)
							{
								int a = 0;
								int b = 1;
								a = b;
							}
						}

						*tmp_cell = *cell;
						nStartLocateCounts++;
					}else{
						originCellId = 0;
						originCellReaderName = "";
					}

					if (!CheckStartRule())
					{
						return 1;
					}

					if (nStartLocateCounts == 5)
					{
						int a = 0;
						int b = 1;
						a = b;
					}
					
					if (last_cell && cell)
					{
						//只判断怠速
						int nCounts = 0;
						if (cell->strSubjectReader == last_cell->strSubjectReader)
						{
							nCounts = ceil(cell->id - last_cell->id);
						}
						else{
							double distance = sqrt(pow(tmp_pos->cx - this->x,2) + pow(tmp_pos->cy - this->y,2)) * map_scale;
							nCounts = distance / CELL_WIDTH;
						}
						//允许调整按ct号的最大格子数
						int diff_ct = abs(cell->card_stamp_time - last_cell->card_stamp_time);
						int thre_value = MAX_CELL_FORWARD_JUMP;
						if (diff_ct > 0)
						{
							thre_value = MAX_CELL_FORWARD_JUMP*diff_ct;
						}

						//怠速保留 0704实现
						//越界，向前大跳或向后大跳
						if (nCounts >= thre_value)
						{
							isIdling = false;
							if (this->nIncrease != 0)
							{
								int nCalcStep = 0;
								if (last_cell->nStep < 1)
								{
									nCalcStep = 1;
								}else{
									nCalcStep = last_cell->nStep;
								}
								cell = GetNextCell(last_cell,this->nIncrease,nCalcStep);
								if (cell)
								{
									cell->isFit = true;
								}
							}	
						}else if (!CheckIdleStatus())
						{
							isIdling = false;
							//非怠速
						}else{
							isIdling = true;
							acce_cur_state = STATE_ACCE_IDLING;
							//如果怠速了，则直接取上一次的结果
							if (last_cell)
							{
								cell = last_cell;
							}
						}

						//1.越界：定位id相对于上一个实际id超过15格以上，直接根据上一个v来设置实际id
						//2.加速：定位id速度高于上一个实际的id的v，采取lv*a+cv*b
						//3.减速：定位id速度高于上一个实际的id的v，采取lv*a+cv*b
						//4.匀速：匀速前进
						//5.回跳：定位id显示回跳，5次以内，保持匀速前进或减速前进，5次掉头开始掉头
						//6.怠速：
						//7.静止：

						//越界
						//double distance = sqrt(pow(tmp_pos->cx - this->x,2) + pow(tmp_pos->cy - this->y,2)) * map_scale;
						//int nCounts = ceil(distance / CELL_WIDTH);
						//
						////允许调整按ct号的最大格子数
						//int diff_ct = abs(cell->card_stamp_time - last_cell->card_stamp_time);
						//int thre_value = MAX_CELL_FORWARD_JUMP;
						//if (diff_ct > 0)
						//{
						//	thre_value = MAX_CELL_FORWARD_JUMP*diff_ct;
						//}

						////怠速保留 0704实现
						////越界，向前大跳或向后大跳
						//if (nCounts >= thre_value)
						//{
						//	 if (isRebound(tmp_pos))
						//	 {
						//		 //如果回退，则是向后大跳
						//		 //如果5次以内回退
						//		 if (change_cell_dir < MAX_CHANGE_DIR_COUNTS)
						//		 {
						//			 int calcStep = 0;

						//			 if (last_cell)
						//			 {
						//				 //在不知下一时刻如何走的情况下，不做减速，
						//				 nCounts = last_cell->nStep;
						//				 //20170703 17:03 根据本次定位格子id和上一次格子id的格子差值取半减速
						//				 //calcStep = abs((originCellId - last_cell->id)/2);
						//			 }
						//			 if (abs(calcStep) < 1E-2)
						//			 {
						//				 calcStep = 1;
						//			 }
						//			 if (this->nIncrease != 0)
						//			 {
						//				 cell = GetNextCell(last_cell,this->nIncrease,calcStep);
						//				 if (cell)
						//				 {
						//					 cell->isFit = true;
						//				 }
						//			 }	
						//			 change_cell_dir++;
						//		 }else{
						//			 change_cell_dir = 0;
						//		 }
						//	 } 
						//	 else
						//	 {
						//		 //否则是向前大跳
						//		 //按上一次速度做匀速运动
						//		 if (last_cell)
						//		 {
						//			 nCounts = last_cell->nStep;
						//		 }
						//		 if (abs(nCounts) < 1E-2)
						//		 {
						//			 nCounts = 1;
						//		 }
						//		 if (this->nIncrease != 0)
						//		 {
						//			 cell = GetNextCell(last_cell,this->nIncrease,nCounts);
						//			 //cell = GetNextCell(last_cell,this->nIncrease,nCounts,originCellReaderName);
						//			 //cell = GetNextCell(last_cell,this->nIncrease,nCounts,tmp_cell);
						//			 if (cell)
						//			 {
						//				 cell->isFit = true;
						//			 }
						//		 } 
						//	 }
						//}else if (!CheckIdleStatus())
						//{
						//	//如果不是怠速
						//	//if (isRebound(tmp_pos))
						//	if (isRebound(cell))
						//	{
						//		//如果5次以内回退
						//		if (change_cell_dir < MAX_CHANGE_DIR_COUNTS)
						//		{
						//			int nCounts = 0;
						//			if (last_cell)
						//			{
						//				//在不知下一时刻如何走的情况下，不做减速，
						//				nCounts = last_cell->nStep;
						//				//nCounts = ceil(last_cell->nStep/2);	//20170703 16:46 做取半减速
						//			}
						//			if (abs(nCounts) < 1E-2)
						//			{
						//				nCounts = 1;
						//			}
						//			if (this->nIncrease != 0)
						//			{
						//				//cell = GetNextCell(last_cell,this->nIncrease,nCounts,originCellReaderName);
						//				cell = GetNextCell(last_cell,this->nIncrease,nCounts);
						//				if (cell)
						//				{
						//					cell->isFit = true;
						//				}
						//			}
						//			change_cell_dir++;
						//		}else{
						//			change_cell_dir = 0;
						//		}
						//	}else{
						//		//不回退，判断是否大跳
						//		//判断是否加速、减速、匀速
						//		//如果权值相同，以下三个可以使用同一逻辑处理
						//		//int calcStep = 0;

						//		//if (nCounts > last_cell->nStep)
						//		//{
						//		//	//加速,权值可调
						//		//	int step = ceil(THIS_V_POWER * last_cell->nStep + LAST_V_POWER * nCounts);

						//		//	//2017/07/02新增
						//		//	switch (this->nIncrease)
						//		//	{
						//		//	case 1:
						//		//		//如果步进后的格子id会超过原始定位格子id，
						//		//		//则取原始定位格子和上一次定位格子的差
						//		//		if (step + last_cell->id >= originCellId)
						//		//		{
						//		//			calcStep =  abs(last_cell->id - originCellId);
						//		//		} 
						//		//		else
						//		//		{
						//		//			calcStep = step;
						//		//		}
						//		//		break;
						//		//	case -1:
						//		//		if (last_cell->id - step >= originCellId)
						//		//		{
						//		//			calcStep = step;
						//		//		} 
						//		//		else
						//		//		{
						//		//			calcStep = abs(last_cell->id - originCellId);
						//		//		}
						//		//		break;
						//		//	}

						//		//	if (abs(calcStep) < 1E-2)
						//		//	{
						//		//		calcStep = 1;
						//		//	}
						//		//	if (this->nIncrease != 0)
						//		//	{
						//		//		cell = GetNextCell(last_cell,this->nIncrease,calcStep,tmp_cell);
						//		//		if (cell)
						//		//		{
						//		//			cell->isFit = true;
						//		//		}
						//		//	}
						//		//} 
						//		//else if (nCounts < last_cell->nStep)
						//		//{
						//		//	//减速,权值可调
						//		//	//采取向下取整，并考虑方向性
						//		//	int step = 0;

						//		//	if (last_cell)
						//		//	{
						//		//		step = originCellId - last_cell->id;
						//		//	}

						//		//	calcStep = ceil(THIS_V_POWER * last_cell->nStep + LAST_V_POWER * nCounts);

						//		//	if (abs(calcStep) < 1E-2)
						//		//	{
						//		//		calcStep = 1;
						//		//	}
						//		//	if (this->nIncrease != 0)
						//		//	{
						//		//		cell = GetNextCell(last_cell,this->nIncrease,calcStep,tmp_cell);
						//		//		if (cell)
						//		//		{
						//		//			cell->isFit = true;
						//		//		}
						//		//	}
						//		//}else
						//		//{
						//		//	if (last_cell->id > tmp_cell->id && this->nIncrease == -1)
						//		//	{
						//		//		calcStep = ceil(abs(last_cell->id - tmp_cell->id)/2);
						//		//	}
						//		//	//匀速运动,权值可调
						//		//	if (this->nIncrease != 0)
						//		//	{
						//		//		cell = GetNextCell(last_cell,this->nIncrease,calcStep,tmp_cell);
						//		//		if (cell)
						//		//		{
						//		//			cell->isFit = true;
						//		//		}
						//		//	}
						//		//}
						//	}
						//}else{
						//	acce_cur_state = STATE_ACCE_IDLING;
						//	//如果怠速了，则直接取上一次的结果
						//	if (last_cell)
						//	{
						//		cell = last_cell;
						//	}
						//}
					}
				}
			}
		}
	}
	
	if (cell)
	{
		SaveCardAlgoData(cell);
	}	

	return ret;
}

std::shared_ptr<Cell> Card::GetNextCell(std::shared_ptr<Cell> lastCell,int increase,int nIndex,std::shared_ptr<Cell> newCell)
{
	if (lastCell->id == 0)
	{
		return nullptr;
	}

	std::shared_ptr<Cell> cell = std::make_shared<Cell>();

	int nFindId = 0;
	bool bFind = false;
	std::string findReaderName = "";

	if (last_cell && newCell == nullptr)
	{
		//处理500ms定时器的处理
		int nNewCellId = 0;

		findReaderName = lastCell->strSubjectReader;

		if (findReaderName == "")
		{
			return nullptr;
		}
		switch (increase)
		{
		case 1:
			nNewCellId = lastCell->id + nIndex;
			if (nNewCellId > mpCellPath->find(lastCell->strSubjectReader)->second.size())
			{
				nFindId = mpCellPath->find(lastCell->strSubjectReader)->second.size();
				cell->isChangePath = false;
			}else{
				nFindId = nNewCellId;
			}
			break;
		case -1:
			nNewCellId = lastCell->id - nIndex;
			if (nNewCellId < 0)
			{
				nFindId = 1;
				cell->isChangePath = false;
			}else{
				nFindId = nNewCellId;
			}
			break;
		default:
			break;
		}

		if (findReaderName == "")
		{
			return nullptr;
		}

		for (std::list<std::shared_ptr<Cell>>::iterator it = mpCellPath->find(findReaderName)->second.begin();it != mpCellPath->find(findReaderName)->second.end();++it)
		{
			if ((*it)->id == nFindId)
			{
				cell->id = (*it)->id;
				cell->top = (*it)->top;
				cell->left = (*it)->left;
				cell->bottom = (*it)->bottom;
				cell->right = (*it)->right;
				cell->isHasBranch = (*it)->isHasBranch;
				cell->maxReaderId = (*it)->maxReaderId;
				cell->minReaderId = (*it)->minReaderId;
				break;
			}
		}	
	}else if (last_cell && newCell)
	{
		//处理两种情况，一种是分站内的格子移动，另一种是跨分站格子的移动
		if (last_cell->strSubjectReader == newCell->strSubjectReader)
		{
			cell->isChangePath = false;	
			findReaderName = lastCell->strSubjectReader;
			cell->strSubjectReader = findReaderName;
			//分站内的格子移动
			switch (increase)
			{
			case 1:
				if (lastCell->id + nIndex > mpCellPath->find(lastCell->strSubjectReader)->second.size())
				{
					//存在跨分站的问题,先找到路径中的最大分站
					//根据路径集中找到最大分站的列表，
					nFindId = mpCellPath->find(lastCell->strSubjectReader)->second.size();	
				}else{
					nFindId = lastCell->id + nIndex;
				}
				break;
			case -1:
				if (lastCell->id - nIndex < 0)
				{
					//nFindId = 1;
					//存在跨分站的问题
					//先找到路径中的最小分站
					//根据路径集中找到最小分站的列表，
					nFindId = 1;
				}else{
					nFindId = lastCell->id - nIndex;
				}	
				break;
			default:
				cell = newCell;
				break;
			}
		}else{
			//另一种是跨分站格子的移动
			cell->isChangePath = true;	
			findReaderName = newCell->strSubjectReader;
			if (findReaderName != "")
			{
				cell->strSubjectReader = findReaderName;
			}	

			int oldCellOffset = 0;
			int newCellOffset = 0;
			int CellOffset = 0;

			if (lastCell->maxReaderId == newCell->maxReaderId)
			{
				oldCellOffset = mpCellPath->find(lastCell->strSubjectReader)->second.size() - lastCell->id;
				newCellOffset = mpCellPath->find(newCell->strSubjectReader)->second.size() - newCell->id;
				CellOffset = ceil(sqrt(pow(oldCellOffset,2)+pow(newCellOffset,2)));

				nFindId = mpCellPath->find(newCell->strSubjectReader)->second.size() - abs(CellOffset - oldCellOffset);

				if (nFindId == 0)
				{
					nFindId = mpCellPath->find(newCell->strSubjectReader)->second.size();
				}

				nIncrease = -1;
			}else if (lastCell->maxReaderId == newCell->minReaderId)
			{
				oldCellOffset = mpCellPath->find(lastCell->strSubjectReader)->second.size() - lastCell->id;
				newCellOffset = newCell->id;
				CellOffset = ceil(sqrt(pow(oldCellOffset,2) + pow(newCellOffset,2)));

				nFindId = abs(CellOffset - oldCellOffset);

				if (nFindId == 0)
				{
					nFindId = 1;
				}
				nIncrease = 1;
			}else if (lastCell->minReaderId == newCell->maxReaderId)
			{
				oldCellOffset = lastCell->id;
				newCellOffset = mpCellPath->find(newCell->strSubjectReader)->second.size() - newCell->id;
				CellOffset = ceil(sqrt(pow(oldCellOffset,2)+pow(newCellOffset,2)));

				nFindId = mpCellPath->find(newCell->strSubjectReader)->second.size() - abs(CellOffset - oldCellOffset);

				if (nFindId == 0)
				{
					nFindId = mpCellPath->find(newCell->strSubjectReader)->second.size();
				}
				nIncrease = -1;
			}else if (lastCell->minReaderId == newCell->minReaderId)
			{
				oldCellOffset = lastCell->id;
				newCellOffset = newCell->id;
				CellOffset = ceil(sqrt(pow(oldCellOffset,2)+pow(newCellOffset,2)));

				nFindId = abs(CellOffset - oldCellOffset);

				if (nFindId == 0)
				{
					nFindId = 1;
				}

				nIncrease = 1;
			}else{
				findReaderName = "";
			}
		}

		if (findReaderName == "")
		{
			return nullptr;
		}

		for (std::list<std::shared_ptr<Cell>>::iterator it = mpCellPath->find(findReaderName)->second.begin();it != mpCellPath->find(findReaderName)->second.end();++it)
		{
			if ((*it)->id == nFindId)
			{
				cell->id = (*it)->id;
				cell->top = (*it)->top;
				cell->left = (*it)->left;
				cell->bottom = (*it)->bottom;
				cell->right = (*it)->right;
				cell->isHasBranch = (*it)->isHasBranch;
				cell->maxReaderId = (*it)->maxReaderId;
				cell->minReaderId = (*it)->minReaderId;
				break;
			}
		}
	}

	return cell;
}

std::shared_ptr<Cell> Card::GetNextCell(std::shared_ptr<Cell> lastCell,int increase,int nIndex)
{
	if (lastCell == nullptr || lastCell->id == 0)
	{
		return nullptr;
	}

	std::shared_ptr<Cell> cell = std::make_shared<Cell>();

	int step = 0 , nFindId = 0;

	step = lastCell->id + nIndex*increase;

	int nSize = 0;
	nSize = mpCellPath->find(lastCell->strSubjectReader)->second.size();

	if (step > nSize)
	{
		nFindId = nSize;
	}else if (step < 0)
	{
		nFindId = 1;
	}else{
		nFindId = step;
	}

	bool bFind = false;
	for (std::list<std::shared_ptr<Cell>>::iterator it = mpCellPath->find(lastCell->strSubjectReader)->second.begin();it != mpCellPath->find(lastCell->strSubjectReader)->second.end();++it)
	{
		if ((*it)->id == nFindId)
		{
			cell = *it;
			cell->interval_time = lastCell->interval_time;
			bFind = true;
			break;
		}
	}

	if (!bFind)
	{
		return nullptr;
	}

	return cell;
}

/*
 * 如果怠速返回true，否则返回false 
*/
bool Card::CheckIdleStatus()
{
	std::mutex mu;
	std::lock_guard<mutex> lg(mu);

	if (his_cell.size() < 5)
	{
		return false;
	}

	int flag = 0;
	/*std::list<std::shared_ptr<Cell>>::iterator ref = his_cell.begin();
	for (std::list<std::shared_ptr<Cell>>::iterator it = his_cell.begin();it != his_cell.end();++it)
	{
		if ((*it)->id != (*ref)->id)
		{
			flag = 1;
			break;
		}
	}*/
	int i = 0;
	std::list<std::shared_ptr<Cell>>::reverse_iterator ref = his_cell.rbegin();
	for (std::list<std::shared_ptr<Cell>>::reverse_iterator ref_it = his_cell.rbegin();ref_it != his_cell.rend();++ref_it)
	{
		if ((*ref)->id != (*ref_it)->id)
		{
			flag = 1;
			break;
		}
		i++;
		if (i == 4)
		{
			break;
		}
	}

	if (flag == 0)
	{
		return true;
	}

	//拟合法
	std::vector<int> vt,vc;
	vt.resize(0);
	vc.resize(0);

	//反向取MAX_FIT_DATA_COUNTS个元素
	double val[MAX_FIT_DATA_COUNTS] = {0};
	i = MAX_FIT_DATA_COUNTS - 1;
	for (std::list<std::shared_ptr<Cell>>::reverse_iterator it = his_cell.rbegin();it != his_cell.rend();++it)
	{
		if (!(*it)->isOverThre)//大跳数据不参与怠速判断
		{
			//如果非越界，则参与计算怠速判断,
			val[i] = (*it)->id;
			i--;
		}	
		if (i < 0)
		{
			break;
		}
	}

	int nAcs = 0;
	int nDesc = 0;
	//统计方向
	for (int k = 0; k < MAX_FIT_DATA_COUNTS - 1;k++)
	{
		if (val[k] < val[k+1])
		{
			nAcs++;	
		}else{		
			nDesc++;
		}
	}

	if (nAcs > nDesc)
	{
		nIncrease = 1;
	}else if (nAcs < nDesc)
	{
		nIncrease = -1;
	}else
	{
		//相等的话就保持原来的方向
	}
	if (card_id == "0020000001002")
	{
		TRACE(_T("nIncrease : %d \r\n"),nIncrease);
	}

	i = 0;
	for (; i < MAX_FIT_DATA_COUNTS;i++)
	{
		vt.push_back(i+1);
		vc.push_back(val[i]);
	}

	Fit fit;
	fit.linearFit(vt,vc);

	if (fit.getR() < FIT_RELIABILITY)
	{
		acce_cur_state = ACCELERATE_STATE::STATE_ACCE_IDLING;
		return true;
	}

	return false;
}

/*
 * 功能：确定启动规则，如果怠速，则不能运动，否则开始运动
 *
 * param
 *		无
 *
 * return
 *		怠速			返回false
 *		运动			返回true
*/
bool Card::CheckStartRule()
{
	//前5次不运动
	if (nStartLocateCounts < 5)//哈哈哈 先注释掉
	{
		return false;
	}

	//如果第六次开始还是怠速状态，则不运动
	if (CheckIdleStatus())
	{
		return false;
	}

	return true;
}

/*
 * 作用：检查是否大跳
 *
 * param
 *		cell		当前定位格子信息
 *
 * return
 *		如果存在大跳，则返回true；否则，返回false
 *
*/
bool Card::isOverThreshold()
{
	int cnt = 0;
	/*cnt = abs(cell->card_stamp_time - last_cell->card_stamp_time);

	int diff_cell_id = abs(cell->id - last_cell->id);

	if (diff_cell_id > cnt * MAX_CELL_FORWARD_JUMP)
	{
		cell->isOverThre = true;
		return true;
	}*/

	//判定方式改为反向遍历取相邻两个元素进行判断
	std::list<std::shared_ptr<Cell>>::reverse_iterator it = his_cell.rbegin();
	std::list<std::shared_ptr<Cell>>::reverse_iterator it_last = it;
	std::advance(it_last,1);

	if (it == his_cell.rend() || it_last == his_cell.rend())
	{
		return false;
	}

	cnt = abs((*it)->card_stamp_time - (*it_last)->card_stamp_time);

	int diff_cell_id = abs((*it)->id - (*it_last)->id);

	if (diff_cell_id > cnt * MAX_CELL_FORWARD_JUMP)
	{
		(*it)->isOverThre = true;
		return true;
	}

	return false;
}

int Card::SetCellPathMap(std::shared_ptr<CellPathMap> pCellPath)
{
	this->mpCellPath = pCellPath;

	return 0;
}

int Card::ChangeSystemTime(SYSTEMTIME& st,const int condition)
{
	int nMs = st.wMilliseconds;
	int nSecond = st.wSecond;
	int nMinute = st.wMinute;
	int nHour = st.wHour;

	if (condition == 0)
	{
		if (st.wMilliseconds < 500)
		{
			nMs = 0;
		}else{
			nMs = 500;
		}
	} 
	else
	{
		if (st.wMilliseconds == 0)
		{
			nMs = 500;
		}else if(st.wMilliseconds == 500){
			nMs = 0;
			nSecond += 1;
			if (nSecond >= 60)
			{
				nSecond = 0;
				nMinute += 1;
				if (nMinute >= 60)
				{
					nMinute = 0;
					nHour += 1;
				}
			}
		}
	}
	
	st.wMilliseconds = nMs;
	st.wSecond = nSecond;
	st.wMinute = nMinute;
	st.wHour = nHour;

	return 0;
}

int Card::ChangeSystemTime(SYSTEMTIME& st)
{
	int nMs = st.wMilliseconds;
	int nSecond = st.wSecond;
	int nMinute = st.wMinute;
	int nHour = st.wHour;

	//如果毫秒数大于1000，向秒进一格
	if (st.wMilliseconds + 500 >= 1000)
	{
		nMs = st.wMilliseconds + 500 - 1000;

		//如果秒大于60，向分进一格
		if (st.wSecond + 1 >= 60)
		{
			nSecond = st.wSecond + 1 - 60;
			//如果分大于60，向时进一格
			if (st.wMinute + 1 >= 60)
			{
				nMinute = st.wMinute + 1 - 60;
				//如果时大于24，向天进一格
				if (st.wHour + 1 >= 24)
				{
					nHour = st.wHour + 1 - 24;
				}else{
					nHour = st.wHour + 1;
				}
			}else{
				nMinute = st.wMinute + 1;
			}				
		}else{
			nSecond = st.wSecond + 1;
		}			
	}else{
		nMs = st.wMilliseconds + 500;
	}

	st.wMilliseconds = nMs;
	st.wSecond = nSecond;
	st.wMinute = nMinute;
	st.wHour = nHour;

	return 0;
}

//void Card::handle_data_thread(LPVOID pContext)
//{
//	CYAServerDlg* pDlg = (CYAServerDlg*)pContext;
//	while (TRUE)
//	{
//		WaitForSingleObject(hEvent,INFINITE);
//		ResetEvent(hEvent);
//		char sql[LENGTH_SQL] = {0};
//		char chTime[30] = {0};
//		char chIntervalTime[30] = {0};
//		sprintf_s(chTime,
//			30,
//			"%u-%u-%u %u:%u:%u.%u",
//			cellDealTime.wYear,
//			cellDealTime.wMonth,
//			cellDealTime.wDay,
//			cellDealTime.wHour,
//			cellDealTime.wMinute,
//			cellDealTime.wSecond,
//			cellDealTime.wMilliseconds);
//
//		sprintf_s(chIntervalTime,
//			30,
//			"%u-%u-%u %u:%u:%u.%u",
//			last_cell->interval_time.wYear,
//			last_cell->interval_time.wMonth,
//			last_cell->interval_time.wDay,
//			last_cell->interval_time.wHour,
//			last_cell->interval_time.wMinute,
//			last_cell->interval_time.wSecond,
//			last_cell->interval_time.wMilliseconds);
//
//		sprintf_s(sql,
//			"insert ignore into his_cell_location(card_id,cur_time,interval_time,origin_reader_name,origin_cell_id,real_reader_name,real_cell_id,card_stamp_time,isFit,speed,mileage,map_id,area_id) values('%s','%s','%s','%s',%d,'%s',%d,%d,%d,%.4f,%.4f,%d,%d);",
//			card_id.c_str(),
//			chTime,
//			chIntervalTime,
//			originCellReaderName.c_str(),
//			originCellId,
//			curCellReaderName.c_str(),
//			curCellId,
//			time_stamp_last,
//			is_fit_pos,
//			get_speed(),
//			mileage,
//			map_id,
//			area_id);
//
//		send_sql_message(sql);
//		pDlg->send_json_data(JSON_CMD_VALUE_PUSH,pDlg->get_json_position());
//	}
//}

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;

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

		return DIST_COUNT_LESS_THAN_TWO;
	}

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

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

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

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

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

		return DIST_COUNT_LESS_FOR_TIMESTAMP_ERROR;
	}

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

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

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

		return DIST_COUNT_LESS_FOR_SYNC_NUM_DIFFER_FIVE;
	}

	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 (m_nLastLocateT!=0 && this->last_locate.x != INVALID_COORDINATE && this->last_locate.y != INVALID_COORDINATE)
	{
		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;
			}
			if (pos->is_back)
			{
			}else{
				this->m_nStream = nStream;
			}	
		}
	}

	int nSign = 1;
	if(pos->cvx == 0){
		if(pos->cvy > 0){
			nSign = 1;
		}
	}else{
		if(pos->cvx > 0){
			nSign = 1;
		}else{
			nSign = -1;
		}
	}
	if (m_nLastLocateT!=0)
	{
		//此变量用于计算减速求新坐标而减速的距离
		this->last_locate.d_offset = sqrt(pow(pos->cx - this->last_locate.x,2) + pow(pos->cy - this->last_locate.y,2));
	}
	
	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;
	is_fit_pos = pos->is_fit;

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

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

	//拟合点作为参考点
	//拟合数据的计算
	if (his_pos.size() == REF_POSITION_NUM)
	{
		his_pos.pop_front();
	}
	his_pos.push_back(p);
	
	int nRef = REF_POSITION_NUM * 4;
	if (long_his_pos.size() == nRef)
	{
		long_his_pos.pop_front();
	}
	long_his_pos.push_back(p);
	
	if(is_ref_pos)
	{
		//只有为参考点时，才会进行拟合点的计算
		if (his_pos.size() == REF_POSITION_NUM)
		{
			if (!have_long_fit_pos)
			{
				CalcFittingData();
				cur_fit_nums = 0;
			}	
		}

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

	return 0;
}

/*
 * 作用：保存算法计算结果数据
 *
 * param
 *		cell		单元格
 * 
 * return
 *		返回0
 *
*/
int Card::SaveCardAlgoData(std::shared_ptr<Cell>& cell)
{
	//计算卡本次相对于上一次的步进格子数
	int nStep = 0;
	SYSTEMTIME sysit;
	std::string strTime = "";
	if (cell && last_cell)
	{
		if (!cell->isChangePath)
		{
			nStep = abs(cell->id - last_cell->id);
		}else{
			nStep = cell->nStep;
		}	
	}

	if (this->nStartLocateCounts > 5)
	{
		if (last_cell)
		{
			if (CFunctions::CheckSystemTimeValid(last_cell->interval_time))
			{
				sysit = last_cell->interval_time;
				strTime = CFunctions::systime2string(last_cell->interval_time);
			}	
		}	
	}

	//格子换算成坐标
	std::shared_ptr<POS> pos = std::make_shared<POS>();
	pos = Cell2Position(cell);

	curCellReaderName = cell->strSubjectReader;
	curCellId = cell->id;
	
//	std::mutex mu;
//	mu.lock();
//	//更新格子信息
//	last_cell = cell;
//	last_cell->nStep = nStep;
//	last_cell->dataSource = 1;
//	last_cell->card_stamp_time = time_stamp_cal;
//	time_stamp_last = time_stamp_cal;
//
//	if (this->nStartLocateCounts == 5)
//	{
//		::GetLocalTime(&last_cell->interval_time);
//		if (last_cell->interval_time.wMilliseconds < 500)
//		{
//			last_cell->interval_time.wMilliseconds = 0;
//		} 
//		else
//		{
//			last_cell->interval_time.wMilliseconds = 500;
//		}
//	}else if(this->nStartLocateCounts > 5){
//			last_cell->interval_time = sysit;
//			ChangeSystemTime(last_cell->interval_time);
//	}
//	
//#ifdef DEBUG
//	TRACE(_T("savecardalgodata: interval_time = %s\r\n"),CFunctions::c2wc(strTime.c_str()));
//#else
//	char log[200] = {0};
//	
//	if (strTime != "")
//	{
//		//sprintf_s(log,"savecardalgodata: interval_time = %s",strTime.c_str());
//		//std::string strLog = log;
//		//Log::write_log(FILE_TYPE::SYS_S,strLog,true);
//	}
//	
//#endif // DEBUG
//	locate_cells.push_back(last_cell);
//	mu.unlock();

	//更新格子信息
	cell->nStep = nStep;
	cell->dataSource = 1;
	cell->card_stamp_time = time_stamp_cal;
	time_stamp_last = time_stamp_cal;

	std::mutex mu;
	mu.lock();
	if (this->nStartLocateCounts == 5)
	{
		if (card_id == "0020000001043")
		{
			int a = 0;
			int b = 1;
			a = b;
		}
		::GetLocalTime(&cell->interval_time);
		if (cell->interval_time.wMilliseconds < 500)
		{
			cell->interval_time.wMilliseconds = 0;
		} 
		else
		{
			cell->interval_time.wMilliseconds = 500;
		}
	}else if(this->nStartLocateCounts > 5){
		if (last_cell && CFunctions::CompareSystemTime(cell->interval_time,last_cell->interval_time) != 0)
		{
			cell->interval_time = last_cell->interval_time;
			ChangeSystemTime(cell->interval_time);
		}	
	}
	if (cell)
	{
		last_cell = cell;
		locate_cells.push_back(last_cell);
	}else
		return 1;
	//ResetEvent(hEvent);
	::PostMessage(pMainWnd,WM_CARD_SEND_JSON,(WPARAM)this,(LPARAM)last_cell.get());
	//::PostMessage(pMainWnd,WM_CARD_SEND_JSON,(WPARAM)this,(LPARAM)0);
	mu.unlock();

	if (!is_fit_pos && m_nLastLocateT != 0)
	{
		//确保第一次不会进来算速度
		if (this->x != INVALID_COORDINATE && this->y != INVALID_COORDINATE)
		{
			//pos为本次定位坐标，(x,y)为上一次定位坐标
			double distance = sqrt(pow(pos->cx - this->x,2) + pow(pos->cy - this->y,2))*map_scale;
			this->mileage = distance;
			int nCounts = ceil(distance/CELL_WIDTH);
			this->v = nCounts*CELL_WIDTH/(SEND_CELL_INTERVAL/1000.0);
			this->v*=3.6;
		}
	}else{
		this->v = this->v / 2;
	}

	if (this->v > 30)
	{
		this->v = 25;
	}

	this->x = pos->cx;
	this->y = pos->cy;
	this->z = 0;

	if (m_nLastLocateT != 0 && this->last_locate.x != INVALID_COORDINATE && this->last_locate.y != INVALID_COORDINATE)
	{
		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;
			}
			this->m_nStream = nStream;
		}
	}

	//如果怠速，则速度置为零
	if (CheckIdleStatus())
	{
		this->v = 0;
	}

	is_fit_pos = pos->is_fit;

	this->last_locate.x = pos->cx;
	this->last_locate.y = pos->cy;
	this->last_locate.z = 0;
	this->last_locate.sync_num = this->m_nCalcSyncNum;
	this->m_nLastLocateT = this->m_nCalcSyncNum;
	this->last_locate.v = this->v;

	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);*/
		p->cx = cx;
		p->cy = cy;
		p->cvx = 0;
		p->cvy = 0;
		p->update = false;
	}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;
					p->posx = INVALID_COORDINATE;
					p->posy = INVALID_COORDINATE;
					p->cx   = INVALID_COORDINATE / (map_scale*1.0);
					p->cy   = INVALID_COORDINATE / (map_scale*1.0);
					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(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;
					p->posx = INVALID_COORDINATE;
					p->posy = INVALID_COORDINATE;
					p->cx   = INVALID_COORDINATE / (map_scale*1.0);
					p->cy   = INVALID_COORDINATE / (map_scale*1.0);

					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;
				p->posx = INVALID_COORDINATE;
				p->posy = INVALID_COORDINATE;
				p->cx   = INVALID_COORDINATE / (map_scale*1.0);
				p->cy   = INVALID_COORDINATE / (map_scale*1.0);

				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::CopySolution(std::shared_ptr<POS> source,std::shared_ptr<POS>& dest)
{
	dest->posx = source->posx;
	dest->posy = source->posy;
	dest->cvx  = source->cvx;
	dest->cvy  = source->cvy;
	return 0;
}

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()){ // 没有解
		TRACE(_T("没有接"));
		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;
		if (it_first != pRdm->end())
		{
			dist = sqrt(pow(udm_pos.at(0)->posx - it_first->second->x,2)+pow(udm_pos.at(0)->posy - it_first->second->y,2));
			if (dist < NEAR_READER && it_first->second->special == 0)
			{
				TRACE(_T("4002"));
				return SOLUTION_NEAR_READER;
			}
		}
		if (it_second != pRdm->end())
		{
			dist = sqrt(pow(udm_pos.at(0)->posx - it_second->second->x,2)+pow(udm_pos.at(0)->posy - it_second->second->y,2));
			if (dist < NEAR_READER && it_second->second->special == 0)
			{
				TRACE(_T("40002"));
				return SOLUTION_NEAR_READER;
			}

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

		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 = p.y = p.z = l1.x = l1.y = l1.z = l2.x = l2.y = l2.z = 0;

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

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

				if (!LocateAlgorithm::IsInLine(p,l1,l2))
				{
					continue;
				}else{
					if (((*it_pos)->nFirstReader == nfr &&(*it_pos)->nSecondReader == nfs)||
						((*it_pos)->nFirstReader == nfs &&(*it_pos)->nSecondReader == nfr))
					{
						continue;
					}else{
						dMinDiff = (*it_pos)->dis_diff;
						it_find = it_pos;
					}
					
				}	
			}
		}
		if (it_find != udm_pos.end())
		{
			if ((*it_find)->dis_diff < 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);
		TRACE(_T("未计算出结果"));
		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;
			TRACE(_T("无解"));
			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;
}

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

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

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

// 只显示优先级最高的状态
int const Card::StateBiz() 
{
	state_biz = 0;
	if(status_help == STATUS_ERROR){
		state_biz = STATUS_HELP;
		return state_biz;
	}

	if(status_lost == STATUS_ERROR){
		state_biz = STATUS_LOST;
		return state_biz;
	}

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

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

	if(status_over_speed == STATUS_ERROR){
		state_biz = STATUS_OVER_SPEED;
		return state_biz;
	}

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

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

MapInfo::MapInfo( void )
{

}

MapInfo::~MapInfo( void )
{

}

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

Dept::Dept()
{
}

Dept::~Dept()
{
}

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

OccLevel::OccLevel()
{
}

OccLevel::~OccLevel()
{
}

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

Adhoc::~Adhoc()
{
}

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

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

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

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

Chamber::~Chamber()
{

}

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

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

	return 0;
}

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

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

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

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

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

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

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

bool Section::is_has_chamber()
{
	bool bIsHasEmptyChamber = false;
	if (mp_champer_list.size()<=0)
	{
		return bIsHasEmptyChamber;
	}
	
	ChamberMap::iterator it = mp_champer_list.begin();
	for (;it!=mp_champer_list.end();++it)
	{
		if (it->second->m_nIsUsed == false)
		{
			bIsHasEmptyChamber = true;
			break;
		}
	}

	return bIsHasEmptyChamber;
}

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

	nTotals = mp_vehicle_list.size();

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

	return m_nState;
}

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

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

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

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

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

	return bRet;
}

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

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

	return bRet;
}

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

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

PatrolTask::~PatrolTask()
{
}

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

	duration_last = duration_stay_min = duration_stay_max = duration_ranging = 0;
}

PatrolPoint::~PatrolPoint()
{
}

BanShift::BanShift()
{

}

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

BanShift::~BanShift()
{

}