#include "stdafx.h"
#include "classdef.h"
#include "constdef.h"
#include "structdef.h"
#include "locate_algorithm.h"

#include <math.h>

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

	card_type = cardtype;
	card_id = cardid;
	coor_offset_x = offset_x;
	coor_offset_y = offset_y;
	this->z_offset = z_offset;
	
	GetSystemTime(&deal_time);
	//GetSystemTime(&down_time);
	down_time = up_time = enter_area_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);

	x =	y = z = last_x = last_y = last_z = stored_x = stored_y = stored_z = 0;
	xx = yy = zz = 0;
	x1 = y1 = z1 = x2 = y2 = z2 = x3 = y3 = z3 = x4 = y4 = z4 = 0;
	a = 0;
	t = 0;
	init_postion = false;
	is_first_location = true;

	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 = DT_INIT;
	pos_state_park = pos_state_park_old = AAT_ENTER;
	pos_state_count = pos_state_park_count = 0;
	dept_id = group_id = 0;
	power_state = power_state_last = 0;

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

	sync_num = 0;

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

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

	reader_tickcount = time_stamp = time_stamp_last = 0;

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

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

	m_nCalcSyncNum = 0;
	m_nSyncNumInList = 0;
	last_locate.tt = 0;
	last_locate.a = last_locate.antenna_id = last_locate.reader_id = last_locate.t = 0;
	last_locate.x = last_locate.y = last_locate.z = last_locate.d = last_locate.d_offset = last_locate.v = 0.0;

	m_bUseFilter = false;
	m_nFilterType = NO_FILTER;
	m_pKalmanFilter = NULL;
	m_nLastLocateT = 0;
	origin_locate.x = origin_locate.y = origin_locate.z = origin_locate.v = 0.0;
	last_vx = last_vy = 0.0;
	v = 0.0;
	ins_weight = INS_WEIGHT;
	uwb_weight = UWB_WEIGHT;
	acce_cur_state = 0;
	acce_last_state = 0;
	b_long_interval = false;
	accelerate_state = accelerate_state_last = ACCELERATE_INIT_STATE;	//默认初始的为-10
	ins_direction = 0;
	direction = 0;
	antenna_angle = antenna_angle_last = 0;

	time_stamp_last = 0;
	reader_id_last  = 0;

	_dists.swap(DistQueMap());
	//m_pKalmanFilter = new CKalmanFilter();
	//m_pKalmanFilter->Initial(1);
	//m_pKalmanFilter->m_bFlag = false;
}

Card::Card( void )
{
	
}

//void Card::set_pos_state( int val )
//{ 
//	if(val == pos_state) {
//		pos_state_count++;
//		return;
//	}
//	pos_state = val; 
//	pos_state_count = 1;
//}

void Card::set_reader( Reader* preader ) // 设置卡时间
{	
	//TRACE(_T("time memcpy 11\n"));
	this->rec_time = time(NULL);
	if(preader->reader_id == this->reader_id){ // 所在分站没有发生变化
		this->pos_state_count++;
	}else{ // 分站变化
		this->p_reader = preader;
		this->reader_id = preader->reader_id;
		this->enter_reader_time = this->rec_time;

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

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

/*
 * 设置地图集覆盖范围，适用TDOA
 *
*/
void Card::set_reader_path(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;
	/*CString str = _T("");
	str.Format(_T("state : %d \r\n"),acce_state);
	TRACE(str);*/

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

			if (this->x == this->last_x)
			{
				if (this->y >= this->last_y)
				{
					nSign = 1;
				}else{
					nSign = -1;
				}
			}else{
				if (this->x > this->last_x)
				{
					nSign = 1;
				}else{
					nSign = -1;
				}
			}

			if (nSign == this->direction)
			{
				this->isoutput = true;
			} 
			else
			{
				
				double distance = sqrt(pow(this->x - this->last_x,2) + pow(this->y - this->last_y,2));
				if ( distance > this->map_scale)
				{
					//两点之间的距离是否大于一个像素的距离，满足条件，输出json
					this->isoutput = true;
					//否则只输出到日志
				}else{
					this->isoutput = false;
				}
				/*char str_output[100] = {0};
				sprintf_s(str_output,"x: %f , y: %f , lx: %f, ly: %f ,nSign: %d, lstd: %d, scale: %f ,distance: %f \r\n",this->x,this->y,this->last_x,this->last_y,nSign,this->direction,this->map_scale,distance);
				str = _T("");
				str = str_output;
				TRACE(str);*/
			}

			this->direction = nSign;
		}
	}	
}

