#include <iterator>
#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <string>
#include <stdlib.h>
#include <time.h>

//#include <opencv2/core.hpp>
#include <opencv2/opencv.hpp>

#include <unordered_map>

#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/gicp.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/registration/transformation_validation_euclidean.h>

#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/gicp.h>
#include <pcl/registration/ia_ransac.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/registration/transformation_validation_euclidean.h>

#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/registration/icp.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/features/shot.h>
#include <pcl/search/kdtree.h>

#include <boost/thread/thread.hpp>
#include <boost/filesystem.hpp>
#include <pcl-1.8/pcl/common/eigen.h>
#include <pcl-1.8/pcl/registration/transformation_validation.h>

#include <pcl/filters/extract_indices.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>

#include <limits>
#include <fstream>
#include <vector>
#include <Eigen/Core>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/registration/ia_ransac.h>

#include "CeresICP.h"
#include "utils.h"
#include "ModelRepresentation.h"
#include "LidarPointCloudHandler.h"

const bool IS_DEBUG = false;
typedef  pcl::PointCloud<pcl::PointXYZ>::Ptr CloudPointer;
typedef pcl::PointXYZI PointType;

void findAllObject(CloudPointer laserCloudSrc, CloudPointer laserCloudRef, std::vector<float> transformMatrix,
                   std::string folderName){
    // Transform to raw if the transform is given
    if(transformMatrix.size()>0) {
        utils::pointCloudMapToRaw(laserCloudSrc, transformMatrix);
        utils::pointCloudMapToRaw(laserCloudRef, transformMatrix);
    }
    std::unique_ptr<LidarPointCloudHandler> cloudHandler(new LidarPointCloudHandler(laserCloudSrc));
    cloudHandler->setReferenceCloud(laserCloudRef);
    CloudPointerRGB cloudIntegration = cloudHandler->pointCloudDecomposition();

    utils::myPCLViewer::getInstance()->changePointCloud(cloudIntegration);
    // Save the result
    CloudPointer staticObject(new pcl::PointCloud<pcl::PointXYZ>);
    CloudPointer movingObject(new pcl::PointCloud<pcl::PointXYZ>);
    CloudPointer groundObject(new pcl::PointCloud<pcl::PointXYZ>);
    for(int i = 0; i < cloudIntegration->points.size(); i++){
        pcl::PointXYZ newPoint;
        pcl::PointXYZRGB curPoint = cloudIntegration->points[i];
        newPoint.x = curPoint.x;
        newPoint.y = curPoint.y;
        newPoint.z = curPoint.z;
        if(transformMatrix.size() >0){
//            if(!utils::checkWithEffectiveRegion(newPoint, transformMatrix)){
//                continue;
//            }
        }
        else{
            if(!utils::checkWithPriorInfo(newPoint)){
                continue;
            }
        }
        if(curPoint.r == 255){
            if(true) {  // Check with some prior info here
                staticObject->points.push_back(newPoint);   // Notice
                groundObject->points.push_back(newPoint);
            }
        }
        if(curPoint.g == 255){
            if(true) {  // Check with some prior info here
                staticObject->points.push_back(newPoint);
            }
        }
        else if(curPoint.b == 255){
            if(true) {  // Check with some prior info here
                //if(utils::checkWithEffectiveRegion(newPoint))//, transformMatrix))
                    movingObject->points.push_back(newPoint);
            }
        }
    }
    utils::pointCloudRawToMap(movingObject, transformMatrix);
    utils::pointCloudRawToMap(staticObject, transformMatrix);
    // save all the objects into a folder
    //std::string SaveFolderPrefix = "/home/lifan/result_all/" + folderName;
    //boost::filesystem::path dir(SaveFolderPrefix);
    //boost::filesystem::create_directory(dir);
    std::string SaveFolderPrefix = "/home/lifan/LIDAR/result_all/";
    //SaveFolderPrefix = SaveFolderPrefix + '/';
    //std::string groundFileName = folderName + "_ground";
    std::string movingFileName = folderName + "_moving";
    std::string staticFileName = folderName + "_static";
    //std::string curbFileName = folderName + "_curb";
    //utils::pclPointCloudWriter(SaveFolderPrefix, groundFileName, groundObject);
    utils::pclPointCloudWriter(SaveFolderPrefix, movingFileName, movingObject);
    utils::pclPointCloudWriter(SaveFolderPrefix, staticFileName, staticObject);
    //utils::pclPointCloudWriter(SaveFolderPrefix, curbFileName, curbObject);

}

std::vector<std::pair<int, int>> clusterMatchingICP(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clusters1,
                                                    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clusters2)
{
    // parameters
    int pointNumThreshold = 100;  // a cloud contains less points will be discarded.
    double scoreThreshold = 0.0;  // matches under this will be discarded.
    int maxIterNum = 100000;
    double distanceThreshold = 5;

    int clusterNum1 = clusters1.size();
    int clusterNum2 = clusters2.size();

    std::vector<std::pair<int, int>> matches;

    // create ICP and config
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    // Set the max correspondence distance to 5m
    icp.setMaxCorrespondenceDistance(3); // 5

    // Set the maximum number of iterations (criterion 1)
    icp.setMaximumIterations(1000000);   // 100000  //1000000

    // Set the transformation epsilon (criterion 2)
    // Set the euclidean distance difference epsilon (criterion 3)
    icp.setEuclideanFitnessEpsilon(0.01);   // 3

    std::set<int> unmatched;
    std::vector<double> distVec;
    for (int i = 0; i < clusterNum2; i++){
        unmatched.insert(i);
    }

    for (int i = 0; i < clusterNum1; i++){
        std::cout<< i <<endl;
        icp.setInputSource(clusters1[i]);
        double maxScore = 0;
        int bestMatch = -1;
        for (int j = 0; j< clusterNum2; j++){
            if (unmatched.find(j) == unmatched.end()){        //  first come, first use rule. may use a global optima later
                continue;
            }
            icp.setInputTarget(clusters2[j]);
            pcl::PointCloud<pcl::PointXYZ> Final;
            icp.align(Final);
            bool isConverge = icp.hasConverged();
            if (!isConverge){
                continue;
            }
            double score = icp.getFitnessScore();
            std::cout << "has converged:" << isConverge << " score: " <<
                      score << std::endl;
            std::cout << icp.getFinalTransformation() << std::endl;
            if (score > maxScore and score >= scoreThreshold){     // if current score is larger and is over the threshold, save it
//                pcl::PointCloud<pcl::PointXYZ>::Ptr transformed = utils::transformCloudFromMatrix(icp.getFinalTransformation(), clusters1[i]);
//                utils::pairwiseVisualizeCloud(transformed, clusters2[j]);
                maxScore = score;
                bestMatch = j;
            }
        }
        if (bestMatch == -1){   // no valid match found
            continue;
        }
        unmatched.erase(bestMatch);   // remove from unmatched clusters
        matches.push_back(std::make_pair(i, bestMatch));
        double distance = utils::calcDistanceBetweenClouds(*clusters1[i], *clusters2[bestMatch]);
        distVec.push_back(distance);
        std::cout << i<< "  " << bestMatch <<endl;
    }

    // show all matches
//    for(int i =0; i<matches.size(); i++){
//        const std::string current = "Match " +  std::to_string(matches[i].first) + " on " + std::to_string(matches[i].second) +
//                                    " with distance of " + std::to_string(distVec[i]);
//        cout << current << endl;
//        utils::pairwiseVisualizeCloud(clusters1[matches[i].first], clusters2[matches[i].second], current);
//    }
    return matches;

}

std::vector<std::pair<int, int>> clusterMatchingICP(std::string pointCloudPath1,
                                                    int clusterNum1, std::string pointCloudPath2, int clusterNum2)
{
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clusters1;
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clusters2;
    std::vector<std::pair<int, int>> matches;

    clusters1 = utils::loadCloudsFromDirectory(pointCloudPath1, clusterNum1);
    clusters2 = utils::loadCloudsFromDirectory(pointCloudPath2, clusterNum2);
    return clusterMatchingICP(clusters1, clusters2);
}

// TOCHECK the threshold here
int clusterRematchingICP(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> templates,
                         std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> targets){
    // create ICP and config
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    icp.setMaxCorrespondenceDistance(3);
    icp.setMaximumIterations(1000000);
    icp.setEuclideanFitnessEpsilon(0.01);
    double scoreThreshold = 0.0;

    icp.setInputSource(templates[templates.size()-1]);
    double maxScore = 0;
    int bestMatch = -1;
    for (int j = 0; j< targets.size(); j++){
        icp.setInputTarget(targets[j]);
        pcl::PointCloud<pcl::PointXYZ> Final;
        icp.align(Final);
        bool isConverge = icp.hasConverged();
        if (!isConverge){
            continue;
        }
        double score = icp.getFitnessScore();
        std::cout << "has converged:" << isConverge << " score: " <<
                  score << std::endl;
        std::cout << icp.getFinalTransformation() << std::endl;
        if (score > maxScore and score >= scoreThreshold){     // if current score is larger and is over the threshold, save it
//                pcl::PointCloud<pcl::PointXYZ>::Ptr transformed = utils::transformCloudFromMatrix(icp.getFinalTransformation(), clusters1[i]);
//                utils::pairwiseVisualizeCloud(transformed, clusters2[j]);
            maxScore = score;
            bestMatch = j;
        }
    }
    return bestMatch;

}

// do multi-frame matching among a list of clouds, based on the first frame
// assume all cluster in first frame is valid (may not necessary)
// visualize in the same window
void multiFrameMatching(std::string pointCloudPath, int cloudNum){
    std::string folderSuffix = "cloud";
    std::string clusterPrefix = "cloud_cluster_";
    std::string clusterSuffix = ".pcd";
    std::vector<std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>> results;
    std::vector<int> groups;
    std::string templateFolder = pointCloudPath + folderSuffix + std::to_string(1) + '/';
    for(int _begin = 1;_begin < cloudNum; _begin ++){
        std::string curFolder = pointCloudPath + folderSuffix + std::to_string(_begin) + '/';
        std::string nextFolder = pointCloudPath + folderSuffix + std::to_string(_begin+1) + '/';
        int curClusterNum = utils::getFileCountInFolder(curFolder)-1;   // -1 for the viewer.sh in folder
        int nextClusterNum = utils::getFileCountInFolder(nextFolder)-1;
        if(_begin == 1){   // create the vector to hold each cloud in the first frame
            groups.resize(curClusterNum);
            for(int i=0; i<curClusterNum; i++){
                std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> curClouds;
                std::string curPath = curFolder + clusterPrefix + std::to_string(i) + clusterSuffix;
                pcl::PointCloud<pcl::PointXYZ>::Ptr curCloud(new pcl::PointCloud<pcl::PointXYZ>);
                pcl::PCLPointCloud2 cloud_blob;
                pcl::io::loadPCDFile(curPath.c_str(), cloud_blob);
                pcl::fromPCLPointCloud2(cloud_blob, *curCloud); // convert from pcl/PCLPointCloud2 to pcl::PointCloud<T>
                curClouds.push_back(curCloud->makeShared());
                results.push_back(curClouds);
                groups[i] = i;
            }
        }
        std::vector<std::pair<int, int>> matches = clusterMatchingICP(curFolder, curClusterNum, nextFolder, nextClusterNum);
        for(int i=0; i<groups.size(); i++){
            bool isFound = false;
            if(groups[i] == -1){   // to help unmatched clusters

            }
            for (int j=0; j<matches.size(); j++){
                if(matches[j].first == groups[i]){
                    groups[i] = matches[j].second;
                    isFound = true;
                    break;
                }
            }
            if(!isFound){
                groups[i] = -1;
            }
        }
        std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> nextClusters = utils::loadCloudsFromDirectory(nextFolder, nextClusterNum);
        for(int i=0; i<groups.size(); i++){
            if(groups[i] == -1)
                continue;
            results[i].push_back(nextClusters[groups[i]]);
        }
        cout << "Cur Group is: " << groups[0] << endl;
    }
    for(int i=0; i<results.size(); i++){
        cout << "Current showing cluster " << i << " The size is " << results[i].size() << endl;
        utils::multiVisualizeCloud(results[i]);
    }

}

