server/pointSmooth/trafficLight.cpp

#include "stdafx.h"
#include "trafficLight.h"
#include "carManager.h"
#include "ProcessRemodule.h"
// #include "INIFILE.h"
// #include "def.h"
//路口红绿灯
//大巷红绿灯
NAMESPACE_POINT_BEGIN(NAMESPACE_POINT)
//路口规则
//小车避让大车规则
//代理模式
//装饰器模式
//抽象工厂模式

//x->大        speed 正
//x->恒  
//      y->大  speed 正 
//      y->小  speed 负
//x->小        speed 负

//这里的初始化红绿灯组vector初始化，因数据库配置和实际情况进行初始化，不能保证通用，需要在不同的情形下特殊处理
//这里是根据区域划分红绿灯，然后根据红绿灯位置按照（上行或者下行）进行排序
//这里初始化根据，每一条巷道为一组，遇到拐点，则是新的巷道。因为，拐点必有红绿灯。
//需要考虑到T字口和十字口中有车辆过来，但非是指定区域即辅运大巷
    std::vector<std::string> Rule::find_near(int x,int y,int dist,const std::string & card_no) 
    {
        std::vector<std::string> v;
        if(auto pt = m_owner.lock())
            v = pt->find_near(x,y,dist,card_no);
        return std::move(v);
    }

    std::shared_ptr<card_interface> Rule::getCard(const std::string &card_no)
    {
        //return m_[card_no];
        std::shared_ptr<card_interface> c = nullptr;
        if(auto pt = m_owner.lock())
          c = pt->getCard(card_no);
        return c;
    }
    //谁先到达路口红绿灯指定的距离，which side turn green.then else turn red..
    //
    bool CrossingRule::getBigCarFlag(const std::string &card_no)
    {
        bool flag=false;
        auto cardPtr = getCard(card_no);
        if(cardPtr!=nullptr)
            flag =  cardPtr->getBigCarFlag();
        return flag?false:true;
    }
   //here should another thread to handle it .
    bool  CrossingRule::handleManualControl(std::shared_ptr<TrafficLightGroup> pt) 
    {
        if(pt->timeout()) 
        {
            pt->resetGroup();
            return true;
        }
        return false;
    }
     bool CrossingRule::findNearLight(const std::string & card_no,std::shared_ptr<TrafficLightGroup> tlg_ptr,bool a )
    {
        bool flag = false;
        auto cardPtr = getCard(card_no);
		double distance = 0.0;
        if(cardPtr!=nullptr)
        {
            bool special = false; 
            line l;
            double distDirect = 0;
            double x=cardPtr->getX();
            double y=cardPtr->getY();
            double v=cardPtr->getv();
           auto lightinfo = findNearLightInfo(x,y,tlg_ptr);
           special = std::get<0>(lightinfo);
           l = std::get<1>(lightinfo);
           distDirect = std::get<2>(lightinfo);
		   distance = l.dist(op::point(x,y));
		   //debug_print_syslog(0,"findNearLight.x:%0.3f,y:%0.3f,v:%0.3f,dist:%0.3f,",x,y,v,distDirect);
		   if (!a)
		   {
			   if(special)
			   {
				   //判断是否在线上
				   if(l.contain(x,y,0.5))
				   {
					   //判断行车方向  
					   double c = v*distDirect;
					   if(c<0)
						   flag = true;
				   }
			   }
		   }
		   else
		   {
			   if((tlg_ptr->m_gId == 2 || tlg_ptr->m_gId == 4)||tlg_ptr->m_gId == 11)
			   {
				   flag = true;
			   }else if(tlg_ptr->m_gId == 8 ||tlg_ptr->m_gId == 18||tlg_ptr->m_gId == 20)
			   {
				   flag = true;
			   }
			   else if(l.contain(x,y,1.0))
			   {
				   double c = v*distDirect;
				   if(c<0)
					   flag = true;
			   }else{
				   debug_print_syslog(0,"lamp light: findNearLight: gid: %d,card_id: %s, distance: %.3f",tlg_ptr->m_gId,card_no.c_str(),l.dist(op::point(x,y)));
			   }
			   
			   //判断是否在线上
			   //if(l.contain(x,y,1.0))
			   //{
				  // double c = v*distDirect;
				  // //debug_print_syslog(0,"findNearLight:%0.3f,%s",c,c<0?"TRUE":"FALSE");
				  // if(c<0)
					 //  flag = true;
			   //}
		   }

		   debug_print_syslog(0,"lamp light :findNearLight flag: %s gid: %d,card_id: %s,distance: %.3f,a: %d, special: %d",flag?"TRUE":"FALSE",tlg_ptr->m_gId,card_no.c_str(),distance,a,special);
		}else{
			debug_print_syslog(0,"lamp light :findNearLight gid: %d,card_id: %s ",tlg_ptr->m_gId,card_no.c_str());
		}
		
        return flag?false:true;
    }
    bool CrossingRule::handleSpecialAvondance(std::shared_ptr<TrafficLightGroup> pt) 
    {
        //查看是否是大车
         //true标识特殊红绿灯，需要检测大车避让规则
        bool flag = false;
        do{
            auto vt = find_near(static_cast<int>(pt->x),static_cast<int>(pt->y),(int)(ConfStruct::getCs().max_scope_bigCar),itostr(pt->m_gId));
            //筛选大车
            if(vt.empty()) {
				flag = true;break;
			}
            vt.erase(std::remove_if(vt.begin(),vt.end(),[&](const std::string &card_no){
				return getBigCarFlag(card_no);
			}),vt.end());
			if(vt.empty()) {
				flag = true;
				break;
			}
			vt.erase(std::remove_if(vt.begin(),vt.end(),[](const std::string &name){
				if(0 == name.compare(0,3,"002"))
					return false;
				else
					return true;}),vt.end());
            if(vt.empty()) {
				flag = true;
				break;
			}
            //查看离最近的点是否为特殊点，判断位置是否在直线上，判断方向
            vt.erase(std::remove_if(vt.begin(),vt.end(),[&](const std::string &card_no){
				return findNearLight(card_no,pt);
			}),vt.end());
            if(vt.empty()) {
				flag = true;
				break;
			}
            //特殊规则保留id且优先级2，避让规则置空id，优先级2
            if(pt->noCar())
            {
                if(pt->load()== PRI_AVOIDANCE)
                  break;
            }
            else
            {
                if(pt->load()== PRI_AVOIDANCE)
                {
                    auto it = std::find(vt.begin(),vt.end(),pt->getId());
                    if(it != vt.end())
                        break;
                }
            }
            //排序，找到离之最近的大车
            std::sort(vt.begin(),vt.end(),[&](const std::string &card_no,const std::string & card){return sortCar(card_no,card,pt);});
            changeCarState(vt[0],pt);
            pt->setCrossing(vt[0],PRI_AVOIDANCE);
            //here return directly
            return true;
        }while(false);
        if(flag)
        {
            //找不到之前的车辆。则初始化
            if(pt->load() == PRI_AVOIDANCE && !pt->noCar())
                pt->resetGroup();
            else
                flag = false;
        }
        return flag;
    }
    bool  CrossingRule::handleCrossing(std::shared_ptr<TrafficLightGroup> pt) 
    {    
        bool flag = false;
		int exit_flag = -1;
        //查找一定范围的车辆
        do{
            auto vt = find_near(static_cast<int>(pt->x),static_cast<int>(pt->y),pt->scope(),itostr(pt->m_gId));
			if(!vt.empty())
			{
				//std::cout <<""<<std::endl;
				debug_print_syslog(0,"lamp light: CrossingRule::handleCrossing gid: %d,scope: %.3f",pt->m_gId,pt->m_scope);
			}
			vt.erase(std::remove_if(vt.begin(),vt.end(),[&](const std::string &name)->bool{
				bool a = false;
				debug_print_syslog(0,"lamp light: CrossingRule::handleCrossing card_id: %s, gid: %d,line: %d",name.c_str(),pt->m_gId,__LINE__);
				if(0 == name.compare(0,3,"002"))
					a =  findNearLight(name,pt,true);
				else
					a = true;
				return a;
				}),vt.end());
            //如果为空表示没找到车
            if(vt.empty()) 
            {
                if(!pt->noCar()&& pt->load() == PRI_CROSSING)   
                {
                    //释放红绿灯组
                    pt->resetGroup(); 
                    flag = true;
                }
				exit_flag = 1;
                break;
            }
            //是否已经有车记录
            if(!pt->noCar())
            {
                auto it = std::find(vt.begin(),vt.end(),pt->getId());
                if(it!=vt.end()){
					exit_flag = 2;
					break;
				}
            }
            std::sort(vt.begin(),vt.end(),[&](const std::string &card_no,const std::string & card){
				return sortCar(card_no,card,pt);
			});
			//设置优先级以及卡id
            pt->setCrossing(vt[0],PRI_CROSSING);
            //修改红绿灯组状态
            changeCarState(vt[0],pt);
            flag = true;
        }while(false);
		if (exit_flag!=-1)
		{
			debug_print_syslog(0,"lamp light: gid: %d, exit_flag : %d",pt->m_gId,exit_flag);
		}
        return flag;
    }
    std::vector<std::shared_ptr<TrafficLightGroup>> CrossingRule::handleRule(std::shared_ptr<card_interface> c/*=nullptr*/)
    {
		debug_print_syslog(0,"lamp light:CrossingRule::handleRule size: %d, line: %d",m_CTrafficLightGroupMap.size(),__LINE__);
        std::vector<std::shared_ptr<TrafficLightGroup>> plist;
        for(auto &pt : m_CTrafficLightGroupMap) 
        {
            pt->getTurn();//获取控制权
            int priority = pt->load();
			//debug_print_syslog(0,"lamp light: priority: %d",priority);
			
            bool flag = false;
            do{
                if(priority == PRI_MANUALCONTROL)
                {
                    flag =  handleManualControl(pt) ;
                    if(flag)
                      break;
                }
                if(pt->load() <= PRI_AVOIDANCE && pt->getflag())
                {
                    flag =  handleSpecialAvondance(pt);
                    if(flag)
                      break;
                }
                if(pt->load()<=PRI_CROSSING)
                {
                    flag = handleCrossing(pt);
                    if(flag)
					{
						break;
					}
                }
            }while(false);
            pt->releaseTurn();//释放控制权。
            if(flag)
            {
                plist.push_back(pt);
            }
        }
        //for(const auto & g : plist) 
        //  std::cout <<"CCCCCCCCCCCCCCCCCCCCCCCC"<< g->showinfo()<<std::endl;
		return std::move(plist);
    }
 
    void CrossingRule::changeCarState(std::string & card_no,std::shared_ptr<TrafficLightGroup> tlg_ptr)
    {
        int lightId = 0;
        auto cardPtr = getCard(card_no);
        if(cardPtr!=nullptr)
        {
			double x=cardPtr->getX();
			double y=cardPtr->getY();
			auto lightinfo = findNearLightInfo(x,y,tlg_ptr);

			std::time_t t = time(NULL);
			if(t-cardPtr->get_last_recv_time()>= 10)
			{
				return;
			}

			lightId = std::get<3>(lightinfo);
			if(tlg_ptr->m_sCardId.empty())
			{
				tlg_ptr->m_sCardId = card_no;
			}
			debug_print_syslog(0,"lamp light: changeCarState card: %s, light id: %d",card_no.c_str(),lightId);
			tlg_ptr->setLight(lightId,GREEN,RED);

            /*double x=cardPtr->getX();
            double y=cardPtr->getY();
            auto lightinfo = findNearLightInfo(x,y,tlg_ptr);

            lightId = std::get<3>(lightinfo);
			debug_print_syslog(0,"red green: changeCarState card: %s, light id: %d",card_no.c_str(),lightId);
            tlg_ptr->setLight(lightId,GREEN,RED);*/
        }
    }
    std::vector<std::shared_ptr<TrafficLightGroup>> AvoidanceRule::handleRule(std::shared_ptr<card_interface> c/*=nullptr*/)
    {
		debug_print_syslog(0,"lamp light: AvoidanceRule::handleRule size: %d, line: %d",m_CTrafficLightGroupMap.size(),__LINE__);
        int aid = c->getAreaId();
        double x = c->getX();
        double y = c->getY();
        double v = c->getv();
        auto it = m_CTrafficLightGroupMap.find(aid);
        std::vector<std::shared_ptr<TrafficLightGroup>> vt;
        if(it != m_CTrafficLightGroupMap.end())
        {
			//根据车卡的定位坐标以及定位区域信息获取红绿灯组
            auto a = findGroup(x,y,aid,v,c);
			debug_print_syslog(0, "lamp light: card: %s, x:%.4f, y:%.4f, led group size: %d, c size: %d",c->cardId().c_str(),x,y,a.size(),c->size());

            if(a.size()==0)  // 所在区域没有可以控制的红绿灯组
            {
                if(c->size()==1) // 当前车辆控制的路口为1个，释放该路口控制
				{
					debug_print_syslog(0, "red green: a is 0, c is 1");
                    eraseTrafficLightGroup(true,c,vt);
				}
                else if(c->size()==2) // 当前车辆控制的路口为两个，释放这两个路口的控制
				{
					debug_print_syslog(0, "red green: a is 0, c is 2");
                    eraseTwoGroup(c,vt);
				}
            }
            else if(a.size()==1) // 所在区域存在一个红绿灯组
            {
                if(c->size()==0) // 当前卡没有绑定红绿灯组，绑定此灯组，并设置灯组颜色为绿色？
				{
					debug_print_syslog(0, "red green: a is 1, c is 0");
                    add_one(a[0],c,vt,GREEN,RED);
				}
                else if(c->size()==1) // 当前卡绑定了一个红绿灯组
                {
                    if(c->front() != a[0]) // 当前卡绑定的红绿灯组与a不是同一个，不是，释放之前的绑定，再绑定灯组a
                    {
						debug_print_syslog(0, "red green: a is 1, c is 1 c != a");
                        eraseTrafficLightGroup(true,c,vt);
                        add_one(a[0],c,vt,GREEN,RED);
                    }
					else                  // 当前卡绑定的红绿灯组与a是同一个
					{
						debug_print_syslog(0, "red green: a is 1, c is 1 c == a");
						auto i = findNearLightInfo(x,y,a[0]);
						int ld = std::get<3>(i);
						debug_print_syslog(0, "red green: a is 1, c is 1 c == a old id: %d, new id: %d",a[0]->m_nLightId, ld);
						if(ld != a[0]->m_nLightId) // 灯组中离车最近的灯ID已经不是原来绑定路口的灯ID了，说明正在经过路口，需释放控制
						{
							eraseTrafficLightGroup(true,c,vt);
						}
						//if (Diff(a[0],point(x,y),GREEN,RED))
							//vt.push_back(a[0]);
						//else   // 车辆位置超过路口灯组位置，释放灯组
							//eraseTrafficLightGroup(true,c,vt);

					}
                }
                else if(c->size()==2) // 当前卡绑定了两个红绿灯组
                {
                    auto pt = a[0];
                    if(c->front() == pt)
                    {
                        eraseTrafficLightGroup(false,c,vt);
                        if(pt->load()<=2)
                        {
                            changeGroup(pt,point(x,y),GREEN,RED);
                            vt.push_back(pt);
                        }
                         
                    }
                    else if(c->back() == pt)
                    {
                        eraseTrafficLightGroup(true,c,vt);
                        if(pt->load()<=2)
                        {
                            changeGroup(pt,point(x,y),GREEN,RED);
                            vt.push_back(pt);
                        }
                    }
                    else
                    {
                        eraseTwoGroup(c,vt);
                        add_one(pt,c,vt,GREEN,RED);
                    }
                }
            }
            else if(a.size()==2) // 所在区域存在两个红绿灯组，巷道相遇逻辑
            {
                bool b = getMidCar(c,a[0],a[1]); // 判断两个灯组中是否有车
                LIGHT_COLOUR la,lb;
                if(b)
                {
                    la = RED;
                    lb = SPARK;
                }
                else
                {
                    la = GREEN;
                    lb = RED;
                }

                if(c->size()==0)
                {
                   add_one(a[0],c,vt,la,lb);
                   add_one(a[1],c,vt,GREEN,SPARK);
                }
                else if(c->size()==1)
                {
                    if(c->front() == a[0]) 
                    {
                        if(a[0]->load()<=2)
                        {
                            changeGroup(a[0],point(x,y),la,lb);
                            vt.push_back(a[0]);
                        }
                        add_one(a[1],c,vt,GREEN,SPARK);
                    }
                    else if(c->front() == a[1])
                    {
                        if(a[1]->load()<=2)
                        {
                            changeGroup(a[1],point(x,y),GREEN,SPARK);
                            vt.push_back(a[1]);
                        }
                        c->push_front(a[0]);
                        if(a[0]->load()<2)
                        {
                            changeGroup(a[0],point(x,y),la,lb);
                            vt.push_back(a[0]);
                        }
                    }
                    else
                    {
                        eraseTrafficLightGroup(false,c,vt);
                        add_one(a[0],c,vt,la,lb);
                        add_one(a[1],c,vt,GREEN,SPARK);
                    }
                }
                else if(c->size()==2)
                {
                    bool b1 = c->find(a[0]);
                    bool b2 = c->find(a[1]);
                    if(b1)
                    {
                        if(b2)
                        {
							if(c->front() == a[0])
							{
								if (Diff(a[0],point(x,y),la,lb))
									vt.push_back(a[0]);
								if (Diff(a[1],point(x,y),GREEN,SPARK))
									vt.push_back(a[1]);
							}
							else
                            {
                                c->clear();
                                if(a[0]->load()<=2)
                                {
                                    changeGroup(a[0],point(x,y),la,lb);
                                    vt.push_back(a[0]);
                                }
                                if(a[1]->load()<=2)
                                {
                                    changeGroup(a[1],point(x,y),GREEN,SPARK);
                                    vt.push_back(a[1]);
                                }
                                c->push_back(a[0]);
                                c->push_back(a[1]);
                            }
                        }
                        else
                        {
                            if(c->front() == a[0])
                            {
                                eraseTrafficLightGroup(false,c,vt);
                                add_one(a[1],c,vt,GREEN,SPARK);
                            }
                            else
                            {
                                eraseTrafficLightGroup(true,c,vt);

                                if(a[0]->load()<=2)
                                {
                                    changeGroup(a[0],point(x,y),la,lb);
                                    vt.push_back(a[0]);
                                }
                                add_one(a[1],c,vt,GREEN,SPARK);
                            }
                        }
                    }
                    else
                    {
                        if(b2)
                        {
                            if(c->front() == a[1])
                            {
                                eraseTrafficLightGroup(false,c,vt);
                            }
                            else
                            {
                                eraseTrafficLightGroup(true,c,vt);
                            }
                            if(a[1]->load()<=2)
                            {
                                changeGroup(a[1],point(x,y),GREEN,SPARK);
                                vt.push_back(a[1]);
                            }
                            c->push_front(a[0]);
                            if(a[0]->load()<2)
                            {
                                changeGroup(a[0],point(x,y),la,lb);
                                vt.push_back(a[0]);
                            }
                        }
                        else
                        {
                            eraseTwoGroup(c,vt);
                            add_one(a[0],c,vt,la,lb);
                            add_one(a[1],c,vt,GREEN,SPARK);
                        }
                    }
                }
            }
        }

		return std::move(vt);
    }

	std::shared_ptr<RunRedLight> AvoidanceRule::check_run_red_light(std::shared_ptr<card_interface> c,double reddis)
	{
		int aid = c->getAreaId();
		double x = c->getX();
		double y = c->getY();
		double vv = c->getv();
		bool bealarm = false;
		int LightId =0;
		int32_t groupid=0;
		auto it = m_CTrafficLightGroupMap.find(aid);
		if(it != m_CTrafficLightGroupMap.end())
		{
			double d= 0;
			double dis = 0;
			auto v = it->second;
			for(std::size_t i = 0;i<v.size();i++)
			{
				//如果此灯组被控制了，而且不是自己控制
				if ((v[i]->get_status()&&v[i]->m_sCardId != c->cardId())||(!v[i]->noCar()&&v[i]->getId() != c->cardId()))
				{
					LightId =-1;
					if (v[i]->get_status())
					{
						LightId = v[i]->m_nLightId;
					}
					else
					{
						for(auto & px :v[i]->m_pTrafficLight)
						{
							if (px->getState()==GREEN)
							{
								LightId = px->getid();
							}
						}
					}
					if (LightId ==-1)
					{
						continue;
					}
					d = v[i]->dist_direct(x,y); // 求灯组到车辆的距离（车辆在灯组的左下方，则距离值取反）
					std::shared_ptr<TrafficLight> & light = v[i]->get (LightId);
					if (light)
					{
						if(d<=0) // 车辆在灯组左边或下边
						{
							if(vv>0) // 速度方向与前进方向一致
							{ 
								double dis = light->dist(x,y);
								if (dis < reddis)
								{
									bealarm = true;
									groupid = v[i]->m_gId;
									break;
								}
							}
						}
						else
						{
							if(vv<0)
							{
								dis = light->dist(x,y);
								if (dis < reddis)
								{
									groupid = v[i]->m_gId;
									bealarm = true;
									break;
								}
							}
						}
					}		
				}			
			}
			if (bealarm)
			{
				//闯红灯
				std::shared_ptr<RunRedLight> redalarm = std::make_shared<RunRedLight>();
				redalarm->m_group_id = groupid;
				redalarm->m_light_id = LightId;
				//车辆id，
				redalarm->m_vehicle_id=0;
				//区域id，
				redalarm->m_area_id=aid;
				//闯红灯时间
				time_t tCurTime;
				tCurTime = time(NULL);
				redalarm->m_time=CFunctions::time_t2string(tCurTime);
				return redalarm;
			}
		}
		return nullptr;
	}

	// 给车卡c绑定路口灯组控制,并更改灯组中灯的颜色状态
    void AvoidanceRule::add_one(std::shared_ptr<TrafficLightGroup> & pt,std::shared_ptr<card_interface> &c,std::vector<std::shared_ptr<TrafficLightGroup>> &vt,LIGHT_COLOUR lc,LIGHT_COLOUR lc_)
    {
        double x = c->getX();
        double y = c->getY();
        c->push_back(pt);
        if(pt->load()<2) // 红绿灯组优先级取初始值0或路口控制1 
        {
            changeGroup(pt,point(x,y),lc,lc_);
            vt.push_back(pt);
        }
		auto i = findNearLightInfo(x,y,pt);
		int ld = std::get<3>(i);
		pt->m_nLightId = ld;
		pt->m_sCardId = c->cardId(); // 绑定此灯组的卡号
		debug_print_syslog(0, "lamp light: add_one light group : %d, light id: %d card: %s", pt->m_gId, pt->m_nLightId, pt->m_sCardId.c_str());
		pt->set_status(true);
    }
	 bool AvoidanceRule::Diff(std::shared_ptr<TrafficLightGroup> &p,point pt,LIGHT_COLOUR lc,LIGHT_COLOUR lc_)
	{
		bool b =false;
		auto i = findNearLightInfo(pt.x,pt.y,p);
		int ld = std::get<3>(i);
		//if(ld != p->m_nLightId) // 灯组中离车最近的灯ID已经不是原来绑定路口的灯ID了，说明正在经过路口，需释放控制
		//{
			//return false;
		//}
		if (p->Diff(ld,lc,lc_))
			b = true;
		return b;
	}
	 // 改变点p定位附近灯组的状态，满足条件的灯状态改为lc，否在为lc_
    void AvoidanceRule::changeGroup(std::shared_ptr<TrafficLightGroup> &p,point pt,LIGHT_COLOUR lc,LIGHT_COLOUR lc_)
    {
		// 1. 在p中取离pt最近的灯组信息
        auto i = findNearLightInfo(pt.x,pt.y,p);
        // 2. 获取灯组中临近车辆的灯ID
		int ld = std::get<3>(i);
		p->m_nLightId = ld;
        p->getTurn();
		// 3. 设置灯组中ld号灯的颜色为lc，其他为lc_
		debug_print_syslog(0,"lamp light: changeGroup light id: %d, set lc: %d, lc_: %d",ld,lc, lc_);
        p->setAvoidance(ld,lc,lc_);
        p->releaseTurn();
    }
    void AvoidanceRule::eraseTwoGroup(std::shared_ptr<card_interface> &c,std::vector<std::shared_ptr<TrafficLightGroup>> &vt)
    {
        eraseTrafficLightGroup(true,c,vt);
        eraseTrafficLightGroup(false,c,vt);
    }
    void  AvoidanceRule::eraseTrafficLightGroup(bool a,std::shared_ptr<card_interface> &c,std::vector<std::shared_ptr<TrafficLightGroup>> &vt)
    {
        std::shared_ptr<TrafficLightGroup> p = nullptr;
        bool flag = false;
        if(a)
        {
            p = c->front();
            c->pop_front();
        }
        else
        {
            p = c->back();
            c->pop_back();
        }
        p->getTurn();
        if(p->load()<=2)
        {
		   debug_print_syslog(0, "lamp light: eraseTrafficLightGroup group id: %d, card: %s",p->m_gId, c->cardId().c_str());
           p->resetGroup();// 重置灯组
           flag = true;
        }
        p->releaseTurn();
		p->set_status(false);
        if(flag)
          vt.push_back(p);
    }

	// 根据车辆与灯组的位置关系，查找灯组
    std::vector<std::shared_ptr<TrafficLightGroup>> AvoidanceRule::findGroup(double x,double y,int areaid,double vv, std::shared_ptr<card_interface> c)
    {
		debug_print_syslog(0,"lamp light: AvoidanceRule::findGroup: x: %.3f,y: %.3f,areaid: %d,vv: %.3f,line: %d",x,y,areaid,vv,__LINE__);
        std::vector<std::shared_ptr<TrafficLightGroup>>  arr;
        auto v = m_CTrafficLightGroupMap[areaid]; // 获取车辆所在地图区域内的所有红绿灯组
        double d= 0;
        for(std::size_t i = 0;i<v.size();i++)
        {
			//如果此灯组被控制了，则不允许使用
			if (v[i]->get_status()&&v[i]->m_sCardId != c->cardId())
			{
				debug_print_syslog(0, "red green: has control this light group");
				continue;
			}

            d = v[i]->dist_direct(x,y); // 求灯组到车辆的距离（车辆在灯组的左下方，则距离值取反）
            if(d<=0) // 车辆在灯组左边或下边
            {
                if(vv>0) // 速度方向与前进方向一致
                { 
                   if(v[i]->dist(x,y)<=ConfStruct::getCs().max_scope_bigCar ) // 车辆到灯组的距离小于100
                      arr.push_back( v[i]);
                   if(i+1<v.size())            //紧挨着的一个灯组到车辆距离小于100
                     if(v[i+1]->dist(x,y)<=ConfStruct::getCs().max_scope_bigCar )
                        arr.push_back(v[i+1]);
                }
                else if(vv<0) // 速度方向与前进方向不一致，判断运动方向的灯组是否在可控范围
                {
                    if(i>=1 && v[i-1]->dist(x,y)<=ConfStruct::getCs().max_scope_bigCar) 
                         arr.push_back(v[i-1]);
                    if(i>=2&& v[i-2]->dist(x,y)<=ConfStruct::getCs().max_scope_bigCar )
                      arr.push_back(v[i-2]);
                }
                break;
            }
        }
        if(d>0) // 车辆在灯组右边或上边
        {
            if(vv<0)
            {
                std::size_t i = v.size();
                if(i>=1 && v[i-1]->dist(x,y)<=ConfStruct::getCs().max_scope_bigCar )
                    arr.push_back(v[i-1]);
                if(i>=2&&v[i-2]->dist(x,y)<=ConfStruct::getCs().max_scope_bigCar )
                  arr.push_back(v[i-2]);
            }
        }
        return std::move(arr);
    }
    bool AvoidanceRule::getMidCar(std::shared_ptr<card_interface> bigcar,std::shared_ptr<TrafficLightGroup> & one,std::shared_ptr<TrafficLightGroup> & two)
    {
        //get dist between tow groups
        double dist = one->dist(*two);
        //use geo_hash get intersection vector..
        auto vt_one = find_near(static_cast<int>(one->x),static_cast<int>(one->y),static_cast<int>(dist),itostr(one->m_gId));
        auto vt_two = find_near(static_cast<int>(two->x),static_cast<int>(two->y),static_cast<int>(dist),itostr(two->m_gId));
		vt_one.erase(std::remove_if(vt_one.begin(),vt_one.end(),[](const std::string &name){
			if(0 == name.compare(0,3,"002"))
				return false;
			else
				return true;}),vt_one.end());
		vt_two.erase(std::remove_if(vt_two.begin(),vt_two.end(),[](const std::string &name){
				if(0 == name.compare(0,3,"002"))
					return false;
				else
					return true;}),vt_two.end());
		std::sort(vt_one.begin(),vt_one.end());
        std::sort(vt_two.begin(),vt_two.end());
        std::vector<std::string> vt_intersection;
        do{
            std::set_intersection(vt_one.begin(),vt_one.end(),
                        vt_two.begin(),vt_two.end(),std::back_inserter(vt_intersection));
            if(vt_intersection.empty()) break;
           //筛选出符合条件的车辆 
            vt_intersection.erase(std::remove_if(vt_intersection.begin(),vt_intersection.end(),[&](const std::string &cardno)->bool{
                            auto cardPtr = getCard(cardno);
                            if(cardPtr == nullptr)
                                return true;
                            return findTwoLightGroupMidCar(one,two,bigcar,cardPtr);
                        }),vt_intersection.end());
            //离第一个红绿灯距离进行排序
        }while(false);
        if(vt_intersection.empty()) 
          return false;
        else 
          return true;
    }