void Card::algo_tof(int cnt)
{
	int i = 0, nCount = 0;
	POS *p = NULL;

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

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

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

		init_postion = true;
	}	

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

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

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

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

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

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

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

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

	this->x = this->last_locate.x;
	this->y = this->last_locate.y;
	this->v = ((int)(this->last_locate.v*1000))/1000.0;
	this->z = 0;
	this->a = 0;
}

void Card::algo_tdoa(int cnt)
{
	if(cnt < 2){
		return;
	}

	//主要处理当相同卡的时间戳的数据中存在同步序号大于5的情况，如果有大于5的数据则丢弃此数据
	DistMap::iterator it = _dists.front().distmap.begin();
	int k = 0;
	int dst = 0;
	int st = 0;
	bool bRet = false;
	for(; it != _dists.front().distmap.end(); ++it,k++){	
		if(k==0){
			st = it->second->st;
		}else{
			dst = abs(st - it->second->st);
			if(dst >= 5){
				bRet = true;
				break;
			}
		}
		p_dists_locate[k] = it->second;
	}

	if(bRet){
		this->x = this->last_locate.x;
		this->y = this->last_locate.y;
		this->z = INVALID_COORDINATE;
		return;
	}

	//HeapSort(p_dists_locate,k);
	SelectSort(p_dists_locate,k);

	ReceiveDataMap pRdm;
	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;

	for(int i=0;i<k;i++){
		if(i==0){
			card_time_stamp = p_dists_locate[i]->t;
			maxSyncTimes = p_dists_locate[i]->st;
			acce_state = p_dists_locate[i]->acce_state;
			acce_state_last = p_dists_locate[i]->acce_state_last;
			ins_direction = p_dists_locate[i]->ins_direction;
		}
		else{
			if(maxSyncTimes < p_dists_locate[i]->st){
				maxSyncTimes = p_dists_locate[i]->st;
				acce_state = p_dists_locate[i]->acce_state;
				acce_state_last = p_dists_locate[i]->acce_state_last;
				ins_direction = p_dists_locate[i]->ins_direction;
			}
		}
		ReceiveDataMap::iterator prdm_it = pRdm.find(p_dists_locate[i]->reader_id);
		if(prdm_it == pRdm.end()){
			if(p_dists_locate[i]->tt == LLONG_MAX ){
				//如果同步时间戳存在异常值，则不走算法定位，直接返回上一次结果值
				bRet = true;
				break;
			}
			//保存信息用于定位
			ReceiveData prd;
			prd.reader_id = p_dists_locate[i]->reader_id;
			prd.antenna_id = p_dists_locate[i]->antenna_id;
			prd.rec_time_stamp = p_dists_locate[i]->tt;
			prd.x = p_dists_locate[i]->x*this->map_scale;
			prd.y = p_dists_locate[i]->y*this->map_scale;
			prd.z = p_dists_locate[i]->z*this->map_scale;
			prd.special = p_dists_locate[i]->special;
			pRdm[prd.reader_id] = prd;
		}		
	}

	//存在异常，直接返回上一次结果值
	if(bRet){
		pRdm.clear();
		this->x = this->last_locate.x;
		this->y = this->last_locate.y;
		this->z = INVALID_COORDINATE;
		return;
	}

	if (this->time_stamp_last > this->time_stamp_cal)
	{
		this->x = this->last_locate.x;
		this->y = this->last_locate.y;
		this->z = INVALID_COORDINATE;
		return;
	}

	this->time_stamp_last = this->time_stamp_cal;

	int i = 0, nCount = 0;
	POS *p = NULL;
	bool bOutput = false;
	this->z = 0;

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

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

		//TRACE(_T("%d,%f,%f"),this->time_stamp_cal,p->posx/this->map_scale,p->posy/this->map_scale);
		bool bRet = false;
		bRet = LocateAlgorithm::CheckPosInValid(p,&pRdm,this->map_scale);
		if(bRet){
			p->posx = INVALID_COORDINATE ;
			p->posy = INVALID_COORDINATE ;
			p->posz = -2 ;
		}

		double interval_time = this->p_reader->reader_interval_time;

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

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

		int nSign = 1;

		sync_data sdNew;
		sync_data sd;
		
		cx = p->posx / (this->map_scale*1.0);
		cy = p->posy / (this->map_scale*1.0);
		cz = p->posz / (this->map_scale*1.0);
		
#ifdef ALGORITHM_TYPE_INS
		int acce_direction = 0;		//加速度计速度方向	0-静止，1-前进，-1-后退
		int uwb_direction = 0;		//uwb速度方向，同上
		bool bUseKalman = false;
		bool bOriginLocate = false;	//原始定位是否成功
		bool bDirectReturn = false;	//是否直接返回本次定位结果
#endif

		if(p->posx != INVALID_COORDINATE && p->posy != INVALID_COORDINATE){
#ifdef ALGORITHM_TYPE_INS
			bOriginLocate = true;	//原始定位成功
#endif
			//定位成功
			if(this->m_nLastLocateT == 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{
				//现在的关于同步序号的处理是这样的：
				//如果定位成功，就把这次定位成功的同步数据：同步序号，坐标；x,y方向的速度，扔到一个队列里，
				//后来定位成功的就会先根据同步序号差用加速度抛一次；
				//抛不掉，就用队列里的同步数据（从后往前找），找到第一个与当前同步序号相差大于5的同步数据来进行第二次计算速度以及加速度，
				//如果加速度大于5，就不要此次的定位数据，
				//如果通过加速度判断就将队列中从头开始到此同步数据的所有元素都丢弃，并插入新的此次同步数据
				this->m_dFirstDiff = p->dFirstDiff;
				this->m_dSecondDiff = p->dSecondDiff;

				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 sdl = *it;
					deltaT = (maxSyncTimes - sdl.sync_num)*interval_time;
					//避免同一个同步序号下存在多个不同卡序号
					if (deltaT < 1E-2)
					{
						deltaT = 0.2;
					}
					cvx = (cx - sd.x)*this->map_scale/deltaT;
					cvy = (cy - sd.y)*this->map_scale/deltaT;

					double avx = (cvx - sd.vx) / deltaT;
					double avy = (cvy - sd.vy) / deltaT;
					double av = sqrt(pow(avx,2) + pow(avy,2));
					//车卡的加速度
					switch(this->card_type){
					case CT_PERSON:
						if(av > PERSON_ACCELERATE_THRESHOLD){
							this->x = this->last_locate.x;
							this->y = this->last_locate.y;
							return;
						}
						break;
					case CT_VEHICLE:
						if(av > VECHILE_ACCELERATE_THRESHOLD){
							//保留上次结果
							this->x = this->last_locate.x;
							this->y = this->last_locate.y;
							this->b_long_interval = false;
							return;
						}
						break;
					}
					deltaT = 0;
					cvx = cvy = 0;
				}

				//从队列尾部开始查找，找到第一个同步序号与当前计算卡的同步序号相差5个以上的数据
				//list<sync_data>::reverse_iterator it = m_syncNumList.rbegin();	
				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){
						sd = *it;
						break;
					}
					else{
						if(maxSyncTimes - it->sync_num < 0 && maxSyncTimes < 100){
							//如果最新同步号小于列表中的同步号则
							if(maxSyncTimes + 65536 - it->sync_num >=5 ){
								bOverflow = true;
								sd = *it;
							}
						}else{
							continue;
						}
					}
				}
				//根据溢出条件来计算deltaT
				if(bOverflow){
					deltaT = (maxSyncTimes + 65536 - sd.sync_num)*interval_time;
				}else{
					deltaT = (maxSyncTimes - sd.sync_num)*interval_time;
				}
				//使用间隔来修正速度
				if(deltaT - 1.0 >= 0){
					if (deltaT > 10)
					{
						this->b_long_interval = true;
					}
					//转为m/s
					cvx = (cx - sd.x)*this->map_scale/deltaT;
					cvy = (cy - sd.y)*this->map_scale/deltaT;

					//速度正负的判断：以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;
						}
					}

					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){
							this->x = this->last_locate.x;
							this->y = this->last_locate.y;
							return;
						}
						break;
					case CT_VEHICLE:
						if(av > VECHILE_ACCELERATE_THRESHOLD){
							//保留上次结果
							this->x = this->last_locate.x;
							this->y = this->last_locate.y;
							return;
						}
						break;
					}
					this->last_locate.acceleration = av;
					this->last_vx = cvx;
					this->last_vy = cvy;	
					cv = cv*3.6;

					//速度的限制
					if(fabs(cv) > MAX_VECHILE_SPEED){
						this->x = this->last_locate.x;
						this->y = this->last_locate.y;
						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;
						}					
					}

					//更新值为当前值并插入队列
					//sync_data sdNew;
					sdNew.sync_num = maxSyncTimes;
					this->m_nSyncNumInList = sd.sync_num;
					//sdNew.x = cx;
					//sdNew.y = cy;
					sdNew.vx = cvx;
					sdNew.vy = cvy;
					//m_syncNumList.push_back(sdNew);	
					sdNew.update = true;
