server/ant.h

#ifndef _ANT_LIST_HPP_
#define _ANT_LIST_HPP_

#include <math.h>
#include <array>
#include <deque>
#include <tuple>
#include <memory>
#include <algorithm>
#include <sstream>
#include "log.h"
#include "line.h"
#include "point.h"
#include "write-copy.h"
#include"net-service.h"
#include"common.h"

struct path
{
    std::array<line_v,2>	m_line;
    std::array<double,2>    m_slope;
    path()
    {
        for(auto &d:m_slope)
            d=0;
    }
    std::string to_str() const
    {
        std::stringstream ss;
        for(int i=0;i<2;i++)
        {
            ss<<"line:" <<m_line[i].to_string()<<"slope:"<<m_slope[i]<< " cos:"<<m_line[i].cos()<<" sin:"<<m_line[i].sin()<<" | ";
        }
        return ss.str();
    }
    bool vaild() const
    {
        return !m_line[0].empty();
    }
    line_v & operator[](int i)
    {
        return m_line[i];
    }
    const line_v & operator[](int i) const
    {
        return m_line[i];
    }

};
//?
struct algo_config
{
    const char*desc;
    int min_msg_cnt;
    int best_msg_cnt;
    double min_wait_time;
    double max_wait_time;
};
//
struct ant :point
{
    std::vector<path> m_path;
    path & operator[](int i)
    {
        return m_path[i];
    }
    const path & operator[](int i) const
    {
        return m_path[i];
    }
    size_t size() const
    {
        return m_path.size();
    }
    std::vector<point> getsol(const double &dist) const
    {
        std::vector<point> v;
        for(const auto & p : m_path)
        {
            double d  = dist;
            if(p.vaild())
            {
                point pt;
                if(dist <= p.m_line[0].length() || (dist > p.m_line[0].length() && p.m_line[1].empty()))
                {
                    d += d*p.m_slope[0];
                    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
                {
                    d -= p.m_line[0].length();
                    d += d*p.m_slope[1];
                    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());
                }
                v.push_back(pt);
                //std_info("get_sol:x:%.2f,y:%.2f",pt.x,pt.y);
            }
            else
                std_error("%s","ant::getsol empty path..");
        }
        return std::move(v);
    }

};

struct site:point
{
    static algo_config g_config[];
    int      m_algo;			//TOF:0,TDOA:1
    int      m_num_dims;	    //1维:0,2维:1,3维:2
    double  m_scale = 2.0;  // 地图比例尺

    point    m_position;
    int index()const;
    const algo_config&config()const;
    int id()const
    {
        return m_id;
    }
    site(int id=-1);

    mutable double m_height=1.5;
    int m_id;
    bool m_path_empty;
    std::array<ant,2> m_ant;

    mutable double m_ant_dist=0;
    mutable double m_ant_dist_sum_new=0;
    mutable int m_ant_dist_cnt_new=0;

    ///分站位置 READER_TYPE_ID
    int m_reader_type_id = 0;
    int m_map_id = 0;
    int m_area_id = 0;

    /// 设备类型，分站、通信分站、交通灯等
    int m_device_type_id=0;

    /// 指定分站定位类型：一维定位，二维定位，三维定位
    int m_dimension=0;

    std::shared_ptr<client> m_clt=nullptr;
    void set_client(std::shared_ptr<client> clt)
    {
        m_clt = clt;
    }

    std::shared_ptr<client> get_client()
    {
        return m_clt;
    }

    bool is_up_site()
    {
        return READER_TYPE_ID_UP == m_reader_type_id;
    }

    point get_dstp(const point pt) const
    {
        point tmp;
        for(const auto & p : m_ant[0].m_path)
        {
            for(int i=0;i<2;i++)
            {
                if(!p[i].empty())
                {
                    if(p[i].contain(pt,0.01))
                    {
                        //if(i==0)
                        // return *this;
                        //else
                        tmp = p[i][0];
                    }
                }
            }
        }
        return tmp;
    }

