algo-l-zmg/select_tool.cpp

#include "select_tool.h"
#include "ant.h"
#include "card_path.h"
#include "log.h"
#include "card.h"
#include "loc_common.h"
#include "GaussianFilter/GaussianFilter_emxAPI.h"
#include <deque>
#include "load_raw.h"
#include "tool_time.h"

//opt_tool_main select_point_object::m_opt_tool;

void select_point_object::att_initiate()
{
    m_fitk.add_tool(30,5,5);
    m_fitk.add_tool(30,5,6);
    m_fitk.add_tool(30,10,10);
    m_fitk.add_tool(30,10,15);

    m_fitk.add_tool(20,5,6);
    m_fitk.add_tool(30,4,6);
    m_fitk.add_tool(40,4,6);

    m_fita.add_tool(15,5,5);
    m_begin=m_last=0;
}

//
int select_point_object::select_max_solution(const float& lhs, const float& rhs, const std::vector<point> vp, loc_point& lp)
{
    if(lhs > rhs){
        lp.set(vp[0]);
        return 0;
    }else{
        lp.set(vp[1]);
        return 1;
    }
}

//
int select_point_object::select_min_solution(const float& lhs, const float& rhs, const std::vector<point> vp, loc_point& lp)
{
    if(lhs < rhs){
        lp.set(vp[0]);
        return 0;
    }else{
        lp.set(vp[1]);
        return 1;
    }
}

loc_point select_point_object::select_solution_impl(const std::vector<point> vp,const std::vector<loc_message>&lm)
{
    bool flag = false;
    loc_point lp;

    //如果1维pdoa定位
    logn_info(3, "select_solution_impl status, dims=%d, loc_type=%d, vp's size=%d", lm[0].m_sit->m_num_dims, lm[0].m_loc_type, vp.size());

    switch(lm[0].m_loc_type){
        case LDT_TOF:
            if(vp.size()==4)
            {
                m_d.grow().reset().set_source(lm[0],lm[1]);
                flag = true;
            }
            if(vp.size()==2 && lm[0].m_card_ct - last_ct() == 1) 
            {
                m_d.grow().reset().set_source(lm[0]);
                flag = true;
            }
            if(flag)
            {
                if(!select_solution(vp,lm[0].m_sit.get(),lp))
                {
                    m_cur_fit.reset();
                }
            }
            break;
        case LDT_PDOA:
            select_pdoa_solution_impl(vp, lm, lp);    
            break;
    }
    
    m_ct = lm[0].m_card_ct;
    
    return lp;
}

bool select_point_object::select_pdoa_solution_impl(const std::vector<point> vp, const std::vector<loc_message>& lm, loc_point& lp)
{
    switch(get_algo_index(lm[0].tool_index())){
        case 6:
			if(lm[0].m_sync_ct){
				return select_pdoa_1d_solution_original(vp, lm, lp);
			}else{
				return select_pdoa_1d_solution(vp, lm, lp);
			}
            break;
        case 7:
            return select_pdoa_2d_solution(vp, lm, lp);
            break;
        case 8:
            return select_pdoa_3d_solution(vp, lm, lp);
            break;
    }

    return false;
}

/*
 * 当下井方向选解策略
 *
 * */
bool select_point_object::select_pdoa_solution(const std::vector<point> vp, const std::vector<loc_message>& lm, loc_point&lp, const float& pdoa)
{
    if(1 == vp.size())
    {
        bool dir = (pdoa > EPS);
        logn_info(3,"[pdoa] card_id=%d, ct=%d, vp0x=%.4f, vp0y=%.4f", lm[0].m_card_id, lm[0].m_card_ct, vp[0].x, vp[0].y);
        if(lm[0].m_sit->m_ant[0].x == lm[0].m_sit->m_ant[1].x)
        {
			if(0 == lm[0].m_sit->m_pdoa_direction){
				if(dir){
					if(vp[0].x > lm[0].m_sit->m_ant[0].x){
						lp.set(vp[0]);
						lp.m_useless = true;
					}else{
						logn_warn(3, "[pdoa] solution is not correct, card_id=%d", lm[0].m_card_id);
						lp.m_useless = false;
					}
				}else{
					if(vp[0].x < lm[0].m_sit->m_ant[0].x){
						lp.set(vp[0]);
						lp.m_useless = true;
					}else{
						logn_warn(3, "[pdoa] solution is not correct, card_id=%d", lm[0].m_card_id);
						lp.m_useless = false;
					}
				}
			}else{
				if(dir){
					if(vp[0].x < lm[0].m_sit->m_ant[0].x){
						lp.set(vp[0]);
						lp.m_useless = true;
					}else{
						lp.m_useless = false;
					}
				}else{
					if(vp[0].x > lm[0].m_sit->m_ant[0].x){
						lp.set(vp[0]);
						lp.m_useless = true;
					}else{
						lp.m_useless = false;
					}
				}
			}
		}else{
            logn_info(3, "[pdoa] select one solution, card_id=%d", lm[0].m_card_id);
			if(dir){	
			}
        }

		return lp.m_useless;

    }else if(2 == vp.size()){
        // select one solution from two solution by reader and phase direction
        logn_info(3, "[pdoa] select one solution from 2, card_id=%d, ct=%d, site_dir=%d, vp0x=%.4f, vp0y=%.4f, vp1x=%.4f, vp1y=%.4f", lm[0].m_card_id, lm[0].m_card_ct, lm[0].m_sit->m_pdoa_direction, vp[0].x, vp[0].y, vp[1].x, vp[1].y);
        bool dir = (pdoa > EPS);
        int max = -1, min = -1;
        if(0 == lm[0].m_sit->m_pdoa_direction)
        {
            if(lm[0].m_sit->m_ant[0].x == lm[0].m_sit->m_ant[1].x){
                dir?(max = select_max_solution(vp[0].x, vp[1].x, vp, lp)):(min = select_min_solution(vp[0].x, vp[1].x, vp, lp));
            }else if(lm[0].m_sit->m_ant[0].y == lm[0].m_sit->m_ant[1].y){
                dir?(max = select_max_solution(vp[0].x, vp[1].x, vp, lp)):(min = select_min_solution(vp[0].x, vp[1].x, vp, lp));
            }else if((lm[0].m_sit->m_ant[0].x != lm[0].m_sit->m_ant[1].x) && (lm[0].m_sit->m_ant[0].y != lm[0].m_sit->m_ant[1].y)){
                dir?(max = select_max_solution(vp[0].x, vp[1].x, vp, lp)):(min = select_min_solution(vp[0].x, vp[1].x, vp, lp));
            }
        }else{
            if(lm[0].m_sit->m_ant[0].x == lm[0].m_sit->m_ant[1].x){
                // 分站天线在竖直巷道
                dir?(min = select_min_solution(vp[0].y, vp[1].y, vp, lp)):(max = select_max_solution(vp[0].y, vp[1].y, vp, lp));
            }else if(lm[0].m_sit->m_ant[0].y == lm[0].m_sit->m_ant[1].y){
                // 分站天线在水平巷道
                dir?(min = select_min_solution(vp[0].x, vp[1].x, vp, lp)):(max = select_max_solution(vp[0].x, vp[1].x, vp, lp));
            }else if((lm[0].m_sit->m_ant[0].x != lm[0].m_sit->m_ant[1].x) && (lm[0].m_sit->m_ant[0].y != lm[0].m_sit->m_ant[1].y)){
                dir?(min = select_min_solution(vp[0].x, vp[1].x, vp, lp)):(max = select_max_solution(vp[0].x, vp[1].x, vp, lp));
            }
        }

        logn_info(3, "[pdoa] card_id=%d, cur_pdoa=%.2f, dir=%s site_dir=%d, min=%d, max=%d", lm[0].m_card_id, pdoa, (dir?"true":"false"), lm[0].m_sit->m_pdoa_direction, min, max);
    }

    return true;
}

