#include "RosInterface.hpp"

#include "CoordSys.h"

namespace jf {
RosInterface::RosInterface(const std::string& prj_dir, const std::string& cfg_dir, int tgt_len) : m_nhdNodeHandle("~"), m_miLib(prj_dir, cfg_dir, tgt_len) {
    // 注册回调函数
    m_subAdama = m_nhdNodeHandle.subscribe("/AdmaData", 100, &RosInterface::ReceiveAdma, this);

    // 设置发布
    m_pubMapData = m_nhdNodeHandle.advertise<bs_map_msgs::MapDataMsgs>("/MapData", 10);

    // for test
    Adma adma = {};
    adma.ins_long_abs = 120.44553759999999;
    adma.ins_lat_abs = 31.596058699999997;
    adma.ins_height = 14.850000;
    adma.ins_yaw = 312;
    adma.turn_lamp_state = 0;
    // adma.ins_long_abs = 120.4376732;
    // adma.ins_lat_abs = 31.5876113;
    // adma.ins_height = 14.850000;
    // adma.ins_yaw = 137;
    // adma.turn_lamp_state = 0;

    MapData map = {};
    m_miLib.GetMapData(adma, map);
    PublishMapData(map);

    // adma.ins_long_abs = 120.44519768;
    // adma.ins_lat_abs = 31.58407482;
    // adma.ins_height = 14.850000;
    // adma.ins_yaw = 90;
    // adma.turn_lamp_state = 0;

    // map = {};
    // m_miLib.GetMapData(adma, map);
    // PublishMapData(map);
}

RosInterface::~RosInterface() {}

void RosInterface::ReceiveAdma(const gnss_wheel_msgs::adma_data_msgs::ConstPtr& msg) {
    Adma adma = {};

    auto start = std::chrono::system_clock::now().time_since_epoch() / std::chrono::milliseconds(1);  // use this

    adma.message_id = msg->message_id;
    adma.status_gps_mode = msg->status_gps_mode;
    adma.ins_gnss_ts_msec_epoch = msg->ins_gnss_ts_msec_epoch;
    adma.ins_time_msec = msg->ins_time_msec;
    adma.ins_time_week = msg->ins_time_week;
    adma.leap_seconds = msg->leap_seconds;
    adma.start_timestamp_ms_local = msg->start_timestamp_ms_local;
    adma.ins_lat_abs = msg->ins_lat_abs;
    adma.ins_long_abs = msg->ins_long_abs;
    adma.ins_height = msg->ins_height;
    adma.ins_roll = msg->ins_roll;
    adma.ins_pitch = msg->ins_pitch;
    adma.ins_yaw = 360 - msg->ins_yaw;
    adma.gps_cog = msg->gps_cog;
    adma.gps_cog_in_enu = msg->gps_cog_in_enu;
    adma.ins_vel_frame_x = msg->ins_vel_frame_x;
    adma.ins_vel_frame_y = msg->ins_vel_frame_y;
    adma.ins_vel_frame_z = msg->ins_vel_frame_z;
    adma.ins_vel_hor_x = msg->ins_vel_hor_x;
    adma.ins_vel_hor_y = msg->ins_vel_hor_y;
    adma.ins_vel_hor_z = msg->ins_vel_hor_z;
    adma.acc_hor_x = msg->acc_hor_x;
    adma.acc_hor_y = msg->acc_hor_y;
    adma.acc_hor_z = msg->acc_hor_z;
    adma.turn_lamp_state = msg->turn_lamp_state;

    MapData map = {};
    if (true == m_miLib.GetMapData(adma, map)) {
        PublishMapData(map);
        auto end = std::chrono::system_clock::now().time_since_epoch() / std::chrono::milliseconds(1);  // use this
        ROS_INFO("ReceiveAdma, Gnss: %lf, %lf, %lf, Heading: %lf, Lamp: %d, CostTime: %d", msg->ins_long_abs, msg->ins_lat_abs, msg->ins_height, 360 - msg->ins_yaw, msg->turn_lamp_state, end - start);
    }
    return;
}

void RosInterface::PublishMapData(const MapData& data) {
    auto func_ConvertPoint3D = [](const Point3D& in, bs_map_msgs::Point3D& out) {
        out.d_longitude = in.d_longitude;
        out.d_latitude = in.d_latitude;
        out.f_altitude = in.f_altitude;
    };

    auto func_ConvertGeometry = [&func_ConvertPoint3D](const std::vector<Point3D>& in, std::vector<bs_map_msgs::Point3D>& out) {
        for (Point3D point3d : in) {
            bs_map_msgs::Point3D p = {};

            func_ConvertPoint3D(point3d, p);

            out.push_back(p);
        }
    };

    bs_map_msgs::MapDataMsgs msg = {};

    // Road
    // RoadLinks
    for (RoadLink roadlink : data.road.roadLinks) {
        bs_map_msgs::RoadLink rl = {};

        // LaneEdge
        for (LaneEdge laneedge : roadlink.edges) {
            bs_map_msgs::LaneEdge le = {};

            func_ConvertGeometry(laneedge.edgeGeometry, le.edgeGeometry);
            func_ConvertPoint3D(laneedge.laneLineChangeStart, le.laneLineChangeStart);
            func_ConvertPoint3D(laneedge.laneLineChangeEnd, le.laneLineChangeEnd);
            le.f_LineLongth = laneedge.f_LineLongth;
            le.u8_laneEdgeType = laneedge.u8_laneEdgeType;
            le.u8_laneColor = laneedge.u8_laneColor;
            le.u8_laneEdgeNum = laneedge.u8_laneEdgeNum;
            le.u8_laneEdgeIndex = laneedge.u8_laneEdgeIndex;
            le.u8_laneLeftRoadEdge = laneedge.u8_laneLeftRoadEdge;
            le.u8_laneRightRoadEdge = laneedge.u8_laneRightRoadEdge;

            rl.edges.push_back(le);
        }

        // Lane
        for (Lane lane : roadlink.lanes) {
            bs_map_msgs::Lane l = {};

            func_ConvertGeometry(lane.geoLines, l.geoLines);
            l.f_width = lane.f_width;
            l.f_curvature = lane.f_curvature;
            l.f_slope = lane.f_slope;
            l.u8_turnWaiting = lane.u8_turnWaiting;
            l.u8_laneIndex = lane.u8_laneIndex;

            rl.lanes.push_back(l);
        }

        rl.u8_driveLaneNumber = roadlink.u8_driveLaneNumber;

        rl.u8_driveRoadProbability = roadlink.u8_driveRoadProbability;

        rl.u8_TotalLaneNum = roadlink.u8_TotalLaneNum;

        msg.road.roadLinks.push_back(rl);
    }

    // StopLine
    msg.road.stopLine.bStopLineValidFlag = data.road.stopLine.bStopLineValidFlag;
    msg.road.stopLine.f_stopLineDis = data.road.stopLine.f_stopLineDis;
    func_ConvertGeometry(data.road.stopLine.stopLinePoints, msg.road.stopLine.stopLinePoints);

    // PedestrianCrossing
    msg.road.pedestrianCrossing.bPedestrianCrossingValidFlag = data.road.pedestrianCrossing.bPedestrianCrossingValidFlag;
    msg.road.pedestrianCrossing.f_pedestrianCrossingDis = data.road.pedestrianCrossing.f_pedestrianCrossingDis;
    func_ConvertGeometry(data.road.pedestrianCrossing.f_pedestrianCrossingPoints, msg.road.pedestrianCrossing.f_pedestrianCrossingPoints);

    // Crossing
    msg.road.crossing.bCrossingValidFlag = data.road.crossing.bCrossingValidFlag;
    msg.road.crossing.u8_crossingType = data.road.crossing.u8_crossingType;
    msg.road.crossing.f_crossingDistance = data.road.crossing.f_crossingDistance;
    func_ConvertGeometry(data.road.crossing.geoAreaPoints, msg.road.crossing.geoAreaPoints);

    // Ramp
    msg.road.ramp.bRampValidFlag = data.road.ramp.bRampValidFlag;
    msg.road.ramp.u8_rampType = data.road.ramp.u8_rampType;
    msg.road.ramp.f_rampStartPointDis = data.road.ramp.f_rampStartPointDis;
    msg.road.ramp.f_rampEndPointDis = data.road.ramp.f_rampEndPointDis;
    func_ConvertPoint3D(data.road.ramp.rampStartPoint, msg.road.ramp.rampStartPoint);
    func_ConvertPoint3D(data.road.ramp.rampEndPoint, msg.road.ramp.rampEndPoint);
    for (LaneEdge rampedge : data.road.ramp.rampEdges) {
        bs_map_msgs::LaneEdge le = {};

        func_ConvertGeometry(rampedge.edgeGeometry, le.edgeGeometry);
        func_ConvertPoint3D(rampedge.laneLineChangeStart, le.laneLineChangeStart);
        func_ConvertPoint3D(rampedge.laneLineChangeEnd, le.laneLineChangeEnd);
        le.f_LineLongth = rampedge.f_LineLongth;
        le.u8_laneEdgeType = rampedge.u8_laneEdgeType;
        le.u8_laneColor = rampedge.u8_laneColor;
        le.u8_laneEdgeNum = rampedge.u8_laneEdgeNum;
        le.u8_laneEdgeIndex = rampedge.u8_laneEdgeIndex;
        le.u8_laneLeftRoadEdge = rampedge.u8_laneLeftRoadEdge;
        le.u8_laneRightRoadEdge = rampedge.u8_laneRightRoadEdge;

        msg.road.ramp.rampEdges.push_back(le);
    }

    // Change
    msg.road.changePoint.bChangeValidFlag = data.road.changePoint.bChangeValidFlag;
    msg.road.changePoint.i32_changeType = data.road.changePoint.i32_changeType;
    msg.road.changePoint.f_changeDistance = data.road.changePoint.f_changeDistance;
    func_ConvertPoint3D(data.road.changePoint.changePoint, msg.road.changePoint.changePoint);

    // Speed
    msg.road.speed.bSpeedLimitValidFlag = data.road.speed.bSpeedLimitValidFlag;
    msg.road.speed.u16_CurrentRoadSDLimit_Max = data.road.speed.u16_CurrentRoadSDLimit_Max;
    msg.road.speed.u16_CurrentRoadSDLimit_Min = data.road.speed.u16_CurrentRoadSDLimit_Min;
    msg.road.speed.u16_NextRoadSDLimit_Max = data.road.speed.u16_NextRoadSDLimit_Max;
    msg.road.speed.u6_NextRoadSDLimit_Min = data.road.speed.u6_NextRoadSDLimit_Min;
    msg.road.speed.f_distanceToNextSpeedLimit = data.road.speed.f_distanceToNextSpeedLimit;

    // TrafficLight
    msg.road.trafficLight.bTrafficLightValidFlag = data.road.trafficLight.bTrafficLightValidFlag;
    func_ConvertPoint3D(data.road.trafficLight.trafficLightLoc, msg.road.trafficLight.trafficLightLoc);
    msg.road.trafficLight.f_distanceToTrafficLight = data.road.trafficLight.f_distanceToTrafficLight;

    // RSU
    msg.road.rsu.bRsuValidFlag = data.road.rsu.bRsuValidFlag;
    func_ConvertPoint3D(data.road.rsu.rsuLocation, msg.road.rsu.rsuLocation);
    msg.road.rsu.f_distanceToRsu = data.road.rsu.f_distanceToRsu;

    // Tunnel
    msg.road.tunnel.bTunnelValidFlag = data.road.tunnel.bTunnelValidFlag;
    msg.road.tunnel.u8_TunnelType = data.road.tunnel.u8_TunnelType;
    msg.road.tunnel.f_distanceToTunnel = data.road.tunnel.f_distanceToTunnel;
    msg.road.tunnel.f_tunnel_length = data.road.tunnel.f_tunnel_length;
    func_ConvertPoint3D(data.road.tunnel.startPoint, msg.road.tunnel.startPoint);
    func_ConvertPoint3D(data.road.tunnel.endPoint, msg.road.tunnel.endPoint);

    msg.road.u8_CurrentLane = data.road.u8_CurrentLane;

    msg.road.f_dashSolidChangeDis = data.road.f_dashSolidChangeDis;

    msg.bOnMapFlag = data.bOnMapFlag;

    // Gnss
    msg.gnssInfo.u64_time_msec = data.gnssInfo.u64_time_msec;
    msg.gnssInfo.u64_time_week = data.gnssInfo.u64_time_week;
    msg.gnssInfo.u64_EpochTimestamp = data.gnssInfo.u64_EpochTimestamp;
    func_ConvertPoint3D(data.gnssInfo.locationRel, msg.gnssInfo.locationRel);
    func_ConvertPoint3D(data.gnssInfo.locationAbs, msg.gnssInfo.locationAbs);
    msg.gnssInfo.f_pitch = data.gnssInfo.f_pitch;
    msg.gnssInfo.f_roll = data.gnssInfo.f_roll;
    msg.gnssInfo.f_yaw = data.gnssInfo.f_yaw;
    msg.gnssInfo.f_acceleX = data.gnssInfo.f_acceleX;
    msg.gnssInfo.f_acceleY = data.gnssInfo.f_acceleY;
    msg.gnssInfo.f_acceleZ = data.gnssInfo.f_acceleZ;
    msg.gnssInfo.u8_turnLampState = data.gnssInfo.u8_turnLampState;
    msg.gnssInfo.u64_AdmaMsgReceivedTime = data.gnssInfo.u64_AdmaMsgReceivedTime;
    msg.gnssInfo.u64_SensorNodeProcessTime = data.gnssInfo.u64_SensorNodeProcessTime;
    msg.gnssInfo.u64_MapProcessTime = data.gnssInfo.u64_MapProcessTime;

    // 测试用
    TestProgram(msg);

    m_pubMapData.publish(msg);

    return;
}

void RosInterface::TestProgram(const bs_map_msgs::MapDataMsgs& msg) {
    auto func_PrintLaneEdge = [](std::ofstream& ofsQgisLaneEdge, const bs_map_msgs::LaneEdge& edge) {
        for (const bs_map_msgs::Point3D point3d : edge.edgeGeometry) {
            ofsQgisLaneEdge << point3d.d_longitude << ", " << point3d.d_latitude << ", " << edge.u8_laneEdgeType << std::endl;
        }
    };
    static bool bFirst = true;

    std::ofstream ofsQgisLaneEdgeL_L;
    const std::string strQgisLaneEdgeL_L = "LaneEdgeL_L.csv";
    ofsQgisLaneEdgeL_L.open(strQgisLaneEdgeL_L.c_str(), std::ios::out | std::ios::trunc);
    ofsQgisLaneEdgeL_L.setf(std::ios::fixed, std::ios::floatfield);
    ofsQgisLaneEdgeL_L.precision(15);
    ofsQgisLaneEdgeL_L << "lon, lat, info" << std::endl;

    std::ofstream ofsQgisLaneEdgeL;
    const std::string strQgisLaneEdgeL = "LaneEdgeL.csv";
    ofsQgisLaneEdgeL.open(strQgisLaneEdgeL.c_str(), std::ios::out | std::ios::trunc);
    ofsQgisLaneEdgeL.setf(std::ios::fixed, std::ios::floatfield);
    ofsQgisLaneEdgeL.precision(15);
    ofsQgisLaneEdgeL << "lon, lat, info" << std::endl;

    std::ofstream ofsQgisLaneEdgeR;
    const std::string strQgisLaneEdgeR = "LaneEdgeR.csv";
    ofsQgisLaneEdgeR.open(strQgisLaneEdgeR.c_str(), std::ios::out | std::ios::trunc);
    ofsQgisLaneEdgeR.setf(std::ios::fixed, std::ios::floatfield);
    ofsQgisLaneEdgeR.precision(15);
    ofsQgisLaneEdgeR << "lon, lat, info" << std::endl;

    std::ofstream ofsQgisLaneEdgeR_R;
    const std::string strQgisLaneEdgeR_R = "LaneEdgeR-R.csv";
    ofsQgisLaneEdgeR_R.open(strQgisLaneEdgeR_R.c_str(), std::ios::out | std::ios::trunc);
    ofsQgisLaneEdgeR_R.setf(std::ios::fixed, std::ios::floatfield);
    ofsQgisLaneEdgeR_R.precision(15);
    ofsQgisLaneEdgeR_R << "lon, lat, info" << std::endl;

    std::ofstream ofsQgisLaneEdge0;
    const std::string strQgisLaneEdge0 = "LaneEdge0.csv";
    ofsQgisLaneEdge0.open(strQgisLaneEdge0.c_str(), std::ios::out | std::ios::trunc);
    ofsQgisLaneEdge0.setf(std::ios::fixed, std::ios::floatfield);
    ofsQgisLaneEdge0.precision(15);
    ofsQgisLaneEdge0 << "lon, lat, info" << std::endl;

    std::ofstream ofsQgisLaneEdge1;
    const std::string strQgisLaneEdge1 = "LaneEdge1.csv";
    ofsQgisLaneEdge1.open(strQgisLaneEdge1.c_str(), std::ios::out | std::ios::trunc);
    ofsQgisLaneEdge1.setf(std::ios::fixed, std::ios::floatfield);
    ofsQgisLaneEdge1.precision(15);
    ofsQgisLaneEdge1 << "lon, lat, info" << std::endl;

    std::ofstream ofsQgisLaneEdge2;
    const std::string strQgisLaneEdge2 = "LaneEdge2.csv";
    ofsQgisLaneEdge2.open(strQgisLaneEdge2.c_str(), std::ios::out | std::ios::trunc);
    ofsQgisLaneEdge2.setf(std::ios::fixed, std::ios::floatfield);
    ofsQgisLaneEdge2.precision(15);
    ofsQgisLaneEdge2 << "lon, lat, info" << std::endl;

    std::ofstream ofsQgisLaneEdge3;
    const std::string strQgisLaneEdge3 = "LaneEdge3.csv";
    ofsQgisLaneEdge3.open(strQgisLaneEdge3.c_str(), std::ios::out | std::ios::trunc);
    ofsQgisLaneEdge3.setf(std::ios::fixed, std::ios::floatfield);
    ofsQgisLaneEdge3.precision(15);
    ofsQgisLaneEdge3 << "lon, lat, info" << std::endl;

    std::ofstream ofsQgisLaneEdge4;
    const std::string strQgisLaneEdge4 = "LaneEdge4.csv";
    ofsQgisLaneEdge4.open(strQgisLaneEdge4.c_str(), std::ios::out | std::ios::trunc);
    ofsQgisLaneEdge4.setf(std::ios::fixed, std::ios::floatfield);
    ofsQgisLaneEdge4.precision(15);
    ofsQgisLaneEdge4 << "lon, lat, index" << std::endl;

    std::ofstream ofsQgisLaneEdge5;
    const std::string strQgisLaneEdge5 = "LaneEdge5.csv";
    ofsQgisLaneEdge5.open(strQgisLaneEdge5.c_str(), std::ios::out | std::ios::trunc);
    ofsQgisLaneEdge5.setf(std::ios::fixed, std::ios::floatfield);
    ofsQgisLaneEdge5.precision(15);
    ofsQgisLaneEdge5 << "lon, lat, info" << std::endl;

    std::ofstream ofsQgisLaneEdge6;
    const std::string strQgisLaneEdge6 = "LaneEdge6.csv";
    ofsQgisLaneEdge6.open(strQgisLaneEdge6.c_str(), std::ios::out | std::ios::trunc);
    ofsQgisLaneEdge6.setf(std::ios::fixed, std::ios::floatfield);
    ofsQgisLaneEdge6.precision(15);
    ofsQgisLaneEdge6 << "lon, lat, info" << std::endl;

    std::ofstream ofsQgisLaneEdge7;
    const std::string strQgisLaneEdge7 = "LaneEdge7.csv";
    ofsQgisLaneEdge7.open(strQgisLaneEdge7.c_str(), std::ios::out | std::ios::trunc);
    ofsQgisLaneEdge7.setf(std::ios::fixed, std::ios::floatfield);
    ofsQgisLaneEdge7.precision(15);
    ofsQgisLaneEdge7 << "lon, lat, info" << std::endl;

    std::ofstream ofsQgisLaneEdge8;
    const std::string strQgisLaneEdge8 = "LaneEdge8.csv";
    ofsQgisLaneEdge8.open(strQgisLaneEdge8.c_str(), std::ios::out | std::ios::trunc);
    ofsQgisLaneEdge8.setf(std::ios::fixed, std::ios::floatfield);
    ofsQgisLaneEdge8.precision(15);
    ofsQgisLaneEdge8 << "lon, lat, info" << std::endl;

    std::ofstream ofsQgisLaneEdge9;
    const std::string strQgisLaneEdge9 = "LaneEdge9.csv";
    ofsQgisLaneEdge9.open(strQgisLaneEdge9.c_str(), std::ios::out | std::ios::trunc);
    ofsQgisLaneEdge9.setf(std::ios::fixed, std::ios::floatfield);
    ofsQgisLaneEdge9.precision(15);
    ofsQgisLaneEdge9 << "lon, lat, info" << std::endl;

    std::ofstream ofsQgisLaneEdge10;
    const std::string strQgisLaneEdge10 = "LaneEdge10.csv";
    ofsQgisLaneEdge10.open(strQgisLaneEdge10.c_str(), std::ios::out | std::ios::trunc);
    ofsQgisLaneEdge10.setf(std::ios::fixed, std::ios::floatfield);
    ofsQgisLaneEdge10.precision(15);
    ofsQgisLaneEdge10 << "lon, lat, info" << std::endl;

    double dLeftLength = 0.0;
    double dRightLength = 0.0;
    for (const bs_map_msgs::RoadLink& link : msg.road.roadLinks) {
        int laneIndex = link.u8_driveLaneNumber;
        for (int i = 0; i < link.edges.size(); i++) {
            auto edge = link.edges.at(i);
            if (i == laneIndex - 2) {
                func_PrintLaneEdge(ofsQgisLaneEdgeL_L, edge);
            } else if (i == laneIndex - 1) {
                func_PrintLaneEdge(ofsQgisLaneEdgeL, edge);
                for (int j = 0; j < edge.edgeGeometry.size() - 1; j++) {
                    bs_map_msgs::Point3D p1 = edge.edgeGeometry.at(j);
                    bs_map_msgs::Point3D p2 = edge.edgeGeometry.at(j + 1);
                    dLeftLength += PointsDis(jf::Point(p1.d_longitude, p1.d_latitude, 0), jf::Point(p2.d_longitude, p2.d_latitude, 0));
                }
            } else if (i == laneIndex) {
                func_PrintLaneEdge(ofsQgisLaneEdgeR, edge);
                for (int j = 0; j < edge.edgeGeometry.size() - 1; j++) {
                    bs_map_msgs::Point3D p1 = edge.edgeGeometry.at(j);
                    bs_map_msgs::Point3D p2 = edge.edgeGeometry.at(j + 1);
                    dRightLength += PointsDis(jf::Point(p1.d_longitude, p1.d_latitude, 0), jf::Point(p2.d_longitude, p2.d_latitude, 0));
                }
            } else if (i == laneIndex + 1) {
                func_PrintLaneEdge(ofsQgisLaneEdgeR_R, edge);
            }
        }
    }
    int nGeoCnt = 0;
    for (const bs_map_msgs::RoadLink& link : msg.road.roadLinks) {
        nGeoCnt += link.edges.at(0).edgeGeometry.size();
        for (int i = 0; i < link.edges.size(); i++) {
            auto edge = link.edges.at(i);
            switch (i) {
                case 0:
                    func_PrintLaneEdge(ofsQgisLaneEdge0, edge);
                    break;
                case 1:
                    func_PrintLaneEdge(ofsQgisLaneEdge1, edge);
                    break;
                case 2:
                    func_PrintLaneEdge(ofsQgisLaneEdge2, edge);
                    break;
                case 3:
                    func_PrintLaneEdge(ofsQgisLaneEdge3, edge);
                    break;
                case 4:
                    func_PrintLaneEdge(ofsQgisLaneEdge4, edge);
                    break;
                case 5:
                    func_PrintLaneEdge(ofsQgisLaneEdge5, edge);
                    break;
                case 6:
                    func_PrintLaneEdge(ofsQgisLaneEdge6, edge);
                    break;
                case 7:
                    func_PrintLaneEdge(ofsQgisLaneEdge7, edge);
                    break;
                case 8:
                    func_PrintLaneEdge(ofsQgisLaneEdge8, edge);
                    break;
                case 9:
                    func_PrintLaneEdge(ofsQgisLaneEdge9, edge);
                    break;
                default:
                    func_PrintLaneEdge(ofsQgisLaneEdge10, edge);
                    break;
            }
        }
    }
    if (nGeoCnt > 50) {
        int b = 0;
    }
    std::cout << "edge geo count: " << nGeoCnt << std::endl;

    ofsQgisLaneEdge0.close();
    ofsQgisLaneEdge1.close();
    ofsQgisLaneEdge2.close();
    ofsQgisLaneEdge3.close();
    ofsQgisLaneEdge4.close();
    ofsQgisLaneEdge5.close();
    ofsQgisLaneEdge6.close();
    ofsQgisLaneEdge7.close();
    ofsQgisLaneEdge8.close();
    ofsQgisLaneEdge9.close();
    ofsQgisLaneEdge10.close();

    // return;

    static int s_nAdmaIndex = -1;
    s_nAdmaIndex++;

    std::ofstream ofsQgisAdma;
    const std::string strQgisAdama = "Adma.csv";

    if (true == bFirst) {
        // 插入标签
        ofsQgisAdma.open(strQgisAdama.c_str(), std::ios::out | std::ios::trunc);
        ofsQgisAdma << "lon, lat, info" << std::endl;
    } else {
        ofsQgisAdma.open(strQgisAdama.c_str(), std::ios::out | std::ios::app);
    }
    ofsQgisAdma.setf(std::ios::fixed, std::ios::floatfield);
    ofsQgisAdma.precision(15);

    ofsQgisAdma << msg.gnssInfo.locationAbs.d_longitude << ", " << msg.gnssInfo.locationAbs.d_latitude << ", " << msg.road.stopLine.f_stopLineDis << std::endl;

    std::ofstream ofsQgisObj;
    const std::string strQgisObj = "Obj.csv";
    ofsQgisObj.open(strQgisObj.c_str(), std::ios::out | std::ios::trunc);
    ofsQgisObj.precision(15);

    if (true == bFirst) {
        // 插入标签
        ofsQgisObj << "lon, lat, info, name, dis" << std::endl;
    }

    if (true == msg.road.stopLine.bStopLineValidFlag) {
        auto& stopline = msg.road.stopLine;
        for (int i = 0; i < stopline.stopLinePoints.size(); i++) {
            ofsQgisObj << stopline.stopLinePoints.at(i).d_longitude << "," << stopline.stopLinePoints.at(i).d_latitude << "," << s_nAdmaIndex << ",StopLine," << stopline.f_stopLineDis << std::endl;
        }
    }
    if (true == msg.road.pedestrianCrossing.bPedestrianCrossingValidFlag) {
        auto& pedestrian = msg.road.pedestrianCrossing;
        ofsQgisObj << pedestrian.f_pedestrianCrossingPoints.at(0).d_longitude << ", " << pedestrian.f_pedestrianCrossingPoints.at(0).d_latitude << "," << s_nAdmaIndex << ",pedestrian," << pedestrian.f_pedestrianCrossingDis << std::endl;
    }
    if (true == msg.road.crossing.bCrossingValidFlag) {
        auto& cross = msg.road.crossing;
        ofsQgisObj << cross.geoAreaPoints.at(0).d_longitude << "," << cross.geoAreaPoints.at(0).d_latitude << "," << s_nAdmaIndex << ",crossing," << cross.f_crossingDistance << std::endl;
    }
    if (true == msg.road.ramp.bRampValidFlag) {
        auto& ramp = msg.road.ramp;
        ofsQgisObj << ramp.rampStartPoint.d_longitude << "," << ramp.rampStartPoint.d_latitude << "," << s_nAdmaIndex << ",rampstart," << ramp.f_rampStartPointDis << std::endl;
        ofsQgisObj << ramp.rampEndPoint.d_longitude << "," << ramp.rampEndPoint.d_latitude << "," << s_nAdmaIndex << ",rampend," << ramp.f_rampEndPointDis << std::endl;
        func_PrintLaneEdge(ofsQgisObj, msg.road.ramp.rampEdges.at(0));
        func_PrintLaneEdge(ofsQgisObj, msg.road.ramp.rampEdges.at(1));
    }
    if (true == msg.road.changePoint.bChangeValidFlag) {
        auto& change = msg.road.changePoint;
        ofsQgisObj << change.changePoint.d_longitude << "," << change.changePoint.d_latitude << "," << s_nAdmaIndex << ",change," << change.f_changeDistance << std::endl;
    }
    // if (true == msg.road.speed.bSpeedLimitValidFlag) {
    //     auto& speed = msg.road.speed;
    //     ofsQgisObj << msg.gnssInfo.locationAbs.d_longitude << ", " << msg.gnssInfo.locationAbs.d_latitude << ", " << s_nAdmaIndex << ",speed," << speed.f_distanceToNextSpeedLimit << std::endl;
    // }
    if (true == msg.road.trafficLight.bTrafficLightValidFlag) {
        auto& tl = msg.road.trafficLight;
        ofsQgisObj << tl.trafficLightLoc.d_longitude << ", " << tl.trafficLightLoc.d_latitude << "," << s_nAdmaIndex << ",trafficlight," << tl.f_distanceToTrafficLight << std::endl;
    }
    if (true == msg.road.rsu.bRsuValidFlag) {
        auto& rsu = msg.road.rsu;
        ofsQgisObj << rsu.rsuLocation.d_longitude << ", " << rsu.rsuLocation.d_latitude << "," << s_nAdmaIndex << ",rus," << rsu.f_distanceToRsu << std::endl;
    }
    if (true == msg.road.tunnel.bTunnelValidFlag) {
        auto& tunnel = msg.road.tunnel;
        ofsQgisObj << tunnel.startPoint.d_longitude << "," << tunnel.startPoint.d_latitude << "," << s_nAdmaIndex << ",tunnel," << tunnel.f_distanceToTunnel << std::endl;
    }

    ofsQgisObj.close();
    ofsQgisAdma.close();

    bFirst = false;

    return;
}

}  // namespace jf