/*
 * main.cpp
 *
 * Author: Philipp Kainz, Martin Urschler, Samuel Schulter, Paul Wohlhart, Vincent Lepetit
 * Institution: Medical University of Graz and Graz University of Technology, Austria
 *
 */

#include <iostream>
#include <string>
#include <fstream>
#include <ostream>
#include <eigen3/Eigen/Core>
#include <vector>
#include <stdexcept>

#include <boost/lexical_cast.hpp>

#include "AppContextCellDetRegr.h"
#include "DataLoaderCellDetRegr.h"
#include "SampleImgPatch.h"
#include "LabelMLRegr.h"
#include "SplitFunctionImgPatch.h"
#include "SplitEvaluatorMLRegr.h"
#include "LeafNodeStatisticsMLRegr.h"
#include "CellDetectorRegr.h"

#include "icgrf.h"

using namespace std;

// typedefs for easier use later
// define the template for the image patch (src is uchar (byte), integral is float))
typedef SplitFunctionImgPatch<uchar, float, AppContextCellDetRegr> TSplitFunctionImgPatch;
typedef RandomForest<SampleImgPatch, LabelMLRegr, TSplitFunctionImgPatch, SplitEvaluatorMLRegr<SampleImgPatch, AppContextCellDetRegr>, LeafNodeStatisticsMLRegr<SampleImgPatch, AppContextCellDetRegr>, AppContextCellDetRegr> TRegressionForest;
typedef Node<SampleImgPatch, LabelMLRegr, TSplitFunctionImgPatch, LeafNodeStatisticsMLRegr<SampleImgPatch, AppContextCellDetRegr>, AppContextCellDetRegr> TNode;

// load the forest and return a pointer to the instance
TRegressionForest* loadForest(AppContextCellDetRegr* apphp){
    // Set the forest parameters
    RFCoreParameters* rfparams = new RFCoreParameters();
    rfparams->m_debug_on = apphp->debug_on;
    rfparams->m_quiet = apphp->quiet;
    rfparams->m_max_tree_depth = apphp->max_tree_depth;
    rfparams->m_min_split_samples = apphp->min_split_samples;
    rfparams->m_num_node_tests = apphp->num_node_tests;
    rfparams->m_num_node_thresholds = apphp->num_node_thresholds;
    rfparams->m_num_random_samples_for_splitting = apphp->num_random_samples_for_splitting;
    rfparams->m_num_trees = apphp->num_trees;
    rfparams->m_bagging_method = (TREE_BAGGING_TYPE::Enum)apphp->bagging_type;
    if (!apphp->quiet)
        cout << (*rfparams) << endl;

    // fix the number of classes to 1
    apphp->num_classes = 1;
    // set univariate target
    apphp->num_target_variables = 1;

    // Loading the forest
    if (!apphp->quiet)
        std::cout << "Loading the forest ... ";
    TRegressionForest* rf;
    switch (apphp->method)
    {
    case RF_METHOD::STDRF:
        rf = new TRegressionForest(rfparams, apphp);
        break;
    default:
        throw std::runtime_error("main.cpp: unknown rf-method defined, must be 0!");
    }
    rf->Load(apphp->path_trees);
    if (!apphp->quiet){
        std::cout << " done." << std::endl;
    }
    return rf;
}

// train a forest on data specified in apphp
void train(AppContextCellDetRegr* apphp, std::vector<boost::filesystem::path>& trainImgFilenames)
{
    // 0) some initial stuff (scaling)
    for (size_t i = 0; i < apphp->patch_size.size(); i++)
        apphp->patch_size[i] = (int)((double)apphp->patch_size[i] * apphp->general_scaling_factor);

    // 1) read the data
    if (!apphp->quiet)
        cout << "Load training data ..." << endl;

    DataLoaderCellDetRegr mydataloader(apphp);
    DataSet<SampleImgPatch, LabelMLRegr> dataset_train = mydataloader.LoadTrainData(trainImgFilenames);

    int num_train_samples, num_classes, num_target_variables, num_feature_channels;
    mydataloader.GetTrainDataProperties(num_train_samples, num_classes, num_target_variables, num_feature_channels);
    // set some information for splitfunctions etc. in the app-context
    apphp->num_classes = num_classes;
    apphp->num_target_variables = num_target_variables;
    apphp->num_feature_channels = num_feature_channels;

    if (!apphp->quiet)
    {
        std::cout << "Dataset information: " << std::endl;
        std::cout << num_train_samples << " patches, ";
        std::cout << num_target_variables << "-d regression targets" << std::endl;
    }

    // 2) prepare the forest - fill the random forest core settings
    RFCoreParameters* rfparams = new RFCoreParameters();
    rfparams->m_debug_on = apphp->debug_on;
    rfparams->m_quiet = apphp->quiet;
    rfparams->m_max_tree_depth = apphp->max_tree_depth;
    rfparams->m_min_split_samples = apphp->min_split_samples;
    rfparams->m_num_node_tests = apphp->num_node_tests;
    rfparams->m_num_node_thresholds = apphp->num_node_thresholds;
    rfparams->m_num_random_samples_for_splitting = apphp->num_random_samples_for_splitting;
    rfparams->m_num_trees = apphp->num_trees;
    rfparams->m_bagging_method = (TREE_BAGGING_TYPE::Enum)apphp->bagging_type;
    if (!apphp->quiet)
        cout << (*rfparams) << endl;

    // 3) train the forest
    TRegressionForest* rf;
    switch (apphp->method)
    {
    case RF_METHOD::STDRF:
        rf = new TRegressionForest(rfparams, apphp);
        break;
    default:
        throw std::runtime_error("main.cpp: unknown rf-method defined, must be 0!");
    }
    if (!apphp->quiet)
        std::cout << "Training ... " << std::endl << std::flush;
    rf->Train(dataset_train);
    if (!apphp->quiet)
        cout << "done" << endl << flush;

    boost::filesystem::path path_trees(apphp->path_trees);
    if (!boost::filesystem::exists(path_trees)){
        if(boost::filesystem::create_directories(path_trees)) {
            if (!apphp->quiet)
                cout << "Created 'trees' directory in '" << path_trees << "'." << endl;
        } else {
            if (!apphp->quiet)
                cout << "Could not create 'trees' directory!" << endl;
        }
    }

    // 4.1) and save results
    rf->Save(apphp->path_trees);

    // 4.2) delete the training samples
    dataset_train.DeleteAllSamples();

    {
        // clear the static vector in the data loader and free the memory
        cout << "Clearing image_feature_data vector... ";
        DataLoaderCellDetRegr::image_feature_data.clear();
        std::vector<std::vector<cv::Mat> > tmp;
        tmp.swap(DataLoaderCellDetRegr::image_feature_data);
        cout << "done." << endl;
    }

    // delete the RF
    delete(rfparams);
    delete(rf);
}

// load a trained forest to predict the test images
void predictTestImages(AppContextCellDetRegr* apphp){

    // 0) some initial stuff (scaling)
    for (size_t i = 0; i < apphp->patch_size.size(); i++)
        apphp->patch_size[i] = (int)((double)apphp->patch_size[i] * apphp->general_scaling_factor);

    boost::filesystem::path path_pred(apphp->path_predictionimages);
    if (!boost::filesystem::exists(path_pred)){
        if(boost::filesystem::create_directories(path_pred)) {
            if (!apphp->quiet)
                cout << "Created 'predictions' directory in '" << path_pred << "'." << endl;
        } else {
            if (!apphp->quiet)
                cout << "Could not create 'predictions' directory!" << endl;
        }
    }

    TRegressionForest* rf;
    rf = loadForest(apphp);

    // let the detector run over the image and produce a distance transform of the estimated
    // cell centers
    CellDetectorRegr detector(rf,apphp);
    detector.PredictTestImages();

    // delete the rf
    delete(rf);
}

// analyze the leaf nodes in the forest
void analyze_leaves(AppContextCellDetRegr* apphp)
{
    TRegressionForest* rf;
    rf = loadForest(apphp);

    // 5) analyse the trees
    std::vector<std::vector<TNode* > > all_leafs;
    all_leafs = rf->GetAllLeafNodes();
    for (size_t t = 0; t < all_leafs.size(); t++)
    {
        for (size_t i = 0; i < all_leafs[t].size(); i++)
        {
            cout << "Tree " << t << ", node " << i << "/" << all_leafs[t].size() << " in depth " << all_leafs[t][i]->m_depth << ":" << endl;
            all_leafs[t][i]->m_leafstats->Print();
        }
    }
}

// analyze the split nodes
void analyze_splitnodes(AppContextCellDetRegr* apphp)
{
    TRegressionForest* rf;
    rf = loadForest(apphp);

    // analyse the trees and take split nodes
    std::vector<std::vector<TNode* > > all_internalnodes;
    all_internalnodes = rf->GetAllInternalNodes();
    cout << "tree node depth ch1 ch2 p1.x p1.y p2.x p2.y re1.x re1.y re1.width re1.height re2.x re2.y re2.width re2.height pxs.size th splitfctn" << endl;
    for (size_t t = 0; t < all_internalnodes.size(); t++)
    {
        for (size_t i = 0; i < all_internalnodes[t].size(); i++)
        {
            //cout << "Tree " << t << ", node " << i << "/" << all_internalnodes[t].size() << " in depth " << all_internalnodes[t][i]->m_depth << ":" << endl;
            cout << t << " " << i << " " << all_internalnodes[t][i]->m_depth << " ";
            all_internalnodes[t][i]->m_splitfunction->Print();
            cout << endl;
        }
    }
}

/**
 * Perform the leave one out cross validation.
 *
 * @param apphp
 */
