#include "multi_traj_functions.h"
#include <pcl/octree/octree.h>

using namespace boost::filesystem;

vector<string> getActiveTraces(
        const string &activeTracePath)
{
    vector<string> ret;
    std::ifstream ifs(activeTracePath);
    if(!ifs){
        LOG(WARNING) << activeTracePath << " load fail!";
        return ret;
    }
    string line;
    while(getline(ifs, line)){
        ret.push_back(line);
    }
    ifs.close();
    return ret;
}

void getCrossTasks(
        const vector<string> &allTask,
        vector<CloseTrajInfo> &crossTasks)
{
    if(allTask.size() < 2){
        return;
    }
    vector<TaskInfo> allTaskInfo;
    TaskInfo taskInfo;
    if(!getTaskInfo(allTask.front(), taskInfo)){
        LOG(WARNING) << "currTask getTaskInfo fail!";
        return;
    }
    allTaskInfo.push_back(taskInfo);
    for(size_t i = 1; i < allTask.size(); i++){
        string task = allTask[i];
        if(boost::filesystem::path(task).filename().string() ==
                boost::filesystem::path(allTask.front()).filename().string()){
            continue;
        }
        if(!getTaskInfo(task, taskInfo)){
            continue;
        }
        allTaskInfo.push_back(taskInfo);
    }
    if(allTaskInfo.size() < 2){
        return;
    }
    vector<TaskInfo> closeTaskInfo = allTaskInfo;
    if(allTaskInfo.front().meshOutPath != ""){
        closeTaskInfo = meshOutFilter(allTaskInfo);
    }
//    closeTaskInfo.insert(closeTaskInfo.begin(), allTaskInfo.front());
    if(closeTaskInfo.size() < 2){
        LOG(INFO) << "closeTaskInfo.size() < 2";
        closeTaskInfo = allTaskInfo;
    }
    for(size_t i = 1; i < closeTaskInfo.size(); i++){
        TaskInfo task = closeTaskInfo[i];
        LOG(INFO) << "closeTask: " << task.taskPath;
    }
    crossTasks = crossFilter(closeTaskInfo);
}

bool getTaskInfo(
        const string &taskPath,
        TaskInfo &taskInfo)
{
    if (!is_directory(taskPath)){
//        LOG(INFO) << "no directory: " << taskPath;
        return false;
    }
    string rawTraceDir = taskPath + "/raw_trace";
    if(!is_directory(rawTraceDir)){
//        LOG(INFO) << "no directory: " << rawTraceDir;
        return false;
    }
    string trajPath = rawTraceDir + "/sample.gps";
    if(!exists(trajPath)){
       trajPath = rawTraceDir + "/ie.txt";
    }
    if(!exists(trajPath)){
//        LOG(INFO) << "dont exists: " << trajPath;
        return false;
    }
    taskInfo.taskPath = taskPath;
    taskInfo.trajPath = trajPath;
    string meshOutPath = taskPath + "/meshes.out";
    if(exists(meshOutPath)){
       taskInfo.meshOutPath = meshOutPath;
    }
    taskInfo.meshId =
            stoi(boost::filesystem::path(taskPath).filename().string());
    return true;
}

vector<TaskInfo> meshOutFilter(
        const vector<TaskInfo> &allTaskInfo){
    vector<TaskInfo> ret;
    if(allTaskInfo.size() < 2){
        return ret;
    }
    TaskInfo currTaskInfo = allTaskInfo.front();
    ret.push_back(currTaskInfo);
    MeshOut currMeshOut =
            parseMeshOut(currTaskInfo.meshOutPath);
    if(currMeshOut.size() == 0){
        LOG(INFO) << "currMeshOut.size() == 0!";
        return allTaskInfo;
    }
    for(size_t i = 1; i < allTaskInfo.size(); i++){
        TaskInfo taskInfo = allTaskInfo[i];
        if(checkMeshOut(parseMeshOut(taskInfo.meshOutPath),
                        currMeshOut)){
            ret.push_back(taskInfo);
        }
    }
    return ret;
}

MeshOut parseMeshOut(
        const string &meshOutPath){
    MeshOut ret;
    std::ifstream ifs(meshOutPath);
    if(!ifs){
        LOG(WARNING) << meshOutPath << " load fail!";
        return ret;
    }
    string line;
    while(getline(ifs, line)){
        vector<string> line_vec;
        boost::split(line_vec, line, boost::is_any_of(","), boost::token_compress_on);
        if(line_vec.size() != 2){
            continue;
        }
        ret.push_back(make_pair(line_vec.front(), line_vec.back()));
    }
    ifs.close();
    return ret;
}

bool checkMeshOut(
        const MeshOut& query,
        const MeshOut& target){
    for(auto &q : query){
        for(auto &t : target){
            if(q.first == t.first &&
                    q.second == t.second){
                return true;
            }
        }
    }
    return false;
}

