server/classdef.cpp

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

#include "classdef.h"
#include "constdef.h"
#include "structdef.h"
#include "locate_algorithm.h"
#include "./system_basic_info/SystemAnalysis.h"
#include "ProcessRemodule.h"
#include "log_process_module.h"
#include "Functions/Functions.h"
#include "algorithm/Fit.h"
#include "mylog/log_module.h"

#pragma warning(disable: 4244)

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

Card::Card( string cardid, int cardtype, double z_offset)
	:m_isWarning(false),m_minDistanceToWarningpoint(DBL_MAX),m_warning_threshold(DBL_MAX),m_CardHasBeenInDrivingfaceAlarm(false),
	m_DrivingfaceAlarmFlag(-1),shift_id(0),m_indexForRcvOneReader(0)
{
	m_warning_point_id = -1;
	BOOL bRet = InitializeCriticalSectionAndSpinCount(&m_csCard, MAXCRITICALSECTIONSPINCOUNT);
	is_drivingface_Initialization=false;
	card_type = cardtype;
	card_id = cardid;
	this->z_offset = z_offset;

	::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;
	a = 0;
	t = 0;
	init_postion = false;
	is_first_location = true;
	is_algo_first_location = true;
	b_pos_change = false;
	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 = level_id = worktype_id = vehice_type_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 = 0;
	driver_id = "";
	shift_type_id = 1; 

	reader_tickcount = time_stamp;
	time_stamp_last = -1;

	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_registered = true;
	is_deal_by_algo = false;
	pTofReaderPathMap = NULL;
	pTdoaReaderPathMap = NULL;
	last_s_locate_reader[0] = last_s_locate_reader[1] = -1;
	m_syncNumList.clear();

	m_nCalcSyncNum = 0;
	m_nSyncNumInList = 0;
	last_locate.tt = 0;
	last_locate.a = last_locate.antenna_id = last_locate.reader_id = last_locate.t = 0;
	last_locate.d = last_locate.d_offset = last_locate.v = 0.0;
	last_locate.x = last_locate.y = last_locate.z = INVALID_COORDINATE;
	last_x = last_y = last_z = INVALID_COORDINATE;
	this->driving_face_cur_shift_start_x = this->driving_face_cur_shift_start_y = this->driving_face_cur_shift_start_z= this->driving_face_cur_shift_x= this->driving_face_cur_shift_y=this->driving_face_cur_shift_z = 0;

	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 = 0;

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

	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;

	isIdling = false;
	data_cell_time = 0;
	calc_cell_time = 0;
	last_cell = nullptr;
	::GetLocalTime(&cellDealTime);

	drive_face_location_write_time = 0;
	idle_pos_list.resize(0);

	fit_b = fit_k = fit_r = cell_a = 0.0;
	last_k = 0;
	delta_ct = 0;

	time_last_tof = time(NULL);

}

Card::Card( void )
	:m_isWarning(false),m_minDistanceToWarningpoint(DBL_MAX),m_warning_threshold(DBL_MAX),m_CardHasBeenInDrivingfaceAlarm(false),
	m_DrivingfaceAlarmFlag(-1)
{
	m_warning_point_id = -1;
}

