server/module_service/module_traffic_light_rule.cpp

#include <cfloat>
#include "module_traffic_light_rule.h"
#include "module_traffic_light_manager.h"

// 车辆与指定灯组g距离比较，lhs近则返回true，否则返回false
bool rule::sort_vehicle(const uint64_t lhs, const uint64_t rhs, const traffic_light_group_ptr& g)
{
    pos_data* v1 = traffic_light_manager::instance()->get_position(lhs);
    pos_data* v2 = traffic_light_manager::instance()->get_position(rhs);
    double d1 = DBL_MAX, d2 = DBL_MAX;
    if(nullptr != v2){
        d2 = g->dist(v2->x, v2->y);
    }
    if(nullptr != v1){
        d1 = g->dist(v1->x, v1->y);
    }

    return d1 < d2;
}

// 根据车辆等级进行排序
bool rule::sort_vehicle_by_level(const uint64_t lhs, const uint64_t rhs, const traffic_light_group_ptr& g)
{
    //pos_data* lp = traffic_light_manager::instance()->get_position(lhs);
    //pos_data* rp = traffic_light_manager::instance()->get_position(rhs);

    //return lp->
    return true;
}

// 从灯组g中找到最近的灯
traffic_light_ptr rule::find_nearby_light(const point& p, traffic_light_group_ptr g)
{
    traffic_light_ptr pl = nullptr;
    double max = DBL_MAX;
    //查找最近的红绿灯
    for(auto it = g->m_vt_lights.begin(); it != g->m_vt_lights.end(); ++it)
    {
        traffic_light_ptr tmp = traffic_light_manager::instance()->find_light((*it)->m_light_id);
        double dt = tmp->dist(p.x, p.y) * traffic_light_manager::instance()->get_scale();
        if(dt < max){
            max = dt;
            pl = tmp;
        }
    }

    return pl;
}

// 设置灯的颜色
void crossing_rule::change_state(const uint64_t vid, traffic_light_group_ptr g)
{
    pos_data* cp = traffic_light_manager::instance()->get_position(vid);
    if(nullptr != cp){
        // 查找离车辆最近的红绿灯
        traffic_light_ptr pl = find_nearby_light(point(cp->x, cp->y), g);
        // 设置来车方向为绿灯，其他方向为红灯
        if(nullptr != pl){
            log_info("[traffic_light] card_id=%lld, light_id=%d, light_shape=%d", vid, pl->m_light_id, light_shape::green_all_on);
            g->set_light(pl->m_light_id, light_shape::green_all_on, light_shape::red_all_on);
        }
    }else{
        log_warn("[traffic_light] it can't get vehicle position. vid=%lld, lg's id=%d", vid, g->m_group_id);
    }
}

// 获得车辆是否大车的标记
bool crossing_rule::get_vehicle_state(const uint64_t& vid)
{
    bool s = false;

    pos_data* v = traffic_light_manager::instance()->get_position(vid);
    if(nullptr != v){
        s = v->m_bigger;
    }

    return s;
}
// 从红绿灯组中找距离最近的红绿灯
// 后续改为找上行或下行灯
bool crossing_rule::find_light(const uint64_t& vid, traffic_light_group_ptr g, bool a)
{
    pos_data* cp = traffic_light_manager::instance()->get_position(vid);
    if(nullptr != cp){
        line l;
        traffic_light_ptr pl = find_nearby_light(point(cp->x, cp->y), g);
        if(nullptr == pl){
            return false;
        }

        if(!a){
            if(1 == pl->m_special){
                // 判断是否在线上
                if(pl->m_line_group.contain(cp->x, cp->y, 5)){
                    // 判断行车方向
                    double d = cp->m_speed * pl->m_direct_distance;
                    if(d < 0){
                        return true;
                    }
                }
            }
        }else{
            // 判断是否在线上
            if(pl->m_line_group.contain(cp->x, cp->y, 5.0)){
                double d = cp->m_speed * pl->m_direct_distance;
                if(d < 0){
                    return true;
                }
            }
        }
    }

    return false;
}

// 手工控制
bool crossing_rule::handle_manual_ctrl(traffic_light_group_ptr g)
{
    if(g->is_time_out()){
        g->reset();
        return true;
    }

    return false;
}