/*  当出井方向切换分站选解
   本方法是根据之前通过距离的半径圆和基站路径集线段相交算出来的两个定位结果（即相交点），通过pdoa的正负
   进一步确认正确的定位坐标是那个相交点。
   输入：vp，两个定位结果（需要结果pdoa值的左右方向判断来推算出正确的定位结果），
   两个point （100，100），（100，200）
   是之前通过距离和基站路径集推算出来。
   输入：lm，定位卡的相关信息
   输入：pdoa，pdoa值
   输出：lp，选解的结果（根据pdoa的正负从vp两个point里面选出一个定位坐标结果）
   返回值：选解是否成功
   作者：祝云飞
*/
bool select_point_object::select_pdoa_solution2(const std::vector<point> vp, const std::vector<loc_message>& lm, loc_point&lp, const float& pdoa)
{
    if(1 == vp.size())
    {
        bool dir = (pdoa > EPS);
        logn_info(3,"[pdoa] card_id=%d, ct=%d, vp0x=%.4f, vp0y=%.4f", lm[0].m_card_id, lm[0].m_card_ct, vp[0].x, vp[0].y);
        if(lm[0].m_sit->m_ant[0].x == lm[0].m_sit->m_ant[1].x)
        {
			if(0 == lm[0].m_sit->m_pdoa_direction){
				if(dir){
					if(vp[0].x > lm[0].m_sit->m_ant[0].x){
						lp.set(vp[0]);
						lp.m_useless = true;
					}else{
						logn_warn(3, "[pdoa] solution is not correct, card_id=%d", lm[0].m_card_id);
						lp.m_useless = false;
					}
				}else{
					if(vp[0].x < lm[0].m_sit->m_ant[0].x){
						lp.set(vp[0]);
						lp.m_useless = true;
					}else{
						logn_warn(3, "[pdoa] solution is not correct, card_id=%d", lm[0].m_card_id);
						lp.m_useless = false;
					}
				}
			}else{
				if(dir){
					if(vp[0].x < lm[0].m_sit->m_ant[0].x){
						lp.set(vp[0]);
						lp.m_useless = true;
					}else{
						lp.m_useless = false;
					}
				}else{
					if(vp[0].x > lm[0].m_sit->m_ant[0].x){
						lp.set(vp[0]);
						lp.m_useless = true;
					}else{
						lp.m_useless = false;
					}
				}
			}
		}else{
            logn_info(3, "[pdoa] select one solution, card_id=%d", lm[0].m_card_id);
			if(dir){	
			}
			if (0 == lm[0].m_sit->m_pdoa_direction) {
				if (dir) {
					if (vp[0].x > lm[0].m_sit->m_ant[0].x) {
						lp.set(vp[0]);
						lp.m_useless = true;
					}
					else {
						logn_warn(3, "[pdoa] solution is not correct, card_id=%d", lm[0].m_card_id);
						lp.m_useless = false;
					}
				}
				else {
					if (vp[0].x < lm[0].m_sit->m_ant[0].x) {
						lp.set(vp[0]);
						lp.m_useless = true;
					}
					else {
						logn_warn(3, "[pdoa] solution is not correct, card_id=%d", lm[0].m_card_id);
						lp.m_useless = false;
					}
				}
			}
			else {
				if (dir) {
					if (vp[0].x < lm[0].m_sit->m_ant[0].x) {
						lp.set(vp[0]);
						lp.m_useless = true;
					}
					else {
						lp.m_useless = false;
					}
				}
				else {
					if (vp[0].x > lm[0].m_sit->m_ant[0].x) {
						lp.set(vp[0]);
						lp.m_useless = true;
					}
					else {
						lp.m_useless = false;
					}
				}
			}
        }

		return lp.m_useless;

    }else if(2 == vp.size()){
        // select one solution from two solution by reader and phase direction
        logn_info(3, "[pdoa] select one solution from 2, card_id=%d, ct=%d, site_dir=%d, vp0x=%.4f, vp0y=%.4f, vp1x=%.4f, vp1y=%.4f", lm[0].m_card_id, lm[0].m_card_ct, lm[0].m_sit->m_pdoa_direction, vp[0].x, vp[0].y, vp[1].x, vp[1].y);
        bool dir = (pdoa > EPS);
        int max = -1, min = -1;
		if(lm[0].m_sit->m_ant[0].x == lm[0].m_sit->m_ant[1].x){
			dir?(max = select_max_solution(vp[0].y, vp[1].y, vp, lp)):(min = select_min_solution(vp[0].y, vp[1].y, vp, lp));
		}else if(lm[0].m_sit->m_ant[0].y == lm[0].m_sit->m_ant[1].y){
			dir?(max = select_max_solution(vp[0].y, vp[1].y, vp, lp)):(min = select_min_solution(vp[0].y, vp[1].y, vp, lp));
		}else if((lm[0].m_sit->m_ant[0].x != lm[0].m_sit->m_ant[1].x) && (lm[0].m_sit->m_ant[0].y != lm[0].m_sit->m_ant[1].y)){
			dir?(max = select_max_solution(vp[0].y, vp[1].y, vp, lp)):(min = select_min_solution(vp[0].y, vp[1].y, vp, lp));
		}

		logn_info(3, "[pdoa] card_id=%d, cur_pdoa=%.2f, dir=%s site_dir=%d, min=%d, max=%d", lm[0].m_card_id, pdoa, (dir?"true":"false"), lm[0].m_sit->m_pdoa_direction, min, max);
    }

    return true;
}


