server/module_service/module_traffic_light_rule.cpp

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

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

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

        return false;
    }

    // 是否已经有车记录
    if(g->m_card_id > 0)
    {
        auto it = std::find(vtl.begin(), vtl.end(), g->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", g->m_card_id);
            return false;
        }
    }

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

    // 设置优先级以及卡id
    g->set_crossing(vtl[0], priority_crossing);
    // 修改红绿灯组状态
    change_state(vtl[0], g);

    return true;
}

bool crossing_rule::handle_avoidance(traffic_light_group_ptr g)
{
    // 查看是否是大车，true表示特殊红绿灯，需要检测大车避让规则
    bool flag = false;

    do{
        auto vt = traffic_light_manager::instance()->find_nearby_vehicle(point(static_cast<int>(g->x), static_cast<int>(g->y)), g->m_scope, 0);
        log_info("[traffic_light] handle avoidance, gid=%d, x=%.2f, y=%.2f, scope=%.2f, vehicle's size=%d", g->m_group_id, g->x, g->y, g->m_scope, vt.size());
        // 筛选大车
        if(vt.empty()){
            flag = true;
            break;
        }
        vt.erase(std::remove_if(vt.begin(), vt.end(), [&](const uint64_t& vid){
            return get_vehicle_state(vid);
                    }),
                vt.end());

        if(vt.empty()){
            flag = true;
            break;
        }

        // 查看离最近的点是否为特殊点，判断位置是否在直线上，判断方向
        vt.erase(std::remove_if(vt.begin(), vt.end(), [&](const uint64_t& vid){
            return find_light(vid, g);
                    }),
                vt.end());
        if(vt.empty()){
            flag = true;
            break;
        }

        // 特殊规则保留id且优先级2，避让规则置空id，优先级2
        if(priority_avoidance == g->get_priority()){
            if(0 == g->m_card_id){
                break;
            }else{
                auto it = std::find(vt.begin(), vt.end(), g->m_card_id);
                if(it != vt.end()){
                    break;
                }
            }
        }

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

        change_state(vt[0], g);
        g->set_crossing(vt[0], priority_avoidance);

        return true;
    }while(false);

    if(flag){
        // 找不到之前的车辆，则初始化
        if(priority_avoidance == g->get_priority() && g->m_card_id > 0){
            g->reset();
        }else{
            flag = false;
        }
    }

    return flag;
}

vt_traffic_group crossing_rule::handle_rule(pos_data& p)
{
    vt_traffic_group llist;
    log_info("[traffic_light] crossing_rule::handle_rule, group's size=%d", m_vt_group.size());
    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 && it->m_special){
                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();
        if(flag){
            llist.push_back(it);
        }
    }

    return std::move(llist);
}

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;
    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 = vg[i]->dist_direct(po.x, po.y);
        // 车辆在灯组左边或下边
        log_info("[traffic_light] avoidance_rule::find_group, area_id=%d, area_group=%d, dist=%.2f", area_id, vg.size(), dist);
        if(dist <= 0){
            // 速度方向与前进方向一致
            if(speed > 0){
                // 车辆到灯组的距离小于指定距离
                if(vg[i]->dist(po.x, po.y) <= g_max_scope){
                    llist.push_back(vg[i]);
                }

                // 紧挨着的一个灯组到车距离小于指定阈值
                if(i + 1 < vg.size()){
                    if(vg[i+1]->dist(po.x, po.y) <= g_max_scope){
                        llist.push_back(vg[i+1]);
                    }
                }
            }else if(speed < 0){
                // 速度方向与前进方向不一致，判断运动方向的灯组是否在可控范围
                if(i >= 1 && vg[i - 1]->dist(po.x, po.y) <= g_max_scope){
                    llist.push_back(vg[i-1]);
                }
                if(i >= 2 && vg[i-2]->dist(po.x, po.y) <= g_max_scope){
                    llist.push_back(vg[i-2]);
                }
            }

            break;
        }

        log_info("[traffic_light] the vehicle is in light group's left or down side.");
    }

    // 车辆在灯组右边或上边
    if(dist > 0 && speed < 0){
        std::size_t i = vg.size();
        if(i >= 1 && vg[i-1]->dist(po.x, po.y) <= g_max_scope){
            llist.push_back(vg[i-1]);
        }
        if(i >= 2 && vg[i-2]->dist(po.x, po.y) <= g_max_scope){
            llist.push_back(vg[i-2]);
        }
        log_info("[traffic_light] the vehicle is in light group's right or up side");
    }

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

    if(vti.empty()){
        return false;
    }

    // 筛选出符合条件的车辆，离第一个红绿灯距离进行排序
    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 %d 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();
}

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

vt_traffic_group avoidance_rule::handle_rule(pos_data& p)
{
    vt_traffic_group llist;

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

    log_info("[traffic_light] avoidance_rule::handle_rule, group's size=%d, area_id=%d", m_map_area_group.size(), p.m_area_id);
    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, green, red);
                    break;
                case 1:
                    //当前卡绑定了1个红绿灯组
                    if(p.m_light_group.front() != vtg[0]->m_group_id){
                        // 检查当前卡绑定的红绿灯组与区域找到的是不是同一个
                        // 不是，释放之前的绑定，再绑定新灯组
                        erase(true, p, llist);
                        insert(vtg[0], p, llist, green, red);
                    }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), green, red);
                    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), green, red);
                    llist.push_back(g);
                }
            }else{
                // 两个都不是，全部删除
                erase(true, p, llist);
                erase(false, p, llist);
                insert(g, p, llist, green, red);
            }
        }else if(2 == vtg.size()){
            // 所在区域存在两个红绿灯组，巷道相遇逻辑
            // 判断两个灯组中是否有车
            bool b = get_vehicle(p, vtg);
            light_color la, lb;
            if(b){
                la = red;
                lb = spark;
            }else{
                la = green;
                lb = red;
            }

            switch(p.m_light_group.size()){
                case 0:
                    // 当前卡没有绑定红绿灯组，绑定2个灯组
                    insert(vtg[0], p, llist, la, lb);
                    insert(vtg[1], p, llist, 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, 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), 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, 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), green, spark)){
                                        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), 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);
                                }
                            }
                            else
                            {
                                if(p.m_light_group.front() == vtg[0]->m_group_id){
                                    erase(false, p, llist);
                                    insert(vtg[1], p, llist, green, spark);
                                }else{
                                    erase(true, p, llist);
                                    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, 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);
                                }

                                if(vtg[1]->get_priority() <= priority_avoidance){
                                    change_group(vtg[1], point(p.x, p.y), green, spark);
                                    llist.push_back(vtg[1]);
                                }
                                p.m_light_group.push_front(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]);
                                }
                            }else{
                                erase(true, p, llist);
                                erase(true, p, llist);
                                insert(vtg[0], p, llist, la, lb);
                                insert(vtg[1], p, llist, green, spark);
                            }
                        }
                    }
                    break;     
            }
        }
    }else{
        log_info("[traffic_light] the %d area has no light group", p.m_area_id);
    }

    return std::move(llist);
}