bool crossing_rule::handle_crossing(traffic_light_group_ptr group)
{
    if(group->m_scope <= 0){
        return false;
    }

    // 查找一定范围的车辆
    std::vector<uint64_t> vtl = traffic_light_manager::instance()->find_nearby_vehicle(point(static_cast<int>(group->x), static_cast<int>(group->y)), group->m_scope, group->m_group_id);
    //log_info("[traffic_light] handle crossing rule.gid=%d, group_scope=%.2f, vehicle's num=%d", group->m_group_id, group->m_scope, vtl.size());
    if(vtl.empty()){
        if(group->m_card_id > 0 && group->m_priority == priority_crossing)
        {
            // 释放红绿灯组
            group->reset();
            return true;
        }

        return false;
    }

    // 是否已经有原控制车辆记录
    if(group->m_card_id > 0)
    {
        // check stream
        if(group->get_stream_state() != group->get_stream(group->m_card_id)){
            log_info("[traffic_light] %d's vehicle is passed crossing that is gid=%d, old_stream=%d, cur_stream=%d",group->m_card_id, group->m_group_id, group->get_stream_state(), group->get_stream(group->m_card_id));
            traffic_light_manager::instance()->vtlg_erase(group->m_card_id, group);
            return false;
        }
        auto it = std::find(vtl.begin(), vtl.end(), group->m_card_id);
        if(it != vtl.end()){
            log_info("[traffic_light] handle crossing rule, there has vehicle that control the light group, card_id=%d", group->m_card_id);
            return false;
        }
    }

    std::sort(vtl.begin(), vtl.end(), [&](const uint64_t& lhs, const uint64_t& rhs){
            return sort_vehicle(lhs, rhs, group);
            });

    move_stream ms = group->get_stream(vtl[0]);

    pos_data* p = traffic_light_manager::instance()->get_position(vtl[0]);
    if((group->m_card_id == 0) && (p->m_speed * p->dist_direct(*group) < 0)){
        log_info("[traffic_light] gid=%d is not same to vehicle=%d direction", group->m_group_id, p->m_card_id);
        return false;
    }

    // 对需要控制的N个灯组进行控制
    std::vector<traffic_light_group_ptr> vt_groups = traffic_light_manager::instance()->find_stream_groups(group, ms);
    if(vt_groups.empty()){
        log_info("[traffic_light] 找不到灯组");
        return false;
    }

    std::string s = "[";
    for_each(vt_groups.begin(), vt_groups.end(), [&s](traffic_light_group_ptr g){ s.append(std::to_string(g->m_group_id));});
    s.append("]");
    log_info("[traffic_light] the %d's vehicle control light group of %s", vtl[0], s.c_str());

    for(auto it : vt_groups){
        traffic_light_manager::instance()->vtlg_insert(vtl[0], it);
        it->set_crossing(vtl[0], priority_crossing);
        it->set_stream_state(ms);
        change_state(vtl[0], it);
    }

    return true;
}

//避让规则处理
//1)车辆等级规则
//2)上下行规则
bool crossing_rule::handle_avoidance(traffic_light_group_ptr g)
{
    if(g->m_card_id == 0){
        return false;
    }

    bool ret = false; 
    ret = avoidance_by_level(g);
    if(ret){
        return ret;
    }else{
        return avoidance_by_updown(g);
    }
}