    void count_ant_dist(double dist_tof1, double dist_tof2)const
    {
        if(dist_tof1<10 || dist_tof2<10)
            return;
        double dist = fabs(dist_tof1 - dist_tof2);
        if(dist>5)
            return;
        m_ant_dist_sum_new += dist;
        m_ant_dist_cnt_new++;

        if(m_ant_dist_cnt_new >= 2500)
        {
            m_ant_dist = m_ant_dist_sum_new / m_ant_dist_cnt_new;
            m_ant_dist_sum_new = 0;
            m_ant_dist_cnt_new = 0;
        }
    }
    void swap()
    {
        auto v0 =  m_ant[0].m_path;
        auto v1 =  m_ant[1].m_path;
        std::copy (std::begin(v0),std::end(v0),std::back_inserter(m_ant[1].m_path));
        std::copy (std::begin(v1),std::end(v1),std::back_inserter(m_ant[0].m_path));
    }
    double ant_dist()const
    {
        return m_ant[0].dist(m_ant[1]);
    }

    bool is_path_empty()const
    {
        return m_path_empty;
    }

    bool have_valid_path()const
    {
        return m_id != -1 && ant_dist() > 0.1;
    }

    std::string to_string()const
    {
        std::stringstream ss;
        ss<<"site_id:"<<m_id<<"x:"<<x<<" y: "<<y<<" scale:"<<m_scale;
        for(const auto a:m_ant)
        {
            ss<<"<";
            for(const auto p:a.m_path)
            {
                ss<<"{"<<p.to_str()<<"}";
            }
            ss<<">";
        }
        return ss.str();
    }

    const point&path(int i)const
    {
        static point p;
        if(i>=(int)m_ant[0].m_path.size())
            return p ;
        return m_ant[0].m_path[i].m_line[0][1];
    }

    std::vector<point> solving(int ant_id, double dist)const
    {
        const ant &a = m_ant[ant_id];
        if(dist<50 && dist>0)
        {
            if(dist<m_height)
            {
                m_height=dist;
                dist=0;
            }
            else
            {
                dist=sqrt(dist*dist-m_height*m_height);
            }
        }
        return std::move(a.getsol(dist));
    }
    ant operator[](int i)
    {
        return m_ant[i];
    }
    const ant operator[](int i) const
    {
        return m_ant[i];
    }
};


struct sit_list:single_base<sit_list,int,std::shared_ptr<site>>
{
    void load(const char*ant_file,const char*path_file)
    {
        read_sit_list(ant_file);
        read_ant_path(path_file);
    }

    void load_from_db()
    {
        load("data_reader_antenna.txt","path_tof.txt");
        init_site();
    }

    void read_sit_list(const char*fname);
    void read_ant_path(const char*fname);
    void init_site();
};

struct loc_message
{
    site     m_sit;
    uint64_t m_num_ticks; //tof时间片m_tof或tdoa相对root时间
    uint64_t m_loc_time;
    uint32_t m_card_id;
    int32_t	 m_card_ct;
    int8_t   m_card_type;
    int8_t   m_ant_id;
    int16_t  m_rav;
    int16_t  m_acc;
    uint16_t m_sync_ct;
    uint16_t m_rssi;


    loc_message();
    loc_message(site s,uint64_t num_ticks,uint64_t timestamp,
                uint32_t cardid,int32_t ct,int8_t type,int8_t antid,
                int16_t rav,int16_t acc,uint16_t sync_ct,uint16_t rssi)
        :m_sit(s)
        ,m_num_ticks(num_ticks)
        ,m_loc_time(timestamp)
        ,m_card_id(cardid)
        ,m_card_ct(ct)
        ,m_card_type(type)
        ,m_ant_id(antid)
        ,m_rav(rav)
        ,m_acc(acc)
        ,m_sync_ct(sync_ct)
        ,m_rssi(rssi)
    {}
    int tool_index()const;
};

#endif