/*bool select_point_object::select_pdoa_1d_solution(const std::vector<point> vp, const std::vector<loc_message>& lm, loc_point& lp)
{
    uint64_t id = lm[0].m_card_type;
    id = ((id<<32) | lm[0].m_card_id);
    auto card = card_list::instance()->get(id);

    double dist_tof = lm[0].m_num_ticks * 15.65 * 2.996 * 1e-4;

    if((card->m_buff_size != 100 || dist_tof < SAFE) && fabs(lm[0].m_poa[1] - 10.0) < 1E-4){
        return false;
    }

    int idx = lm[0].tool_index();
    uint8_t type = idx>>4;
    uint8_t dim  = idx&0x03;
    idx = type*3 + dim - 1;

    if(card->m_buff_size == 100 && dist_tof > SAFE && fabs(lm[0].m_poa[1] - 10.0) < 1E-4){
		lp.x = card->m_last_point.x;
		lp.y = card->m_last_point.y;

        double diff_time = (lm[0].m_loc_time - card->m_last_recv_time)*1.0/1000.0;
        double speed = get_speed(lp, card->m_last_point, diff_time) * lm[0].m_sit->m_scale; 
        if(speed > 15.0){
            lp.m_useless = false;
            return false;
        }

        lp.m_useless = true;
        logn_info(3, "[pdoa] solution info: card_id=%d, ct=%d, useless=%d, locate_x=%.4f, locate_y=%.4f, speed=%.3f, opt=true", lm[0].m_card_id, lm[0].m_card_ct, lp.m_useless, lp.x, lp.y, speed);

        card->m_last_pdoa_diff = card->m_pdoa_diff;
        card->m_last_ct = lm[0].m_card_ct;
        card->m_last_dist = dist_tof;
        card->m_cb_point.push_back(lp);
        card->m_last_site_id = lm[0].m_sit->m_id;
        card->m_last_site_dir = lm[0].m_sit->m_pdoa_direction;
        card->m_last_recv_time = lm[0].m_loc_time;
        card->m_last_point.x = lp.x;
        card->m_last_point.y = lp.y;
        lp.m_speed = card->m_speed = 0;

        return true;
    }

    card->m_pdoa_diff = get_pdoa(lm[0].m_poa, lm[0].m_sit->m_pdoa_offset);

    logn_info(3, "[pdoa] card_id=%d, pdoa=%.3f, poa[0]=%.3f, poa[1]=%.3f, poa[2]=%.3f, offset=%.4f, dist=%.3f", id, card->m_pdoa_diff, lm[0].m_poa[0], lm[0].m_poa[1], lm[0].m_poa[2], lm[0].m_sit->m_pdoa_offset, dist_tof);

    // after a long time,there is not recvied data, then reset pdoa list
    if(((card->m_last_dist < SAFE && dist_tof >= SAFE) || (lm[0].m_loc_time - card->m_last_recv_time > 30*1000)) && card->m_buff_size == 100){
        logn_info(3, "[pdoa] reset pdoa buffer, card_id=%d, cur_dist=%.3f, cur_time=%lld, last_dist=%.3f, last_time=%lld", lm[0].m_card_id, dist_tof, lm[0].m_loc_time, card->m_last_dist, card->m_last_recv_time);
        card->clear();
    }

    //first adjust, first 5 frame, not output located point
    uint16_t ct = lm[0].m_card_ct;
    if(card->m_buff_size < 5){
        card->m_cb_pdoa.push_back(card->m_pdoa_diff);
        ++card->m_buff_size;
        card->m_cb_tof.push_back(dist_tof);
        card->m_last_ct = ct;
        card->m_last_dist = dist_tof;
        card->m_last_recv_time = lm[0].m_loc_time;
        return false;
    }

    // choose 3 poa's direction as the card's direction
    if(5 == card->m_buff_size){
        card->m_buff_size = 100;
        int positive = 0;
        for(int i = 0;i < 5; ++i){
            if(card->m_cb_pdoa[i] > 0){
                ++positive;
            }
        }
        if(positive >= 3){
            for(int i = 0; i < 5; ++i){
                if(card->m_cb_pdoa[i] < 0){
                    card->m_cb_pdoa[i] = -1.0*card->m_cb_pdoa[i];
                }
            }
        }else{
            for(int i = 0;i < 5;++i){
                if(card->m_cb_pdoa[i] > 0){
                    card->m_cb_pdoa[i] = -1.0*card->m_cb_pdoa[i];
                }
            }
        }
    }

    // tof filter
    int ct_offset = -1;
    float offset = 0.0f;
    ct_offset = ct - card->m_last_ct;
    offset = card->m_cb_tof[4] - dist_tof;

    if(1 == ct_offset)
    {
        if(offset < 0){
            offset = -1.0*offset;
        }

        // need call opt way
        if(offset > 15){
            // illeglal tof
            logn_info(3, "[pdoa] the distance's is bigger than 15, card_id=%d, ct=%d, cur_dist=%.3f, last_ct=%d, last_dist=%.3f", lm[0].m_card_id, lm[0].m_card_ct, dist_tof, card->m_last_ct, card->m_cb_tof[4]);
            return false;
        }
    }

    card->m_last_dist = dist_tof;
    //card->m_cb_tof.push_back(dist_tof);
    logn_info(3, "[pdoa] normal, card_id=%d, ct=%d, cur_dist=%.3f, last_ct=%d, last_dist=%.3f", lm[0].m_card_id, lm[0].m_card_ct, dist_tof, card->m_last_ct, card->m_cb_tof[4]);

    //pdoa meadian filter
    std::array<float, 5> pdoa = {0};
    for(int i = 0; i < 5; ++i){
        pdoa[i] = card->m_cb_pdoa[i];
    }

    std::sort(pdoa.begin(), pdoa.end());

    float meadian_pdoa = pdoa[2];
    logn_info(3, "[pdoa] card_id=%d, ct=%d, before pdoa filter: meadian_pdoa=%.4f, cur_pdoa=%.4f, last_pdoa=%.4f, pdoa[0]=%.4f, pdoa[1]=%.4f, pdoa[2]=%.4f, pdoa[3]=%.4f, pdoa[4]=%.4f", lm[0].m_card_id, lm[0].m_card_ct, meadian_pdoa, card->m_pdoa_diff, card->m_last_pdoa_diff, card->m_cb_pdoa[0], card->m_cb_pdoa[1], card->m_cb_pdoa[2], card->m_cb_pdoa[3], card->m_cb_pdoa[4]);

    float cur_pdoa = card->m_pdoa_diff;

	bool change = false;
    if(lm[0].m_loc_time - card->m_last_recv_time < 30*1000){
        if(card->m_last_site_id !=0 && card->m_last_site_id != lm[0].m_sit->m_id){
            // pdoa change from site A to site B
            if(card->m_last_site_dir == 0 && lm[0].m_sit->m_pdoa_direction == 0){
                change = true;
            }else if(card->m_last_site_dir == 0 && lm[0].m_sit->m_pdoa_direction == 1){
                // keep same direction
            }else if(card->m_last_site_dir == 1 && lm[0].m_sit->m_pdoa_direction == 0){
                // keep same direction
            }else if(card->m_last_site_dir == 1 && lm[0].m_sit->m_pdoa_direction == 1){
                change = true;
            }

            if(change){
                for(int i = 0;i < 5; ++i){
                    card->m_cb_pdoa[i] *= -1.0;
                }
            }

			// 下面逻辑判断当前pdoa值是否有异常，如有异常就改变方向
			int same_direction = 0;
			for(int i = 0;i < 5;++i){
				if(card->m_cb_pdoa[i] * cur_pdoa > 0){
					same_direction++;
				}
			} 
			if(same_direction < 3){
				cur_pdoa*=-1;
			}

            logn_info(3, "[pdoa] card_id=%d, change site from %d to %d, pdoa change status=%d", lm[0].m_card_id, card->m_last_site_id, lm[0].m_sit->m_id, (change?1:0));

			card->m_cb_pdoa.push_back(cur_pdoa);
			card->m_cb_tof.push_back(dist_tof);
			// 定位退出
			if(!save_pdoa_1d(card, vp, lm, lp, cur_pdoa)){
				return false;
			}

			return true
        }
    }

   	// 5 meter away filter, abnormal poa
    //bool is_opposite = false;
    if(dist_tof > SAFE)
    { 
        int same_direction = 0;
        for(int i = 0;i< 5;++i){
            if(card->m_cb_pdoa[i] * cur_pdoa > 0){
                same_direction++;
            }
        }  

		if(same_direction < 3){
			logn_info(3, "[pdoa] at 5m away, the pdoa is opposite, card_id=%d, ct=%d, old_pdoa=%.3f", lm[0].m_card_id, lm[0].m_card_ct, cur_pdoa);

			float last_k = 0.0f, curr_k = 0.0f, llast_k = 0.0f;
			llast_k = card->m_cb_tof[3] - card->m_cb_tof[2];
			last_k = card->m_cb_tof[4] - card->m_cb_tof[3];
			curr_k = dist_tof - card->m_cb_tof[4];

			if(curr_k > 0 && last_k < 0 && llast_k < 0)
			{
				// find the site
				logn_info(3, "[pdoa] card has cross the site, card_id=%d, ct=%d", lm[0].m_card_id, lm[0].m_card_ct);
				// 缓存切换方向
			}else{
				card->m_cb_pdoa.push_back(cur_pdoa);
				card->m_cb_tof.push_back(cur_tof);
				// 定位退出
				if(!save_pdoa_1d(card, vp, lm, lp, cur_pdoa)){
					return false;
				}

				return true
			}
		}
	}else{
		// judge the position of site
        logn_info(3, "[pdoa] 5m in, card_id=%d, ct=%d, distance=%.4f", lm[0].m_card_id, lm[0].m_card_ct, dist_tof);
        float last_k = 0.0f, curr_k = 0.0f, llast_k = 0.0f;
        llast_k = card->m_cb_tof[3] - card->m_cb_tof[2];
        last_k = card->m_cb_tof[4] - card->m_cb_tof[3];
        curr_k = dist_tof - card->m_cb_tof[4];

        if(curr_k > 0 && last_k < 0 && llast_k < 0)
        {
            // find the site
            logn_info(3, "[pdoa] card has cross the site, card_id=%d, ct=%d", lm[0].m_card_id, lm[0].m_card_ct);
            for(int i = 0; i < 5;++i)
            {
                card->m_cb_pdoa[i] = 100;
            }
            //如果当前值方向和中值方向相同了，认为切了方向，因为距离信息当前方向和中值方向应该不同
			if(((0.0 - card->m_pdoa_diff) < EPS) == ((0.0 - meadian_pdoa) < EPS))
            {
                card->m_pdoa_diff = -card->m_pdoa_diff;
            }
		}
    }

   logn_info(3, "[pdoa] after pdoa filter, card_id=%d, ct=%d, current_pf=%.4f, last_pf=%.4f, cb[0]=%.4f, cb[1]= %.4f, cb[2]=%.4f, cb[3]=%.4f, cb[4]=%.4f, meadian=%.4f, tof[0]=%.4f, tof[1]=%.4f, tof[2]=%.4f, tof[3]=%.4f, tof[4]=%.4f", lm[0].m_card_id, lm[0].m_card_ct, card->m_pdoa_diff, card->m_last_pdoa_diff, card->m_cb_pdoa[0], card->m_cb_pdoa[1], card->m_cb_pdoa[2], card->m_cb_pdoa[3], card->m_cb_pdoa[4], meadian_pdoa, card->m_cb_tof[0], card->m_cb_tof[1], card->m_cb_tof[2], card->m_cb_tof[3], card->m_cb_tof[4]);
	
	card->m_cb_tof.push_back(dist_tof);
    card->m_cb_pdoa.push_back(card->m_pdoa_diff);

	if(!save_pdoa_1d(card, vp, lm, lp, cur_pdoa)){
		return false;
	}
    
	return true;
}*/