// 根据车辆等级处理避让
bool crossing_rule::avoidance_by_level(traffic_light_group_ptr& group)
{
    // 根据车辆等级处理避让规则
    // 查找红绿灯组附近车辆
    auto vt = traffic_light_manager::instance()->find_nearby_vehicle(point(static_cast<int>(group->x), static_cast<int>(group->y)), group->m_scope, group->m_group_id);
    
    // 红绿灯组附近无车辆
    if(vt.empty()){
        if(priority_avoidance == group->get_priority() && group->m_card_id > 0){
            bool exist = traffic_light_manager::instance()->vtlg_exist(group->m_card_id, group->m_group_id);
            if(!exist){
                // 重置灯组控制权
                group->reset();
            }
        }
        return false;
    }
    
    if(vt.size() == 1 && vt[0] == group->m_card_id){
        log_info("[traffic_light] crossing_rule::avoidance_by_level, group is controlled, gid=%d, cid=%d", group->m_group_id, group->m_card_id);
        return false;
    }

    log_info("[traffic_light] crossing_rule::avoidance_by_level, gid=%d, x=%.2f, y=%.2f, scope=%.2f, ctrl_cid=%d, ivehicle's size=%d", group->m_group_id, group->x, group->y, group->m_scope, group->m_card_id, vt.size());

    if(group->m_card_id > 0 && get_vehicle_state(group->m_card_id)){
        //检查是否释放控制权
        if(group->get_stream_state() != move_stream::unknown)
        {
            // 如果车辆运行方向与灯组上下行方向不一致，则重置
            if(group->get_stream_state() != group->get_stream(group->m_card_id))
            {
                traffic_light_manager::instance()->vtlg_erase(group->m_card_id, group);
            }
        }
        return false;
    }

    bool exist_bigger_car = false;
    if(group->m_card_id == 0){
        for(size_t i = 0;i < vt.size(); ++i){
            if(get_vehicle_state(vt[i])){
                exist_bigger_car = true;
                break;
            }
        }
    }
    if(exist_bigger_car || (group->m_card_id > 0 && !get_vehicle_state(group->m_card_id))){
        // only keep vehicle's state that is bigger flag
        vt.erase(std::remove_if(vt.begin(), vt.end(), [&](const uint64_t& vid){
                    if(get_vehicle_state(vid)){
                    return false;
                    }else{
                    return true;
                    }
                    }),
                vt.end());

        if(vt.empty()){
            return false;
        }
    }
    // 删除不在红绿灯组所在路径上的车辆
    vt.erase(std::remove_if(vt.begin(), vt.end(), [&](const uint64_t& vid){
                pos_data* p = traffic_light_manager::instance()->get_position(vid);
                if(group->is_at_path(*p)){
                return false;
                }else{
                return true;
                }
                }),
            vt.end());
    if(vt.empty()){
        return false;
    }

    // 排序，找到离之最近的大车
    std::sort(vt.begin(), vt.end(), [&](const uint64_t& vid, const uint64_t& cid){
            return sort_vehicle(vid, cid, group);
            });

    move_stream ms = group->get_stream(vt[0]);
    if(ms == move_stream::unknown){
        return false;
    }

    // 对需要控制的N个灯组进行控制
    std::vector<traffic_light_group_ptr> vt_groups = traffic_light_manager::instance()->find_stream_groups(group, ms);
    if(vt_groups.empty()){
        log_info("[traffic_light] 找不到灯组");
        return false;
    }

    std::string s = "[";
    for_each(vt_groups.begin(), vt_groups.end(), [&s](traffic_light_group_ptr g){ s.append(std::to_string(g->m_group_id));});
    s.append("]");
    log_info("[traffic_light] the %d's vehicle control light group of %s", vt[0], s.c_str());

    for(auto& it : vt_groups){
        traffic_light_manager::instance()->vtlg_insert(vt[0], it);
        it->set_crossing(vt[0], priority_avoidance);
        it->set_stream_state(ms);
        change_state(vt[0], it);
    }

    return true;
}