/*
 * 重置算法相关参数的数据
 *
 * param
 *		无
 *
 * return
 *		无
*/
void Card::reset()
{
	//以下参数与算法相关
	//坐标初始值为0
	m_nLastLocateT = 0;
	time_stamp_last = -1;
	b_long_interval = false;
	is_first_location = true;
	last_locate.sync_num = 0;
	is_algo_first_location = true;

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

/*
* 设置地图集覆盖范围，适用TOF
*
*/
void Card::set_reader_path_tof(std::shared_ptr<ReaderPathMap> rpm)
{
	this->pTofReaderPathMap = 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
	if (STATE_ACCE_STATIC == acce_state || STATE_ACCE_IDLING == acce_state)
	{
		this->isoutput = false;
	}
	else
	{
		if (this->x == this->output_x && this->y == this->output_y) //last_x,last_y
		{
			count_idle++;
			this->isoutput = false;
		}else{
			//如果运动状态时，
			int nSign = 0;
			int nState = 0;

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

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

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

/*
 * TOF一维定位算法
 * 
 * param
 *		cnt		数据条数
 *
 * return
 *		成功返回0，否则返回非零值
 *
*/
int Card::algo_tof_1d(int cnt)
{
	int i = 0, nCount = 0;
	bool bFound = false;

	//组装数据部分
	map<unsigned long long, std::shared_ptr<_coordinate>> tmp_dists_locate;

	if (cnt > 1)
	{
		for(DistMap::iterator it = _dists.front().distmap.begin(); it != _dists.front().distmap.end(); ++it){
			bFound = false;
			// 判断是否已经加入到列表
			for(int j = 0; j < i; j++){
				if(tmp_dists_locate.find(it->second->reader_id) != tmp_dists_locate.end()){
					//已存在，则退出查找for循环
					bFound = true;
					break;
				}
			}
			if(i < DIST_COUNT && !bFound){
				tmp_dists_locate.insert(make_pair(i,(it->second)));
				i++;
			}
		}
	}else{
		/*for(DistMap::iterator it = _dists.front().distmap.begin(); it != _dists.front().distmap.end(); ++it,i++){
			tmp_dists_locate.insert(make_pair(i,(it->second)));
		}*/
		return 1;
	}

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

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

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

	TOF_REFER_DATA refer_data;
	it = tmp_dists_locate.find(key);
	if (it != tmp_dists_locate.end())
	{
		refer_data.nCardTimeStamp = it->second->t;
		refer_data.llDeltaTime = 0;
		refer_data.nReaderId = it->second->reader_id;
		refer_data.nAntennaIndex = it->second->antenna_id;
		refer_data.dDistance = it->second->d;
		refer_data.x = it->second->x;
		refer_data.y = it->second->y;
		refer_data.z = it->second->z;
		refer_data.a = it->second->a;
	}

	std::shared_ptr<POS> p = nullptr;
	std::shared_ptr<POS> tmpPos = std::make_shared<POS>();

	std::advance(it,1);
	i = 1;
	bool isExist = false;
	int nReaderId = 0;			//分站id
	int nAntennaIndex = 0;		//天线索引
	double dist = 0.0;			//距离

	std::vector<std::shared_ptr<POS>> udm_pos;
	udm_pos.resize(0);

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

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

		if(p == nullptr)
		{
			continue;
		}
		isExist = true;
		udm_pos.push_back(p);

	}
	if (!isExist )
	{
		if (p == nullptr)
		{
			p = std::make_shared<POS>();
		}
		p->posx = refer_data.x;
		p->posy = refer_data.y;
		p->posz = refer_data.z;
		p->cx = p->posx/map_scale;
		p->cy = p->posy/map_scale;
		p->cz = p->posz/map_scale;
	}else{
		//对多解求平均
		double posx = 0;
		double posy = 0;
		double posz = 0;

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

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

	if (CheckPositionValid(p))
	{
		p->delta_time = refer_data.nCardTimeStamp - this->t;
	}

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

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

	return 0;
}

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

	if(cnt < 2){
		LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_141);
		m_afmData.bStatus = true;
		m_afmData.strCardId = this->card_id;
		m_afmData.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(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 != -1 && 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;
			}
		}

		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(is_algo_first_location){
				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，就不要此次的定位数据，
				//如果通过加速度判断就将队列中从头开始到此同步数据的所有元素都丢弃，并插入新的此次同步数据

				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;
								break;
							}
						}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->last_locate.y = this->y;
				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;
				//如果是第一次定位，则下一次即为非第一次定位了
				if (this->is_algo_first_location)
				{
					this->is_algo_first_location = false;
				}

				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{
			//最新通过算法算出的结果
			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.数据有效性判断
	ret = CheckDistData(cnt);
	if(ret){
		return ret;
	}

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

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

	bool bIsBack = false;

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

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

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

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

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

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

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

	return ret;
}

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(abs(this->x - this->last_locate.x) <= 1E-4)
	{
		this->right_y = this->left_y = this->y;
		if(this->y >= this->last_locate.y)
		{
			this->right_x = this->x - offset;
			this->left_x  = this->x + offset;
		}
		else
		{
			this->right_x = this->x + offset;
			this->left_x  = this->x - offset;
		}
	}
	else
	{
		if(abs(this->y - this->last_locate.y) <= 1E-4)
		{
			this->right_x = this->left_x = this->x;
			if(this->x > this->last_locate.x)
			{
				this->right_y = this->y + offset;
				this->left_y = this->y - offset;
			}
			else
			{
				this->right_y = this->y - offset;
				this->left_y = this->y + offset;
			}
		}
		else
		{
			double k = -1.0 / (this->y - this->last_locate.y)/(this->x - this->last_locate.x);
			double m = this->y - k * this->x;

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

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

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

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

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

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

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

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

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

bool Card::is_pos_state_changed()  // 考勤
{
	bool ret = false;

	if(pos_state != pos_state_old)
	{
		if((PDT_INIT == pos_state_old && PDT_UP == pos_state ) || (PDT_INIT == pos_state && PDT_UP == pos_state_old) )
		{
			pos_state = pos_state_old = PDT_UP;
			pos_state_count = pos_state_confirm_times;
			ret = false;
		}
		else if(pos_state_count >= pos_state_confirm_times)
		{
			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 ceil(fabs(v));
}

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

	switch(ranging_type){
	case LOCATEDATATYPE::LDT_TDOA:
		//ret = algo_tdoa_1d(cnt);
		ret = DiscreteLocate(cnt);
		break;
	case LOCATEDATATYPE::LDT_TOF:
		ret = algo_tof_1d(cnt);
		break;
	default:
		;
	}
	if (ret == 0)
	{
		m_indexForRcvOneReader = 0;
		inspect_coordinate(this->acce_cur_state);
	}
	else if(DIST_COUNT_LESS_THAN_TWO == ret)
	{
		m_indexForRcvOneReader++;
	}
	else
	{
		m_indexForRcvOneReader=0;
	}
	if (m_indexForRcvOneReader >= INDEXFORRECONEREADER)
	{
		this->v = 0;
	}

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

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

	if ((isNeedWrited || nStartLocateCounts<6) && m_nCalcSyncNum > 0 && ret == 0)
	{
		std::mutex mu;
		std::lock_guard<mutex> lg(mu);
		std::string flag1 = is_fit_pos?"Y":"N";
		int nSource1 = 1;		
		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, cv: %f, source: %d, fit_k : %f, fit_b: %f, fit_r: %f",
			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(),
			get_speed(),
			nSource1,
			fit_k,
			fit_b,
			fit_r
			);

		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 = "";
	this->state= 0;
	this->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_lost){
		//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)? STATE_ACCE_MOTION : STATE_ACCE_STATIC;

	if(state_moving == STATE_ACCE_STATIC){
		ret += "静止";
	}else if(state_moving == STATE_ACCE_MOTION){
		if(CT_VEHICLE == card_type && 0 == v)
		{
			state_moving = STATE_ACCE_IDLING;
			ret += "怠速";
		}		
		else
		{
			ret += "运动";
		}
	}else if(state_moving == STATE_ACCE_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 == 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;
	}
	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;
}

/*
 * 卡尔曼滤波处理模块
 *
 * param
 *		pos		定位结果
 *
 * return
 *		处理成功返回0，否则返回非零值
 *
*/
int Card::KalmanFilterProcess(std::shared_ptr<POS>& pos)
{
	//卡尔曼的时间差
	double kalman_detal_t = 0;
	//分站同步序号的间隔时间，单位为秒
	double interval_time = 0.2;

	kalman_detal_t = GetDeltaT();

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

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

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

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

		if(this->m_pKalmanFilter->m_nCounts == 1){
			//第一次直接赋值
			this->m_pKalmanFilter->m_pCar->x = this->m_pKalmanFilter->m_pCar->z;
		}
		if(this->m_pKalmanFilter->m_nCounts == 2){
			//两次处理
			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) > ZERO_PRECISION){
				if (abs(pos->ref_x) < ZERO_PRECISION && abs(pos->ref_y) < ZERO_PRECISION)
				{
				}else{
					this->m_pKalmanFilter->m_pCar->x(1,0) = (this->m_pKalmanFilter->m_pCar->x(0,0) - pos->ref_x*this->map_scale)/pos->diff_reader_sync_num;
					this->m_pKalmanFilter->m_pCar->x(3,0) = (this->m_pKalmanFilter->m_pCar->x(2,0) - pos->ref_y*this->map_scale)/pos->diff_reader_sync_num;
				}	
			}	
		}
	}

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

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

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

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

			cp.reset();
		}		

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

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

	return 0;
}

/*
 * 检查溢出
*/
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;
}