void loocv(AppContextCellDetRegr* apphp){
    // train the forest on N-1 images
    // 1) load foreground and target image data
    // read in all file names from the training data directory
    vector<boost::filesystem::path> trainImgFilenames;
    vector<boost::filesystem::path> allImgFilenames;
    // list all image names in the train directory
    ListAllTrainImgFilenames(apphp, allImgFilenames);

    // the path to the validation image within the train directory
    boost::filesystem::path valImgFilename;

    string loocv_root;
    // make the cross validation directory structure
    if (apphp->use_unique_loocv_path){
        std::time_t t = std::time(NULL);
        char date_str[20];
        std::strftime(date_str, 20, "%Y-%m-%d_%H%M%S", std::localtime(&t));

        loocv_root = apphp->path_loocv + "/" + date_str;
    }else{
        loocv_root = apphp->path_loocv;
    }

    boost::filesystem::path path_loocv(loocv_root);
    if (!boost::filesystem::exists(path_loocv)){
        if(boost::filesystem::create_directories(path_loocv)) {
            if (!apphp->quiet)
                cout << "Created 'loocv' root directory in '" << loocv_root << "'." << endl;
        } else {
            if (!apphp->quiet)
                cout << "Could not create 'loocv' root directory!" << endl;
        }
    }

    // span a for loop around these data
    for (int cross_val_idx = 0; cross_val_idx < allImgFilenames.size(); cross_val_idx++){

        std::vector<string> dirs;
        dirs.push_back(loocv_root + "/" + boost::lexical_cast<string>(cross_val_idx) + "/" + apphp->loocv_path_trees_prefix);
        dirs.push_back(loocv_root + "/" + boost::lexical_cast<string>(cross_val_idx) + "/" + apphp->loocv_path_predictions_prefix);
        //dirs.push_back(loocv_root + "/" + boost::lexical_cast<string>(cross_val_idx) + "/" + apphp->loocv_path_detections_prefix);

        // make the directories
        for (int i = 0; i < dirs.size(); i++){
            boost::filesystem::path tmp(dirs[i]);
            if (!boost::filesystem::exists(tmp)){
                if (boost::filesystem::create_directories(tmp)){
                    if (!apphp->quiet)
                        cout << "Created directory '" << tmp.c_str() << "'." << endl;
                } else {
                    if (!apphp->quiet)
                        cout << "Could not create directory '" << tmp.c_str() << "'!" << endl;
                }
            }
        }

        // change some directories to the new locations
        // in order to redirect the output to the correct location
        apphp->path_trees = dirs[0] + "/";
        apphp->path_predictionimages = dirs[1] + "/";
        apphp->path_detectionimages = dirs[2] + "/";

        // redirect the path to the fixed dataset
        apphp->path_fixedDataset = loocv_root + "/" +
                boost::lexical_cast<string>(cross_val_idx) + "/fixed_dataset.txt";

        cout << endl << "Running leave-one-out cross validation ";
        cout << (cross_val_idx+1) << " of " << allImgFilenames.size() << endl;

        valImgFilename = allImgFilenames[cross_val_idx];
        trainImgFilenames.clear();
        trainImgFilenames = allImgFilenames;
        // remove the index from the train images
        trainImgFilenames.erase(trainImgFilenames.begin() + cross_val_idx);
        if (!apphp->quiet){
            cout << "training on n=" << trainImgFilenames.size() << " images" << endl;
            cout << "validation image: " << valImgFilename.filename().c_str() << endl;
        }

        // train the random forest model
        train(apphp, trainImgFilenames);

        // predict the validation image
        // let the detector run over the image and produce a distance transform of the estimated
        // cell centers
        TRegressionForest* rf;
        rf = loadForest(apphp);

        CellDetectorRegr detector(rf,apphp);
        detector.PredictSingleImage(valImgFilename);

        delete(rf);
    }
}

int main(int argc, char* argv[])
{

    // 1) read input arguments
    std::string path_configfile;
    if (argc < 2)
    {
        std::cout << "Specify a config file" << std::endl;
        exit(-1);
    }
    else
    {
        path_configfile = argv[1];
    }

    // 2) read configuration
    AppContextCellDetRegr apphp; // regression detection
    apphp.Read(path_configfile);

    if (!apphp.quiet){
        std::cout << "Parsed configuration file from " << path_configfile << std::endl;
        std::cout << apphp;
        std::cout << std::endl;
    }

    std::vector<boost::filesystem::path> trainImgFilenames;
    //try
    //{
    switch (apphp.mode) // the mode the forest is run in
    {
    case 0:
        ListAllTrainImgFilenames(&apphp, trainImgFilenames);
        train(&apphp, trainImgFilenames);
        break;
    case 1:
        predictTestImages(&apphp);
        break;
    case 2:
        ListAllTrainImgFilenames(&apphp, trainImgFilenames);
        train(&apphp, trainImgFilenames);
        predictTestImages(&apphp);
        break;
    case 3:
        analyze_leaves(&apphp);
        break;
    case 4:
        analyze_splitnodes(&apphp);
        break;
    case 5:
        // each cross validation is writing to a subdirectory
        // otherwise, tree and prediction files get overwritten every time!
        // do a leave-one-out cross-validation
        loocv(&apphp);
        break;
    default:
        throw std::runtime_error("main.cpp: wrong mode specified!");
        break;
    }
    //}
    //catch (std::exception& e)
    //{
    //	std::cout << "ERROR occured, exception thrown:" << std::endl;
    //	std::cout << e.what() << std::endl;
    //}

    if (!apphp.quiet)
        std::cout << "Program should be finished now ..." << std::endl << std::flush;
    return 0;
}


// Copyright (C) 2020  I. Bogoslavskyi, C. Stachniss
//
// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#include "utils/velodyne_utils.h"

#include <boost/algorithm/string.hpp>
#include <boost/algorithm/string/predicate.hpp>

#include <opencv/cv.h>
#include <opencv/highgui.h>

#include <cassert>
#include <fstream>
#include <string>
#include <vector>

#include "utils/cloud.h"

namespace depth_clustering {

using std::string;
using std::vector;

Cloud::Ptr ReadKittiCloudTxt(const string& path) {
  std::locale::global(std::locale("en_US.UTF-8"));
  fprintf(stderr, "Reading cloud from %s.\n", path.c_str());
  auto cloud_ptr = Cloud::Ptr(new Cloud);
  std::ifstream file(path.c_str());
  for (std::string line; std::getline(file, line, '\n');) {
    // here we parse the line
    vector<string> coords_str;
    boost::split(coords_str, line, boost::is_any_of(" "));
    if (coords_str.size() != 4) {
      fprintf(stderr, "ERROR: format of line is wrong.\n");
      continue;
    }
    RichPoint point;
    point.x() = std::stof(coords_str[0]);
    point.y() = std::stof(coords_str[1]);
    point.z() = std::stof(coords_str[2]);
    cloud_ptr->push_back(point);
  }
  return cloud_ptr;
}

Cloud::Ptr CloudFromMat(const cv::Mat& image, const ProjectionParams& config) {
  auto cloud_ptr = Cloud::Ptr(new Cloud);
  for (int row = 0; row < image.rows; ++row) {
    for (int col = 0; col < image.cols; ++col) {
      float meters = image.at<float>(row, col);
      float angle_xy = config.AngleFromCol(col).val();
      float angle_z = config.AngleFromRow(row).val();
      RichPoint point;
      point.x() = meters * cos(angle_z) * cos(angle_xy);
      point.y() = meters * cos(angle_z) * sin(angle_xy);
      point.z() = meters * sin(angle_z);
      point.ring() = row;
      cloud_ptr->push_back(point);
    }
  }
  return cloud_ptr;
}

Cloud::Ptr ReadKittiCloud(const string& path) {
  Cloud::Ptr cloud(new Cloud);
  std::fstream file(path.c_str(), std::ios::in | std::ios::binary);
  if (file.good()) {
    file.seekg(0, std::ios::beg);
    float intensity = 0;
    for (int i = 0; file.good() && !file.eof(); ++i) {
      RichPoint point;
      file.read(reinterpret_cast<char*>(&point.x()), sizeof(float));
      file.read(reinterpret_cast<char*>(&point.y()), sizeof(float));
      file.read(reinterpret_cast<char*>(&point.z()), sizeof(float));
      // ignore intensity
      file.read(reinterpret_cast<char*>(&intensity), sizeof(float));
      cloud->push_back(point);
    }
    file.close();
  }
  return cloud;
}

cv::Mat FixKITTIDepth(const cv::Mat& original) {
  cv::Mat fixed = original;
  for (int r = 0; r < fixed.rows; ++r) {
    auto correction = MOOSMAN_CORRECTIONS[r];
    for (int c = 0; c < fixed.cols; ++c) {
      if (fixed.at<float>(r, c) < 0.001f) {
        continue;
      }
      fixed.at<float>(r, c) -= correction;
    }
  }
  return fixed;
}

cv::Mat MatFromDepthPng(const string& path) {
  cv::Mat depth_image = cv::imread(path, CV_LOAD_IMAGE_ANYDEPTH);
  depth_image.convertTo(depth_image, CV_32F);
  depth_image /= 500.;
  return FixKITTIDepth(depth_image);
}

}  // namespace depth_clustering


// Copyright 2004 The Trustees of Indiana University.

// Use, modification and distribution is subject to the Boost Software
// License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//  Authors: Nick Edmonds
//           Andrew Lumsdaine

// #define PBGL_ACCOUNTING

#include <boost/graph/use_mpi.hpp>

#include <boost/graph/distributed/compressed_sparse_row_graph.hpp>
#include <boost/graph/distributed/adjacency_list.hpp>

#include <boost/graph/distributed/mpi_process_group.hpp>

#include <boost/test/minimal.hpp>
#include <boost/random.hpp>
#include <boost/property_map/parallel/distributed_property_map.hpp>
#include <boost/graph/distributed/graphviz.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/properties.hpp>

#include <boost/graph/rmat_graph_generator.hpp>
#include <boost/graph/small_world_generator.hpp>
#include <boost/graph/erdos_renyi_generator.hpp>