// 根据车辆上下行处理避让
bool crossing_rule::avoidance_by_updown(traffic_light_group_ptr& group)
{
    // 1.检查控制灯组的车辆是否为上行车，如果是，则直接返回，表示上行车辆已控制灯组
    // 2.如果是下行车，则查看新车辆是否为上行，如果是，则控制
    if(group->m_card_id){
        if(group->get_stream(group->m_card_id) == move_stream::up_stream){
            return false;
        }
        //检查是否释放控制权
        if(group->get_stream_state() != group->get_stream(group->m_card_id))
        {
            traffic_light_manager::instance()->vtlg_erase(group->m_card_id, group);
            return false;
        }
    }

    auto vt = traffic_light_manager::instance()->find_nearby_vehicle(point(static_cast<int>(group->x), static_cast<int>(group->y)), group->m_scope, group->m_group_id);
    if(vt.empty()){
        if(priority_avoidance == group->get_priority() && group->m_card_id > 0){
            bool exist = traffic_light_manager::instance()->vtlg_exist(group->m_card_id, group->m_group_id);
            if(!exist){
                // 重置灯组控制权
                group->reset();
            }
        }
        return false;
    }

    log_info("[traffic_light] handle avoidance_by_updown, gid=%d, x=%.2f, y=%.2f, scope=%.2f, vehicle's size=%d", group->m_group_id, group->x, group->y, group->m_scope, vt.size());

    bool exist_up_stream = false;
    if(group->m_card_id == 0){
        for(size_t i = 0; i < vt.size(); ++i){
            if(group->get_stream(vt[i]) == move_stream::up_stream){
                exist_up_stream = true;
                break;
            }
        }
    }
    
    log_info("[traffic_light] handle avoidance_by_updown, gid=%d, control_card_id=%d, exist_up_stream_vehicle=%s", group->m_group_id, group->m_card_id, (exist_up_stream?"true":"false"));

    if(exist_up_stream || group->m_card_id > 0){
        // erase vehicles that's state of down_stream
        vt.erase(std::remove_if(vt.begin(), vt.end(), [&](uint64_t cid){
                    if(group->get_stream(cid) == move_stream::down_stream){
                        return true;
                    }else{
                        return false;
                    }
                    }), vt.end());

        if(vt.empty()){
            //log_info("[traffic_light] handle avoidance_by_updown")
            return false;
        }
    }

    log_info("[traffic_light] the number of vehicle that's state of up_stream = %d", vt.size());
    for(size_t i = 0;i < group->m_path.size();i++){
        log_info("%s", group->m_path[i].to_str().c_str());
    }

    // 根据车辆远近进行排序
    std::sort(vt.begin(), vt.end(), [&](const uint64_t& vid, const uint64_t& cid){
            return sort_vehicle(vid, cid, group);
            });

    // 每辆车上下行处理
    // 根据点与灯组的带方向距离和灯组中上行灯与下行灯之间的带方向距离信息，来判断车辆行径方向是上行or下行
    // 判断位置与红绿灯组的方向与速度得方向是否一致
    // 如果车辆静止不动的话，不考虑
    
    move_stream ms = group->get_stream(vt[0]);
    // 对需要控制的N个灯组进行控制
    std::vector<traffic_light_group_ptr> vt_groups = traffic_light_manager::instance()->find_stream_groups(group, ms);
    if(vt_groups.empty()){
        log_info("[traffic_light] 找不到灯组");
        return false;
    }

    std::string s = "[";
    for_each(vt_groups.begin(), vt_groups.end(), [&s](traffic_light_group_ptr g){ s.append(std::to_string(g->m_group_id));});
    s.append("]");
    log_info("[traffic_light] the %d's vehicle control light group of %s", vt[0], s.c_str());

    for(auto& it : vt_groups){ 
        if(it->m_card_id == 0
           || (ms == move_stream::up_stream && it->get_stream_state() == move_stream::down_stream)
          )
        {
            // 以下两种情况，改变灯组的控制权
            // 1.车辆为上行；
            // 2.灯组无控制权
            traffic_light_manager::instance()->vtlg_insert(vt[0], it);
            it->get_turn();
            it->set_crossing(vt[0], priority_avoidance);
            it->set_stream_state(ms);
            change_state(vt[0], it);

            log_info("[traffic_light] vehicle get group control, cid = %d, group_id=%d", vt[0], it->m_group_id);
        }    
    }

    return true;
}

void crossing_rule::handle_rule(pos_data& p)
{
    //log_info("[traffic_light] crossing_rule::handle_rule, group's size=%d", m_vt_group.size());
    if(m_pos.size()<2){
        log_warn("[traffic_light] pos_data's size = %d", m_pos.size());
        return;
    }
    
    for(auto& it : m_vt_group){
        // 获取控制权
        it->get_turn();
        int priority = it->get_priority();
        //log_info("[traffic_light] crossing_rule::handle_rule, gid=%d, priority=%d, special=%d", it->m_group_id, priority, it->m_special);
        bool flag = false;
        while(true){
            if(priority_manual_ctrl == priority){
                flag = handle_manual_ctrl(it);
                //log_info("[traffic_light] crossing_rule::handle_rule, after call handle_manual_ctrl, flag=%d", flag);
                if(flag){
                    break;
                }
            }

            if(priority <= priority_avoidance){
                flag = handle_avoidance(it);
                //log_info("[traffic_light] crossing_rule::handle_rule, after call handle_avoidance, flag=%d", flag);
                if(flag){
                    break;
                }
            }

            if(priority <= priority_crossing){
                flag = handle_crossing(it);
                //log_info("[traffic_light] crossing_rule::handle_rule, after call handle_crossing, flag=%d", flag);
                if(flag){
                    break;
                }
            }

            break;
        };

        it->release_turn();
    }
}

std::shared_ptr<run_red_light> crossing_rule::check_run_red_light(pos_data& p)
{
    return nullptr;
}

