#include "stdafx.h"
#include "locate_algorithm.h"
#include <math.h>
#include <algorithm>
#include "../common/matrix/_Matrix.h"
#include "../common/Functions/Functions.h"

#include "ProcessRemodule.h"
#include "log_process_module.h"

using namespace std;

// ant 天线， tar定位目标， d 测距
void AOAformula( struct _coordinate* ant, double angle, struct _coordinate* tar, bool is_line /*= false*/, double z_offset /*= 0*/ )
{
	double aa = sin(angle);
	double bb = cos(angle);
	double h = ant->z;
	if(is_line){
		h = z_offset;
	}
	tar->x = ant->x + sqrt(ant->d * ant->d - h * h) * cos(angle); 
	tar->y = ant->y + sqrt(ant->d * ant->d - h * h) * sin(angle);
	tar->z = ant->z; // 高度
	tar->a = ant->a; // 角度
	tar->t = ant->t; // 时间戳
	tar->d_offset = ant->d_offset;
	tar->reader_id = ant->reader_id;
	tar->d = ant->d;
}

// ant 天线， last 上次坐标
double Distanceformula( struct _coordinate* ant, struct _coordinate* last )
{
	double x0,y0;
	double tan_diff = tan(last->a) - tan(ant->a);
	double ret = 0;
	if(tan_diff == 0){
		ret = sqrt((ant->x - last->x) * (ant->x - last->x) +  (ant->y - last->y) * (ant->y - last->y));
	}else{
		x0 = (last->x * tan(last->a) - ant->x * tan(ant->a) + ant->y - last->y) / (tan(last->a) - tan(ant->a));
		y0 = (((last->x - ant->x) * tan(last->a) * tan(ant->a)) + ant->y * tan(last->a) - last->y * tan(ant->a)) / (tan(last->a) - tan(ant->a));
		ret = sqrt((x0 - ant->x) * (x0 - ant->x) + (y0 - ant->y) * (y0 - ant->y)) 
			+ sqrt((x0 - last->x) * (x0 - last->x) + (y0 - last->y) * (y0 - last->y));
	}
	// n = Nodeformula( n1, n2);
	return ret;
}

// 接收到的新数据
void algorithm_locate( struct _coordinate** coor_list, int coor_size, struct _coordinate* hist, struct _coordinate* dest, double dist_limit, double z_offset)
{
	double d0;  //历史定位坐标与实时定位坐标之间的距离；
	double dN;  //利用速率V和时间戳理论计算出行进距离；
	double Angle1,Angle2,Angle3;  //三个坐标时，任何2坐标的角度
	double Angle;   //两个坐标之间的角度
	double dLong;
	double dLenth;
	int position;  //定位点位置

	switch(coor_size){		
	case 1: // 1条定位数据，5.3.1
		{
			if(coor_list[0]->d > dist_limit){ // 如果测距的距离大于某个固定值,此时判断此位置与天线角度一致
				//coor_list[0]->d -= coor_list[0]->d_offset;
				AOAformula(coor_list[0], coor_list[0]->a, dest, true, z_offset);
			}else { // 测距距离小于某值
				//利用速率V和时间戳理论计算出行进距离
				dN = coor_list[0]->v * (coor_list[0]->t - hist->t);
				//计算实时定位分站（天线）坐标和历史定位坐标之间的距离
				d0 = Distanceformula(coor_list[0], hist);
				if(hist->x - coor_list[0]->x == 0){
					Angle = (coor_list[0]->y >= hist->y) ? 90 : 270;
					Angle = Angle * M_PI / 180;
				}else{
					//计算历史坐标与实时分站坐标的角度，用来表明历史坐标位于定位坐标的方向
					Angle = atan((hist->y - coor_list[0]->y)/(hist->x - coor_list[0]->x));
				}

				// 与历史坐标同向		
				//如果定位距离小于两个坐标之间的距离，说明定位位置在两个坐标之内
				if(d0 >= coor_list[0]->d){
					if(dN >= coor_list[0]->d){
						Angle += M_PI;  //与历史坐标反方向
					}
				}else{  
					//如果定位距离大于两个坐标之间的距离，说明定位位置在两个坐标之外,接下来只要判断在哪个yu坐标的位置即可
					//如果理论进行距离大于两者坐标之间的距离，则说明与历史坐标在同一方向，反之在反方向
					if(dN < coor_list[0]->d){ 
						Angle += M_PI;  //与历史坐标反方向
					}
				}
				//coor_list[0]->d -= coor_list[0]->d_offset;
				AOAformula(coor_list[0], Angle, dest, true, z_offset);  //最终定位坐标计算
			}
			break;
		}
	case 2: //2条定位数据，5.3.2
		{
			//2条定位数据中分站(天线)坐标与历史定位坐标之间的角度，以此推出这三个坐标是否在一条直线上或者呈三角形
			if(coor_list[0]->x == hist->x ){
				Angle1 = (coor_list[0]->y == hist->y) ? 0 : ((coor_list[0]->y > hist->y) ? 90 : 270);
				Angle1 = Angle1 * M_PI / 180;
			}else{
				Angle1 = atan((coor_list[0]->y - hist->y) / (coor_list[0]->x - hist->x)); 
			}
			if(coor_list[1]->x == hist->x ){
				Angle2 = (coor_list[1]->y == hist->y) ? 0 : ((coor_list[1]->y > hist->y)? 90 : 270);
				Angle2 = Angle2 * M_PI / 180;
			}else{
				Angle2 = atan((coor_list[1]->y - hist->y) / (coor_list[1]->x - hist->x)); 
			}
			if(coor_list[1]->x == coor_list[0]->x ){
				Angle3 = (coor_list[1]->y >= coor_list[0]->y) ? 90 : 270; // 必须在不同点
				Angle3 = Angle3 * M_PI / 180;
			}else{
				Angle3 = atan((coor_list[1]->y - coor_list[0]->y) / (coor_list[1]->x - coor_list[0]->x)); 
				if(Angle3 > M_PI){
					Angle3 -= M_PI;
				}
				/*if(coor_list[0]->x > coor_list[1]->x)
				Angle3 += M_PI;*/
			}
			//2个角度相同或者相差180度说明在一条直线上
			//if(coor_list[0]->a == coor_list[1]->a || coor_list[0]->a == coor_list[1]->a + M_PI || coor_list[0]->a == coor_list[1]->a - M_PI){
			if(Angle1 == Angle2 || Angle1 == Angle2 + M_PI || Angle1 == Angle2 - M_PI){
				//取两个距离之间的最大值,保存最短距离的数据作为定位点
				dLong = max(coor_list[0]->d, coor_list[1]->d);
				position = (coor_list[0]->d > coor_list[1]->d) ? 1 : 0;
				dLenth = Distanceformula(coor_list[0], coor_list[1]);
				if(dLong > dLenth){
					Angle = Angle3 + ((position + 1)%2) * M_PI; 
				}else{
					Angle = Angle3 +  position * M_PI;
				}
				//2个坐标的距离小于其2个距离中的最大值，则说明定位点在2个坐标的外侧，
				//且在距离短的那条作为定位坐标，定位角度与此相同
				//2个坐标的距离大于这两条距离中的任何一个，则说明定位点在两个坐标之间，取任意一条作为定位点,定位角度与其角度相反（+180）
				// AOAformula(coor_list[position], Angle, dest);  //最终定位坐标计算

				// 根据测量的结果，调整距离d 
				// x2 - (d2 - ((d1+d2) - (x2-x1))/2) 靠近x2  x2 > x1,d1 > d2
				// x1 + (d1 - ((d1+d2) - (x2-x1))/2) 靠近x1  x2 > x1,d1 < d2
				// 化简得到 (x1 + x2 + d1 - d2)/2
				// 显示的d = ((d1 + d2) - abs(x2-x1))/2
				double d_offset;
				if(fabs(coor_list[0]->x - coor_list[1]->x) > fabs(coor_list[0]->y - coor_list[1]->y)){ // 水平方向
					d_offset = (coor_list[0]->d + coor_list[1]->d - fabs(coor_list[0]->x - coor_list[1]->x))/2;
				}else{ // 垂直方向
					d_offset = (coor_list[0]->d + coor_list[1]->d - fabs(coor_list[0]->y - coor_list[1]->y))/2;
				}
				dest->d_offset = d_offset;
				coor_list[0]->d_offset = d_offset;
				coor_list[1]->d_offset = d_offset;
				coor_list[position]->d -= d_offset;
				AOAformula(coor_list[position], Angle, dest, true, z_offset);
			}
			//岔路口形式
			else {
				dLenth = Distanceformula(coor_list[0], coor_list[1]); //2个天线坐标之间的距离
				if(dLenth > (coor_list[0]->d  + coor_list[1]->d)) { //取一条作为定位参考数据，采用1条定位数据算法,参考5.3.1
					//如果测距的距离大于某个固定值,此时判断此位置与天线角度一致，
					if(coor_list[0]->d > dist_limit){
						AOAformula(coor_list[0], coor_list[0]->a, dest);  //最终定位坐标计算
					}
					//测距距离小于某值
					else{
						dN = coor_list[0]->v * ( hist->t - coor_list[0]->t);  //利用速率V和时间戳理论计算出行进距离；
						d0 = Distanceformula(coor_list[0], hist);  //计算实时定位分站（天线）坐标和历史定位坐标之间的距离
						if(hist->x == coor_list[0]->x){
							Angle = (hist->y >= coor_list[0]->y) ? 90 : 270; //计算历史坐标与实时分站坐标的角度，用来表明历史坐标位于定位坐标的方向
							Angle = Angle * M_PI / 180;
						}else{
							Angle = atan((hist->y - coor_list[0]->y) / (hist->x - coor_list[0]->x));
						}

						//如果定位距离小于两个坐标之间的距离，说明定位位置在两个坐标之内
						if(d0 > coor_list[0]->d){
							if(dN >= coor_list[0]->d){
								Angle += M_PI; //与历史坐标反方向
							}
						}
						else{
							//如果定位距离大于两个坐标之间的距离，说明定位位置在两个坐标之外,接下来只要判断在哪个坐标的位置即可
							//如果理论进行距离大于两者坐标之间的距离，则说明与历史坐标在同一方向，反之在反方向
							if(dN < coor_list[0]->d){
								Angle += M_PI;
							}
						}
						AOAformula(coor_list[0], Angle, dest, true, z_offset);  //最终定位坐标计算
					}
				}
				else { //采用2条定位数据算法，采用5.3.2				
					//取两个距离之间的最大值,保存最短距离的数据作为定位点
					dLong = max(coor_list[0]->d, coor_list[1]->d);
					position = (coor_list[0]->d > coor_list[1]->d) ? 1 : 0;
					dLenth = Distanceformula(coor_list[0], coor_list[1]);
					//2个坐标的距离小于其2个距离中的最大值，则说明定位点在2个坐标的外侧，
					//且在距离短的那条作为定位坐标，定位角度与此相同
					//2个坐标的距离大于这两条距离中的任何一个，则说明定位点在两个坐标之间，取任意一条作为定位点,定位角度与其角度相反
					if(dLong > dLenth){
						Angle = Angle3 + ((position + 1)%2) * M_PI; 
					}else{
						Angle = Angle3 +  position * M_PI;
					}
					AOAformula(coor_list[position], Angle, dest, true, z_offset);  //最终定位坐标计算
				}
			}
			break;
		}
	case 3: // 3条定位数据，三点定位法 
		{
			// 在同一直线上不能使用三点定位
			locatebycordinate(coor_list, dest);

			break;
		}
	case 4: // 4条数据，取前3条
		{
			locatebycordinate(coor_list, dest);
			break;
		}
	default:
		{
			dest->x = -1;
			dest->y = -1;
			dest->a = 0;
			break;
		}
	}
}

void locatebycordinate( struct _coordinate** coor_list, struct _coordinate* dest )
{
	double h = 0;
	int i = 0;
	int j = 0;
	double temp = 0;

	double x[3]; // 坐标
	double y[3]; // 坐标
	double z[3];
	double d[3];
	double p[2]; 
	_Matrix pi;

	matrix_set(&pi,3,2);
	matrix_init(&pi);

	for(i=0;i<3;i++){
		x[i] = coor_list[i]->x;
		y[i] = coor_list[i]->y;
		z[i] = coor_list[i]->z;
		d[i] = coor_list[i]->d; // d * CM_PIX * 100;
	}
	//获得坐标
	matrix_write(&pi,0,0,x[0]);
	matrix_write(&pi,0,1,y[0]);
	h = z[0];
	d[0] = sqrt(d[0] * d[0] - h * h);

	matrix_write(&pi,1,0,x[1]);
	matrix_write(&pi,1,1,y[1]);
	h = z[1];
	d[1] = sqrt(d[1] * d[1]- h * h);

	matrix_write(&pi,2,0,x[2]);
	matrix_write(&pi,2,1,y[2]);
	h = z[2];
	d[2] = sqrt(d[2] * d[2] - h * h);

	if (locate_func(&pi,d,p) > 0)
	{
		dest->x = p[0];
		dest->y = p[1];
	}else
	{
		dest->x = -1;
		dest->y = -1;
	}
}

void algorithm_locate_ex( _coordinate** coor_list, int coor_size, _coordinate* hist, _coordinate* dest, double dist_limit )
{
	switch (coor_size)
	{
	case 1:
		{
			if(!hist){ // 初次计算，只有一个分站数据的时候跳出，不计算坐标
				return;
			}
			// 获取历史节点与当前分站的角度，判断是否仍在同一直线
			double angle = getAngleByAngle(coor_list[0], hist);
			if(0 == tan(angle)){ // 在同一直线
				double dist_temp = getDistance(hist, coor_list[0]); // 获得上一次的距离
				// 获得移动的距离
				double dist_mv = fabs(dist_temp - coor_list[0]->d);
				// 沿着原移动方向运动
				calcCoordinate(hist, dist_mv, hist->a, dest); //
			}else{ // 有角度，找交点
				_coordinate* pt_cross = getCross(hist, coor_list[0]);
				double dist_temp = getDistance(pt_cross, coor_list[0]);
				if(dist_temp >= coor_list[0]->d){
					// 避免动画穿墙，先将该点定位到交点上
					dest->x = pt_cross->x;
					dest->y = pt_cross->y;
					dest->z = coor_list[0]->z;
					dest->a = getAngleByCoordinate(pt_cross, coor_list[0]); // 运行方向
				}else{ // 在当前方向上运行, 理论上不存在
					dist_temp = (getDistance(hist, pt_cross) - (coor_list[0]->d - dist_temp)); // 位移
					calcCoordinate(hist, dist_temp, hist->a, dest);
				}
			}
			break;
		}
	case 2:
		{
			double angle_anc = getAngleByAngle(coor_list[0], coor_list[1]);
			if(0 == tan(angle_anc)){ // 在同一直线
				double dist_anc = getDistance(coor_list[0], coor_list[1]);
				double dist_offset;
				if(dist_anc < coor_list[0]->d || dist_anc < coor_list[1]->d ) {//分站到标识卡的距离大于分站之间的距离，同一侧
					dist_offset = fabs(coor_list[0]->d - coor_list[1]->d) - dist_anc;
					double angle_anc_run = getAngleByCoordinate(coor_list[0], coor_list[1]); // 分站2一侧
					if(coor_list[0]->d < coor_list[1]->d){ // 分站1一侧
						angle_anc_run += M_PI;
					}
					calcCoordinate(coor_list[0], coor_list[0]->d - dist_offset / 2, angle_anc_run, dest);
				}else{ //在分站之间
					dist_offset = coor_list[0]->d + coor_list[1]->d - dist_anc;
					calcCoordinate(coor_list[0], coor_list[0]->d + dist_offset / 2, getAngleByCoordinate(coor_list[0], coor_list[1]), dest);
				}
			}else{ // 有夹角, 一定在两分站之间
				if(hist){
					double angle_anc1 = getAngleByAngle(coor_list[0], hist);
					double angle_anc2 = getAngleByAngle(coor_list[1], hist);
					// 找到交点
					_coordinate* pt_cross = getCross(coor_list[0], coor_list[1]);
					// 交点到分站的距离
					double dist_anc1 = getDistance(pt_cross, coor_list[0]);
					double dist_anc2 = getDistance(pt_cross, coor_list[1]);

					if(0 == tan(angle_anc1)){
						if(dist_anc1 >= coor_list[0]->d){ // 以直线上为准
							calcCoordinate(hist, fabs(dist_anc1 - coor_list[0]->d), hist->a, dest);
							//calcCoordinate(coor_list[0], coor_list[0]->d, getAngleByCoordinate(coor_list[0], hist), dest);
						}else{ // 超过直接定位到交点上
							dest->x = pt_cross->x;
							dest->y = pt_cross->y;
							dest->a = getAngleByCoordinate(pt_cross, coor_list[1]);
						}
					}else if(0 == tan(angle_anc2)){ // 与分站1在同一直线
						if(dist_anc2 >= coor_list[1]->d){ // 以直线上为准
							calcCoordinate(hist, fabs(dist_anc2 - coor_list[1]->d), hist->a, dest);
							//calcCoordinate(coor_list[1], coor_list[1]->d, getAngleByCoordinate(coor_list[1], hist), dest);
						}else{ // 超过直接定位到交点上
							dest->x = pt_cross->x;
							dest->y = pt_cross->y;
							dest->a = getAngleByCoordinate(pt_cross, coor_list[0]);
						}
					}else { // 三叉口
						// 找到交点
						_coordinate* pt_cross1 = getCross(hist, coor_list[0]);
						_coordinate* pt_cross2 = getCross(hist, coor_list[1]);

						if(coor_list[0]->d >= coor_list[1]->d){
							dest->x = pt_cross2->x;
							dest->y = pt_cross2->y;							
							dest->z = coor_list[1]->z;
							dest->a = coor_list[1]->a;
						}else{
							dest->x = pt_cross1->x;
							dest->y = pt_cross1->y;							
							dest->z = coor_list[0]->z;
							dest->a = coor_list[0]->a;
						}
					}
				}else{ // 先定位到交点,避免穿墙
					_coordinate* pt_cross = getCross(coor_list[0], coor_list[1]);
					dest->x = pt_cross->x;
					dest->y = pt_cross->y;
					// 比较距离,定位到距离近的方向上
					if(fabs(getDistance(pt_cross, coor_list[0]) - coor_list[0]->d) >= fabs(getDistance(pt_cross, coor_list[1]) - coor_list[1]->d)){
						dest->z = coor_list[1]->z;
						dest->a = coor_list[1]->a;
					}else{
						dest->z = coor_list[0]->z;
						dest->a = coor_list[0]->a;
					}
				}
			}
			break;
		}
	case 3:
		{
			// 判断分站是否在同一直线上
			if(is_all_inline(coor_list, coor_size)){
				algorithm_locate_ex(coor_list, 2, hist, dest, dist_limit);
				break;
			}
			locatebycordinate(coor_list, dest);
			break;
		}
	case 4:
		{
			// 判断分站是否在同一直线上
			if(is_all_inline(coor_list, coor_size)){
				algorithm_locate_ex(coor_list, 2, hist, dest, dist_limit);
				break;
			}
			locatebycordinate(coor_list, dest);
			break;
		}
	default:
		break;
	}
}