void curbFilter(std::string pointCloudPath){
    CloudPointer curCloud = utils::loadCloudFromPath(pointCloudPath);
    CloudPointer outputCloud(new pcl::PointCloud<pcl::PointXYZ>);
    std::vector<std::vector<int>> pointIndices;
    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
    std::set<int> inliersSet;
    pointIndices = utils::voxelizePointCloud(curCloud);
    vector<double> groundCoeffi = {0, 1, 0};
    vector<double> frontCoeffi = {0, 0, 1};
    for(int i = 0; i < pointIndices.size(); i++){
        CloudPointer vCloud(new pcl::PointCloud<pcl::PointXYZ>);
        for(int j = 0; j < pointIndices[i].size(); j++){
            vCloud->points.push_back(curCloud->points[pointIndices[i][j]]);
        }
        // utils::visualizeCloud(vCloud);
        CurbModel *cModel = new CurbModel();
        cModel->setInputPointCloud(vCloud);
        cModel->setGroundCoeffi(groundCoeffi);
        cModel->setFrontCoeffi(frontCoeffi);
        if(cModel->validateModel()){
            for(int j = 0; j < pointIndices[i].size(); j++){
                inliersSet.insert(pointIndices[i][j]);
            }
        }
    }
    std::set<int>::iterator it;
    for(it = inliersSet.begin(); it != inliersSet.end(); ++it){
        inliers->indices.push_back(*it);
    }
    pcl::ExtractIndices<pcl::PointXYZ> extract;
    extract.setInputCloud(curCloud);
    extract.setIndices(inliers);
    extract.setNegative(false);
    extract.filter(*outputCloud);
    utils::visualizeCloud(outputCloud);
}

void staticCloudPointMatch(CloudPointer laserCloudSrc, CloudPointer laserCloudRef, cv::Mat image, vector<float> transform, std::string pairName){
    pcl::search::KdTree<pcl::PointXYZ> KdTree;
    KdTree.setInputCloud(laserCloudRef->makeShared());
    ofstream outputFile("/home/lifan/LIDAR/workspace/corr/" + pairName + "-points.txt");

    //part for transform
    cv::Mat rotation(3, 3, CV_64F);
    cv::Mat translation(3, 1, CV_64F);
    cv::Mat K(3, 3, CV_64F);
    cv::Mat H(3, 4, CV_64F);

    string transformPath = "/home/lifan/camera-lidar-test/calib_cam_to_range_00.txt";
    string KPath = "/home/lifan/camera-lidar-test/calib_cam_to_cam.txt";

    utils::loadKFromTXT(KPath, K);

    cv::Mat P(4, 4, CV_64F);
    utils::loadTransformFromTXT(transformPath, P);
    P.at<double>(3, 0) = 0;
    P.at<double>(3, 1) = 0;
    P.at<double>(3, 2) = 0;
    P.at<double>(3, 3) = 1;
    cout << P <<endl;
    cout << K <<endl;
    P = P.inv();

    vector<cv::Point2f> outputPoints;

    for(auto point:laserCloudSrc->points){
        std::vector<int> indices(1);
        std::vector<float> sqr_distances(1);

        KdTree.nearestKSearch(point, 1, indices, sqr_distances);
        pcl::PointXYZ matchPoint = laserCloudRef->points[indices[0]];

        pcl::PointXYZ pointCamera = utils::pointAssociateTobeMapped(point, transform);
        pcl::PointXYZ matchCamera = utils::pointAssociateTobeMapped(matchPoint, transform);
        float temp = pointCamera.x;
        pointCamera.x = pointCamera.z;
        pointCamera.z = pointCamera.y;
        pointCamera.y = temp;

        temp = matchCamera.x;
        matchCamera.x = matchCamera.z;
        matchCamera.z = matchCamera.y;
        matchCamera.y = temp;

        cv::Point2f pointDehomo;
        if(!utils::transformLidarPointToCamera(pointCamera, pointDehomo, K, P))
            continue;

        outputPoints.push_back(pointDehomo);

        cv::Point2f matchPointDehomo;
        utils::transformLidarPointToCamera(matchCamera, matchPointDehomo, K, P);

        // For output
        outputFile << point.x << " " <<point.y << " " << point.z << " "
                   << matchPoint.x << " " << matchPoint.y << " " << matchPoint.z << endl;
        outputFile << pointDehomo.x << " "<< pointDehomo.y << " "  <<  matchPointDehomo.x << " "  << matchPointDehomo.y << endl;
    }
    outputFile.close();

//    for (int i = 0; i < outputPoints.size(); i++) {
//
//             if (outputPoints[i].x > 0 && outputPoints[i].x < image.cols && outputPoints[i].y > 0 && outputPoints[i].y < image.rows) {
//                cv::circle(image, outputPoints[i], 1, cv::Scalar(0, 255, 0), 1);
//            }
//        }
//        cv::namedWindow("Display window", cv::WINDOW_AUTOSIZE);// Create a window for display.
//        cv::imshow("Display window", image);                   // Show our image inside it.
//        cv::waitKey(0);
}

void movingCloudClusterMatch(CloudPointer laserCloudSrc, CloudPointer laserCloudRef, cv::Mat image, vector<float> transform, std::string pairName){
    ofstream outputFile("/home/lifan/LIDAR/workspace/corr/" + pairName + "-clusters.txt");

    //part for transform
    cv::Mat rotation(3, 3, CV_64F);
    cv::Mat translation(3, 1, CV_64F);
    cv::Mat K(3, 3, CV_64F);
    cv::Mat H(3, 4, CV_64F);

    string transformPath = "/home/lifan/camera-lidar-test/calib_cam_to_range_00.txt";
    string KPath = "/home/lifan/camera-lidar-test/calib_cam_to_cam.txt";

    utils::loadKFromTXT(KPath, K);

    cv::Mat P(4, 4, CV_64F);
    utils::loadTransformFromTXT(transformPath, P);
    P.at<double>(3, 0) = 0;
    P.at<double>(3, 1) = 0;
    P.at<double>(3, 2) = 0;
    P.at<double>(3, 3) = 1;
    cout << P <<endl;
    cout << K <<endl;
    P = P.inv();

    vector<cv::Point2f> outputPoints;

    std::vector<CloudPointer> srcClouds;
    std::vector<CloudPointer> refClouds;

    utils::getVehicleClusterFromCloud(laserCloudSrc, srcClouds);
    utils::getVehicleClusterFromCloud(laserCloudRef, refClouds);

    std::vector<std::pair<int, int>> matches = clusterMatchingICP(srcClouds, refClouds);
    for(auto match:matches){
        CloudPointer srcCloud = srcClouds[match.first];
        CloudPointer refCloud = refClouds[match.second];
        outputFile << "{" << endl;
        outputFile << "[" << endl;
        for(auto point:srcCloud->points){

            pcl::PointXYZ pointCamera = utils::pointAssociateTobeMapped(point, transform);
            float temp = pointCamera.x;
            pointCamera.x = pointCamera.z;
            pointCamera.z = pointCamera.y;
            pointCamera.y = temp;

            cv::Point2f pointDehomo;
            if(!utils::transformLidarPointToCamera(pointCamera, pointDehomo, K, P))
                continue;

            outputPoints.push_back(pointDehomo);

            // For output
            outputFile << point.x << " " <<point.y << " " << point.z << endl;
            outputFile << pointDehomo.x << " " << pointDehomo.y << endl;
        }
        outputFile << "]" << endl;
        outputFile << "[" << endl;
        for(auto point:refCloud->points){

            pcl::PointXYZ pointCamera = utils::pointAssociateTobeMapped(point, transform);
            float temp = pointCamera.x;
            pointCamera.x = pointCamera.z;
            pointCamera.z = pointCamera.y;
            pointCamera.y = temp;

            cv::Point2f pointDehomo;
            if(!utils::transformLidarPointToCamera(pointCamera, pointDehomo, K, P))
                continue;

            outputPoints.push_back(pointDehomo);

            // For output
            outputFile << point.x << " " <<point.y << " " << point.z << endl;
            outputFile << pointDehomo.x << " " << pointDehomo.y << endl;
        }
        outputFile << "]" << endl;
        outputFile << "}" << endl;

    }
//    for (int i = 0; i < outputPoints.size(); i++) {
//
//        if (outputPoints[i].x > 0 && outputPoints[i].x < image.cols && outputPoints[i].y > 0 && outputPoints[i].y < image.rows) {
//            cv::circle(image, outputPoints[i], 1, cv::Scalar(0, 255, 0), 1);
//        }
//    }
//    cv::namedWindow("Display window", cv::WINDOW_AUTOSIZE);// Create a window for display.
//    cv::imshow("Display window", image);                   // Show our image inside it.
//    cv::waitKey(0);
}

void imageLidarVisualizeAll(CloudPointer laserCloudSrc, cv::Mat image, vector<float> transform){
    //part for transform
    cv::Mat rotation(3, 3, CV_64F);
    cv::Mat translation(3, 1, CV_64F);
    cv::Mat K(3, 3, CV_64F);
    cv::Mat H(3, 4, CV_64F);
    cv::Mat D(1, 5, CV_64F);

    string transformPath = "/home/lifan/camera-lidar-test/calib_cam_to_range_00.txt";
    string KPath = "/home/lifan/camera-lidar-test/calib_cam_to_cam.txt";
//    string transformPath = "/home/lifan/camera-lidar-test/old_params/2/calib_cam_to_range_00.txt";
//    string KPath = "/home/lifan/camera-lidar-test/old_params/2/calib_cam_to_cam.txt";

    utils::loadKFromTXT(KPath, K);
    utils::loadDFromTXT(KPath, D);
    cv::Mat P(4, 4, CV_64F);
    utils::loadTransformFromTXT(transformPath, P);
    P.at<double>(3, 0) = 0;
    P.at<double>(3, 1) = 0;
    P.at<double>(3, 2) = 0;
    P.at<double>(3, 3) = 1;
    cout << P <<endl;
    cout << K <<endl;
    cout << D << endl;
    P = P.inv();
    vector<cv::Point2f> outputPoints;
    vector<double> distance;
    cv::Mat rvec;
    cv::Rodrigues(P(cv::Range(0, 3), cv::Range(0, 3)), rvec);
    for(auto point:laserCloudSrc->points){

        pcl::PointXYZ pointCamera = utils::pointAssociateTobeMapped(point, transform);

        float temp = pointCamera.x;
        pointCamera.x = pointCamera.z;
        pointCamera.z = pointCamera.y;
        pointCamera.y = temp;

        cv::Point2f pointDehomo;
        cv::Point3f pointCamera_new(pointCamera.x, pointCamera.y, pointCamera.z);
        vector<cv::Point3f> input;
        vector<cv::Point2f> output;
        input.push_back(pointCamera_new);
        if(!utils::transformLidarPointToCamera(pointCamera, pointDehomo, K, P))
            continue;

//        cv::projectPoints(input,  rvec, P(cv::Range(0, 3), cv::Range(3, 4)), K,  D, output);
//        utils::correctDistortion(pointDehomo, D);
//        outputPoints.push_back(output[0]);
        distance.push_back((pointCamera_new.x * pointCamera_new.x) + (pointCamera_new.y * pointCamera_new.y) + (pointCamera_new.z
                                                                                * pointCamera_new.z));
        outputPoints.push_back(pointDehomo);

    }
    double maxDistance = *max_element(distance.begin(), distance.end());
    for (int i = 0; i < outputPoints.size(); i++) {

        if (outputPoints[i].x > 0 && outputPoints[i].x < image.cols && outputPoints[i].y > 0 && outputPoints[i].y < image.rows) {
            cv::circle(image, outputPoints[i], 1, cv::Scalar(0, 255 - 255 * min((distance[i] / 350), 1.0), 0), 1);
        }
    }
    cv::namedWindow("Display window", cv::WINDOW_AUTOSIZE);// Create a window for display.
    cv::imshow("Display window", image);                   // Show our image inside it.
    cv::waitKey(0);

}

