mine_server/algo/loc_tool.cpp

#include <vector>
#include <map>
#include <log.h>
#include <ant.h>
#include <loc_tool.h>
#include <message.h>
#include "loc_common.h"
#include <Eigen/Core>
#include <Eigen/Dense>
#include "ya_setting.h"
#include "../server/common_tool.h"
#include "../server/websocket/ws_common.h"
#include "../server/tool_time.h"
#include "../server/websocket/wsTimerThread.h"
#include "../server/db/db_tool.h"

// pdoa三维定位
std::vector<point> loc_tool_pdoa_3_base::calc_location(std::vector<loc_message>& loc)
{
	return std::vector<point>();
}
int loc_tool_pdoa_3_base::index() 
{
	return 8;
}

// pdoa二维定位
std::vector<point> loc_tool_pdoa_2_base::calc_location(std::vector<loc_message>& loc)
{
    if( 1 > loc.size())
    {
        logn_info(3, "[pdoa] loc_tool_pdoa_2_base::calc_location, the nums of location data is less than 2");
        return std::vector<point>();
    }
    logn_info(3, "[pdoa] loc_tool_pdoa_2_base::calc_location, loc.size=%d", loc.size());
    std::vector<pdoa_msg_ptr> vps;

    for(auto it = loc.begin(); it != loc.end(); ++it)
    {
        double d = (*it).m_num_ticks*15.65*2.996*1e-4;
        double angle = (*it).m_sit->m_ant[(*it).m_ant_id].m_angle;
        double x = (*it).m_sit->m_ant[(*it).m_ant_id].x;
        double y = (*it).m_sit->m_ant[(*it).m_ant_id].y;
	    double p = (*it).get_pdoa(0); 

        logn_info(3, "[pdoa] calc_position's param, d=%.2f, tof=%ld, angle=%.2f, x=%.2f, y=%.2f, pdoa=%.4f, m_scale=%.2f", d, (*it).m_num_ticks , angle, x, y, p, (*it).m_sit->m_scale);
            
        pdoa_param pp(p, d, angle, x, y);
        if(!cal_position_pdoa(vps, pp)){
            break;
        }
    }

    if(2 > vps.size()){
        logn_info(3, "[pdoa] solutions too less, size=%d", vps.size());
        return std::vector<point>();
    }

    for(auto it = vps.cbegin();it != vps.cend(); ++it){
        logn_info(3, "[pdoa] possible solution: x=%.4f, y=%.4f", (*it)->x, (*it)->y);
    }

    pdoa_msg_ptr p_msg;
    std::size_t count = vps.size();
    if(count <= 0){
        return std::vector<point>();
    }
    for(std::size_t i = 0; i < count; ++i){
        if(vps[i]->y > 0){
            p_msg = vps[i];
            p_msg->x /= loc[0].m_sit->m_scale;
            p_msg->y /= loc[0].m_sit->m_scale;
        }
    }

    double _angle = 3.1415926*p_msg->angle/180.0;
    double x = p_msg->x_ant + p_msg->x*cos(_angle) - p_msg->y*sin(_angle);
    double y = p_msg->y_ant + p_msg->x*sin(_angle) + p_msg->y*cos(_angle);
    logn_info(3, "[pdoa] save,card_id=%d, x=%.4f, y=%.4f, ant_x=%.4f, ant_y=%.4f, angle = %.3f, site_angle=%.3f", loc[0].m_card_id, x, y, p_msg->x_ant, p_msg->y_ant, _angle, p_msg->angle);

    std::vector<point> vp;
    vp.push_back(point(x, y));

    return vp;
}
int loc_tool_pdoa_2_base::index() 
{
	return 7;
}

/* 
 * 计算pdoa定位坐标，并将定位结果保存在vps内
 *
 */