#include <boost/graph/distributed/connected_components.hpp>
#include <boost/graph/distributed/connected_components_parallel_search.hpp>
#include <boost/graph/distributed/betweenness_centrality.hpp>
#include <boost/graph/distributed/delta_stepping_shortest_paths.hpp>

#include <time.h>
#include <sys/time.h>

#include <iostream>
#include <iomanip>
#include <vector>
#include <stdint.h>

// Edge distribution tags select a generator
struct rmat_edge_distribution_tag { };
struct rmat_unique_edge_distribution_tag { };

using namespace boost;
using boost::graph::distributed::mpi_process_group;

/****************************************************************************
 * Timing
 ****************************************************************************/
#ifndef PBGL_ACCOUNTING

typedef double time_type;

inline time_type get_time()
{
  timeval tp;
  gettimeofday(&tp, 0);
  return tp.tv_sec + tp.tv_usec / 1000000.0;
}

std::string print_time(time_type t)
{
  std::ostringstream out;
  out << std::setiosflags(std::ios::fixed) << std::setprecision(2) << t;
  return out.str();
}

#endif // PBGL_ACCOUNTING

/****************************************************************************
 * Edge weight generator iterator                                           *
 ****************************************************************************/
template<typename F, typename RandomGenerator>
class generator_iterator
{
public:
  typedef std::input_iterator_tag iterator_category;
  typedef typename F::result_type value_type;
  typedef const value_type&       reference;
  typedef const value_type*       pointer;
  typedef void                    difference_type;

  explicit
  generator_iterator(RandomGenerator& gen, const F& f = F())
    : f(f), gen(&gen)
  {
    value = this->f(gen);
  }

  reference operator*() const  { return value; }
  pointer   operator->() const { return &value; }

  generator_iterator& operator++()
  {
    value = f(*gen);
    return *this;
  }

  generator_iterator operator++(int)
  {
    generator_iterator temp(*this);
    ++(*this);
    return temp;
  }

  bool operator==(const generator_iterator& other) const
  { return f == other.f; }

  bool operator!=(const generator_iterator& other) const
  { return !(*this == other); }

private:
  F f;
  RandomGenerator* gen;
  value_type value;
};

template<typename F, typename RandomGenerator>
inline generator_iterator<F, RandomGenerator>
make_generator_iterator( RandomGenerator& gen, const F& f)
{ return generator_iterator<F, RandomGenerator>(gen, f); }

/****************************************************************************
 * Edge Property                                                            *
 ****************************************************************************/
typedef int weight_type;

struct WeightedEdge {
  WeightedEdge(weight_type weight = 0) : weight(weight) { }

  weight_type weight;

  template<typename Archiver>
  void serialize(Archiver& ar, const unsigned int /*version*/)
  {
    ar & weight;
  }
};

/****************************************************************************
 * Algorithm Tests                                                          *
 ****************************************************************************/
template <typename Graph>
void test_directed_sequential_algorithms(const Graph& g)
{ }

template <typename Graph>
void test_undirected_sequential_algorithms(const Graph& g)
{
  std::vector<unsigned int> componentS(num_vertices(g));
  typedef iterator_property_map<
      std::vector<unsigned int>::iterator,
      typename property_map<Graph, vertex_index_t>::type>
    ComponentMap;
  ComponentMap component(componentS.begin(), get(vertex_index, g));

  time_type start = get_time();
  unsigned int num_components = connected_components(g, component);
  time_type end = get_time();

  std::cerr << "    Sequential connected Components time = "
            << print_time(end - start) << " seconds.\n"
            << "    " << num_components << " components identified\n";
}

template <typename Graph, typename EdgeWeightMap>
void test_directed_csr_only_algorithms(const Graph& g, EdgeWeightMap weight,
                                       typename graph_traits<Graph>::vertices_size_type num_sources,
                                       typename property_traits<EdgeWeightMap>::value_type C)
{
  typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor;
  typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator;
  typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
  typedef typename graph_traits<Graph>::edges_size_type edges_size_type;

  typedef typename boost::graph::parallel::process_group_type<Graph>::type
    process_group_type;

  process_group_type pg = process_group(g);
  typename process_group_type::process_id_type id = process_id(pg);
  typename process_group_type::process_size_type p = num_processes(pg);

  vertices_size_type n = num_vertices(g);
  n = boost::parallel::all_reduce(pg, n, std::plus<vertices_size_type>());

  edges_size_type m = num_edges(g);
  m = boost::parallel::all_reduce(pg, m, std::plus<edges_size_type>());

  //
  // Betweenness Centrality (Approximate)
  //
  queue<vertex_descriptor> delta_stepping_vertices;

  {
    // Distributed Centrality Map
    typedef typename property_map<Graph, vertex_index_t>::const_type IndexMap;
    typedef iterator_property_map<std::vector<int>::iterator, IndexMap> CentralityMap;

    std::vector<int> centralityS(num_vertices(g), 0);
    CentralityMap centrality(centralityS.begin(), get(vertex_index, g));

    // Calculate number of vertices of degree 0
    vertices_size_type n0 = 0;
    BGL_FORALL_VERTICES_T(v, g, Graph) {
      if (out_degree(v, g) == 0) n0++;
    }
    n0 = boost::parallel::all_reduce(pg, n0, std::plus<vertices_size_type>());

    queue<vertex_descriptor> sources;

    {
      assert(num_sources >= p);  // Don't feel like adding a special case for num_sources < p

      minstd_rand gen;
      uniform_int<vertices_size_type> rand_vertex(0, num_vertices(g) - 1);
      std::vector<vertex_descriptor> all_sources, local_sources;
      vertices_size_type local_vertices = vertices_size_type(floor((double)num_sources / p));
      local_vertices += (id < (num_sources - (p * local_vertices)) ? 1 : 0);

      while (local_vertices > 0) {
        vertex_iterator iter = vertices(g).first;
        std::advance(iter, rand_vertex(gen));

        if (out_degree(*iter, g) != 0
            && std::find(local_sources.begin(), local_sources.end(), *iter) == local_sources.end()) {
          local_sources.push_back(*iter);
          --local_vertices;
        }
      }
      all_gather(pg, local_sources.begin(), local_sources.end(), all_sources);
      std::sort(all_sources.begin(), all_sources.end());
      for (typename std::vector<vertex_descriptor>::iterator iter = all_sources.begin();
           iter != all_sources.end(); ++iter) {
        sources.push(*iter);
        delta_stepping_vertices.push(*iter);
      }
    }

    // NOTE: The delta below assumes uniform edge weight distribution
    time_type start = get_time();
    brandes_betweenness_centrality(g, buffer(sources).weight_map(weight).
                                   centrality_map(centrality).lookahead((m / n) * (C / 2)));
    time_type end = get_time();

    edges_size_type exactTEPs = edges_size_type(floor(7 * n* (n - n0) / (end - start)));

    if (id == 0)
      std::cerr << "    Betweenness Centrality Approximate (" << num_sources << " sources) = "
                << print_time(end - start) << " (" << exactTEPs << " TEPs)\n";
  }

  //
  // Delta stepping performance (to compare to SSSP inside BC
  //
  if (false) {
    typedef typename property_map<Graph, vertex_index_t>::const_type IndexMap;
    typedef iterator_property_map<std::vector<int>::iterator, IndexMap> DistanceMap;

    std::vector<int> distanceS(num_vertices(g), 0);
    DistanceMap distance(distanceS.begin(), get(vertex_index, g));

    while(!delta_stepping_vertices.empty()) {

      time_type start = get_time();
      delta_stepping_shortest_paths(g, delta_stepping_vertices.top(), dummy_property_map(),
                                    distance, weight);
      time_type end = get_time();

      delta_stepping_vertices.pop();
      distance.reset();

      if (id == 0)
        std::cerr << "    Delta-stepping shortest paths = " << print_time(end - start)
                  << std::endl;
    }
  }

}

template <typename Graph>
void test_directed_algorithms(const Graph& g)
{
}

template <typename Graph>
void test_undirected_algorithms(const Graph& g)
{
  typedef typename boost::graph::parallel::process_group_type<Graph>::type
    process_group_type;

  process_group_type pg = process_group(g);
  typename process_group_type::process_id_type id = process_id(pg);
  typename process_group_type::process_size_type p = num_processes(pg);

  // Connected Components
  std::vector<unsigned int> local_components_vec(num_vertices(g));
  typedef iterator_property_map<
      std::vector<unsigned int>::iterator,
      typename property_map<Graph, vertex_index_t>::type>
    ComponentMap;
  ComponentMap component(local_components_vec.begin(), get(vertex_index, g));

  int num_components;

  time_type start = get_time();
  num_components = connected_components(g, component);
  time_type end = get_time();
  if (id == 0)
    std::cerr << "    Connected Components time = " << print_time(end - start)
              << " seconds.\n"
              << "    " << num_components << " components identified\n";

  start = get_time();
  num_components = boost::graph::distributed::connected_components_ps(g, component);
  end = get_time();
  if (id == 0)
    std::cerr << "    Connected Components (parallel search) time = "
              << print_time(end - start) << " seconds.\n"
              << "    " << num_components << " components identified\n";
}

/****************************************************************************
 * Graph Type Tests                                                         *
 ****************************************************************************/

// TODO: Re-seed PRNG before sequential tests to get the same graph as the
// distributed case?

//
// Compressed Sparse Row
//