#ifdef ALGORITHM_TYPE_INS
					uwb_direction = (nSign == 1)?1:-1;		//uwb形式判断不出静止或者怠速
#endif
				}else{
					//cv = this->last_locate.v;
					cv = this->origin_locate.v;
#ifdef ALGORITHM_TYPE_INS
					uwb_direction = ins_direction;//不用this->accelerate_state_last此值是因为进入算法后这个值可能已经被更新了
#endif		
				}
			}	
		}	
#ifdef ALGORITHM_TYPE_INS
		else{
			uwb_direction = ins_direction;
		}
#endif

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

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

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

			if(cweight*uwb_direction < 0){
				//惯导和uwb定位方向不一致，定位失败
				//增加一次判断，与上一次的进行比对，如果和上一次的一致，则送进卡尔曼滤波
				if(cweight*acce_state_last > 0){
					bUseKalman = true;
				}else{
					bUseKalman = false;
				}
			}else{
				//惯导和uwb定位方向一致，定位成功
				//送进卡尔曼滤波
				bUseKalman = true;
			}
		}
		if (abs(cweight) > 0)
		{
			ins_direction = cweight/abs(cweight) > 0?1:-1;
		}else{
			//如果加速度计的状态权重为零
			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){
#ifdef ALGORITHM_TYPE_INS
			if(bUseKalman){
#endif
				double kalman_detal_t = (maxSyncTimes - this->last_locate.st)*interval_time;

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

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

						if(deltaT!=0){
							/*this->m_pKalmanFilter->m_pCar->x(1,0) = (this->m_pKalmanFilter->m_pCar->x(0,0) - this->last_locate.x)/deltaT;
							this->m_pKalmanFilter->m_pCar->x(3,0) = (this->m_pKalmanFilter->m_pCar->x(2,0) - this->last_locate.y)/deltaT;*/
							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){
					p->nFirstReader = last_s_locate_reader[0];
					p->nSecondReader = last_s_locate_reader[1];
				}
				//增加地图集的判定，判断定位结果是否在地图集上
				//如果不在地图集上，需要再次定位
				//需要带出定位结果的分站信息，
				//利用地图集中分站信息再次定位
				POS kalman_p;
				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)){
					//再一次定位到地图集上
					POS* cp = LocateAlgorithm::Pos(&kalman_p,*pTdoaReaderPathMap);

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

					if(cp){
						delete cp;
						cp = NULL;
					}
				}		

				this->last_locate.x = this->x = this->m_pKalmanFilter->m_pCar->x(0,0) / this->map_scale;
				this->last_locate.y = this->y = this->m_pKalmanFilter->m_pCar->x(2,0) / this->map_scale;
				this->last_locate.z = this->z;
				this->last_locate.st = 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;
				this->v = sqrt(pow(this->m_pKalmanFilter->m_pCar->x(1,0),2) + pow(this->m_pKalmanFilter->m_pCar->x(3,0),2)); //*this->map_scale
				this->v*=3.6;	//转为km/h

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

				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->v*=nSign;

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

				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);
				}