bool loc_tool_pdoa_2_base::cal_position_pdoa(std::vector<pdoa_msg_ptr>& vps, const pdoa_param& pa)
{
    float b = 0.075;
    float d = 0.5*b;

    float p = pa.pdoa*b / (2.0*3.1415926);
    logn_info(3,"[pdoa] before adjust, p = %.4f, pdoa=%.4f", p, pa.pdoa);
    p = -2919.8*pow(p, 4.0) - 88.74*pow(p, 3.0) + 5.6182*p*p + 1.1041*p - 0.00079884;
    //p = -0.0222*pow(p, 4.0) + 0.0328*pow(p, 3.0) + 0.0729*p*p + 0.854*p + 0.0111;
    //p *= 0.01;

    float alpha = p / d;
    if(fabs(alpha) > 1.0){
        logn_info(3, "[pdoa] p>d, alpha=%.4f, p=%.4f, d=%.4f", alpha, p , d);
        return false;
    }

    //double x = (d*d + 2*pa.r*p - p*p) / (2*d);
    //double y = (pa.r - 0.5*p)*sqrt(1.0 - (p/d)*(p/d));

	double angle = acos(alpha)*180/3.14;
	logn_info(3, "[pdoa] angle=%.2f, distance=%.2f", angle, pa.r);

    double x = d / 2.0 + alpha*(pa.r - p / 2.0);
    double y = (pa.r - 0.5*p)*sqrt(1.0 - alpha*alpha);

    logn_info(3, "[pdoa] param, distance=%.4f, p=%.4f, d=%.4f, x=%.4f, y=%.4f, p/d=%.4f", pa.r, p, d, x, y, alpha);

    vps.push_back(std::make_shared<pdoa_message>(x, y, pa.a, pa.r, pa.ax, pa.ay, pa.pdoa, angle));
    vps.push_back(std::make_shared<pdoa_message>(x, -y, pa.a, pa.r, pa.ax, pa.ay, pa.pdoa, angle));

    return true;
}

// pdoa一维定位
std::vector<point> loc_tool_pdoa_1_base::calc_location(std::vector<loc_message>& loc)
{
    //logn_info(3, "[pdoa] loc_tool_pdoa_1_base::calc_location, card_id=%d, loc.size=%d", loc[0].m_card_id, loc.size());
    int32_t last_ct = -1;
    std::vector<point> vc;
    std::vector<loc_message> lm;

    for(auto rit = loc.rbegin(); rit != loc.rend(); ++rit){
        if(rit->m_sit->is_path_empty() || rit->m_num_ticks <= 0){
            continue;
        }
        if(-1 == last_ct){
            last_ct = rit->m_card_ct;
        }

        double dist_tof = rit->m_num_ticks*15.65*2.996*1e-4/rit->m_sit->m_scale;
        auto v = rit->m_sit->solving_pdoa(rit->m_ant_id, dist_tof);
        lm.insert(lm.begin(), *rit);
        vc.insert(std::end(vc), std::begin(v), std::end(v));
    }
    loc.swap(lm);
	
    return std::move(vc);
}

int loc_tool_pdoa_1_base::index() 
{
	return 6;
}

// tdoa三维定位
std::vector<point> loc_tool_tdoa_3_base::calc_location(std::vector<loc_message>& loc)
{
	return std::vector<point>();
}
int loc_tool_tdoa_3_base::index() 
{
	return 5;
}