// 查找灯组
vt_traffic_group avoidance_rule::find_group(const pos_data& p)
{
    return find_group(point(p.x, p.y), p.m_area_id, p.m_speed, p);
}

vt_traffic_group avoidance_rule::find_group(const point& po , int area_id, double speed, const pos_data& pd)
{
    vt_traffic_group llist;

    if(0 == pd.m_card_id || 0 == pd.m_type){
        std::move(llist);
    }

    // 获取车辆所在地图区域内的所有红绿灯组
    vt_traffic_group vg = m_map_area_group[area_id];

    double dist = 0.0;
    double scale = traffic_light_manager::instance()->get_scale();

    double alpha = 0.0;
    int gid = 0;
    for(std::size_t i = 0; i < vg.size(); ++i){
        traffic_light_group_ptr pg = vg[i];
        if(nullptr == pg){
            continue;
        }

        // 如果此灯组被控制了，则不允许使用
        if(vg[i]->get_status() && vg[i]->m_card_id != pd.m_card_id){
            log_info("[traffic_light] card_id=%lld was control light group, gid=%d", pd.m_card_id, vg[i]->m_group_id);
            continue;
        }
        // 求灯组到车辆的距离（车辆在灯组的左下方，则距离值取反）
        dist = m_pos[1].dist_direct(vg[i]->x, vg[i]->y) * scale;
        point vp1(vg[i]->x - m_pos[0].x, vg[i]->y - m_pos[0].y), vp2(m_pos[1].x - m_pos[0].x, m_pos[1].y - m_pos[0].y);
        int f1 = (vg[i]->x > m_pos[0].x ? 1 : -1);
        int f2 = (m_pos[1].x > m_pos[0].x ? 1: -1);
        if(f1*f2 > 0 && fabs(dist) < vg[i]->m_scope)
        {
            double tmp = (vp1.x*vp2.x + vp1.y*vp2.y)/(sqrt(vp1.x*vp1.x + vp1.y+vp1.y) * sqrt(vp2.x*vp2.x + vp2.y*vp2.y));
            tmp = (tmp>0?(tmp>1?fabs(tmp-1):tmp):(tmp>-1?fabs(tmp):fabs(tmp+1)));
            if(tmp < alpha){
                alpha = tmp;
                gid = vg[i]->m_group_id;
            }
            llist.push_back(vg[i]);
            log_info("[traffic_light] avoidance_rule::find_group, possiable light_group_id=%d, alpha=%.2f, area_id=%d, area_light_group_size=%d, scope=%.2f, dist=%.2f, min_alpha=%.2f, speed=%.2f, direction=%d", vg[i]->m_group_id, tmp, area_id, vg.size(), vg[i]->m_scope, dist, alpha, speed, f1*f2);    
        }
    }

    if(gid > 0){
        llist.erase(std::remove_if(llist.begin(), llist.end(), [&](const traffic_light_group_ptr& g){
                    if(g->m_group_id != gid){
                        return true;
                    }else{
                        return false;
                    }
                    }), llist.end());
    }

    //检查接收基站所在路径有没有此灯组，没有则删除
    std::vector<pos_data> vtp;
    vtp.insert(vtp.begin(), m_pos.begin(), m_pos.end());

    llist.erase(std::remove_if(llist.begin(), llist.end(), [&](const traffic_light_group_ptr& g){
                    if(!traffic_light_manager::instance()->on_path(vtp, point(g->x, g->y))){
                         log_info("[traffic_light] the %d light group is not on %d site path.", g->m_group_id, vtp[0].m_site_id);
                         return true;
                    }else{
                        log_info("[traffic_light] the %d light group is on %d site path.", g->m_group_id, vtp[0].m_site_id);
                        return false;
                    }
                }),
            llist.end()
            );
  
    return std::move(llist);
}

bool avoidance_rule::find_vehicle_in_group(vt_traffic_group& vg, std::vector<pos_data>& vv)
{
    if(vv.size() < 2){
        log_error("[traffic_light] vv's size less than 2");
        return false;
    }

    bool flag = true;
    double d = vg[0]->dist(vv[1].x, vv[1].y);
    line tl(*vg[0], *vg[1]);
    if(tl.contain(vv[1].x, vv[1].y, 1))
    {
        if(vv[1].m_speed * vv[0].m_speed < 0 && d > g_mid_vehicle_length_group){
            flag = false;
        }
    }

    return flag;
}