double getAngleByAngle( _coordinate* p1, _coordinate* p2 )
{
	if(p1->x == p2->x) return 0;
	if(p1->y == p2->y) return 0;
	return double(p2->a - p1->a);
}

double getAngleByCoordinate( _coordinate* p1, _coordinate* p2 )
{
	if(p1->x == p2->x && p1->y == p2->y){ // 如果坐标相等，则跨越，不考虑折返情况
		return p1->a;
	}
	if(p1->x == p2->x){
		if(p1->y >= p2->y){
			return M_PI_2 * 3;
		}
		return M_PI_2;
	}
	if(p1->y == p2->y){
		if(p1->x > p2->x){
			return M_PI;
		}
		return 0;
	}
	return atan((p2->y - p1->y) / (p2->x - p1->x));
}

_coordinate* getCross( _coordinate* p1, _coordinate* p2 )
{
	_coordinate* p = new _coordinate; 
	double b1 = p1->y - tan(p1->a) * p1->x;
	double b2 = p2->y - tan(p2->a) * p2->x;

	if(p1->a == 0 || p1->a == M_PI){ // 第一个点水平
		p->y = p1->y;
		if(p2->a == M_PI_2 || p2->a == M_PI_2 * 3){ // 第二个点垂直
			p->x = p2->x;
		}else{ // 第二个点有角度
			p->x = (p->y - b2) / tan(p2->a);
		}
		return p;
	}else if(p1->a == M_PI_2 || p1->a == M_PI_2 * 3){ // 第一个点垂直
		p->x = p1->x;
		if(p2->a == 0 || p2->a == M_PI){ // 第二个点水平
			p->y = p2->y;
		}else{ // 第二个点有角度
			p->y = tan(p2->a) * p->x + b2;
		}
		return p;
	}else { // 有角度
		if(p2->a == 0 || p2->a == M_PI){ // 第二点水平
			p->y = p2->y;
			p->x = (p->y - b1) / tan(p1->a);
			return p;
		}else if(p2->a == M_PI_2 || p2->a == M_PI_2 * 3){ // 第二点垂直
			p->x = p2->x;
			p->y = p->x * tan(p1->a) + b1;
			return p;
		}
	}
	p->x = (b1-b2)/(tan(p2->a) - tan(p1->a));
	p->y = tan(p1->a) * p->x + b1;
	return p;
}

// 不考虑折返
void calcCoordinate( _coordinate* p1, double dist, double angle, _coordinate* dest )
{
	dest->x = p1->x;
	dest->y = p1->y;
	dest->z = p1->z;

	dest->x += dist * cos(angle);
	dest->y += dist * sin(angle);

	dest->a = angle;
}

double getDistance( _coordinate* p1, _coordinate *p2 )
{
	return sqrt(pow(p1->x - p2->x, 2) + pow(p1->y - p2->y, 2));
}

bool is_all_inline( _coordinate** coor_list, int cnt )
{
	bool ret = false;
	if(cnt < 3) return ret;
	if(coor_list[0]->x == coor_list[1]->x && coor_list[0]->x == coor_list[2]->x){
		ret = true;
	}else if(coor_list[0]->y == coor_list[1]->y && coor_list[0]->y == coor_list[2]->y){
		ret = true;
	}else{
		ret = false;
	}	
	return ret;
}

POS* LocateAlgorithm::Pos(ReaderPathMap rpm,int sta_num,int ant,double dist,INFO_PRE info_pre)
{
	POS* p = new POS;
	p->posx = 0;
	p->posy = 0;
	p->pos_radius = 999999.9; //inf在matlab中表示负无穷

	double d[2];
	double xcross[2];
	double ycross[2];

	int seg_num = 0;

	if(rpm.find(sta_num) == rpm.end())
	{
		return NULL;
	}
	seg_num = rpm.find(sta_num)->second->nRealCalcPoints - 1;

	if(seg_num < 0 ){
		return NULL;
	}

	int j = 0;
	for(int i =0;i<seg_num;i++){

		if(rpm.find(sta_num)->second->px[i]==-1||rpm.find(sta_num)->second->px[i+1]==-1)
		{
			continue;
		}

		SOLUTION *r = NULL;
		r = GetPos(rpm,sta_num,ant,dist,i);
		
		if(r == NULL){
			continue;
		}
		//x的列数count取2，是因为解最多为2
		int count = 2; 
		for(j =0;j<2;j++){
			xcross[j] = r->x[j];
			ycross[j] = r->y[j];
		}

		for(j = 0;j < count;j++){
			double d1 = xcross[j] - rpm.find(info_pre.sta_num)->second->x[info_pre.ant];
			double d2 = ycross[j] - rpm.find(info_pre.sta_num)->second->y[info_pre.ant];
			d[j] = sqrt(pow(d1,2) + pow(d2,2));
		}
		for(j = 0;j < count;j++){
			d[j] = fabs(d[j] - info_pre.dist);
		}

		double c = 999999.9;
		int k = -1;
		for(j = 0;j < count;j++){
			if(d[j] < c){
				c = d[j];
				k = j;
			}
		}

		if(c<p->pos_radius){
			p->posx = xcross[k];
			p->posy = ycross[k];
			p->pos_radius = c;
		}
	}

	return p;
}

/*
 * TOF定位：遍历路段集和指定分站天线为圆心的坐标
 *
 * param
 *		trpm			tof地图路径集
 *		reader_id		分站id
 *		antenna_id		分站天线索引
 *		dist			距离此分站天线的距离
 *		refer_data		参考数据信息
 *
 * return
 *		返回定位的坐标，如果无返回nullptr
 *
*/
std::shared_ptr<POS> LocateAlgorithm::tof_locate_1d(std::shared_ptr<TOFReaderPathMap> trpm,int reader_id,int antenna_idx,double dist,TOF_REFER_DATA refer_data)
{
	std::shared_ptr<POS> p = std::make_shared<POS>();

	double d[MAX_READER_TDOA_PATH_NUMS] = {0};
	double xcross[MAX_READER_TDOA_PATH_NUMS] = {0};
	double ycross[MAX_READER_TDOA_PATH_NUMS] = {0};

	int seg_num = 0;

	if(trpm->find(reader_id) == trpm->end() || trpm->find(refer_data.nReaderId) == trpm->end())
	{
		return nullptr;
	}
	seg_num = trpm->find(reader_id)->second->nRealCalcPoints - 1;
	//判断路径集的点数是否大于两个点，两点才可以构成线段路径
	if(seg_num < 0 ){
		return nullptr;
	}

	int j = 0;
	int m = 0;
	int nCounts = 0;
	//对每一个线段所在的直线和以天线为圆心的圆求解，最多为两个解
	for(int i = 0;i < seg_num;i++){
		//如果线段两端点的坐标不存在，则寻找下一个线段
		if(trpm->find(reader_id)->second->px[i] == -1 || trpm->find(reader_id)->second->px[i+1] == -1)
		{
			continue;
		}

		//求线段所在直线和分站readerid的antenna_idx天线为圆心，半径为dist的圆的两个交点
		std::shared_ptr<SOLUTION> r = GetPos(trpm,reader_id,antenna_idx,dist,i);
		//如果解不存在，则继续寻找下一个线段
		if(r == nullptr || r->nCount <= 0){
			continue;
		}

		int k = -1;
		double c = 999999.9;
		//如果解存在，则保存两个解
		for(j = 0;j < r->nCount;j++){
			//保存解
			xcross[j] = r->x[j];
			ycross[j] = r->y[j];
			//计算解与参考分站中的天线坐标的距离distance
			double d1 = xcross[j] - trpm->find(refer_data.nReaderId)->second->x[refer_data.nAntennaIndex];
			double d2 = ycross[j] - trpm->find(refer_data.nReaderId)->second->y[refer_data.nAntennaIndex];
			double distance = sqrt(pow(d1,2) + pow(d2,2));
			d[j] = distance;
		}

		for (j = 0;j < r->nCount; j++)
		{
			if (d[j] < c)
			{
				c = d[j];
				k = j;
			}
		}

		if(c < p->pos_radius){
			p->posx = xcross[k];
			p->posy = ycross[k];
			p->pos_radius = c;
		}
	}

	return p;
}

std::shared_ptr<POS> LocateAlgorithm::TdoaLocate2d(std::shared_ptr<ReceiveDataMap> pRdm)
{
	if (pRdm->size() < 3)
	{
		return nullptr;
	}
	
	double BS[4][2] = {0};
	double h[3] = {0};

	unsigned int i = 0;
	ReceiveDataMap::iterator first = pRdm->begin();
	for (ReceiveDataMap::iterator it = pRdm->begin();it != pRdm->end();++it)
	{
		BS[i][0] = it->second->x;
		BS[i][1] = it->second->y;
		if (i > 0)
		{
			h[i-1] = CFunctions::getDistance(it->first - first->first,CFunctions::TDOA);	
		}
		i++;
		if (i > pRdm->size())
		{
			break;
		}
	}
	std::shared_ptr<POS> p = std::make_shared<POS>();

	double a  = (BS[0][0]+BS[1][0]+BS[2][0])/2;
	double c1 = 1.49445;
	double c2 = 1.49445;
	int maxgen = 1000;   
	unsigned int sizepop = 100;   
	int Vmax = 100;
	int Vmin = -100;
	int popmax = 50000;
	int popmin = -50000;

	/*初始化参数*/
	/*产生初始粒子和速度*/
	unsigned int j = 0;
	double fitness[1][100] = {0};
	double qq1 = 0,qq2 = 0,re = 0;
	double pop[100][2] = {0},V[100][2] = {0};//sizepop = 100; 
	for(i=0;i<sizepop;i++)
	{
		for(j = 0;j<2;j++)
		{
			pop[i][j] =a* (rand()+1)/(RAND_MAX+2.0) ;
			V[i][j] = 10*(rand()+1)/(RAND_MAX+2.0);
		}
	}
	for(i = 0;i < sizepop;i++)
	{
		qq1 = pop[i][0];
		qq2 = pop[i][1];
		re = pow((sqrt((qq1-BS[1][0])*(qq1-BS[1][0])+( qq2-BS[1][1])*( qq2-BS[1][1]))-sqrt((qq1-BS[0][0])*(qq1-BS[0][0])+( qq2-BS[0][1])*( qq2-BS[0][1]))-h[0]),2)
			+pow(( sqrt((qq1-BS[2][0])*(qq1-BS[2][0])+( qq2-BS[2][1])*( qq2-BS[2][1]))-sqrt((qq1-BS[0][0])*(qq1-BS[0][0])+( qq2-BS[0][1])*( qq2-BS[0][1]))-h[1]),2);
		fitness[0][i]=re;
	}
	/*产生初始粒子和速度*/

	/*个体极值和群体极值*/
	double t = 0;
	double fitnesscopy[1][100] = {0};
	for(i=0;i<sizepop;i++)
	{
		fitnesscopy[0][i] = fitness[0][i];
	}

	for(i=0;i<sizepop;i++)
	{
		for(j=i+1;j<sizepop;j++)
		{
			if (fitnesscopy[0][i]>fitnesscopy[0][j])
			{
				t = fitnesscopy[0][i];
				fitnesscopy[0][i]=fitnesscopy[0][j];
				fitnesscopy[0][j] = t;
			}
		}
	}//正确
	double bestfitness = fitnesscopy[0][0];
	int bestindex = 0,kk = 0;
	for(i=0;i<sizepop;i++)
	{
		if(fitness[0][i]==bestfitness)
		{
			kk = i;
		}
	}
	bestindex = kk;//正确
	double zbest[1][2] = {0};
	for (j = 0;j<2;j++)
	{
		zbest[0][j] = pop[bestindex][j];//全局最佳
	}
	double gbest[100][2] = {0};
	for(i = 0;i<100;i++)
	{
		for (j = 0;j<2;j++)
		{
			gbest[i][j] = pop[i][j];//个体最佳
		}
	}
	double fitnessgbest[1][100] = {0};
	double fitnesszbest;
	for(i = 0;i<100;i++)
	{
		fitnessgbest[0][i]=fitness[0][i];
	}

	fitnesszbest = bestfitness;
	/*个体极值和群体极值*/
	/*迭代寻优*/
	double w;
	double ws = 0.9;
	double we = 0.4;
	for(i = 0;i<maxgen;i++)
	{
		w = ws - (ws-we)*(i/maxgen)*(i/maxgen);
		for(j=0;j<sizepop;j++)
		{
			V[j][0]= w*V[j][0]+c1*(rand()+1)/(RAND_MAX+2.0)*(gbest[j][0]-pop[j][0])+c2*(rand()+1)/(RAND_MAX+2.0)*(zbest[j][0]-pop[j][0]);
			V[j][1]= w*V[j][1]+c1*(rand()+1)/(RAND_MAX+2.0)*(gbest[j][1]-pop[j][1])+c2*(rand()+1)/(RAND_MAX+2.0)*(zbest[j][1]-pop[j][1]);   
			if(V[j][0]>Vmax)
			{
				V[j][0]=Vmax;
			}
			if(V[j][1]>Vmax)
			{
				V[j][1]=Vmax;
			}
			if(V[j][0]<Vmin)
			{
				V[j][0]=Vmin;
			}
			if(V[j][1]<Vmin)
			{
				V[j][1]=Vmin;
			}
			//种群更新//
			pop[j][0]=pop[j][0]+ V[j][0];
			pop[j][1]= pop[j][1]+V[j][1];
			if(pop[j][0]>popmax)
			{
				pop[j][0]=popmax;
			}
			if(pop[j][1]>popmax)
			{
				pop[j][1]=popmax;
			}
			if(pop[j][0]<popmin)
			{
				pop[j][0]=popmin;
			}
			if(pop[j][1]<popmin)
			{
				pop[j][1]=popmin;
			}
			//种群更新//
			double qq3,qq4,re1;    
			qq3 = pop[j][0];
			qq4 = pop[j][1];
			re1=pow((sqrt((qq3-BS[1][0])*(qq3-BS[1][0])+( qq4-BS[1][1])*( qq4-BS[1][1]))-sqrt((qq3-BS[0][0])*(qq3-BS[0][0])+( qq4-BS[0][1])*( qq4-BS[0][1]))-h[0]),2)
				+pow(( sqrt((qq3-BS[2][0])*(qq3-BS[2][0])+( qq4-BS[2][1])*( qq4-BS[2][1]))-sqrt((qq3-BS[0][0])*(qq3-BS[0][0])+( qq4-BS[0][1])*( qq4-BS[0][1]))-h[1]),2);
			fitness[0][j]=re1;
		}//正确
		for(j=0;j<sizepop;j++)
		{
			if( fitness[0][j] < fitnessgbest[0][j])
			{
				gbest[j][0] = pop[j][0];
				gbest[j][1] = pop[j][1];
				fitnessgbest[0][j] = fitness[0][j];
			}//个体最优更新

			if(fitness[0][j] < fitnesszbest)
			{
				zbest[0][0] = pop[j][0];
				zbest[0][1] = pop[j][1];
				fitnesszbest = fitness[0][j];
			}//群体最优更新
		}
	}

	p->posx = zbest[0][0];
	p->posy = zbest[0][1];

	return p;
}