int main(int argc, char** argv)
{
    const int mode = 3;
//    int mode = std::atoi(argv[1]);
//    long startString = std::atol(argv[2]);

    long start = 148882466705;
    const int buffer = 4;
    const int deviation = 40;
    utils::myPCLViewer::createInstance();
    for(int i = 0; i < 2300; i++) {
        long startString = start + 5 * i;
        std::string cloudDir = "/home/lifan/LIDAR/workspace/6/mappoints/";
        std::string fileSuffix = ".pcd";
//        std::string cloudDir = "/home/lifan/LIDAR/result_all/";
//        std::string fileSuffix = "_moving.pcd";

        std::string imageDir = "/home/lifan/LIDAR/workspace/6/imgs/";
//        std::string cloudDir = "/home/lifan/LIDAR/workspace/20170228/mappoints/";
//        std::string imageDir = "/home/lifan/LIDAR/workspace/20170228/image-lidar/";

        std::string srcCloudNumber = utils::convertTimeStampToFileString(startString);
        std::string imageNumber = utils::convertTimeStampToFileString(startString);
        std::string refCloudNumber = utils::convertTimeStampToFileString(startString + buffer * deviation);

        std::string srcPointCloudPathString = cloudDir + srcCloudNumber + fileSuffix;
        std::string refPointCloudPathString = cloudDir + refCloudNumber + fileSuffix;
        std::string srcTransform = cloudDir + srcCloudNumber + ".txt";
        CloudPointer laserCloudSrc = utils::loadCloudFromPath(srcPointCloudPathString);
        CloudPointer laserCloudRef = utils::loadCloudFromPath(refPointCloudPathString);
        if(laserCloudSrc->points.size() == 0){
            start = start + 1;
            startString = start + 5 * i;
            srcCloudNumber = utils::convertTimeStampToFileString(startString);
            srcPointCloudPathString = cloudDir + srcCloudNumber + fileSuffix;
            srcTransform = cloudDir + srcCloudNumber + ".txt";
            laserCloudSrc = utils::loadCloudFromPath(srcPointCloudPathString);

            if(laserCloudSrc->points.size() == 0){
                start = start - 1;
                continue;
            }

            refCloudNumber = utils::convertTimeStampToFileString(startString + buffer * deviation);
            refPointCloudPathString = cloudDir + refCloudNumber + fileSuffix;
            laserCloudRef = utils::loadCloudFromPath(refPointCloudPathString);
        }
        if(laserCloudRef->points.size() == 0){
            refCloudNumber = utils::convertTimeStampToFileString(startString + 1 + buffer * deviation);
            refPointCloudPathString = cloudDir + refCloudNumber + fileSuffix;
            laserCloudRef = utils::loadCloudFromPath(refPointCloudPathString);
        }
        if(laserCloudRef->points.size() == 0){
            continue;
        }
        vector<float> transform = utils::loadTransformFromTXT(srcTransform);
        std::string imagePathString = imageDir + srcCloudNumber + ".jpg";
        cout << "Finish Loading " << endl;
        cout << srcPointCloudPathString << endl;
        cout << refPointCloudPathString << endl;
        switch(mode){
            case 1: {
                cv::Mat image = cv::imread(imagePathString, CV_LOAD_IMAGE_COLOR);
                movingCloudClusterMatch(laserCloudSrc, laserCloudRef, image, transform, srcCloudNumber);
            }
                break;
            case 2: {
                cv::Mat image = cv::imread(imagePathString, CV_LOAD_IMAGE_COLOR);
                if(image.cols == 0)
                    continue;
                imageLidarVisualizeAll(laserCloudSrc, image, transform);
            }
                break;
            case 3: {
                findAllObject(laserCloudSrc, laserCloudRef, transform, srcCloudNumber);
            }
                break;
            case 4: {
                cv::Mat image = cv::imread(imagePathString, CV_LOAD_IMAGE_COLOR);
                staticCloudPointMatch(laserCloudSrc, laserCloudRef, image, transform, srcCloudNumber);
            }
                break;
            // TEST HERE
            case 5: {
//                utils::pointCloudMapToRaw(laserCloudSrc, transform);
//                std::unique_ptr<LidarPointCloudHandler> cloudHandler(new LidarPointCloudHandler(laserCloudSrc));
//
//                cloudHandler->removeGroundPlane();
//                //utils::myPCLViewer::getInstance()->changePointCloud(cloudHandler->getSourcePointCloud());
//                std::vector<CloudPointer> res = cloudHandler->pointCloudSegmentation();
//                utils::myPCLViewer::getInstance()->changeMultiPointCloud(res);
            }
                break;
            default:break;
        }
    }
    return 0;
}


/**************************************************************************\
 *   This file is part of CaSPER.                                          *
 *                                                                         *
 *   Copyright:                                                            *
 *   2009-2009 - Marco Correia <marco.v.correia@gmail.com>           *
 *                                                                         *
 *   Licensed under the Apache License, Version 2.0 (the "License");       *
 *   you may not use this file except in compliance with the License.      *
 *   You may obtain a copy of the License at                               *
 *            http://www.apache.org/licenses/LICENSE-2.0                   *
 *   Unless required by applicable law or agreed to in writing, software   *
 *   distributed under the License is distributed on an                    *
 *   "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,          *
 *   either express or implied.                                            *
 *   See the License for the specific language governing permissions and   *
 *   limitations under the License.                                        *
 \*************************************************************************/

#include <boost/xpressive/xpressive.hpp>
#include <libxml++/libxml++.h>
#include <bindings/cpp/cpp.h>
#include <bindings/cpp/print.h>

#include <iostream>

#include "exprparser.h"

using namespace boost::xpressive;
using namespace regex_constants;
using namespace Casperbind::cpp;

ExprParser::ExprParser()
{
	integer	= +digit;
	boolean	= as_xpr("true") | "false";
	id		= alpha >> *alnum;

	eNeg = "neg(" >> by_ref(integerExpr) >> ")";
	eAbs = "abs(" >> by_ref(integerExpr) >> ")";
	eAdd = "add(" >> by_ref(integerExpr) >> ","
									 >> by_ref(integerExpr) >> ")";
	eSub	= "sub(" >> by_ref(integerExpr) >> ","
									 >> by_ref(integerExpr) >> ")";
	eMul	= "mul(" >> by_ref(integerExpr) >> ","
									 >> by_ref(integerExpr) >> ")";
	eDiv	= "div(" >> by_ref(integerExpr) >> ","
									 >> by_ref(integerExpr) >> ")";
	eMod	= "mod(" >> by_ref(integerExpr) >> ","
									 >> by_ref(integerExpr) >> ")";
	ePow	= "pow(" >> by_ref(integerExpr) >> ","
									 >> by_ref(integerExpr) >> ")";
	eMin	= "min(" >> by_ref(integerExpr) >> ","
									 >> by_ref(integerExpr) >> ")";
	eMax	= "max(" >> by_ref(integerExpr) >> ","
									 >> by_ref(integerExpr) >> ")";
	eIf	= "if(" >>  by_ref(boolExpr) >> ","
									>> by_ref(integerExpr) >> ","
									>> by_ref(integerExpr)>> ")";

	integerExpr = integer | id | eNeg | eAbs | eAdd | eSub
			| eMul | eDiv | eMod | ePow | eMin | eMax | eIf;

	pNot= "not(" >> by_ref(boolExpr) >> ")";
	pAnd= "and(" >> by_ref(boolExpr) >> "," >> by_ref(boolExpr) >> ")";
	pOr	= "or(" >> by_ref(boolExpr) >> "," >> by_ref(boolExpr) >> ")";
	pXOr= "xor(" >> by_ref(boolExpr) >> ","	>> by_ref(boolExpr) >> ")";
	pIff= "iff(" >> by_ref(boolExpr) >> ","	>> by_ref(boolExpr) >> ")";
	pEq	= "eq(" >> integerExpr >> "," >> integerExpr >> ")";
	pNe	= "ne(" >> integerExpr >> "," >> integerExpr >> ")";
	pGe	= "ge(" >> integerExpr >> "," >> integerExpr >> ")";
	pGt	= "gt(" >> integerExpr >> "," >> integerExpr >> ")";
	pLe	= "le(" >> integerExpr >> "," >> integerExpr >> ")";
	pLt	= "lt(" >> integerExpr >> "," >> integerExpr >> ")";

	boolExpr = boolean | pNot | pAnd | pOr | pXOr | pIff | pEq
			| pNe | pGe | pGt | pLe | pLt;
}

SharedSymbol ExprParser::parse(const std::string& s,
		const std::map<std::string,casperbind::cpp::SharedSymbol>& mpars) const
{
	smatch what;
    if (!regex_match( s, what, boolExpr))
      throw xmlpp::validity_error("expected boolean expression or predicate : "+s);

    assert(what.nested_results().size()==1);

    return parseBoolExpression(what,mpars);
}

SharedSymbol ExprParser::parseBoolExpression(const smatch& what,
		const std::map<std::string,casperbind::cpp::SharedSymbol>& mpars) const
{
	assert(what.nested_results().size()==1);

	smatch::nested_results_type::const_iterator begin = what.nested_results().begin();
	smatch::nested_results_type::const_iterator end = what.nested_results().end();

	if (begin->regex_id() == boolean.regex_id())
	{
		if (begin->str() == "true")
			return int(1);
		else
		if (begin->str() == "false")
			return int(0);
		assert(0);
	}
	else
	if (begin->regex_id() == pNot.regex_id())
	{
		SymbolArray pars(1);
		pars[0] = parseBoolExpression(*begin->nested_results().begin(),mpars);
		return Predicate(Predicate::pNot,pars);
	}
	else
	if (begin->regex_id() == pAnd.regex_id() or
		begin->regex_id() == pOr.regex_id() or
		begin->regex_id() == pXOr.regex_id() or
		begin->regex_id() == pIff.regex_id())
	{
		SymbolArray pars(2);
		pars[0] = parseBoolExpression(*begin->nested_results().begin(),mpars);
		pars[1] = parseBoolExpression(*++begin->nested_results().begin(),mpars);
		if (begin->regex_id() == pAnd.regex_id())
			return Predicate(Predicate::pAnd,pars);
		else
		if (begin->regex_id() == pOr.regex_id())
			return Predicate(Predicate::pOr,pars);
		else
		if (begin->regex_id() == pXOr.regex_id())
			return Predicate(Predicate::pXOr,pars);
		else
		if (begin->regex_id() == pIff.regex_id())
			return Predicate(Predicate::pIff,pars);
	}
	else
	if (begin->regex_id() == pEq.regex_id() or
		begin->regex_id() == pNe.regex_id() or
		begin->regex_id() == pGe.regex_id() or
		begin->regex_id() == pGt.regex_id() or
		begin->regex_id() == pLe.regex_id() or
		begin->regex_id() == pLt.regex_id())
	{
		SymbolArray pars(2);
		pars[0] = parseIntExpression(*begin->nested_results().begin(),mpars);
		pars[1] = parseIntExpression(*++begin->nested_results().begin(),mpars);
		if (begin->regex_id() == pEq.regex_id())
			return Predicate(Predicate::pEqual,pars);
		else
		if (begin->regex_id() == pNe.regex_id())
			return Predicate(Predicate::pDistinct,pars);
		else
		if (begin->regex_id() == pGe.regex_id())
			return Predicate(Predicate::pGreaterEqual,pars);
		else
		if (begin->regex_id() == pGt.regex_id())
			return Predicate(Predicate::pGreater,pars);
		else
		if (begin->regex_id() == pLe.regex_id())
			return Predicate(Predicate::pLessEqual,pars);
		else
		if (begin->regex_id() == pLt.regex_id())
			return Predicate(Predicate::pLess,pars);
	}
	assert(0);
}