//后续小车避让大车规则，只需要找到前方的两个红绿灯，找到两个对比一下之前的记录，如果灯组改变了，则修改，如果没变，则判断是否有小车，进行改变
//如果只找到一个红绿灯，查看是否含有记录，如果有，则不坐任何处理。
    bool  AvoidanceRule::findTwoLightGroupMidCar(std::shared_ptr<TrafficLightGroup> one,std::shared_ptr<TrafficLightGroup>two,std::shared_ptr<card_interface> bigcar,std::shared_ptr<card_interface> car) 
    {   
        bool flag = true;
        double x = car->getX();
        double y = car->getY();
        double v = car->getv();

        double vv = bigcar->getv();
        double d = one->dist(x,y);
        line l(*one,*two);
        if(l.contain(x,y,1))
        {
            if(v*vv<0 && d > ConfStruct::getCs().mid_car_length_goroup)
              flag = false;
        }
        
        return flag;
    }
	std::vector<std::string> DecoratorRule::find_near(double x,double y,double dist,std::string cardid)
	{
		auto v = m_->find_near(x,y,dist,cardid);
		return std::move(v);
	}
	std::shared_ptr<card_interface> DecoratorRule::getCard(const std::string &card_no)
	{
		return m_->getCard(card_no);
	}

	void DecoratorRuleCrossing::run()
	{
		while(!b_exit)
		{
			//do handle crossingRule.
			auto vt = m_rule->handleRule();
			m_->getSendLightInfo(std::move(vt),"Crossing");
			std::this_thread::sleep_for(std::chrono::seconds(2));
		}
	}
	void DecoratorRuleAvoidance::handleRule(std::shared_ptr<card_interface> c)
	{
		static bool f = false;
		if(!f){std::dynamic_pointer_cast<AvoidanceRule>(m_rule)->sort();f=true;}
		if(c==nullptr) 
			return ;
		if(!c->getBigCarFlag())
			return;
		auto vt = m_rule->handleRule(c);
		if(vt.size()>0){
			debug_print_syslog(0,"lamp light: gid: %d, size: %d",vt[0]->getId(),vt.size());
		}
		
		m_->getSendLightInfo(std::move(vt),"Avoidance");
	}
	std::shared_ptr<RunRedLight>  DecoratorRuleAvoidance::check_run_red_light(std::shared_ptr<card_interface> c,double dis)
	{
		static bool f = false;
		if(!f){std::dynamic_pointer_cast<AvoidanceRule>(m_rule)->sort();f=true;}
		if(c==nullptr) 
			return nullptr;
		return m_rule->check_run_red_light(c, dis);
	}
