server-whatever/module_service/module_traffic_light.cpp

#include <algorithm>
#include "module_traffic_light.h"
#include "module_traffic_light_manager.h"
#include "websocket/wsTimerThread.h"

void traffic_light_group::reset()
{
    m_time_stamp = 0;
    m_ctrl_name = "";
    m_card_id = 0;
    m_stream_state = move_stream::unknown;
    set_status(false);
    set_priority(priority_init);

    log_info("[traffic_light] reset light group, gid=%d", m_group_id);
    for(auto it = m_vt_lights.begin();it != m_vt_lights.end(); ++it){
        auto tl = traffic_light_manager::instance()->find_light((*it)->m_light_id);
        if(tl != nullptr){
            tl->set_state();
            send_cmd_light(tl);
        }
    }
}

void traffic_light_group::insert(traffic_light_ptr ptl)
{
    if(exist(ptl->m_light_id)){
        return;
    }

    ptl->m_direct_distance = this->dist_direct(*ptl);
    line tmp(*this, *ptl);
    ptl->m_line_group = tmp;
    m_vt_lights.push_back(ptl);
    if(1 == ptl->m_special){
        m_special = 1;
    }
}

bool traffic_light_group::exist(const int& lid)
{
    if(m_vt_lights.size() == 0){
        return false;
    }

    bool exist = false;
    for(size_t i = 0; i < m_vt_lights.size(); ++i){
        if(m_vt_lights[i]->m_light_id == lid){
            exist = true;
            break;
        }
    }

    return exist;
}

bool traffic_light_group::is_at_path(point p)
{
    return true;
}

// 设置灯组内所有灯的形状，指定灯ld的形状为lc，其他灯的形状为lcr
void traffic_light_group::set_light(const int& ld, const int& lc, const int& lcr)
{
    log_info("[traffic_light] set light group status, total_lights=%d, light_id=%d, status=%d, reverse_status=%d", m_vt_lights.size(), ld, lc, lcr);
    for(auto it = m_vt_lights.begin(); it != m_vt_lights.end(); ++it){
        if((*it)->m_light_id == ld){
            (*it)->set_state(lc);
            send_cmd_light(*it);
        }else{
            (*it)->set_state(lcr);
            send_cmd_light(*it);
        }
    }
}

move_stream traffic_light_group::get_stream(const int& cid)
{
    move_stream ms = move_stream::unknown;

    pos_data* p = traffic_light_manager::instance()->get_position(cid);

    if(nullptr == p){
        return ms;
    }

    if(size()<2){
        return ms;
    }

    double dist = p->dist_direct(point(x, y)) * traffic_light_manager::instance()->get_scale();

    if(dist * updown_dist() > 0){
        ms = move_stream::up_stream;
    }else if(dist * updown_dist() < 0){
        ms = move_stream::down_stream;
    }

    log_info("[traffic_light] cid=%d, stream=%d, dist_of_card_group=%.3f, updown=%.3f", p->m_card_id, ms, dist, updown_dist());

    return ms;
}

bool traffic_light_group::is_different(const int& ld, const int& lc, const int& lcr)
{
    bool ret = false;

    get_turn();
    if(get_priority() <= priority_avoidance)
    {
        for(auto it = m_vt_lights.begin(); it != m_vt_lights.end(); ++it)
        {
            if((*it)->m_light_id == ld)
            {
                if((*it)->m_state != lc)
                {
                    ret = true;
                    break;
                }
            }else{
                if((*it)->m_state != lcr)
                {
                    ret = true;
                    break;
                }
            }
        }
    }
    if(ret){
        set_avoidance(ld, lc, lcr);
    }

    release_turn();

    return ret;
}

// 向指定灯下发灯的状态
void traffic_light_group::send_cmd_light(traffic_light_ptr& ptl)
{
    if(nullptr == ptl){
        return;
    } 

    sys::light_state ls(m_group_id, ptl->m_light_id, ptl->m_state, ((m_card_id == 0)?"":tool_other::to_cid(2, m_card_id)));
    swsTimerThrd.upt_light_state(ls);

    traffic_light_manager::instance()->send_light_data(ptl->m_light_id, DT_LIGHT, ptl->m_state);
}