// tdoa二维定位
std::vector<point> loc_tool_tdoa_2_base::calc_location(std::vector<loc_message>& locm)
{
    if(locm.size() < 3){
        return std::vector<point>();
    }
	
    std::vector<point> sol;     // 解的列表,返回值
    sol.resize(0);

    std::vector<algo_solution> _algo_sol;
    _algo_sol.resize(0);

    std::vector<double> vtk;    // 保存ki
    std::vector<double> vtd;    // 保存di
    std::vector<point> vtc;     // 保存xi，1 yi，1
    std::vector<point> vtp;     // 保存三角形的顶点坐标
    std::vector<int> vts;

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

    // 保存卡数据中第一条的分站坐标和插值时间
    point fp;
    unsigned long long fts = 0;

	double _height_offset = 0.0;
	if (true)
	{
		//按时间戳进行排序
		std::multimap< uint64_t, loc_message> map_sort_msg;
		for (auto iter = locm.begin(); iter != locm.end(); ++iter)
		{
			map_sort_msg.insert(std::make_pair(iter->m_num_ticks, *iter));
		}
		locm.clear();
		for (auto iter = map_sort_msg.begin(); iter != map_sort_msg.end(); ++iter)
		{
			locm.push_back(iter->second);
		}
	}

    loc_message datas[3];
    for(auto iter0 = locm.begin(); iter0 != locm.end(); ++iter0)
    {
        for(auto iter1 = iter0 + 1; iter1 != locm.end(); ++iter1)
        {
            for(auto iter2 = iter1 + 1; iter2 != locm.end(); ++iter2)
            {
                vtk.clear();
                vtd.clear();
                vtc.clear();
                vts.clear();
                vtp.clear();

                datas[0] = *(iter0);
                datas[1] = *(iter1);
                datas[2] = *(iter2);

				bool hasInvalidValue = false;
				for (int i = 0; i < 3; ++i)
				{
					if (datas[i].m_num_ticks == LONGLONG_MAX)
					{
						hasInvalidValue = true;
					}
				}

				if (hasInvalidValue)
				{
					continue;
				}
				

				bool bContinue = false;

                for(int i = 0; i < 3; ++i)
                {
                    double k = 0.0;
                    k = pow(datas[i].m_sit->x, 2) + pow(datas[i].m_sit->y, 2);
                    vtk.push_back(k);

                    if(i == 0)
                    {
                        fts = datas[i].m_num_ticks;
						fp.x = datas[i].m_sit->x;
						fp.y = datas[i].m_sit->y;

                        vtd.push_back(0.0);
                    }else{
                        long long diff_time = datas[i].m_num_ticks - fts;
                        double d = diff_time* DWT_TIME_UNITS * SPEED_OF_LIGHT;

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

                        vtd.push_back(d);
                    }

                    vtc.push_back(point(datas[i].m_sit->x - fp.x, datas[i].m_sit->y - fp.y));
					vtp.push_back(point(datas[i].m_sit->x, datas[i].m_sit->y));
                }

                double a[4] = {0};
                double dt = vtd[1]*vtc[2].y - vtd[2]*vtc[1].y;
				if (fabs(dt) < 1E-5 || fabs(vtd[1]) < 1E-5) {
                    continue;
                }

                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;
                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.0, B = 0.0, C = 0.0;
                A = pow(a[3], 2) - 1 - pow(a[0], 2);
                B = 2*(a[2]*a[3] + fp.x + a[0]*(fp.y - a[1]));
                C = pow(a[2], 2) - pow(fp.x, 2) - pow(fp.y - a[1], 2) - pow(_height_offset, 2);

                std::vector<point> _vp;
                double delta = pow(B, 2) - 4*A*C;
                if(delta > 0){
                    point p;
                    p.x = ((-1.0)*B + sqrt(delta))/(2.0*A);
                    p.y = a[0]*p.x + a[1];
                    _vp.push_back(p);

                    p.x = ((-1)*B - sqrt(delta))/(2.0*A);
                    p.y = a[0]*p.x + a[1];
                    _vp.push_back(p);
                }else{
                     continue;
                }
				
                int idx = -1;
                for(std::size_t i = 0; i < _vp.size(); ++i)
                {
					bool cond[2] = { false };

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

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

                    if(cond[0] && cond[1]){
                        idx = i;
                    }
                }

                // 判断求出的解是否在基站构成的三角形内


                if(idx != -1){
                    if(is_in_triangle(vtp, _vp[idx]))
                    {
						_vp[idx].site1 = datas[0].m_sit->id();
						_vp[idx].site2 = datas[1].m_sit->id();
						_vp[idx].site3 = datas[2].m_sit->id();
                        sol.push_back(_vp[idx]);
                        algo_solution _as;
                        _as.m_pos = std::make_shared<point>(_vp[idx]);
                        _as.m_triangle = std::make_shared<triangle>();

                        site_point _sp(point(datas[0].m_sit->x, datas[0].m_sit->y));
                        _sp.m_site_id = datas[0].m_sit->m_id;

                        _as.m_triangle->m_vertex.push_back(std::move(_sp));

                        site_point _sp1(point(datas[1].m_sit->x, datas[1].m_sit->y));
                        _sp1.m_site_id = datas[1].m_sit->m_id;
                        _as.m_triangle->m_vertex.push_back(std::move(_sp1));

                        site_point _sp2(point(datas[2].m_sit->x, datas[2].m_sit->y));
                        _sp2.m_site_id = datas[2].m_sit->m_id;
                        _as.m_triangle->m_vertex.push_back(std::move(_sp2));

                        _algo_sol.push_back(_as);
                    }
					else{
						_vp[idx].site1 = datas[0].m_sit->id();
						_vp[idx].site2 = datas[1].m_sit->id();
						_vp[idx].site3 = datas[2].m_sit->id();
						sol.push_back(_vp[idx]);
                    }
                }
				else
				{
					if (_vp.size() > 0)
					{
						_vp[0].site1 = datas[0].m_sit->id();
						_vp[0].site2 = datas[1].m_sit->id();
						_vp[0].site3 = datas[2].m_sit->id();
						sol.push_back(_vp[0]);
					}
				}
            }
        }
    }
	bool bSelect = true;
	
	/*if (CYaSetting::m_sys_setting.mp_sid_solution.find(4) != CYaSetting::m_sys_setting.mp_sid_solution.end())
	{
		int nSolution = CYaSetting::m_sys_setting.mp_sid_solution[4];
		if (nSolution != 0)
		{
			bSelect = false;
		}
	}*/
	
	
	if (bSelect)
	{
		
		// 如果有多于1个的解，
		// 需要计算该解到其对应三角形质心的距离，
		// 然后根据这个距离排序，选择距离最小的解
		if (_algo_sol.size() > 1)
		{
			double min_distance = 99999999999.0;
			point _p;
			for (auto it = _algo_sol.cbegin(); it != _algo_sol.cend(); ++it)
			{
				double d = (*it).m_triangle->get_distance(*((*it).m_pos));
				if (d < min_distance) {
					min_distance = d;
					_p = *((*it).m_pos);
					sol.clear();
					sol.push_back(*((*it).m_pos));
				}
			}
		}
		else if (_algo_sol.size() == 1) {
			sol.clear();
			sol.push_back(*(_algo_sol[0].m_pos));
		}
	}
	return sol;
}