/*
* 组装拟合数据，主要是前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)
{
	int fit_ret = 0;
	//拟合预测解
	is_ref_pos = true;
	std::shared_ptr<POS> fit_pos = GetPosFromFittingData();
	if (fit_pos == nullptr)
	{
		return 1;
	}

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

	return fit_ret;
}

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

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

//根据点的信息获得格子信息
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 = std::make_shared<Cell>();
			*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;
}

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

	return bRet;
}

double Card::GetFittingDegree(std::list<std::shared_ptr<Cell>> ltCells)
{
	if (ltCells.size() < 5)
	{
		return 1.0;
	}

	double degree = 0.0;
	double ctArray[5] = {0.0};
	double cidArray[5] = {0};

	//获取5个元素的值
	int i = 4;
	for (std::list<std::shared_ptr<Cell>>::reverse_iterator rit = ltCells.rbegin();rit != ltCells.rend();++rit)
	{
		ctArray[i] = (*rit)->card_stamp_time;
		cidArray[i] = (*rit)->id;
		i--;
		if (i<0)
		{
			break;
		}
	}

	//构造拟合对象
	Fit f;
	f.linearFit(ctArray,cidArray,5,true);

	//获得拟合度
	degree = f.getR();

	return degree;
}

int Card::MappingCell2FitLinear(std::shared_ptr<Cell>& cell)
{
	if (cell == nullptr)
	{
		return 1;
	}

	double ctArray[5] = {0.0};
	double cidArray[5] = {0};

	//获取新5个元素的值
	int i = 4;
	for (std::list<std::shared_ptr<Cell>>::reverse_iterator rit = his_cell.rbegin();rit != his_cell.rend();++rit)
	{
		ctArray[i] = (*rit)->card_stamp_time;
		cidArray[i] = (*rit)->id;
		i--;
		if (i<0)
		{
			break;
		}
	}

	//第五点和前4点拟合的直线方程y = kx + b的k,b
	Fit f;
	f.linearFit(ctArray,cidArray,5,true);
	if (f.getR()!=1)
	{
		double k = f.getSlope();
		double b = f.getIntercept();
		//取得垂直线的k
		/*double k1 = -1.0/k;
		double b1 = cidArray[4] - k1*ctArray[4];*/

		fit_k = k;
		fit_b = b;

		double y = fit_k*ctArray[4] + b;

		/*double x = (b1 - b) /(k + k1);
		double y = k1*x + b1;*/
		//这里的y即为第五点格子id投影到直线上的格子id

		int newCellId = int(y);

		//将新格子id进行更新
		if (newCellId < mpCellPath->find(curCellReaderName)->second.size())
		{
			cell->id = newCellId;
		}else{
			int n = cell->id;
			int b = n;
			n = b;
		}
	}

	return 0;
}

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

			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
					//人卡格子为空，不去保存了！
					//by lsp 2017.07.09 @长治
					//Log::write_log(FILE_TYPE::JSON_S, "person before Position2Cell!!!!!！", true);
					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);
						//保存格子到单元格，用于计算怠速判断
						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实现
						//越界，向前大跳或向后大跳
						//有对应，越界（）
						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;
							}
						}						
					}
				}
			}
		}
	}
	
	if (cell)
	{
		//我要去保存人卡格子了！
		//by lsp 2017.07.09 @长治
		Log::write_log(FILE_TYPE::JSON_S, "I am going to save person cell!!!!!！", true);
		SaveCardAlgoData(cell);
	}else{
		//人卡格子为空，不去保存了！
		//by lsp 2017.07.09 @长治
		Log::write_log(FILE_TYPE::JSON_S, "person cell is null!!!!!！", true);
	}	

	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== 5862
//					cell = Position2Cell(tmp_pos);
//					if (cell && cell->id == 0) 
//					{
//						return 1; //cellId为0，直接走人
//					}
//					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();
//						}
//
//						*tmp_cell = *cell;
//						nStartLocateCounts++;
//					}else{
//						originCellId = 0;
//						originCellReaderName = "";
//					}
//
//					if (!CheckStartRule())
//					{
//						return 1;
//					}
//					
//					if (last_cell && cell)
//					{
//						//怠速
//						if (CheckIdleStatus())
//						{
//							//怠速，取上次结果
//							isIdling = true;
//							acce_cur_state = STATE_ACCE_IDLING;
//							//如果怠速了，则直接取上一次的结果
//							if (last_cell)
//							{
//								cell = last_cell;
//							}
//						} 
//						else
//						{
//							if (last_cell->id == cell->id)
//							{
//								//如果本次定位的格子和上一次的格子id相同，则向前走一格
//								int nCalcStep = 1;
//								cell = GetNextCell(last_cell,this->nIncrease,nCalcStep);
//								if (cell)
//								{
//									cell->isFit = true;
//								}
//							}else{
//								//非怠速
//								int nCounts = 0;
//								if (cell->strSubjectReader == last_cell->strSubjectReader)
//								{
//									nCounts = ceil(abs(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;
//								}
//								//1.大跳，只考虑往前跳
//								if (nCounts >= thre_value)
//								{
//									if (nOverThreCounts < 3)
//									{
//										nOverThreCounts++;
//										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 (isRebound(cell))
//								{
//									//2.回退，5个周期内的都不认
//									//如果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);
//											if (cell)
//											{
//												cell->isFit = true;
//											}
//										}
//										change_cell_dir++;
//									}else{
//										change_cell_dir = 0;
//									}
//								}else{
//									//3.正常运动
//								}	
//							}
//						}
//						//非怠速
//						//先处理回退
//						/*if (isRebound(tmp_pos))
//						{
//							if (this->nIncrease != 0)
//							{
//								int nCalcStep = 0;
//								nCalcStep = ceil(last_cell->nStep / 2);
//								if (nCalcStep < 1)
//								{
//									nCalcStep = 1;
//								}
//								cell = GetNextCell(last_cell,this->nIncrease,nCalcStep);
//								if (cell)
//								{
//									cell->isFit = true;
//								}
//							}	
//						}*/
//
//						////只判断怠速
//						//int nCounts = 0;
//						//if (cell->strSubjectReader == last_cell->strSubjectReader)
//						//{
//						//	nCounts = ceil(abs(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;
//						//	}
//						//}
//					}
//				}
//			}
//		}
//	}
//	
//	if (cell)
//	{
//		SaveCardAlgoData(cell);
//	}	
//
//	return ret;
//}

