#ifndef _ROS_MATCH_H_
#define _ROS_MATCH_H_
#include <Gaussian.h>
#include <ros/ros.h>

#include "CoordSys.h"
#include "MapMatching.hpp"
#include "adu_msgs/MeData.h"
#include "can_msgs/Frame.h"
#include "gnss_imu_msgs/GNSS.h"
#include "jc_map_msgs/MatchingResult.h"
#include "localization_msgs/Localization.h"

namespace jf {
class RosMatch {
   public:
    RosMatch();
    ~RosMatch();

    // 接受GNSS的信息
    void ReceiveGnss(const gnss_imu_msgs::GNSS::ConstPtr& msg);

    // 接受EyeQ4的信息
    void ReceiveAduData(const adu_msgs::MeData::ConstPtr& msg);

    // 接受egomotion的信息
    void ReceiveLocalization(const localization_msgs::Localization::ConstPtr& msg);

   private:
    // 发布地图匹配结果的进程
    void PublishMatchResultProc();

    // 发布地图输出数据的进程
    void PublishMapInfoProc();

    // 发布地图匹配结果
    void PublishMatchResult(const IGnssFootLocation& data);

    // 发布定位信息的CanFd消息
    void PublishLocCanFd(const localization_msgs::Localization::ConstPtr& msg, double speed);

    // 转换车道位置信息
    bool ConvertLanePos(int src, LaneMarkingSide& dst);
    // 转换车道颜色
    bool ConvertLaneColor(int src, HMC::LaneMarkingColor& dst);
    // 转换车道跨越属性
    bool ConvertLaneCross(int src_double, int src_single, HMC::LaneMarkingType& dst);
    // 转换道路边界线位置
    bool ConvertLinkPos(int src, LinkMarkingSide& dst);
    // 转换标牌
    bool ConvertTsr(const adu_msgs::TsrInfo& src, ITrafsignMap& dst);

   private:
    // 地图匹配相关
    MapMatching m_mmMatch;
    ros::NodeHandle m_nhNodeHandle;
    ros::Subscriber m_subGnss;
    ros::Subscriber m_subEyeQ4;
    ros::Subscriber m_subLoc;
    ros::Publisher m_pubLocCanFd;
    ros::Publisher m_pubMatchResult;
    std::thread* m_thdPubMatchResult;
    std::thread* pub_map_info_thread_;
};
}  // namespace jf
#endif  // _ROS_MATCH_H_