// R-MAT
template <typename ProcessGroup, typename RandomGenerator, typename Distribution>
void test_csr(const ProcessGroup& pg, RandomGenerator& gen, Distribution& distrib,
              bool sequential_tests, size_t N, size_t M, size_t C,
              double a, double b, double c, double d, size_t num_sources,
              rmat_edge_distribution_tag)
{
  if (process_id(pg) == 0)
    std::cerr << "  R-MAT\n";

  typedef compressed_sparse_row_graph<directedS, no_property, WeightedEdge, no_property,
                                      distributedS<mpi_process_group> > Graph;

  Graph g(sorted_rmat_iterator<RandomGenerator, Graph>(gen, N, M, a, b, c, d),
          sorted_rmat_iterator<RandomGenerator, Graph>(),
          make_generator_iterator(gen, uniform_int<int>(1, C)),
          N, pg, distrib);

  test_directed_algorithms(g);
  test_directed_csr_only_algorithms(g, get(&WeightedEdge::weight, g), num_sources, C);

  if (sequential_tests && process_id(pg) == 0) {
    typedef compressed_sparse_row_graph<directedS, no_property, WeightedEdge>
      seqGraph;

    seqGraph sg(edges_are_sorted,
                sorted_rmat_iterator<RandomGenerator, seqGraph>(gen, N, M, a, b, c, d),
                sorted_rmat_iterator<RandomGenerator, seqGraph>(),
                make_generator_iterator(gen, uniform_int<int>(1, C)),
                N);

    test_directed_sequential_algorithms(sg);
  }
}

// R-MAT with unique edges
template <typename ProcessGroup, typename RandomGenerator, typename Distribution>
void test_csr(const ProcessGroup& pg, RandomGenerator& gen, Distribution& distrib,
              bool sequential_tests, size_t N, size_t M, size_t C,
              double a, double b, double c, double d, size_t num_sources,
              rmat_unique_edge_distribution_tag)
{
  if (process_id(pg) == 0)
    std::cerr << "  R-MAT - unique\n";

  typedef compressed_sparse_row_graph<directedS, no_property, WeightedEdge, no_property,
                                      distributedS<mpi_process_group> > Graph;

  // Last boolean parameter makes R-MAT bidirectional
  Graph g(sorted_unique_rmat_iterator<RandomGenerator, Graph>(gen, N, M, a, b, c, d),
          sorted_unique_rmat_iterator<RandomGenerator, Graph>(),
          make_generator_iterator(gen, uniform_int<int>(1, C)),
          N, pg, distrib);

  test_directed_algorithms(g);
  test_directed_csr_only_algorithms(g, get(&WeightedEdge::weight, g), num_sources, C);

  if (sequential_tests && process_id(pg) == 0) {
    typedef compressed_sparse_row_graph<directedS, no_property, WeightedEdge>
      seqGraph;

    seqGraph sg(edges_are_sorted,
                sorted_unique_rmat_iterator<RandomGenerator, seqGraph>(gen, N, M, a, b, c, d),
                sorted_unique_rmat_iterator<RandomGenerator, seqGraph>(),
                make_generator_iterator(gen, uniform_int<int>(1, C)),
                N);

    test_directed_sequential_algorithms(sg);
  }
}

// Erdos-Renyi
template <typename ProcessGroup, typename RandomGenerator, typename Distribution>
void test_csr(const ProcessGroup& pg, RandomGenerator& gen, Distribution& distrib,
              bool sequential_tests, size_t N, size_t M, size_t C, size_t num_sources)
{
  if (process_id(pg) == 0)
    std::cerr << "  Erdos-Renyi\n";

  double _p = static_cast<double>(M) / (N*N);

  typedef compressed_sparse_row_graph<directedS, no_property, WeightedEdge, no_property,
                                      distributedS<mpi_process_group> > Graph;

  Graph g(sorted_erdos_renyi_iterator<RandomGenerator, Graph>(gen, N, _p/2),
          sorted_erdos_renyi_iterator<RandomGenerator, Graph>(),
          make_generator_iterator(gen, uniform_int<int>(1, C)),
          N, pg, distrib);

  test_directed_algorithms(g);
  test_directed_csr_only_algorithms(g, get(&WeightedEdge::weight, g), num_sources, C);

  if (sequential_tests && process_id(pg) == 0) {
    typedef compressed_sparse_row_graph<directedS, no_property, WeightedEdge>
      seqGraph;

    seqGraph sg(edges_are_sorted,
                sorted_erdos_renyi_iterator<RandomGenerator, seqGraph>(gen, N, _p/2),
                sorted_erdos_renyi_iterator<RandomGenerator, seqGraph>(),
                make_generator_iterator(gen, uniform_int<int>(1, C)),
                N);

    test_directed_sequential_algorithms(sg);
  }
}

// Small World
template <typename ProcessGroup, typename RandomGenerator, typename Distribution>
void test_csr(const ProcessGroup& pg, RandomGenerator& gen, Distribution& distrib,
              bool sequential_tests, size_t N, size_t M, size_t C, double p,
              size_t num_sources)
{
  if (process_id(pg) == 0)
    std::cerr << "  Small-World\n";

  int k = M / N;

  typedef compressed_sparse_row_graph<directedS, no_property, WeightedEdge, no_property,
                                      distributedS<mpi_process_group> > Graph;

  Graph g(small_world_iterator<RandomGenerator, Graph>(gen, N, k, p),
          small_world_iterator<RandomGenerator, Graph>(),
          make_generator_iterator(gen, uniform_int<int>(1, C)),
          N, pg, distrib);

  test_directed_algorithms(g);
  test_directed_csr_only_algorithms(g, get(&WeightedEdge::weight, g), num_sources, C);

  if (sequential_tests && process_id(pg) == 0) {
    typedef compressed_sparse_row_graph<directedS, no_property, WeightedEdge>
      seqGraph;

    seqGraph sg(edges_are_sorted,
                small_world_iterator<RandomGenerator, seqGraph>(gen, N, k, p),
                small_world_iterator<RandomGenerator, seqGraph>(),
                make_generator_iterator(gen, uniform_int<int>(1, C)),
                N);

    test_directed_sequential_algorithms(sg);
  }
}

//
// Adjacency List
//

// R-MAT
template <typename ProcessGroup, typename RandomGenerator, typename Distribution>
void test_adjacency_list(const ProcessGroup& pg, RandomGenerator& gen, Distribution& distrib,
                         bool sequential_tests, size_t N, size_t M, size_t C,
                         double a, double b, double c, double d,
                         rmat_edge_distribution_tag)
{
  if (process_id(pg) == 0)
    std::cerr << "R-MAT\n";

  {
    typedef adjacency_list<vecS, distributedS<mpi_process_group, vecS>,
                           directedS, no_property, WeightedEdge> Graph;

    Graph g(rmat_iterator<RandomGenerator, Graph>(gen, N, M, a, b, c, d),
            rmat_iterator<RandomGenerator, Graph>(),
            make_generator_iterator(gen, uniform_int<int>(1, C)),
            N, pg, distrib);

    test_directed_algorithms(g);
  }

  {
    typedef adjacency_list<vecS, distributedS<mpi_process_group, vecS>,
                           undirectedS, no_property, WeightedEdge> Graph;

    Graph g(rmat_iterator<RandomGenerator, Graph>(gen, N, M, a, b, c, d),
            rmat_iterator<RandomGenerator, Graph>(),
            make_generator_iterator(gen, uniform_int<int>(1, C)),
            N, pg, distrib);

    test_undirected_algorithms(g);
  }

  if (sequential_tests && process_id(pg) == 0) {
    {
      typedef adjacency_list<vecS, vecS, directedS, no_property, property<edge_weight_t, int> >
        seqGraph;

      seqGraph sg(rmat_iterator<RandomGenerator, seqGraph>(gen, N, M, a, b, c, d),
                  rmat_iterator<RandomGenerator, seqGraph>(),
                  make_generator_iterator(gen, uniform_int<int>(1, C)),
                  N);

      test_directed_sequential_algorithms(sg);
    }

    {
      typedef adjacency_list<vecS, vecS, undirectedS, no_property, property<edge_weight_t, int> >
        seqGraph;

      seqGraph sg(rmat_iterator<RandomGenerator, seqGraph>(gen, N, M, a, b, c, d),
                  rmat_iterator<RandomGenerator, seqGraph>(),
                  make_generator_iterator(gen, uniform_int<int>(1, C)),
                  N);

      test_undirected_sequential_algorithms(sg);
    }
  }
}

// R-MAT with unique edges
template <typename ProcessGroup, typename RandomGenerator, typename Distribution>
void test_adjacency_list(const ProcessGroup& pg, RandomGenerator& gen, Distribution& distrib,
                         bool sequential_tests, size_t N, size_t M, size_t C,
                         double a, double b, double c, double d,
                         rmat_unique_edge_distribution_tag)
{
  if (process_id(pg) == 0)
    std::cerr << "  R-MAT - unique\n";

  {
    typedef adjacency_list<vecS, distributedS<mpi_process_group, vecS>,
                           directedS, no_property, WeightedEdge> Graph;

    Graph g(sorted_unique_rmat_iterator<RandomGenerator, Graph>(gen, N, M, a, b, c, d),
            sorted_unique_rmat_iterator<RandomGenerator, Graph>(),
            make_generator_iterator(gen, uniform_int<int>(1, C)),
            N, pg, distrib);

    test_directed_algorithms(g);
  }

  {
    typedef adjacency_list<vecS, distributedS<mpi_process_group, vecS>,
                           undirectedS, no_property, WeightedEdge> Graph;

    Graph g(sorted_unique_rmat_iterator<RandomGenerator, Graph>(gen, N, M, a, b, c, d),
            sorted_unique_rmat_iterator<RandomGenerator, Graph>(),
            make_generator_iterator(gen, uniform_int<int>(1, C)),
            N, pg, distrib);

    test_undirected_algorithms(g);
  }

  if (sequential_tests && process_id(pg) == 0) {
    {
      typedef adjacency_list<vecS, vecS, directedS, no_property, property<edge_weight_t, int> >
        seqGraph;

      seqGraph sg(sorted_unique_rmat_iterator<RandomGenerator, seqGraph>(gen, N, M, a, b, c, d),
                  sorted_unique_rmat_iterator<RandomGenerator, seqGraph>(),
                  make_generator_iterator(gen, uniform_int<int>(1, C)),
                  N);

      test_directed_sequential_algorithms(sg);
    }

    {
      typedef adjacency_list<vecS, vecS, undirectedS, no_property, property<edge_weight_t, int> >
        seqGraph;

      seqGraph sg(sorted_unique_rmat_iterator<RandomGenerator, seqGraph>(gen, N, M, a, b, c, d),
                  sorted_unique_rmat_iterator<RandomGenerator, seqGraph>(),
                  make_generator_iterator(gen, uniform_int<int>(1, C)),
                  N);

      test_undirected_sequential_algorithms(sg);
    }
  }
}