//拟合度实现方案
//int Card::DiscreteLocate(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;
//	std::shared_ptr<POS> pos = nullptr;
//
//	//1.数据有效性判断
//	ret = CheckDistData(cnt);
//	if(ret){
//		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)
//			{
//				return 1;
//			}
//
//			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)
//			{
//				return 1;
//			}
//
//			bool bRet = false;
//			bRet = LocateAlgorithm::IsOnMap(pos,pTdoaReaderPathMap);
//			if (!bRet)
//			{
//				return 1;
//			}
//			if (card_id == "0020000001023" && time_stamp_cal == 12885)
//			{
//				int aa = 1;
//				aa = his_cell.size();
//			}
//			if (ret)
//			{
//				if (his_cell.size() < 5)
//				{
//					return 1;
//				}
//				//如果从多解中未选出解，则按上一次格子的步进匀速运动
//				int nStep = 1;
//				if (last_cell->nStep > 1)
//				{
//					nStep = last_cell->nStep;
//				}
//				std::shared_ptr<Cell> cell = GetNextCell(last_cell,nIncrease,nStep,nullptr);
//				if (cell)
//				{
//					cell->originId = cell->id;
//				}
//			}else{
//				std::shared_ptr<POS> tmp_pos = std::make_shared<POS>();
//				//获取分站信息
//				bool bRet = false;
//				*tmp_pos = *pos;
//
//				if (ret == 0)
//				{	
//					//将坐标转为格子信息
//					cell = Position2Cell(tmp_pos);
//					if (cell)
//					{
//						originCellId = cell->id;
//						originCellReaderName = cell->strSubjectReader;
//						cell->card_stamp_time = time_stamp_cal;
//						cell->originId = cell->id;
//						cell->strOriginReaderName = cell->strSubjectReader;
//						nStartLocateCounts++;
//
//						if (lvCells.size() >= MAX_FIT_DATA_COUNTS)
//						{
//							lvCells.pop_front();
//						}
//						lvCells.push_back(cell);
//
//						if (!is_algo_first_location)
//						{
//							double degree = 0;
//							degree = GetFittingDegree(lvCells);
//
//							bool bOverThre = false;
//							bool isDirectOutput = false;	//是否直接输出数据
//
//							if (degree - 0.9 > ZERO_PRECISION)
//							{
//								bOverThre = false;
//								isDirectOutput = true;
//								pos->update = true;
//							} else if ((degree - 0.7) > ZERO_PRECISION && (0.9 - degree) > ZERO_PRECISION)
//							{
//								//向前大跳
//								bOverThre = true;
//								lvCells.pop_back();
//							}else if ((degree - 0.5) > ZERO_PRECISION &&(0.7 - degree) > ZERO_PRECISION)
//							{
//								bOverThre = false;
//							}
//							else if(0.5 - degree > ZERO_PRECISION)
//							{
//								//向后大跳
//								bOverThre = true;
//								lvCells.pop_back();
//							}
//
//							if (!isDirectOutput)
//							{
//								if (bOverThre)
//								{
//									//根据之前的拟合方程k,b进行补格子
//									int tmpOriginId = 0;
//									std::string tmpOriginReaderName = "";
//									tmpOriginId = cell->originId;
//									tmpOriginReaderName = cell->strOriginReaderName;
//
//									//根据上一次数据计算格子数，并根据上一次方向进行偏移
//									double y = 0;
//									std::list<std::shared_ptr<Cell>>::iterator it = lvCells.begin();
//
//									y = fit_k*(this->time_stamp_cal - (*it)->card_stamp_time + 1) + fit_b;
//
//									int nCellId  = 0 ;
//									std::string strReaderName = "";
//									//判断两次格子的id是否在相同分站内，不在走跨分站逻辑
//									if ((*it)->strOriginReaderName != cell->strOriginReaderName)
//									{
//										int nRealId = 0;
//										nCellId = LocateAlgorithm::round(y);
//										//换基站了
//										int first_min = (*it)->minReaderId;		//first_cell_temp对应的最小分站
//										int first_max = (*it)->maxReaderId;		//first_cell_temp对应的最大分站
//										int cell_min = cell->minReaderId;		//cell对应的最小分站
//										int cell_max = cell->maxReaderId;		//cell对应的最大分站
//
//										if(first_min == cell_min){	
//											if (nCellId == 0)
//											{
//												nRealId = 1;
//												strReaderName = cell->strSubjectReader;
//											}else if(nCellId > 0){
//												nRealId = nCellId;
//												strReaderName = (*it)->strSubjectReader;
//											}else if(nCellId < 0){
//												nRealId = abs(nCellId);
//												strReaderName = cell->strSubjectReader;
//											}
//										}
//										if(first_min == cell_max){	
//											if (nCellId == 0)
//											{
//												nRealId = mpCellPath->find(cell->strSubjectReader)->second.size();
//												strReaderName = cell->strSubjectReader;
//											}else if(nCellId > 0){
//												nRealId = nCellId;
//												strReaderName = (*it)->strSubjectReader;
//											}else if(nCellId < 0){
//												nRealId = mpCellPath->find(cell->strSubjectReader)->second.size() - abs(nCellId);
//												strReaderName = cell->strSubjectReader;
//											}
//										}
//										if(first_max == cell_min){	
//											//只存在nCellId>0的情况，但还要区分它是否大于所在分站的总格子数
//											if (nCellId >= mpCellPath->find((*it)->strSubjectReader)->second.size())
//											{
//												nRealId = nCellId - mpCellPath->find(cell->strSubjectReader)->second.size();
//												if (nRealId == 0)
//												{
//													nRealId = 1;
//												}
//												strReaderName = cell->strSubjectReader;
//											}else if(nCellId < mpCellPath->find((*it)->strSubjectReader)->second.size()){
//												nRealId = nCellId;
//												strReaderName = (*it)->strSubjectReader;
//											}
//										}
//										if(first_max == cell_max){
//											if (nCellId >= mpCellPath->find((*it)->strSubjectReader)->second.size())
//											{
//												nRealId = mpCellPath->find(cell->strOriginReaderName)->second.size() - (nCellId - mpCellPath->find((*it)->strSubjectReader)->second.size());
//												strReaderName = cell->strOriginReaderName;
//											}else if(nCellId < mpCellPath->find((*it)->strSubjectReader)->second.size()){
//												nRealId = nCellId;
//												strReaderName = (*it)->strSubjectReader;
//											}
//										}
//										nCellId = nRealId;
//									}else{
//										nCellId = LocateAlgorithm::round(y);
//										strReaderName = cell->strOriginReaderName;
//									}
//									//将新格子的信息除了格子id信息
//									cell = FindCell(strReaderName,nCellId);
//									if (cell)
//									{
//										cell->originId = tmpOriginId;
//										cell->strOriginReaderName = tmpOriginReaderName;
//									}
//								} 
//								else
//								{
//									colourfulCloudsChasingTheMoon(cell,his_cell);
//								}
//							}else{
//								//直接输出数据需要放入拟合队列中
//								colourfulCloudsChasingTheMoon(cell,his_cell);
//							}
//						}else{
//							//如果是第一次定位结果，调用以下函数将cell放入到his_cell队列中
//							if (his_cell.size() > 0)
//							{
//								his_cell.erase(his_cell.begin(),his_cell.end());
//								fit_k = fit_b = 0;
//							}
//							colourfulCloudsChasingTheMoon(cell,his_cell);
//						}
//					}else{
//						originCellId = 0;
//						originCellReaderName = "";
//					}
//				}
//			}
//		}
//	}
//
//	if (cell)
//	{
//		//做加速度处理
//		double dAcce = 0;
//		bool bRet = false;
//		if (abs(fit_k - last_k) < ZERO_PRECISION)
//		{
//			bRet = true;
//		}else{
//			dAcce = abs((fit_k - last_k)/(delta_ct))*1.2;
//			if (dAcce > 10)
//			{
//				bRet = false;
//			}else{
//				bRet = true;
//			}
//		}
//
//		if (bRet)
//		{
//			SaveCardAlgoData(cell);
//		}
//		//SaveCardAlgoData(cell);
//	}	
//
//	return ret;
//}