/*
 * 选择一维定位的解,不采用任何优化方法
 *
 * 参数
 *		const std::vector<point> vp			可能定位解
 *		const std::vector<loc_message>& lm	定位原始数据集
 *		loc_point& lp						保存最终定位解
 *
 * 返回值
 *		true表示成功选解，false表示选解失败
 *
 * */
bool select_point_object::select_pdoa_1d_solution_original(const std::vector<point> vp, const std::vector<loc_message>& lm, loc_point& lp)
{
	uint64_t id = lm[0].m_card_type;
	id = ((id << 32) | lm[0].m_card_id);

	auto card = card_list::instance()->get(id);
	double dist = lm[0].m_num_ticks * 15.65 * 2.996 * 1e-4;

	double pdoa = get_pdoa(lm[0].m_poa, lm[0].m_sit->m_pdoa_offset);
	logn_info(3, "[pdoa_o] card_id=%d, pdoa=%.2f, dist=%.2f, vp0x=%.2f, vp0y=%.2f, vp1x=%.2f, vp2y=%.2f", lm[0].m_card_id, pdoa, dist, vp[0].x, vp[0].y, vp[1].x, vp[1].y);

	double dt = (lm[0].m_loc_time - card->m_last_recv_time)*1.0 / 1000.0;
	double speed = fabs(dist - card->m_last_dist) / dt;

	if (!select_pdoa_solution2(vp, lm, lp, pdoa)) {
		return false;
	}

	logn_info(3, "[pdoa_o] card_id=%d, ct=%d, useless=%d, x=%.4f, y=%.4f, speed=%.3f, d=%.2f", lm[0].m_card_id, lm[0].m_card_ct, 1, lp.x, lp.y, speed);

	lp.m_useless = true;
	card->m_last_pdoa_diff = pdoa;
	card->m_last_ct = lm[0].m_card_ct;
	card->m_last_dist = dist;
	card->m_last_site_id = lm[0].m_sit->m_id;
	card->m_last_site_dir = lm[0].m_sit->m_pdoa_direction;
	card->m_last_point.x = lp.x;
	card->m_last_point.y = lp.y;
	lp.m_speed = card->m_speed = speed;
	card->m_cb_point.push_back(lp);
	card->m_vtp_dist.push_back(dist);

	return true;
}
/*
 * 选择一维定位的解,不采用任何优化方法
 *
 * 参数
 *		const std::vector<point> vp			可能定位解
 *		const std::vector<loc_message>& lm	定位原始数据集
 *		loc_point& lp						保存最终定位解
 *
 * 返回值
 *		true表示成功选解，false表示选解失败
 *
 * */