std::shared_ptr<POS> LocateAlgorithm::TdoaLocate3d(std::shared_ptr<ReceiveDataMap> pRdm)
{
	if (pRdm->size() < 3)
	{
		return nullptr;
	}
	std::shared_ptr<POS> p = std::make_shared<POS>();

	double BS[4][3] = {0};
	double h[3] = {0};

	int i = 0;
	ReceiveDataMap::iterator first = pRdm->begin();
	for (ReceiveDataMap::iterator it = pRdm->begin();it != pRdm->end();++it)
	{
		BS[i][0] = it->second->x;
		BS[i][1] = it->second->y;
		BS[i][2] = it->second->z;
		if (i > 0)
		{
			h[i-1] = CFunctions::getDistance(it->first - first->first,CFunctions::TDOA);	
		}
		i++;
		if (i > pRdm->size())
		{
			break;
		}
	}

	/*初始化参数*/
	double a = (BS[0][0]+BS[1][0]+BS[2][0]+BS[3][0])/2;
	double c1=1.49445;
	double c2=1.49445;
	int maxgen = 1000;   
	int sizepop = 100;   
	int Vmax = 10;
	int Vmin = -10;
	int popmax = 50;
	int popmin = -50;

	/*初始化参数*/
	/*产生初始粒子和速度*/
	int j;
	double fitness[1][100] = {0};
	double qq1,qq2,qq5,re;
	double pop[100][3] = {0},V[100][3] = {0};//sizepop = 100; 
	//srand((unsigned)time(NULL));
	for(i=0;i<sizepop;i++)
	{
		for(j = 0;j<3;j++)
		{
			//pop[i][j] =a* (rand()+1)/(RAND_MAX+2.0) ;
			//V[i][j] = 10*(rand()+1)/(RAND_MAX+2.0);
			pop[i][j] =a* (( rand())/double(RAND_MAX));
			V[i][j] = 10* ( (rand())/double(RAND_MAX));
		}
	}//正确
	for(i=0;i<sizepop;i++)
	{
		qq1= pop[i][0];
		qq2= pop[i][1];
		qq5 = pop[i][2];
		re=pow(( sqrt((qq1-BS[1][0])*(qq1-BS[1][0])+( qq2-BS[1][1])*( qq2-BS[1][1])+(qq5-BS[1][2])*(qq5-BS[1][2])  )-sqrt((qq1-BS[0][0])*(qq1-BS[0][0])+( qq2-BS[0][1])*( qq2-BS[0][1])+(qq5-BS[0][2])*(qq5-BS[0][2] ))-h[0]),2)
			+pow(( sqrt((qq1-BS[2][0])*(qq1-BS[2][0])+( qq2-BS[2][1])*( qq2-BS[2][1])+(qq5-BS[2][2])*(qq5-BS[2][2])  )-sqrt((qq1-BS[0][0])*(qq1-BS[0][0])+( qq2-BS[0][1])*( qq2-BS[0][1])+ (qq5-BS[0][2])*(qq5-BS[0][2]))-h[1]),2)
			+pow(( sqrt((qq1-BS[3][0])*(qq1-BS[3][0])+( qq2-BS[3][1])*( qq2-BS[3][1])+(qq5-BS[3][2])*(qq5-BS[3][2])  )-sqrt((qq1-BS[0][0])*(qq1-BS[0][0])+( qq2-BS[0][1])*( qq2-BS[0][1])+ (qq5-BS[0][2])*(qq5-BS[0][2]))-h[2]),2);
		fitness[0][i]=re;
	}
	/*产生初始粒子和速度*/

	/*个体极值和群体极值*/
	double t;double fitnesscopy[1][100] = {0};
	for(i=0;i<sizepop;i++)
	{
		fitnesscopy[0][i] = fitness[0][i];
	}

	for(i=0;i<sizepop;i++)
	{
		for(j=i+1;j<sizepop;j++)
		{
			if (fitnesscopy[0][i]>fitnesscopy[0][j])
			{
				t = fitnesscopy[0][i];
				fitnesscopy[0][i]=fitnesscopy[0][j];
				fitnesscopy[0][j] = t;
			}
		}
	}//正确
	double bestfitness = fitnesscopy[0][0];
	int bestindex,kk;
	for(i=0;i<sizepop;i++)
	{
		if(fitness[0][i]==bestfitness)
		{
			kk = i;
		}
	}
	bestindex = kk;//正确
	double zbest[1][3] = {0};
	for (j = 0;j<3;j++)
	{
		zbest[0][j] = pop[bestindex][j];//全局最佳
	}
	double gbest[100][3] = {0};
	for(i = 0;i<100;i++)
	{
		for (j = 0;j<3;j++)
		{
			gbest[i][j] = pop[i][j];//个体最佳
		}
	}
	double fitnessgbest[1][100] = {0};
	double fitnesszbest;
	for(i = 0;i<100;i++)
	{
		fitnessgbest[0][i]=fitness[0][i];
	}

	fitnesszbest = bestfitness;
	/*个体极值和群体极值*/
	/*迭代寻优*/
	double w;
	double ws = 0.9;
	double we = 0.4;
	//srand((unsigned)time(NULL));
	for(i = 0;i<maxgen;i++)
	{
		w = ws - (ws-we)*(i/maxgen)*(i/maxgen);
		for(j=0;j<sizepop;j++)
		{
			//V[j][0]= w*V[j][0]+c1*(rand()+1)/(RAND_MAX+2.0)*(gbest[j][0]-pop[j][0])+c2*(rand()+1)/(RAND_MAX+2.0)*(zbest[j][0]-pop[j][0]);
			//V[j][1]= w*V[j][1]+c1*(rand()+1)/(RAND_MAX+2.0)*(gbest[j][1]-pop[j][1])+c2*(rand()+1)/(RAND_MAX+2.0)*(zbest[j][1]-pop[j][1]); 
			//V[j][2]= w*V[j][2]+c1*(rand()+1)/(RAND_MAX+2.0)*(gbest[j][2]-pop[j][2])+c2*(rand()+1)/(RAND_MAX+2.0)*(zbest[j][2]-pop[j][2]); 
			V[j][0]= w*V[j][0]+c1*((rand())/double(RAND_MAX))*(gbest[j][0]-pop[j][0])+c2*((rand())/double(RAND_MAX))*(zbest[j][0]-pop[j][0]);//rand(0-1)
			V[j][1]= w*V[j][1]+c1*((rand())/double(RAND_MAX))*(gbest[j][1]-pop[j][1])+c2*((rand())/double(RAND_MAX))*(zbest[j][1]-pop[j][1]); 
			V[j][2]= w*V[j][2]+c1*((rand())/double(RAND_MAX))*(gbest[j][2]-pop[j][2])+c2*((rand())/double(RAND_MAX))*(zbest[j][2]-pop[j][2]); 

			if(V[j][0]>Vmax)
			{
				V[j][0]=Vmax;
			}
			if(V[j][1]>Vmax)
			{
				V[j][1]=Vmax;
			}
			if(V[j][2]>Vmax)
			{
				V[j][2]=Vmax;
			}
			if(V[j][0]<Vmin)
			{
				V[j][0]=Vmin;
			}
			if(V[j][1]<Vmin)
			{
				V[j][1]=Vmin;
			}
			if(V[j][2]<Vmin)
			{
				V[j][2]=Vmin;
			}

			//种群更新//
			pop[j][0]=pop[j][0]+ V[j][0];
			pop[j][1]= pop[j][1]+V[j][1];
			pop[j][2]= pop[j][2]+V[j][2];
			if(pop[j][0]>popmax)
			{
				pop[j][0]=popmax;
			}
			if(pop[j][1]>popmax)
			{
				pop[j][1]=popmax;
			}
			if(pop[j][2]>popmax)
			{
				pop[j][2]=popmax;
			}
			if(pop[j][0]<popmin)
			{
				pop[j][0]=popmin;
			}
			if(pop[j][1]<popmin)
			{
				pop[j][1]=popmin;
			}
			if(pop[j][2]<popmin)
			{
				pop[j][2]=popmin;
			}
			//种群更新//
			double qq3,qq4,qq6,re1;    
			qq3 = pop[j][0];
			qq4 = pop[j][1];
			qq6= pop[j][2];
			re1=pow((sqrt((qq3-BS[1][0])*(qq3-BS[1][0])+( qq4-BS[1][1])*( qq4-BS[1][1])+(qq6-BS[1][2])*(qq6-BS[1][2])   )-sqrt((qq3-BS[0][0])*(qq3-BS[0][0])+( qq4-BS[0][1])*( qq4-BS[0][1])+(qq6-BS[0][2])*(qq6-BS[0][2] ))-h[0]),2)
				+pow(( sqrt((qq3-BS[2][0])*(qq3-BS[2][0])+( qq4-BS[2][1])*( qq4-BS[2][1])+(qq6-BS[2][2])*(qq6-BS[2][2])  )-sqrt((qq3-BS[0][0])*(qq3-BS[0][0])+( qq4-BS[0][1])*( qq4-BS[0][1])+ (qq6-BS[0][2])*(qq6-BS[0][2]))-h[1]),2)
				+pow(( sqrt((qq3-BS[3][0])*(qq3-BS[3][0])+( qq4-BS[3][1])*( qq4-BS[3][1])+(qq6-BS[3][2])*(qq6-BS[3][2])  )-sqrt((qq3-BS[0][0])*(qq3-BS[0][0])+( qq4-BS[0][1])*( qq4-BS[0][1])+ (qq6-BS[0][2])*(qq6-BS[0][2]))-h[2]),2);
			fitness[0][j]=re1;
		}//正确
		for(j=0;j<sizepop;j++)
		{
			if( fitness[0][j] < fitnessgbest[0][j])
			{
				gbest[j][0] = pop[j][0];
				gbest[j][1] = pop[j][1];
				gbest[j][2] = pop[j][2];
				fitnessgbest[0][j] = fitness[0][j];
			}//个体最优更新

			if(fitness[0][j] < fitnesszbest)
			{
				zbest[0][0] = pop[j][0];
				zbest[0][1] = pop[j][1];
				zbest[0][2] = pop[j][2];
				fitnesszbest = fitness[0][j];
			}//群体最优更新
		}
	}
	/*迭代寻优*/

	p->posx = zbest[0][0];
	p->posy = zbest[0][1];
	p->posz = zbest[0][2];

	return p;
}

/*
 * 通过fang算法计算tdoa 2维定位结果
 * 
 * param
 *		pRdm		分站数据集
 *
 * return
 *		返回定位结果
 *
*/
std::shared_ptr<POS> LocateAlgorithm::tdoa_locate_2d_by_fang(std::shared_ptr<ReceiveDataMap> pRdm)
{
	if (pRdm->size() < 3)
	{
		return nullptr;
	}
	std::shared_ptr<POS> p = std::make_shared<POS>();

	//2D定位
	std::vector<double> vtk;		//保存ki
	std::vector<double> vtd;		//保存di
	std::vector<_coordinate> vtc;	//保存xi,1  yi,1
	std::vector<_point> vtp;		//保存三角形的顶点坐标
	std::vector<int> vts;

	vtk.resize(0);
	vtd.resize(0);
	vtc.resize(0);
	vts.resize(0);

	_coordinate tc;

	int i = 0;
	unsigned long long time_stamp = 0;	//保存第一次的插值时间戳
	for (ReceiveDataMap::iterator it = pRdm->begin();it != pRdm->end()&&i<3;++it)
	{
		double k =  0 ;
		k = pow(it->second->x,2) + pow(it->second->y,2) ;//+ pow(it->second->z,2)
		vtk.push_back(k);

		if (i == 0 )
		{
			time_stamp = it->second->rec_time_stamp;
			tc.x = it->second->x;
			tc.y = it->second->y;
			//tc.z = it->second->z;
		}
		long long diff_time = it->second->rec_time_stamp - time_stamp;
		double d = 0;
		d = CFunctions::getDistance(diff_time,CFunctions::TDOA);

		if (i>0)
		{
			if (d>0)
			{
				vts.push_back(1);
			} 
			else
			{
				vts.push_back(-1);
			}
		}

		vtd.push_back(d);

		_coordinate dtc;
		dtc.x = it->second->x - tc.x;
		dtc.y = it->second->y - tc.y;
		//dtc.z = it->second->z - tc.z;
		vtc.push_back(dtc);

		_point p;
		p.x = it->second->x;
		p.y = it->second->y;
		vtp.push_back(p);
		i++;
	}

	double a[4] = {0};
	double dt = 0;

	dt = vtd[1]*vtc[2].y - vtd[2]*vtc[1].y;
	if (fabs(dt) < 1E-5)
	{
		return nullptr;
	}
	a[0] = (vtd[2]*vtc[1].x - vtd[1]*vtc[2].x)/dt;
	a[1] = (-1)*((vtk[1] - vtk[0] - pow(vtd[1],2))*vtd[2] - (vtk[2] - vtk[0] - pow(vtd[2],2))*vtd[1])*0.5/dt;

	if (fabs(vtd[1]) < 1E-5)
	{
		return nullptr;
	}
	a[2] = 0.5*(vtk[1] - vtk[0] - pow(vtd[1],2) - 2*vtc[1].y*a[1])/vtd[1];
	a[3] = -1*(vtc[1].x + vtc[1].y*a[0])/vtd[1];

	double A = 0, B = 0, C = 0;
	A = pow(a[3],2) - 1 - pow(a[0],2);
	B = 2*(a[2]*a[3] + tc.x + a[0]*(tc.y - a[1]));
	C = pow(a[2],2) - pow(tc.x,2) - pow(tc.y-a[1],2);

	_point pos[MAX_READER_TDOA_PATH_NUMS];
	double delta = 0.0;
	int count = 0;

	delta = pow(B,2) - 4*A*C;
	if (delta > 0)
	{
		pos[0].x = ((-1)*B + sqrt(delta))/(2*A);
		pos[0].y = a[0]*pos[0].x + a[1];
		//TRACE(_T("x1: %.2f , y1: %.2f \r\n"),pos[0].x,pos[0].y);
		pos[1].x = ((-1)*B - sqrt(delta))/(2*A);
		pos[1].y = a[0]*pos[1].x + a[1];
		//TRACE(_T("x2: %.2f , y2: %.2f \r\n"),pos[1].x,pos[1].y);
		count  = 2;
	}else{
		return nullptr;
	}

	int idx = -1;
	int points = 0;
	for (int i=0;i<count;i++)
	{
		//if (IsInTriangle(vtp,pos[i]))
		{
			//TRACE(_T("point is in triangle. the index is : %d \r\n"),i);
			bool condition[2] = {false,false};
			double d1 = 0,d2 = 0;

			//对两解进行判断，需要同时满足两个条件
			//1.解到点1和点2的距离差的方向性和之前的参数相同
			//2.解到点1和点3的距离差的方向性和之前的参数相同
			d1 = sqrt(pow(vtp[1].x - pos[i].x,2) + pow(vtp[1].y - pos[i].y,2)) - sqrt(pow(vtp[0].x - pos[i].x,2) + pow(vtp[0].y - pos[i].y,2));
			d2 = sqrt(pow(vtp[2].x - pos[i].x,2) + pow(vtp[2].y - pos[i].y,2)) - sqrt(pow(vtp[0].x - pos[i].x,2) + pow(vtp[0].y - pos[i].y,2));

			if ((d1 < 0&&vts[0]==-1)||(d1>0&&vts[0] == 1))
			{
				condition[0] = true;
			}
			if ((d2 < 0&&vts[1]==-1)||(d2>0&&vts[1] == 1))
			{
				condition[1] =true;
			}

			if (condition[0]&&condition[1])
			{
				idx = i;
			}
		}//else{
			//TRACE(_T("point is not in triangle. the index is : %d \r\n"),i);
		//}
	}

	if (points == 2)
	{
		idx = -1;
		//TRACE(_T("There hava 2 point.\r\n"));
	}

	if (idx != -1)
	{
		//TRACE(_T("the idx is : %d \r\n"),idx);
		p->posx = pos[idx].x;
		p->posy = pos[idx].y;
	}else{
		return nullptr;
	}

	return p;
}