#ifdef ALGORITHM_TYPE_INS
			}else{
				//这组数据的处理方法是：
				//如果第一次成功定位，但状态是静止，就取第一次成功定位值(这是为了处理当第一次成功定位，但状态是静止的，此时取上一次定位值为零的问题)，
				//后续如果不管定位成功还是失败，只要状态是静止的，就输出上一次成功定位值，并更新同步序号。
				//取上次结果
				if (bDirectReturn)
				{
					if(this->map_scale > 0){
						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{
					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;
				}
				/*if(this->last_locate.x == 0 && this->last_locate.y == 0){

				}else{

				}
				*/
				this->m_nLastLocateT = this->m_nCalcSyncNum = this->last_locate.st = this->sync_num = maxSyncTimes;
	
				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);	

					this->m_nSyncNumInList = maxSyncTimes;
				}

				this->is_deal_by_algo = true;
			}
#endif
		}else{
			//TRACE(_T("no kalman . \n"));
			//最新通过算法算出的结果
			if(p->posx == INVALID_COORDINATE || p->posy == INVALID_COORDINATE){
				this->x = this->last_locate.x;
				this->y = this->last_locate.y;
				this->z = this->last_locate.z;
			}else{
				if(this->map_scale > 0){
					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);
				}

				this->v = cv;

				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.st = maxSyncTimes;
			}
			this->a = 0;
		}
	}

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

	pRdm.clear();
}

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

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;
		dq.distmap[s] = dist;
		_dists.push_back(dq);
	}else{
		_dists[idx].distmap[s] = dist;
	}

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

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

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

	//int nTimeStamp;
	//if(time_stamp_max == 0 && offset != 0){
	//	nTimeStamp = 0xFFFF;
	//}else{
	//	nTimeStamp = time_stamp_max - offset;
	//}
	//int ret = 0, tmp = -1;
	//for(int i = 0; i < DIST_COUNT; i++){
	//	if(NULL == p_dists[i]) break;
	//	if(p_dists[i]->t == nTimeStamp){
	//		ret++;
	//	}
	//}
	//return ret;
}