//只有通过加速度判断丢失数据部分
int Card::DiscreteLocate(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;
	std::shared_ptr<POS> pos = nullptr;

	//1.数据有效性判断
	ret = CheckDistData(cnt);
	if(ret){
		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)
			{
				return 1;
			}

			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)
			{
				return 1;
			}

			bool bRet = false;
			bRet = LocateAlgorithm::IsOnMap(pos,pTdoaReaderPathMap);
			if (!bRet)
			{
				return 1;
			}
			if (card_id == "0020000001061" && time_stamp_cal == 11049)
			{
				int aa = 1;
				aa = his_cell.size();
			}
			if (ret)
			{
				if (his_cell.size() < 5)
				{
					return 1;
				}
				//如果从多解中未选出解，则按上一次格子的步进匀速运动
				int nStep = 1;
				if (last_cell->nStep > 1)
				{
					nStep = last_cell->nStep;
				}
				std::shared_ptr<Cell> cell = GetNextCell(last_cell,nIncrease,nStep,nullptr);
				if (cell)
				{
					cell->originId = cell->id;
				}
			}else{
				std::shared_ptr<POS> tmp_pos = std::make_shared<POS>();
				//获取分站信息
				bool bRet = false;
				*tmp_pos = *pos;

				if (ret == 0)
				{	
					//将坐标转为格子信息
					cell = Position2Cell(tmp_pos);
					if (cell)
					{
						originCellId = cell->id;
						originCellReaderName = cell->strSubjectReader;
						cell->card_stamp_time = time_stamp_cal;
						cell->originId = cell->id;
						cell->strOriginReaderName = cell->strSubjectReader;
						nStartLocateCounts++;

						if (lvCells.size() >= MAX_FIT_DATA_COUNTS)
						{
							lvCells.pop_front();
						}
						lvCells.push_back(cell);

						colourfulCloudsChasingTheMoon(cell,his_cell);
					}else{
						originCellId = 0;
						originCellReaderName = "";
					}
				}
			}
		}
	}

	if (cell)
	{
		//做加速度处理
		double dAcce = 0;
		bool bRet = false;
		if (abs(fit_k - last_k) < ZERO_PRECISION)
		{
			bRet = true;
		}else{
			dAcce = abs((fit_k - last_k)/(delta_ct))*1.2;
			if (dAcce > 10)
			{
				bRet = false;
				if(his_cell.size() > 0){
					his_cell.pop_back();
				}
				if (lvCells.size())
				{
					lvCells.pop_back();
				}
				return 1;
			}else{
				bRet = true;
			}
		}

		if (bRet)
		{
			SaveCardAlgoData(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 = "";

	std::mutex mu;
	std::lock_guard<mutex> lg(mu);

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

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

		CellPathMap::iterator it_cpm = mpCellPath->end();
		switch (increase)
		{
		case 1:
			nNewCellId = lastCell->id + nIndex;
			it_cpm = mpCellPath->find(lastCell->strSubjectReader);
			if (it_cpm == mpCellPath->end())
			{
				return nullptr;
			}
			//if (nNewCellId > mpCellPath->find(lastCell->strSubjectReader)->second.size())
			if (nNewCellId > it_cpm->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;

	std::string strFindReader = "";
	strFindReader = lastCell->strSubjectReader;

	if (strFindReader == "")
	{
		return last_cell;
	}

	int nSize = 0;
	nSize = mpCellPath->find(strFindReader)->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(strFindReader)->second.begin();it != mpCellPath->find(strFindReader)->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;
			cell->interval_time = lastCell->interval_time;
			bFind = true;
			break;
		}
	}

	if (!bFind)
	{
		return nullptr;
	}

	return cell;
}

//根据分站名称和格子id查找出格子的其他所有信息
std::shared_ptr<Cell> Card::FindCell(std::string strReaderName,int cellid)
{
	std::shared_ptr<Cell> cell = std::make_shared<Cell>();

	if (strReaderName == "" || cellid == 0)
	{
		return nullptr;
	}

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

	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;
	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
	{
		//相等的话就保持原来的方向
	}

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

int Card::ChangeSystemTimeBySecond(SYSTEMTIME& st)
{
	//只考虑到小时
	int nMs = st.wMilliseconds;
	int nSecond = st.wSecond;
	int nMinute = st.wMinute;
	int nHour = st.wHour;

	//毫秒数保持不变
	nMs = st.wMilliseconds;

	//如果秒大于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;
	}			

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

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

	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)
{
	int nRet = 0;
	if (ranging_type == LOCATEDATATYPE::LDT_TDOA)
	{
		nRet = SaveTdoaData(pos);
	}else if (ranging_type == LOCATEDATATYPE::LDT_TOF)
	{
		nRet = SaveTofData(pos);
	}

	return nRet;
}

int Card::SaveTofData(const std::shared_ptr<POS> pos)
{
	//if(tmpPos->posx != 0 && tmpPos->posy != 0){
	//if((abs(pos->posx) < ZERO_PRECISION && abs(pos->posy) < ZERO_PRECISION) || pos->posx == INVALID_COORDINATE && pos->posy == INVALID_COORDINATE){
	if(!CheckPositionValid(pos)){
		return 1;
	}else{		
		double newPosX = pos->cx;
		double newPosY = pos->cy;

		if (is_algo_first_location)
		{
			is_algo_first_location = false;	
		}else{
			this->m_nStream = GetStream(last_locate.x,last_locate.y,pos->cx,pos->cy);
			double d = sqrt(pow(newPosX - this->last_locate.x,2) + pow(newPosY - this->last_locate.y,2))*this->map_scale;
			pos->delta_time = difftime(p_reader->reader_time,time_last_tof);
			this-> v = d / pos->delta_time;	//速度计算有问题，因为delta_time时间问题
		}
		time_last_tof = p_reader->reader_time;

		this->x = this->last_locate.x = newPosX;
		this->y = this->last_locate.y = newPosY;
		this->z = 0;
		this->t = this->last_locate.t = time_stamp_cal;		//difT

		this->is_deal_by_algo = true;
	}

	return 0;
}

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

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

		this->algo_calc_offset();

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

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

		this->m_nMoveDirection = nSign;

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

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

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

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

	if (is_algo_first_location)
	{
		is_algo_first_location = false;
	}

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

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

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

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

	//拟合点作为参考点
	//拟合数据的计算
	if (his_pos.size() == REF_POSITION_NUM)
	{
		his_pos.pop_front();
	}
	his_pos.push_back(p);
	
	int nRef = REF_POSITION_NUM * 4;
	if (long_his_pos.size() == nRef)
	{
		long_his_pos.pop_front();
	}
	long_his_pos.push_back(p);
	
	if(is_ref_pos)
	{
		//保存历史数据
		std::shared_ptr<POS> p = std::make_shared<POS>();
		p->posx = this->x*map_scale;
		p->posy = this->y*map_scale;
		p->card_count = this->time_stamp_cal;

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

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

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

	return 0;
}

/*
 * 作用：保存算法计算结果数据
 *
 * 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 (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;

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

	if (is_algo_first_location)
	{
		//第一次定位，将定位时间的毫秒数改为500或者0
		::GetLocalTime(&cell->interval_time);
		if (cell->interval_time.wMilliseconds < 500)
		{
			cell->interval_time.wMilliseconds = 0;
		} 
		else
		{
			cell->interval_time.wMilliseconds = 500;
		}
	}else{
		if (card_type == CT_VEHICLE && last_cell && CFunctions::CompareSystemTime(cell->interval_time,last_cell->interval_time) != 0)
		{
			cell->interval_time = last_cell->interval_time;
			ChangeSystemTime(cell->interval_time);
		}
		//下面判断是人卡，并且指向上一个cell的指针存在，并且
		//if (card_type == CT_PERSON)
		//{
		//	::GetLocalTime(&cell->interval_time);
		//	ChangeMsTimeToZeroOrFive(cell->interval_time); //ms四舍五入
		//	//如果与上一个格子的时刻是一样的，那么这次的cell就没有必要插入了，直接舍弃
		//	if( last_cell && CFunctions::CompareSystemTime(cell->interval_time,last_cell->interval_time) == 0){	
		//		return 1; //这个格子舍弃，直接返回，返回错误值
		//	}
		//}
	}

	if (cell)
	{
		if (last_cell == nullptr)
		{
			last_cell = std::make_shared<Cell>();
		}
		::GetLocalTime(&cell->deal_time);
		*last_cell = *cell;
		locate_cells.push_back(last_cell);
	}

	//std::mutex mu;
	//mu.lock();
	//if (this->nStartLocateCounts == 5)
	//{
	//	::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){
	//	//刘思平修订的代码Part 1/Sum 1:begin
	//	//以下代码区分了人卡和车卡的不同，车卡是固定500ms时隙的，而人卡不会补时隙，也就是说只会将当前时刻的毫秒四舍五入为0ms或者500ms
	//	//这个地方要修改，只是针对车卡的
	//	//by lsp on 2017.07.09 @山西长治
	//	if (card_type == CT_VEHICLE && last_cell && CFunctions::CompareSystemTime(cell->interval_time,last_cell->interval_time) != 0)
	//	{
	//		cell->interval_time = last_cell->interval_time;
	//		ChangeSystemTime(cell->interval_time);
	//	}
	//	//下面判断是人卡，并且指向上一个cell的指针存在，并且
	//	if (card_type == CT_PERSON)
	//	{
	//		::GetLocalTime(&cell->interval_time);
	//		ChangeMsTimeToZeroOrFive(cell->interval_time); //ms四舍五入
	//		//如果与上一个格子的时刻是一样的，那么这次的cell就没有必要插入了，直接舍弃
	//		if( last_cell && CFunctions::CompareSystemTime(cell->interval_time,last_cell->interval_time) == 0){
	//			mu.unlock();		
	//			return 1; //这个格子舍弃，直接返回，返回错误值
	//		}
	//	}	
	//}

	//if (cell)
	//{
	//	if (last_cell == nullptr)
	//	{
	//		last_cell = std::make_shared<Cell>();
	//	}
	//	*last_cell = *cell;
	//	locate_cells.push_back(last_cell);
	//}else
	//	return 1;
	//mu.unlock();

	//if (!is_algo_first_location)
	//{
	//	//确保第一次不会进来算速度
	//	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/1000.0;
	//		int nCounts = ceil(distance/CELL_WIDTH);
	//		this->v = nCounts*CELL_WIDTH/(SEND_CELL_INTERVAL/1000.0);
	//		this->v*=3.6;
	//	}
	//}
	this->v = fit_k*0.6*3.6;

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

	if (!is_algo_first_location )
	{
		this->m_nStream = GetStream(this->last_locate.x,this->last_locate.y,this->x,this->y);
	}

	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;

	if (is_algo_first_location)
	{
		is_algo_first_location = false;
	}

	//反向取MAX_FIT_DATA_COUNTS个元素
	double val[MAX_FIT_DATA_COUNTS] = {0};
	int 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 (pos)
	{
	}

	return 0;
}

//刘思平修订的代码Part 2/Sum 2:
//以下函数将当前时刻的毫秒四舍五入为0ms或者500ms
//by lsp on 2017.07.09 @山西长治
int Card::ChangeMsTimeToZeroOrFive(SYSTEMTIME& st)
{
	int nMs = st.wMilliseconds;

	//如果毫秒数大于1000，向秒进一格
	if (nMs < 500)
	{
		nMs = 0;
	}else{
		nMs = 500;
	}

	st.wMilliseconds = nMs;

	return 0;
}
//代码到此修改完毕
//刘思平修订的代码Part 2/Sum 2

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

			cv = cv*3.6;
			//速度的限制
			if(fabs(cv) > MAX_VECHILE_SPEED){
				p->reason = CHECK_VEHICLE_OVER_SPEED;
				p->posx = INVALID_COORDINATE;
				p->posy = INVALID_COORDINATE;
				p->posz = INVALID_COORDINATE;
				p->cx = p->posx / map_scale;
				p->cy = p->posy / map_scale;
				p->cz = p->posz / map_scale;

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

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

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

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

	return 0;
}

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

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

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

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

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

	return true;
}

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

/*
 * 判断是否为怠速状态
 *
 * param
 *		pos			定位结果
 *
 * return
 *		如果怠速则返回true，否则返回false
 *
*/
bool Card::IsIdleStatus(std::shared_ptr<POS> pos)
{
	if (idle_pos_list.size() < REF_POSITION_NUM)
	{
		return false;
	}

	bool bRet = false;

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

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

		ax /= REF_POSITION_NUM;
		ay /= REF_POSITION_NUM;
	}

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

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

	return bRet;
}

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

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

	int nStream = 0;

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

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

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

	return true;
}

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

	bool bRet = false;

	double delta = difftime(p_reader->reader_time,time_last_tof);

	if (delta < ZERO_PRECISION)
	{
	}else{
		double distance = 0;
		distance = sqrt(pow(pos->posx - last_locate.x*map_scale,2) + pow(pos->posy - last_locate.y*map_scale,2));
		double a = 0;
		a = distance / pow(delta,2);
		if (a > 5)
		{
			bRet = false;
		}else{
			pos->cvx = pos->posx - last_locate.x*map_scale / delta;
			pos->cvy = pos->posy - last_locate.y*map_scale / delta;
			bRet = true;
		}

		double v = 0;
		v = distance/delta;
		if (v > 3)
		{
			bRet = false;
		}
	}

	return bRet;
}

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

	return nStream;
}

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

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

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

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

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

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

	return 0;
}

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

	return 0;
}

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

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

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

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

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

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

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

		return 0;
	}

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

	return 0;
}

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

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

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

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

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

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

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

	return dt;
}

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

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