std::vector<std::shared_ptr<POS>> LocateAlgorithm::tdoa_locate_2d_by_fang(std::shared_ptr<ReceiveDataUnorderedMap> pRdm)
{
	std::vector<std::shared_ptr<POS>> vt_pos;
	vt_pos.resize(0);

	if (pRdm->size() < 3)
	{
		return std::move(vt_pos);
	}

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

	//2D定位
	std::vector<double> vtk;		//保存ki
	std::vector<double> vtd;		//保存di
	std::vector<_coordinate> vtc;	//保存xi,1  yi,1
	std::vector<_point> vtp;		//保存三角形的顶点坐标
	std::vector<int> vts;

	vtk.resize(0);
	vtd.resize(0);
	vtc.resize(0);
	vts.resize(0);

	_coordinate tc;

	int i = 0;
	unsigned long long time_stamp = 0;	//保存第一次的插值时间戳
	for (auto it = pRdm->begin();it != pRdm->end()&&i<3;++it)
	{
		double k =  0 ;
		k = pow(it->second->x,2) + pow(it->second->y,2) ;//+ pow(it->second->z,2)
		vtk.push_back(k);

		if (i == 0 )
		{
			time_stamp = it->second->rec_time_stamp;
			tc.x = it->second->x;
			tc.y = it->second->y;
			//tc.z = it->second->z;
		}
		long long diff_time = it->second->rec_time_stamp - time_stamp;
		double d = 0;
		d = CFunctions::getDistance(diff_time,CFunctions::TDOA);

		if (i>0)
		{
			if (d>0)
			{
				vts.push_back(1);
			} 
			else
			{
				vts.push_back(-1);
			}
		}

		vtd.push_back(d);

		_coordinate dtc;
		dtc.x = it->second->x - tc.x;
		dtc.y = it->second->y - tc.y;
		//dtc.z = it->second->z - tc.z;
		vtc.push_back(dtc);

		_point p;
		p.x = it->second->x;
		p.y = it->second->y;
		vtp.push_back(p);
		i++;
	}

	double a[4] = {0};
	double dt = 0;

	dt = vtd[1]*vtc[2].y - vtd[2]*vtc[1].y;
	if (fabs(dt) < ZERO_PRECISION)
	{
		//return nullptr;
		return std::move(vt_pos);
	}
	a[0] = (vtd[2]*vtc[1].x - vtd[1]*vtc[2].x)/dt;
	a[1] = (-1)*((vtk[1] - vtk[0] - pow(vtd[1],2))*vtd[2] - (vtk[2] - vtk[0] - pow(vtd[2],2))*vtd[1])*0.5/dt;

	if (fabs(vtd[1]) < ZERO_PRECISION)
	{
		//return nullptr;
		return std::move(vt_pos);
	}
	a[2] = 0.5*(vtk[1] - vtk[0] - pow(vtd[1],2) - 2*vtc[1].y*a[1])/vtd[1];
	a[3] = -1*(vtc[1].x + vtc[1].y*a[0])/vtd[1];

	double A = 0, B = 0, C = 0;
	A = pow(a[3],2) - 1 - pow(a[0],2);
	B = 2*(a[2]*a[3] + tc.x + a[0]*(tc.y - a[1]));
	C = pow(a[2],2) - pow(tc.x,2) - pow(tc.y-a[1],2);

	_point pos[MAX_READER_TDOA_PATH_NUMS];
	double delta = 0.0;
	int count = 0;

	delta = pow(B,2) - 4*A*C;
	if (delta > 0)
	{
		pos[0].x = ((-1)*B + sqrt(delta))/(2*A);
		pos[0].y = a[0]*pos[0].x + a[1];
		//TRACE(_T("x1: %.2f , y1: %.2f \r\n"),pos[0].x,pos[0].y);
		pos[1].x = ((-1)*B - sqrt(delta))/(2*A);
		pos[1].y = a[0]*pos[1].x + a[1];
		//TRACE(_T("x2: %.2f , y2: %.2f \r\n"),pos[1].x,pos[1].y);
		count  = 2;
	}else{
		//return nullptr;
		return std::move(vt_pos);
	}

	for (auto i = 0;i < count;i++)
	{
		bool condition[2] = {false,false};
		double d1 = 0,d2 = 0;

		d1 = sqrt(pow(vtp[1].x - pos[i].x,2) + pow(vtp[1].y - pos[i].y,2)) - sqrt(pow(vtp[0].x - pos[i].x,2) + pow(vtp[0].y - pos[i].y,2));
		d2 = sqrt(pow(vtp[2].x - pos[i].x,2) + pow(vtp[2].y - pos[i].y,2)) - sqrt(pow(vtp[0].x - pos[i].x,2) + pow(vtp[0].y - pos[i].y,2));

		if ((d1 < 0&&vts[0]==-1)||(d1>0&&vts[0] == 1))
		{
			condition[0] = true;
		}
		if ((d2 < 0&&vts[1]==-1)||(d2>0&&vts[1] == 1))
		{
			condition[1] =true;
		}

		if (condition[0]&&condition[1])
		{
			std::shared_ptr<POS> p = std::make_shared<POS>();
			p->posx = pos[i].x;
			p->posy = pos[i].y;
			vt_pos.push_back(std::move(p));
		}
	}

	for (std::vector<std::shared_ptr<POS>>::iterator it_pos = vt_pos.begin();it_pos != vt_pos.end();)
	{
		_point p;
		p.x = (*it_pos)->posx;
		p.y = (*it_pos)->posy;
		p.z = 0;
		if (!IsInTriangle(vtp,p))
		{
			it_pos = vt_pos.erase(it_pos);
		}else{
			it_pos++;
		}
	}
	////根据解是否在三角形内部来取舍点
	//int idx = -1;
	//int points = 0;
	//for (int i = 0;i<count;i++)
	//{
	//	if (IsInTriangle(vtp,pos[i]))
	//	{
	//		TRACE(_T("point is in triangle. the index is : %d \r\n"),i);
	//		bool condition[2] = {false,false};
	//		double d1 = 0,d2 = 0;

	//		//对两解进行判断，需要同时满足两个条件
	//		//1.解到点1和点2的距离差的方向性和之前的参数相同
	//		//2.解到点1和点3的距离差的方向性和之前的参数相同
	//		d1 = sqrt(pow(vtp[1].x - pos[i].x,2) + pow(vtp[1].y - pos[i].y,2)) - sqrt(pow(vtp[0].x - pos[i].x,2) + pow(vtp[0].y - pos[i].y,2));
	//		d2 = sqrt(pow(vtp[2].x - pos[i].x,2) + pow(vtp[2].y - pos[i].y,2)) - sqrt(pow(vtp[0].x - pos[i].x,2) + pow(vtp[0].y - pos[i].y,2));

	//		if ((d1 < 0&&vts[0]==-1)||(d1>0&&vts[0] == 1))
	//		{
	//			condition[0] = true;
	//		}
	//		if ((d2 < 0&&vts[1]==-1)||(d2>0&&vts[1] == 1))
	//		{
	//			condition[1] =true;
	//		}

	//		if (condition[0]&&condition[1])
	//		{
	//			idx = i;
	//		}
	//	}else{
	//		TRACE(_T("point is not in triangle. the index is : %d \r\n"),i);
	//	}
	//}

	//if (points == 2)
	//{
	//	idx = -1;
	//	//TRACE(_T("There hava 2 point.\r\n"));
	//}

	//if (idx != -1)
	//{
	//	//TRACE(_T("the idx is : %d \r\n"),idx);
	//	p->posx = pos[idx].x;
	//	p->posy = pos[idx].y;
	//}else{
	//	//return nullptr;
	//	return std::move(vt_pos);
	//}

	return std::move(vt_pos);;
}

/*
 * 计算tdoa中，由于高度误差存在影响，计算去掉高度误差后的新距离差
 *
 * param
 *		md		实际测量距离差
 *		d		距离差两分站之间的距离
 *		h		高度误差
 *
 * return
 *		返回实际的距离差
 *
*/
double LocateAlgorithm::cal_real_distance_diff(const double& md,const double& d,const double& h,const int& s)
{
	double A = 0.0,B = 0.0,C = 0.0;

	A = 2*md;
	B = 2*d;
	C = 2*md*pow(h,2) - pow(d,2) + pow(md,2);

	double delta = 0.0;
	delta = pow(B,2) - 4*A*C;

	if (delta > 0)
	{
		/*	int sign = 0;
		double d1 = 0.0, d2 = 0.0;

		for (unsigned int i = 1;i < 3;i++)
		{
		d1 = 0.0, d2 = 0.0;
		d1 = (-1*B + pow(-1,i)*sqrt(delta))/(2*A);
		d2 = d - d1;
		if (d1 - d2 > 0)
		{
		sign = 1;
		}else{
		sign = -1;
		}

		if (sign == s)
		{
		break;
		}
		}*/
	}else if (fabs(delta) < ZERO_PRECISION)
	{
	}else{
		return INVALID_COORDINATE;
	}
	return INVALID_COORDINATE;
}

/*
 * TDOA算法实现
 * 函数名：LocatePos,此函数与Pos函数的区别是本函数两两遍历求坐标
 *		遍历求解
 * param
 *		pRdm	------		存放参与计算的分站信息，主要信息包含同步后的时间戳
 *	    trpm    ------		地图集
 *
 * return
 *	    返回最终结果坐标
 *
*/
std::unique_ptr<POS> LocateAlgorithm::LocatePos(std::shared_ptr<ReceiveDataMap> pRdm,std::shared_ptr<TDOAReaderPathMap> trpm)
{
	std::unique_ptr<POS> pos(new POS);

	int nNoReaderPathIdx = 0;
	int nFirstReader[MAX_READER_TDOA_PATH_NUMS] = {-1};
	int nSecondReader[MAX_READER_TDOA_PATH_NUMS] ={-1};

	_coordinate r;
	int r_idx = 0;
	r.reason = 0;

	for (ReceiveDataMap::iterator first = pRdm->begin();first != pRdm->end();++first)
	{
		//如果两级都能找到才运行继续后续操作，否则，表明没有此路径地图集
		TDOAReaderPathMap::iterator rdm_it = trpm->find(first->second->reader_id);
		if(rdm_it == trpm->end()){
			continue;
		}

		double ref_dist = 0.0;   //两分站之间无路径的距离差
		_coordinate res[MAX_READER_TDOA_PATH_NUMS];
		int res_idx = 0;
		for(int i = 0;i < MAX_READER_TDOA_PATH_NUMS;i++){
			res[i].x = INVALID_COORDINATE;
			res[i].y = INVALID_COORDINATE;
			res[i].z = INVALID_COORDINATE;
		}
		//存储无路径的两分站的id和坐标
		ReceiveData tmp_reader[MAX_READER_TDOA_PATH_NUMS];

		//存储和第一条分站存在路径的分站信息
		ReceiveData tmp_dist_reader[MAX_READER_TDOA_PATH_NUMS];
		//和第一个分站存在地图集的分站个数，在丁字或十字路口，数量可能为3个或4个
		int nDistReaders = 0;

		//获取第一个时间戳
		ReceiveData f1;
		f1.antenna_id = first->second->antenna_id;
		f1.reader_id = first->second->reader_id;
		f1.rec_time_stamp = first->second->rec_time_stamp;
		f1.x = first->second->x;
		f1.y = first->second->y;
		f1.z = first->second->z;

		ReceiveDataMap::iterator second = first;
		std::advance(second,1);
		//从第二个开始遍历
		for(;second != pRdm->end();++second){
			//获取第二个时间戳
			ReceiveData f2;
			f2.antenna_id = second->second->antenna_id;
			f2.reader_id = second->second->reader_id;
			f2.rec_time_stamp = second->second->rec_time_stamp;
			f2.x = second->second->x;
			f2.y = second->second->y;
			f2.z = second->second->z;

			//时间戳异常
			if(f1.rec_time_stamp == LLONG_MAX || f2.rec_time_stamp == LLONG_MAX){
				continue;
			}

			if(f1.reader_id == f2.reader_id){
				continue;
			}

			ReaderPathMap::iterator rpm_it = trpm->find(f1.reader_id)->second->find(f2.reader_id);
			if(rpm_it == trpm->find(f1.reader_id)->second->end()){
				continue;
			}

			//根据距离的正负，后续判断计算位置取舍时使用
			int nSign = 1;
			long long diffTime = f1.rec_time_stamp - f2.rec_time_stamp;

			//计算位置
			double dist = CFunctions::getDistance(diffTime,CFunctions::TDOA);

			double readers_dist = sqrt(pow(f1.x - f2.x,2) + pow(f1.y - f2.y,2));

			if(fabs(dist) - readers_dist > 0){
				continue;
			}

			//如果和第一条分站存在地图集
			tmp_dist_reader[nDistReaders].reader_id = f2.reader_id;
			tmp_dist_reader[nDistReaders].x = f2.x;
			tmp_dist_reader[nDistReaders].y = f2.y;
			tmp_dist_reader[nDistReaders].z = f2.z;
			tmp_dist_reader[nDistReaders].rec_time_stamp = f2.rec_time_stamp;
			nDistReaders++;

			std::shared_ptr<ReaderPath> pRP = trpm->find(f1.reader_id)->second->find(f2.reader_id)->second;

			tmp_reader[nNoReaderPathIdx].reader_id = f2.reader_id;
			tmp_reader[nNoReaderPathIdx].x = f2.x;
			tmp_reader[nNoReaderPathIdx].y = f2.y;
			tmp_reader[nNoReaderPathIdx].z = f2.z;
			tmp_reader[nNoReaderPathIdx].rec_time_stamp = f2.rec_time_stamp;
			nNoReaderPathIdx++;

			//两分站之间的线段个数
			int seg_num = pRP->nRealCalcPoints - 1;

			if(seg_num == 0 || seg_num > 100){
				continue;
			}

			//因为双曲线与分站之间第i条线段或者第j条线段分别有两焦点
			//或者分站之间就一条直线，有两焦点
			double xcross[2] = {INVALID_COORDINATE,INVALID_COORDINATE};
			double ycross[2] = {INVALID_COORDINATE,INVALID_COORDINATE};
			double zcross[2] = {INVALID_COORDINATE,INVALID_COORDINATE};

			int nIdx = 0;

			//根据线段个数开始计算
			for(int i = 0;i < seg_num;i++){
				//计算位置坐标，双曲线和线段相交的交点
				std::shared_ptr<SOLUTION> r = LocateAlgorithm::GetPos(pRP,dist,i);

				//无解或解无效
				if(r == nullptr ){
					continue;
				}
				if(r->nCount == 0){
					continue;
				}

				if(r->nCount == 1){
					xcross[nIdx] = r->x[0];
					ycross[nIdx] = r->y[0];

					res[res_idx].x = xcross[nIdx];
					res[res_idx].y = ycross[nIdx];
					res[res_idx].z = zcross[nIdx];

					nIdx++;
					res_idx++;
				}

				if(r->nCount == 2){
					for(int j = 0; j < 2;j++){
						xcross[j] = r->x[j];
						ycross[j] = r->y[j];
						nIdx++;
					}
				}

				if(nIdx == 2){
					//解到两焦点之间的距离
					double deltad[2] = {0};
					for(int j = 0; j < 2;j ++){
						double d[2] = {0};
						double dx1 = xcross[j] - pRP->x[0];
						double dy1 = ycross[j] - pRP->y[0];
						d[0] = sqrt(pow(dx1,2) + pow(dy1,2));

						double dx2 = xcross[j] - pRP->x[1];
						double dy2 = ycross[j] - pRP->y[1];
						d[1] = sqrt(pow(dx2,2) + pow(dy2,2));

						deltad[j] = d[0] - d[1];
					}

					int idx = 0;	

					//应该和之前计算的dist同方向
					if(dist > 0){
						for(int j = 0;j < 2;j++){
							if(deltad[j] > 0){
								idx = j;
								break;
							}
						}	
					}else{
						for(int j = 0;j < 2;j++){
							if(deltad[j] < 0){
								idx = j;
								break;
							}
						}	
					}
					nFirstReader[res_idx]  = f1.reader_id;
					nSecondReader[res_idx] = f2.reader_id;

					res[res_idx].x = xcross[idx];
					res[res_idx].y = ycross[idx];
					res[res_idx].z = zcross[idx];
					res_idx++;
				}		
				continue;
			}
		}

		if(res_idx == 1){
			bool bValid = true;

			//如果定位坐标在分站附近，需要判断此分站是否为special，如果为special则接受此定位结果
			for(ReceiveDataMap::iterator it = pRdm->begin();it!=pRdm->end();++it){
				double dist = sqrt(pow(res[0].x - it->second->x,2) + pow(res[0].y - it->second->y,2));	
				if(dist<4){
					if(it->second->special == 0){
						r.reason = ALGO_FAILED_CONDITION_12;
						ALGORITHM_FAILED(ALGO_FAILED_CONDITION_12);
						bValid = false;
					}
				}
			}

			if(pRdm->size()>=3&&bValid){
				double dist[2] = {0};
				//更改选取分站方式:
				//选取和第一个分站有地图集的两个分站来参与判别，
				//例如：分站顺序是2，1,3，其中（1,2）和（2,3）之间有地图集，那么就选取1,3来参与判别
				//如果分站顺序是1,2，3，其中（1，2）有地图集，(1,3)之间无地图集，那么此时就不使用以下方法进行判别
				if(nDistReaders == 2){
					dist[0] = sqrt(pow(res[0].x - tmp_dist_reader[0].x,2) + pow(res[0].y - tmp_dist_reader[0].y,2));
					dist[1] = sqrt(pow(res[0].x - tmp_dist_reader[1].x,2) + pow(res[0].y - tmp_dist_reader[1].y,2));

					double dif1 = fabs(dist[0] - dist[1]);

					long long dt = tmp_dist_reader[0].rec_time_stamp - tmp_dist_reader[1].rec_time_stamp;
					double dif2 = fabs(CFunctions::getDistance(dt,CFunctions::TDOA));

					double dif = fabs(dif1 - dif2);

					if(dif>10){
						r.reason = ALGO_FAILED_CONDITION_13;
						ALGORITHM_FAILED(ALGO_FAILED_CONDITION_13);
						bValid = false;
					}	
				}
			}

			if(bValid){
				r.x = res[0].x;
				r.y = res[0].y;
				r.z = res[0].z;
				for (int i=0;i<MAX_READER_TDOA_PATH_NUMS;i++)
				{
					pos->dDiff[i] = 0;
				}
			}
		}else if(res_idx >= 2){
			double d = 99999.9;

			long long dt = tmp_reader[0].rec_time_stamp - tmp_reader[1].rec_time_stamp;
			//计算位置
			double ref_dist = abs(CFunctions::getDistance(dt,CFunctions::TDOA));

			for(int i = 0;i<res_idx;i++){
				double d_tmp = abs(sqrt(pow(res[i].x - tmp_reader[0].x,2) + pow(res[i].y - tmp_reader[0].y,2)) - sqrt(pow(res[i].x - tmp_reader[1].x,2) + pow(res[i].y - tmp_reader[1].y,2)));
				double d_diff = abs(d_tmp - ref_dist);
				pos->dDiff[i] = d_diff;
				if(d_diff < d){
					d = d_diff;
					r_idx = i;
				}
			}

			if(pRdm->size()>=3){
				if(pos->dDiff[r_idx] < 4){
					r.x = res[r_idx].x;
					r.y = res[r_idx].y;
					r.z = res[r_idx].z;
				}else{
					r.reason = ALGO_FAILED_CONDITION_14;
					ALGORITHM_FAILED(ALGO_FAILED_CONDITION_14);
				}
			}
		}else{
			//解的个数<=0
			continue;
		}

		//表示找到解了，后面的数据都不可信
		if (r.x != INVALID_COORDINATE && r.y != INVALID_COORDINATE)
		{
			break;
		}
	}
	//最终得到的结果解
	pos->posx = r.x;
	pos->posy = r.y;
	pos->posz = 0;
	pos->reason = r.reason;

	if(pos->posx != INVALID_COORDINATE && pos->posy != INVALID_COORDINATE){
		pos->nFirstReader = nFirstReader[r_idx];
		pos->nSecondReader = nSecondReader[r_idx];
	}

	return pos;
}