SharedSymbol ExprParser::parseIntExpression(const smatch& what,
		const std::map<std::string,casperbind::cpp::SharedSymbol>& mpars) const
{
	assert(what.nested_results().size()==1);

	smatch::nested_results_type::const_iterator begin = what.nested_results().begin();
	smatch::nested_results_type::const_iterator end = what.nested_results().end();

	assert(what.nested_results().size()==1);

	if (begin->regex_id() == integer.regex_id())
		return int(atoi(begin->str().c_str()));
	else
	if (begin->regex_id() == id.regex_id())
	{
		std::map<std::string,casperbind::cpp::SharedSymbol>::const_iterator
				it = mpars.find(begin->str());
		if (it==mpars.end())
			throw xmlpp::validity_error("undeclared variable appearing in expression : "+begin->str());
		return it->second;
	}
	else
	if (begin->regex_id() == eNeg.regex_id() or
		begin->regex_id() == eAbs.regex_id())
	{
		SymbolArray pars(1);
		pars[0] = parseIntExpression(*begin->nested_results().begin(),mpars);
		if (begin->regex_id() == eNeg.regex_id())
			return Expression(Expression::eSym,pars);
		else
		if (begin->regex_id() == eAbs.regex_id())
			return Expression(Expression::eAbs,pars);
	}
	else
	if (begin->regex_id() == eAdd.regex_id() or
		begin->regex_id() == eSub.regex_id() or
		begin->regex_id() == eMul.regex_id() or
		begin->regex_id() == eDiv.regex_id() or
		begin->regex_id() == eMod.regex_id() or
		begin->regex_id() == ePow.regex_id() or
		begin->regex_id() == eMin.regex_id() or
		begin->regex_id() == eMax.regex_id())
	{
		SymbolArray pars(2);
		pars[0] = parseIntExpression(*begin->nested_results().begin(),mpars);
		pars[1] = parseIntExpression(*++begin->nested_results().begin(),mpars);
		if (begin->regex_id() == eAdd.regex_id())
			return Expression(Expression::eAdd,pars);
		else
		if (begin->regex_id() == eSub.regex_id())
			return Expression(Expression::eSub,pars);
		else
		if (begin->regex_id() == eMul.regex_id())
			return Expression(Expression::eMul,pars);
		else
		if (begin->regex_id() == eDiv.regex_id())
			return Expression(Expression::eDiv,pars);
		else
		if (begin->regex_id() == eMod.regex_id())
			return Expression(Expression::eMod,pars);
		else
		if (begin->regex_id() == ePow.regex_id())
			return Expression(Expression::ePow,pars);
		else
		if (begin->regex_id() == eMin.regex_id())
			return Expression(Expression::eMin,pars);
		else
		if (begin->regex_id() == eMax.regex_id())
			return Expression(Expression::eMax,pars);
	}
	else
	if (begin->regex_id() == eIf.regex_id())
	{
		SymbolArray pars(3);
		pars[0] = parseBoolExpression(*begin->nested_results().begin(),mpars);
		pars[1] = parseIntExpression(*++begin->nested_results().begin(),mpars);
		pars[2] = parseIntExpression(*++(++begin->nested_results().begin()),mpars);
		return Expression(Expression::eIfThenElse,pars);
	}

	assert(0);
}

#if 0
int main(int argc, char** argv)
{
	std::map<std::string,casperbind::cpp::SharedSymbol> mpars;
	mpars["V0"] = casperbind::cpp::Variable(casperbind::cpp::IntDomain(casperbind::cpp::IntRange(1,10)));
	mpars["V1"] = casperbind::cpp::Variable(casperbind::cpp::IntDomain(casperbind::cpp::IntRange(2,4)));

	std::string s(argv[1]);
	std::cout << "input string is [" << s << "]" << std::endl;
	ExprParser e;
	SharedSymbol o = e.parse(s,mpars);
	std::cout << o << std::endl;
}
#endif


// Displays AMASS sequence in a OpenGL 3D viewer
// 2 optional arguments:
// 1. model type, default H
//    options: S H X (SMPL SMPL-H SMPL-X)
//    additional: Xp (SMPLX with hand PCA) Z (SMPLX old v1.0) Zp (SMPLX v1.0
//    PCA)
// 2. sequence (AMASS .npz) path. If not specified,
//    opens a blank viewer with option to browse and load a npz
#include <Eigen/Geometry>
#include <algorithm>
#include <chrono>
#include <iostream>

#include "imfilebrowser.h"
#include "meshview/meshview.hpp"
#include "meshview/meshview_imgui.hpp"
#include "smplx/sequence.hpp"
#include "smplx/smplx.hpp"
#include "smplx/util.hpp"

using namespace smplx;

template <class ModelConfig>
static int run(std::string path) {
    SequenceAMASS amass(path);
    Gender gender = amass.gender;

    // * Construct SMPL body model
    Model<ModelConfig> model(gender);
    Body<ModelConfig> body(model);

    if (amass.n_frames) {
        // If not empty, load shape/pose
        amass.set_shape(body);
        amass.set_pose(body, 0);
    }
    body.update();

    // * Set up meshview viewer
    meshview::Viewer viewer;

    auto& smpl_mesh =
        viewer.add_mesh(body.verts(), model.faces, 0.8f, 0.5f, 0.6f);
    // Due to different coordinate system, all AMASS dada are rotated 90 degs
    // CCW on x-axis; we undo this rotation using the model matrix
    smpl_mesh.rotate(
        Eigen::AngleAxisf(M_PI * .5f, Eigen::Vector3f(-1.f, 0.f, 0.f))
            .toRotationMatrix());

    viewer.draw_axes = true;  // Press a to hide axes
    auto center_camera_on_human = [&]() {
        // Set camera's center of rotation to transformed root joint
        viewer.camera.center_of_rot = (/* model matrix */ smpl_mesh.transform *
                                       /* deformed root joint */ body.joints()
                                           .row(0)
                                           .transpose()
                                           .homogeneous())
                                          .template head<3>();
    };
    viewer.camera.dist_to_center = 4.f;  // Zoom out a little
    center_camera_on_human();

    // * Animation state
    int frame = 0;        // Current frame
    int frame_start = 0;  // Frame when we started animating
    std::chrono::high_resolution_clock::time_point
        time_start;  // Time when we started anim
    bool playing = false;
    bool camera_follow_human =
        true;  // Whether to automatically follow the human

    bool updated;

    // Copy AMASS frame 'frame' to body and update mesh + (optionally) camera
    auto update_frame = [&]() {
        if (amass.n_frames == 0) return;  // Empty sequence
        amass.set_pose(body, (size_t)frame);
        body.update();
        smpl_mesh.verts_pos().noalias() = body.verts();
        smpl_mesh.faces.noalias() = model.faces;
        if (camera_follow_human) {
            // Follow the human with camera (set c.o.r. to root joint)
            center_camera_on_human();
        }
        updated = true;
    };

    // Helper to toggle play/pause
    auto toggle_play = [&]() {
        playing = !playing;
        if (playing) {
            frame = frame_start;
            time_start = std::chrono::high_resolution_clock::now();
        } else {
            frame_start = frame;
        }
        viewer.loop_wait_events = !playing;
    };

    // Set key handler
    viewer.on_key = [&](int key, meshview::input::Action action,
                        int mods) -> bool {
        if (action == meshview::input::Action::press) {
            if (key == 'R') {
                frame_start = 0;
                playing = false;
                update_frame();
            } else if (key == 'L') {
                camera_follow_human = !camera_follow_human;
            } else if (key == ' ') {
                toggle_play();
            }
        }
        return true;
    };
    viewer.on_open = []() { ImGui::GetIO().IniFilename = nullptr; };
    viewer.on_loop = [&]() {
        updated = false;
        if (playing) {
            double delta =
                std::chrono::duration<double>(
                    std::chrono::high_resolution_clock::now() - time_start)
                    .count();
            int nx_frame =
                static_cast<int>(std::floor(delta * amass.frame_rate)) +
                frame_start;
            if (nx_frame > frame) {
                if (nx_frame >= amass.n_frames) {
                    playing = false;
                    frame_start = 0;
                } else {
                    frame = nx_frame;
                    update_frame();
                }
            }
        }
        return updated;
    };
    viewer.on_gui = [&]() {
        updated = false;
        static ImGui::FileBrowser open_file_dialog;
        if (open_file_dialog.GetTitle().empty()) {
            open_file_dialog.SetTypeFilters({".npz"});
            open_file_dialog.SetTitle("Open AMASS npz");
        }
        ImGui::SetNextWindowPos(ImVec2(10, 10), ImGuiCond_Once);
        ImGui::SetNextWindowSize(ImVec2(300, 180), ImGuiCond_Once);
        ImGui::Begin("Control", NULL);
        ImGui::Text("Model: %s  Gender: %s", model.name(),
                    util::gender_to_str(model.gender));
        if (amass.n_frames) {
            ImGui::TextWrapped("Seq: %s", path.c_str());
            ImGui::Text("Frame %i (%i total)", frame, (int)amass.n_frames);
            if (ImGui::SliderInt("Frame##framectl", &frame, 0,
                                 (int)amass.n_frames - 1)) {
                frame_start = frame;
                if (playing)
                    time_start = std::chrono::high_resolution_clock::now();
                update_frame();
            }
            if (ImGui::Button(playing ? "Pause" : "Play")) {
                toggle_play();
            }
            ImGui::SameLine();
            if (ImGui::Button("Reset")) {
                frame = frame_start = 0;
                playing = false;
                update_frame();
            }
            ImGui::SameLine();
        } else {
            ImGui::TextWrapped(
                "Please click 'Open AMASS npz' and select a file. "
                "Ignore the current camera angle, it will be correct on open.");
        }
        if (ImGui::Button("Open AMASS npz")) {
            frame = frame_start = 0;
            playing = false;
            // Need to disable glfwWaitEvents because
            // it makes the dialog unclickable
            viewer.loop_wait_events = false;
            open_file_dialog.Open();
        }
        ImGui::Checkbox("Camera follows human", &camera_follow_human);
        ImGui::End();  // Control

        open_file_dialog.Display();
        if (open_file_dialog.HasSelected()) {
            // Load new sequence
            path = open_file_dialog.GetSelected().string();
            amass.load(path);
            amass.set_shape(body);

            if (amass.gender != gender) {
                // Have to change the gender
                model.load(amass.gender);
                gender = amass.gender;
            }
            open_file_dialog.ClearSelected();
            update_frame();
            viewer.loop_wait_events = true;
        }
        return updated;
    };
    viewer.show();
    return 0;
}

