server/classdef.cpp

#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*/ )
{
	InitializeCriticalSection(&m_csCard);

	card_type = cardtype;
	card_id = cardid;
	coor_offset_x = offset_x;
	coor_offset_y = offset_y;
	this->z_offset = z_offset;
	
	GetSystemTime(&deal_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;
	}
	cal_location3 = new CalLocation3(KALMAN_OFFSET_COOR_TRI, KALMAN_OFFSET_MOVE_TRI, KALMAN_OFFSET_RANGING_TRI, KALMAN_INTERVAL_TRI);
	p3_anchors = new Point3[ANCHOR_COUNT];
	is_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 = uwb_weight = weight = 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 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();
	}
	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 = 0;i<DIST_COUNT;i++){
		delete p_dists[i];
		p_dists[i] = NULL;
		delete p_dists_locate[i];
		p_dists_locate[i] = NULL;
	}
	
	if(p_dists){
		delete[] p_dists;
		p_dists = NULL;
	}

	if(p_dists_locate){
		delete[] p_dists_locate;
		p_dists_locate = NULL;
	}

	if(p3_anchors){
		delete[] p3_anchors;
		p3_anchors = NULL;
	}

	if(cal_location3){
		delete cal_location3;
	}

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

void Card::get_coordinate_3d( int cnt )
{
	double d_dist[ANCHOR_COUNT];
	Point3 pt3, pt3_1;
	for(int i = 0; i < cnt; i++){
		d_dist[i] = p_dists_locate[i]->d;
	}

	Mat mat(ANCHOR_COUNT, 1, d_dist);
	if(is_anchor_changed){
		set_anchors(cnt);
		pt3 = cal_location3->initPos(mat);
	} else{
		pt3 = cal_location3->estiPos(mat);
	}
	pt3_1 = cal_location3->getError();

	x = pt3.x;
	y = pt3.y;
	z = pt3.z;
	x1 = pt3_1.x;
	y1 = pt3_1.y;
	z1 = pt3_1.z;
}

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

void Card::set_anchors( int cnt )
{
	for(int i = 0; i < cnt; i++){
		p3_anchors[i].x = p_dists[i]->x;
		p3_anchors[i].y = p_dists[i]->y;
		p3_anchors[i].z = p_dists[i]->z;
	}
	cal_location3->setAnchors(p3_anchors, ANCHOR_COUNT);
	is_anchor_changed = false;
}

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

/*
 * 采用TOF或者TDOA算法进行定位计算
 *
 * param
 *		cnt		------	_dists数据条数
 *
 * return
 *		无返回值
 *
*/
void Card::get_coordinate( int cnt )
{
	int i = 0, nCount = 0;
	POS *p = NULL;
	POS tmpPos;
	tmpPos.posx = 0;
	tmpPos.posy = 0;
	tmpPos.pos_radius = 0;

#ifdef ALGORITHM_TOF
//	if(cnt < 2) return; // 只有一个测量值，忽略不计算
	int difT = 1;
	//int i = 0, nCount = 0;
	bool bfound = false;
	DistMap::iterator it = _dists.front()->mp_dist.begin();
	if(cnt > 2){
		for(; it != _dists.front()->mp_dist.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()->mp_dist.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;

		//InitStation();
	}	

	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;

	/*POS *p = NULL;
	POS tmpPos;
	tmpPos.posx = 0;
	tmpPos.posy = 0;
	tmpPos.pos_radius = 0;*/

	if(info_pre.t == 5){
		int sss = 0;
		sss = info_pre.t;
	}
	//this->p_reader->readerCoveragePath
	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 = TOFLocateAlgorithm::Pos(station,sta_num,ant,dist,info_pre);
		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;

#elif defined ALGORITHM_TYPE_TDOA
	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;
				//continue;
			}
		}
		p_dists_locate[k] = it->second;
	}

	this->x = 0;
	this->y = 0;
	this->z = 0;

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

	for(int i=0;i<k;i++){
		if(i==0){
			maxSyncTimes = p_dists_locate[i]->st;
			acce_state = p_dists_locate[i]->acce_state;
		}
		else{
			if(maxSyncTimes < p_dists_locate[i]->st){
				maxSyncTimes = p_dists_locate[i]->st;
				acce_state = p_dists_locate[i]->acce_state;
			}
		}
		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;
		}		
	}

	acce_state = 2;
	bool bOutput = false;
	int nSize = 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 ins_state = 0;
		int uwb_state = 0;
		bool bUseKalman = false;
		bool bOriginLocateState = false;