SOLUTION* LocateAlgorithm::GetPos(ReaderPathMap rpm,int sta_num,int ant,double dist,int i)
{
	double x1 = 0.0;
	double y1 = 0.0;
	double x2 = 0.0;
	double y2 = 0.0;

	double deta = 0.0;  //b^2 - 4*a*c;
	int count = 0;	//方程解的个数

	if((rpm.find(sta_num)->second->px[i] - rpm.find(sta_num)->second->px[i+1]) == 0){
		//方程一的解
		x1 = rpm.find(sta_num)->second->px[i];
		y1 = rpm.find(sta_num)->second->y[ant] - dist;
		x2 = rpm.find(sta_num)->second->px[i];
		y2 = dist + rpm.find(sta_num)->second->y[ant];
		count = 2;
	}else{
		//计算斜率
		double k = (rpm.find(sta_num)->second->py[i] - rpm.find(sta_num)->second->py[i+1])/(rpm.find(sta_num)->second->px[i] - rpm.find(sta_num)->second->px[i+1]);

		//方程二
		//确保有两解的情况
		//圆心为s[sta_num].x[ant],s[sta_num].y[ant];
		double a = pow(k,2) + 1;
		double b = 2*(k*(rpm.find(sta_num)->second->py[i] - k*rpm.find(sta_num)->second->px[i] - rpm.find(sta_num)->second->y[ant])- rpm.find(sta_num)->second->x[ant]);
		double d = rpm.find(sta_num)->second->py[i] - k * rpm.find(sta_num)->second->px[i] - rpm.find(sta_num)->second->y[ant]; 
		double c = pow(rpm.find(sta_num)->second->x[ant],2) + pow(d,2) - pow(dist,2);

		//根据deta = b^2 - 4*a*c 判断方程是否有解
		deta = pow(b,2) - 4*a*c; //
		if(deta>0){
			count = 2;
		}else if(deta == 0){
			count = 1;
		}
		else{
			count = 0;
		}

		if(count == 0){
			return NULL;
		}

		//得两解
		x1 = -(b + sqrt(deta))/(2*a);
		x2 = (-b + sqrt(deta) )/(2*a);
		y1 = k*x1 + rpm.find(sta_num)->second->py[i] - k*rpm.find(sta_num)->second->px[i];
		y2 = k*x2 + rpm.find(sta_num)->second->py[i] - k*rpm.find(sta_num)->second->px[i];
	}

	double x[3] = {0,0,0};
	double y[3] = {0,0,0};
	
	x[0] = x1;
	x[1] = x2;
	y[0] = y1;
	y[1] = y2;

	if(count>0){
		for(int t = 0;t<count;t++){
			if(x[t] < min(rpm.find(sta_num)->second->px[i],rpm.find(sta_num)->second->px[i+1]) - 0.01
				||x[t] > max(rpm.find(sta_num)->second->px[i],rpm.find(sta_num)->second->px[i+1]) + 0.01
				||y[t] < min(rpm.find(sta_num)->second->py[i],rpm.find(sta_num)->second->py[i+1])
				||y[t] > max(rpm.find(sta_num)->second->py[i],rpm.find(sta_num)->second->py[i+1])){
				x[t] = 0;
				y[t] = 0;
			}
		}
	}

	SOLUTION* result = new SOLUTION;
	memset(result->x,0,3*sizeof(double));
	memset(result->y,0,3*sizeof(double));
	memset(result->z,0,3*sizeof(double));
	result->x[0] = x[0];
	result->x[1] = x[1];
	result->y[0] = y[0];
	result->y[1] = y[1];
	result->z[0] = 0;
	result->z[1] = 0;

	return result;
}

/*
 *
 * param
 *		trpm
 *		sta_num
 *		ant
 *		dist
 *		i
 *
 * return
 *
*/
std::shared_ptr<SOLUTION> LocateAlgorithm::GetPos(std::shared_ptr<TOFReaderPathMap> trpm,int reader_id,int antenna_idx,double dist,int seg_idx)
{
	std::shared_ptr<SOLUTION> s = std::make_shared<SOLUTION>();

	double x1 = 0.0;
	double y1 = 0.0;
	double x2 = 0.0;
	double y2 = 0.0;

	double deta = 0.0;  //b^2 - 4*a*c;
	int count = 0;	//方程解的个数

	if(abs(trpm->find(reader_id)->second->px[seg_idx] - trpm->find(reader_id)->second->px[seg_idx + 1]) < ZERO_PRECISION){
		//方程一的解
		x1 = trpm->find(reader_id)->second->px[seg_idx];
		y1 = trpm->find(reader_id)->second->y[antenna_idx] - dist;
		x2 = trpm->find(reader_id)->second->px[seg_idx];
		y2 = dist + trpm->find(reader_id)->second->y[antenna_idx];
		count = 2;
	}/*else if(abs(trpm->find(sta_num)->second->py[i] - trpm->find(sta_num)->second->py[i+1]) < ZERO_PRECISION){
		y1 = y2 = trpm->find(sta_num)->second->py[i];
		x1 = trpm->find(sta_num)->second->y[ant] - dist;
		x2 = trpm->find(sta_num)->second->y[ant] + dist;
		count = 2;
	}*/else{
		//计算斜率
		double k = (trpm->find(reader_id)->second->py[seg_idx] - trpm->find(reader_id)->second->py[seg_idx + 1])/(trpm->find(reader_id)->second->px[seg_idx] - trpm->find(reader_id)->second->px[seg_idx + 1]);
		//double k = (trpm->find(reader_id)->second->y[seg_idx] - trpm->find(reader_id)->second->y[seg_idx + 1])/(trpm->find(reader_id)->second->x[seg_idx] - trpm->find(reader_id)->second->x[seg_idx + 1]);
		//3.4286
		//double b = trpm->find(reader_id)->second->y[antenna_idx] - k*trpm->find(reader_id)->second->x[antenna_idx];
		double b = trpm->find(reader_id)->second->py[seg_idx] - k*trpm->find(reader_id)->second->px[seg_idx];
		double A = pow(k,2) + 1;
		double B = 2*k*(b - trpm->find(reader_id)->second->y[antenna_idx]) - 2*trpm->find(reader_id)->second->x[antenna_idx];
		double C = pow(trpm->find(reader_id)->second->x[antenna_idx],2) + pow(b - trpm->find(reader_id)->second->y[antenna_idx],2) - pow(dist,2);
		
		deta = pow(B,2) - 4*A*C;
		//方程二
		//确保有两解的情况
		//圆心为s[sta_num].x[ant],s[sta_num].y[ant];
		//double a = pow(k,2) + 1;
		//double b = 2*(k*(trpm->find(sta_num)->second->py[i] - k*trpm->find(sta_num)->second->px[i] - trpm->find(sta_num)->second->y[ant])- trpm->find(sta_num)->second->x[ant]);
		//double d = trpm->find(sta_num)->second->py[i] - k * trpm->find(sta_num)->second->px[i] - trpm->find(sta_num)->second->y[ant]; 
		////double b = 2*k*d - 2*trpm->find(sta_num)->second->px[i];
		//double c = pow(trpm->find(sta_num)->second->x[ant],2) + pow(d,2) - pow(dist,2);

		////根据deta = b^2 - 4*a*c 判断方程是否有解
		//deta = pow(b,2) - 4*a*c; //
		if(deta>0){
			count = 2;
		}else if(deta == 0){
			count = 1;
		}
		else{
			count = 0;
		}

		if(count == 0){
			return NULL;
		}

		//得两解
		x1 = -(B + sqrt(deta))/(2*A);
		x2 = (-B + sqrt(deta) )/(2*A);
		//x1 = -(b + sqrt(deta))/(2*a);
		//x2 = (-b + sqrt(deta) )/(2*a);
		y1 = k*x1 + trpm->find(reader_id)->second->py[seg_idx] - k*trpm->find(reader_id)->second->px[seg_idx];
		y2 = k*x2 + trpm->find(reader_id)->second->py[seg_idx] - k*trpm->find(reader_id)->second->px[seg_idx];
	}

	double x[3] = {0,0,0};
	double y[3] = {0,0,0};

	x[0] = x1;
	x[1] = x2;
	y[0] = y1;
	y[1] = y2;

	int nValidCount = count;
	if(count > 0){
		for(int t = 0;t < count;t++){
			if(x[t] < min(trpm->find(reader_id)->second->px[seg_idx],trpm->find(reader_id)->second->px[seg_idx+1])
				||x[t] > max(trpm->find(reader_id)->second->px[seg_idx],trpm->find(reader_id)->second->px[seg_idx+1])
				||y[t] < min(trpm->find(reader_id)->second->py[seg_idx],trpm->find(reader_id)->second->py[seg_idx+1])
				||y[t] > max(trpm->find(reader_id)->second->py[seg_idx],trpm->find(reader_id)->second->py[seg_idx+1])){
					x[t] = INVALID_COORDINATE;
					y[t] = INVALID_COORDINATE;
					nValidCount--;
			}
		}
	}

	s->nCount = nValidCount;
	switch (s->nCount)
	{
	case 1:
		for (int i = 0;i < count;i++)
		{
			if (x[i] != INVALID_COORDINATE && y[i] != INVALID_COORDINATE)
			{
				s->x[0] = x[i];
				s->y[0] = y[i];
				break;
			}
		}
		break;
	case 2:
		for (int i = 0;i < s->nCount;i++)
		{
			s->x[i] = x[i];
			s->y[i] = y[i];
		}
		break;
	default:
		break;
	}

	return s;
}

/*
 * Tof二维定位实现
 * 采用最小二乘法，对多个方程求最优解
 *
 * param
 *		pRdtm		定位数据
 *
 * return
 *		定位结果
*/
std::shared_ptr<algorithm::POS> LocateAlgorithm::tof_locate_2d(std::shared_ptr<ReceiveDataTofVector>& pRdtv)
{
	std::shared_ptr<algorithm::POS> sp_pos = std::make_shared<algorithm::POS>();

	//方法一：牛顿迭代法
	//Eigen::VectorXd x(2);
	//Eigen::MatrixXd data(3,3);

	//int i = 0;
	//for (auto it = pRdtv->begin();it != pRdtv->end();++it,++i)
	//{
	//	data(i,0) = (*it)->x;
	//	data(i,1) = (*it)->y;
	//	//data(i,2) = (*it)->z;

	//	data(i,2) = (*it)->distance;
	//}

	//TofFunctor functor(data);
	//Eigen::NumericalDiff<TofFunctor> numDiff(functor);
	//Eigen::LevenbergMarquardt<Eigen::NumericalDiff<TofFunctor>,double> lm(numDiff);
	//lm.parameters.maxfev = 2000;
	//lm.parameters.xtol = 1.0e-10;

	//// 计算后，如果计算出结果则返回该结果
	//int ret = lm.minimize(x);

	////x=-5.56 y=1.06 实际位置(-5.55,1.151)
	////有偏差数据的测试位置：(-5.399,1.288)
	//if(ret > 0) 
	//{
	//	double a = x[0];
	//	double b = x[1];
	//	double c = x[2];
	//	//double d = x[3];

	//	sp_pos->posx = x[0];
	//	sp_pos->posy = x[1];
	//	//sp_pos->posz = x[2];
	//}

	//方法二：此方法可以进入现场数据调试
	Eigen::MatrixXf A(3,4);
	Eigen::VectorXf B(3,1);
	Eigen::MatrixXf X;

	int i = 0;
	for (auto it = pRdtv->begin();it != pRdtv->end();++it,++i)
	{
		A(i,0) = A(i,1) = 1;
		A(i,2) = -2*(*it)->x;
		A(i,3) = -2*(*it)->y;

		B(i,0) = pow((*it)->distance,2) - pow((*it)->x,2) - pow((*it)->y,2);
	}

	X = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B);

	double a = X(0);
	double b = X(1);
	double c = X(2);
	double d = X(3);

	//x=-5.56 y=1.06 实际位置(-5.55,1.151)
	//有偏差数据的测试位置：(-5.399,1.288)
	sp_pos->posx = X(2);	
	sp_pos->posy = X(3);

	return std::move(sp_pos);
}

