#include "fskApp.h"

#include "MapConfig.h"

using namespace jf;

void fskApp::Init(const std::string &projectDir, const string &configJsonPath) {
    MapConfig::Init(projectDir, configJsonPath);
<<<<<<< HEAD
=======
    // HdLib lib = {};
>>>>>>> bosch

#define PrintfRadas 0
#if PrintRadas
    auto func_printInfo = [](ofstream &ofs, const hdradas &radas) {
        if (ofs.is_open()) {
            ajson::string_stream ss = {};
            ajson::save_to(ss, radas);
            std::string s = ss.str();

            ofs.setf(std::ios::fixed, std::ios::floatfield);
            ofs.precision(15);
            ofs << ss.str() << std::endl;
        }
    };

    std::vector<Point> vctptNodes = {Point(121.43489937, 24.96102204), Point(121.48922824, 24.98743922), Point(121.55536731, 24.97467970), Point(121.27893400, 25.11639777), Point(121.283009075, 24.995459583)};
    for (auto p : vctptNodes) {
        Point ptNear = {};
        LaneEndpoint stEp = {};
        double dDistance = 0;
        lib.GetNearestEndpoint(p, MapConfig::route_rect_lon, MapConfig::route_rect_lat, ptNear, stEp, dDistance);
        stringstream ssName;
        ssName << stEp.lLinkId;
        ofstream ofsInfo = {};
        ofsInfo.open(ssName.str().c_str(), std::ios::out | std::ios::trunc);
        HdLink stLink = {};
        lib.GetLinkData(stEp.lLinkId, stLink);

        ofsInfo << "{" << std::endl;

        ofsInfo << "\"PrePreLink\":[" << std::endl;

        for (auto lPreId : stLink.Plinks) {
            HdLink stPreLink = {};
            lib.GetLinkData(lPreId, stPreLink);

            // 前前组link
            for (auto lPrePreId : stPreLink.Plinks) {
                HdLink stPrePreLink = {};
                lib.GetLinkData(lPrePreId, stPrePreLink);
                for (auto stRadas : stPrePreLink.radases) {
                    func_printInfo(ofsInfo, stRadas);
                    ofsInfo << "," << std::endl;
                }
            }
        }

        ofsInfo << "]," << std::endl;
        ofsInfo << "\"PreLink\":[" << std::endl;

        for (auto lPreId : stLink.Plinks) {
            HdLink stPreLink = {};
            lib.GetLinkData(lPreId, stPreLink);

            // 前组link
            for (auto stRadas : stPreLink.radases) {
                func_printInfo(ofsInfo, stRadas);
                ofsInfo << "," << std::endl;
            }
        }

        ofsInfo << "]," << std::endl;
        ofsInfo << "\"CurLink\":[" << std::endl;

        // 当前组link
        for (auto stRadas : stLink.radases) {
            func_printInfo(ofsInfo, stRadas);
            ofsInfo << "," << std::endl;
        }

        ofsInfo << "]," << std::endl;
        ofsInfo << "\"NxtLink\":[" << std::endl;

        for (auto lNxtId : stLink.Nlinks) {
            HdLink stNxtLink = {};
            lib.GetLinkData(lNxtId, stNxtLink);

            // 后组link
            for (auto stRadas : stNxtLink.radases) {
                func_printInfo(ofsInfo, stRadas);
                ofsInfo << "," << std::endl;
            }
        }

        ofsInfo << "]," << std::endl;
        ofsInfo << "\"NxtNxtLink\":[" << std::endl;

        for (auto lNxtId : stLink.Nlinks) {
            HdLink stNxtLink = {};
            lib.GetLinkData(lNxtId, stNxtLink);

            // 后后一组link
            for (auto lNxtNxtId : stNxtLink.Nlinks) {
                HdLink stNxtNxtLink = {};
                lib.GetLinkData(lNxtNxtId, stNxtNxtLink);
                for (auto stRadas : stNxtNxtLink.radases) {
                    func_printInfo(ofsInfo, stRadas);
                    ofsInfo << "," << std::endl;
                }
            }
        }

        ofsInfo << "]" << std::endl;

        ofsInfo << "}" << std::endl;

        ofsInfo.close();
    }
#endif

#define Test 1
#if Test
    ofstream ofsLog = {};
    ofsLog.open(MapConfig::road_plan_file_dir + "Bc.log", std::ios::out | std::ios::trunc);
    ofsLog.setf(std::ios::fixed, std::ios::floatfield);
    ofsLog.precision(15);

    ofstream ofsQgis = {};
    ofsQgis.open(MapConfig::road_plan_file_dir + "Bc.csv", std::ios::out | std::ios::trunc);
    ofsQgis.setf(std::ios::fixed, std::ios::floatfield);
    ofsQgis.precision(15);
    ofstream ofsPlanExitQgis = {};
    ofsPlanExitQgis.open(MapConfig::road_plan_file_dir + "PlanExit.csv", std::ios::out | std::ios::trunc);
    ofsPlanExitQgis.setf(std::ios::fixed, std::ios::floatfield);
    ofsPlanExitQgis.precision(15);
    ofstream ofsPlanEnterQgis = {};
    ofsPlanEnterQgis.open(MapConfig::road_plan_file_dir + "PlanEnter.csv", std::ios::out | std::ios::trunc);
    ofsPlanEnterQgis.setf(std::ios::fixed, std::ios::floatfield);
    ofsPlanEnterQgis.precision(15);
    ofstream ofsPlanSpecialRoadQgis = {};
    ofsPlanSpecialRoadQgis.open(MapConfig::road_plan_file_dir + "PlanSpecialRoad.csv", std::ios::out | std::ios::trunc);
    ofsPlanSpecialRoadQgis.setf(std::ios::fixed, std::ios::floatfield);
    ofsPlanSpecialRoadQgis.precision(15);
    ofstream ofsPlanLaneChgQgis = {};
    ofsPlanLaneChgQgis.open(MapConfig::road_plan_file_dir + "PlanLaneChg.csv", std::ios::out | std::ios::trunc);
    ofsPlanLaneChgQgis.setf(std::ios::fixed, std::ios::floatfield);
    ofsPlanLaneChgQgis.precision(15);
    ofsPlanLaneChgQgis << "Lon,"
                       << "Lat,"
                       << "Dis" << std::endl;
    ofstream ofsPlanIntervQgis = {};
    ofsPlanIntervQgis.open(MapConfig::road_plan_file_dir + "PlanInterv.csv", std::ios::out | std::ios::trunc);
    ofsPlanIntervQgis.setf(std::ios::fixed, std::ios::floatfield);
    ofsPlanIntervQgis.precision(15);
    ofstream ofsPlanEndQgis = {};
    ofsPlanEndQgis.open(MapConfig::road_plan_file_dir + "PlanEnd.csv", std::ios::out | std::ios::trunc);
    ofsPlanEndQgis.setf(std::ios::fixed, std::ios::floatfield);
    ofsPlanEndQgis.precision(15);
    ofstream ofsNaviTunnelQgis = {};
    ofsNaviTunnelQgis.open(MapConfig::road_plan_file_dir + "NaviTunnel.csv", std::ios::out | std::ios::trunc);
    ofsNaviTunnelQgis.setf(std::ios::fixed, std::ios::floatfield);
    ofsNaviTunnelQgis.precision(15);
    ofsNaviTunnelQgis << "Lon,"
                      << "Lat,"
                      << "Dis" << std::endl;
    ofstream ofsNaviSpeedChgQgis = {};
    ofsNaviSpeedChgQgis.open(MapConfig::road_plan_file_dir + "NaviSpeedChg.csv", std::ios::out | std::ios::trunc);
    ofsNaviSpeedChgQgis.setf(std::ios::fixed, std::ios::floatfield);
    ofsNaviSpeedChgQgis.precision(15);
    ofstream ofsNaviConfBifQgis = {};
    ofsNaviConfBifQgis.open(MapConfig::road_plan_file_dir + "NaviConfBif.csv", std::ios::out | std::ios::trunc);
    ofsNaviConfBifQgis.setf(std::ios::fixed, std::ios::floatfield);
    ofsNaviConfBifQgis.precision(15);
    ofstream ofsNaviExitQgis = {};
    ofsNaviExitQgis.open(MapConfig::road_plan_file_dir + "NaviExit.csv", std::ios::out | std::ios::trunc);
    ofsNaviExitQgis.setf(std::ios::fixed, std::ios::floatfield);
    ofsNaviExitQgis.precision(15);
    ofstream ofsNaviEnterQgis = {};
    ofsNaviEnterQgis.open(MapConfig::road_plan_file_dir + "NaviEnter.csv", std::ios::out | std::ios::trunc);
    ofsNaviEnterQgis.setf(std::ios::fixed, std::ios::floatfield);
    ofsNaviEnterQgis.precision(15);
    ofstream ofsPassibleLanesStartQgis = {};
    ofsPassibleLanesStartQgis.open(MapConfig::road_plan_file_dir + "PassibleLanesStartQgis.csv", std::ios::out | std::ios::trunc);
    ofsPassibleLanesStartQgis.setf(std::ios::fixed, std::ios::floatfield);
    ofsPassibleLanesStartQgis.precision(15);
    ofsPassibleLanesStartQgis << "Lon,"
                              << "Lat,"
                              << "Alt" << std::endl;
    ofstream ofsSpeedStartQgis = {};
    ofsSpeedStartQgis.open(MapConfig::road_plan_file_dir + "ofsSpeedStartQgis.csv", std::ios::out | std::ios::trunc);
    ofsSpeedStartQgis.setf(std::ios::fixed, std::ios::floatfield);
    ofsSpeedStartQgis.precision(15);
    ofsSpeedStartQgis << "Lon,"
                      << "Lat,"
                      << "Speed" << std::endl;
#endif

    roadPlanParam p;
    p.vctvptWayNode = {
        {
            Point(121.4321871, 24.9606813, 0),
            Point(121.4870398, 24.9884947, 0),
            Point(121.5247922, 24.9690884, 0),
            Point(121.5557046, 24.9746703, 0),
            Point(121.5951300, 25.0017709, 0),
            Point(121.6217626, 25.0366728, 0),
            Point(121.6438142, 25.0666160, 0),
        },
        {
            Point(121.6410861, 25.0720361, 0),
            Point(121.5256492, 25.0751276, 0),
            Point(121.4377254, 25.0688255, 0),
            Point(121.2905429, 25.0331866, 0),
            Point(121.2720344, 25.0176878, 0),
            Point(121.2164717, 24.9700719, 0),
            Point(121.1417027, 24.8978488, 0),
            Point(121.0445985, 24.8769026, 0),
            Point(120.9919198, 24.7606049, 0),
        },
        {
            Point(120.9926276, 24.7558282, 0),
            Point(121.1875660, 24.7966995, 0),
            Point(121.2499723, 24.8783621, 0),
            Point(121.3248686, 24.9391803, 0),
            Point(121.4323087, 24.9607702, 0),
        },
    };
    p.vctvptHumanDriveAreaNode = {
        {
            Point(121.432545, 24.965091, 0),
            Point(121.431275, 24.959825, 0),
        },
        {
            Point(121.395197, 25.146547, 0),
            Point(121.384966, 25.145039, 0),
        },
        {
            Point(121.291221, 25.117828, 0),
            Point(121.279057, 25.116516, 0),
        },
        {
            Point(121.427773, 24.959229, 0),
            Point(121.432789, 24.964025, 0),
        },
    };
    p.vctptInterventionNode = {
        // Point(121.291221, 25.117828, 0),
        // Point(121.285534, 25.116753, 0),
    };
    // p.vctptInterventionNode = {Point(121.392947, 25.149975, 0)};
    p.vctptEndNode = {
        Point(121.398693, 25.140424, 0),
        Point(121.291349, 25.117877, 0),
        Point(121.427462, 24.959411, 0),
    };
    p.vctlUnpassableLane = {2216475199, 2217235399};
    p.plan_past_dis = 100;
    p.plan_future_dis = 5000;
    p.navi_future_dis = 500;
    p.path_file_name = MapConfig::road_plan_file_dir + "path.json";
    HdWorld::getSingleton()->LoadAllTilesOnce();
    m_rdPlan.Init(p);

    // std::vector<Point> vctptNewNode = {Point(121.4622043, 25.0674354),
    // Point(121.4326912, 24.9604906)}; std::vector<Point> vctptNewNode =
    // {Point(121.4622043, 25.0674354), Point(121.4328753, 24.9640254)};
    // std::vector<Point> vctptNewNode = {Point(121.4622043, 25.0674354),
    // Point(121.3924994, 25.1500034)}; std::vector<Point> vctptNewNode =
    // {Point(121.4622043, 25.0674354), Point(121.3114102, 24.9292747)};
    // std::vector<Point> vctptNewNode = {Point(121.39274094, 25.15004775),
    // Point(121.39270309, 25.15014851)}; std::vector<Point> vctptNewNode =
    // {Point(121.4523141, 25.0750931), Point(121.39270309, 25.15014851)};
    // std::vector<Point> vctptNewNode = {Point(121.39261758, 25.15014827),
    // Point(121.39276026, 25.14991998)}; std::vector<Point> vctptNewNode =
    // {Point(121.39261758, 25.15014827), Point(121.2329071, 25.0762655)};
    // std::vector<Point> vctptNewNode = {Point(121.3569805, 24.9368172),
    // Point(121.2152917, 25.0540343)}; std::vector<Point> vctptNewNode =
    // {Point(121.2150469, 25.0541833), Point(121.42972579, 24.95874756)};
    // std::vector<Point> vctptNewNode = {Point(121.43238610, 24.96318580),
    // Point(121.3927427, 25.1500476), Point(121.43469378, 24.96131120)};
    // m_rdPlan.ResetPath(vctptNewNode);
    // return;

#if Test
    std::vector<LinkEndpoint> vctLinkPath = m_rdPlan.GetPath();
    for (size_t i = 0; i < vctLinkPath.size(); i++) {
        const LinkEndpoint &stLinkEp = vctLinkPath[i];
        jf_log_info("[fskApp::init] current index {0}, link lValue {1}", i, stLinkEp.lLinkId.lValue);
        ofsLog << "Link Id: " << stLinkEp.lLinkId.lValue << std::endl;
        const HdLink *phlLink = lib.GetLinkData(stLinkEp.lLinkId);
        // lib.LoadTile(stLinkEp.ptEpNode);
        if (phlLink) {
            if (phlLink->vctvdGeoLink.empty() == false) ofsSpeedStartQgis << phlLink->vctvdGeoLink[0][0] << "," << phlLink->vctvdGeoLink[0][1] << "," << phlLink->nSpeedLimit << std::endl;
            std::vector<const HdLane *> vcthlAllLanes = {};
            std::vector<const HdLane *> vcthlPassableLanes = {};
            m_rdPlan.GetPassableLanes(phlLink->liId, vcthlAllLanes, vcthlPassableLanes);
            for (auto lane : vcthlPassableLanes) {
                ofsPassibleLanesStartQgis << lane->vctvdGeoLink[0][0] << "," << lane->vctvdGeoLink[0][1] << "," << lane->vctvdGeoLink[0][2] << std::endl;
            }

            for (auto node : phlLink->vctvdGeoLink) {
                Point ptCurPos = {node[0], node[1], node[2]};
                std::vector<RoadPlanFrame1> vctPlanFrame1 = {};
                std::vector<RoadPlanFrame2> vctPlanFrame2 = {};
                // if (phlLink->lValue.lValue == 2195093221)
                // {
                //     ptCurPos = {121.23780020683823, 25.03486448179869, 0};
                // }
                if (m_rdPlan.GetPlanInfo(ptCurPos, phlLink->liId, vctPlanFrame1, vctPlanFrame2)) {
                    ofsLog << "PlanFrame: " << std::endl;

                    for (size_t i = 0; i < vctPlanFrame1.size(); i++) {
                        ofsLog << "Lon: " << vctPlanFrame1[i].lon << ", Lat: " << vctPlanFrame1[i].lat << std::endl;
                        ofsLog << "distance: " << vctPlanFrame2[i].distance << ", changePointType: " << vctPlanFrame2[i].changePointType << ", changePointLane: " << vctPlanFrame2[i].changePointLane << ", speedLimit: " << vctPlanFrame2[i].speedLimit
                               << ", changePointNum: " << vctPlanFrame2[i].changeLaneNum << ", isOut: " << vctPlanFrame2[i].isOut << std::endl;

                        ofsQgis << vctPlanFrame1[i].lon << ", " << vctPlanFrame1[i].lat << std::endl;

                        switch (vctPlanFrame2[i].changePointType) {
                            case 1:
                            case 2:
                                ofsPlanExitQgis << vctPlanFrame1[i].lon << ", " << vctPlanFrame1[i].lat << std::endl;
                                break;
                            case 3:
                            case 4:
                                ofsPlanEnterQgis << vctPlanFrame1[i].lon << ", " << vctPlanFrame1[i].lat << std::endl;
                                break;
                            case 5:
                            case 6:
                                ofsPlanSpecialRoadQgis << vctPlanFrame1[i].lon << ", " << vctPlanFrame1[i].lat << std::endl;
                                break;
                            case 7:
                            case 8:
                                ofsPlanLaneChgQgis << ptCurPos.lon << ", " << ptCurPos.lat << ", " << vctPlanFrame2[i].distance << std::endl;
                                ofsPlanLaneChgQgis << vctPlanFrame1[i].lon << ", " << vctPlanFrame1[i].lat << std::endl;
                                break;
                            case 9:
                                ofsPlanIntervQgis << vctPlanFrame1[i].lon << ", " << vctPlanFrame1[i].lat << std::endl;
                                break;
                            case 10:
                                ofsPlanEndQgis << vctPlanFrame1[i].lon << ", " << vctPlanFrame1[i].lat << std::endl;
                                break;

                            default:
                                break;
                        }
                    }
                }
                std::vector<RoadNaviFrame1> vctNaviFrame1 = {};
                std::vector<RoadNaviFrame2> vctNaviFrame2 = {};
                if (m_rdPlan.GetNaviInfo(ptCurPos, phlLink->liId, vctNaviFrame1, vctNaviFrame2)) {
                    ofsLog << "NaviFrame: " << std::endl;

                    for (size_t i = 0; i < vctNaviFrame1.size(); i++) {
                        ofsLog << "Lon: " << vctNaviFrame1[i].lon << ", Lat: " << vctNaviFrame1[i].lat << std::endl;
                        ofsLog << "distance: " << vctNaviFrame2[i].distance << ", changePointType: " << vctNaviFrame2[i].changePointType << ", changePointLane: " << vctNaviFrame2[i].changePointLane << std::endl;

                        ofsQgis << vctNaviFrame1[i].lon << ", " << vctNaviFrame1[i].lat << std::endl;

                        switch (vctNaviFrame2[i].changePointType) {
                            case 1:
                            case 2:
                                ofsNaviTunnelQgis << ptCurPos.lon << ", " << ptCurPos.lat << ", " << vctNaviFrame2[i].distance << std::endl;
                                ofsNaviTunnelQgis << vctNaviFrame1[i].lon << ", " << vctNaviFrame1[i].lat << ", " << 0 << std::endl;
                                break;
                            case 3:
                                ofsNaviSpeedChgQgis << vctNaviFrame1[i].lon << ", " << vctNaviFrame1[i].lat << std::endl;
                                break;
                            case 4:
                            case 5:
                            case 6:
                                ofsNaviConfBifQgis << vctNaviFrame1[i].lon << ", " << vctNaviFrame1[i].lat << std::endl;
                                break;
                            case 7:
                                ofsNaviExitQgis << vctNaviFrame1[i].lon << ", " << vctNaviFrame1[i].lat << std::endl;
                                break;
                            case 8:
                                ofsNaviEnterQgis << vctNaviFrame1[i].lon << ", " << vctNaviFrame1[i].lat << std::endl;
                                break;

                            default:
                                break;
                        }
                    }
                }
            }
        }
    }

    ofsLog.close();
    ofsQgis.close();
    ofsPlanExitQgis.close();
    ofsPlanEnterQgis.close();
    ofsPlanSpecialRoadQgis.close();
    ofsPlanLaneChgQgis.close();
    ofsPlanIntervQgis.close();
    ofsPlanEndQgis.close();
    ofsNaviTunnelQgis.close();
    ofsNaviSpeedChgQgis.close();
    ofsNaviConfBifQgis.close();
    ofsNaviExitQgis.close();
    ofsNaviEnterQgis.close();
    ofsPassibleLanesStartQgis.close();
    ofsSpeedStartQgis.close();
#endif
}

void fskApp::run() {}

#ifdef STANDALONE
int main(int argc, char **argv) {
    assert(argc == 3);
    fskApp app;
    app.Init(argv[1], argv[2]);
    app.Run();
    return 0;
}
#endif