int loc_tool_tdoa_2_base::index() 
{
	return 4;
}

bool loc_tool_tdoa_2_base::is_in_triangle(const std::vector<point>& vps, const point& p)
{
    double sabc = 0.0, sadb = 0.0, sbdc = 0.0, sadc = 0.0;
    sabc = get_triangle_area(vps[0], vps[1], vps[2]);
    sadb = get_triangle_area(vps[0], p, vps[1]);
    sbdc = get_triangle_area(vps[1], p, vps[2]);
    sadc = get_triangle_area(vps[0], p, vps[2]);

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

    return false;
}

double loc_tool_tdoa_2_base::get_triangle_area(const point& p0,const point& p1, const point& p2)
{
    return std::abs((p1.x - p0.x)*(p2.y - p1.y) - (p1.y - p0.y)*(p2.x - p1.x))/2.0;
}

// tdoa一维定位
std::vector<point> loc_tool_tdoa_1_base::calc_location(std::vector<loc_message>& locm)
{
	return std::vector<point>();
}
int loc_tool_tdoa_1_base::index() 
{
	return 3;
}


// tof 三基站三维定位
std::vector<point> loc_tool_tof_3_base::calc_location(std::vector<loc_message>&locm)
{
	std::vector<point> vtp;

	std::vector< sys::tof_data_oneAntenna> vec_td;
	std::string now = tool_time::to_str_ex(tool_time::now_to_ms());
	int index = 0;
	sys::tof_data_oneAntenna td;
	for (auto iter = locm.begin(); iter != locm.end(); iter++, index++)
	{
		// 更新数据到发送线程
		//if (iter->m_sit->m_id != 169)
		{
			td.map_site_id_dist[iter->m_sit->m_id] = iter->m_num_ticks*15.65*2.996*1e-4 + iter->m_sit->m_tof_dist_offset;
		}
		
		td.cid = tool_other::type_id_to_str(iter->m_card_type, iter->m_card_id);
		
		td.cur_time = now;
		
	}
	vec_td.push_back(td);
	//swsTimerThrd.upt_tof_data_oneAntenna(vec_td);

// 	if (false)
// 	{
// 		// 数据入库
// 		char sql[1024] = { 0 };
// 		snprintf(sql,
// 			1024,
// 			"insert into his_raw_data_card_tof_multi_site(card_id, distance1, site_id1, distance2, site_id2, distance3, site_id3, cur_time) values('%s', %.2f, %d, %.2f, %d, %.2f, %d, '%s')",
// 			td.cid.c_str(),
// 			td.dist1,
// 			td.sid1,
// 			td.dist2,
// 			td.sid2,
// 			td.dist3,
// 			td.sid3,
// 			now.c_str()
// 		);
// 		db_tool::PushAsync(sql);
// 		log_info("[sql] %s", sql);
// 	}
	
	std::vector<point> vts;
	std::vector<double> vtd;
	for(size_t i = 0;i < locm.size();++i){
		double dist = locm[i].m_num_ticks*15.65*2.996*1e-4;
		log_info("dist_%d:%.2f", i, dist);
		vtd.push_back(dist);
		vts.push_back(point(locm[i].m_sit->x, locm[i].m_sit->y, locm[i].m_sit->z));	
		log_info("site_%d:x:%.2f,y:%.2f,z:%.2f", i, locm[i].m_sit->x, locm[i].m_sit->y, locm[i].m_sit->z);
	}

	// 准备A矩阵的元素值
	double X10 = 2*(vts[1].x - vts[0].x);
	double X20 = 2*(vts[2].x - vts[0].x);
	double Y10 = 2*(vts[1].y - vts[0].y);
	double Y20 = 2*(vts[2].y - vts[0].y);
	double Z10 = 2*(vts[1].z - vts[0].z);
	double Z20 = 2*(vts[2].z - vts[0].z);

	// 准备B矩阵元素值
	double R0 = vts[0].value();
	double R1 = vts[1].value();
	double R2 = vts[2].value();

	double B10 = vtd[0]*vtd[0] - vtd[1]*vtd[1] + R1 - R0;
	double B20 = vtd[0]*vtd[0] - vtd[2]*vtd[2] + R2 - R0;

	// 构造A矩阵和B矩阵
	Eigen::Matrix<double, 2, 3> A;
	A << X10, Y10, Z10,
	  X20, Y20, Z20;

	Eigen::Matrix<double, 2, 1> B;
	B << B10,
	  B20;

	Eigen::Vector3d X = A.colPivHouseholderQr().solve(B);

	double x = 0.0, y = 0.0;
	x = X[0];
	y = X[1];

	// 将x,y代入园方程求解z坐标 ax*x + b*x + c = 0
	double a = 1;
	double b = -2* vts[0].z;
	double c = vts[0].z * vts[0].z - vtd[0]*vtd[0] + (x - vts[0].x)*(x - vts[0].x) + (y - vts[0].y)*(y - vts[0].y);

	double z0 = (-1*b + sqrt(b*b - 4*a*c))/(2*a);	
	double z1 = (-1*b - sqrt(b*b - 4*a*c))/(2*a);

	vtp.push_back(point(x, y, z0));
	vtp.push_back(point(x, y, z1));

	return std::move(vtp);
}