bool avoidance_rule::get_vehicle(const pos_data& p, vt_traffic_group& vg)
{
    double d = vg[0]->dist(*vg[1]);

    std::vector<uint64_t> vt1 = traffic_light_manager::instance()->find_nearby_vehicle(point(static_cast<int>(vg[0]->x), static_cast<int>(vg[0]->y)), static_cast<int>(d), vg[0]->m_card_id);
    std::vector<uint64_t> vt2 = traffic_light_manager::instance()->find_nearby_vehicle(point(static_cast<int>(vg[1]->x), static_cast<int>(vg[1]->y)), static_cast<int>(d), vg[1]->m_card_id);

    std::sort(vt1.begin(), vt1.end());
    std::sort(vt2.begin(), vt2.end());

    std::vector<uint64_t> vti;

    std::set_intersection(vt1.begin(), vt1.end(), vt2.begin(), vt2.end(), std::back_inserter(vti));

    // 筛选出符合条件的车辆，离第一个红绿灯距离进行排序
    vti.erase(std::remove_if(vti.begin(), vti.end(), [&](const uint64_t& vid) -> bool {
                pos_data* tp = traffic_light_manager::instance()->get_position(vid);
                if(tp == nullptr){
                return true;
                }

                std::vector<pos_data> vv;
                vv.push_back(p);
                vv.push_back(*tp);
                return find_vehicle_in_group(vg, vv);
                }));

    return vti.empty()?false:true;
}

void avoidance_rule::insert(traffic_light_group_ptr g, pos_data& p, vt_traffic_group& vg, const int lc, const int lcr)
{
    p.m_light_group.push_back(g->m_group_id);
    if(g->get_priority() < priority_avoidance){
        // 红绿灯组优先级取初始值0或路口控制1
        change_group(g, point(p.x, p.y), lc, lcr);
        vg.push_back(g);
    }

    traffic_light_ptr pl = find_nearby_light(point(p.x, p.y), g);
    g->m_green_light_id = pl->m_light_id;
    // 灯组绑定车辆的卡号
    g->m_card_id = p.m_card_id;
    g->set_status(true);
}

void avoidance_rule::erase(bool a, pos_data& p, vt_traffic_group& vg)
{
    if(p.m_light_group.size() <= 0){
        return;
    }

    traffic_light_group_ptr pg = nullptr;

    bool flag = false;
    if(a){
        pg = traffic_light_manager::instance()->find_group(p.m_light_group.front());
        p.m_light_group.pop_front();
    }else{
        pg = traffic_light_manager::instance()->find_group(p.m_light_group.back());
        p.m_light_group.pop_back();
    }

    if(nullptr == pg){
        return;
    }

    pg->get_turn();
    if(pg->get_priority() <= priority_avoidance){
        // 重置灯组
        pg->reset();
        flag = true;
    }

    pg->release_turn();
    pg->set_status(false);

    if(flag){
        vg.push_back(pg);
    }
}

void avoidance_rule::change_group(const traffic_light_group_ptr& g, const point& p, const int& lc, const int& lcr)
{
    // 1.取离p最近的灯组信息
    traffic_light_ptr pl = find_nearby_light(point(p.x, p.y), g);
    if(nullptr == pl){
        return;
    }
    log_info("[traffic_light] change light's status in the %d's light group", g->m_group_id);
    // 2.获取灯组中临近车辆的灯id
    g->m_green_light_id = pl->m_light_id;
    g->get_turn();

    // 3.设置灯组中灯号为ld的灯颜色为lc，其余灯颜色为lcr
    g->set_avoidance(pl->m_light_id, lc, lcr);
    g->release_turn();
}

void avoidance_rule::change_group(const traffic_light_group_ptr& g, const move_stream& s, const int& lc, const int& lcr)
{
    g->get_turn();
    for_each(g->m_vt_lights.begin(), g->m_vt_lights.end(), [&](traffic_light_ptr& l){ 
            if(l->m_stream == s){
                g->m_green_light_id = l->m_light_id;
                g->set_light(l->m_light_id, lc, lcr);
            }            
            });

    g->release_turn();
}

bool avoidance_rule::is_different(traffic_light_group_ptr g, const point& p, const int& lc, const int& lcr)
{
    bool b = false;

    traffic_light_ptr pl = find_nearby_light(p, g);
    if(g->is_different(pl->m_light_id, lc, lcr)){
        b = true;
    }

    return b;
}