MapInfo::MapInfo( void )
{

}

MapInfo::~MapInfo( void )
{

}

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

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

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

Adhoc::~Adhoc()
{
}

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

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

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

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

Chamber::~Chamber()
{

}

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

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

	return 0;
}

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

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

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

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

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

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

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

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

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

	return bIsHasEmptyChamber;
}

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

	nTotals = mp_vehicle_list.size();

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

	return m_nState;
}

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

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

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

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

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

	return bRet;
}

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

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

	return bRet;
}

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

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

PatrolTask::~PatrolTask()
{
}

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

	duration_last = duration_stay_min = duration_stay_max = duration_ranging = 0;
}

PatrolPoint::~PatrolPoint()
{
}

BanShift::BanShift()
{

}

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

BanShift::~BanShift()
{
}

/*
// 模仿matlab线性拟合工具，返回y=ax+b，并且输出R:相关系数，0不相关，1很相关
// 返回值代表：是否怠速静止
// by lsp on 07.10.2017 @山西长治
*/
bool Card::cftool(float *data_x, float *data_y, int data_n, std::vector<float> &vResult)
{
	//如果数组中所有元素相等，那么直接宣布是怠速
	int flag = 0;       // flag 表示是否遇到了不相等的元素
	int i;
	for (i = 1; i < data_n; i++) {
		if (data_y[i] != data_y[0]) {  // 遇到了不等于x[0]的元素，设置 flag = 1，然后跳出循环
			flag = 1; break;
		}
	}
	if (flag == 1) {        // 输出判断结果
		printf("不相等，继续调用线性拟合方法判断是否是怠速\n");
	} else {
		return true;
	}

	float A = 0.0;
	float B = 0.0;
	float C = 0.0;
	float D = 0.0;
	float E = 0.0;
	float F = 0.0;

	for (int i=0; i<data_n; i++)
	{
		A += data_x[i] * data_x[i];
		B += data_x[i];
		C += data_x[i] * data_y[i];
		D += data_y[i];
	}

	// 计算斜率a和截距b
	float a, b, temp = 0;
	if( temp = (data_n*A - B*B) )// 判断分母不为0
	{
		a = (data_n*C - B*D) / temp;
		b = (A*D - B*C) / temp;
	}
	else
	{
		a = 1;
		b = 0;
	}

	// 计算相关系数r
	float Xmean, Ymean;
	Xmean = B / data_n;
	Ymean = D / data_n;

	float tempSumXX = 0.0, tempSumYY = 0.0;
	for (int i=0; i<data_n; i++)
	{
		tempSumXX += (data_x[i] - Xmean) * (data_x[i] - Xmean);
		tempSumYY += (data_y[i] - Ymean) * (data_y[i] - Ymean);
		E += (data_x[i] - Xmean) * (data_y[i] - Ymean);
	}
	F = sqrt(tempSumXX) * sqrt(tempSumYY);

	float r;
	r = E / F;

	vResult.push_back(a);
	vResult.push_back(b);
	vResult.push_back(r*r);

	fit_r = vResult.at(2);

	if(vResult.at(2) < 0.7)
		return true;
	else
		return false;	
}

