
#include "RosMapInfo.h"

namespace jf {

    RosMapInfo::RosMapInfo(): Thread("MapInfoProc") {
        node.setCallbackQueue(&my_queue);
        sub1 = node.subscribe("/localization_result", 1, &RosMapInfo::CallBack_ReceiveLocalization, this);
        sub2 = node.subscribe("/mm/AduToMM", 1, &RosMapInfo::CallBack_ReceiveAduToMM, this);
        publisher_hu = node.advertise<hu_msgs::DisplayObj>("hu/DisplayObj",10);
    }

    void RosMapInfo::CallBack_ReceiveLocalization(const localization_msgs::Localization &localizaitionMsg) {
        this->localization = localizaitionMsg;
    }

    void RosMapInfo::CallBack_ReceiveAduToMM(const adu_msgs::AduToMMInfo  &aduToMMInfoMsg) {
        this->aduToMmInfo = aduToMMInfoMsg;
    }

    /******线程管理相关****/
    bool RosMapInfo::Start() {
        return Thread::start();
    }

    bool RosMapInfo::Stop() {
        return Thread::stop(1000, true);
    }

    U32 RosMapInfo::run() {
        while (false == signalled()) {

            HUInfoHandle();
            ros::Rate rate(25);
            while (ros::ok) {
                HUInfoHandle();
                my_queue.callAvailable(ros::WallDuration());
                rate.sleep();
            }
             Stop();
        }
        stop_event().reset();
    }

    /********业务相关********/

    /**
     * 所有业务处理函数放在此处
     */
    void RosMapInfo::HUInfoHandle() {
        hu_msgs::DisplayObj displayObj;
        //121.201226,31.252951    121.23499610,31.27523929

        this->localization.pose_.position_.x_ = 121.23499610;
        this->localization.pose_.position_.y_ = 31.27523929;
        this->localization.pose_.position_.z_ = 0;
        this->localization.pose_.orientation_.quaternion_w_ = 351.871000;

        /******关联自车位置到相应车道*****/
        Point pointIn(this->localization.pose_.position_.x_, this->localization.pose_.position_.y_,
                      this->localization.pose_.position_.z_);
        displayObj.selfPosition.lon = this->localization.pose_.position_.x_;
        displayObj.selfPosition.lat = this->localization.pose_.position_.y_;
        displayObj.selfPosition.alt = this->localization.pose_.position_.z_;

        XYZ pointIn_Gauss = jf::Convert(pointIn, 3);
        std::vector<double> dev = {pointIn_Gauss.x, pointIn_Gauss.y, 0};//TODO 0处表示车辆航向角

        HdLinkId carHdLinkId;  //自车所在的link
        HdLaneId carHdLaneId;  //自车所在的lane
        if (!mapInfo.locateLinkandLane(pointIn, this->localization.pose_.orientation_.quaternion_w_, carHdLinkId,
                                       carHdLaneId))
            return;
        /******获取当前车道的一些属性******/
        bool LeftLane_Crossable = false; //是否可以变道到左侧相邻车道
        bool RightLane_Crossable = false; //是否可以变道到右侧相邻车道
        mapInfo.getCurrentLocationInfo(pointIn, 0, carHdLinkId, carHdLaneId, displayObj.speedLimit,
                                       displayObj.LeftLaneEdgeId1, displayObj.LeftLaneEdgeId2, LeftLane_Crossable,
                                       displayObj.RightLaneEdgeId1, displayObj.RightLaneEdgeId2, RightLane_Crossable);
        //车道颜色赋值
        if (LeftLane_Crossable) {
            if (this->aduToMmInfo.LaneChgAutStsForAsyHiWay == 7) {
                displayObj.LeftLaneColor = 2;  //蓝色 正在向左边变道
            } else if (this->aduToMmInfo.LaneChgAutStsForAsyHiWay == 3) {
                displayObj.LeftLaneColor = 3;  //红色 向左边变道失败
            } else {
                displayObj.LeftLaneColor = 0;  //绿色 可以向左边变道
            }
        } else {
            displayObj.LeftLaneColor = 1; //黄色 不能变道
        }
        if (RightLane_Crossable) {
            if (this->aduToMmInfo.LaneChgAutStsForAsyHiWay == 8) {
                displayObj.RightLaneColor = 2;  //蓝色 正在向左边变道
            } else if (this->aduToMmInfo.LaneChgAutStsForAsyHiWay == 4) {
                displayObj.RightLaneColor = 3;  //红色 向左边变道失败
            } else {
                displayObj.RightLaneColor = 0;  //绿色 可以向左边变道
            }
        } else {
            displayObj.RightLaneColor = 1; //黄色 不能变道
        }
        /*****组织视觉标志物和车道线的数据*****/
        EdgeInfoExport(carHdLinkId, carHdLaneId, pointIn, displayObj);
//        TargetsInfoExport(dev,displayObj);
//        PathSelectInfoExport(dev,displayObj);

        publisher_hu.publish(displayObj);


        /**********打印测试*********/
        std::string cstrFileName = "/home/muyusheng/JIKA_Edge.csv";
        std::string cstrFileName2 = "/home/muyusheng/JIKA_CARPOINT.csv";
        std::ofstream ofsQgisResult;
        std::ofstream ofsQgisResult2;
        // 创建csv文件
        ofsQgisResult.open(cstrFileName.c_str(), std::ios::out | std::ios::trunc);
        ofsQgisResult.setf(std::ios::fixed, std::ios::floatfield);
        ofsQgisResult.precision(15);

        ofsQgisResult2.open(cstrFileName2.c_str(), std::ios::out | std::ios::trunc);
        ofsQgisResult2.setf(std::ios::fixed, std::ios::floatfield);
        ofsQgisResult2.precision(15);
        // 插入标签
        ofsQgisResult << "lon"
                      << ", "
                      << "lat"
                      << ", "
                      << "ID"
                      << std::endl;

        ofsQgisResult2 << "lon"
                       << ", "
                       << "lat"
                       << ", "
                       << "ID"
                       << std::endl;
        ofsQgisResult2 << this->localization.pose_.position_.x_ << "," << this->localization.pose_.position_.y_ << ","
                       << 0 << std::endl;

        for (hu_msgs::RoadEdge roadEdge: displayObj.roadEdge) {
            for (hu_msgs::HuPoint huPoint: roadEdge.vctvdGeoLink) {
                ofsQgisResult << huPoint.lon << "," << huPoint.lat << "," << roadEdge.EdgeId << std::endl;
            }
        }

        ofsQgisResult.close();
        ofsQgisResult2.close();
    }