// Erdos-Renyi
template <typename ProcessGroup, typename RandomGenerator, typename Distribution>
void test_adjacency_list(const ProcessGroup& pg, RandomGenerator& gen, Distribution& distrib,
                         bool sequential_tests, size_t N, size_t M, size_t C)
{
  if (process_id(pg) == 0)
    std::cerr << "  Erdos-Renyi\n";

  double _p = static_cast<double>(M) / N*N;

  {
    typedef adjacency_list<vecS, distributedS<mpi_process_group, vecS>,
                           directedS, no_property, WeightedEdge> Graph;

    Graph g(erdos_renyi_iterator<RandomGenerator, Graph>(gen, N, _p/2),
            erdos_renyi_iterator<RandomGenerator, Graph>(),
            make_generator_iterator(gen, uniform_int<int>(1, C)),
            N, pg, distrib);

    test_directed_algorithms(g);
  }

  {
    typedef adjacency_list<vecS, distributedS<mpi_process_group, vecS>,
                           undirectedS, no_property, WeightedEdge> Graph;

    Graph g(erdos_renyi_iterator<RandomGenerator, Graph>(gen, N, _p/2),
            erdos_renyi_iterator<RandomGenerator, Graph>(),
            make_generator_iterator(gen, uniform_int<int>(1, C)),
            N, pg, distrib);

    test_undirected_algorithms(g);
  }

  if (sequential_tests && process_id(pg) == 0) {
    {
      typedef adjacency_list<vecS, vecS, directedS, no_property, property<edge_weight_t, int> >
        seqGraph;

      seqGraph sg(erdos_renyi_iterator<RandomGenerator, seqGraph>(gen, N, _p/2),
                  erdos_renyi_iterator<RandomGenerator, seqGraph>(),
                  make_generator_iterator(gen, uniform_int<int>(1, C)),
                  N);

      test_directed_sequential_algorithms(sg);
    }

    {
      typedef adjacency_list<vecS, vecS, undirectedS, no_property, property<edge_weight_t, int> >
        seqGraph;

      seqGraph sg(erdos_renyi_iterator<RandomGenerator, seqGraph>(gen, N, _p/2),
                  erdos_renyi_iterator<RandomGenerator, seqGraph>(),
                  make_generator_iterator(gen, uniform_int<int>(1, C)),
                  N);

      test_undirected_sequential_algorithms(sg);
    }
  }
}

// Small World
template <typename ProcessGroup, typename RandomGenerator, typename Distribution>
void test_adjacency_list(const ProcessGroup& pg, RandomGenerator& gen, Distribution& distrib,
                         bool sequential_tests, size_t N, size_t M, size_t C, double p)
{
  if (process_id(pg) == 0)
    std::cerr << "  Small-World\n";

  int k = M / N;

  {
    typedef adjacency_list<vecS, distributedS<mpi_process_group, vecS>,
                           directedS, no_property, WeightedEdge> Graph;

    Graph g(small_world_iterator<RandomGenerator, Graph>(gen, N, k, p),
            small_world_iterator<RandomGenerator, Graph>(),
            make_generator_iterator(gen, uniform_int<int>(1, C)),
            N, pg, distrib);

    test_directed_algorithms(g);
  }

  {
    typedef adjacency_list<vecS, distributedS<mpi_process_group, vecS>,
                           undirectedS, no_property, WeightedEdge> Graph;

    Graph g(small_world_iterator<RandomGenerator, Graph>(gen, N, k, p),
            small_world_iterator<RandomGenerator, Graph>(),
            make_generator_iterator(gen, uniform_int<int>(1, C)),
            N, pg, distrib);

    test_undirected_algorithms(g);
  }

  if (sequential_tests && process_id(pg) == 0) {
    {
      typedef adjacency_list<vecS, vecS, directedS, no_property, property<edge_weight_t, int> >
        seqGraph;

      seqGraph sg(small_world_iterator<RandomGenerator, seqGraph>(gen, N, k, p),
                  small_world_iterator<RandomGenerator, seqGraph>(),
                  make_generator_iterator(gen, uniform_int<int>(1, C)),
                  N);

      test_directed_sequential_algorithms(sg);
    }

    {
      typedef adjacency_list<vecS, vecS, undirectedS, no_property, property<edge_weight_t, int> >
        seqGraph;

      seqGraph sg(small_world_iterator<RandomGenerator, seqGraph>(gen, N, k, p),
                  small_world_iterator<RandomGenerator, seqGraph>(),
                  make_generator_iterator(gen, uniform_int<int>(1, C)),
                  N);

      test_undirected_sequential_algorithms(sg);
    }
  }
}

void usage()
{
  std::cerr << "Algorithm Performance Test\n"
            << "Usage : algorithm_performance [options]\n\n"
            << "Options are:\n"
            << "\t--vertices v\t\t\tNumber of vertices in the graph\n"
            << "\t--edges v\t\t\tNumber of edges in the graph\n"
            << "\t--seed s\t\t\tSeed for synchronized random number generator\n"
            << "\t--max-weight miw\t\tMaximum integer edge weight\n"
            << "\t--rewire-probability\t\tRewire-probabitility (p) for small-world graphs\n"
            << "\t--dot\t\t\t\tEmit a dot file containing the graph\n"
            << "\t--verify\t\t\tVerify result\n"
            << "\t--degree-dist\t\t\tOutput degree distribution of graph\n"
            << "\t--sequential-graph\t\tRun sequential graph tests\n"
            << "\t--erdos-renyi\t\t\tRun tests on Erdos-Renyi graphs\n"
            << "\t--small-world\t\t\tRun tests on Small World graphs\n"
            << "\t--rmat\t\t\t\tRun tests on R-MAT graphs\n"
            << "\t--rmat-unique\t\t\tRun tests on R-MAT graphs with no duplicate edges\n"
            << "\t--csr <bool>\t\t\tRun tests using CSR graphs\n"
            << "\t--adjacency-list <bool>\t\tRun tests using Adjacency List graphs\n";
}

int test_main(int argc, char* argv[])
{
  mpi::environment env(argc, argv);

  rand48 gen;

  // Default args
  size_t n = 100,
    m = 8*n,
    c = 100,
    num_sources = 32,
    num_headers = 16 * 1024,
    batch_buffer_size = 1024 * 1024;
  uint64_t seed = 1;
  bool emit_dot_file = false,
    verify = false,
    sequential_graph = false,
    show_degree_dist = false,
    erdos_renyi = false,
    small_world = false,
    rmat = false,
    rmat_unique = false,
    csr = false,
    adj_list = false;
  double p = 0.1,
    rmat_a = 0.5,
    rmat_b = 0.25,
    rmat_c = 0.1,
    rmat_d = 0.15;

  // Parse args
  for (int i = 1; i < argc; ++i) {
    std::string arg = argv[i];

    if (arg == "--vertices")
      n = boost::lexical_cast<size_t>( argv[i+1] );

    if (arg == "--seed")
      seed = boost::lexical_cast<uint64_t>( argv[i+1] );

    if (arg == "--edges")
      m = boost::lexical_cast<size_t>( argv[i+1] );

    if (arg == "--max-weight")
      c = boost::lexical_cast<int>( argv[i+1] );

    if (arg == "--batch-buffer-size") {
      batch_buffer_size = boost::lexical_cast<size_t>( argv[i+1] );
      num_headers = batch_buffer_size / 16;
      num_headers = num_headers > 0 ? num_headers : 1;
    }

    if (arg == "--rewire-probability")
      p = boost::lexical_cast<double>( argv[i+1] );

    if (arg == "--num-sources")
      num_sources = boost::lexical_cast<size_t>( argv[i + 1] );

    if (arg == "--erdos-renyi")
      erdos_renyi = true;

    if (arg == "--small-world")
      small_world = true;

    if (arg == "--rmat")
      rmat = true;

    if (arg == "--rmat-unique")
      rmat_unique = true;

    if (arg == "--dot")
      emit_dot_file = true;

    if (arg == "--verify")
      verify = true;

    if (arg == "--degree-dist")
      show_degree_dist = true;

    if (arg == "--sequential-graph")
      sequential_graph = true;

    if (arg == "--csr")
      csr = std::string(argv[i+1]) == "true";

    if (arg == "--adjacency-list")
      adj_list = std::string(argv[i+1]) == "true";
  }

  mpi_process_group pg(num_headers, batch_buffer_size);

  if (argc == 1) {
    if (process_id(pg) == 0)
      usage();
    exit(-1);
  }

  gen.seed(seed);

  parallel::variant_distribution<mpi_process_group> distrib
    = parallel::block(pg, n);

  if (csr) {
    if (process_id(pg) == 0)
      std::cerr << "CSR Graph Tests\n";

    if (erdos_renyi)
      test_csr(pg, gen, distrib, sequential_graph, n, m, c, num_sources);
    if (small_world)
      test_csr(pg, gen, distrib, sequential_graph, n, m, c, p, num_sources);
    if (rmat)
      test_csr(pg, gen, distrib, sequential_graph, n, m, c,
               rmat_a, rmat_b, rmat_c, rmat_d, num_sources, rmat_edge_distribution_tag());
    if (rmat_unique)
      test_csr(pg, gen, distrib, sequential_graph, n, m, c,
               rmat_a, rmat_b, rmat_c, rmat_d, num_sources, rmat_unique_edge_distribution_tag());
  }

  gen.seed(seed); // DEBUG

  if (adj_list) {
    if (process_id(pg) == 0)
      std::cerr << "Adjacency List Graph Tests\n";

    if (erdos_renyi)
      test_adjacency_list(pg, gen, distrib, sequential_graph, n, m, c);
    if (small_world)
      test_adjacency_list(pg, gen, distrib, sequential_graph, n, m, c, p);
    if (rmat)
      test_adjacency_list(pg, gen, distrib, sequential_graph, n, m, c,
                          rmat_a, rmat_b, rmat_c, rmat_d, rmat_edge_distribution_tag());
    if (rmat_unique)
      test_adjacency_list(pg, gen, distrib, sequential_graph, n, m, c,
                          rmat_a, rmat_b, rmat_c, rmat_d, rmat_unique_edge_distribution_tag());
  }

  return 0;
}