int loc_tool_tof_3_base::index() 
{
	return 2;
}

// tof二维定位
std::vector<point> loc_tool_tof_2_base::calc_location(std::vector<loc_message>&locm)
{
	std::vector<point> vtp;

	std::vector< sys::tof_data_oneAntenna> vec_td;
	std::string now = tool_time::to_str_ex(tool_time::now_to_ms());
	int index = 0;
	sys::tof_data_oneAntenna td;
	for (auto iter = locm.begin(); iter != locm.end(); iter++, index++)
	{
		// 更新数据到发送线程
		//if (iter->m_sit->m_id != 169)
		{
			td.map_site_id_dist[iter->m_sit->m_id] = iter->m_num_ticks*15.65*2.996*1e-4 + iter->m_sit->m_tof_dist_offset;
		}

		td.cid = tool_other::type_id_to_str(iter->m_card_type, iter->m_card_id);

		td.cur_time = now;

	}
	vec_td.push_back(td);
	swsTimerThrd.upt_tof_data_oneAntenna(vec_td);

	// 	if (false)
	// 	{
	// 		// 数据入库
	// 		char sql[1024] = { 0 };
	// 		snprintf(sql,
	// 			1024,
	// 			"insert into his_raw_data_card_tof_multi_site(card_id, distance1, site_id1, distance2, site_id2, distance3, site_id3, cur_time) values('%s', %.2f, %d, %.2f, %d, %.2f, %d, '%s')",
	// 			td.cid.c_str(),
	// 			td.dist1,
	// 			td.sid1,
	// 			td.dist2,
	// 			td.sid2,
	// 			td.dist3,
	// 			td.sid3,
	// 			now.c_str()
	// 		);
	// 		db_tool::PushAsync(sql);
	// 		log_info("[sql] %s", sql);
	// 	}

	std::vector<point> vts;
	std::vector<double> vtd;
	for (size_t i = 0; i < locm.size(); ++i) {
		double dist = locm[i].m_num_ticks*15.65*2.996*1e-4;
		log_info("dist_%d:%.2f", i, dist);
		vtd.push_back(dist);
		vts.push_back(point(locm[i].m_sit->x, locm[i].m_sit->y, locm[i].m_sit->z));
		log_info("site_%d:x:%.2f,y:%.2f,z:%.2f", i, locm[i].m_sit->x, locm[i].m_sit->y, locm[i].m_sit->z);
	}

	return std::vector<point>();
}
int loc_tool_tof_2_base::index() 
{
	return 1;
}