/*
//by lsp on 2017.07.10 @山西长治高河调度室
//by lsp on 2017.07.10 @山西长治
//让上帝来决定下一个格子怎么走吧！
//int返回值：1表示失败，并且返回的cell也会为空
//           0表示成功，并且会返回正确位置的cell
//给我一个格子
*/
int Card::colourfulCloudsChasingTheMoon(std::shared_ptr<Cell> &cell, std::list<std::shared_ptr<Cell>> &myCellList){
	try{
		if (cell && cell->id != 0)
		{
			//入队列，用的algorithm文件中的MAX_FIT_DATA_COUNTS常量，现在定义的是5
			if (myCellList.size() >= MAX_FIT_DATA_COUNTS)
			{
				myCellList.pop_front();
				myCellList.push_back(cell); //格子入栈
			}else{
				//只入栈
				myCellList.push_back(cell); //格子入栈
				return 1;
			}
			float* pX = new float[MAX_FIT_DATA_COUNTS];
			float* pY = new float[MAX_FIT_DATA_COUNTS];
			memset(pX, 0, MAX_FIT_DATA_COUNTS*sizeof(float));
			memset(pY, 0, MAX_FIT_DATA_COUNTS*sizeof(float));
			size_t i = 0;
			if (myCellList.size() == MAX_FIT_DATA_COUNTS){
				//构造his_cell中的x, y队列
				//x用ct值来表示
				//ct存在：cell->card_stamp_time				
				float step; //把坐标往前挪
				std::list<std::shared_ptr<Cell>>::iterator first_cell = myCellList.begin(); //后续总是跟第一个点比较距离
				for (std::list<std::shared_ptr<Cell>>::iterator it = myCellList.begin(); it != myCellList.end() ; ++it)
				{
					if(it == first_cell){
						pY[i] = (*it)->id; //第一个值，当然直接入数组，而且后面的值都要向它看齐
						
					}else{
						if ((*first_cell)->strSubjectReader == (*it)->strSubjectReader){
							pY[i] = (*it)->id; //跟前者在相同基站之间							
						}else
						{
							int last_min = (*first_cell)->minReaderId;	//last_cell_temp对应的最小分站
							int last_max = (*first_cell)->maxReaderId;	//last_cell_temp对应的最大分站
							int cell_min = (*it)->minReaderId;			//cell对应的最小分站
							int cell_max = (*it)->maxReaderId;			//cell对应的最大分站
							if(last_min == cell_min){
								//pY[i] = pY[i - 1] + pY[i - 1] - 1 + (*it)->id; //换基站了	
								pY[i] = 0 - (*it)->id; 
							}
							if(last_min == cell_max){
								//pY[i] = pY[i - 1] + pY[i - 1] - 1 + mpCellPath->find((*it)->strSubjectReader)->second.size() - (*it)->id + 1; //换基站了
								int nSize = 0;
								nSize = mpCellPath->find((*it)->strSubjectReader)->second.size();
								int nDiff = (*it)->id - nSize;
								float a = nDiff*1.0;
								pY[i] = a;
								//pY[i] = (*it)->id - mpCellPath->find((*it)->strSubjectReader)->second.size();
							}
							if(last_max == cell_min){
								pY[i] = mpCellPath->find((*first_cell)->strSubjectReader)->second.size()  + (*it)->id; //换基站了	
							}
							if(last_max == cell_max){
								pY[i] = mpCellPath->find((*first_cell)->strSubjectReader)->second.size()  + mpCellPath->find((*it)->strSubjectReader)->second.size() - (*it)->id + 1; //换基站了	
							}
						}
					}					
					
					//设置横坐标
					if(i == 0){
						pX[i] = 1;	
						step = (*it)->card_stamp_time - 1;						
					}else{
						if((*it)->card_stamp_time - step < 0){
							pX[i] = (*it)->card_stamp_time - step + 65535;							
						}else{
							pX[i] = (*it)->card_stamp_time - step;
						}											
					}
					i++;
				}
				//上面构造完毕，检测其线性相关性
				std::vector<float> vResult;
				vResult.resize(0);
				//不做怠速判断
				bool cft = cftool(pX, pY, MAX_FIT_DATA_COUNTS, vResult);
				cft = false;
				float k = 0.0;
				k = vResult.at(0);
				float b = 0.0;
				b = vResult.at(1);

				last_k = 0;
				last_k = fit_k;
				std::list<std::shared_ptr<Cell>>::reverse_iterator it_cell = myCellList.rbegin();
				int first_ct = 0, second_ct = 0;
				if (it_cell != myCellList.rend())
				{
					first_ct = (*it_cell)->card_stamp_time;
					++it_cell;
					if (it_cell != myCellList.rend())
					{
						second_ct = (*it_cell)->card_stamp_time;
					}
				}
				
				delta_ct = first_ct - second_ct;

				fit_k = k;
				fit_b = b;

				float Relation = 0.0;
				Relation = vResult.at(2);
				//如果是怠速，则当前cell的真实id=idlastcell的真实id
				if(cft){
					//如果怠速了，则直接取上一次的结果
					Log::write_log(FILE_TYPE::SQL_S, "be quiet now!!!!original id: " + CFunctions::int2string(cell->id) + "  realId: "+ CFunctions::int2string(cell->realId) + "  cardid: " + this->card_id, true);
					if (last_cell)
					{
						cell->realId = last_cell->realId; //据说是复制赋值
					}else{
						cell->realId = cell->id; //第一次没有last_cell，直接原始值为实际realId
					}
				}else{
					std::list<std::shared_ptr<Cell>>::reverse_iterator end_cell = myCellList.rbegin();
					//int realId = floor(pY[0] * k + b); //考虑到有可能跨基站，处理下形成realId
					int realId = floor(pX[4] * k + b);
					std::string strReaderName = "";
					std::string tmpOriginReaderName = "";
					int tmpOriginId = 0;
					tmpOriginId = cell->originId;
					tmpOriginReaderName = cell->strOriginReaderName;
					if ((*first_cell)->strSubjectReader == (*end_cell)->strSubjectReader){
							cell->realId = realId; //跟前者在相同基站之间	
							strReaderName = (*first_cell)->strSubjectReader;
						}else
						{
							//换基站了
							int first_min = (*first_cell)->minReaderId; //first_cell_temp对应的最小分站
							int first_max = (*first_cell)->maxReaderId; //first_cell_temp对应的最大分站
							int cell_min = (*end_cell)->minReaderId;	//cell对应的最小分站
							int cell_max = (*end_cell)->maxReaderId;	//cell对应的最大分站
							if(first_min == cell_min){
								//cell->realId = realId - pY[0]; 	
								if (realId == 0)
								{
									cell->realId = 1;
									strReaderName = (*end_cell)->strSubjectReader;
								}else if(realId > 0){
									cell->realId = realId;
									strReaderName = (*first_cell)->strSubjectReader;
								}else{
									cell->realId = abs(realId);
									strReaderName = (*end_cell)->strSubjectReader;
								}
							}
							if(first_min == cell_max){
								if (realId == 0)
								{
									cell->realId = mpCellPath->find((*end_cell)->strSubjectReader)->second.size();
									strReaderName = (*end_cell)->strSubjectReader;
								}else if(realId > 0){
									//cell->realId = mpCellPath->find((*end_cell)->strSubjectReader)->second.size() - (realId - pY[0]) + 1; 
									cell->realId = realId;
									strReaderName = (*first_cell)->strSubjectReader;
								}else{
									cell->realId = mpCellPath->find((*end_cell)->strSubjectReader)->second.size() + realId;
									strReaderName = (*end_cell)->strSubjectReader;
								}
							}
							if(first_max == cell_min){
								//cell->realId = realId - (mpCellPath->find((*first_cell)->strSubjectReader)->second.size() - pY[0]); 	
								//cell->realId = realId - mpCellPath->find((*first_cell)->strSubjectReader)->second.size();
								//strReaderName = (*first_cell)->strSubjectReader;
								if (realId >= mpCellPath->find((*first_cell)->strSubjectReader)->second.size())
								{
									cell->realId = realId - mpCellPath->find((*first_cell)->strSubjectReader)->second.size();
									if (cell->realId == 0)
									{
										cell->realId = 1;
									}
									strReaderName = (*end_cell)->strSubjectReader;
								}else{
									cell->realId = mpCellPath->find((*first_cell)->strSubjectReader)->second.size() - realId;
									strReaderName = (*first_cell)->strSubjectReader;
								}
							}
							if(first_max == cell_max){
								//cell->realId = (mpCellPath->find((*end_cell)->strSubjectReader))->second.size() - (realId - ((mpCellPath->find((*first_cell)->strSubjectReader))->second.size() - pY[0])) + 1;
								//strReaderName = (*end_cell)->strSubjectReader;
								if (realId >= mpCellPath->find((*first_cell)->strSubjectReader)->second.size())
								{
									cell->realId = mpCellPath->find((*end_cell)->strSubjectReader)->second.size() - (realId - mpCellPath->find((*first_cell)->strSubjectReader)->second.size());
									strReaderName = (*end_cell)->strSubjectReader;
								} 
								else
								{
									cell->realId = realId;
									strReaderName = (*first_cell)->strSubjectReader;
								}
							}
						}	
						cell = FindCell(strReaderName,cell->realId);
						if (cell)
						{
							cell->originId = tmpOriginId;
							cell->strOriginReaderName = tmpOriginReaderName;
						}
				}
			}else{
				cell = nullptr;
				return 1; //1表示失败，并且返回的cell也会为空
			}		
		}
		return 0; //表示成功
	}catch (...)
	{
		return 1;
	}	
}


