server-whatever/ant.cpp

#include <stdio.h>
#include <string.h>
#include <math.h>
#include "ant.h"
#include "db_api/CDBSingletonDefine.h"
#include "event.h"
#include "tool_time.h"
#include "area.h"
#include "card_path.h"

int site::index()const
{
	//return m_algo+(m_num_dims<<1);
    return (m_algo<<4) + m_num_dims;
}

site::site(int id)
	:m_algo(0)
	,m_num_dims(0)
	,m_id(id)
	,m_path_empty(true)
{
	m_time=time(0);
	for(uint32_t i=0;i<m_timeoff_count.size();i++)
		m_timeoff_count[i]=0;
}

const algo_config&site::config()const
{
	return g_config[index()];
}

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

	it=itm-1;
	for(int i=0;i<2 && it>=v_line.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("site-path: %s",p.to_str().c_str());
	}
}

void ant::set_path(const std::vector<line_v>&v_line_)
{
	std::vector<line_v> vl(v_line_);
	vl.reserve(vl.size()+2);
	//找到距离天线最近的端点
	auto min_it = vl.begin();
	double dist = 10;
	for(auto it = vl.begin(); it != vl.end(); it++)
	{
		double d = it->as_line().dist(*this);
		if(d < dist)
		{
			dist = d;
			min_it = it;
		}
	}

	if(min_it==vl.end())
	{
		log_error("分站路径距离分站太远site_id=%d",m_id);
		return;
	}

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

		if(min_it->v[0].z)//slope ..555
		{
			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(vl,min_it+1);
	}
}