// tof一维定位
std::vector<point> loc_tool_tof_1_base::calc_location(std::vector<loc_message>&locm)
{
    int32_t last_ct = -1;
    std::vector<point> vec;
    std::vector<loc_message> lm;

	std::string now = tool_time::to_str_ex(tool_time::now_to_ms());
	std::unordered_map <std::string, sys::tof_data> map_tof_data;
    for(auto rit = locm.rbegin();rit != locm.rend();rit++)
    {
		auto cid = tool_other::type_id_to_str(rit->m_card_type, rit->m_card_id);
        if(rit->m_sit->is_path_empty() || rit->m_num_ticks == 0)
          continue;
        if(last_ct == -1)
          last_ct = rit->m_card_ct;
        else if(last_ct != rit->m_card_ct)
          continue;
		double dist_tof = rit->m_num_ticks*15.65*2.996*1e-4;
		map_tof_data[cid].cid = cid;
		map_tof_data[cid].sid = rit->m_sit->m_id;
		map_tof_data[cid].cur_time = now;
		map_tof_data[cid].ant_dist[rit->m_ant_id] = dist_tof;
		if (map_tof_data[cid].ant_dist[0] != 0 && 
			map_tof_data[cid].ant_dist[1] != 0)
		{
			rit->m_sit->count_ant_dist(map_tof_data[cid].ant_dist[0], map_tof_data[cid].ant_dist[1]);
		}
		map_tof_data[cid].ant_diff = floor(rit->m_sit->m_ant_dist*10.0 + 0.5) / 10.0;
        auto v = rit->m_sit->solving(rit->m_ant_id, dist_tof);
		
        lm.insert(lm.begin(),*rit);
        vec.insert(std::end(vec),std::begin(v),std::end(v));

// 		if (map_tof_data[cid].sid == 100)
// 		{
// 			return std::move(vec);
// 		}
    }

	for (auto map_iter = map_tof_data.begin(); map_iter != map_tof_data.end(); map_iter++)
	{
		//swsTimerThrd.upt_tof_data(map_iter->second);
		if (map_iter->second.ant_dist[0] > map_iter->second.ant_dist[1] &&
			map_iter->second.ant_dist[0] + map_iter->second.ant_dist[1] > 2.5)
		{
			log_info("sended=%f", (map_iter->second.ant_dist[0] + map_iter->second.ant_dist[1]) / 2);
		}
		else if (map_iter->second.ant_dist[0] < map_iter->second.ant_dist[1] &&
			map_iter->second.ant_dist[0] + map_iter->second.ant_dist[1] > 2.2)
		{
			log_info("sended=%f", (map_iter->second.ant_dist[0] + map_iter->second.ant_dist[1]) / (-2));
		}
		else
		{
			log_info("sended=%f", (map_iter->second.ant_dist[0] - map_iter->second.ant_dist[1]) / 2);
		}

		std::vector< sys::tof_data_oneAntenna> vec_td;
		std::string now = tool_time::to_str_ex(tool_time::now_to_ms());
		sys::tof_data_oneAntenna td;
			// 更新数据到发送线程

		td.map_site_id_dist[map_iter->second.sid * 10 + 1] = map_iter->second.ant_dist[0];
		td.map_site_id_dist[map_iter->second.sid * 10 + 2] = map_iter->second.ant_dist[1];

		auto site_ptr1 = sit_list::instance()->get(map_iter->second.sid * 10 + 1);
		auto site_ptr2 = sit_list::instance()->get(map_iter->second.sid * 10 + 2);

		if (nullptr == site_ptr1 || nullptr == site_ptr2)
		{
			log_warn("[tof] 分站信息缺失");
			break;
		}

		double x1 = site_ptr1->x;
		double y1 = site_ptr1->y;
		double x2 = site_ptr2->x;
		double y2 = site_ptr2->y;

		double d1 = map_iter->second.ant_dist[0];
		double d2 = map_iter->second.ant_dist[1];

		double d = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2));

		double sin_a = (y2 - y1) / d;
		double cos_a = (x2 - x1) / d;

		int direction = 1;
		if (d2 > d&&d1 < d2)
		{
			direction = -1;
		}

		if (direction == -1)
		{
			double angle = asin(sin_a) * 180 / 3.1415 + site_ptr1->m_tof_dist_offset;
			sin_a = sin(angle*3.1415 / 180);
			cos_a = cos(angle*3.1415 / 180);
		}
		//else
		//{
		//	double angle = asin(sin_a) * 180 / 3.1415 + site_ptr2->m_tof_dist_offset;
		//	sin_a = sin(angle*3.1415 / 180);
		//	cos_a = cos(angle*3.1415 / 180);
		//}
		

		log_info("zmg1x1=%f, y1=%f, cos_a=%f, sin_a=%f, d1=%f, direction=%d", (x1 + x2) / 2, (y1 + y2) / 2, cos_a, sin_a, d1, direction);
		double x = (x1+x2)/2 + cos_a * d1*direction;
		double y = (y1+y2)/2 + sin_a * d1*direction;

		td.x = x;
		td.y = y;


		
		