void traffic_light_group::set_path(const std::vector<line_v>& vl)
{
    if(vl.empty()){
        return;
    }

    const auto& find_line = [](const point& pt, int first, std::vector<line_v>& vl_){
        for(auto it = vl_.begin();it != vl_.end(); ++it){
            if(it->v[first].dist(pt) < 1){
                return it;
            }

            if(it->v[first?0:1].dist(pt) < 1){
                point p = it->v[0];
                it->v[0] = it->v[1];
                it->v[1] = p;
                return it;
            }
        }

        return vl_.end();
    };

    std::vector<line_v> vl_(vl.begin()+1, vl.end());
    std::vector<line_v> target(vl.begin(), vl.begin()+1);

    for(;;){
        auto it = find_line(target.back().v[1], 0, vl_);
        if(it == vl_.end()){
            break;
        }

        target.insert(target.end(), it, it+1);
        vl_.erase(it);
    }

    for(;;){
        auto it = find_line(target.front().v[0], 1, vl_);
        if(it == vl_.end()){
            break;
        }

        target.insert(target.begin(), it, it+1);
        vl_.erase(it);
    }

    for(int i = 0, c = target.size();i < c;++i){
        log_info("[traffic_light] path-id=%d, line=%s", i, target[i].to_string().c_str());    
    }

    std::vector<line_v> vline(target);
    vline.reserve(vline.size()+2);
    // 找到距离红绿灯组最近的端点
    auto min_it = vline.begin();
    double dist = 10;
    for(auto it = vline.begin(); it != vline.end(); ++it){
        double d = it->as_line().dist(*this);
        if(d < dist){
            dist = d;
            min_it = it;
        }
    }

    if(min_it == vline.end()){
        return;
    }

    if(abs(min_it->v[0].dist(*this)-dist) < 1){
        set_path(vline, min_it);
    }else if(abs(min_it->v[1].dist(*this) - dist) < 1){
        set_path(vl, min_it+1);
    }else{
        point p = min_it->projection(*this);
        vline.insert(min_it+1, line_v(p, min_it->v[1]));
        min_it->set_point(1, p);

        if(min_it->v[0].z){
            double slope = min_it->v[0].z;
            double len_a = min_it->v[0].dist((min_it+1)->v[1]);
            double len_0 = slope*min_it->length()/len_a;
            double len_1 = slope-len_0;

            min_it->v[0].z = min_it->v[1].z = len_0;
            (min_it+1)->v[0].z = (min_it+1)->v[1].z = len_1;
        }

        set_path(vline, min_it+1);
    }
}

void traffic_light_group::set_path(const std::vector<line_v>& vl, std::vector<line_v>::const_iterator itm)
{
    auto it = itm;
    for(int i=0;i<2&&it!=vl.end();++it,++i){
        m_path[0][i] = *it;
    }

    it = itm-1;
    for(int i=0;i<2&&it>=vl.begin();--it,++i){
        m_path[1][i] = *it;
        m_path[1][i].swap_point();
    }

    for(int i=0;i<2;i++){
        point p = m_path[i][0].line::projection(*this);
        m_path[i][0].set_point(0, p);
    }

    for(const auto& p: m_path){
        log_info("[traffic_light] light group's path: %s", p.to_str().c_str());
    }
}

/*
 *
 * param
 *      name        user's name
 *      ld          light id
 *      lc          ligth color
 * */
void traffic_light_group::set_manual_ctrl(const std::string& name, const int& ld, const int& lc)
{
    get_turn();
    m_ctrl_name = name;
    m_time_stamp = time(NULL);
    set_priority(priority_manual_ctrl);
    m_card_id = 0;
    auto ptl = std::find_if(m_vt_lights.begin(), m_vt_lights.end(),[=](traffic_light_ptr lp){
            if(lp->m_light_id == ld)
            {
            return true;
            }else{
            return false;
            }
            });
    if(ptl != m_vt_lights.end()){
        (*ptl)->set_state(lc);
    }

    release_turn();
}

/*
 * 计算上行与下行灯之间的距离，带方向
 * */
double traffic_light_group::updown_dist()
{
    point up;
    point down;

    for(auto it : m_vt_lights){
        switch(it->m_stream){
            case 1:
                //上行
                up = point(it->x, it->y, it->z);
                break;
            case 2:
                // 下行
                down = point(it->x, it->y, it->z);
                break;
            default:
                break;
        }
    }

    if(up.empty()||down.empty()){
        log_info("[traffic_light] the %d's light group, up_light empty=%s, down_light empty=%s", m_group_id, (up.empty()?"true":false), (down.empty()?"true":"false"));
        return 0;
    }

    return up.dist_direct(down);
}