//Header for this file
#include "modetrack.h"
//C System-Headers
#include <termios.h>  /* POSIX terminal control definitions */
#include <sys/ioctl.h>
#include <fcntl.h>//fopen(),fclose()
#include <unistd.h>//read(), write()
//C++ System headers
#include <vector>//vector
#include <string>//string
#include <fstream>//iss*
#include <chrono>// timing functions
#include <cmath>//sqrt, abs
#include <iostream>//cout
#include <typeinfo>//typeid
#include <algorithm> // transform, find
#include <functional> // plus/minus
#include <utility>//std::make_pair
#include <map>//std::map
//Boost Headers
#include <boost/algorithm/string.hpp>//split() and is_any_of for parsing .csv files
#include <boost/lexical_cast.hpp>//lexical cast (unsurprisingly)
//Miscellaneous Headers
#include <omp.h>//OpenMP pragmas
//Project specific Headers
//

ModeTrack::ModeTrack() {

    //initialize best fit curves
    PopulateBestFitCurves();
}

ModeTrack::~ModeTrack() {}

inline void SubtractBackground(std::vector<double>& data,std::vector<double>& background) {

    data.resize(background.size());
    std::transform(data.begin(), data.end(), background.begin(), data.begin(),std::minus<double>());
}

std::vector< data_triple<double> > SubtractBackground(const std::vector< data_triple<double> >& data,
        const std::vector<double>& background) {

    std::vector< data_triple<double> > new_signal;
    new_signal.reserve( background.size() );

    for( uint i = 0; i < background.size() ; i++ ) {

        double length = data[i].cavity_length;
        double frequency = data[i].frequency_MHz;
        double power = data[i].power_dBm - background[i];

        new_signal.push_back( data_triple< double >( length, frequency, power ) );
    }

    return new_signal;
}

double ModeTrack::GetPeaks( const std::vector<data_triple<double> >& data_str, int mode_number, Method filter_method ) {

    std::vector< data_triple< double > > clean_signal = SubtractBackground( data_str, background );

    std::vector<double> power_list = data_triples_to_power( clean_signal );

    auto peak_list = FindPeaks( power_list, filter_method) ;
    std::cout << "Number of peaks identified: " << peak_list.size() << std::endl;

    //check to see if any mode were identified
    //if not return the default value of 'zero'

    if( peak_list.size() >= 1 ) {

        auto identified_peaks = CompareAndFill(peak_list, clean_signal);

        //check to see if requested mode number has been identified
        //if it has return the frequency at which it was found
        //if not return zero, which will be interpreted as 'not found'
        if ( identified_peaks.count(mode_number) == 1 ) {
            return identified_peaks[mode_number];
        } else {
            throw mode_track_failure( "Requested mode was not identified." );
        }
    } else {
        throw mode_track_failure( "No modes were identified." );
    }
}

double ModeTrack::GetPeaksGauss( const std::vector<data_triple<double> >& data_str, int mode_number ) {

    try {
        return GetPeaks( data_str, mode_number, Gauss );
    } catch (const mode_track_failure& e) {
        throw mode_track_failure( "Could not identify requested mode in data set" );
    }
//    return GetPeaks( data_str, mode_number, Gauss );
}

double ModeTrack::GetPeaksBiLat( const std::vector<data_triple<double> >& data_str, int mode_number) {

    try {
        return GetPeaks( data_str, mode_number, BiLat );
    } catch (const mode_track_failure& e) {
        throw mode_track_failure( "Could not identify requested mode in data set" );
    }

//    return GetPeaks( data_str, mode_number, BiLat );
}

void ModeTrack::SetLowerBound( double frequency ) {
    upper_bound = frequency;
}

void ModeTrack::SetUpperBound( double frequency ) {
    lower_bound = frequency;
}

double ModeTrack::GetMaxPeak( const std::vector< data_triple<double> >& data_list ) {

    std::vector<double> power_list = data_triples_to_power( data_list );

    uint peak_index = 0;
    FindMaxima( power_list );

    try {
        peak_index = FindMaxima( power_list );
    } catch (const mode_track_failure& e) {
        throw mode_track_failure( "No maxima peak found in data set." );
    }

    data_triple<double> data_entry = data_list.at( peak_index );
    return data_entry.frequency_MHz;
}

void ModeTrack::SetBackground( const std::vector<data_triple< double > >& background_list ) {

    background = data_triples_to_power( background_list );

}

std::vector<double> ModeTrack::Derivative( const std::vector<double>& data_list ) {

    std::vector<double> der_list;

    std::deque<double> data_deque (data_list.begin(),data_list.end());
    //Pad data_list with zeros at front and back
    for ( unsigned int i=0; i < 2; i++ ) {
        data_deque.push_front(0.0);
        data_deque.push_back(0.0);
    }

    #pragma omp parallel for ordered schedule(dynamic)
    for( unsigned int n = 2; n < data_list.size() + 2; n++ ) {

        double der = ( -data_deque.at( n + 2 ) + 8.0*data_deque.at( n + 1 ) - 8*data_deque.at( n - 1 )+data_deque.at( n - 2 ) ) / 12.0;

        #pragma omp ordered
        der_list.push_back(der);
    }
    return(der_list);
}

//sum all enteries in a vector, with optional parameter of raising each entry to a power
//used by Standard Deviation function
inline double sum ( const std::vector< double >& data_list, double exponent ) {

    double tot = 0.0;

    for ( const auto& val : data_list ) {
        tot += std::pow( val, exponent );
    }

    return tot;
}

//define a gaussian function with standard deviation sigma and a mean value of zero
//used in the construction of gaussian kernels
inline double gaussian ( double x, double sigma ) {
    return 1.0 / ( std::sqrt(M_PI_2)*sigma )*std::exp( -0.5 * std::pow( x/sigma, 2.0 ) );
}

//generate a gaussian kernel of radius 'r', suitable for convolutions
//kernel will have a standard deviation of r/2.
std::vector<double> ModeTrack::GaussKernel( int r ) {

    double sigma = static_cast<double>( r )/2.0;

    std::vector<double> vals;

    for( int i = -r; i<= r ; i ++ ) {
        vals.push_back( gaussian( i, sigma ) );
    }

    //normalize kernel before returning
    return Normalize( vals );
}

double ModeTrack::StdDev( const std::vector<double>& data_list ) {

    //compute mean value of data set
    double sum_x = sum( data_list, 1.0 );
    double n = static_cast<double>( data_list.size() );
    double mean = sum_x/n;

    //compute variance taking into account Bessel's correction eg n/(n-1)
    double sum_x2 = sum( data_list, 2.0 );
    double sigma_sqr = sum_x2/( n - 1.0 ) - n/( n - 1.0 )*std::pow( mean, 2.0 );

    //return square root of variance
    return std::sqrt( sigma_sqr );
}

std::vector<double> ModeTrack::Normalize( const std::vector<double>& data_list ) {

    double norm_factor = std::sqrt( sum( data_list, 2.0 ) );
    auto normalized_data = data_list;

    std::transform( normalized_data.begin(), \
                    normalized_data.end(), \
                    normalized_data.begin(), \
                    std::bind1st(std::divides<float>(), norm_factor) );

    return normalized_data;
}

std::vector< double > ModeTrack::Convolve( const std::vector<double>& signal, const std::vector<double>& kernel ) {

    int kernel_size = kernel.size();
    int half_k_size = (kernel_size - 1 )/2;

    int signal_size = signal.size();
    int signal_max_index = signal_size - 1;

    std::vector< double > output( signal.size(), 0.0 );

    #pragma omp parallel for
    for ( int i = 0 ; i < signal_size ; i++ ) {

        double conv_elem = 0.0;

        int k_max = ( ( i + half_k_size ) > signal_max_index )?( signal_max_index + half_k_size - i ):(kernel_size);
        int k_min = ( ( i - half_k_size ) < 0 )?( half_k_size - i ):(0);

        for ( int j = k_min ; j < k_max ; j++ ) {

            conv_elem += signal[ i + j - half_k_size]*kernel[ j ];
        }

        for ( int j = 0 ; j < k_min ; j++ ) {

            conv_elem += signal[ -i - j + half_k_size ]*kernel[ j ];
        }

        for ( int j = k_max ; j < kernel_size ; j++ ) {

            conv_elem += signal[ 2*signal_max_index - i - j + half_k_size ]*kernel[ j ];

        }

        output[i] = conv_elem;

    }

    return output;
}

//Convolve the input list 'data_list' with a gaussian kernel of radius 15
//serves as a low-pass filter that surpressed noise.
std::vector<double> ModeTrack::GaussBlur( const std::vector<double>& data_list ) {

    auto gauss_matrix = GaussKernel( 15 );
    return Convolve( data_list, gauss_matrix );
}

struct RetrieveVal {
    template <typename T>
    typename T::first_type operator()(T keyValuePair) const {
        return keyValuePair.second;
    }
};

//copy values stored in std::map to std::vec
//std::transform(map.begin(), map.end(), std::back_inserter(vec), RetrieveVal());