NAMESPACE_POINT_END(NAMESPACE_POINT)
#if 0
//4,'name','label','state',4727, 275, 0, 10, 200, '2018-04-18 11:18:12',90
void InitData::read_dat_group(const char * fname)
{
    FILE *fp = fopen(fname,"r");
	char buf[512];
	int32_t t,id,areaid,mapid,ctime;
    double x,y,scope;
	char* s[20];
	while(fgets(buf,sizeof(buf),fp))
	{
		t=str_split(buf,&s[0]);
		if(t<8)
			continue;
        id = atoi(s[0]);
        x = atof(s[4]);
        y = -atof(s[5]);
        scope = atof(s[7]);
        mapid = atoi(s[10]);
        areaid = atoi(s[11]);
        ctime = atoi(s[12]);
        std::cout << s[0] <<' '<< s[4] <<' ' << s[5] <<' '<<s[7] <<std::endl;
        //crossing
        std::shared_ptr<TrafficLightGroup> tmp = nullptr;
        //if(scope != 0)
        //{
            tmp = std::make_shared<TrafficLightGroup>(x,y,0.0,id,mapid,areaid,scope,ctime);
           // m_CrossingMap.push_back(tmp);
        //}
       // else
        //    tmp = std::make_shared<AvoidanceTrafficLightGroup>(x,y,0,id,mapid,areaid);
        m_Map.insert({id,tmp});
    }
}