/*pdoa一维定位算法
card_location_base::on_location 调用进来的
作用：计算pdoa值（距离、角度）和定位结果lp（是传给web的最后的结果）
输入：
vp：距离数据（已经根据硬件读来的数据中算出了两个解，根据距离求出的位于基站两边的同距离的两个解）
lm：卡信息相关数据消息输入
输出：算出角度，然后判断左右方向。
lp，含有x、y坐标
added by zengminguo
从vp中取出
处理：算出pdoa的正负值，判断左右方向。算出角度。
*/
bool select_point_object::select_pdoa_1d_solution(const std::vector<point> vp, const std::vector<loc_message>& lm, loc_point& lp)
{
    uint64_t id = lm[0].m_card_type;
    id = ((id<<32) | lm[0].m_card_id);
    auto card = card_list::instance()->get(id);

    double dist = lm[0].m_num_ticks * 15.65 * 2.996 * 1e-4;

    int idx = lm[0].tool_index();
    uint8_t type = idx>>4;
    uint8_t dim  = idx&0x03;
    idx = type*3 + dim - 1;

	double pdoa = get_pdoa(lm[0].m_poa, lm[0].m_sit->m_pdoa_offset);
	card->m_pdoa_diff = pdoa;

	{
		if (card->m_last_site_id != lm[0].m_sit->m_id)
		{
			card->m_gaussian_count = 5;
			card->deqInput.clear();
		}

		if (card->deqInput.size() >= 20)
		{
			card->deqInput.pop_front();
		}
		card->deqInput.push_back(card->m_pdoa_diff);

		double pdoaInput[20];
		double pdoaOutput[20];

		int iIndex = 0;
		for (std::deque<double>::iterator iter = card->deqInput.begin(); iter != card->deqInput.end(); iter++)
		{
			pdoaInput[iIndex] = *iter;
			iIndex++;
		}

		//通过滤波计算出pdoa的正负值，使得左右判断准确，然后通过左右计算出最终定位坐标结果
		//现在默认采用高斯滤波（离基站近的时候）加移动均值滤波（非离基站近距离）的方式，使得定位效果最好。
		// added by zengminguo 20220424
		auto pdoaGaussian = card->m_pdoa_diff;
		emxArray_real_T* vecInput = emxCreateWrapper_real_T(pdoaInput, 1, card->deqInput.size());
		emxArray_real_T* vecOutput = emxCreateWrapper_real_T(pdoaOutput, 1, card->deqInput.size());
		card->m_gaussian_filter.GaussianFilter(vecInput, vecOutput);
		pdoaGaussian = pdoaOutput[card->deqInput.size() - 1];
		auto pdoaMovAvg = card->m_pdoa_diff;
		pdoaMovAvg = card->m_mov_avg_filter.MovAvgFilter(pdoaMovAvg);

		if (pdoaGaussian*card->m_last_pdoa_diff < 0)//反向
		{
			if (dist < 25 || card->m_gaussian_count>0)//在过站区域
			{
				card->m_pdoa_diff = pdoaGaussian;
				card->m_gaussian_count > 0 ? card->m_gaussian_count-- : card->m_gaussian_count;
			}
			else
			{
				if (fabs(pdoaMovAvg) >= 0.5)
				{
					card->m_pdoa_diff = pdoaMovAvg;
				}
				else
				{
					card->m_pdoa_diff = card->m_last_pdoa_diff;
				}
			}
		}
		else
		{
			card->m_pdoa_diff = pdoaGaussian;
		}
	}

	//pdoa = card->m_pdoa_diff; //先分析原始数据，不滤波

	// 1，进行新基站的时候，方向的稳定性，前5次判断基站的方向。通过5条，谁多是哪个方向。（前5秒的数据也给web）值高河之前的动画解决卡顿问题。
	// 后台采集介绍下。数据、接口、怎么编码 地图。做5个插值保证平滑。没有进一步定位了，
	//	2，只要我这个车辆的方向一直负方向，遇到正置负。唯一特例规则正负10米内，认。换了之后就一直为正。

	if (card->m_last_site_id != lm[0].m_sit->m_id)
	{
		card->m_count_plus = 0;
		card->m_count_minus = 0;
		card->m_site_crossed = false;
	}

	if (fabs(card->m_count_plus) + fabs(card->m_count_minus) < 5)
	{
		if (pdoa > 0)
		{
			card->m_count_plus++;
		}
		else
		{
			card->m_count_minus++;
		}
	}

	if (fabs(dist) > 10.0&&card->m_site_crossed == false)
	{
		if (card->m_count_plus > card->m_count_minus)
		{
			card->m_direction = 1.0;
		}
		else
		{
			card->m_direction = -1.0;
		}
	}
	else if (fabs(dist) <= 10.0&&fabs(card->m_last_dist) > 10.0)
	{
		card->m_direction_changing = true;
	}

	if (card->m_direction_changing)//正处于跨站状态
	{
		if (pdoa*card->m_last_pdoa_diff < 0.0)//反向
		{
			card->m_direction = card->m_direction*(-1.0);
			card->m_direction_changing = false;
			card->m_site_crossed = true;
		}
	}

	pdoa = fabs(pdoa)*card->m_direction;
	//card->m_last_pdoa_diff = card->m_pdoa_diff;
	logn_info(3, "[pdoa] card_id=%d, pdoa=%.2f, dist=%.2f, vp0x=%.2f, vp0y=%.2f, vp1x=%.2f, vp1y=%.2f", lm[0].m_card_id, pdoa, dist, vp[0].x, vp[0].y, vp[1].x, vp[1].y);

	// 距离 zengmg
	lp.m_dist = (pdoa > 0 ? 1 : -1)*dist;

	auto loc_time = lm[0].m_loc_time;
	if (load_raw::m_is_history)
	{
		if (load_raw::m_vec_date.size() > load_raw::m_index)
		{
			loc_time = tool_time::to_time(load_raw::m_vec_date[load_raw::m_index]);
		}
	}

	// 计算速度逻辑
	//double dt = (loc_time - card->m_last_recv_time)*1.0 / 1000.0;
	double dt = (lm[0].m_time_tmp - card->m_time_tmp_last)*1.0 / 1000.0;
	if (dt == 0)
	{
		dt = 1;
	}
	double speed = fabs(dist - card->m_last_dist) / dt;
	lp.m_speed = speed;
	card->m_last_recv_time = loc_time;

	// 计算角度逻辑
	do
	{
		float b = 0.075;
		float d = 0.5*b;

		float p = pdoa * b / (2.0*3.1415926);
		//logn_info(3, "[pdoa] before adjust, p = %.4f, pdoa_old=%.4f", p, 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);
			break;
		}

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

		lp.m_angle = angle;

	} while (0);

	if (lm[0].m_sit->m_pdoa_direction == 1) {
		pdoa *= -1;
	}

	if (!select_pdoa_solution2(vp, lm, lp, pdoa)) {
		log_info("[select_point_object] select_pdoa_solution2 error");
		return false;
	}
	
	card->m_time = time(0) * 1000;

	lp.m_useless = true;

	log_info("[pdoa] card_id=%d, ct=%d, pdoa=%.2f, dd=%.2f, dt=%.2f", lm[0].m_card_id, lm[0].m_card_ct, pdoa, dt*speed, dt);
	log_info("[pdoa] solution info: card_id=%d, ct=%d, useless=%d, locate_x=%.4f, locate_y=%.4f, speed=%.3f, d=%.2f", lm[0].m_card_id, lm[0].m_card_ct, lp.m_useless, lp.x, lp.y, speed, dt*speed);



	if (false)
	{
		if (card->m_last_recv_time > 0 && fabs(dt) > 30.0)
		{
			// 直接定位

			logn_info(3, "[pdoa] card_id=%d, pdoa=%.2f, dt=%.2f, speed=%.2f, last_time=%llu", lm[0].m_card_id, pdoa, dt, speed, card->m_last_recv_time);

			card->m_last_recv_time = lm[0].m_loc_time;

			return false;
		}

		card->m_last_recv_time = lm[0].m_loc_time;

		int n = card->m_vtp_dist.size();
		bool change_site = false;
		bool over_site = false;
		//切换基站,默认取正值
		if (lm[0].m_sit->m_id != card->m_last_site_id) {
			// 切换基站要考虑两种情况
			// 1）入井方向的切换
			if (card->m_cb_stream_idx[0] < lm[0].m_sit->m_down_stream_idx) {
				if (lm[0].m_sit->m_pdoa_direction == 0) {
					pdoa *= (pdoa > 0 ? 1 : -1);
				}
				else {
					pdoa *= (pdoa < 0 ? -1 : 1);
				}
			}
			else {
				// 2）出井方向的切换
				if (lm[0].m_sit->m_pdoa_direction == 0) {
					pdoa *= (pdoa < 0 ? 1 : -1);
				}
				else {
					pdoa *= (pdoa > 0 ? -1 : 1);
				}
			}

			if (!select_pdoa_solution2(vp, lm, lp, pdoa)) {
				return false;
			}

			card->m_cb_stream_idx.push_back(lm[0].m_sit->m_down_stream_idx);
			card->m_last_site_id = lm[0].m_sit->m_id;
			card->m_vtp_dist.clear();
			card->m_vtp_dist.resize(0);
			change_site = true;
			card->m_over_site = false;

			logn_info(3, "[pdoa] change site, card_id=%d, last_site=%d, last_dist=%.2f, cur_site=%d, cur_dist=%.2f", lm[0].m_card_id, card->m_last_site_id, card->m_last_dist, lm[0].m_sit->m_id, dist);
		}
		else if (!card->m_over_site && n >= 3) {
			// 过分站逻辑
			if (!select_pdoa_solution2(vp, lm, lp, pdoa)) {
				return false;
			}

			//double dd = dist - card->m_last_dist;
			double d = lp.dist(card->m_last_point)*lm[0].m_sit->m_scale;

			logn_info(3, "[pdoa] origin info, card_id=%d, ox=%.2f, oy=%.2f, lx=%.2f, ly=%.2f, d=%.2f, dt=%.2f", lm[0].m_card_id, lp.x, lp.y, card->m_last_point.x, card->m_last_point.y, d, dt);
			// 如果选出的解是正确解，即距离和测距距离相等
			double k1 = (card->m_vtp_dist[n - 2] - card->m_vtp_dist[n - 1]) / dt;
			double k2 = (card->m_vtp_dist[n - 1] - dist) / dt;
			logn_info(3, "[pdoa] card_id=%d, k1=%.2f, k2=%.2f, d=%.2f", lm[0].m_card_id, k1, k2, d);

			if (((k1 > 1 && k2 < -1) || (k1 < -1 && k2 > 1)) || (k1*k2 < 0 && fabs(d - dist - card->m_last_dist) < 2.0)) {
				over_site = true;
				card->m_over_site = true;
				//过基站还要分以下两种情况：
				//1）入井时的从正到负
				//2）出井时的从负到正
				logn_info(3, "[pdoa] card_id=%d, cur_idx=%d, last_idx=%d", lm[0].m_card_id, card->m_cb_stream_idx[0], card->m_cb_stream_idx[1]);
				if (card->m_cb_stream_idx[1] > card->m_cb_stream_idx[0]) {
					//1）入井时的从正到负
					pdoa *= (pdoa > 0 ? -1 : 1);
					/*if(lm[0].m_sit->m_pdoa_direction == 0){
					  pdoa *= (pdoa<0?-1:1);
					  }else{
					  pdoa *= (pdoa>0?1:-1);
					  }*/
				}
				else {
					//2）出井时的从负到正
					pdoa *= (pdoa < 0 ? -1 : 1);
					/*if(lm[0].m_sit->m_pdoa_direction == 0){
					  pdoa *= (pdoa<0?-1:1);
					  }else{
					  pdoa *= (pdoa>0?-1:1);
					  }*/
				}
				if (!select_pdoa_solution2(vp, lm, lp, pdoa)) {
					return false;
				}
			}
		}

		// 如果没有切换基站且没有过基站
		if (!change_site && !over_site) {
			if (!select_pdoa_solution2(vp, lm, lp, pdoa)) {
				return false;
			}

			double d = lp.dist(card->m_last_point)*lm[0].m_sit->m_scale;

			logn_info(3, "[pdoa] select solution, card_id=%d, x=%.2f, y =%.2f, last_x=%.2f, last_y=%.2f, d=%.2f, pd=%.2f, dt=%.2f", lm[0].m_card_id, lp.x, lp.y, card->m_last_point.x, card->m_last_point.y, dist, d, dt);


			if (fabs(d - fabs(dist - card->m_last_dist)) > 2.0) {
				if (!card->m_last_point.empty() && (lm[0].m_sit->m_id == card->m_last_site_id)) {
					int idx = 0;
					double min = 999999.0;
					for (size_t i = 0; i < vp.size(); ++i) {
						double d = vp[i].dist(card->m_last_point)*lm[0].m_sit->m_scale;
						if (fabs(d - speed * dt) < min) {
							idx = i;
							min = fabs(d - speed * dt);
						}
					}
					lp.set(vp[idx]);
				}
			}

			d = lp.dist(card->m_last_point)*lm[0].m_sit->m_scale;
			/*if(fabs(d-dist) > 0.5 && !card->m_last_point.empty()){
				logn_info(3, "[pdoa] card_id=%d, d=%.2f, dist=%.2f, lp.x=%.2f, lp.y=%.2f , solution is not correct!", lm[0].m_card_id, d, dist, lp.x, lp.y);
				return false;
			}*/

			logn_info(3, "[pdoa] unilateral processing, card_id=%d", lm[0].m_card_id);
		}

		lp.m_useless = true;

		logn_info(3, "[pdoa] card_id=%d, ct=%d, pdoa=%.2f, dd=%.2f, dt=%.2f", lm[0].m_card_id, lm[0].m_card_ct, pdoa, dt*speed, dt);
		logn_info(3, "[pdoa] solution info: card_id=%d, ct=%d, useless=%d, locate_x=%.4f, locate_y=%.4f, speed=%.3f, d=%.2f", lm[0].m_card_id, lm[0].m_card_ct, lp.m_useless, lp.x, lp.y, speed, dt*speed);

	}
   	
	if (!(card->m_isVehicleMap&&card->is_person()))
	{
		card->m_last_pdoa_diff = pdoa;
		card->m_last_ct = lm[0].m_card_ct;
		card->m_last_dist = dist;
		card->m_last_site_id = lm[0].m_sit->m_id;
		card->m_last_site_dir = lm[0].m_sit->m_pdoa_direction;
		card->m_last_point.x = lp.x;
		card->m_last_point.y = lp.y;
		int cell_index_diff = int(dist);
		int cell_index_current = lm[0].m_sit->m_cell_index - cell_index_diff;
		card->m_cell_index = cell_index_current;
		lp.m_speed = card->m_speed = speed;
		card->m_cb_point.push_back(lp);
		card->m_vtp_dist.push_back(dist);
	}

	return true; 	    
}