std::vector<op::point> LocateAlgorithm::calc_pos(const op::point& circle_center, const double& radius, const op::point* pLine)
{
	vector<op::point> ret;

	double r = radius;
	op::point p1, p2;
	if (abs(circle_center.x - pLine[0].x) < 1E-4&&abs(circle_center.x - pLine[1].x) < 1E-4) {
		p1.x = pLine[0].x;
		p1.y = circle_center.y + r;

		p2.x = pLine[0].x;
		p2.y = circle_center.y - r;
	}
	else {
		double k = (pLine[1].y - pLine[0].y) / (pLine[1].x - pLine[0].x);
		double b = pLine[1].y - k * pLine[1].x;
		double A = pow(k, 2) + 1;
		double B = 2 * k*(b - circle_center.y) - 2 * circle_center.x;
		double C = pow(circle_center.x, 2) + pow(b - circle_center.y, 2) - pow(r, 2);
		double dt = pow(B, 2) - 4 * A*C;

		if (dt <= 0) {
			return ret;
		}

		//得两解
		p1.x = -(B + sqrt(dt)) / (2 * A);
		p2.x = (-B + sqrt(dt)) / (2 * A);
		p1.y = k * p1.x + b;
		p2.y = k * p2.x + b;
	}
	ret.push_back(p1);
	ret.push_back(p2);

	return ret;
}

/*
 * TDOA算法实现
 * 函数名：Pos
 *		遍历求解
 * param
 *		pRdm	------		存放参与计算的分站信息，主要信息包含同步后的时间戳
 *	    trpm    ------		地图集
 *
 * return
 *	    返回最终结果坐标
 *
*/
std::shared_ptr<POS> LocateAlgorithm::Pos(std::shared_ptr<ReceiveDataMap> pRdm,std::shared_ptr<TDOAReaderPathMap> trpm)
{
	std::shared_ptr<POS> pos = std::make_shared<POS>();
	double a = 0;
	double ref_dist = 0.0;   //两分站之间无路径的距离差
	
	_coordinate res[MAX_READER_TDOA_PATH_NUMS];
	int res_idx = 0;
	for(int i = 0;i < MAX_READER_TDOA_PATH_NUMS;i++){
		res[i].x = INVALID_COORDINATE;
		res[i].y = INVALID_COORDINATE;
		res[i].z = INVALID_COORDINATE;
	}

	bool bFirst = true;
	int nNoReaderPathIdx = 0;
	int nFirstReader[MAX_READER_TDOA_PATH_NUMS] = {-1};
	int nSecondReader[MAX_READER_TDOA_PATH_NUMS] ={-1};

	//存储无路径的两分站的id和坐标
	ReceiveData tmp_reader[MAX_READER_TDOA_PATH_NUMS];

	//存储和第一条分站存在路径的分站信息
	ReceiveData tmp_dist_reader[MAX_READER_TDOA_PATH_NUMS];
	int nDistReaders = 0;

	ReceiveDataMap::iterator first = pRdm->begin();
	ReceiveDataMap::iterator second = first;
	//偏移到第二个元素
	std::advance(second,1);

	//获取第一个时间戳
	ReceiveData f1;
	f1.antenna_id = first->second->antenna_id;
	f1.reader_id = first->second->reader_id;
	f1.rec_time_stamp = first->second->rec_time_stamp;
	f1.x = first->second->x;
	f1.y = first->second->y;
	f1.z = first->second->z;

	//从第二个开始遍历
	for(;second != pRdm->end();++second){
		//获取第二个时间戳
		ReceiveData f2;
		f2.antenna_id = second->second->antenna_id;
		f2.reader_id = second->second->reader_id;
		f2.rec_time_stamp = second->second->rec_time_stamp;
		f2.x = second->second->x;
		f2.y = second->second->y;
		f2.z = second->second->z;

		//时间戳异常
		if(f1.rec_time_stamp == LLONG_MAX || f2.rec_time_stamp == LLONG_MAX){
			continue;
		}

		if(f1.reader_id == f2.reader_id){
			continue;
		}

		//如果两级都能找到才运行继续后续操作，否则，表明没有此路径地图集
		TDOAReaderPathMap::iterator rdm_it = trpm->find(f1.reader_id);
		if(rdm_it == trpm->end()){
			continue;
		}

		ReaderPathMap::iterator rpm_it = trpm->find(f1.reader_id)->second->find(f2.reader_id);
		if(rpm_it == trpm->find(f1.reader_id)->second->end()){
			continue;
		}else{
			//如果和第一条分站存在地图集
			tmp_dist_reader[nDistReaders].reader_id = f2.reader_id;
			tmp_dist_reader[nDistReaders].x = f2.x;
			tmp_dist_reader[nDistReaders].y = f2.y;
			tmp_dist_reader[nDistReaders].z = f2.z;
			tmp_dist_reader[nDistReaders].rec_time_stamp = f2.rec_time_stamp;
			nDistReaders++;
		}

		//根据距离的正负，后续判断计算位置取舍时使用
		int nSign = 1;
		long long diffTime = f1.rec_time_stamp - f2.rec_time_stamp;

		//计算位置
		double dist = CFunctions::getDistance(diffTime,CFunctions::TDOA);

		double readers_dist = sqrt(pow(f1.x - f2.x,2) + pow(f1.y - f2.y,2));

		if(fabs(dist) - readers_dist > 0){
			continue;
		}

		std::shared_ptr<ReaderPath> pRP = trpm->find(f1.reader_id)->second->find(f2.reader_id)->second;

		//如果和第一条分站存在地图集
		tmp_reader[nNoReaderPathIdx].reader_id = f2.reader_id;
		tmp_reader[nNoReaderPathIdx].x = f2.x;
		tmp_reader[nNoReaderPathIdx].y = f2.y;
		tmp_reader[nNoReaderPathIdx].z = f2.z;
		tmp_reader[nNoReaderPathIdx].rec_time_stamp = f2.rec_time_stamp;
		nNoReaderPathIdx++;

		//两分站之间的线段个数
		int seg_num = pRP->nRealCalcPoints - 1;

		if(seg_num == 0 || seg_num > 100){
			continue;
		}

		//因为双曲线与分站之间第i条线段或者第j条线段分别有两焦点
		//或者分站之间就一条直线，有两焦点
		double xcross[2] = {INVALID_COORDINATE,INVALID_COORDINATE};
		double ycross[2] = {INVALID_COORDINATE,INVALID_COORDINATE};
		double zcross[2] = {INVALID_COORDINATE,INVALID_COORDINATE};

		int nIdx = 0;

		//根据线段个数开始计算
		for(int i = 0;i < seg_num;i++){
			//计算位置坐标，双曲线和线段相交的交点
			std::shared_ptr<SOLUTION> r = LocateAlgorithm::GetPos(pRP,dist,i);

			//无解或解无效
			if(r == nullptr ||r->nCount == 0){
				continue;
			}

			if(r->nCount == 1){
				xcross[nIdx] = r->x[0];
				ycross[nIdx] = r->y[0];

				res[res_idx].x = xcross[nIdx];
				res[res_idx].y = ycross[nIdx];
				res[res_idx].z = zcross[nIdx];

				nIdx++;
				res_idx++;
			}

			if(r->nCount == 2){
				for(int j = 0; j < 2;j++){
					xcross[j] = r->x[j];
					ycross[j] = r->y[j];
					nIdx++;
				}
			}

			if(nIdx == 2){
				//解到两焦点之间的距离
				double deltad[2] = {0};
				for(int j = 0; j < 2;j ++){
					double d[2] = {0};
					double dx1 = xcross[j] - pRP->x[0];
					double dy1 = ycross[j] - pRP->y[0];
					d[0] = sqrt(pow(dx1,2) + pow(dy1,2));

					double dx2 = xcross[j] - pRP->x[1];
					double dy2 = ycross[j] - pRP->y[1];
					d[1] = sqrt(pow(dx2,2) + pow(dy2,2));

					deltad[j] = d[0] - d[1];
				}

				int idx = 0;	

				//应该和之前计算的dist同方向
				if(dist > 0){
					for(int j = 0;j < 2;j++){
						if(deltad[j] > 0){
							idx = j;
							break;
						}
					}	
				}else{
					for(int j = 0;j < 2;j++){
						if(deltad[j] < 0){
							idx = j;
							break;
						}
					}	
				}
				nFirstReader[res_idx]  = f1.reader_id;
				nSecondReader[res_idx] = f2.reader_id;

				res[res_idx].x = xcross[idx];
				res[res_idx].y = ycross[idx];
				res[res_idx].z = zcross[idx];
				res_idx++;
			}		
			continue;
		}
	}

	_coordinate r;
	r.x = INVALID_COORDINATE;
	r.y = INVALID_COORDINATE;
	r.z = INVALID_COORDINATE;

	if(res_idx == 1){
		bool bValid = true;

		//如果定位坐标在分站附近，需要判断此分站是否为special，如果为special则接受此定位结果
		for(ReceiveDataMap::iterator it = pRdm->begin();it!=pRdm->end();++it){
			double dist = sqrt(pow(res[0].x - it->second->x,2) + pow(res[0].y - it->second->y,2));	
			if(dist<NEAR_READER){
				if(it->second->special == 0){
					r.reason = ALGO_FAILED_CONDITION_12;
					ALGORITHM_FAILED(ALGO_FAILED_CONDITION_12);
					bValid = false;
				}
			}
		}

		if(pRdm->size()>=3&&bValid){
			double dist[2] = {0};
			//更改选取分站方式:
			//选取和第一个分站有地图集的两个分站来参与判别，
			//例如：分站顺序是2，1,3，其中（1,2）和（2,3）之间有地图集，那么就选取1,3来参与判别
			//如果分站顺序是1,2，3，其中（1，2）有地图集，(1,3)之间无地图集，那么此时就不使用以下方法进行判别
			if(nDistReaders == 2){
				//如果第二个分站与这两个分站无地图集则不该使用如下逻辑
				dist[0] = sqrt(pow(res[0].x - tmp_dist_reader[0].x,2) + pow(res[0].y - tmp_dist_reader[0].y,2));
				dist[1] = sqrt(pow(res[0].x - tmp_dist_reader[1].x,2) + pow(res[0].y - tmp_dist_reader[1].y,2));

				double dif1 = fabs(dist[0] - dist[1]);
				long long dt = tmp_dist_reader[0].rec_time_stamp - tmp_dist_reader[1].rec_time_stamp;
				double dif2 = fabs(CFunctions::getDistance(dt,CFunctions::TDOA));
				double dif = fabs(dif1 - dif2);
				if(dif>MIN_DIFFER_DISTANCE){
					r.reason = ALGO_FAILED_CONDITION_13;
					ALGORITHM_FAILED(ALGO_FAILED_CONDITION_13);
					bValid = false;
				}	
			}
		}

		if(bValid){
			r.x = res[0].x;
			r.y = res[0].y;
			r.z = res[0].z;
			for (int i=0;i<MAX_READER_TDOA_PATH_NUMS;i++)
			{
				pos->dDiff[i] = 0;
			}
		}
	}

	int r_idx = 0;

	if(res_idx >= 2){
		double d = 99999.9;
		
		long long dt = tmp_reader[0].rec_time_stamp - tmp_reader[1].rec_time_stamp;
		//计算位置
		double ref_dist = abs(CFunctions::getDistance(dt,CFunctions::TDOA));

		for(int i = 0;i<res_idx;i++){
			double d_tmp = abs(sqrt(pow(res[i].x - tmp_reader[0].x,2) + pow(res[i].y - tmp_reader[0].y,2)) - sqrt(pow(res[i].x - tmp_reader[1].x,2) + pow(res[i].y - tmp_reader[1].y,2)));
			double d_diff = abs(d_tmp - ref_dist);
			pos->dDiff[i] = d_diff;
			if(d_diff < d){
				d = d_diff;
				r_idx = i;
			}
		}

		if(pRdm->size() >= 3){
			if(pos->dDiff[r_idx] < NEAR_READER){
				r.x = res[r_idx].x;
				r.y = res[r_idx].y;
				r.z = res[r_idx].z;
			}else{
				r.reason = ALGO_FAILED_CONDITION_14;
				ALGORITHM_FAILED(ALGO_FAILED_CONDITION_14);
			}
		}
	}

	//最终得到的结果解
	pos->posx = r.x;
	pos->posy = r.y;
	pos->posz = 0;
	pos->reason = r.reason;

	if(pos->posx != INVALID_COORDINATE && pos->posy != INVALID_COORDINATE){
		pos->nFirstReader = nFirstReader[r_idx];
		pos->nSecondReader = nSecondReader[r_idx];
	}

	return pos;
}

std::shared_ptr<POS> LocateAlgorithm::Pos(std::shared_ptr<POS> pos,std::shared_ptr<TDOAReaderPathMap> trpm)
{
	if(trpm->find(pos->nFirstReader) == trpm->end()){
		return nullptr;
	}
	if(trpm->find(pos->nFirstReader)->second->find(pos->nSecondReader)==trpm->find(pos->nFirstReader)->second->end()){
		return nullptr;
	}

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

	_coordinate res[2];
	int res_idx = 0;
	for(int i = 0;i < 2;i++){
		res[i].x = INVALID_COORDINATE;
		res[i].y = INVALID_COORDINATE;
		res[i].z = INVALID_COORDINATE;
	}

	//获得地图集
	std::shared_ptr<ReaderPath> pRP = trpm->find(pos->nFirstReader)->second->find(pos->nSecondReader)->second;
	//距离差
	double distance = 0.0;
	distance = sqrt(pow(pos->posx - pRP->x[0],2 ) + pow(pos->posy - pRP->y[0],2 )) - 
		sqrt(pow(pos->posx - pRP->x[1],2 ) + pow(pos->posy - pRP->y[1],2 ));

	//两分站之间的线段个数
	int seg_num = pRP->nRealCalcPoints - 1;

	if(seg_num == 0){
		return nullptr;
	}

	double xcross[2] = {INVALID_COORDINATE,INVALID_COORDINATE};
	double ycross[2] = {INVALID_COORDINATE,INVALID_COORDINATE};
	double zcross[2] = {INVALID_COORDINATE,INVALID_COORDINATE};

	int nIdx = 0;
	int nResIdx = 0;

	for(int i = 0;i < seg_num;i++){
		//计算位置坐标，双曲线和线段相交的交点
		std::shared_ptr<SOLUTION> r = GetPos(pRP,distance,i);

		//无解或解无效
		if(r == nullptr){
			continue;
		}
		if(r->nCount == 0){
			continue;
		}

		if(r->nCount == 1){
			xcross[nIdx] = r->x[0];
			ycross[nIdx] = r->y[0];
			nIdx++;
		}
		if(r->nCount == 2){
			for(int j = 0; j < 2;j++){
				xcross[j] = r->x[j];
				ycross[j] = r->y[j];
				nIdx++;
			}
		}

		if(nIdx == 2){
			//解到两焦点之间的距离
			double deltad[2] = {0};
			for(int j = 0; j < 2;j ++){
				double d[2] = {0};
				double dx1 = xcross[j] - pRP->x[0];
				double dy1 = ycross[j] - pRP->y[0];
				d[0] = sqrt(pow(dx1,2) + pow(dy1,2));

				double dx2 = xcross[j] - pRP->x[1];
				double dy2 = ycross[j] - pRP->y[1];
				d[1] = sqrt(pow(dx2,2) + pow(dy2,2));

				deltad[j] = d[0] - d[1];
			}

			int idx = 0;	
			//应该和之前计算的dist同方向
			if(distance > 0){
				for(int j = 0;j < 2;j++){
					if(deltad[j] > 0){
						idx = j;
						break;
					}
				}	
			}else{
				for(int j = 0;j < 2;j++){
					if(deltad[j] < 0){
						idx = j;
						break;
					}
				}	
			}
			res[nResIdx].x = xcross[idx];
			res[nResIdx].y = ycross[idx];
			res[nResIdx].z = zcross[idx];
		}		
		continue;
	}

	if (res[nResIdx].x == INVALID_COORDINATE && res[nResIdx].y == INVALID_COORDINATE)
	{
		return nullptr;
	}

	p->posx = res[nResIdx].x;
	p->posy = res[nResIdx].y;
	p->posz = res[nResIdx].z;

	return p;
}