void avoidance_rule::handle_rule(pos_data& p)
{
    if(m_pos.size() < 2){
        log_warn("[traffic_light] pos_data's size = %d", m_pos.size());
        return;
    }

    vt_traffic_group llist;

    if(p.m_card_id == 0|| p.m_type == 0){
        return;
    }

    log_info("[traffic_light] avoidance_rule::handle_rule, group's size=%d, area_id=%d his_pos's size=%d, pos0.x=%.2f, pos0.y=%.2f, pos1.x=%.2f, pos2.y=%.2f", m_map_area_group.size(), p.m_area_id, m_pos.size(), m_pos[0].x, m_pos[0].y, m_pos[1].x, m_pos[1].y);
    auto it = m_map_area_group.find(p.m_area_id);
    if(it != m_map_area_group.end()){
        // 根据车卡的定位坐标以及定位区域信息获取红绿灯组
        vt_traffic_group vtg = find_group(point(p.x, p.y), p.m_area_id, p.m_speed, p);
        //log_info("[traffic_light] handle avoidance rule, area has light, area_id=%d, card_id=%d, light_group's size=%d", p.m_area_id, p.m_card_id, vtg.size());
        if(vtg.size() == 0){
            // 所在区域没有可以控制的红绿灯组
            switch(p.m_light_group.size())
            {
                case 1:
                    // 当前车辆控制的路口为1个，释放该路口控制
                    erase(true, p, llist);
                    break;
                case 2:
                    // 当前车辆控制的路口为两个，释放这两个路口的控制
                    erase(true, p, llist);
                    erase(false, p, llist);
                    break;
            }
        }else if(1 == vtg.size()){
            // 所在区域存在1个红绿灯组
            switch(p.m_light_group.size())
            {
                case 0:
                    // 当前卡没有绑定红绿灯组，绑定此灯组，并设置灯组颜色为绿色
                    insert(vtg[0], p, llist, light_shape::green_all_on, light_shape::red_all_on);
                    break;
                case 1:
                    //当前卡绑定了1个红绿灯组
                    if(p.m_light_group.front() != vtg[0]->m_group_id){
                        // 检查当前卡绑定的红绿灯组与区域找到的是不是同一个
                        // 不是，释放之前的绑定，再绑定新灯组
                        erase(true, p, llist);
                        insert(vtg[0], p, llist, light_shape::green_all_on, light_shape::red_all_on);
                    }else{
                        // 当前卡绑定的红绿灯组与区域找到的是同一个
                        traffic_light_ptr pl = find_nearby_light(point(p.x, p.y), vtg[0]);
                        if(nullptr != pl && pl->m_light_id != vtg[0]->m_green_light_id){
                            erase(true, p, llist);
                        }
                    }
                    break;
            }
        }else if(2 == p.m_light_group.size()){
            // 当前卡绑定了2个红绿灯组
            traffic_light_group_ptr g = vtg[0];
            if(p.m_light_group.front() == g->m_group_id){
                // 第一个绑定的红绿灯组与区域找到的是同一个，删除第二个
                erase(false, p, llist);
                if(g->get_priority() <= priority_avoidance){
                    change_group(g, point(p.x, p.y), light_shape::green_all_on, light_shape::red_all_on);
                    llist.push_back(g);
                }
            }else if(p.m_light_group.back() == g->m_group_id){
                // 第二个绑定的红绿灯组与区域找到的是同一个，删除第一个
                erase(true, p, llist);
                if(g->get_priority() <= priority_avoidance){
                    change_group(g, point(p.x, p.y), light_shape::green_all_on, light_shape::red_all_on);
                    llist.push_back(g);
                }
            }else{
                // 两个都不是，全部删除
                erase(true, p, llist);
                erase(false, p, llist);
                insert(g, p, llist, light_shape::green_all_on, light_shape::red_all_on);
            }
        }else if(2 == vtg.size()){
            // 所在区域存在两个红绿灯组，巷道相遇逻辑
            // 判断两个灯组中是否有车
            bool b = get_vehicle(p, vtg);
            int la, lb;
            if(b){
                la = light_shape::red_all_on;
                lb = light_shape::red_spark;
            }else{
                la = light_shape::green_all_on;
                lb = light_shape::red_all_on;
            }

            switch(p.m_light_group.size()){
                case 0:
                    // 当前卡没有绑定红绿灯组，绑定2个灯组
                    insert(vtg[0], p, llist, la, lb);
                    insert(vtg[1], p, llist, light_shape::green_all_on, light_shape::green_spark);
                    break;
                case 1:
                    // 当前卡有绑定1个红绿灯组，更新
                    if(p.m_light_group.front() == vtg[0]->m_group_id){
                        if(vtg[0]->get_priority() <= priority_avoidance){
                            change_group(vtg[0], point(p.x, p.y), la, lb);
                            llist.push_back(vtg[0]);
                        }
                        insert(vtg[1], p, llist, light_shape::green_all_on, light_shape::green_spark);
                    }else if(p.m_light_group.front() == vtg[1]->m_group_id){
                        if(vtg[1]->get_priority() <= priority_avoidance)
                        {
                            change_group(vtg[1], point(p.x, p.y), light_shape::green_all_on, light_shape::green_spark);
                            llist.push_back(vtg[1]);
                        }
                        p.m_light_group.push_front(vtg[0]->m_group_id);
                        if(vtg[0]->get_priority() < priority_avoidance)
                        {
                            change_group(vtg[0], point(p.x, p.y), la, lb);
                            llist.push_back(vtg[0]);
                        }
                    }else{
                        // 两个都不相等
                        erase(false, p, llist);
                        insert(vtg[0], p, llist, la, lb);
                        insert(vtg[1], p, llist, light_shape::green_all_on, light_shape::green_spark);
                    }
                    break;
                case 2:
                    // 当前卡有绑定2个红绿灯组，更新
                    {
                        std::list<int>::iterator ito = std::find(p.m_light_group.begin(), p.m_light_group.end(), vtg[0]->m_group_id);
                        std::list<int>::iterator itt = std::find(p.m_light_group.begin(), p.m_light_group.end(), vtg[1]->m_group_id);

                        if(ito != p.m_light_group.end())
                        {
                            if(itt != p.m_light_group.end())
                            {
                                if(p.m_light_group.front() == vtg[0]->m_group_id){
                                    if(is_different(vtg[0], point(p.x, p.y), la, lb)){
                                        llist.push_back(vtg[0]);
                                    }
                                    if(is_different(vtg[1], point(p.x, p.y), light_shape::green_all_on, light_shape::red_all_on)){
                                        llist.push_back(vtg[1]);
                                    }
                                }else{
                                    p.m_light_group.clear();
                                    if(vtg[0]->get_priority() <= priority_avoidance){
                                        change_group(vtg[0], point(p.x, p.y), la, lb);
                                        llist.push_back(vtg[0]);
                                    }
                                    if(vtg[1]->get_priority() <= priority_avoidance){
                                        change_group(vtg[1], point(p.x, p.y), light_shape::green_all_on, light_shape::green_spark);
                                        llist.push_back(vtg[1]);
                                    }
                                    p.m_light_group.push_back(vtg[0]->m_group_id);
                                    p.m_light_group.push_back(vtg[1]->m_group_id);
                                }
                                if(vtg[0]->get_priority() <= priority_avoidance){
                                    change_group(vtg[0], point(p.x, p.y), la, lb);
                                    llist.push_back(vtg[0]);
                                }
                                insert(vtg[1], p, llist, light_shape::green_all_on, light_shape::green_spark);
                            }
                            else
                            {
                                if(itt != p.m_light_group.end()){
                                    if(p.m_light_group.front() == vtg[1]->m_group_id){
                                        erase(false, p, llist);
                                    }else{
                                        erase(true, p, llist);
                                    }
                                    erase(true, p, llist);
                                    erase(true, p, llist);
                                    insert(vtg[0], p, llist, la, lb);
                                    insert(vtg[1], p, llist, light_shape::green_all_on, light_shape::green_spark);
                                }
                            }
                        }
                        break;     
                    }
            }
        }
        else if(vtg.size() > 2){
            log_info("[traffic_light] control light group's size = %d", vtg.size());
            for(auto it : vtg){
                move_stream s = it->get_stream(p.m_card_id);
                change_group(it, s, light_shape::green_all_on, light_shape::green_spark);
            }
        }
    }else{
        log_info("[traffic_light] the %d area has no light group", p.m_area_id);
    }
}

std::shared_ptr<run_red_light> avoidance_rule::check_run_red_light(pos_data& p)
{
    return nullptr;
}