bool select_point_object::select_pdoa_2d_solution(const std::vector<point> vp, const std::vector<loc_message>& lm, loc_point& lp)
{
    logn_info(3, "[pdoa] begin select 2d solution, vp's size=%d", vp.size());
    int i = 1;
    for(auto it = vp.cbegin(); it != vp.cend();++it){
        logn_info(3, "[pdoa] useless %d : card_id=%d, x=%.4f, y=%.4f", i, lm[0].m_card_id,(*it).x, (*it).y);
        ++i;
    }

    if(1 == vp.size()){
        lp.x = vp[0].x;
        lp.y = vp[0].y;
        lp.m_useless = true;
    }

    uint64_t id = lm[0].m_card_type;
    id = ((id<<32) | lm[0].m_card_id);
    auto card = card_list::instance()->get(id);

    card->m_last_point.x = lp.x;
    card->m_last_point.y = lp.y;

    return true;
}

bool select_point_object::select_pdoa_3d_solution(const std::vector<point> vp, const std::vector<loc_message>& lm, loc_point& lp)
{
    return true;
}

int select_point_object::get_algo_index(const int& idx)
{
    uint8_t index = 0;
    uint8_t type = idx>>4;
    uint8_t dim = idx & 0x03;
    index = type*3 + dim - 1;
    log_info("[pdoa] algo=%d", index);

    return index;
}

double select_point_object::get_speed(const loc_point& p, const point& lp, const double& offset)
{
    double speed = -1.0;
    if(!(p.x <= EPS && p.y <= EPS)){
        speed = sqrt(pow(p.x - lp.x, 2) + pow(p.y - lp.y, 2))/offset;
    }

    logn_info(3, "[pdoa] calc_speed, p.x=%.3f, p.y=%.3f, lp.x=%.3f, lp.y=%.3f, diff_time=%lld", p.x, p.y, lp.x, lp.y, offset);

    return speed;
}

double select_point_object::get_distance(const int& rid, const int& lrid)
{
    double d = 0.0;

    /*auto cs = sit_list()->instance()->get(rid);
    auto ls = sit_list()->instance()->get(lrid);

    if(cs&&ls){
        d = cs.dist(ls);
        log_info("[pdoa] get distance from current site %d to last site %d", rid, lrid);
    }*/

    return d;
}