int main(int argc, char** argv) {
    std::string path = argc > 2 ? argv[2] : "";
    std::string model_name = argv[1];
    for (auto& c : model_name) c = std::toupper(c);
    if (argc < 2 || model_name == "S") {
        return run<model_config::SMPL>(path);
    } else if (model_name == "H") {
        return run<model_config::SMPLH>(path);
    } else if (model_name == "X") {
        return run<model_config::SMPLX>(path);
    } else if (model_name == "Z") {
        return run<model_config::SMPLX_v1>(path);
    } else if (model_name == "Xp") {
        return run<model_config::SMPLXpca>(path);
    } else if (model_name == "Zp") {
        return run<model_config::SMPLXpca_v1>(path);
    }
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "Time/Time.hpp"

#include <boost/functional/hash.hpp>
#include <cmath>
#include <ostream>
#include <pup.h>
#include <utility>

#include "ErrorHandling/Assert.hpp"

// Time implementations

Time::Time(Slab slab, rational_t fraction) noexcept
    // clang-tidy: move trivially copyable type
    : slab_(std::move(slab)), fraction_(std::move(fraction)) {  // NOLINT
  range_check();
  compute_value();
}

Time Time::with_slab(const Slab& new_slab) const noexcept {
  if (new_slab == slab_) {
    return *this;
  } else if (is_at_slab_start()) {
    if (slab_.start_ == new_slab.start_) {
      return new_slab.start();
    } else {
      ASSERT(slab_.start() == new_slab.end(),
             "Can't move " << fraction_ << " " << slab_ << " to slab "
             << new_slab);
      return new_slab.end();
    }
  } else {
    ASSERT(is_at_slab_end(), "Can't move " << fraction_ << " " << slab_
                                           << " to slab " << new_slab);
    if (slab_.end_ == new_slab.end_) {
      return new_slab.end();
    } else {
      ASSERT(slab_.end() == new_slab.start(),
             "Can't move " << fraction_ << " " << slab_ << " to slab "
             << new_slab);
      return new_slab.start();
    }
  }
}

Time& Time::operator+=(const TimeDelta& delta) noexcept {
  *this = this->with_slab(delta.slab_);
  fraction_ += delta.fraction_;
  range_check();
  compute_value();
  return *this;
}

Time& Time::operator-=(const TimeDelta& delta) noexcept {
  *this = this->with_slab(delta.slab_);
  fraction_ -= delta.fraction_;
  range_check();
  compute_value();
  return *this;
}

bool Time::is_at_slab_start() const noexcept { return fraction_ == 0; }
bool Time::is_at_slab_end() const noexcept { return fraction_ == 1; }
bool Time::is_at_slab_boundary() const noexcept {
  return is_at_slab_start() or is_at_slab_end();
}

void Time::pup(PUP::er& p) noexcept {
  p | slab_;
  p | fraction_;
  p | value_;
}

bool Time::StructuralCompare::operator()(const Time& a, const Time& b) const {
  if (a.fraction().numerator() != b.fraction().numerator()) {
    return a.fraction().numerator() < b.fraction().numerator();
  }
  if (a.fraction().denominator() != b.fraction().denominator()) {
    return a.fraction().denominator() < b.fraction().denominator();
  }
  return a.slab() < b.slab();
}

void Time::compute_value() noexcept {
  if (is_at_slab_end()) {
    // Protection against rounding error.
    value_ = slab_.end_;
  } else {
    value_ = slab_.start_ + (slab_.duration() * fraction_).value();
  }
}

void Time::range_check() const noexcept {
  ASSERT(fraction_ >= 0 and fraction_ <= 1,
         "Out of range slab fraction: " << fraction_);
}

// TimeDelta implementations

TimeDelta::TimeDelta(Slab slab, rational_t fraction) noexcept
    // clang-tidy: move trivially copyable type
    : slab_(std::move(slab)), fraction_(std::move(fraction)) {}  // NOLINT

TimeDelta TimeDelta::with_slab(const Slab& new_slab) const noexcept {
  return {new_slab, fraction()};
}

double TimeDelta::value() const noexcept {
  return (slab_.end_ - slab_.start_) * fraction_.value();
}

bool TimeDelta::is_positive() const noexcept { return fraction_ > 0; }

TimeDelta& TimeDelta::operator+=(const TimeDelta& other) noexcept {
  ASSERT(slab_ == other.slab_, "Can't add TimeDeltas from different slabs");
  fraction_ += other.fraction_;
  return *this;
}

TimeDelta& TimeDelta::operator-=(const TimeDelta& other) noexcept {
  ASSERT(slab_ == other.slab_,
         "Can't subtract TimeDeltas from different slabs");
  fraction_ -= other.fraction_;
  return *this;
}

TimeDelta TimeDelta::operator+() const noexcept { return *this; }

TimeDelta TimeDelta::operator-() const noexcept { return {slab_, -fraction_}; }

TimeDelta& TimeDelta::operator*=(const rational_t& mult) noexcept {
  fraction_ *= mult;
  return *this;
}

TimeDelta& TimeDelta::operator/=(const rational_t& div) noexcept {
  fraction_ /= div;
  return *this;
}

void TimeDelta::pup(PUP::er& p) noexcept {
  p | slab_;
  p | fraction_;
}

// Free function implementations

// Time <cmp> Time

bool operator==(const Time& a, const Time& b) noexcept {
  if (a.slab() == b.slab()) {
    return a.fraction() == b.fraction();
  } else {
    return (a.slab().is_followed_by(b.slab()) and
            a.is_at_slab_end() and
            b.is_at_slab_start()) or
           (a.slab().is_preceeded_by(b.slab()) and
            a.is_at_slab_start() and
            b.is_at_slab_end()) or
           (a.slab().start_ == b.slab().start_ and
            a.is_at_slab_start() and
            b.is_at_slab_start()) or
           (a.slab().end_ == b.slab().end_ and
            a.is_at_slab_end() and
            b.is_at_slab_end());
  }
}

bool operator!=(const Time& a, const Time& b) noexcept { return not(a == b); }

bool operator<(const Time& a, const Time& b) noexcept {
  if (a.slab() == b.slab()) {
    return a.fraction() < b.fraction();
  }
  if (not a.slab().overlaps(b.slab())) {
    return a != b and a.slab() < b.slab();
  }
  if ((a.slab().start() == b.slab().start() and a.is_at_slab_start()) or
      (a.slab().end() == b.slab().end() and a.is_at_slab_end())) {
    return a.with_slab(b.slab()) < b;
  } else {
    return a < b.with_slab(a.slab());
  }
}

bool operator>(const Time& a, const Time& b) noexcept { return b < a; }

bool operator<=(const Time& a, const Time& b) noexcept { return not(a > b); }

bool operator>=(const Time& a, const Time& b) noexcept { return not(a < b); }

// TimeDelta <cmp> TimeDelta

bool operator==(const TimeDelta& a, const TimeDelta& b) noexcept {
  return a.slab() == b.slab() and a.fraction() == b.fraction();
}

bool operator!=(const TimeDelta& a, const TimeDelta& b) noexcept {
  return not(a == b);
}

bool operator<(const TimeDelta& a, const TimeDelta& b) noexcept {
  ASSERT(a.slab() == b.slab(), "Can't check cross-slab TimeDelta inequalities");
  return a.fraction() < b.fraction();
}

bool operator>(const TimeDelta& a, const TimeDelta& b) noexcept {
  return b < a;
}

bool operator<=(const TimeDelta& a, const TimeDelta& b) noexcept {
  return not(a > b);
}

bool operator>=(const TimeDelta& a, const TimeDelta& b) noexcept {
  return not(a < b);
}

// Time <op> Time

TimeDelta operator-(const Time& a, const Time& b) noexcept {
  if (a.slab() == b.slab()) {
    return {a.slab(), a.fraction() - b.fraction()};
  } else if (a.slab().is_followed_by(b.slab())) {
    if (a.is_at_slab_end()) {
      return {b.slab(), -b.fraction()};
    } else {
      ASSERT(b.is_at_slab_start(),
             "Can't subtract times from different slabs: " << a << " - " << b);
      return {a.slab(), a.fraction() - 1};
    }
  } else {
    ASSERT(a.slab().is_preceeded_by(b.slab()),
           "Can't subtract times from different slabs: " << a << " - " << b);
    if (a.is_at_slab_start()) {
      return {b.slab(), 1 - b.fraction()};
    } else {
      ASSERT(b.is_at_slab_end(),
             "Can't subtract times from different slabs: " << a << " - " << b);
      return {a.slab(), a.fraction()};
    }
  }
}

// Time <op> TimeDelta, TimeDelta <op> Time
Time operator+(Time a, const TimeDelta& b) noexcept {
  a += b;
  return a;
}

Time operator+(const TimeDelta& a, Time b) noexcept {
  b += a;
  return b;
}

Time operator-(Time a, const TimeDelta& b) noexcept {
  a -= b;
  return a;
}

// TimeDelta <op> TimeDelta

TimeDelta operator+(TimeDelta a, const TimeDelta& b) noexcept {
  a += b;
  return a;
}

TimeDelta operator-(TimeDelta a, const TimeDelta& b) noexcept {
  a -= b;
  return a;
}

double operator/(const TimeDelta& a, const TimeDelta& b) noexcept {
  // We need to use double/double here because this is the
  // implementation of TimeDelta/TimeDelta.
  return (a.fraction() / b.fraction()).value() *
         (a.slab().duration().value() / b.slab().duration().value());
}

// rational <op> TimeDelta, TimeDelta <op> rational
TimeDelta operator*(TimeDelta a, const TimeDelta::rational_t& b) noexcept {
  a *= b;
  return a;
}

TimeDelta operator*(const TimeDelta::rational_t& a, TimeDelta b) noexcept {
  b *= a;
  return b;
}

TimeDelta operator/(TimeDelta a, const TimeDelta::rational_t& b) noexcept {
  a /= b;
  return a;
}

// Miscellaneous other functions for Time

std::ostream& operator<<(std::ostream& os, const Time& t) noexcept {
  return os << t.slab() << ":" << t.fraction();
}

size_t hash_value(const Time& t) noexcept {
  // We need equal Times to have equal hashes.  Times at matching ends
  // of slabs are equal, so we can't use the slab and fraction in that
  // case.
  if (t.is_at_slab_boundary()) {
    return boost::hash<double>{}(t.value());
  } else {
    size_t h = 0;
    boost::hash_combine(h, t.slab());
    boost::hash_combine(h, t.fraction());
    return h;
  }
}

// clang-tidy: do not modify std namespace (okay for hash)
namespace std {  // NOLINT
size_t hash<Time>::operator()(const Time& t) const noexcept {
  return boost::hash<Time>{}(t);
}
}  // namespace std

// Miscellaneous other functions for TimeDelta

TimeDelta abs(TimeDelta t) noexcept {
  if (not t.is_positive()) {
    t *= -1;
  }
  return t;
}

std::ostream& operator<<(std::ostream& os, const TimeDelta& dt) noexcept {
  return os << dt.slab() << ":" << dt.fraction();
}


/*
 Copyright (C) 2017 Sascha Meiers
 Distributed under the MIT software license, see the accompanying
 file LICENSE.md or http://www.opensource.org/licenses/mit-license.php.
 */

#ifndef utils_hpp
#define utils_hpp

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/algorithm/string.hpp>
#include <htslib/sam.h>

inline uint32_t alignmentLength(bam1_t const* rec) {
    uint32_t* cigar = bam_get_cigar(rec);
    uint32_t alen = 0;
    for (std::size_t i = 0; i < rec->core.n_cigar; ++i)
      if (bam_cigar_op(cigar[i]) == BAM_CMATCH) alen+=bam_cigar_oplen(cigar[i]);
    return alen;
  }

struct CellInfo {
    unsigned median_bin_count;      /* raw counts */
    float mean_bin_count;           /* after removal of bad bins */
    std::string sample_name;
    std::string cell_name;
    std::string bam_file;
    int32_t id;                     /* position in conf.f_in */
    float nb_p, nb_r, nb_a;         /* NB parameters */
    bool pass_qc;                   /* set to false if no (good) SS library */

    /* Alignment statistics */
    unsigned n_mapped, n_pcr_dups, n_supplementary, n_low_mapq, n_read2s;
    unsigned n_counted, n_unmap;
    CellInfo() : median_bin_count(0), mean_bin_count(0), id(-1), nb_p(0),
                 nb_r(0), nb_a(0), pass_qc(true), n_mapped(0), n_pcr_dups(0),
                 n_supplementary(0), n_low_mapq(0), n_read2s(0), n_counted(0),
                 n_unmap(0)
    {}
};

struct SampleInfo {
    std::vector<float> means;
    std::vector<float> vars;
    float p;
    SampleInfo() : p(0.33) {}
};

/**
 * Write CellInfo to file
 */
bool write_cell_info(std::string const & f_out,
                     std::vector<CellInfo> const & cells)
{
    std::ofstream out(f_out);
    if (out.is_open()) {
        out << "# sample:  Sample (has multiple cells)" << std::endl;
        out << "# cell:    Name of the cell." << std::endl;
        out << "# mapped:  Total number of reads seen" << std::endl;
        out << "# suppl:   Supplementary, secondary or QC-failed reads (filtered out)" << std::endl;
        out << "# dupl:    Reads filtered out as PCR duplicates" << std::endl;
        out << "# mapq:    Reads filtered out due to low mapping quality" << std::endl;
        out << "# read2:   Reads filtered out as 2nd read of pair" << std::endl;
        out << "# good:    Reads used for counting." << std::endl;
        out << "# pass1:   Enough coverage? If false, ignore all columns from now" << std::endl;
        out << "# nb_p:    Negative Binomial parameter p. Constant for one sample." << std::endl;
        out << "# nb_r:    Negative Binomial parameter r. We use NB(p,r/2) * NB(p,r/2) in WC states, but NB(p,(1-a)*r)*NB(p,a*r) in WW or CC states." << std::endl;
        out << "# nb_a:    Negative Binomial parameter a (alpha) used for zero expectation (see above)." << std::endl;
        out << "# bam:     Bam file of this cell" << std::endl;
        out << "sample\tcell\tmedbin\tmapped\tsuppl\tdupl\tmapq\tread2\tgood\tpass1\tnb_p\tnb_r\tnb_a\tbam" << std::endl;

        for (CellInfo const & cell : cells) {
            out << cell.sample_name << "\t";
            out << cell.cell_name << "\t";
            out << cell.median_bin_count << "\t";
            out << cell.n_mapped << "\t";
            out << cell.n_supplementary << "\t";
            out << cell.n_pcr_dups << "\t";
            out << cell.n_low_mapq << "\t";
            out << cell.n_read2s << "\t";
            out << cell.n_counted << "\t";
            out << cell.pass_qc << "\t";
            out << cell.nb_p << "\t";
            out << cell.nb_r << "\t";
            out << cell.nb_a << "\t";
            out << cell.bam_file << std::endl;
        }
    } else {
        std::cerr << "[Warning] Cannot write to " << f_out << std::endl;
        return false;
    }
    return true;
}

template <typename TReturn>
using TMedianAccumulator = boost::accumulators::accumulator_set<TReturn, boost::accumulators::stats<boost::accumulators::tag::median> >;

template <typename TReturn>
using TMeanVarAccumulator = boost::accumulators::accumulator_set<TReturn, boost::accumulators::stats<boost::accumulators::tag::mean, boost::accumulators::tag::variance> >;

double sum(std::vector<double> const & vec)
{
    double sum = 0;
    for (double d : vec)
        sum += d;
    return(sum);
}

// from Delly
inline bool get_RG_tag(std::string const & tag,
                       std::string const& header,
                       std::string& output,
                       bool allow_multiple_matches = false)
{
    std::set<std::string> values;
    typedef std::vector<std::string> TStrParts;
    TStrParts lines;
    boost::split(lines, header, boost::is_any_of("\n"));
    TStrParts::const_iterator itH = lines.begin();
    TStrParts::const_iterator itHEnd = lines.end();
    for(;itH!=itHEnd; ++itH) {
        if (itH->find("@RG")==0) {
            TStrParts keyval;
            boost::split(keyval, *itH, boost::is_any_of("\t "));
            TStrParts::const_iterator itKV = keyval.begin();
            TStrParts::const_iterator itKVEnd = keyval.end();
            for(;itKV != itKVEnd; ++itKV) {
                size_t sp = itKV->find(":");
                if (sp != std::string::npos) {
                    std::string field = itKV->substr(0, sp);
                    if (field == tag) {
                        std::string value = itKV->substr(sp+1);
                        values.insert(value);
                    }
                }
            }
        }
    }
    if (values.size() == 1) {
        output = *(values.begin());
        return true;
    } else if (values.size() > 1) {
        if (allow_multiple_matches) {
            output = *(values.begin());
            return true;
        } else {
            output = "";
            return false;
        }
    } else {
        output = "";
        return false;
    }
}

/**
 * template <class InputIter, class ForwardIter, class BinaryPredicate, class BinaryFunction> ForwardIter reduce_adjacent(InputIter first, InputIter last,ForwardIter result, BinaryPredicate _bool_mergeable, BinaryFunction _merge_func)
 * STL-style algorithm to merge/reduce adjacent elements (if they are mergeable) and return a shortened list
 *
 * @param first input iterator.
 * @param end input iterator end.
 * @param result output iterator. Can point to *first to do inplace operations.
 * @param _bool_mergeable Function stating whether two consecutive elements are mergeable.
 * @param _merge_func Funciton to merge to consecutive elements. Must return same type.
 */
template <class InputIter, class ForwardIter, class BinaryPredicate, class BinaryFunction>
ForwardIter reduce_adjacent(InputIter first, InputIter last,
                            ForwardIter result,
                            BinaryPredicate _bool_mergeable,
                            BinaryFunction _merge_func)
{
    if (first == last) return result; // skip empty container

    auto elem = *first; // copy first element anyways.
    while (++first != last) { // loop skippes first elem
        if ( !_bool_mergeable(elem, *first) ) {
            *(result++) = elem;
            elem = *first;
        } else {
            elem = _merge_func(elem, *first);
        }
    }
    *(result++) = elem;
    return result;
}

#endif /* utils_hpp */


/*
 * File:   Topology.hpp
 * Author: emanuele
 *
 * Created on 27 April 2012, 11:18
 */

#ifndef TOPOLOGY_HPP
#define	TOPOLOGY_HPP

#include "ContentElement.hpp"
#include <string>
#include "boost/graph/adjacency_list.hpp"
#include "boost/graph/graph_traits.hpp"
#include <vector>
#include <map>
#include <set>
#include <fstream>
#include <boost/property_map/dynamic_property_map.hpp>

// forward declarations to avoid include circles
class Scheduler;
class Flow;

using std::string;

/**
 * enum to differentiate upstream, downstream, metro and core (symmetric) links.
 * Note that this is not assigned to the edge, but rather computed through the
 * values of ponCustomers of the two vertices connected by the edge.
 */
enum EdgeType {UPSTREAM, DOWNSTREAM, METRO, CORE, UNKNOWN_TYPE};

/**
 *  A struct used to define bundled properties for nodes in the topology graph.
 */
struct NetworkNode {
    int ponCustomers; /**< number of customers attached to this PON node. if == 0, then this is a core node and not a PON. */
    int asid; /**< All the PONs attached to the same (set of) core node(s) belong to the same Access Section (AS). This is needed to compute locality. */
};
/**
 * A struct used to define bundled properties for edges in the topology graph.
 * Note that edges are uni-directional, i.e., a link is a composed by a pair
 * of edges with opposite directions.
 */
struct NetworkEdge {
    double length; /**< The length of the edge in meters. */
    Capacity maxCapacity; /**< The maximum bandwidth available on this edge. */
    Capacity spareCapacity; /**< The amount of bandwidth currently availabe on this edge, i.e., after substracting the capacity already used by Flows transiting on it. */
    std::set<Flow*> activeFlows; /**< The set of Flows actively using this edge at the moment. */
    Capacity peakCapacity; /**< The highest bandwidth collectively used by Flows on this edge at any given time in the current simulation round. */
    EdgeType type; /**< The EdgeType of this edge. Used to differentiate between core, metro and access traffic. */
};

// Our graph template
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS,
        NetworkNode, NetworkEdge> DGraph;
// Utility definitions to save time
typedef DGraph::vertex_descriptor Vertex;
typedef boost::graph_traits<DGraph>::vertex_iterator VertexIterator;
typedef boost::graph_traits<DGraph>::edge_descriptor Edge;
typedef boost::graph_traits<DGraph>::edge_iterator EdgeIterator;
typedef std::vector<Vertex> VertexVec;
typedef std::map<unsigned int, std::vector<Vertex> > PredecessorMap;

typedef std::map<unsigned int, Vertex> VertexMap;
typedef std::map<Vertex, std::vector<unsigned int> > DistanceMap;
typedef std::map<Edge, Capacity> LoadMap;

/**
 * A struct used to keep track of a number of statistics on traffic observed
 * during the simulations. Each vector has as many elements as the number of
 * rounds in the simulation; at the end of each round the appropriate statistics
 * are computed and saved here.
 */
struct NetworkStats {
  std::vector<Capacity> avgTot; /**< Average traffic observed on an edge, across the whole topology. */
  std::vector<Capacity> avgCore; /**< Average traffic observed on a core edge. */
  std::vector<Capacity> avgAccessUp; /**< Average traffic observed on an upstream access edge. */
  std::vector<Capacity> avgAccessDown; /**< Average traffic observed on a downstream access edge. */
  std::vector<Capacity> peakCore; /**< Peak traffic observed on a core edge. */
  std::vector<Capacity> peakAccessUp; /**< Peak traffic observed on an upstream access edge. */
  std::vector<Capacity> peakAccessDown; /**< Peak traffic observed on a downstream access edge. */
  std::vector<Capacity> avgPeakCore; /**< Average peak traffic observed on a core edge. */
  std::vector<Capacity> avgPeakAccessUp; /**< Average peak traffic observed on an upstream access edge. */
  std::vector<Capacity> avgPeakAccessDown; /**< Average peak traffic observed on a downstream access edge. */
  std::vector<Capacity> avgMetro; /**< Average traffic observed on a metro edge (if present). */
  std::vector<Capacity> peakMetro; /**< Peak traffic observed on a metro edge (if present). */
  std::vector<Capacity> avgPeakMetro; /**< Average peak traffic observed on a metro edge (if present). */
};

/**
 * A representation of the physical topology over which the data is transmitted.
 * It is based on the Boost Graph library, specifically on the Adjacency_List.
 * Bundled properties are used to store information of interests for edges and
 * links. Routing, bandwidth assignment and traffic statistic collection are all
 * performed by this class.
 */
class Topology {
protected:
    DGraph topology; /**< The graph representation of the topology. */
    uint numVertices; /**< The total number of vertices in the topology. */
    uint numEdges; /**< The total number of edges in the topology. */
    uint numMetroEdges; /**< The number of metro edges in the topology. */
    uint numCoreEdges; /**< The number of core edges in the topology. */
    uint numCustomers; /**< The total number of custoemrs (i.e., end-users) in the topology. */
    uint numASes;  /**< The total number of Access Sections (ASes) in the topology. Typically each core (or metro/core) node has an associated AS. */
    DistanceMap dMap; /**< A map keeping track of the distance between vertices, used for routing purposes. */
    PredecessorMap pMap; /**< A map of predecessors in the pre-computed shourtest path routes, used for routing purposes. */
    Vertex centralServer; /**< The core network vertex where we are placing the central repository, i.e., the source of all ContentElement when they are first introduced in the catalog or when there is no other source available. */
    VertexMap asCacheMap; /**< A map matching each AS identifier with the vertex where the CDN server is located. */
    NetworkStats stats; /**< A collection of statistic measurements of the traffic circulating over this topology. @see NetworkStats */
    VertexVec ponNodes; /**< A vector of all the graph vertices with a non-zero number of NetworkNode::ponCustomers attached to them. These vertices will have a single link connecting them to a metro or metro/core node, representing the shared fiber tree of a PON. */
    string fileName; /**< The name of the input file used to generate the topology. */
    LoadMap loadMap; /**< A Map associating to each edge the total traffic it has observed. Used to compute the traffic statistics at the end of each round. */
    uint bitrate; /**< While not technically a topology parameter, the bitrate of the encoded content is required both in updateCapacity to estimate the time at which users will change channel (and thus to set the userViewEta) and to figure out if there's enough capacity to serve a new customer.*/
    Capacity minFlowIncrease;/**< minFlowIncrease is used in updateCapacity when adding bandwidth to a flow
     * due to the removal of some completed flow; if the increase is below this
     * threshold, no update is performed (to reduce unnecessary small changes
     * and speed up simulations). Note: this seems to hardly have any effect on
     * simulation speed, and might be removed in the future. @deprecated */
    boost::dynamic_properties snapDp; /**< dynamic_properties are used to take snapshots of the network in
     * Topology::printTopology(), e.g., write a graphml file with all the network metrics.
     * Generating this object every time in that method would be needlessly
     * expensive, so it's populated in the constructor provided that snapshots
     * are actually going to be taken (i.e., if sanpshotFreq > 0). */
    std::map<uint, uint> ASCustomersMap; /**< A map that keeps track of how many peers are present in each AS.
     * This is needed when calculating content rates in the cache optimization
     * problem. The map is populated in the constructor, when determining the
     * number of users per PON. */

    // Utility methods to streamline internal subroutines.
    /**
     * Iteratively adjusts the available bandwidth at each of the edges that
     * compose the specified route.
     * @param route The route we want to update the capacity of.
     * @param toAdd The amount of bandwidth that needs to be added (or subtracted, if negative) to each of the edges of the specified route.
     */
    void updateRouteCapacity(std::vector<Edge> route, Capacity toAdd);

    /**
     * Updates the ETA of a Flow after a bandwidth assignment change.
     * @param flow The Flow whose bandwidth has just changed.
     * @param scheduler A pointer to the Scheduler, which is needed to update the queue order in case of a change of ETA.
     */
    void updateEta(Flow* flow, Scheduler* scheduler);

public:
    /**
     * Builds the topology from a topology file. The recommended option is to
     * specify a graphml topology file (which MUST have a .graphml extension).
     * Look at some of the default graphml topologies in the Topologies folder
     * for examples of the properties recognized by the simulator. Comments
     * inside the constructor's code itself can also be useful in this regard.
     *
     * Alternatively, the topology can be specified using a text file with the following conventions
     * (no quotes, #something means number of elements of type something, arguments
     * in brackets are optional):
     * The first line must be "#CoreNodes #CoreEdges". #CoreEdges is assumed to be
     * the number of undirected core edges, there will actually be 2*#CoreEdges in
     * the graph (see below).
     * Then #CoreNodes lines should follow with the format:
     * "asid #PONNodes AvgPONCustomers DevPONCustomers downstreamCapacity upstreamCapacity (cs)"
     * asid is the identifier of the metro/core node and its relative Access Segment;
     * it is currently ignored and assumed to be a progressive integer starting from 0.
     * #PONNodes is the number of PONs attached to that metro/core node;
     * AvgPONCustomers is the mean value of a gaussian distribution describing the
     * number of customers attached to each of this node's PONs;
     * DevPONCustomers is the standard deviation of the above mentioned distribution;
     * downstreamCapacity and upstreamCapacity are the capacities in Mbps of each PON;
     * cs (optional) indicates that this metro/core node hosts the central repository
     * server.
     * After this, #CoreEdges lines should follow, each defining an edge between
     * two metro/core nodes, in the following format:
     * "sourceId destId capacity capacityRev"
     * The two capacities are used to define respectively the capacities
     * of the sourceId->destId edge and the destId->sourceId edge (to account for
     * asymmetric links); if negative, it is assumed to be infinite.
     * The graph is directed but for each pair of vertices, sourceId and destId, two
     * links (one per direction) are created.
     *
     * @param fileName the name of the file from which the topology should be generated.
     * @param vm the set of simulation parameters specified at command line by the user.
     */
    Topology(string fileName, po::variables_map vm);
    /**
     * Retrieves all the vertices with customers attached to them.
     * In other words, all the vertices with a non-zero value of
     * NetworkNode::ponCustomers.
     * @return a vector of vertices with end-users attached to them.
     */
    VertexVec getPonNodes() const;
    /**
     * Computes the hop-distance between two vertices.
     * @param source The source vertex of the route.
     * @param dest The destination vertex of the route.
     * @return The number of hops required to go from source to dest over the shortest route.
     */
    uint getDistance(uint source, uint dest) const;
    /**
     * Retrieves the shortest path between a source and a destination Vertex.
     * @param source The source vertex of the route.
     * @param dest The destination vertex of the route.
     * @return A vector of Edges that constitute the shortest route between source and dest.
     */
    std::vector<Edge> getRoute(uint source, uint dest) const;
    /**
     * Retrieves the shortest path between a source and a destination user.
     * @param source The source user of the route.
     * @param destination The destination user of the route.
     * @return A vector of Edges that constitute the shortest route between source and destination.
     */
    std::vector<Edge> getRoute(PonUser source, PonUser destination) const {
      return this->getRoute(source.first, destination.first);
    }
    /**
     * Updates the available capacity over the edges of the topology after the
     * addition or removal of a data Flow.
     * A centralized bandwidth sharing algorithm is implemented here. Each streaming
     * flow transiting over a particular link is entitled to at least an equal share
     * of the available capacity of that link. This process is repeated across each
     * link of the flow's route to identify the tightest capacity constraint
     * (i.e., the bottleneck), which determines the capacity assigned to the flow.
     * This capacity is updated dynamically whenever a flow enters (respectively leaves)
     * the network; however, only the bottleneck for the new (respectively leaving)
     * flow is used to calculate the new capacity allocation. This is done to
     * alleviate the time complexity of the bandwidth sharing algorithm,
     * at the expense of some loss in terms of optimal resource allocation.
     * No individual flow is allowed to receive more than MAX_FLOW_SPEED of
     * bandwidth, in an attempt to model both network operators constraints on
     * maximum available capacity per user and the limits imposed by TCP
     * mechanisms (e.g., due to the congestion control mechanisms, as we do not
     * simulate packet loss).
     * @param flow The data flow that has been just added or removed.
     * @param scheduler A pointer to the Scheduler, which will ensure that any change to the ETA of some Flow will be reflected in the ordering of the queued events.
     * @param addNotRemove True if the flow is a new transfer being added, false if it is a completed flow that needs to be removed.
     */
    void updateCapacity(Flow* flow, Scheduler* scheduler,
          bool addNotRemove);
    /**
     * Computes the traffic statistics for the round that just ended, stores them
     * in the NetworkStats structures, and prints them to screen.
     * @param currentRound The simulation round that just ended.
     * @param roundDuration The length in seconds of the simulation round that just ended.
     */
    void printNetworkStats(uint currentRound, uint roundDuration);
    /**
     * Removes all the Flows from the network edges, restoring their capacity
     * to their original maximum.
     */
    void resetFlows();
    /**
     * Updates the LoadMap by adding the data transmitted through the specified
     * Flow to each of the Edges composing its route. Invoked by the TopologyOracle
     * once the Flow has completed its transfer.
     * @param flow The Flow that has just completed its data transfer.
     */
    void updateLoadMap(Flow* flow);
    /**
     * Resets the content of LoadMap, setting the total transferred Mb for each
     * Edge in the topology to 0.
     */
    void resetLoadMap();
    /**
     * Retrieves the Vertex where the central repository is located.
     * @return The Vertex where the central repository is located.
     */
    Vertex getCentralServer() const;
    /**
     * Checks whether a Flow between source and dest would be local, i.e., if it
     * could be completed without leaving the Access Section (AS) of the destination.
     * @param source The proposed source for the Flow.
     * @param dest The destination of the Flow, i.e., the requester of the video item.
     * @return True if the transfer between source and dest would be local, false otherwise.
     */
    bool isLocal(Vertex source, Vertex dest) const;
    /**
     * Retrieves the Vertex hosting the local CDN cache for the AS of the specified node.
     * @param node The Vertex for which we are seeking the local CDN.
     * @return The Vertex hosting the local CDN cache for the AS of the specified node.
     */
    Vertex getLocalCache(Vertex node) const;
    /**
     * Retrieves a vector with all the Vertices hosting local CDN caches for some AS.
     * @return A vector with all the Vertices hosting local CDN caches for some AS.
     */
    VertexVec getLocalCacheNodes() const;
    /**
     * Checks whether there is enough spare capacity in the network to serve a request
     * from source to destination.
     * @param source The proposed source for the item requested.
     * @param destination The destination of the request.
     * @return True if there is enough bandwidth to at least transfer the item at its encoding bitrate, false otherwise.
     */
    bool isCongested(PonUser source, PonUser destination) const;
    /**
     * Adds a new uni-directional Edge between two Vertices. Utility method that
     * is not currently used in the simulator.
     * @deprecated
     * @param src The origin Vertex of the new Edge.
     * @param dest The destination Vertex of the new Edge.
     * @param cap The maximum capacity available on the new Edge. If negative, it is assumed to be UNLIMITED.
     * @param type The EdgeType of the new Edge.
     * @return True if the new Edge was successfully added, false otherwise.
     */
    bool addEdge(Vertex src, Vertex dest, Capacity cap, EdgeType type);
    /**
     * Prints a graphml snapshot of the current topology, including the available
     * capacity on metro/core edges. Useful to visualize the traffic generated
     * by the model being used.
     * @param time The SimTime at which the snapshot is being taken.
     * @param round The current round of simulation.
     */
    void printTopology(SimTime time, uint round);
    /**
     * Retrieves the index of the Access Section (AS) to which the specified user belongs.
     * @param node The user we want to know the AS of.
     * @return The index of the AS of the specified user.
     */
    uint getAsid(PonUser node) const {
      return topology[node.first].asid;
    }
    /**
     * Returns the type of the specified Edge, i.e., whether it is a core, metro,
     * or access edge.
     * @param e The Edge we want to know the type of.
     * @return The EdgeType of the specified Edge.
     * @see EdgeType
     */
    EdgeType getEdgeType(Edge e) const;
    /**
     * Retrieves the number of customers attached to the specified Vertex.
     * @param v The Vertex we want to know the number of customers attached to.
     * @return The number of cusotmers attached to the specified Vertex.
     */
    uint getPonCustomers(Vertex v) const;

  string getFileName() const {
    return fileName;
  }
  /**
   * Returns the total number of customers (or end-users) present in the network.
   * @return The total number of customers present in the network.
   */
  uint getNumCustomers() const {
    return numCustomers;
  }
  /**
   * Returns the total number of Access Sections (ASes) present in the network.
   * @return The total number of ASes present in the network.
   */
  uint getNumASes() const {
    return numASes;
  }

  NetworkStats getNetworkStats() const {
    return this->stats;
  }

  /**
   * Retrieves the number of customers for a given Access Section (AS).
   * @param asid The identifier of the AS we want to know the number of customers of.
   */
  uint getASCustomers(uint asid) const {
    return ASCustomersMap.at(asid);
  }
};

#endif	/* TOPOLOGY_HPP */


/*
@copyright Louis Dionne 2015
@copyright Zach Laine 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/fold_left.hpp>
#include <boost/hana/tuple.hpp>

#include "measure.hpp"
#include <cstdlib>

int main () {
    boost::hana::benchmark::measure([] {
        long double result = 0;
        for (int iteration = 0; iteration < 1 << 10; ++iteration) {
            auto values = boost::hana::make_tuple(
                <%= input_size.times.map { 'std::rand()' }.join(', ') %>
            );

            result += boost::hana::fold_left(values, 0, [](auto state, auto t) {
                return state + t;
            });
        }
    });
}


/*
 * test_gdal.cpp
 *
 * Test the Graph Library for Autonomous and Dynamic Systems
 *
 * author:  Pierrick Koch <pierrick.koch@laas.fr>
 * created: 2013-09-23
 * license: BSD
 */
#define BOOST_TEST_MODULE const_string test
#include <boost/test/included/unit_test.hpp>

#include <string>
#include <cmath>
#include <sstream>

#include "gdalwrap/gdal.hpp"

BOOST_AUTO_TEST_SUITE( gladys )

BOOST_AUTO_TEST_CASE( test_gdal_equality )
{
    std::string path = "/tmp/test_gladys_gdal.tif";
    gdalwrap::gdal gdal_to_file;
    gdal_to_file.set_size(5, 320, 240);
    gdal_to_file.set_utm(31);
    gdal_to_file.set_custom_origin(12.3, 14.5);
    gdal_to_file.set_transform(123, 456, 0.4, 0.6);
    gdal_to_file.save(path);
    // load the file we just saved
    gdalwrap::gdal gdal_from_file(path);

    BOOST_CHECK_EQUAL( gdal_from_file.get_width(),  gdal_to_file.get_width()  );
    BOOST_CHECK_EQUAL( gdal_from_file.get_height(), gdal_to_file.get_height() );
    BOOST_TEST_MESSAGE( "GDAL '==' OK" );
}

BOOST_AUTO_TEST_CASE( test_gdal_scale )
{
    size_t size_x  = 320, // width
           size_y  = 240; // height
    double scale_x = 0.4,
           scale_y = 0.6,
           utm_x   = 123.4,
           utm_y   = 456.7;
    std::string path = "/tmp/test_gladys_gdal.tif";
    gdalwrap::gdal obj;
    obj.set_size(2, size_x, size_y);
    obj.set_utm(31); // utm zone
    obj.set_custom_origin(12.3, 14.5);
    obj.set_transform(utm_x, utm_y, scale_x, scale_y);

    BOOST_CHECK_EQUAL(obj.index_pix(0, 0), 0);
    BOOST_CHECK_EQUAL(obj.index_pix(1, 0), 1);
    BOOST_CHECK_EQUAL(obj.index_pix(0, 1), size_x);
}

BOOST_AUTO_TEST_CASE( test_gdal_scale_utm )
{
    size_t size_x  = 320, // width
           size_y  = 240; // height
    double scale_x = 0.4,
           scale_y = 0.6,
           utm_x   = 123.4,
           utm_y   = 456.7;
    std::string path = "/tmp/test_gladys_gdal.tif";
    gdalwrap::gdal obj;
    obj.set_size(2, size_x, size_y);
    obj.set_utm(31); // utm zone
    obj.set_custom_origin(12.3, 14.5);
    obj.set_transform(utm_x, utm_y, scale_x, scale_y);

    BOOST_CHECK_EQUAL(obj.index_utm(utm_x,     utm_y),     0);
    BOOST_CHECK_EQUAL(obj.index_utm(utm_x + 1, utm_y),     std::round(1 / scale_x));
    BOOST_CHECK_EQUAL(obj.index_utm(utm_x,     utm_y + 1), std::round(1 / scale_y) * size_x);
}

// TODO test custom origin {x,y}

BOOST_AUTO_TEST_SUITE_END();


/*
 * This is a modified version of:
 *
 * https://github.com/joyeecheung/md5
 */

#ifndef MD5_HPP
#define MD5_HPP

#include <string>
#include <boost/cstdint.hpp>

class MD5
{
public:
    MD5();
    ~MD5();

    MD5& update(const unsigned char* in, std::size_t inputLen);
    MD5& update(const char* in, std::size_t inputLen);
    MD5& finalize();
    std::string getDigest() const;
    std::string toString() const;

private:

    void transform(const boost::uint8_t block[64]);

private:
    boost::uint8_t buffer[64];  // buffer of the raw data
    boost::uint8_t digest[16];  // result hash, little endian

    boost::uint32_t state[4];  // state (ABCD)
    boost::uint32_t lo, hi;    // number of bits, modulo 2^64 (lsb first)
    bool finalized;  // if the context has been finalized
};

#endif


#define BOOST_TEST_MODULE lue framework algorithm kernel
#include <hpx/config.hpp>
#include <boost/test/unit_test.hpp>
#include "lue/framework/algorithm/kernel.hpp"
#include "lue/framework/test/stream.hpp"

BOOST_AUTO_TEST_CASE(kernel_bool_1d)
{
    using Weight = bool;
    lue::Rank const rank = 1;
    using Kernel = lue::Kernel<Weight, rank>;
    using Shape = lue::ShapeT<Kernel>;

    lue::Radius const radius = 0;
    Shape shape{2 * radius + 1};
    Kernel kernel{shape};

    BOOST_CHECK_EQUAL(lue::rank<Kernel>, rank);
    BOOST_CHECK_EQUAL(lue::shape(kernel), shape);
    BOOST_CHECK_EQUAL(kernel.radius(), radius);
    BOOST_CHECK_EQUAL(lue::nr_elements(kernel), 1);

    std::int32_t sum = std::accumulate(kernel.begin(), kernel.end(), 0);
    BOOST_CHECK_EQUAL(sum, 0);
}

BOOST_AUTO_TEST_CASE(box_kernel_bool_1d)
{
    using Weight = bool;
    lue::Rank const rank = 1;
    using Kernel = lue::Kernel<Weight, rank>;
    using Shape = lue::ShapeT<Kernel>;

    lue::Radius const radius = 1;
    bool const value = true;

    Kernel kernel = lue::box_kernel<Weight, rank>(radius, value);

    Shape shape{2 * radius + 1};
    BOOST_CHECK_EQUAL(lue::shape(kernel), shape);
    BOOST_CHECK_EQUAL(kernel.radius(), radius);
    BOOST_CHECK_EQUAL(lue::nr_elements(kernel), 3);

    std::int32_t sum = std::accumulate(kernel.begin(), kernel.end(), 0);
    BOOST_CHECK_EQUAL(sum, 3);

    BOOST_CHECK_EQUAL(kernel[0], true);
    BOOST_CHECK_EQUAL(kernel[1], true);
    BOOST_CHECK_EQUAL(kernel[2], true);
}

BOOST_AUTO_TEST_CASE(box_kernel_bool_2d)
{
    using Weight = bool;
    lue::Rank const rank = 2;
    using Kernel = lue::Kernel<Weight, rank>;
    using Shape = lue::ShapeT<Kernel>;

    lue::Radius const radius = 1;
    bool const value = true;

    Kernel kernel = lue::box_kernel<Weight, rank>(radius, value);

    Shape shape{2 * radius + 1, 2 * radius + 1};
    BOOST_CHECK_EQUAL(lue::shape(kernel), shape);
    BOOST_CHECK_EQUAL(kernel.radius(), radius);
    BOOST_CHECK_EQUAL(lue::nr_elements(kernel), 9);

    BOOST_CHECK_EQUAL(kernel(0, 0), true);
    BOOST_CHECK_EQUAL(kernel(0, 1), true);
    BOOST_CHECK_EQUAL(kernel(0, 2), true);
    BOOST_CHECK_EQUAL(kernel(1, 0), true);
    BOOST_CHECK_EQUAL(kernel(1, 1), true);
    BOOST_CHECK_EQUAL(kernel(1, 2), true);
    BOOST_CHECK_EQUAL(kernel(2, 0), true);
    BOOST_CHECK_EQUAL(kernel(2, 1), true);
    BOOST_CHECK_EQUAL(kernel(2, 2), true);
}

BOOST_AUTO_TEST_CASE(circle_kernel_bool_1d)
{
    using Weight = bool;
    lue::Rank const rank = 1;
    using Kernel = lue::Kernel<Weight, rank>;
    using Shape = lue::ShapeT<Kernel>;

    lue::Radius const radius = 1;
    bool const value = true;

    Kernel kernel = lue::circle_kernel<Weight, rank>(radius, value);

    Shape shape{2 * radius + 1};
    BOOST_CHECK_EQUAL(lue::shape(kernel), shape);
    BOOST_CHECK_EQUAL(kernel.radius(), radius);
    BOOST_CHECK_EQUAL(lue::nr_elements(kernel), 3);

    std::int32_t sum = std::accumulate(kernel.begin(), kernel.end(), 0);
    BOOST_CHECK_EQUAL(sum, 3);

    BOOST_CHECK_EQUAL(kernel[0], true);
    BOOST_CHECK_EQUAL(kernel[1], true);
    BOOST_CHECK_EQUAL(kernel[2], true);
}

BOOST_AUTO_TEST_CASE(circle_kernel_bool_2d)
{
    using Weight = bool;
    lue::Rank const rank = 2;
    using Kernel = lue::Kernel<Weight, rank>;
    using Shape = lue::ShapeT<Kernel>;

    bool const value = true;

    {
        lue::Radius const radius = 1;
        Kernel kernel = lue::circle_kernel<Weight, rank>(radius, value);

        Shape shape{2 * radius + 1, 2 * radius + 1};
        BOOST_CHECK_EQUAL(lue::shape(kernel), shape);
        BOOST_CHECK_EQUAL(kernel.radius(), radius);
        BOOST_CHECK_EQUAL(lue::nr_elements(kernel), 9);

        BOOST_CHECK_EQUAL(kernel(0, 0), false);
        BOOST_CHECK_EQUAL(kernel(0, 1), true);
        BOOST_CHECK_EQUAL(kernel(0, 2), false);
        BOOST_CHECK_EQUAL(kernel(1, 0), true);
        BOOST_CHECK_EQUAL(kernel(1, 1), true);
        BOOST_CHECK_EQUAL(kernel(1, 2), true);
        BOOST_CHECK_EQUAL(kernel(2, 0), false);
        BOOST_CHECK_EQUAL(kernel(2, 1), true);
        BOOST_CHECK_EQUAL(kernel(2, 2), false);
    }

    {
        lue::Radius const radius = 2;
        Kernel kernel = lue::circle_kernel<Weight, rank>(radius, value);

        Shape shape{2 * radius + 1, 2 * radius + 1};
        BOOST_CHECK_EQUAL(lue::shape(kernel), shape);
        BOOST_CHECK_EQUAL(kernel.radius(), radius);
        BOOST_CHECK_EQUAL(lue::nr_elements(kernel), 25);

        BOOST_CHECK_EQUAL(kernel(0, 0), false);
        BOOST_CHECK_EQUAL(kernel(0, 1), false);
        BOOST_CHECK_EQUAL(kernel(0, 2), true);
        BOOST_CHECK_EQUAL(kernel(0, 3), false);
        BOOST_CHECK_EQUAL(kernel(0, 4), false);

        BOOST_CHECK_EQUAL(kernel(1, 0), false);
        BOOST_CHECK_EQUAL(kernel(1, 1), true);
        BOOST_CHECK_EQUAL(kernel(1, 2), true);
        BOOST_CHECK_EQUAL(kernel(1, 3), true);
        BOOST_CHECK_EQUAL(kernel(1, 4), false);

        BOOST_CHECK_EQUAL(kernel(2, 0), true);
        BOOST_CHECK_EQUAL(kernel(2, 1), true);
        BOOST_CHECK_EQUAL(kernel(2, 2), true);
        BOOST_CHECK_EQUAL(kernel(2, 3), true);
        BOOST_CHECK_EQUAL(kernel(2, 4), true);

        BOOST_CHECK_EQUAL(kernel(3, 0), false);
        BOOST_CHECK_EQUAL(kernel(3, 1), true);
        BOOST_CHECK_EQUAL(kernel(3, 2), true);
        BOOST_CHECK_EQUAL(kernel(3, 3), true);
        BOOST_CHECK_EQUAL(kernel(3, 4), false);

        BOOST_CHECK_EQUAL(kernel(4, 0), false);
        BOOST_CHECK_EQUAL(kernel(4, 1), false);
        BOOST_CHECK_EQUAL(kernel(4, 2), true);
        BOOST_CHECK_EQUAL(kernel(4, 3), false);
        BOOST_CHECK_EQUAL(kernel(4, 4), false);
    }
}


//==================================================================================================
/*!
  @file

  @copyright 2016 J.T.Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SCALAR_DIV_INCLUDED
#define BOOST_SIMD_FUNCTION_SCALAR_DIV_INCLUDED

#include <boost/simd/function/definition/div.hpp>
#include <boost/simd/arch/common/generic/function/div.hpp>

#endif


#include <boost/math/policies/policy.hpp>