// 		td.map_site_id_dist[]
// 		td.dist1 = map_iter->second.ant_dist[0];
// 		td.sid1 = map_iter->second.sid * 10 + 1;
// 		td.dist2 = map_iter->second.ant_dist[1];
// 		td.sid2 = map_iter->second.sid * 10 + 2;
// 		td.dist3 = map_iter->second.ant_dist[1];
// 		td.sid3 = 0;
// 		td.dist4 = map_iter->second.ant_dist[1];
// 		td.sid4 = 0;

		td.cid = map_iter->second.cid;

		td.cur_time = now;

		//zmg
		//if (td.cid == "0010000001002"|| td.cid == "0010000001004")
		{
			vec_td.push_back(td);
// 			static int count = 0;
// 			count++;
// 			if (count >= 10)
// 			{
// 				swsTimerThrd.upt_tof_data_oneAntenna(vec_td);
// 				count = 0;
// 			}
			//static std::map<int, std::vector<int>> map_direction;
			//
			//auto directions = map_direction.find(td.cid);
			//
			//for (std::vector<int> iterDir = directions->second.begin();iterDir!= directions->second.end();iterDir++)
			//{
			//}

			swsTimerThrd.upt_tof_data_oneAntenna(vec_td);
			
// 			// 数据入库
// 			char sql[1024] = { 0 };
// 			snprintf(sql,
// 				1024,
// 				"insert into his_raw_data_card_tof_two_antenna(card_id, distance1, distance2, distanceAntennas, reader_id, cur_time) values('%s', %.2f, %.2f, %.2f, %d, '%s')",
// 				td.cid.c_str(),
// 				td.dist1,
// 				td.dist2,
// 				floor(locm.rbegin()->m_sit->m_ant_dist*10.0 + 0.5) / 10.0,
// 				locm.rbegin()->m_sit->m_id,
// 				now.c_str()
// 			);
// 			db_tool::PushAsync(sql);
// 			log_info("[sql] %s", sql);
		}
		