    /**
     * 道路边界线的处理
     */
    void RosMapInfo::EdgeInfoExport(HdLinkId &hdLinkId, HdLaneId &hdLaneId, Point &pointIn,
                                    hu_msgs::DisplayObj &displayObj) {

        std::deque<HdLinkId> hdLinkIdQueue;
        std::map<HdLinkId, std::map<int, Point>> hdLinkIdQueueTable;
        mapInfo.CreateMapInfo_First(hdLinkId, hdLaneId, pointIn, hdLinkIdQueue, hdLinkIdQueueTable);
        std::vector<HdEdge> vctheEdgeResult;
        mapInfo.CreateMapInfo_Second(hdLinkId, hdLaneId, pointIn, hdLinkIdQueue, hdLinkIdQueueTable, vctheEdgeResult);
        //组织边界线数据
        for (HdEdge hdEdge: vctheEdgeResult) {
            hu_msgs::RoadEdge roadEdge;
            roadEdge.EdgeId = hdEdge.lId;

            roadEdge.nType = hdEdge.nType;
            for (Point pointTmp: hdEdge.vctvdGeoLink) {
                hu_msgs::HuPoint huPoint;
                huPoint.lon = pointTmp.dLon;
                huPoint.lat = pointTmp.dLat;
                huPoint.alt = pointTmp.dAlt;
                roadEdge.vctvdGeoLink.push_back(huPoint);
            }
            //边界线类型
            if (hdEdge.vcthmMark.size() == 1) {
                roadEdge.LaneMarkType = hdEdge.vcthmMark[0].nType;
            } else if (hdEdge.vcthmMark.size() == 2) {
                if (hdEdge.vcthmMark[0].nType == 1) {
                    if (hdEdge.vcthmMark[1].nType == 1) {
                        roadEdge.LaneMarkType = 6; //双实线
                    } else if (hdEdge.vcthmMark[1].nType == 2) {
                        roadEdge.LaneMarkType = 8; //左实右虚
                    }
                } else if (hdEdge.vcthmMark[0].nType == 2) {
                    if (hdEdge.vcthmMark[1].nType == 1) {
                        roadEdge.LaneMarkType = 6; //左虚右实
                    } else if (hdEdge.vcthmMark[1].nType == 2) {
                        roadEdge.LaneMarkType = 8; //双虚
                    }
                }
            }
            displayObj.roadEdge.push_back(roadEdge);
        }
    }