//3, '192.168.1.101', 4727, 100,0,'name','label',100,'state',1,60,'2017/08/10 11:11:11',4,0
void InitData::read_dat_light(const char * fname)
{
    FILE *fp = fopen(fname,"r");
	char buf[512];
	int t,id,readerid,gid,specialFlag;
    double x,y;
    std::string ip;
	char* s[20];
	while(fgets(buf,sizeof(buf),fp))
	{
		t=str_split(buf,&s[0]);
		if(t<8)
			continue;
        id = atoi(s[0]);
        ip = s[1];
        x = atof(s[2]);
        y= -atof(s[3]);
        //mapid = atoi(s[7]);
        readerid = atoi(s[7]);
        //areaid = atoi(s[9]);
        //controltime = atoi(s[10]);
        gid = atoi(s[12]);
        specialFlag = atoi(s[13]);//0正常1表示特殊红绿灯

        std::cout << id  <<' '<< ip  <<' ' << x  <<' '<< y <<' ' << readerid <<' ' <<gid <<std::endl;
        std::shared_ptr<TrafficLight> tmp = std::make_shared<TrafficLight>(id,x,y,0,readerid,ip,specialFlag);
        m_Map[gid]->put(tmp);
    }
}
int main()
{
    std::shared_ptr<Manage> m = std::make_shared<Manage>();
    //test point func
    TrafficLightGroup *ctlg = new TrafficLightGroup(1,1,0,12,12.5,5,10,90) ; 
    std::cout <<"direct:"<< ctlg->dist_direct(3,1)<<std::endl;;
    delete ctlg;    

    //test self-thread.
    //std::shared_ptr<Rule>  r =std::make_shared<CrossingRule> ();
    //DecoratorRule* drc = new DecoratorRuleCrossing(&m);
    //drc->CreateRule();
    //drc->handleRule();
    //delete drc;
    //test line 
    line l(point(100,10),point(120,10));
    std::cout << l.dist(point(130,10))<<std::endl;
    line ll(point(4727,75),point(4727,88));
    std::cout << ll.a<<' '<<ll.b<<' '<<ll.c<<"=illlll"<<std::endl; 
    auto abc = point(4727,75).get_abc(point(4727,88));
    std::cout << std::get<0>(abc)<<' '<<std::get<1>(abc)<<' '<<std::get<2>(abc)<<std::endl;
    line lll(point(4729,75),point(4729,88));
    std::cout << lll.a<<' '<<lll.b<<' '<<lll.c<<"=iixxlllll"<<std::endl; 

    //read config
    InitData Id;
    Id.read_dat_group("dat_group.txt");
    Id.read_dat_light("dat_light.txt");
    Id.showInfo();
    //get map for trafficLightGroups includes TrafficLights
    //std::shared_ptr<Rule> CrossingR = std::make_shared<CrossingRule>(m);
    //CrossingR->put(std::move(Id.m_CrossingMap));
    //std::shared_ptr<Rule> AvoidanceR = std::make_shared<AvoidanceRule>(m);

    uint64_t tt = time(NULL)*1000;
    std::shared_ptr<card_interface> bc24 = std::make_shared<c>("2:4",tt,4470,10,true,-1.1,1);//cardid timet x y bigcar speed,areaid
    std::shared_ptr<card_interface> bc22 = std::make_shared<c>("2:2",tt,4650,75,false,1.1,2);//cardid timet x y bigcar speed.areaid

    m->m_map.insert({"2:4",bc24});
    m->m_map.insert({"2:2",bc22});
//  CrossingR->update(4470,-75,"2:4");
//  这里需要把y转化成-
    m->m_g.update(4650,-75,"2:2");
    //auto av = AvoidanceR->find_near(4600,-75,100,"2:4");
    std::shared_ptr<DecoratorRule> DecCrossingRule = std::make_shared<DecoratorRuleCrossing>(m);
    std::shared_ptr<DecoratorRule> DecAvoidanceRule = std::make_shared<DecoratorRuleAvoidance>(m);

    DecCrossingRule->CreateRule();
    DecAvoidanceRule->CreateRule();

    for(const auto px : Id.m_Map)
    {
        if(px.second->m_gId == 1)
        {
           px.second->store(3);
           px.second->m_controlTime = time(NULL)+30;
        }
        if(px.second->m_gId == 1)
           bc24->push_back(px.second);
        if(px.second->m_gId == 2)
        {
            //px.second->store(2);
           bc24->push_back(px.second);
        }
//        if(px.second->m_gId == 6)
//        {
//          bc24->push_back(px.second);
//          px.second->store(3);
//        }
        DecAvoidanceRule->put(px.second);
        DecCrossingRule->put(px.second);
    }
    //DectoratorRule  do next..
//    DecCrossingRule->handleRule();
    //DecAvoidanceRule->handleRule();

#if 0 
    //test the  sort for TrafficLightGroup............
    std::shared_ptr<TrafficLightGroup> p = std::make_shared<CrossingTrafficLightGroup>(1,2,0,12,12.5,5,10);
    std::shared_ptr<TrafficLightGroup> p1 = std::make_shared<AvoidanceTrafficLightGroup>(2,2,0,12,5,10);
    std::shared_ptr<TrafficLightGroup> p2 = std::make_shared<CrossingTrafficLightGroup>(3,2,0,12,12.5,5,10);
    std::vector<std::shared_ptr<TrafficLightGroup>> vp;
    vp.push_back(p);
    vp.push_back(p2);
    vp.push_back(p1);
    for(auto px:vp)
      std::cout << px->x<<"beforefdfdfd" << px->y<<std::endl;
    std::sort(vp.begin(),vp.end(),[](std::shared_ptr<TrafficLightGroup> one,std::shared_ptr<TrafficLightGroup> two)->bool{std::cout << two->x<<std::endl;return *one<*two;});
    for(auto px:vp)
      std::cout << px->x<<"fdfdfd" << px->y<<std::endl;
#endif  
    //DecAvoidanceRule->handleRule(bc24);
    std::dynamic_pointer_cast<c>(bc24)->x_ = 4727;     
    m->m_g.update(4727,-10,"2:4");
    //m->m_g.update(4604,-75,"2:4");
    std::cout <<"------------------------------------"<<std::endl;
   // DecAvoidanceRule->handleRule(bc24);
    DecCrossingRule->handleRule();

    std::cout<<"******************************************************************************888"<<std::endl;
    std::cout << DecCrossingRule->showInfo()<<std::endl;;
    std::cout<<"************************888"<<std::endl;
//    std::cout << DecAvoidanceRule->showInfo()<<std::endl;;
    std::cout<<"**********************************************888"<<std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(20));

    m->m_g.update(4604,-75,"2:4");

    while(1){std::this_thread::sleep_for(std::chrono::seconds(10));};
    return 0;
}
#endif