// 		if (false)
// 		if (td.dist1 != 0 && td.dist2 != 0 && td.dist3 != 0 && td.dist4 != 0 && td.cid == "0010000001004")
// 		{
// 			double v = 0.15;
// 			static double dis = 10;
// 
// 			static bool down = true;
// 
// 			if (dis < -10)
// 			{
// 				down = false;
// 			}
// 			else if (dis > 10)
// 			{
// 				down = true;
// 			}
// 
// 			if (down)
// 			{
// 				dis -= v;
// 			}
// 			else
// 			{
// 				dis += v;
// 			}
// 			unsigned seed;
// 			seed = time(0);
// 			srand(seed);
// 			auto r = rand() % 20;
// 			double d = (r - 10) / 100.0;
// 			td.dist1 = fabs(dis - 1.05) + d;
// 			td.dist2 = fabs(dis + 1.05) + d;
// 			td.dist3 = td.dist2;
// 			td.dist4 = td.dist2;
// 			r = rand() % 20;
// 			d = (r - 10) / 100.0;
// 			vec_td.push_back(td);
// 			swsTimerThrd.upt_tof_data_oneAntenna(vec_td);
// 
// 			// 数据入库
// 			char sql[1024] = { 0 };
// 			snprintf(sql,
// 				1024,
// 				"insert into his_raw_data_card_tof_two_antenna(card_id, distance1, distance2, distanceAntennas, reader_id, cur_time) values('%s', %.2f, %.2f, %.2f, %d, '%s')",
// 				td.cid.c_str(),
// 				td.dist1,
// 				td.dist2,
// 				floor(locm.rbegin()->m_sit->m_ant_dist*10.0 + 0.5) / 10.0,
// 				locm.rbegin()->m_sit->m_id,
// 				now.c_str()
// 			);
// 			db_tool::PushAsync(sql);
// 			log_info("[sql] %s", sql);
// 		}
	}

    locm.swap(lm);
	return std::move(vec);
}
int loc_tool_tof_1_base::index() 
{
	return 0;
}

loc_tool_main::loc_tool_main()
{
	set_tool(new loc_tool_tof_1_base());
	set_tool(new loc_tool_tof_2_base());
	set_tool(new loc_tool_tof_3_base());
	set_tool(new loc_tool_tdoa_1_base());
	set_tool(new loc_tool_tdoa_2_base());
	set_tool(new loc_tool_tdoa_3_base());
    set_tool(new loc_tool_pdoa_1_base());
    set_tool(new loc_tool_pdoa_2_base());
    set_tool(new loc_tool_pdoa_3_base());
}

loc_tool_main::~loc_tool_main()
{
	for(auto&tool:g_tool)
		delete tool;
}

void loc_tool_main::set_tool(loc_tool*tool)
{
	int index = tool->index();
	if(g_tool[index])
	{
		delete g_tool[index];
		g_tool[index]=0;
	}
	g_tool[index] = tool;
}

std::vector<point> loc_tool_main::calc_location(std::vector<loc_message>&locm)
{
	if(locm.empty())
    {
        return {};
    }
	int tool_index = locm[0].tool_index();
    /*int i = 1, len = locm.size();
	for(; i < len; ++i)
	{
		if(tool_index != locm[i].tool_index())
			break;
	}*/

    uint8_t type = tool_index>>4;
    uint8_t dim = tool_index & 0x03;

    tool_index = type*3 + dim - 1;

    log_info("[algo] loc_tool, tool_index=%d, loc_type=%d, dimension=%d", tool_index, type, dim);
    // 调用对应的算法进行定位计算
	//if(i==len)
	{
		return std::move(g_tool[tool_index]->calc_location(locm));
	}

	//包含至少两种定位方式的基站，目前只考虑两种
	/*std::vector<loc_message> locm1,locm2;
	locm1.assign(locm.begin(),locm.begin()+i);

	for(;i<len;i++)
	{
		if(tool_index!=locm[i].tool_index())
			locm2.push_back(locm[i]);
		else
			locm1.push_back(locm[i]);
	}
    bool flag = false;
    if(locm2[0].tool_index() > tool_index)
        flag = true;
    std::vector<point> rc;
    if(flag && (locm2[0].m_sit->config().best_msg_cnt<=(int)locm2.size()))
    {
		int index=locm2[0].tool_index();
		rc = std::move(g_tool[index]->calc_location(locm2));
        locm.swap(locm2);
    }
    else if(locm1[0].m_sit->config().best_msg_cnt<=(int)locm1.size())
	{
	    rc = std::move(g_tool[tool_index]->calc_location(locm1));
        locm.swap(locm1);
	}
    return std::move(rc);*/
}

loc_tool* loc_tool_main::g_tool[9]={0,0,0,0,0,0,0,0,0};