/*
 * TDOA算法求解实现
 * 函数名：GetPos
 *		实现直线和双曲线求交点
 * param
 *		pRP    ------ 分站路径
 *      dist   ------ 距离差
 *      i	   ------ 第i条线段
 *
 * return
 *      满足条件的解，最多两个
 *
*/
typedef struct 
{
	double x;
	double y;
	double z;
}PositionStr;

int calc_card_position(ReceiveData stationFirst, ReceiveData stationSecond, double distanceDiff, PositionStr& result)
{
	double station_distance = sqrt(pow((stationFirst.x - stationSecond.x), 2) + pow((stationFirst.y - stationSecond.y),2));
    double diff_check = abs(abs(station_distance) - abs(distanceDiff));
	if(abs(distanceDiff) > station_distance)
	{
	    //debug_print_syslog(0, "calc_card_position, distanceDiff is longer than stationDistance, read_id_first: %d,read_id_second: %d, diff_check: %f,\
		//	first_time: %I64u, second_time: %I64u, station_distance: %f, distanceDiff: %f",
		//	stationFirst.reader_id, stationSecond.reader_id,
		//	diff_check, stationFirst.rec_time_stamp, stationSecond.rec_time_stamp, station_distance, distanceDiff);
		return -1;
	}

	if(diff_check < 4)
	{
	    //debug_print_syslog(0, "calc_card_position, read_id_first: %d,read_id_second: %d, diff_check: %f,\
		//	first_time: %I64u, second_time: %I64u, station_distance: %f, distanceDiff: %f",
		//	stationFirst.reader_id, stationSecond.reader_id,
		//	diff_check, stationFirst.rec_time_stamp, stationSecond.rec_time_stamp, station_distance, distanceDiff);
		return -1;
	}
	double m = station_distance + distanceDiff;
	
	double n = station_distance - distanceDiff;
	result.x = ((m * stationSecond.x) + (n * stationFirst.x))/(m + n);
	result.y = ((m * stationSecond.y) + (n * stationFirst.y))/(m + n);
	result.z = 0;
	return 0;
}

std::unique_ptr<POS> LocateAlgorithm::CalcCardPosition(std::shared_ptr<ReceiveDataMap> pRdm,std::shared_ptr<TDOAReaderPathMap> trpm)
{
    std::unique_ptr<POS> pos(new POS);

	double a = 0;
	double ref_dist = 0.0;   //两分站之间无路径的距离差
	
	const int Totals = 10;
	_coordinate res;
	int res_idx = 0;

	bool bFirst = true;
	int nNoReaderPathIdx = 0;
	int nFirstReader[Totals] = {-1};
	int nSecondReader[Totals] ={-1};

	pos->posx = INVALID_COORDINATE;
	pos->posy = INVALID_COORDINATE;
	pos->posz = INVALID_COORDINATE;
	pos->reason = ALGO_FAILED_CONDITION_1;

	//存储无路径的两分站的id和坐标
	ReceiveData tmp_reader[Totals];

	//存储和第一条分站存在路径的分站信息
	ReceiveData tmp_dist_reader[Totals];
	int nDistReaders = 0;

	for(ReceiveDataMap::iterator it_tmp = pRdm->begin(); it_tmp != pRdm->end(); it_tmp++)
	{
	    //debug_print_syslog(0, "Calculate data, read_id_second: %d, rec_time_stamp: %I64u",
		//	it_tmp->second->reader_id, it_tmp->second->rec_time_stamp);
	}
	ReceiveDataMap::iterator first = pRdm->begin();
	ReceiveDataMap::iterator second = first;
	//偏移到第二个元素
	std::advance(second,1);

	//获取第一个时间戳
	ReceiveData f1;
	f1.antenna_id = first->second->antenna_id;
	f1.reader_id = first->second->reader_id;
	f1.rec_time_stamp = first->second->rec_time_stamp;
	f1.x = first->second->x;
	f1.y = first->second->y;
	f1.z = first->second->z;

	//从第二个开始遍历
	for(; second != pRdm->end(); ++second)
	{
		//获取第二个时间戳
		ReceiveData f2;
		f2.antenna_id = second->second->antenna_id;
		f2.reader_id = second->second->reader_id;
		f2.rec_time_stamp = second->second->rec_time_stamp;
		f2.x = second->second->x;
		f2.y = second->second->y;
		f2.z = second->second->z;

		//时间戳异常
		if(f1.rec_time_stamp == LLONG_MAX || f2.rec_time_stamp == LLONG_MAX){
			continue;
		}

		if(f1.reader_id == f2.reader_id){
			continue;
		}

		//如果两级都能找到才运行继续后续操作，否则，表明没有此路径地图集
		TDOAReaderPathMap::iterator rdm_it = trpm->find(f1.reader_id);
		if(rdm_it == trpm->end()){
			continue;
		}

		ReaderPathMap::iterator rpm_it = trpm->find(f1.reader_id)->second->find(f2.reader_id);
		if(rpm_it == trpm->find(f1.reader_id)->second->end()){
			continue;
		}else{
			//如果和第一条分站存在地图集
			tmp_dist_reader[nDistReaders].reader_id = f2.reader_id;
			tmp_dist_reader[nDistReaders].x = f2.x;
			tmp_dist_reader[nDistReaders].y = f2.y;
			tmp_dist_reader[nDistReaders].z = f2.z;
			tmp_dist_reader[nDistReaders].rec_time_stamp = f2.rec_time_stamp;
			nDistReaders++;
		}

		//根据距离的正负，后续判断计算位置取舍时使用
		int nSign = 1;
		long long diffTime = f1.rec_time_stamp - f2.rec_time_stamp;

		//计算位置
		double distDiff = CFunctions::getDistance(diffTime,CFunctions::TDOA);

		PositionStr stationFirst;
		stationFirst.x = f1.x;
		stationFirst.y = f1.y;
		PositionStr stationSecond;
		stationSecond.x = f2.x;
		stationSecond.y = f2.y;
		PositionStr result;
		int rt_val = 0;
		rt_val = calc_card_position(f1, f2, distDiff, result);
		if(0 == rt_val)
		{
		    res.x = result.x;
		    res.y = result.y;
		    res.z = result.z;
		    res.reason = ALGO_LOC_SUCCESSED;
		    res_idx++;
			pos->posx = res.x;
        	pos->posy = res.y;
        	pos->posz = res.z;
        	pos->reason = res.reason;
        	return pos;
		}
		
	}
	return pos;
}

std::shared_ptr<SOLUTION> LocateAlgorithm::GetPos(std::shared_ptr<ReaderPath> pRP,double dist,int i)
{
	//解的个数
	int count = 0;

	//解的坐标
	double x[2];
	double y[2];
	//double z[2];

	for (int t=0;t<2;t++)
	{
		x[t] = y[t] = INVALID_COORDINATE;
	}

	//双曲线的两个焦点分别是
	double x1 = pRP->x[0];
	double y1 = pRP->y[0];
	double z1 = pRP->z[0];

	double x2 = pRP->x[1];
	double y2 = pRP->y[1];
	double z2 = pRP->z[1];

	if(pRP->px[i] - pRP->px[i+1] == 0){
		//x相等，双曲线在Y轴上
		x[0] = x[1] = pRP->px[i];
		y[0] = (y1 + y2 + dist)/2.0;
		y[1] = (y1 + y2 - dist)/2.0;
		count = 2;
	}
	else{
		//双曲线和直线相交
		//直线常数求解
		double k = (pRP->py[i+1] - pRP->py[i])/(pRP->px[i+1] - pRP->px[i]);
		double kb = pRP->py[i] - k*pRP->px[i];

		//求解的常数中间量
		double d1 = 2*(x2 - x1);//m
		double d2 = 2*(y2 - y1);//n
		double d3 = pow(y1,2) + pow(x1,2)- pow(dist,2) - pow(y2,2)  - pow(x2,2);//p
		//方程ax^2 + bx + c = 0
		double a = pow(d1 + d2*k,2) - 4*pow(dist,2)*(pow(k,2) + 1);
		double b = 2*((d3 + d2*kb)*(d1 + d2*k) - 4*pow(dist,2)*(k*(kb - y2) - x2));
		double c = pow(d3 + d2*kb,2) - 4*pow(dist,2)*(pow(x2,2) + pow(kb-y2,2));

		double delta = pow(b,2) - 4*a*c;
		if(delta > 0){
			count = 2;
		}
		else if(delta == 0){
			count = 1;
		}
		else{
			count = 0;
		}

		if(count == 0){
			return nullptr;
		}

		//计算解
		x[0] = -(b + sqrt(delta))/(2*a);
		x[1] = (-b + sqrt(delta) )/(2*a);
		y[0] = k*x[0] + kb;
		y[1] = k*x[1] + kb;	
	}
	int nIdx = 0;
	int nValidCount = count;
	//引入误差
	double deviation = 0.0;
	//判断两解是否在线段范围内
	if(count > 0){
		for(int t = 0; t < count ;t++){
			//两分站之间第i条线段的两端点
			if(x[t] < min(pRP->px[i],pRP->px[i+1]) - deviation||
			   x[t] > max(pRP->px[i],pRP->px[i+1]) + deviation||
			   y[t] < min(pRP->py[i],pRP->py[i+1]) - deviation||
			   y[t] > max(pRP->py[i],pRP->py[i+1]) + deviation
				)
			{
				x[t] = INVALID_COORDINATE;
				y[t] = INVALID_COORDINATE;
				nValidCount--;
				nIdx = t;
			}
		}
	}
	
	std::shared_ptr<SOLUTION> s = std::make_shared<SOLUTION>();

	s->nCount = nValidCount;

	nIdx = -1;
	switch(nValidCount){
	case 1:
		for(int i = 0;i<2;i++){
			if(x[i] != INVALID_COORDINATE && y[i] != INVALID_COORDINATE){
				nIdx = i;
			}
		}
		s->x[0] = x[nIdx];
		s->y[0] = y[nIdx];
		break;
	case 2:
		s->x[0] = x[0];
		s->x[1] = x[1];
		s->y[0] = y[0];
		s->y[1] = y[1];
		break;
	}

	return s;
}

/*
 * 判断坐标在地图集上
 *
 * param
 *		pos   ------		定位的结果
 *		trpm  ------		地图集
 *
 * return
 *		false，不在地图集上，true在地图集上
 *
*/
bool LocateAlgorithm::IsOnMap(std::shared_ptr<POS>& pos,std::shared_ptr<TDOAReaderPathMap> trpm)
{
	//如果地图集中都找不到此分站开始的地图集，就返回false
	//if(trpm->find(pos->nFirstReader) == trpm->end()){
	//	return false;
	//}

	////如果地图集找不到此两分站的地图集，就返回false
	//if(trpm->find(pos->nFirstReader)->second->find(pos->nSecondReader) == trpm->find(pos->nFirstReader)->second->end())
	//{
	//	return false;
	//}

	////保存两分站的坐标
	//double x[2] = {0};
	//double y[2] = {0};

	//for(int i=0;i<2;i++){
	//	x[i] = trpm->find(pos->nFirstReader)->second->find(pos->nSecondReader)->second->x[i];
	//	y[i] = trpm->find(pos->nFirstReader)->second->find(pos->nSecondReader)->second->y[i];
	//}

	//if(x[0] == x[1]){
	//	//误差1cm
	//	if(abs(pos->posx - x[0]) > 1E-2){
	//		return false;
	//	}
	//}else{
	//	double k = (y[1] - y[0])/(x[1] - x[0]);
	//	double b = y[1] - k*x[1];

	//	double calc_y = k*pos->posx + b;

	//	if(abs(calc_y - pos->posy) > 1E-2){
	//		return false;
	//	}
	//}

	bool bExist = false;
	for (TDOAReaderPathMap::iterator first = trpm->begin();first!=trpm->end();++first)
	{
		for (ReaderPathMap::iterator second = first->second->begin();second != first->second->end();++second)
		{
			_point p,start_p,end_p;

			p.x = pos->posx;
			p.y = pos->posy;

			start_p.x = second->second->px[0];
			start_p.y = second->second->py[0];

			end_p.x = second->second->px[1];
			end_p.y = second->second->py[1];

			bExist = LocateAlgorithm::IsInLine(p,start_p,end_p);
			if (bExist)
			{
				pos->nFirstReader = first->first;
				pos->nSecondReader = second->first;
				return bExist;
			}
			////保存两分站的坐标
			//double x[2] = {0};
			//double y[2] = {0};
			//for(int i=0;i<2;i++){
			//	x[i] = second->second->x[i];
			//	y[i] = second->second->y[i];
			//}
			//if(abs(x[0] - x[1]) < 1E-4 && abs(pos->posx - x[0]) < 1E-4){
			//	//误差1cm
			//	if ((pos->posy > y[0] && pos->posy < y[1])||(pos->posy > y[1] && pos->posy < y[0]))
			//	{
			//		bExist = true;
			//		return bExist;
			//	}
			//}else{
			//	double k = (y[1] - y[0])/(x[1] - x[0]);
			//	double b = y[1] - k*x[1];

			//	double calc_y = k*pos->posx + b;

			//	if(abs(calc_y - pos->posy) < 1){
			//		bExist = true;
			//		return bExist;
			//	}
			//}
		}
	}
	
	return bExist;
}

bool LocateAlgorithm::IsInTriangle(std::vector<_point> vtp,_point p)
{
	double sabc = 0,sadb = 0,sbdc = 0,sadc = 0;
	sabc = GetTriangleArea(vtp[0],vtp[1],vtp[2]);
	sadb = GetTriangleArea(vtp[0],p,vtp[1]);
	sbdc = GetTriangleArea(vtp[1],p,vtp[2]);
	sadc = GetTriangleArea(vtp[0],p,vtp[2]);

	double sum = 0.0;
	sum = sadb + sbdc + sadc;
	if ((sabc - sum) > -1E-5 && (sabc - sum) < 1E-5)
	{
		return true;
	} 
	else
	{
		return false;
	}
}

double LocateAlgorithm::GetTriangleArea(_point p0,_point p1,_point p2)
{
	_point ab,bc;

	ab.x = p1.x - p0.x;
	ab.y = p1.y - p0.y;

	bc.x = p2.x - p1.x;
	bc.y = p2.y - p1.y;

	return abs(ab.x*bc.y - ab.y*bc.x)/2.0;
}

/*
 * 点P是否在以start_p和end_p的线段上
 *
 * param
 *		p			识别点
 *		start_p		线段端点
 *		end_p		线段终点
 *
 * return
 *		在线段上返回true，否则返回false
 *
*/
bool LocateAlgorithm::IsInLine(_point p,_point start_p,_point end_p)
{
	double d1 = 0, d2 = 0, d3 = 0;

	d1 = sqrt(pow(p.x - start_p.x,2) + pow(p.y - start_p.y,2));
	d2 = sqrt(pow(p.x - end_p.x,2) + pow(p.y - end_p.y,2));
	d3 = sqrt(pow(start_p.x - end_p.x,2) + pow(start_p.y - end_p.y,2));

	double d4 = 0;
	d4 = abs(d3 - (d1 + d2));
	if (d4 < OFFSET_THRE_IN_LINE)
	{
		//增加点是否在start_p和end_p为顶点的矩形内
		if (abs(start_p.x - end_p.x) < ZERO_PRECISION)
		{
			if (p.y > min(start_p.y,end_p.y) && p.y < max(start_p.y,end_p.y))
			{
				return true;
			}else{
				return false;
			}
		}else if(abs(start_p.y - end_p.y) < ZERO_PRECISION){
			if(p.x > min(start_p.x,end_p.x) && p.x < max(start_p.x,end_p.x)) {
				return true;
			}else{
				return false;
			}
		}else{
			if(p.x > min(start_p.x,end_p.x) && p.x < max(start_p.x,end_p.x) 
				&& p.y > min(start_p.y,end_p.y) && p.y < max(start_p.y,end_p.y))
			{
				return true;
			}else{
				return false;
			}
		}
	}
	return false;
}