Card::~Card(void)
{
	if(m_pKalmanFilter){
		delete m_pKalmanFilter;
		m_pKalmanFilter = NULL;
	}

	for(int i = DIST_COUNT - 1;i >= 0;i--){
		if(p_dists[i] != NULL){
			delete p_dists[i];
			p_dists[i] = NULL;
		}
	}
	
	if(p_dists){
		delete[] p_dists;
		p_dists = NULL;
	}

	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.swap(DistQueMap());
	_dists.clear();

	DeleteCriticalSection(&m_csCard);
}

void Card::get_coordinate_2d( int cnt )
{
	//double d_dist[3];
	//Point2 pt2, pt2_1;
	//for(int i = 0; i < cnt; i++){
	//	d_dist[i] = p_dists_locate[i]->d;

	//}
	//for(int i=0; i< cnt; i++){
	//	if(p_dists[i]->reader_id == 1){
	//		p2_anchors[0].x = p_dists[i]->x;
	//		p2_anchors[0].y = p_dists[i]->y;
	//		d_dist[0] = p_dists[i]->d;
	//	}else if(p_dists[i]->reader_id == 2){
	//		p2_anchors[1].x = p_dists[i]->x;
	//		p2_anchors[1].y = p_dists[i]->y;
	//		d_dist[1] = p_dists[i]->d;
	//	}if(p_dists[i]->reader_id == 3){
	//		p2_anchors[2].x = p_dists[i]->x;
	//		p2_anchors[2].y = p_dists[i]->y;
	//		d_dist[2] = p_dists[i]->d;
	//	}
	//}
	//Mat mat(3, 1, d_dist);

	//if(is_anchor_changed){
	//	cal_location->setAnchors(p2_anchors);
	//	pt2 = cal_location->initPos(mat);
	//} else{
	//	pt2 = cal_location->estiPos(mat);
	//}
	//pt2_1 = cal_location->getError();
	//x = pt2.x;
	//y = pt2.y;
	//z = 0;
	//x1 = pt2_1.x;
	//y1 = pt2_1.y;
	//z1 = 0;
}

