
#include "RosInterface.hpp"
#include "InterfaceDefine.hpp"

namespace jf {

RosInterface::RosInterface(const std::string& prj_dir, const std::string& cfg_dir ,const double sc_dLMReportDis) : m_mapInterface(prj_dir, cfg_dir,sc_dLMReportDis) {
       m_subGpsData= nodeHandle.subscribe("/gps_data", 100, &jf::RosInterface::HandleGpsData, this);
       m_subSetPath = nodeHandle.subscribe("/setPath", 100, &jf::RosInterface::HandleSetPathMsg, this);
       m_pubPathAhead = nodeHandle.advertise<hd_map_msgs::PathAheadMsg>("pathAheadData", 100);
       m_pubPathPlan = nodeHandle.advertise<hd_map_msgs::PathPlanMsg>("pathPlanData", 100);
}

/**
 * 处理接收到的GPS消息数据
 * @param msg
 */
void RosInterface::HandleGpsData(const ins_interface_msgs::GpsData::ConstPtr& msg) {
    ROS_INFO("======GpsData");
    jf::GpsData_ gpsData;
    gpsData.altitude = msg->altitude;
    gpsData.latitude = msg->latitude;
    gpsData.longitude = msg->longitude;
    gpsData.heading_angle = msg->heading_angle;
    gpsData.solution_mode = msg->solution_mode;
    gpsData.time_stamp = msg->time_stamp;
    gpsData.vehicle_speed = msg->vehicle_speed;
    jf::PathAheadMsg_ pathAheadMsg;
    jf::PathPlanMsg_ pathPlanMsg;
    m_mapInterface.dynamicPathPlanning(gpsData, pathPlanMsg);
    m_mapInterface.GetPathAhead(gpsData, pathAheadMsg);  //注意！必须先进行动态路径规划dynamicPathPlanning，再进行道路前方信息的获取GetPathAhead
    CoverToPathPlanMsg(pathPlanMsg);
    CoverToPathAheadMsg(pathAheadMsg);
}
/**
 * 设置全局路径规划目标点
 * @param msg
 */
void RosInterface::HandleSetPathMsg(const ins_interface_msgs::SetPathMsg::ConstPtr& msg) {
    ROS_INFO("==============receive  setPath Msg");
    jf::SetPathMsg_ setPathMsg;
    for (hd_map_msgs::point ros_arrptWayPoint : msg->arrptWayPoints) {
        jf::point_ point;
        point.dLon = ros_arrptWayPoint.dLon;
        point.dLat = ros_arrptWayPoint.dLat;
        point.dAlt = ros_arrptWayPoint.dAlt;
        setPathMsg.arrptWayPoints.push_back(point);
    }
    jf::PathPlanMsg_ pathPlanMsg;
    m_mapInterface.setGlobalPathPlanning(setPathMsg, pathPlanMsg);
    CoverToPathPlanMsg(pathPlanMsg);
}
/**+++++++++++++++++
 * 将非ros消息PathAheadMsg_转化成ros消息并发送
 * @param pathAheadMsg
 */
void RosInterface::CoverToPathAheadMsg(const jf::PathAheadMsg_& pathAheadMsg) {
    hd_map_msgs::PathAheadMsg ros_pathAheadMsg;
    ros_pathAheadMsg.Header.stamp = ros::Time::now();
    ros_pathAheadMsg.Header.frame_id = pathAheadMsg.Header.frame_id;
    ros_pathAheadMsg.Header.seq = pathAheadMsg.Header.seq;
    ros_pathAheadMsg.bIsSuccess = pathAheadMsg.bIsSuccess;
    ros_pathAheadMsg.nCurBlockIndex = pathAheadMsg.nCurBlockIndex;
    ros_pathAheadMsg.ptCurLoc.dAlt = pathAheadMsg.ptCurLoc.dAlt;
    ros_pathAheadMsg.ptCurLoc.dLat = pathAheadMsg.ptCurLoc.dLat;
    ros_pathAheadMsg.ptCurLoc.dLon = pathAheadMsg.ptCurLoc.dLon;

    for (jf::link_ link1 : pathAheadMsg.arrlkAheadPath) {
        hd_map_msgs::link ros_link;
        TransformToRosMsg_link(link1, ros_link);
        ros_pathAheadMsg.arrlkAheadPath.push_back(ros_link);
    }
    for (jf::landmark_ landmark : pathAheadMsg.arrlmAheadLandmark) {
        hd_map_msgs::landmark ros_landmark;
        TransformToRosMsg_landmark(landmark, ros_landmark);
        ros_pathAheadMsg.arrlmAheadLandmark.push_back(ros_landmark);
    }
    m_pubPathAhead.publish(ros_pathAheadMsg);
}
/**
 * 将非ros消息PathPlanMsg_转化成ros消息并发送
 * @param pathPlanMsg
 */
void RosInterface::CoverToPathPlanMsg(const jf::PathPlanMsg_& pathPlanMsg) {
    hd_map_msgs::PathPlanMsg ros_pathPlanMsg;
    TransformToRosMsg_point(pathPlanMsg.ptCurLoc, ros_pathPlanMsg.ptCurLoc);
    ros_pathPlanMsg.bIsSuccess = pathPlanMsg.bIsSuccess;
    ros_pathPlanMsg.bIsChanged = pathPlanMsg.bIsChanged;

        /**********打印测试*********/
        std::string cstrFileName = "/home/muyusheng/blockData.csv";
        std::ofstream ofsQgisResult;
        // 创建csv文件
        ofsQgisResult.open(cstrFileName.c_str(), std::ios::out | std::ios::trunc);
        ofsQgisResult.setf(std::ios::fixed, std::ios::floatfield);
        ofsQgisResult.precision(15);
        static int count = 0;
        // 插入标签
        ofsQgisResult << "lon"
                      << ", "
                      << "lat"
                      << ", "
                      << "num"
                      << ", "
                      << "nLaneSeqNum"
                      << ", "
                      << "lId"
                      << std::endl;


    for (jf::block_ block : pathPlanMsg.arrblPath) {
        hd_map_msgs::block ros_block;
        TransformToRosMsg_block(block, ros_block);

        /////////////////////
        for(hd_map_msgs::point point2 : ros_block.arrptTrack){
            ofsQgisResult<<point2.dLon<<","<<point2.dLat<<","<<count<<","<<ros_block.nLaneSeqNum<<","<<ros_block.lId<<std::endl;
            count++;
        }
        ////////////////////////

        ros_pathPlanMsg.arrblPath.push_back(ros_block);
    }

    //////////////////////
    ofsQgisResult.close();
   //////////////////////////

    ros_pathPlanMsg.Header.stamp = ros::Time::now();
    ros_pathPlanMsg.Header.seq = pathPlanMsg.Header.seq;
    ros_pathPlanMsg.Header.frame_id = pathPlanMsg.Header.frame_id;
    m_pubPathPlan.publish(ros_pathPlanMsg);
}

void RosInterface::TransformToRosMsg_point(const jf::point_& point, hd_map_msgs::point& ros_point) {
    ros_point.dAlt = point.dAlt;
    ros_point.dLat = point.dLat;
    ros_point.dLon = point.dLon;
}
void RosInterface::TransformToRosMsg_block(const jf::block_& block, hd_map_msgs::block& ros_block) {
    ros_block.lId = block.lId;
    ros_block.arrptTrack.clear();
    for (jf::point_ point : block.arrptTrack) {
        hd_map_msgs::point ros_point;
        TransformToRosMsg_point(point, ros_point);
        ros_block.arrptTrack.push_back(ros_point);
    }
    ros_block.dDriveLen = block.dDriveLen;
    ros_block.nDriveDirect = block.nDriveDirect;
    ros_block.nLaneSeqNum = block.nLaneSeqNum;
}
void RosInterface::TransformToRosMsg_edge(const jf::edge_& edge, hd_map_msgs::edge& ros_edge) {
    ros_edge.lId = edge.lId;
    ros_edge.arrptGeo.clear();
    for (jf::point_ point : edge.arrptGeo) {
        hd_map_msgs::point ros_point;
        TransformToRosMsg_point(point, ros_point);
        ros_edge.arrptGeo.push_back(ros_point);
    }
    ros_edge.edgeType = edge.edgeType;
    ros_edge.nAttrib = edge.nAttrib;
}
void RosInterface::TransformToRosMsg_landmark(const jf::landmark_& landmark, hd_map_msgs::landmark& ros_landmark) {
    ros_landmark.nType = landmark.nType;
    ros_landmark.dDistance = landmark.dDistance;
    ros_landmark.ptPos.clear();
    for (jf::point_ point : landmark.ptPos) {
        hd_map_msgs::point ros_point;
        TransformToRosMsg_point(point, ros_point);
        ros_landmark.ptPos.push_back(ros_point);
    }
    ros_landmark.shape = landmark.shape;
    ros_landmark.type1 = landmark.type1;
    ros_landmark.type2 = landmark.type2;
}
void RosInterface::TransformToRosMsg_lane(const jf::lane_& lane, hd_map_msgs::lane& ros_lane) {
    ros_lane.lId = lane.lId;
    ros_lane.nLaneSeqNum = lane.nLaneSeqNum;
    ros_lane.arrlNxtLanes.clear();
    for (int64_t tmp : ros_lane.arrlNxtLanes) {
        ros_lane.arrlNxtLanes.push_back(tmp);
    }
    ros_lane.nLaneWidth = lane.nLaneWidth;
    ros_lane.lLeftEdgeId = lane.lLeftEdgeId;
    ros_lane.lRightEdgeId = lane.lRightEdgeId;
    ros_lane.nLanesCnt = lane.nLanesCnt;
    TransformToRosMsg_point(lane.ptLaneStart, ros_lane.ptLaneStart);
    TransformToRosMsg_point(lane.ptLaneEnd, ros_lane.ptLaneEnd);
    ros_lane.turnWaiting = lane.turnWaiting;
    ros_lane.nLaneType = lane.nLaneType;
    ros_lane.nSpeedLimit = lane.nSpeedLimit;
}
void RosInterface::TransformToRosMsg_link(const jf::link_& link, hd_map_msgs::link& ros_link) {
    ros_link.arregEdges.clear();
    for (jf::edge_ edge : link.arregEdges) {
        hd_map_msgs::edge ros_edge;
        TransformToRosMsg_edge(edge, ros_edge);
        ros_link.arregEdges.push_back(ros_edge);
    }
    ros_link.arrlnLanes.clear();
    for (jf::lane_ lane : link.arrlnLanes) {
        hd_map_msgs::lane ros_lane;
        TransformToRosMsg_lane(lane, ros_lane);
        ros_link.arrlnLanes.push_back(ros_lane);
    }
    ros_link.fEndAngle = link.fEndChangedAngle;
    ros_link.fStartAngle = link.fStartAngle;
    ros_link.nHighLimit = link.nHighLimit;
    ros_link.nRoadLevel = link.nRoadLevel;
    ros_link.nWeightLimit = link.nWeightLimit;
}

}  // namespace jf