//int Card::DiscreteLocate(int cnt)
//{
//	try{
//		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){
//			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)
//				{
//					return 1;
//				}
//
//				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);
//
//				if (ret)
//				{
//					return 1;
//				}else{
//					std::shared_ptr<POS> tmp_pos = std::make_shared<POS>();
//					//获取分站信息
//					bool bRet = false;
//					*tmp_pos = *pos;
//
//					if (ret == 0)
//					{
//						//这里的cell->id是originalId
//						cell = Position2Cell(tmp_pos);
//						//从测试数据中读cell，送给彩云追月形成输出
//						//by lsp on 2017.07.15 @北京林萃公寓						
//						//his_cell = nullptr;
//						//int oiginalId[10] = {1023, 1016, 998, 988, 979, 974, 966, 960, 949, 938}; //原始cellId
//						//int ct[10] ={9859, 9860, 9863, 9865, 9866, 9867, 9868, 9869, 9870, 9872}; //对应的ct号码
//						//int firstStation[10] = {201, 201, 201, 201, 201, 201, 201, 201, 201, 201}; 
//						//int endStation[10] = {202, 202, 202, 202, 202, 202, 202, 202, 202, 202};
//						//std::list<std::shared_ptr<Cell>> myCellList;
//						//myCellList.resize(0);
//						//for (int i=0; i < 10; i++)
//						//{
//						//	//初始化格子
//						//	std::shared_ptr<Cell> cell = std::make_shared<Cell>();
//						//	cell->id = oiginalId[i];
//						//	cell->card_stamp_time = ct[i];
//						//	cell->minReaderId = firstStation[i];
//						//	cell->maxReaderId = endStation[i];
//						//	cell->strSubjectReader = CFunctions::getCellName(firstStation[i], endStation[i]);
//						//	//调用彩云追月来处理
//						//	if(colourfulCloudsChasingTheMoon(cell, myCellList) == 0){
//						//		//输出结果
//						//		Log::write_log(FILE_TYPE::SQL_S, "ct: " + CFunctions::int2string(cell->card_stamp_time) + " station: " + cell->strSubjectReader + " original id: " + CFunctions::int2string(cell->id) + "  realId: "+ CFunctions::int2string(cell->realId) + "  cardid: " + this->card_id, true);
//						//	}
//						//}
//						//Log::write_log(FILE_TYPE::JSON_S, this->card_id + ": have postion2cell, go now!!!!!!!!!!!!!!!", true);
//						//if (cell && cell->id == 0) 
//						//{						
//						//	//问题是下面这一步不可能走过来，只有500ms补的地方才会出现originalId为0
//						//	Log::write_log(FILE_TYPE::JSON_S, this->card_id + ":my cell Id is Zero, so I have to go out!!!!!!!!!!!!!!!", true);
//						//	return 1; //cellId为0，直接走人
//						//}else{
//						//	if(colourfulCloudsChasingTheMoon(cell) && cell != nullptr){
//						//		Log::write_log(FILE_TYPE::SQL_S, "original id: " + CFunctions::int2string(cell->id) + "  realId: "+ CFunctions::int2string(cell->realId) + "  cardid: " + this->card_id, true);
//						//		SaveCardAlgoData(cell);					
//						//	}
//						//}					
//					}
//				}
//			}
//		}
//		return ret;
//	}catch (...)
//	{
//		return 1;
//	}	
//}

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

LeaderArrange::~ LeaderArrange()
{
}

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

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

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

void VehicleTypeAttRule::AddArea(int area_id)
{
	std::vector<int>::iterator itArea = std::find(this->vecArea.begin(), this->vecArea.end(), area_id);
	if(itArea == this->vecArea.end())
	{
		this->vecArea.push_back(area_id);
	}
}