#endif

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

				//从队列尾部开始查找，找到第一个同步序号与当前计算卡的同步序号相差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){
					//转为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 > 3){
							this->x = this->last_locate.x;
							this->y = this->last_locate.y;
							return;
						}
						break;
					case CT_VEHICLE:
						if(av > 5){
							//保留上次结果
							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;
					
					//删除第一个元素到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_state = nSign == 1?1:-1;		//uwb形式判断不出静止或者怠速
#endif
				}else{
					//cv = this->last_locate.v;
					cv = this->origin_locate.v;
#ifdef ALGORITHM_TYPE_INS
					uwb_state = this->accelerate_state_last;
#endif		
				}
			}	
		}	
#ifdef ALGORITHM_TYPE_INS
		else{
			bOriginLocateState = false;
			uwb_state = this->accelerate_state_last;
		}
#endif

#ifdef ALGORITHM_TYPE_INS
		bRet = false;
		switch(acce_state){
		case 0:	//熄火
		case 1:	//怠速
			//如果是熄火或者怠速状态，则将x,y方向的速度置为0，并送到卡尔曼滤波中
			cvx = 0;
			cvy = 0;
			bRet = true;
			break;
		case 2:	//前进
			//如果根据加速度状态判断为前进，则取1
			ins_state = 1;
			break;
		case 3:	//倒退
			//如果根据加速度状态判断为后退，则取-1
			ins_state = -1;
			break;
		}
		if(!bRet){
			double cweight = 0;
			cweight = this->ins_weight * ins_state + this->uwb_weight*uwb_state;
			//如果计算出的权重在合适范围内，就逐渐降低惯导的权限
			//如果计算出的权重超过范围，则重置惯导的权重为90%
			if(cweight>=INS_WEIGHT*-1&&cweight<=INS_WEIGHT){
				this->ins_weight = abs(cweight);
			}else{
				this->ins_weight = INS_WEIGHT;
			}

			this->weight = cweight;

			if(bOriginLocateState){
				if(cweight*uwb_state <= 0){
					//惯导和uwb定位方向不一致，定位失败
					//增加一次判断，与上一次的进行比对，如果和上一次的一致，则送进卡尔曼滤波
					if(cweight*this->accelerate_state_last > 0){
						bUseKalman = true;
					}else{
						bUseKalman = false;
					}
				}else{
					//惯导和uwb定位方向一致，定位成功
					//送进卡尔曼滤波
					bUseKalman = true;
				}
			}else{
				if(k*uwb_state > 0){
					//方向一致，但原始定位失败了
					//送进卡尔曼滤波
					bUseKalman = true;
				}else{
					//方向不一致，而且定位也失败了
					//不送了，直接取上一次的结果
					bUseKalman = false;
				}
			}
		}
		else{
			//不送到卡尔曼滤波
			//bUseKalman = true;
			bUseKalman = false;
		}
#endif
		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(this->last_locate.x == 0 && this->last_locate.y == 0){
					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 = 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){
					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();

#else

#endif
	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();
		while(it_mp_dist != tmp.end()){
			delete it_mp_dist->second;
			it_mp_dist->second = NULL;
			tmp.erase(it_mp_dist++);
		}
	}
	_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()
{
}