algo/select_tool.cpp

#include "select_tool.h"
#include "ant.h"
#include "card_path.h"
#include "log.h"
#include "card.h"
#include "loc_common.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:
            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;
}

// 当出井方向切换分站选解
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){	
			}
        }

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

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

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

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

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

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;

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