PointCloudExport getCloudFromPeriod(
        const PointCloudExport::Ptr cloud,
        pair<uint64_t, uint64_t> period){
    PointCloudExport ret;
    for(const auto& p : *cloud){
        if(p.info > period.first &&
                p.info < period.second){
            ret.push_back(p);
        }
    }
    return ret;
}

bool checkCurrPeriodMerged(
        const vector<pair<size_t, size_t>>& mergedIndexVec,
        size_t retIndex, size_t periodIndex){
    for(auto mergedIndex : mergedIndexVec){
        if(mergedIndex.first == retIndex &&
                mergedIndex.second == periodIndex){
            return true;
        }
    }
    return false;

}

void filterByPosition(
        vector<CloseTrajInfo> &ret,
        const vector<PointCloudExport::Ptr> &pointClouds){
    vector<CloseTrajInfo> filteredRet;
    vector<pair<size_t, size_t>> mergedIndexVec;
    for(size_t retIndex = 0;  retIndex < ret.size(); retIndex++){
        const auto& oneRet = ret.at(retIndex);
        for(size_t periodIndex = 0; periodIndex < oneRet.periods.size(); periodIndex++){
            if(checkCurrPeriodMerged(mergedIndexVec, retIndex, periodIndex)){
                continue;
            }
            auto period = oneRet.periods.at(periodIndex);
            PointCloudExport periodCloud =
                    getCloudFromPeriod(pointClouds.at(oneRet.index), period);
            float resolution = 5.f;
            pcl::octree::OctreePointCloudSearch<PointExport> octree(resolution);
            octree.setInputCloud(boost::make_shared<PointCloudExport>(periodCloud));
            octree.addPointsFromInputCloud();
            vector<pair<size_t, size_t>> aroundIndexVec;
            for(size_t tarRetIndex = retIndex + 1;  tarRetIndex < ret.size(); tarRetIndex++){
                const auto& tarOneRet = ret.at(tarRetIndex);
                for(size_t tarPeriodIndex = 0; tarPeriodIndex < tarOneRet.periods.size(); tarPeriodIndex++){
                    if(checkCurrPeriodMerged(mergedIndexVec, tarRetIndex, tarPeriodIndex)){
                        continue;
                    }
                    auto tarPeriod = tarOneRet.periods.at(tarPeriodIndex);
                    PointCloudExport tarPeriodCloud =
                            getCloudFromPeriod(pointClouds.at(tarOneRet.index), tarPeriod);
                    for(const PointExport& query : tarPeriodCloud.points){
                        int result_index;
                        float sqr_distance;
                        octree.approxNearestSearch(query, result_index, sqr_distance);
                        if(sqrt(sqr_distance) < 30){
                            aroundIndexVec.push_back(make_pair(tarRetIndex, tarPeriodIndex));
                            break;
                        }
                    }
                }
            }
            pair<size_t, size_t> longestIndex = make_pair(retIndex, periodIndex);
            uint32_t longestPeriod = period.second - period.first;
            for(auto index : aroundIndexVec){
                auto aroundPeriod = ret.at(index.first).periods.at(index.second);
                uint32_t periodDuration = aroundPeriod.second - aroundPeriod.first;
                if(periodDuration > longestPeriod){
                    longestPeriod = periodDuration;
                    longestIndex = index;
                }
                mergedIndexVec.push_back(index);
            }
            size_t index = longestIndex.first;
            bool findIndex = false;
            for(auto &filteredOneRet : filteredRet){
                if(filteredOneRet.index == ret.at(index).index){
                    filteredOneRet.periods.push_back(ret.at(index).periods.at(longestIndex.second));
                    findIndex = true;
                    break;
                }
            }
            if(!findIndex){
                CloseTrajInfo closeTrajInfo;
                closeTrajInfo.index = ret.at(index).index;
                closeTrajInfo.periods.push_back(ret.at(index).periods.at(longestIndex.second));
                closeTrajInfo.trajPath = ret.at(index).trajPath;
                filteredRet.push_back(closeTrajInfo);
            }

        }
    }
    filteredRet.swap(ret);
}