std::vector<double> ModeTrack::FindPeaks( const std::vector<double>& data_list, const ModeTrack::Method method ) {

    auto filtered_list = ( method == Gauss )? GaussBlur( data_list ):BilateralFilter( data_list, 10, 2 );
    auto f_prime = Derivative( filtered_list );

    int f_prime_size = f_prime.size() - 1;
    int max_size = filtered_list.size();

    double sigma = StdDev( f_prime );

    std::vector< double > peak_list;

    #pragma omp parallel for ordered schedule(dynamic)
    for( int i = 0; i < f_prime_size; i++ ) {

        bool stat_signifigant = ( f_prime.at(i) < 3*sigma && f_prime.at(i+1) > 3*sigma );
        bool away_from_edges = ( i+1 < max_size - 4 && i+1 > 4 );

        if( stat_signifigant && away_from_edges ) {

            #pragma omp ordered
            peak_list.push_back( static_cast<double>( i + 1 ) );

        }
    }

    return peak_list;
}

uint ModeTrack::FindMaxima( const std::vector<double>& data_list ) {

    auto g_blur = GaussBlur( data_list );
    auto f_prime = Derivative( g_blur );

    uint length = data_list.size();

    double sigma = StdDev( f_prime );

    auto result = std::min_element( data_list.begin(), data_list.end() );
    double min_power = static_cast<double>(*result);

    uint peak_index = 0;
    double peak_power = min_power;

    bool peak_identified = false;

    for( unsigned int i = 0; i < f_prime.size() - 1; i++ ) {

        //Establish criteria for a maxima peak;

        bool maxima = ( f_prime.at(i) > 0*sigma && f_prime.at(i+1) < 0*sigma );
        bool away_from_edges = ( i+1 < (length - 5) && i+1 > 5 );

        if( maxima && away_from_edges ) {

            peak_identified |= true;

            std::cout << "Found peak at: " << i <<std::endl;

            uint found_index = i;
            double found_power = data_list.at(i);

            std::cout << "Power was: " << found_power <<std::endl;

            //check to see if peak index is already an element in the map
            //if duplicate is found check which frequency seperation is the smallest
            //between the current and stored value.
            if( peak_index != 0 ) {
                std::cout << "Duplicate Index Detected" << std::endl;

                //if stored value is greater than current value, replaced stored value with current
                if( found_power > peak_power ) {
                    std::cout << "Found Peak was Greater." << std::endl;
//                        std::cout<<"Power was: "<<found_power<<std::endl;
                    peak_index = found_index;
                    peak_power = found_power;
                    //if stored value is less than current leave stored value unchanged
                } else {
                    std::cout << "Found Peak was Less." << std::endl;
//                        std::cout<<"Power was: "<<found_power<<std::endl;
                }
            } else { //if no duplicate is found create a new entry
                peak_index = found_index;
                peak_power = found_power;
            }

        }
    }

    if( !peak_identified ) {
        throw mode_track_failure( "No local maxima found." );
    }

    return peak_index;
}

inline double QuadraticSpline( double a, double b, double c, double x ) {
    return a*std::pow( x, 2.0 ) + b*x + c;
}

double ModeTrack::GenerateSpline( int mode_number, double length ) {

    double a = std::get<0>(estimated_paths.at(mode_number));
    double b = std::get<1>(estimated_paths.at(mode_number));
    double c = std::get<2>(estimated_paths.at(mode_number));

    return QuadraticSpline(a,b,c,length);
}

void ModeTrack::PopulateBestFitCurves() {

    auto mode_one_coeffs = std::make_tuple(47.9998,-1041.54,8950.56);
    auto mode_two_coeffs = std::make_tuple(44.2758,-1055.35,9610.61);
    auto mode_three_coeffs = std::make_tuple(45.8298,-1139.8,10626.7);
    auto mode_four_coeffs = std::make_tuple(37.697,-1038.49,10780.2);

    estimated_paths.push_back(mode_one_coeffs);
    estimated_paths.push_back(mode_two_coeffs);
    estimated_paths.push_back(mode_three_coeffs);
    estimated_paths.push_back(mode_four_coeffs);
}

template<typename T>
inline void removeDuplicates(std::vector<T>& vec) {
    std::sort(vec.begin(), vec.end());
    vec.erase(std::unique(vec.begin(), vec.end()), vec.end());
}

bool ModeTrack::CheckBounds(double frequency) {
    if( upper_bound == 0.0f || lower_bound == 0.0f ) {
        //if upper or lower bound is not set always return true
        return true;
    } else if (frequency >= upper_bound) {
        return false;
    } else if (frequency <= lower_bound) {
        return false;
    } else {
        return true;
    }
}

template<typename T>
inline void c_print(T text,int color) {

    switch( color ) {
    case 0:
        //red, error text
        std::cout<<"\033[1;31m"<< text << "\033[0m\n";
        break;
    case 2:
        //green, positive confirmation
        std::cout<<"\033[32m"<< text << "\033[0m\n";
        break;
    case 4:
        //magenta, header
        std::cout<<"\033[35m"<< text << "\033[0m\n";
        break;
    case 8:
        //yellow, warning text
        std::cout<<"\033[33m"<< text << "\033[0m\n";
    default:
        break;
    }
}

//peak_list: list of indices (in frequency space) where peaks were found for a particular cavity length
//comparison_list: of data triples at the same cavity length
std::map<uint,double> ModeTrack::CompareAndFill( const std::vector<double>& peak_list,\
        const std::vector< data_triple< double > >& comparison_list ) {

    //format is <peak index,<delta_mu,frequency>>
    //where delta_mu is defined below
    std::map< uint,std::pair< uint,double> > found_peaks;

    //since the cavity length is identical for each element in the set of data triples
    //use the length reported by the first element.
    double g_length = comparison_list.at(0).cavity_length;

    for( const auto& peak_index : peak_list ) {

        std::vector<double> results;

        data_triple<double> data_entry = comparison_list.at(peak_index);

        double frequency = data_entry.frequency_MHz;
        double length = data_entry.cavity_length;

        for (int i = 0; i < 4 ; i++) {
            double estimated_frequency = GenerateSpline(i,length);

            std::cout << "Actual frequency "
                      << frequency
                      << " estimated frequency of peak "
                      << i
                      << " "
                      << estimated_frequency
                      << std::endl;

            double delta_mu = std::abs(frequency - estimated_frequency);
            results.push_back(delta_mu);
        }

        auto minima = std::min_element(std::begin(results), std::end(results));
        auto min_val = *minima;
        auto min_position = std::distance(std::begin(results), minima);

        std::cout<<"Smallest frequency seperation: "<<min_val;
        std::cout<<" at Peak: "<<min_position<<std::endl;

        if(min_val >= max_search_radius) {
            c_print("Identified peak was too far from any estimated value.",0);
            continue;
        }

        std::make_pair(min_val,frequency);

        //check to see if peak index is already an element in the map
        //if duplicate is found check which frequency seperation is the smallest
        //between the current and stored value.
        if( found_peaks.count(min_position) != 0 ) {
//            std::cout<<"Duplicate Index Detected"<<std::endl;

            //if stored value is greater than current value, replaced stored value with current
            if( found_peaks[min_position].first > min_val ) {
//                std::cout<<"Found Peak was Greater."<<std::endl;
                found_peaks[min_position] = std::make_pair(min_val,frequency);
                //if stored value is less than current leave stored value unchanged
            } else {
//                std::cout<<"Found Peak was Less."<<std::endl;
            }
        } else { //if no duplicate is found create a new entry
            found_peaks[min_position] = std::make_pair(min_val,frequency);
        }

    }

    //initalize map to default "error" value of all zeros
    //if this initial value is returned it will caused GetPeaks
    //to return a value of 0.0f which is interpreted as the
    //standard error value.
    if(found_peaks.empty()) {
        found_peaks[0]=std::make_pair(0,0.0);
    }

    //create container to hold identified peaks
    //format is [peak index],frequency
    std::map<uint,double> identified_peaks;

    //iterate from the first peak index to the last peak index, and all values in between
    for(uint i = found_peaks.begin()->first ; i <= found_peaks.rbegin()->first ; i ++) {

        //check if the peak index corresponds to a key in the map of found peaks
        //if peak index is present set frequency for peak using actual data
        //if peak index is not present assume that the peak was 'missed' and
        //fill in data using estimated peak position.
        if( found_peaks.count(i) != 0 ) {

            std::cout<<"Match for peak: "<<i<<", ";
            double peak_frequency = found_peaks[i].second;
//            std::cout<<"Using real value of: "<<peak_frequency<<std::endl;
            std::string message_str = "Using real value of: "+boost::lexical_cast<std::string>(peak_frequency)+"MHz";
            c_print(message_str,2);
            identified_peaks[i] = peak_frequency;

        } else {
            std::cout<<"No match for peak: "<<i<<", ";
            double peak_frequency = GenerateSpline(i,g_length);
//            std::cout<<"Filling with estimate of: "<<peak_frequency<<std::endl;
            std::string message_str = "Filling with estimate of: "+boost::lexical_cast<std::string>(peak_frequency)+"MHz";
            c_print(message_str,8);
            identified_peaks[i] = peak_frequency;
        }
    }

    return identified_peaks;

}

std::vector<double> ModeTrack::BilateralFilter( const std::vector<double>& data_list, double sigma_s, double sigma_r ) {

    int list_size = data_list.size();

    int radius = static_cast<int>( ceil( 5.0/2.0*sigma_s ) );

    //Copy data_list into deque to speed up front/back insertions that will be needed
    //for zero padding
    std::deque<double> data_deque ( data_list.begin(),data_list.end() );
    //Pad data_list with zeros at front and back
    for (int i=1; i<=radius; i++) {
        data_deque.push_front(0.0);
        data_deque.push_back(0.0);
    }

    std::vector<double> convolved_list;

    #pragma omp parallel for ordered schedule(dynamic)
    for( int p = radius; p < list_size + radius; p++ ) {

        double conv_element = 0.0;
        double norm_weight = 0.0;

        for(int q = p -radius; q <= p + radius ; q++) {

            double pos_weight = std::abs( p - q );
            double intens_weight = data_deque.at(p) - data_deque.at(q);
            double val_sigma_s = gaussian( pos_weight, sigma_s );
            double val_sigma_r = gaussian( intens_weight, sigma_r );
            norm_weight += val_sigma_s*val_sigma_r;
            conv_element += val_sigma_s*val_sigma_r*data_deque.at(q);

        }

        #pragma omp ordered
        convolved_list.push_back(conv_element/norm_weight);
    }

    return convolved_list;
}