float select_point_object::get_pdoa(const double poa[], const double& offset)
{
    if(poa == nullptr){
        return -10.0;
    }

    float poa1 = poa[0];
    float poa2 = poa[1];
    //float poa3 = poa[2];

    float pdoa = poa2 - poa1 - offset;
    while(pdoa >= TPI){
        pdoa -= TPI;
    }

    while(pdoa < 0){
        pdoa += TPI;
    }

    pdoa -= PI;

    return pdoa;
}

int select_point_object::find_last(int start)
{
    for(int i=start,len=m_d.size();i<len;i++)
    {
        if(m_d(i).cl()>0)
            return i;
    }

    return -1;
}

int select_point_object::find_first(int start)
{
    for(int i=start,len=m_d.size();i<len;i++)
    {
        if(m_d[i].cl()>0)
            return i;
    }

    return -1;
}

/*
 * 拟合过滤
 *
 * param
 *		loc_point& c					最终保存点
 *		const std::vector<point>& vp	可能解
 *		const double scale				比例尺
 *
 * return
 *
 *
 * */
bool select_point_object::filter_by_fit(loc_point & c,const std::vector<point> & vp,const double scale)
{
    fit_result * fit = get_best_fit();
    if(fit==nullptr || m_begin==nullptr || fit->ke>2)
        return false;
    loc_point &f = *m_begin;
    std::array<solpoint,4> v;
    int cnt = vp.size();
    for(int i=0;i<cnt;i++)
    {
        v[i].set_sol(vp[i],fabs(fit->testk(c.m_time/1000.,f.dist_direct(vp[i]))));
    }
    std::sort(&v[0],&v[0]+cnt);
    double a = getA(fit,scale,v[1].score()-v[0].score());  
    if(a<0)
        return false;
    if(!filter_by_acc(c,v,a))
    {
        c.set_cl(0);
        return true; //false?
    }
    reset_fit(vp[0].dist(v[0])); 
    c[0]=v[0];
    c[1]=v[1];
    return true;

}

/*
 * 加速度过滤
 *
 * param
 *		loc_point& c
 *		std::array<solpoint, 4>& v
 *		double e
 *
 * return
 *
 * */
bool select_point_object::filter_by_acc(loc_point&c,std::array<solpoint,4>&v,double a)
{
    if(!m_last->is_same_site(c))
        return true;

    double td=m_last->time_off(c);

    if(v[0].score()>a*td*td)
        return false;

    return true;
}

/*
 *
 * */
void select_point_object::select_one_ant(loc_point &c,const std::vector<point> & vp)
{
    int last=find_last(1);
    if(last>0)
    {
        loc_point&p=m_d(last);

        int cnt=vp.size();
        std::array<solpoint,4> res;

        //find the shortest dis
        for(int i=0;i<cnt;i++) 
            res[i].set_sol(vp[i],vp[i].dist(p[0]));

        std::sort(&res[0],&res[0]+cnt);

        c[1].set(res[1]);
        c[0].set(res[0]);
        c.inc_cl(5);
    }
}

point select_point_object::select_solution0(std::vector<point> &tvp,const double scale)
{
    loc_point&c=m_d(0);
    if(m_d.size()==1)
    {
        //first point ,only accpet two ants good data.
        if(c.cl()>0 && tvp.size()<4)
        {
            c[1]=tvp[1];
            return c[0]=tvp[0];
        }

        m_d.skip(1);
        return point(0,0);
    }
    select_solution1(c,tvp,scale);
    return c[0];
}

bool select_point_object::select_solution(const std::vector<point> &vp,const site*sit,loc_point &p)
{
    remove_history(); 
    std::vector<point> tvp(vp.begin(),vp.end());
    if(vp.size()==4)
    {
        int c = 0;
        std::array<solpoint,4> res;
        for(int i=0;i<2;i++)
        {
            int x = i+2;
            double d=vp[i].dist(vp[x]);
            if(d<sit->ant_dist()*3)
            {
                res[c++].set_sol(vp[i].middle(vp[x]),d);
            }
            else
            {
                res[c++].set_sol(vp[i]);
                res[c++].set_sol(vp[x]);
            }
        }

        std::sort(&res[0],&res[0]+c);

        tvp.clear();
        for(int i=0;i<c;i++)
            tvp.push_back(res[i]);

        m_d(0).inc_cl(10);
    }
    point pt = select_solution0(tvp,sit->m_scale);
    if(pt.empty() || m_d.empty())
        return false;
    m_d(0).set(pt);
    if(!m_d(0).empty())
        m_d(0).m_dist=sit->dist_direct(pt);
    else
    {
        m_d(0).set_cl(0);
        if(m_last) m_d(0).m_dist=0.01*m_last->m_dist>0?1:-1;
    }

    m_d(0).m_dist1 = sit->dist_direct(pt);
    bool fg = revise_by_history(pt,sit,m_d(0).m_time);
    if(!fg)
    {
        log_warn("out of site path:t=%ld,sit=%d,card_id=%d,ct=%d,tof1=%d,tof2=%d,pt=(%f,%f)\n", m_d(0).m_time, m_d(0).m_sid,m_d(0).m_cid,m_d(0).m_ct, m_d(0).m_tof[0], m_d(0).m_tof[1], pt.x, pt.y);
        return false;	
    }

    if(!card_path::inst().is_at_path(pt))
    {
        m_d(0).set_cl(0);

        log_warn("out of path:t=%ld,sit=%d,card_id=%d,ct=%d,tof1=%d,tof2=%d,pt=(%f,%f)\n", m_d(0).m_time, m_d(0).m_sid,m_d(0).m_cid,m_d(0).m_ct, m_d(0).m_tof[0], m_d(0).m_tof[1], pt.x, pt.y);
        return false;
    }

    m_last=&m_d(0);

    if(m_line.empty() && m_d.size()>=2 && !make_line())
        return false;
    if(!m_line.contain(m_d(0),0.01) || (m_begin && !m_line.contain(m_begin->m_sol[0], 0.01)))
    {
        m_fitk.reset_data();
        m_fita.reset_data();
        m_begin=m_last=nullptr;

        int i0=find_last(1);
        if(i0==-1)
            return false;

        std::vector<point> path=card_path::inst().find_path(m_d(i0),m_d(0));

        m_d.skip(m_d.size()-i0);

        if(path.empty())
        {
            m_line.clear();
            return false;
        }

        m_line.set(path.back(),m_d(0));
    } 
    if(!m_begin)
    {
        int idx=find_first(0);
        if(idx>=0) m_begin=&m_d[idx];
    }

    if(!m_last)
    {
        int idx=find_last(0);
        if(idx>=0) m_last=&m_d(idx);
    }

    if(m_begin && m_d(0).cl())
    {
        int lastID1=find_last(1);
        int lastID2=-1;
        if(lastID1!=-1)
            lastID2=find_last(lastID1+1);
        if(lastID2!=-1)
        {
            double t0=m_last->m_time/1000., t1=m_d(lastID1).m_time/1000., t2=m_d(lastID2).m_time/1000.;
            double d0=m_begin->loc_dist(*m_last), d1=m_begin->loc_dist(m_d(lastID1)), d2=m_begin->loc_dist(m_d(lastID2));
            double k1=(d1-d0)/(t1-t0), k2=(d2-d1)/(t2-t1);
            if(t0-t1<5 && t1-t2<5 && fabs(k2-k1)<0.5)
            {
                double tbegin = t0-3;
                while(tbegin<t1+0.5)
                    tbegin=tbegin+1;
                double tk=(d0-d1)/(t0-t1), tb=d1-tk*t1;		//d1+(d0-d1)/(t0-t1)*(t-t1)
                for(double ti=tbegin;ti<t0;ti=ti+1)
                {
                    m_fitk.add(ti, tk*ti+tb);
                }
            }
        }
        m_fitk.add(m_last->m_time/1000.,m_begin->loc_dist(*m_last));

        if(m_d.size()>1)
        {
            int pre=find_last(1);
            if(pre>0) 
            {
                m_fita.add(m_d(0).m_time/1000.,m_begin->loc_dist(m_d(0))-m_begin->loc_dist(m_d(pre)));
            }
        }
    }

    p.set(pt);
    save_k();
    p.m_useless=fg;

    return true;
}