vector<CloseTrajInfo> crossFilter(
        const vector<TaskInfo> &filteredTaskInfo){
    vector<CloseTrajInfo> ret;
    if(filteredTaskInfo.size() < 2){
        return ret;
    }

    vector<boost::shared_ptr<Trajectory>> trajectories(
                filteredTaskInfo.size());
    for(size_t i = 0; i < filteredTaskInfo.size(); i++){
        initTrajectory(filteredTaskInfo[i].trajPath, trajectories[i]);
    }
    boost::shared_ptr<Trajectory> currTrajectory =
            trajectories.front();
    vector<PointCloudExport::Ptr> pointClouds
            = multiThreadGetPointCloud(trajectories, currTrajectory->getProj());
    LOG(INFO) << "pointClouds.size(): " << pointClouds.size();

    float resolution = 5.f;

    pcl::octree::OctreePointCloudSearch<PointExport> octree(resolution);
    octree.setInputCloud(pointClouds.front());
    octree.addPointsFromInputCloud();
    pcl::io::savePCDFileBinary("/tmp/" + to_string(filteredTaskInfo[0].meshId) + ".pcd", *pointClouds[0]);
    for(size_t pcIndex = 1; pcIndex < pointClouds.size(); pcIndex++){
        pcl::io::savePCDFileBinary("/tmp/" + to_string(filteredTaskInfo[pcIndex].meshId) + ".pcd", *pointClouds[pcIndex]);
        bool found = false;
        bool inPeriod = false;
        uint64_t start = 0, end = 0;
        CloseTrajInfo oneRet;
        vector<pair<uint64_t, uint64_t>> timePeriods;
        float currTime;
        for(const PointExport& query : pointClouds[pcIndex]->points){
            int result_index;
            float sqr_distance;
            octree.approxNearestSearch(query, result_index, sqr_distance);
            if(sqrt(sqr_distance) < 30){
                found = true;
                if(!inPeriod){
                    start = query.info;
                    inPeriod = true;
//                    LOG(INFO) << setprecision(15) << "start: " << start;
                }
            }else if(sqrt(sqr_distance) > 40){
                if(inPeriod){
                    end = query.info;
                    inPeriod = false;
                    timePeriods.push_back(make_pair(start, end));
//                    LOG(INFO) << setprecision(15) << "end: " << end;
                }
            }
            currTime = query.info;
        }
        if(inPeriod){
            timePeriods.push_back(make_pair(start, currTime));
        }
        if(found){
            oneRet.index = pcIndex;
            oneRet.trajPath = filteredTaskInfo[pcIndex].taskPath;
            oneRet.periods = timePeriods;
            ret.push_back(oneRet);
        }
    }
    filterByPosition(ret, pointClouds);
    for(size_t retIndex = 0;  retIndex < ret.size(); retIndex++){
        const auto& oneRet = ret.at(retIndex);
        for(size_t periodIndex = 0; periodIndex < oneRet.periods.size(); periodIndex++){

            PointCloudExport periodCloud =
                    getCloudFromPeriod(pointClouds.at(oneRet.index), oneRet.periods.at(periodIndex));
            if(periodCloud.size()){
                pcl::io::savePCDFileBinary("/tmp/" + to_string(filteredTaskInfo[oneRet.index].meshId) +
                                           "_" + to_string(oneRet.periods.at(periodIndex).first) +
                                           ".pcd", periodCloud);
            }
        }
    }
    return ret;
}

bool initTrajectory(
        const string &trajPath,
        boost::shared_ptr<Trajectory> &trajectory)
{
    string trajFileName =
            boost::filesystem::path(trajPath).filename().string();
//    LOG(INFO) << "trajFileName: " << trajFileName;
    if(trajFileName == "ie.txt"){
        trajectory.reset(new Trajectory(trajPath, PPK));
    }else if(trajFileName == "sample.gps"){
        trajectory.reset(new Trajectory(trajPath, PPK, 10));
    }else{
        return false;
    }
    trajectory->init();
    return true;
}

vector<PointCloudExport::Ptr> multiThreadGetPointCloud(
        vector<boost::shared_ptr<Trajectory>> &trajectories,
        boost::shared_ptr<LocalCartesian> proj)
{
    vector<ThreadPtr> thread_vec;
    vector<PointCloudExport::Ptr> pointClouds;
    pointClouds.resize(trajectories.size());
    for(uint8_t thread_index = 0; thread_index < trajectories.size(); thread_index++){
        pointClouds[thread_index].reset(new PointCloudExport);
        thread_vec.emplace_back(ThreadPtr(new boost::thread(boost::bind(&getPointCloud,
                                                                        proj,
                                                                        trajectories[thread_index],
                                                                        pointClouds[thread_index]))));
    }
    for(uint8_t thread_index = 0; thread_index < thread_vec.size(); thread_index++){
        thread_vec[thread_index]->join();
        thread_vec[thread_index].reset();
    }
    return pointClouds;
}

void getPointCloud(boost::shared_ptr<LocalCartesian> proj,
        boost::shared_ptr<Trajectory> &trajectory,
        PointCloudExport::Ptr& pointCloud)
{
    if(!trajectory){
        return;
    }
    vector<TrajPoint> traj = trajectory->getTrajectory();
    for(const TrajPoint& trajPoint : traj){
        PointExport point;
        Vector3d llh = trajectory->reverse(trajPoint);
        Vector3d translation;
        proj->Forward(llh.x(),llh.y(),llh.x(),
                       translation.x(),
                       translation.y(),
                       translation.z());
        point.x = translation.x();
        point.y = translation.y();
        point.z = translation.z();
        point.info = trajPoint.timestamp;
        pointCloud->push_back(point);
    }
}