bool LocateAlgorithm::PointIsInRect(_point p,_point tp,_point lp,_point bp,_point rp)
{
	//任意四边形有4个顶点  
	int nCount = 4;  
	_point RectPoints[4] = { tp, lp, bp, rp };  
	int nCross = 0;  
	for (int i = 0; i < nCount; i++)   
	{   
		//依次取相邻的两个点  
		_point pStart = RectPoints[i];   
		_point pEnd = RectPoints[(i + 1) % nCount];  

		//相邻的两个点是平行于x轴的，肯定不相交，忽略  
		if ( pStart.y == pEnd.y )   
			continue;  

		//交点在pStart,pEnd的延长线上，pCur肯定不会与pStart.pEnd相交，忽略  
		if ( p.y < min(pStart.y, pEnd.y) || p.y > max( pStart.y, pEnd.y ) )   
			continue;  

		//求当前点和x轴的平行线与pStart,pEnd直线的交点的x坐标  
		double x = (double)(p.y - pStart.y) * (double)(pEnd.x - pStart.x) / (double)(pEnd.y - pStart.y) + pStart.x;  

		//若x坐标大于当前点的坐标，则有交点  
		if ( x > p.x )   
			nCross++;   
	}  

	// 单边交点为偶数，点在多边形之外   
	return (nCross % 2 == 1);   
}

int LocateAlgorithm::CalcTdoaPosition(std::shared_ptr<ReceiveDataMap> pRdm,std::shared_ptr<TDOAReaderPathMap> trpm,std::vector<std::shared_ptr<POS>>& udm_pos)
{
	//这部分应该放到组装数据后检测里
	//一：选择基准分站，条件如下：
	//1.基准分站选择：当前分站和后一个分站有地图集，则选为基准分站
	//2.当前分站和后续所有分站无地图集，则当前分站不是基准分站
	//3.重复1,2过程
	for (ReceiveDataMap::iterator first = pRdm->begin();first != pRdm->end();)
	{
		ReceiveDataMap::iterator second = first;
		//second偏移到第二个元素
		std::advance(second,1);

		//如果两级都能找到才运行继续后续操作，否则，表明没有此路径地图集
		TDOAReaderPathMap::iterator rdm_it = trpm->find(first->second->reader_id);
		if(rdm_it == trpm->end()){
			//表示地图集中不存在第一个分站的路径集，则删除此分站，并继续循环
			pRdm->erase(first);
			first = pRdm->begin();
			continue;
		}

		//统计first分站和后续分站的地图集数量
		int nCount = 0;		
		for (;second!= pRdm->end();++second)
		{
			//确认第一个和第二个分站之间是否有地图集
			ReaderPathMap::iterator rpm_it = trpm->find(first->second->reader_id)->second->find(second->second->reader_id);
			if(rpm_it == trpm->find(first->second->reader_id)->second->end()){
				continue;
			}else{
				nCount++;
				break;
			}
		}

		//如果first分站和后续分站都无地图集，则删除first分站，并重置迭代器
		if (nCount == 0)
		{
			pRdm->erase(first);
			first = pRdm->begin();
		}else{
			first = second;
			break;
		}	
	}

	//定位数据条数不够2条
	if (pRdm->size() < 2)
	{
		return ALGO_CALC_SOLUTION;
	}

	//先做第一遍筛选，
	//根据前三个分站计算坐标，如果有解，直接返回；如果无解，做第二遍筛选
	bool bNoSolution = true;
	//std::unique_ptr<POS> first_pos = LocateAlgorithm::Pos(pRdm,trpm);
	std::shared_ptr<POS> first_pos = LocateAlgorithm::Pos(pRdm,trpm);
	if (first_pos->posx != INVALID_COORDINATE && first_pos->posy != INVALID_COORDINATE)
	{
		std::shared_ptr<POS> sp = std::make_shared<POS>();
		*sp = *first_pos;
		udm_pos.push_back(sp);
		bNoSolution = false;
		return 0;
	}

	//如果无解且数据条数小于等于2，则返回选择失败
	//主要是筛选两条数据，但无法定位的情况
	if (bNoSolution && pRdm->size() <= 2)
	{
		return ALGO_CALC_NO_SOLUTION_WITH_TWO_DATA;
	}

	//开始执行第二遍筛选
	ReceiveDataMap::iterator first = pRdm->begin();
	ReceiveDataMap::iterator second = first;
	if (bNoSolution)
	{
		//因为first和后两个分站已经通过上步判断无解了
		//如果按3偏移，存在一个bug，
		//即在在如下情况下103,104,301,102,105,106情况下
		//在第一步时，当103和104，103和102的结果被判定不合格而无解
		//此处偏移3，即会导致301被跳过
		//所以改为重新对后续求所有解
		//偏移到第二个元素
		std::advance(second,1);	
	} 

	//检查偏移后的元素是否存在
	if (second == pRdm->end())
	{
		//偏移后找不到元素，返回错误
		return ALGO_CALC_ONE_DATA;
	}

	//获取第一个元素中的数据
	ReceiveData f1;
	f1.antenna_id = first->second->antenna_id;
	f1.reader_id = first->second->reader_id;
	f1.rec_time_stamp = first->second->rec_time_stamp;
	f1.x = first->second->x;
	f1.y = first->second->y;
	f1.z = first->second->z;

	for(;second != pRdm->end();++second){
		//获取第二个数据中的元素
		ReceiveData f2;
		f2.antenna_id = second->second->antenna_id;
		f2.reader_id = second->second->reader_id;
		f2.rec_time_stamp = second->second->rec_time_stamp;
		f2.x = second->second->x;
		f2.y = second->second->y;
		f2.z = second->second->z;

		bool bExistPath = false;
		//如果第一级都不存在，则继续找下一个元素
		TDOAReaderPathMap::iterator rdm_it = trpm->find(f1.reader_id);
		if(rdm_it == trpm->end()){
			continue;
		}
		//如果两级都能找到才运行继续后续操作，否则，表明没有此路径地图集
		ReaderPathMap::iterator rpm_it = trpm->find(f1.reader_id)->second->find(f2.reader_id);
		if(rpm_it != trpm->find(f1.reader_id)->second->end()){
			bExistPath = true;
		}

		//根据距离的正负，后续判断计算位置取舍时使用
		int nSign = 1;
		long long diffTime = f1.rec_time_stamp - f2.rec_time_stamp;

		//计算位置
		double dist = CFunctions::getDistance(diffTime,CFunctions::TDOA);

		double readers_dist = sqrt(pow(f1.x - f2.x,2) + pow(f1.y - f2.y,2));

		if(fabs(dist) - readers_dist > 0){
			continue;
		}

		//保存解信息
		std::shared_ptr<POS> pos = std::make_shared<POS>();

		if (bExistPath)
		{
			//如果存在地图集
			std::shared_ptr<ReaderPath> pRP = trpm->find(f1.reader_id)->second->find(f2.reader_id)->second;
			//两分站之间的线段个数
			int seg_num = pRP->nRealCalcPoints - 1;

			if(seg_num == 0 || seg_num > 100){
				continue;
			}

			//因为双曲线与分站之间第i条线段或者第j条线段分别有两焦点
			//或者分站之间就一条直线，有两焦点
			double xcross[2] = {INVALID_COORDINATE,INVALID_COORDINATE};
			double ycross[2] = {INVALID_COORDINATE,INVALID_COORDINATE};
			double zcross[2] = {INVALID_COORDINATE,INVALID_COORDINATE};
			int nIdx = 0;

			for(int i = 0;i < seg_num;i++){
				//计算位置坐标，双曲线和线段相交的交点
				std::shared_ptr<SOLUTION> r = LocateAlgorithm::GetPos(pRP,dist,i);
				//无解或解无效
				if(r == nullptr || r->nCount == 0){
					continue;
				}
				int nPosCounts = 0;
				for (int i=0;i<r->nCount;i++)
				{
					xcross[nIdx] = r->x[i];
					ycross[nIdx] = r->y[i];
					nIdx++;
				}
				switch (nIdx)
				{
				case 1:
					pos->posx = xcross[0];
					pos->posy = ycross[0];
					pos->posz = zcross[0];

					udm_pos.push_back(pos);
					break;
				case 2:
					//筛选解：从两解中选出一个
					//解到两焦点之间的距离
					double deltad[2] = {0};
					for(int j = 0; j < 2;j ++){
						double d[2] = {0};
						double dx1 = xcross[j] - pRP->x[0];
						double dy1 = ycross[j] - pRP->y[0];
						d[0] = sqrt(pow(dx1,2) + pow(dy1,2));

						double dx2 = xcross[j] - pRP->x[1];
						double dy2 = ycross[j] - pRP->y[1];
						d[1] = sqrt(pow(dx2,2) + pow(dy2,2));

						deltad[j] = d[0] - d[1];
					}

					int idx = 0;	

					//应该和之前计算的dist同方向
					if(dist > 0){
						for(int j = 0;j < 2;j++){
							if(deltad[j] > 0){
								idx = j;
								break;
							}
						}	
					}else{
						for(int j = 0;j < 2;j++){
							if(deltad[j] < 0){
								idx = j;
								break;
							}
						}	
					}

					pos->nFirstReader  = f1.reader_id;
					pos->nSecondReader = f2.reader_id;

					pos->posx = xcross[idx];
					pos->posy = ycross[idx];
					pos->posz = zcross[idx];

					udm_pos.push_back(pos);
					break;
				}
			}
		} 
		else
		{
			//如果不存在地图集，需要检测计算结果是否在地图集上，
			//根据距离的正负，后续判断计算位置取舍时使用
			//无地图集的解求坐标方法如下：
			double offset_d = (dist + readers_dist)/2;
			double calc_x = 0.0, calc_y = 0.0;

			//if (f1.x - f2.x < 1E-5)
			if (fabs(f1.x - f2.x) < ZERO_PRECISION)
			{
				//在y轴上
				calc_x = f1.x;
				if (f1.y > f2.y)
				{
					calc_y = f1.y - offset_d;
				} 
				else
				{
					calc_y = f1.y + offset_d;
				}
				
			}//else if (f1.y - f2.y < 1E-5)
			else if (fabs(f1.y - f2.y) < ZERO_PRECISION)
			{
				//在x轴上
				if (f1.x < f2.x)
				{
					calc_x = f1.x + offset_d;
				} 
				else
				{
					calc_x = f1.x - offset_d;
				}
				calc_y = f1.y;
			} 
			else
			{
				//在有斜率的地方
				double arg = atan((f2.y - f1.y)/(f2.x - f1.x));
				if (f1.x < f2.x && f1.y < f2.y)
				{
					calc_x = f1.x + cos(arg)*offset_d;
					calc_y = f1.y + sin(arg)*offset_d;
				} 
				else
				{
					calc_x = f1.x - cos(arg)*offset_d;
					calc_y = f1.y - sin(arg)*offset_d;
				}
				
			}

			pos->posx = calc_x;
			pos->posy = calc_y;
			pos->nFirstReader  = f1.reader_id;
			pos->nSecondReader = f2.reader_id;

			//如果不在，则此解无效,如果存在，则此解有效
			if (IsOnMap(pos,trpm))
			{
				udm_pos.push_back(pos);
			}	
		}	
	}

	if (udm_pos.size() == 0)
	{
		return ALGO_CALC_SOLUTION;
	}

	return 0;
}

int LocateAlgorithm::round(double value)
{
	return (value > 0.0)?floor(value + 0.5):ceil(value - 0.5);
}

bool LocateAlgorithm::CheckPosInValid(POS* pos,ReceiveDataMap* pRdm,double dScale)
{
	if(dScale <= 0.0){
		return false;
	}

	if(pos->posx == INVALID_COORDINATE || pos->posy == INVALID_COORDINATE){
		return false;
	}
	//判断解的无效性
	int nSize = 0;
	bool bRet = false;

	nSize = pRdm->size();
	double dist[2] = {0.0};

	if(nSize == 2){
		int i = 0;
		for(ReceiveDataMap::iterator it = pRdm->begin();it!=pRdm->end();++it,i++){
			dist[i] = sqrt(pow(pos->posx - it->second->x,2)+pow(pos->posy - it->second->y,2));
			if(fabs(dist[i]) < NEAR_READER){
				//误差在范围内，判断此分站是否属于特殊分站
				if(it->second->special){
					bRet = false;
				}else{
					bRet = true;
				}	
			}
		}	
	}

	return bRet;
}

std::shared_ptr<POS> LocateAlgorithm::MappingToPath( std::shared_ptr<POS> pos, std::shared_ptr<TDOAReaderPathMap> trpm )
{
	if(trpm->find(pos->nFirstReader) == trpm->end()){
		return nullptr;
	}
	if(trpm->find(pos->nFirstReader)->second->find(pos->nSecondReader)==trpm->find(pos->nFirstReader)->second->end()){
		return nullptr;
	}

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

	//获得地图集
	std::shared_ptr<ReaderPath> pRP = trpm->find(pos->nFirstReader)->second->find(pos->nSecondReader)->second;

	// 计算两点间斜率
	// 垂直
	if(abs(pRP->x[0] - pRP->x[1]) < 1E-6)
	{
		p->posx = pRP->x[0];
		p->posy = pos->posy;
	}
	// 水平
	else if(abs(pRP->y[0] - pRP->y[1]) < 1E-6)
	{
		p->posx = pos->posx;
		p->posy = pRP->y[0];
	}
	// 有斜率
	else
	{
		// 原理 y = kx + b, y' = （-1/k） x' + b'
		double k1= (pRP->y[1] - pRP->y[0]) / (pRP->x[1] - pRP->x[0]);
		double k2 = -1.0 / k1;

		double b1 = pRP->y[0] - k1 * pRP->x[0];
		double b2 = pos->posy - k2 * pos->posx;	

		p->posx = (b1 - b2)/(k2 - k1);
		p->posy = k2 * p->posx + b2;
	}

	return p;
}

void HeapSort(_coordinate** pCoordinateArray,int nLen)
{
	BuildMaxHeap(pCoordinateArray,nLen);
	
	for(int i = nLen - 1;i > 0;i--){
		SwapElement(pCoordinateArray[0],pCoordinateArray[i]);
		AdjustMaxHeap(pCoordinateArray,0,i-1);
	}
}

void BuildMaxHeap(_coordinate** pCoordinateArray,int nLen)
{
	for(int i = nLen/2 - 1;i > 0;i--){
		AdjustMaxHeap(pCoordinateArray,i,nLen-1);
	}
}

void AdjustMaxHeap(_coordinate** pCoordinateArray,int n,int nHeapSize)
{
	int l = (n+1)*2-1;
	int r = (n+1)*2;

	int max;

	if(l <= nHeapSize && pCoordinateArray[l]->tt > pCoordinateArray[n]->tt){
		max = l;
	}else{
		max = n;
	}

	if(r <= nHeapSize && pCoordinateArray[r]->tt > pCoordinateArray[max]->tt){
		max = r;
	}

	if(max != n){
		SwapElement(pCoordinateArray[n],pCoordinateArray[max]);
		AdjustMaxHeap(pCoordinateArray,max,nHeapSize);
	}
}

void SwapElement(_coordinate* a,_coordinate*b)
{
	_coordinate tmp;
	tmp = *a;
	*a = *b;
	*b = tmp;
}

void SelectSort(_coordinate** pCoordinateArray,int nLen)
{  
    for (int i=0; i<nLen; i++)  
    {  
        int k = i;  
        unsigned long long key = pCoordinateArray[i]->tt;  
        for (int j=i+1; j<nLen; j++)  
        {  
            if (pCoordinateArray[j]->tt < key) 
            {  
                k = j;  
                key = pCoordinateArray[j]->tt;  
            }  
        }  
        if (k!=i)  
            SwapElement(pCoordinateArray[i], pCoordinateArray[k]);  
    }  
}