bool select_point_object::make_line()
{
    int i0=-1,i1=-1;

    if(-1==(i0=find_last(0)))
        return false;

    if(-1==(i1=find_last(i0+1)))
        return false;

    m_line.set(m_d(i0),m_d(i1));

    return true;
}

void select_point_object::remove_history()
{
    loc_point&b=m_d(0);

    if(m_d.size()>120 || (m_d.size()>2 && m_d(1).time_off(b)>max_histime))
    {
        m_d.skip_if([&b,this](loc_point&p){ 
                return p.time_off(b)>max_histime;
                });
        m_fitk.reset_data();
        m_fita.reset_data();
        m_cur_fit.reset();
        m_begin=m_last=nullptr;
        int idx=find_first(0);
        if(idx<0) 
            return;
        m_begin=&m_d[idx];
        idx=find_last(1);
        m_last=&m_d(idx);

        double dist=0,dist2=0;
        for(int len=std::min(15,m_d.size()-1),i=len;i>0;i--)
        {
            if(!m_d(i).cl())
                continue;
            int lastID1=find_last(i+1);
            int lastID2=-1;
            if(lastID1!=-1)
                lastID2=find_last(lastID1+1);
            if(lastID2!=-1)
            {
                double t0=m_d(i).m_time/1000., t1=m_d(lastID1).m_time/1000., t2=m_d(lastID2).m_time/1000.;
                double d0=m_begin->loc_dist(m_d(i)[0]), d1=m_begin->loc_dist(m_d(lastID1)[0]), 
                       d2=m_begin->loc_dist(m_d(lastID2)[0]);
                double k1=(d1-d0)/(t1-t0), k2=(d2-d1)/(t2-t1);
                if(t0-t1<5 && t1-t2<5 && fabs(k2-k1)<0.5)
                {
                    double tbegin = t0-3;
                    while(tbegin<t1+0.5)
                        tbegin=tbegin+1;
                    double tk=(d0-d1)/(t0-t1), tb=d1-tk*t1;		//d1+(d0-d1)/(t0-t1)*(t-t1)
                    for(double ti=tbegin;ti<t0;ti=ti+1)
                    {
                        m_fitk.add(ti, tk*ti+tb);
                    }
                }
            }

            dist=m_begin->loc_dist(m_d(i)[0]);
            m_fitk.add(m_d(i).m_time/1000.,dist);

            if(i==len)
                continue;

            m_fita.add(m_d(i).m_time/1000.,dist-dist2);
            dist2=dist;
        }
        save_k();
    }
}
void select_point_object::save_k()
{
    const fit_result*fk=best_fit_raw(0,4);
    if(!fk)
    {
        m_cur_fit.reset();
        return;
    }
    m_cur_fit=*fk;

    fit_result&r=m_cur_fit;
    card_fit*fa=&m_fita[0];
    if(fa->is_valid() && fa->ke<0.1 && fk->k*fa->k<0)
    {
        double dk=fa->k*fa->num_point;
        r.ka=fa->k;
        if((fk->k+dk)*fk->k<0)
            r.k=0;
        else
            r.k+=dk;

        double y=fk->k*m_fitk(0).x+fk->kb;
        r.kb=y-m_fitk(0).x*r.k;
    }
}
fit_result* select_point_object::best_fit_raw(int num_point,int start,int last)
{
    card_fit*fit = nullptr;

    start = std::max(start,0);
    last  = std::min(last,m_fitk.tool_size());
    if(last == -1)
        last = m_fitk.tool_size();

    for(int i = start; i < last; i++)
    {
        if(!m_fitk[i].is_valid())
            continue;

        if(m_fitk[i].num_point < num_point)
            continue;

        if(fit == nullptr)
        {
            fit = &m_fitk[i];
            continue;
        }

        if(fit->ke > m_fitk[i].ke)
        {
            fit = &m_fitk[i];
        }
    }

    return fit;
}

select_tool::~select_tool()
{
    if(m_spo !=nullptr)
        delete m_spo;
}

loc_point select_tool_person_1::select_solution(const std::vector<point> vp,const std::vector<loc_message>&lm)
{
    loc_point lp;
    //select point.
    uint8_t idx = 0;
    uint8_t type = lm[0].tool_index()>>4;
    uint8_t dim = lm[0].tool_index() & 0x03;

    idx = type*3 + dim - 1;
	
	//logn_info(3, "[pdoa] card_id=%d, idx=%d, dim=%d", lm[0].m_card_id, idx, dim);
    if(idx == 0)  
    {
        if(m_spo==nullptr)
            m_spo = new person_point_filter(this); 
        lp=m_spo->select_solution_impl(vp,lm);
    }
    else if (idx == 2)
    {
        //for now..
        //m_spo = new person_point_filter(); 
        //lp=m_spo->select_solution_impl(vp,lm);
    }else if(6 == idx){
        // pdoa一维定位
        //logn_info(3, "[pdoa] person, select_solution 1d, tool_index=%d", idx);
        if(nullptr == m_spo){
            m_spo= new person_point_filter(this);
        }
        lp = m_spo->select_solution_impl(vp, lm);
    }else if(7 == idx){
        // logn_info(3, "[pdoa] person, select_solution 2d, tool_index=%d", idx);
        // pdoa二维定位
        if(nullptr == m_spo){
            m_spo = new person_point_filter(this);
        }
        lp = m_spo->select_solution_impl(vp, lm);
    }else if(3 == idx){
    }else if(4 == idx){
    }else if(5 == idx){
    }
    else
    {}
    return lp;
}

loc_point select_tool_car_1::select_solution(const std::vector<point> vp,const std::vector<loc_message>&lm)
{
    loc_point lp;
    //select point.
    uint8_t idx = 0;
    uint8_t type = lm[0].tool_index()>>4;
    uint8_t dim = lm[0].tool_index() & 0x03;
	dim = 1;// modified by zengminguo 矿上默认为一维

    idx = type*3 + dim - 1;
    if(idx == 0)  
    {
        if(m_spo==nullptr)
            m_spo = new car_point_filter(this); 
        lp=m_spo->select_solution_impl(vp,lm);
    }
    else if (idx == 2)
    {
        //for now..
        //m_spo = new car_point_filter(); 
        //lp=m_spo->select_solution_impl(vp,lm);
    }else if(6 == idx){
        // pdoa一维定位
        if(nullptr == m_spo){
            m_spo = new car_point_filter(this);
        }
        lp = m_spo->select_solution_impl(vp, lm);
    }else if(7 == idx){
        // pdoa二维定位
        if(nullptr == m_spo)
        {
            m_spo = new car_point_filter(this);
        }
        lp = m_spo->select_solution_impl(vp, lm);
    }else if(3 == idx){
    }else if(4 == idx){
    }else if(5 == idx){
    }
    else
    {}
    return lp;
}

select_tool_manage * select_tool_manage::instance()
{
    static select_tool_manage stm;
    return &stm;
}