std::vector< double > ModeTrack::data_triples_to_power( const std::vector<data_triple<double> >& data_list ) {

    std::vector< double > power_list;
    power_list.reserve( data_list.size() );

    for( uint i = 0; i < data_list.size() ; i++ ) {
        power_list.push_back( data_list[i].power_dBm );
    }

    return power_list;

}


/*
	This file contains the primitive type definition, macros and includes shared by other project files
*/
#ifndef polyvec_api_h_
#define polyvec_api_h_

// libc
#define _USE_MATH_DEFINES
#include <math.h>

// C++ STL
#include <stdint.h>
#include <string>
#include <memory>
#include <vector>
#include <unordered_map>
#include <unordered_set>
#include <limits>

#ifndef FLT_MAX
#define FLT_MAX std::numeric_limits<float>::max()
#endif

// Eigen
#include <Eigen/Core>

// Platform specific macros
#ifdef _MSC_VER

#define break_here throw std::exception()
#define polyvec_inline __forceinline

#define polyvec_win32 1
#define polyvec_posix 0

#else
#define break_here do{assert(0); throw std::runtime_error("error");}while(0)
#define polyvec_inline __attribute__((always_inline)) inline

#define polyvec_win32 0
#define polyvec_posix 1

#endif

#ifndef break_here
#define throw std::exception()
#endif

// Generic macros
#ifndef param_unused
#define param_unused(x) (void)(x)
#endif

#define result_unused(x) (void)(x)

#ifndef assert_break
#define assert_break(x) { if (!(x)) { fprintf(stderr, "\nFAILED ASSERT " #x  "\n      LOCATION %s:%d", __FILE__, __LINE__); break_here; } }
#define assert_break_msg(x, msg) { if (!(x)) { fprintf(stderr, "\nFAILED ASSERT %s"  "\n      LOCATION %s:%d", msg,__FILE__, __LINE__); break_here; } }
#endif

#define array_len(x) (sizeof(x)/sizeof((x)[0]))
#ifndef logic_xor // https://benpfaff.org/writings/clc/logical-xor.html
#define logic_xor(a, b) ((!(a)) != (!(b)))
#endif

#define polyvec_str(__X)   static_cast<std::ostringstream&>(std::ostringstream().flush() << __X).str()
#define polyvec_c_str(__X) polyvec_str(__X).c_str()

//
// Type aliases
namespace polyvec {

    using index = std::int64_t;
    using indexu = std::size_t;

    using str = std::string;
    using hash64 = uint64_t; // let's not confuse this with the other bunch of integers

    template <typename T>
    using ptr = std::unique_ptr<T>;

    template <typename K, typename V>
    using umap = std::unordered_map<K, V>;

    template <typename V>
    using uset = std::unordered_set<V>;

// Matrix aliases
    using byte4 = Eigen::Matrix<uint8_t, 4, 1>;
    using real = double;
    using int2 = Eigen::Vector2i;
    using int3 = Eigen::Vector3i;
    using int4 = Eigen::Vector4i;
    using real2 = Eigen::Vector2d;
    using real4 = Eigen::Vector4d;
    using mat2 = Eigen::Matrix2d;
    using mat3 = Eigen::Matrix3d;
    using mat4 = Eigen::Matrix4d;
    using real2xN = Eigen::Matrix2Xd;
    using real1xN = Eigen::VectorXd;

// Matrix Constants
    extern const index index_null;
    extern const hash64 hash64_null;
    extern const index index_max;
    extern const int   int_max;
    extern const uint32_t uint32_max;
    extern const int32_t int32_max;
    extern const int64_t int64_max;
    extern const double real_max;
    extern const real2 real2_0;
    extern const real2 real2_1;
    extern const real2 real2_lo; // lowest
    extern const real2 real2_hi; // max
#define            real2_xy(xy) (real2((xy), (xy))) // avoid evaluating twice?

    using real3 = Eigen::Vector3d;
    extern const real3 real3_0;
    extern const real3 real3_1;
    extern const real3 real3_lo; // lowest
    extern const real3 real3_hi; // max

//
// Constants
    namespace constants {
        static constexpr double PI          = M_PI;
        static constexpr double PI_inv      = M_1_PI;
        static constexpr double PI_half     = M_PI_2;
        static constexpr double PI_half_inv = 1. / PI_half;
        static constexpr double PI2         = PI * 2;
        static constexpr double PI3_4       = 3 * M_PI_4;
        static constexpr double PI_4        = M_PI_4;
        static constexpr double PIDiv180    = 0.017453292519943295769236907684886;

        static constexpr double SmallEps = 1e-4;
        static constexpr double BigEps   = 1e-07;
        static constexpr double Eps      = 1e-10;
    }

//
// Indexing
// The code works with stricly typed indices that do not allow implicit conversions to other types.
// Warnings should be set to level4/Wall and stop compilation.
//
// Add any overload when required, *keep* in mind that to mantain
// strict typing:
// 1. IF conversion operator to `index`        NO non-explicit constructor from `index`
// 2. IF non-explicit constructor from `index` NO  conversion operator to `index`
// (1) is better as allows having to overload all integer operations
//
// DO NOT COMPARE VALIDITY with -1 as it's a perfectly valid index
//
#define define_typed_index(t, i)\
    struct t{\
        const static t null;\
        i _v;\
        explicit t(i v = index_null) : _v(v) { }\
        operator std::int64_t()const { return _v; }\
        explicit operator bool()const { return _v != null; }\
        t& operator=(i v) { _v = v; return *this; }\
        t& operator=(int v) { _v = (i)v; return *this; }\
        bool operator <(i rhs) const { return (i)_v < rhs;}\
        t& operator++() { ++_v; return *this; }\
        t& operator--() { --_v; return *this; }\
    };\
    inline t operator-(t lhs, t rhs) { return (t)(lhs._v - rhs._v); }\
    inline t operator+(t lhs, t rhs) { return (t)(lhs._v + rhs._v); }\
    inline t operator-(t lhs, int rhs) { return (t)(lhs._v - rhs); }\
    inline t operator+(t lhs, int rhs) { return (t)(lhs._v + rhs); }\
    inline t operator%(t lhs, t rhs) { return (t)(lhs._v % rhs._v); }\
    static_assert(sizeof(i) == sizeof(t), "Replace compiler with less edgy cousin");

#define define_typed_index_hash(t)\
    struct _std_hash_index_##t {\
        hash64 operator() (t##_idx i) const {\
            return (hash64)i._v;\
        }\
    };\
    struct _std_hash_index_pair_##t {\
        hash64 operator() (const std::pair<t##_idx, t##_idx>& p) const {\
            return hash_index_pair(p.first._v, p.second._v);\
        }\
    };

    hash64 hash_index_pair ( index lhs, index rhs );
    hash64 hash_index_tuple ( index e0, index e1, index e2 );

// Since there are many different buffers around and they also may be circular or not
// each index has different types to avoid mistakes (naming helps too). It can be used
// as a normal index, but won't be implicitly converted to other types
    define_typed_index ( vertex_idx, index );
    define_typed_index ( corner_idx, index );
    define_typed_index ( node_idx, index );
    define_typed_index ( edge_idx, index );

    define_typed_index_hash ( vertex );
    define_typed_index_hash ( corner );
    define_typed_index_hash ( node );
    define_typed_index_hash ( edge );

    struct _std_hash_node {
        hash64 operator() ( const std::tuple<corner_idx, corner_idx, corner_idx>& t ) const {
            return hash_index_tuple ( std::get<0> ( t )._v, std::get<1> ( t )._v, std::get<2> ( t )._v );
        }
    };

}

// Prevent astyle from indenting namespace; while not touching already written files
#define NAMESPACE_BEGIN(NAME__) namespace NAME__ {
#define NAMESPACE_END(NAME__)                    }

#endif // polyvec_api_h_


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

  Copyright 2016 NumScale SAS

  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/simd/pack.hpp>
#include <boost/simd/function/rrol.hpp>
#include <boost/simd/meta/cardinal_of.hpp>
#include <simd_test.hpp>

template <typename T, std::size_t N, typename Env>
void test(Env& runtime)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T, N>;

  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  static const std::size_t N1 = sizeof(T)*8-1;
  T a1[N], b[N], c[N], d[N];
  for(std::size_t i = 0; i < N; ++i)
  {
     a1[i] = (i%2) ? T(i) : T(-i);
     b[i] = bs::rrol(a1[i], 1);
     c[i] = bs::rrol(a1[i], N1/2);
     d[i] = bs::rrol(a1[i], N1-1);
   }
  p_t aa1(&a1[0], &a1[0]+N);
  p_t bb(&b[0], &b[0]+N);
  p_t cc(&c[0], &c[0]+N);
  p_t dd(&d[0], &d[0]+N);
  STF_IEEE_EQUAL(bs::rrol(aa1, 1), bb);
  STF_IEEE_EQUAL(bs::rrol(aa1, N1/2), cc);
  STF_IEEE_EQUAL(bs::rrol(aa1, N1-1), dd);
}

STF_CASE_TPL("Check rrol on pack" , STF_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T>;
  static const std::size_t N = bs::cardinal_of<p_t>::value;
  test<T, N>(runtime);
  test<T, N/2>(runtime);
  test<T, N*2>(runtime);
}