    /**
     * 视觉Targets处理
     *
     * @param dev 父坐标和航向角（高斯坐标系）
     */
    void RosMapInfo::TargetsInfoExport(const std::vector<double> &dev, hu_msgs::DisplayObj &displayObj) {

        /**
         * 组织Target数据
         */
        auto TransformTargets = [this, &dev](adu_msgs::AduTargetInfo &aduTargetInfo,
                                             hu_msgs::VisionTarget &visionTarget) {
            visionTarget.TargetId = aduTargetInfo.Idn;
            visionTarget.ObjType = aduTargetInfo.ObjType;
            visionTarget.direction = aduTargetInfo.AgDir;

            std::vector<double> objPointVct = {aduTargetInfo.PosnLgt, aduTargetInfo.PosnLat};
            jf::Point resPoint;
            coordinateTranslation(objPointVct, dev, resPoint);
            visionTarget.position.lon = resPoint.dLon;
            visionTarget.position.lat = resPoint.dLat;
            visionTarget.position.alt = 0;
        };

        uint8_t mainTargetId = this->aduToMmInfo.TgtFrstClstLane;
        uint8_t eventTargetId = this->aduToMmInfo.EventTgt;
        for (adu_msgs::AduTargetInfo aduTargetInfo: this->aduToMmInfo.FusedTargets) {
            if (aduTargetInfo.Idn == mainTargetId) {
                TransformTargets(aduTargetInfo, displayObj.mainTarget);
            }
            if (aduTargetInfo.Idn == eventTargetId) {
                TransformTargets(aduTargetInfo, displayObj.eventTarget);
            }
            //TODO OtherTarget暂时没有输出
        }
    }


    /**
     * 路径指示处理
     * @param dev
     */
    void RosMapInfo::PathSelectInfoExport(const std::vector<double> &dev, hu_msgs::DisplayObj &displayObj) {

        for (int i = 0; i < this->aduToMmInfo.path_x.size(); i++) {
            std::vector<double> obj = {this->aduToMmInfo.path_x[i], this->aduToMmInfo.path_y[i]};
            jf::Point resPoint;
            coordinateTranslation(obj, dev, resPoint);
            hu_msgs::HuPoint huPoint;
            huPoint.lon = resPoint.dLon;
            huPoint.lat = resPoint.dLat;
            huPoint.alt = resPoint.dAlt;
            displayObj.PathSelect.push_back(huPoint);
        }
    }


    /**
     * 平面坐标的转换
     * @param obj 要转换的子坐标
     * @param dev 父坐标和航向角（高斯坐标系）
     * @param result 转换结果
     * @return
     */
    bool RosMapInfo::coordinateTranslation(const std::vector<double> &obj, const std::vector<double> &dev,
                                           jf::Point &result) {

        double objtmp_x = 0.0;
        double objtmp_y = 0.0;
        double dev_heading = (90 - dev[2]) / 180.0 * jf::PI;
        objtmp_x = obj[0] * cos(dev_heading) + obj[1] * sin(dev_heading);
        objtmp_y = obj[1] * cos(dev_heading) - obj[0] * sin(dev_heading);

        double objWorld_x, objWorld_y;
        objWorld_x = dev[0] + objtmp_x;
        objWorld_y = dev[1] + objtmp_y;

        XYZ objWorld(objWorld_x, objWorld_y, 1);
        result = jf::Inverse(objWorld, 3);
        return true;
    }
}