void site::set_path(const std::vector<line_v>&v_line)
{
	if(v_line.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(v_line.begin()+1,v_line.end());
	std::vector<line_v> target(v_line.begin(),v_line.begin()+1);
#if 0
	target[0][0].z=target[0][1].z=slope[0];
	for(int i=0,c=vl.size();i<c;i++)
		vl[i][0].z=vl[i][1].z=slope[i+1];
#endif

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

	log_info("初始化分站路径:site_id=%d",m_id);

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

	for(int i = 0,c = m_ant.size(); i < c; i++)
	{
		if(m_ant[i].empty())
		{
			log_error("分站未设置天线坐标 site_id=%d,ant_index=%d", m_id, i);
			continue;
		}
		m_ant[i].set_path(target);
	}
	m_path_empty= !m_ant[0][0].valid() && !m_ant[0][1].valid() ;
}

double site::get_site_dist(const int& sid)
{
    auto it_site = sit_list::instance()->get(sid);

    if(it_site){
        return this->dist(*it_site);
    }else{
        return 0.0;
    }
}

std::vector<point> ant::getsol(const double &dist) const
{
	std::vector<point> v;
	for(const auto & p : m_path)
	{
		if(!p.valid())
			continue;

		point pt;
		double d  = dist;

        //logn_info(3, "[pdoa] dist=%.2f, line.length()=%.2f, %s", dist, p.m_line[0].length(), p.to_str().c_str());
		if(dist <= p.m_line[0].length())
		{
			d += d*p.m_line[0][0].z;
			pt = point(p.m_line[0][0].x + d*p.m_line[0].cos() , p.m_line[0][0].y + d*p.m_line[0].sin());
		}
		else if(p.m_line[1].length()>0)
		{
			d -= p.m_line[0].length()*(1-d*p.m_line[0][1].z);
			d += d*p.m_line[1][0].z;

			pt = point(p.m_line[1][0].x+d*p.m_line[1].cos(),p.m_line[1][0].y+d*p.m_line[1].sin());
		}
		else
		{
			continue;
		}
		v.push_back(pt);
	}
	return std::move(v);
}

bool visit_site_status::visit(std::shared_ptr<site> s)
{
	time_t now = time(0);
	int diff = now-s->m_time;
	event_tool::instance()->handle_event(OT_DEVICE_READER, ET_READER_ERROR, s->m_id, READER_TIMEOUT, diff, diff>READER_TIMEOUT);
	return true;
}

void sit_list::read_ant_path(int id)
{
	std::string sql = "SELECT reader_id,tof_flag,b_x,b_y,b_z,e_x,e_y,e_z,spacing_ratio FROM dat_reader_path_tof_n";

	std::map<int,std::vector<line_v>> map_path;

	if(-1 == id)
	{
		sql.append(";");
	}
	else
	{
		sql.append(" where reader_id=");
		sql.append(std::to_string(id));
		sql.append(";");

		std_debug("修改path sql=%s", sql.c_str());
		log_info("修改path sql=%s", sql.c_str());
	}

	std::string Error;
	YADB::CDBResultSet DBRes;
	sDBConnPool.Query(sql.c_str(),DBRes,Error);
	int nCount = DBRes.GetRecordCount( Error );
	if (nCount < 1)
	{
		log_error("修改path失败，数据库中找不到(dat_reader_path_tof_n): 分站id=%d", id);
		return ;
	}

	log_info( "read_ant_path. The record count=%ld\n", nCount );
	while ( DBRes.GetNextRecod(Error) )
	{
		int reader_id  = 0;
		DBRes.GetField( "reader_id",reader_id, Error );

		auto site_ptr=sit_list::instance()->get(reader_id);
		if(nullptr==site_ptr)
		{
			log_error("[site] 定义了分站路径，但是没有定义分站:%d",reader_id);
			continue;
		}else if(site_ptr->m_num_dims != 1){
            log_info("[site] 多维定位分站，无需定义路径:%d", reader_id);
            continue;
        }

		int pid=0;
		DBRes.GetField( "tof_flag",pid, Error );

		double b_x= 0;
		DBRes.GetField( "b_x",b_x, Error );

		double b_y= 0;
		DBRes.GetField( "b_y",b_y, Error );

		double b_z= 0;
		DBRes.GetField( "b_z",b_z, Error );

		double e_x= 0;
		DBRes.GetField( "e_x",e_x, Error );

		double e_y= 0;
		DBRes.GetField( "e_y",e_y, Error );

		double e_z= 0;
		DBRes.GetField( "e_z",e_z, Error );

		double spacing_ratio = 0;
		DBRes.GetField( "spacing_ratio",spacing_ratio, Error );

		log_info("src-path:site=%d,x0=%.2f,y0=%.2f,x1=%.2f,y1=%.2f",reader_id,b_x,b_y,e_x,e_y);

		point p1(b_x, b_y, spacing_ratio);
		point p2(e_x, e_y, spacing_ratio);

		map_path.insert(std::make_pair(reader_id,std::vector<line_v>()));
		map_path.find(reader_id)->second.push_back(line_v(p1,p2));
#if 0

		auto &sit_ = *site_ptr;
		if(-1!=id)//清空path
		{
			sit_.clear_path();
			log_info( "修改path 清空path，分站id=%d\n", id );
		}

		if(pid == 0)
		{
			line_v l(p1,p2);
			{
				point px = l.line::projection(sit_);
				sit_.set(px);
				for(int i=0;i<2;i++)
				{
					path p;
					p.m_slope[0] = spacing_ratio;
					p.m_line[0] = line_v(px,l[i]);
					sit_.m_ant[i].m_path.push_back(p);
				}
			}
		}
		else
		{
			ant &a = pid<0?sit_.m_ant[0]:sit_.m_ant[1];
			if(a.m_path.size()!=0)
			{
				path &p = a.m_path[0];
				p.m_line[abs(pid)-1] = line_v(p1,p2);
				p.m_slope[abs(pid)-1] = spacing_ratio;
			}
			else
			{
				path p;
				p.m_line[abs(pid)-1] = line_v(p1,p2);
				p.m_slope[abs(pid)-1] = spacing_ratio;
				a.m_path.push_back(p);
			}
			if(abs(pid)==1)
				sit_.set(p1);
		}
#endif
	}

#if 0
	if(-1==id)
	{
		for(auto&_s:sit_list::instance()->m_map)
		{
			_s.second->deal_path();
		}
	}
	else
	{
		auto sit_ptr=sit_list::instance()->get(id);
		if(sit_ptr)
		{
			sit_ptr->deal_path();
			log_info( "修改path成功，分站id=%d\n", id );
		}
	}
#endif

	for(auto&vl:map_path)
	{
		auto sit_ptr=sit_list::instance()->get(vl.first);
		if(!sit_ptr)
		{
			log_error("[site] 定义了分站路径，但是没有定义分站:%d",vl.first);
			continue;
		}else if(sit_ptr->m_num_dims > 1){
            log_info("[site] 多维定位分站，无需定义路径:%d", vl.first);
            continue;
        }

		sit_ptr->set_path(vl.second);
        point p;
        if(!sit_ptr->m_path_empty){
            p=(*sit_ptr)[0][0][0].line::projection(*sit_ptr);
            sit_ptr->set(p);
        }
        if(p==*sit_ptr)
            log_info("[site_path]%s",sit_ptr->to_string().c_str());
        else
            log_info("[site_path_diff](%f,%f)--%f--%s",p.x,p.y,p.dist(*sit_ptr),sit_ptr->to_string().c_str());
	}

	card_path::init();
}

void sit_list::init_site(const std::string &ids /*=""*/)
{
     std::string sql = "SELECT reader_id, reader_type_id, dat_reader.map_id, \
             area_id, device_type_id, dimension, dat_map.scale,need_power_alarm\
			 ,x,y, pdoa_offset, pdoa_direction, isSpecial\
             FROM dat_reader, dat_map where \
             dat_reader.map_id=dat_map.map_id and state=0";

     if(ids.empty())
     {
         sql.append(";");
     }
     else
     {
         sql.append(" AND reader_id in (");
         sql+=ids;
         sql.append(");");

         std_debug("增加或修改分站 sql=%s", sql.c_str());
         log_info("增加或修改分站 sql=%s", sql.c_str());
     }

     std::string Error;
     YADB::CDBResultSet DBRes;
     sDBConnPool.Query(sql.c_str(),DBRes,Error);
     int nCount = DBRes.GetRecordCount( Error );
     if (nCount < 1)
     {
         log_error("增加或修改失败，数据库中找不到: 分站id=%s", ids.c_str());
         return ;
     }

     log_info( "init_site. The record count=%ld\n", nCount );

     while ( DBRes.GetNextRecod(Error) )
     {
         int reader_id  = 0;
         DBRes.GetField( "reader_id",reader_id, Error );

         auto site_ptr=sit_list::instance()->get(reader_id);
         if(nullptr==site_ptr)
         {
             site_ptr = std::make_shared<site>(reader_id);
             sit_list::instance()->add(reader_id,site_ptr);
         }

         int reader_type_id  = 0;
         DBRes.GetField( "reader_type_id",reader_type_id, Error );

         int map_id  = 0;
         DBRes.GetField( "map_id",map_id, Error );

         int area_id  = 0;
         DBRes.GetField( "area_id",area_id, Error );

         int device_type_id  = 0;
         DBRes.GetField( "device_type_id",device_type_id, Error );

         int dimension  = 0;
         DBRes.GetField( "dimension",dimension, Error );

         double scale= 0;
         DBRes.GetField( "scale",scale, Error );

         int power_alarm  = 0;
         DBRes.GetField( "need_power_alarm",power_alarm, Error );

         site_ptr->m_reader_type_id = reader_type_id;
         site_ptr->m_map_id = map_id;
         site_ptr->m_area_id = area_id;
         site_ptr->m_power_check_enable=power_alarm;
         site_ptr->m_device_type_id = device_type_id;
         site_ptr->m_num_dims = dimension;
         site_ptr->m_scale = scale;
         site_ptr->create_area();

         double x = 0, y = 0;
         DBRes.GetField("x", x, Error);
         DBRes.GetField("y", y, Error);
         site_ptr->x = x;
         site_ptr->y = y;

         double offset = 0.0;
         DBRes.GetField("pdoa_offset", offset, Error);
         site_ptr->m_pdoa_offset = offset;

         int direction = 0;
         if(DBRes.GetField("pdoa_direction", direction, Error)){
            site_ptr->m_pdoa_direction = direction;
         }

         int special = 0;
         if(DBRes.GetField("isSpecial", special, Error)){
            site_ptr->m_special = special;
         }

		 log_info ("site_position: site=%d, x=%.2lf, y=%.2lf, area_id=%d, m_scale=%.2f, pdoa_offset=%.2f, pdoa_direction=%d, special=%d", reader_id, x, y, area_id, scale, offset, direction, special);
     }
}

void sit_list::read_sit_list(int id)
{
    std::string sql = "SELECT antenna_id, a.reader_id, idx, a.x, a.y, a.z, a.angle \
            FROM dat_antenna a, dat_reader r \
            WHERE a.reader_id = r.reader_id";

    if(-1 == id)
    {
        sql.append(";");
    }
    else
    {
        sql.append(" AND antenna_id=");
        sql.append(std::to_string(id));
        sql.append(";");

        log_info("基础数据 增加或修改天线 sql=%s", sql.c_str());
    }

    std::string Error;
    YADB::CDBResultSet DBRes;
    sDBConnPool.Query(sql.c_str(),DBRes,Error);
    int nCount = DBRes.GetRecordCount( Error );
    if (nCount < 1)
    {
        log_error("基础数据 增加或修改失败，数据库中找不到: 天线id=%d", id);
        return ;
    }

    log_info( "init_antenna. The record count=%ld\n", nCount );
    while ( DBRes.GetNextRecod(Error) )
    {
        int antenna_id = 0;
        DBRes.GetField( "antenna_id",antenna_id, Error );

        int reader_id  = 0;
        DBRes.GetField( "reader_id",reader_id, Error );

        int idx=0;
        DBRes.GetField( "idx",idx, Error );

        int antid = idx-1;
        if(antid >= 2 || antid < 0)
          continue;

        double x= 0;
        DBRes.GetField( "x",x, Error );

        double y= 0;
        DBRes.GetField( "y",y, Error );

        double z= 0;
        DBRes.GetField( "z",z, Error );

        double _angle = 0;
        DBRes.GetField("angle", _angle, Error);

        log_info("ant-position:reader=%d, antid=%d, x=%.2lf, y=%.2lf, angle=%.2f", reader_id, antid, x, y, _angle);

        if(-1 == id)
        {
            std::shared_ptr<site> site_ptr=nullptr;
            site_ptr = sit_list::instance()->get(reader_id);
            if(!site_ptr)
              continue;
            site_ptr->m_ant[antid].m_id = antenna_id;
            site_ptr->m_ant[antid].set(x, y);
            site_ptr->m_ant[antid].m_angle = _angle;
            site_ptr->set_ex();
        }
        else
        {
            auto site_ptr = sit_list::instance()->get(reader_id);
            if(site_ptr)
            {
                site_ptr->m_ant[antid].set(x, y);
                site_ptr->m_ant[antid].m_angle = _angle;
                site_ptr->set_ex();
            }
            log_info("基础数据 增加或修改天线成功：天线id:%d,分站id:%d",id,reader_id);
        }
    }
}
void site::create_area()
{
    m_area=std::make_shared<area>(-m_id,0,0,m_scale,m_map_id,1<<7);
}
void site::clear_event()
{
    event_tool::instance()->handle_event(OT_DEVICE_READER,ET_READER_ERROR,id(),READER_TIMEOUT,0,false);
    event_tool::instance()->handle_event(OT_DEVICE_READER,ET_READER_POWER_BY_BATTERY,id(),0,0,false);
}

/*
   处理分站供电状态，交流供电时，ac_down=false，直流供电时,ac_down=true

   目前只有大分站实现了这个功能，并且井下安装时是否接入了该电信号也不确定
   ，所以需要有张表定义某个ID是否需要告警
 */
void site::on_power_status(bool ac_down)//电源状态
{
	if(!m_power_check_enable || ac_down == m_power_ac_down)
	{
        return;
    }
    m_power_ac_down=ac_down;
    event_tool::instance()->handle_event(OT_DEVICE_READER,ET_READER_POWER_BY_BATTERY,id(),tool_time::now_to_seconds(),ac_down,m_power_ac_down);
    log_info("[event warning: reader power supply by battery] reader_id: Power %d->%d.",id(),!m_power_ac_down,m_power_ac_down);
}

algo_config site::g_config[]=
{
	{ "tof-1",  1, 2, 0.1, 1 },
	{ "tdoa-1", 2, 2, 0.1, 1 },
	{ "tof-2",  2, 3, 0.1, 1 },
	{ "tdoa-2", 3, 3, 0.1, 1 },
	{ "tof-3",  3, 4, 0.1, 1 },
	{ "tdoa-3", 4, 4, 0.1, 1 }
};
/*
#ifdef _TEST
int main()
{
	log_init("./log.ini");
    //sit_list *sl = sit_list::instance();
	sit_list::instance()->load("data_reader_antenna.txt","path_tof.txt");
	sit_list::instance()->get(209)->solving(0,100);
	sit_list::instance()->get(209)->solving(1,100.5);
    //std_info("---%d",(*sl)[209].m_ant[0].m_path.size());
    //std_info("---%d",(*sl)[209][0].size());
    //std_info("---%s",(*sl)[209][0][0][0].to_string().c_str());
}
#endif
*/