double Card::get_speed()
{
	double speed = 0; 
	// 计算速度
	return speed;
}

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

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

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

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

	if(power_state == STATUS_ERROR){
		state += STATUS_POWER_LOWER;
		ret += "电量低,";
	}else if(power_state == STATUS_ERROR_SERIOUS){
		state += STATUS_POWER_LOWER_SERIOUS;
		ret += "电量极低,";
	}
	
	if(ret.length() > 0){
		ret = ret.substr(0, ret.length() - 1);
	}else{
		ret = "正常";
	}

	return ret;
}

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

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

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

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

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

	switch(nType){
	case FILTER_KALMAN:
		//分配卡尔曼滤波类型变量并初始化参数
		if(m_pKalmanFilter == NULL){
			m_pKalmanFilter = new CKalmanFilter();
			m_pKalmanFilter->Initial(0.2);
			m_pKalmanFilter->m_bFlag = false;
		}
		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();){
			_coordinate* tmp_coord = (_coordinate*)(it_mp_dist->second);
			tmp.erase(it_mp_dist++);
			if(tmp_coord){
				delete tmp_coord;
				tmp_coord = NULL;
			}
			it_mp_dist = tmp.begin();
		}
	}
	_dists.pop_front();
	//_dists.erase(_dists.begin());
}

Reader::Reader(void)
{
	rec_time = reader_time = lost_time = time(NULL);
	map_scale = 1.0;
	sync_level = 0xFF;
	state = STATUS_DEVICE_NORMAL;
	for(int i = 0;i < ANTENNA_COUNT;i++){
		ant[i] = NULL;
	}

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

	bIsInitCoverage = false;

	reader_interval_time = 0.0;
	m_nIsSpecial = -1;
}


Reader::~Reader(void)
{
	for(int i = 0;i < ANTENNA_COUNT;i++){
		Antenna* tmp = NULL;
		tmp = ant[i];
		if(tmp){
			delete tmp;
			tmp = NULL;
			ant[i] = NULL;
		}
	}

	for(int i = 0; i < ADHOC_COUNT;i++){
		Adhoc * tmp = NULL;
		tmp = adhoc[i];
		if(tmp){
			delete tmp;
			tmp = NULL;
			adhoc[i] = NULL;
		}
	
	}
}

std::string Reader::get_state_text()
{
	string ret = "";
	if(state == STATUS_DEVICE_ERROR){
		ret = "故障";
	}else if(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 = 0;
	count_area_forbidden_person = count_area_forbidden_vehicle = 0;
	time_forbidden_person = time_forbidden_vehicle = time(NULL);
}

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

void Area::init_border(string sz_path)
{
	//std::vector<std::string> vec = split(sz_path, ",");
	//if(vec.size() >= 4){
	//	rect_left = atoi(vec[0].c_str());
	//	rect_top = atoi(vec[1].c_str());
	//	rect_right = atoi(vec[2].c_str());		
	//	rect_bottom = atoi(vec[3].c_str());
	//}else{
	//	rect_left = rect_right = rect_top = rect_bottom = 0;
	//}
	if(sz_path == ""){
		return ;
	}

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

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

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

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

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

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

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

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

bool Card::is_pos_state_pack_changed( int nval )
{
	if(this->pos_state_park == this->pos_state_park_old){
		if(this->pos_state_park_count < nval){
			this->pos_state_park_count++;
		}
		return false;
	}
	//if(this->pos_state_park_count < nval ){ // 未达到确认次数
	//	this->pos_state_park_count++;
	//	return false;
	//}
	this->pos_state_park_old = this->pos_state_park;
	this->pos_state_park_count = 1;
	return true;
}

MapInfo::MapInfo( void )
{

}

MapInfo::~MapInfo( void )
{

}

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

Dept::Dept()
{
}

Dept::~Dept()
{
}

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

Adhoc::~Adhoc()
{
}