/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU Lesser General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver is distributed in the hope that it will be useful,
 *     but WITHOUT ANY WARRANTY; without even the implied warranty of
 *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *     GNU Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#include "catch.hpp"

#include <iostream>
#include <string>
#include <vector>

#include <Eigen/Core>

#include "interface/Input.hpp"
#include "utils/Sphere.hpp"

/*! \class Input
 *  \test \b InputWaveletTest_Wavelet tests input reading on an input file parsed by pcmsolver.py
 */
TEST_CASE("Input reading using GetKw for an input file for a wavelet cavity", "[input][wavelet]")
{
  std::string filename = "@wavelet.inp";
  Input parsedInput = Input(filename);
  std::string units = "ANGSTROM";
  int CODATAyear = 1998;
  std::string type = "WAVELET";
  int patchLevel = 1;
  double coarsity = 0.3;
  bool scaling = true;
  double diagonalScaling = 1.07;
  std::string radiiSet = "BONDI";
  std::string mode = "EXPLICIT";
  Eigen::Vector3d c1, c2, c3;
  c1 <<  0.00,  0.00, 0.00;
  c2 <<  0.00, -0.96, 0.00;
  c3 << -0.905, 0.32, 0.00;
  c2 *= angstromToBohr();
  c3 *= angstromToBohr();
  Sphere sph1(c1, 1.80 * angstromToBohr());
  Sphere sph2(c2, 1.44 * angstromToBohr());
  Sphere sph3(c3, 1.44 * angstromToBohr());
  std::vector<Sphere> spheres;
  spheres.push_back(sph1);
  spheres.push_back(sph2);
  spheres.push_back(sph3);
  std::string solverType = "WAVELET";
  int equationType = 0;
  double probeRadius = 1.385 * angstromToBohr(); // The value for water
  std::string greenInsideType = "VACUUM";
  std::string greenOutsideType = "UNIFORMDIELECTRIC";
  int derivativeInsideType = 0;
  int derivativeOutsideType = 2;
  double epsilonInside = 1.0;
  double epsilonStaticOutside = 78.39;
  double epsilonDynamicOutside = 10.423;

  REQUIRE(units                 == parsedInput.units());
  REQUIRE(CODATAyear            == parsedInput.CODATAyear());
  REQUIRE(type                  == parsedInput.cavityType());
  REQUIRE(patchLevel            == parsedInput.cavityParams().patchLevel);
  REQUIRE(coarsity              == Approx(parsedInput.cavityParams().coarsity));
  REQUIRE(scaling               == parsedInput.scaling());
  REQUIRE(radiiSet              == parsedInput.radiiSet());
  REQUIRE(mode                  == parsedInput.mode());
  REQUIRE(diagonalScaling == Approx(parsedInput.integratorScaling()));
  for (size_t i = 0; i < spheres.size(); ++i) {
    for (int j = 0; j < 3; ++j) {
      REQUIRE(spheres[i].center(j) == Approx(parsedInput.spheres(i).center(j)));
    }
    REQUIRE(spheres[i].radius == Approx(parsedInput.spheres(i).radius));
  }
  REQUIRE(solverType            == parsedInput.solverType());
  REQUIRE(equationType          == parsedInput.equationType());
  REQUIRE(probeRadius           == Approx(parsedInput.cavityParams().probeRadius));
  REQUIRE(greenInsideType       == parsedInput.greenInsideType());
  REQUIRE(greenOutsideType      == parsedInput.greenOutsideType());
  REQUIRE(derivativeInsideType  == parsedInput.insideGreenParams().howDerivative);
  REQUIRE(derivativeOutsideType == parsedInput.outsideStaticGreenParams().howDerivative);
  REQUIRE(epsilonInside         == Approx(parsedInput.insideGreenParams().epsilon));
  REQUIRE(epsilonStaticOutside  == Approx(parsedInput.outsideStaticGreenParams().epsilon));
  REQUIRE(epsilonDynamicOutside == Approx(parsedInput.outsideDynamicGreenParams().epsilon));
}


// Copyright (c) 2016
// Author: Chrono Law
#include <std.hpp>
using namespace std;

// for c++98
#include <boost/config.hpp>

///////////////////////////////////////

template<int N, int M>
struct meta_func
{
    static const int value = N + M;

    //static constexpr int value = N + M;

    // for c++98
    //BOOST_STATIC_CONSTANT(int, value = N + M);
};

template<typename T1, typename T2>
struct select1st
{
    typedef T1 type;
};

template<typename T1, typename T2>
struct forward : select1st<T2, T1>
{};

///////////////////////////////////////

#include "mp_utils.hpp"

mp_arglist<int L, int R>
mp_function static_min
{
    static const int value = (L < R) ? L : R;
};

///////////////////////////////////////
#include <boost/type_traits.hpp>

mp_arglist<mp_arg T>
mp_function demo_func
{
    mp_return(const T*);
};

mp_arglist<mp_arg T>
mp_function demo_func<T*>
{
    mp_return(const T);
};

int main()
{
    cout << "hello meta-programing" << endl;

    cout << meta_func<10, 10>::value << endl;

    //int i = 10, j = 10;
    //meta_func<i, j>::value;

    assert((static_min<10, 20>::value == 10));

    assert((is_same<mp_exec(demo_func<int>) , const int*>::value));
    assert((is_same<mp_exec(demo_func<int*>), const int >::value));

}


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#include "graph.hh"
#include "graph_filtering.hh"

#include "graph_geometric.hh"

#include <boost/python.hpp>

using namespace std;
using namespace boost;
using namespace graph_tool;

typedef graph_tool::detail::get_all_graph_views
    ::apply<graph_tool::detail::filt_scalar_type, mpl::bool_<false>,
            mpl::bool_<true>,mpl::bool_<false>,
            mpl::bool_<true>,mpl::bool_<true> >::type graph_views;

typedef property_map_types::apply<mpl::vector<vector<double> >,
                                  GraphInterface::vertex_index_map_t,
                                  mpl::bool_<false> >::type prop_types;

void geometric(GraphInterface& gi, python::object opoints, double r,
               python::object orange, bool periodic, boost::any pos)
{
    python::object shape = opoints.attr("shape");
    size_t size = python::extract<size_t>(shape[0]);
    vector<vector<double> > points(size);
    vector<pair<double, double> > range(python::len(orange));

    size = python::extract<size_t>(shape[1]);
    for(size_t i = 0; i < points.size(); ++i)
    {
        points[i].resize(size);
        for (size_t j = 0; j < points[i].size(); ++j)
            points[i][j] = python::extract<double>(opoints[i][j]);
    }

    for(size_t i = 0; i < range.size(); ++i)
    {
        range[i].first = python::extract<double>(orange[i][0]);
        range[i].second = python::extract<double>(orange[i][1]);
    }

    run_action<graph_views>()(gi, std::bind(get_geometric(), std::placeholders::_1,
                                            std::placeholders::_2, std::ref(points),
                                            std::ref(range), r,
                                            periodic),
                              prop_types())(pos);
}


#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <string>
#include <sstream>
#include <iostream>
#include <stdexcept>
#include <Eigen/Geometry>
#include <visualization_msgs/Marker.h>
#include <arc_utilities/eigen_helpers.hpp>
#include <arc_utilities/voxel_grid.hpp>
#include <sdf_tools/SDF.h>

#ifndef SDF_HPP
#define SDF_HPP

inline std::vector<uint8_t> FloatToBinary(float value)
{
    uint32_t binary_value = 0;
    memcpy(&binary_value, &value, sizeof(uint32_t));
    std::vector<uint8_t> binary(4);
    // Copy byte 1, least-significant byte
    binary[3] = binary_value & 0x000000ff;
    // Copy byte 2
    binary_value = binary_value >> 8;
    binary[2] = binary_value & 0x000000ff;
    // Copy byte 3
    binary_value = binary_value >> 8;
    binary[1] = binary_value & 0x000000ff;
    // Copy byte 4, most-significant byte
    binary_value = binary_value >> 8;
    binary[0] = binary_value & 0x000000ff;
    return binary;
}

inline float FloatFromBinary(std::vector<uint8_t>& binary)
{
    if (binary.size() != 4)
    {
        std::cerr << "Binary value is not 4 bytes" << std::endl;
        return NAN;
    }
    else
    {
        uint32_t binary_value = 0;
        // Copy in byte 4, most-significant byte
        binary_value = binary_value | binary[0];
        binary_value = binary_value << 8;
        // Copy in byte 3
        binary_value = binary_value | binary[1];
        binary_value = binary_value << 8;
        // Copy in byte 2
        binary_value = binary_value | binary[2];
        binary_value = binary_value << 8;
        // Copy in byte 1, least-significant byte
        binary_value = binary_value | binary[3];
        // Convert binary to float and store
        float field_value = 0.0;
        memcpy(&field_value, &binary_value, sizeof(float));
        return field_value;
    }
}

namespace sdf_tools
{
    class SignedDistanceField
    {
    protected:

        VoxelGrid::VoxelGrid<float> distance_field_;
        std::string frame_;
        bool initialized_;
        bool locked_;

        std::vector<uint8_t> GetInternalBinaryRepresentation(const std::vector<float> &field_data);

        std::vector<float> UnpackFieldFromBinaryRepresentation(std::vector<uint8_t>& binary);

        /*
         * You *MUST* provide valid indices to this function, hence why it is protected (there are safe wrappers available - use them!)
         */
        void FollowGradientsToLocalMaximaUnsafe(VoxelGrid::VoxelGrid<Eigen::Vector3d>& watershed_map, const int64_t x_index, const int64_t y_index, const int64_t z_index) const;

    public:

        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        inline SignedDistanceField(std::string frame, double resolution, double x_size, double y_size, double z_size, float OOB_value) : initialized_(true), locked_(false)
        {
            frame_ = frame;
            VoxelGrid::VoxelGrid<float> new_field(resolution, x_size, y_size, z_size, OOB_value);
            distance_field_ = new_field;
        }

        inline SignedDistanceField(Eigen::Affine3d origin_transform, std::string frame, double resolution, double x_size, double y_size, double z_size, float OOB_value) : initialized_(true), locked_(false)
        {
            frame_ = frame;
            VoxelGrid::VoxelGrid<float> new_field(origin_transform, resolution, x_size, y_size, z_size, OOB_value);
            distance_field_ = new_field;
        }

        inline SignedDistanceField() : initialized_(false), locked_(false) {}

        inline bool IsInitialized() const
        {
            return initialized_;
        }

        inline bool IsLocked() const
        {
            return locked_;
        }

        inline void Lock()
        {
            locked_ = true;
        }

        inline void Unlock()
        {
            locked_ = false;
        }

        inline float Get(const double x, const double y, const double z) const
        {
            return distance_field_.GetImmutable(x, y, z).first;
        }

        inline float Get3d(const Eigen::Vector3d& location) const
        {
            return distance_field_.GetImmutable3d(location).first;
        }

        inline float Get4d(const Eigen::Vector4d& location) const
        {
            return distance_field_.GetImmutable4d(location).first;
        }

        inline float Get(const int64_t x_index, const int64_t y_index, const int64_t z_index) const
        {
            return distance_field_.GetImmutable(x_index, y_index, z_index).first;
        }

        inline std::pair<float, bool> GetSafe(const double x, const double y, const double z) const
        {
            return distance_field_.GetImmutable(x, y, z);
        }

        inline std::pair<float, bool> GetSafe3d(const Eigen::Vector3d& location) const
        {
            return distance_field_.GetImmutable3d(location);
        }

        inline std::pair<float, bool> GetSafe4d(const Eigen::Vector4d& location) const
        {
            return distance_field_.GetImmutable4d(location);
        }

        inline std::pair<float, bool> GetSafe(const int64_t x_index, const int64_t y_index, const int64_t z_index) const
        {
            return distance_field_.GetImmutable(x_index, y_index, z_index);
        }

        /*
         * Setter functions MUST be used carefully - If you arbitrarily change SDF values, it is not a proper SDF any more!
         *
         * Use of these functions can be prevented by calling SignedDistanceField::Lock() on the SDF, at which point these functions
         * will fail with a warning printed to std_err.
         */
        inline bool Set(const double x, const double y, const double z, float value)
        {
            if (!locked_)
            {
                return distance_field_.SetValue(x, y, z, value);
            }
            else
            {
                std::cerr << "Attempt to set value in locked SDF" << std::endl;
                return false;
            }
        }

        inline bool Set3d(const Eigen::Vector3d& location, float value)
        {
            if (!locked_)
            {
                return distance_field_.SetValue3d(location, value);
            }
            else
            {
                std::cerr << "Attempt to set value in locked SDF" << std::endl;
                return false;
            }
        }

        inline bool Set4d(const Eigen::Vector4d& location, float value)
        {
            if (!locked_)
            {
                return distance_field_.SetValue4d(location, value);
            }
            else
            {
                std::cerr << "Attempt to set value in locked SDF" << std::endl;
                return false;
            }
        }

        inline bool Set(const int64_t x_index, const int64_t y_index, const int64_t z_index, const float value)
        {
            if (!locked_)
            {
                return distance_field_.SetValue(x_index, y_index, z_index, value);
            }
            else
            {
                std::cerr << "Attempt to set value in locked SDF" << std::endl;
                return false;
            }
        }

        inline bool Set(const VoxelGrid::GRID_INDEX& index, const float value)
        {
            if (!locked_)
            {
                return distance_field_.SetValue(index, value);
            }
            else
            {
                std::cerr << "Attempt to set value in locked SDF" << std::endl;
                return false;
            }
        }

        inline bool CheckInBounds3d(const Eigen::Vector3d& location) const
        {
            return distance_field_.GetImmutable3d(location).second;
        }

        inline bool CheckInBounds4d(const Eigen::Vector4d& location) const
        {
            return distance_field_.GetImmutable4d(location).second;
        }

        inline bool CheckInBounds(const double x, const double y, const double z) const
        {
            return distance_field_.GetImmutable(x, y, z).second;
        }

        inline bool CheckInBounds(const VoxelGrid::GRID_INDEX& index) const
        {
            return distance_field_.GetImmutable(index.x, index.y, index.z).second;
        }

        inline bool CheckInBounds(const int64_t x_index, const int64_t y_index, const int64_t z_index) const
        {
            return distance_field_.GetImmutable(x_index, y_index, z_index).second;
        }

        inline double GetXSize() const
        {
            return distance_field_.GetXSize();
        }

        inline double GetYSize() const
        {
            return distance_field_.GetYSize();
        }

        inline double GetZSize() const
        {
            return distance_field_.GetZSize();
        }

        inline double GetResolution() const
        {
            return distance_field_.GetCellSizes()[0];
        }

        inline float GetOOBValue() const
        {
            return distance_field_.GetDefaultValue();
        }

        inline int64_t GetNumXCells() const
        {
            return distance_field_.GetNumXCells();
        }

        inline int64_t GetNumYCells() const
        {
            return distance_field_.GetNumYCells();
        }

        inline int64_t GetNumZCells() const
        {
            return distance_field_.GetNumZCells();
        }

    protected:

        inline std::pair<Eigen::Vector3d, double> GetPrimaryComponentsVector(const Eigen::Vector3d& raw_vector) const
        {
            if (std::abs(raw_vector.x()) > std::abs(raw_vector.y()) && std::abs(raw_vector.x()) > std::abs(raw_vector.z()))
            {
                if (raw_vector.x() >= 0.0)
                {
                    return std::make_pair(Eigen::Vector3d(GetResolution() * 0.5, 0.0, 0.0), GetResolution() * 0.5);
                }
                else
                {
                    return std::make_pair(Eigen::Vector3d(GetResolution() * -0.5, 0.0, 0.0), GetResolution() * 0.5);
                }
            }
            else if (std::abs(raw_vector.y()) > std::abs(raw_vector.x()) && std::abs(raw_vector.y()) > std::abs(raw_vector.z()))
            {
                if (raw_vector.y() >= 0.0)
                {
                    return std::make_pair(Eigen::Vector3d(0.0, GetResolution() * 0.5, 0.0), GetResolution() * 0.5);
                }
                else
                {
                    return std::make_pair(Eigen::Vector3d(0.0, GetResolution() * -0.5, 0.0), GetResolution() * 0.5);
                }
            }
            else if (std::abs(raw_vector.z()) > std::abs(raw_vector.x()) && std::abs(raw_vector.z()) > std::abs(raw_vector.y()))
            {
                if (raw_vector.z() >= 0.0)
                {
                    return std::make_pair(Eigen::Vector3d(0.0, 0.0, GetResolution() * 0.5), GetResolution() * 0.5);
                }
                else
                {
                    return std::make_pair(Eigen::Vector3d(0.0, 0.0, GetResolution() * -0.5), GetResolution() * 0.5);
                }
            }
            else if (std::abs(raw_vector.x()) == std::abs(raw_vector.y()))
            {
                const Eigen::Vector3d temp_vector(raw_vector.x(), raw_vector.y(), 0.0);
                return std::make_pair((temp_vector / (temp_vector.norm())) * std::sqrt((GetResolution() * GetResolution() * 0.25) * 2.0), std::sqrt((GetResolution() * GetResolution() * 0.25) * 2.0));
            }
            else if (std::abs(raw_vector.y()) == std::abs(raw_vector.z()))
            {
                const Eigen::Vector3d temp_vector(0.0, raw_vector.y(), raw_vector.x());
                return std::make_pair((temp_vector / (temp_vector.norm())) * std::sqrt((GetResolution() * GetResolution() * 0.25) * 2.0), std::sqrt((GetResolution() * GetResolution() * 0.25) * 2.0));
            }
            else if (std::abs(raw_vector.x()) == std::abs(raw_vector.z()))
            {
                const Eigen::Vector3d temp_vector(raw_vector.x(), 0.0, raw_vector.z());
                return std::make_pair((temp_vector / (temp_vector.norm())) * std::sqrt((GetResolution() * GetResolution() * 0.25) * 2.0), std::sqrt((GetResolution() * GetResolution() * 0.25) * 2.0));
            }
            else
            {
                return std::make_pair((raw_vector / (raw_vector.norm())) * std::sqrt((GetResolution() * GetResolution() * 0.25) * 3.0), std::sqrt((GetResolution() * GetResolution() * 0.25) * 3.0));
            }
        }

        inline double ComputeAxisMatch(const double axis_value, const double check_value) const
        {
            if ((axis_value >= 0.0) == (check_value >= 0.0))
            {
                return std::abs(check_value - axis_value);
            }
            else
            {
                return -std::abs(check_value - axis_value);
            }
        }

        inline Eigen::Vector3d GetBestMatchSurfaceVector(const Eigen::Vector3d& possible_surfaces_vector, const Eigen::Vector3d& center_to_location_vector) const
        {
            const Eigen::Vector3d location_rejected_on_possible = EigenHelpers::VectorRejection(possible_surfaces_vector, center_to_location_vector);
            // Find the axis with the best-match components
            const double x_axis_match = ComputeAxisMatch(possible_surfaces_vector.x(), location_rejected_on_possible.x());
            const double y_axis_match = ComputeAxisMatch(possible_surfaces_vector.y(), location_rejected_on_possible.y());
            const double z_axis_match = ComputeAxisMatch(possible_surfaces_vector.z(), location_rejected_on_possible.z());
            if ((x_axis_match > y_axis_match) && (x_axis_match > z_axis_match))
            {
                return Eigen::Vector3d(possible_surfaces_vector.x(), 0.0, 0.0);
            }
            else if ((y_axis_match > x_axis_match) && (y_axis_match > z_axis_match))
            {
                return Eigen::Vector3d(0.0, possible_surfaces_vector.y(), 0.0);
            }
            else if ((z_axis_match > x_axis_match) && (z_axis_match > y_axis_match))
            {
                return Eigen::Vector3d(0.0, 0.0, possible_surfaces_vector.z());
            }
            else
            {
                assert(false);
                return possible_surfaces_vector;
            }
        }

        /**
         * @brief GetPrimaryEntrySurfaceVector Estimates the real distance of the provided point, comparing it with the cell center location and gradient vector
         * @param boundary_direction_vector
         * @param center_to_location_vector
         * @return vector from center of voxel to primary entry surface, and magnitude of that vector
         */
        inline std::pair<Eigen::Vector3d, double> GetPrimaryEntrySurfaceVector(const Eigen::Vector3d& boundary_direction_vector, const Eigen::Vector3d& center_to_location_vector) const
        {
            if (boundary_direction_vector.squaredNorm() > std::numeric_limits<double>::epsilon())
            {
                const std::pair<Eigen::Vector3d, double> primary_components_vector_query = GetPrimaryComponentsVector(boundary_direction_vector);
                // If the cell is on a surface
                if (primary_components_vector_query.second == (GetResolution() * 0.5))
                {
                    return primary_components_vector_query;
                }
                // If the cell is on an edge or surface
                else
                {
                    // Pick the best-match of the two/three exposed surfaces
                    return std::make_pair(GetBestMatchSurfaceVector(primary_components_vector_query.first, center_to_location_vector), GetResolution() * 0.5);
                }
            }
            else
            {
                return GetPrimaryComponentsVector(center_to_location_vector);
            }
        }

        inline double EstimateDistanceInternal(const double x, const double y, const double z, const int64_t x_idx, const int64_t y_idx, const int64_t z_idx) const
        {
            const std::vector<double> cell_center = GridIndexToLocation(x_idx, y_idx, z_idx);
            const Eigen::Vector3d cell_center_to_location_vector(x - cell_center[0], y - cell_center[1], z - cell_center[2]);
            const double nominal_sdf_distance = (double)distance_field_.GetImmutable(x_idx, y_idx, z_idx).first;

            // Determine vector from "entry surface" to center of voxel
            // TODO: Needs special handling if there's no gradient to work with
            const std::vector<double> raw_gradient = GetGradient(x_idx, y_idx, z_idx, true);
            const Eigen::Vector3d gradient = EigenHelpers::StdVectorDoubleToEigenVector3d(raw_gradient);
            const Eigen::Vector3d direction_to_boundary = (nominal_sdf_distance >= 0.0) ? -gradient : gradient;
            const std::pair<Eigen::Vector3d, double> entry_surface_information = GetPrimaryEntrySurfaceVector(direction_to_boundary, cell_center_to_location_vector);
            const Eigen::Vector3d& entry_surface_vector = entry_surface_information.first;
            const double minimum_distance_magnitude = entry_surface_information.second;

            // Adjust for calculating distance to boundary of voxels instead of center of voxels
            const double center_adjusted_nominal_distance = (nominal_sdf_distance >= 0.0) ? nominal_sdf_distance - (GetResolution() * 0.5) : nominal_sdf_distance + (GetResolution() * 0.5);
            const double minimum_adjusted_distance = arc_helpers::SpreadValue(center_adjusted_nominal_distance, -minimum_distance_magnitude, 0.0, minimum_distance_magnitude);

            // Account for target location being not at the exact center of the voxel
            const double raw_distance_adjustment = EigenHelpers::VectorProjection(entry_surface_vector, cell_center_to_location_vector).norm();
            const double real_distance_adjustment = (minimum_adjusted_distance >= 0.0) ? -raw_distance_adjustment: raw_distance_adjustment;
            const double final_adjusted_distance = minimum_adjusted_distance + real_distance_adjustment;

            // Perform minimum distance thresholding and error checking
            // TODO: do we need to address this magic number somehow?
            if (std::abs(final_adjusted_distance) < GetResolution() * 0.001)
            {
                return 0.0;
            }
            if ((minimum_adjusted_distance >= 0.0) == (final_adjusted_distance >= 0.0))
            {
                return final_adjusted_distance;
            }
            else
            {
                std::cerr << "Center adjusted nominal distance " << minimum_adjusted_distance << " final adjusted_distance " << final_adjusted_distance << std::endl;
                assert(false && "Mismatched minimum and final adjusted distance signs");
            }
        }

    public:

        inline std::pair<double, bool> EstimateDistance(const double x, const double y, const double z) const
        {
            return EstimateDistance4d(Eigen::Vector4d(x, y, z, 1.0));
        }

        inline std::pair<double, bool> EstimateDistance3d(const Eigen::Vector3d& location) const
        {
            const std::vector<int64_t> indices = LocationToGridIndex3d(location);
            if (indices.size() == 3)
            {
                return std::make_pair(EstimateDistanceInternal(location.x(), location.y(), location.z(), indices[0], indices[1], indices[2]), true);
            }
            else
            {
                return std::make_pair((double)distance_field_.GetOOBValue(), false);
            }
        }

        inline std::pair<double, bool> EstimateDistance4d(const Eigen::Vector4d& location) const
        {
            const std::vector<int64_t> indices = LocationToGridIndex4d(location);
            if (indices.size() == 3)
            {
                return std::make_pair(EstimateDistanceInternal(location(0), location(1), location(2), indices[0], indices[1], indices[2]), true);
            }
            else
            {
                return std::make_pair((double)distance_field_.GetOOBValue(), false);
            }
        }

        inline std::vector<double> GetGradient(const double x, const double y, const double z, const bool enable_edge_gradients=false) const
        {
            return GetGradient4d(Eigen::Vector4d(x, y, z, 1.0), enable_edge_gradients);
        }

        inline std::vector<double> GetGradient3d(const Eigen::Vector3d& location, const bool enable_edge_gradients=false) const
        {
            const std::vector<int64_t> indices = LocationToGridIndex3d(location);
            if (indices.size() == 3)
            {
                return GetGradient(indices[0], indices[1], indices[2], enable_edge_gradients);
            }
            else
            {
                return std::vector<double>();
            }
        }

        inline std::vector<double> GetGradient4d(const Eigen::Vector4d& location, const bool enable_edge_gradients=false) const
        {
            const std::vector<int64_t> indices = LocationToGridIndex4d(location);
            if (indices.size() == 3)
            {
                return GetGradient(indices[0], indices[1], indices[2], enable_edge_gradients);
            }
            else
            {
                return std::vector<double>();
            }
        }

        inline std::vector<double> GetGradient(const VoxelGrid::GRID_INDEX& index, const bool enable_edge_gradients=false) const
        {
            return GetGradient(index.x, index.y, index.z, enable_edge_gradients);
        }

        inline std::vector<double> GetGradient(const int64_t x_index, const int64_t y_index, const int64_t z_index, const bool enable_edge_gradients=false) const
        {
            // Make sure the index is inside bounds
            if ((x_index >= 0) && (y_index >= 0) && (z_index >= 0) && (x_index < GetNumXCells()) && (y_index < GetNumYCells()) && (z_index < GetNumZCells()))
            {
                // Make sure the index we're trying to query is one cell in from the edge
                if ((x_index > 0) && (y_index > 0) && (z_index > 0) && (x_index < (GetNumXCells() - 1)) && (y_index < (GetNumYCells() - 1)) && (z_index < (GetNumZCells() - 1)))
                {
                    double inv_twice_resolution = 1.0 / (2.0 * GetResolution());
                    double gx = (Get(x_index + 1, y_index, z_index) - Get(x_index - 1, y_index, z_index)) * inv_twice_resolution;
                    double gy = (Get(x_index, y_index + 1, z_index) - Get(x_index, y_index - 1, z_index)) * inv_twice_resolution;
                    double gz = (Get(x_index, y_index, z_index + 1) - Get(x_index, y_index, z_index - 1)) * inv_twice_resolution;
                    return std::vector<double>{gx, gy, gz};
                }
                // If we're on the edge, handle it specially
                else if (enable_edge_gradients)
                {
                    // Get the "best" indices we can use
                    int64_t low_x_index = std::max((int64_t)0, x_index - 1);
                    int64_t high_x_index = std::min(GetNumXCells() - 1, x_index + 1);
                    int64_t low_y_index = std::max((int64_t)0, y_index - 1);
                    int64_t high_y_index = std::min(GetNumYCells() - 1, y_index + 1);
                    int64_t low_z_index = std::max((int64_t)0, z_index - 1);
                    int64_t high_z_index = std::min(GetNumZCells() - 1, z_index + 1);
                    // Compute the axis increments
                    double x_increment = (high_x_index - low_x_index) * GetResolution();
                    double y_increment = (high_y_index - low_y_index) * GetResolution();
                    double z_increment = (high_z_index - low_z_index) * GetResolution();
                    // Compute the gradients for each axis - by default these are zero
                    double gx = 0.0;
                    double gy = 0.0;
                    double gz = 0.0;
                    // Only if the increments are non-zero do we compute the gradient of an axis
                    if (x_increment > 0.0)
                    {
                        double inv_x_increment = 1.0 / x_increment;
                        double high_x_value = Get(high_x_index, y_index, z_index);
                        double low_x_value = Get(low_x_index, y_index, z_index);
                        // Compute the gradient
                        gx = (high_x_value - low_x_value) * inv_x_increment;
                    }
                    if (y_increment > 0.0)
                    {
                        double inv_y_increment = 1.0 / y_increment;
                        double high_y_value = Get(x_index, high_y_index, z_index);
                        double low_y_value = Get(x_index, low_y_index, z_index);
                        // Compute the gradient
                        gy = (high_y_value - low_y_value) * inv_y_increment;
                    }
                    if (z_increment > 0.0)
                    {
                        double inv_z_increment = 1.0 / z_increment;
                        double high_z_value = Get(x_index, y_index, high_z_index);
                        double low_z_value = Get(x_index, y_index, low_z_index);
                        // Compute the gradient
                        gz = (high_z_value - low_z_value) * inv_z_increment;
                    }
                    // Assemble and return the computed gradient
                    return std::vector<double>{gx, gy, gz};
                }
                // Edge gradients disabled, return no gradient
                else
                {
                    return std::vector<double>();
                }
            }
            // If we're out of bounds, return no gradient
            else
            {
                return std::vector<double>();
            }
        }

        inline Eigen::Vector3d ProjectOutOfCollision(const double x, const double y, const double z, const double stepsize_multiplier = 1.0 / 10.0) const
        {
            const Eigen::Vector4d result = ProjectOutOfCollision4d(Eigen::Vector4d(x, y, z, 1.0), stepsize_multiplier);
            return result.head<3>();
        }

        inline Eigen::Vector3d ProjectOutOfCollisionToMinimumDistance(const double x, const double y, const double z, const double minimum_distance, const double stepsize_multiplier = 1.0 / 10.0) const
        {
            const Eigen::Vector4d result = ProjectOutOfCollisionToMinimumDistance4d(Eigen::Vector4d(x, y, z, 1.0), minimum_distance, stepsize_multiplier);
            return result.head<3>();
        }

        inline Eigen::Vector3d ProjectOutOfCollision3d(const Eigen::Vector3d& location, const double stepsize_multiplier = 1.0 / 10.0) const
        {
            return ProjectOutOfCollision(location.x(), location.y(), location.z(), stepsize_multiplier);
        }

        inline Eigen::Vector3d ProjectOutOfCollisionToMinimumDistance3d(const Eigen::Vector3d& location, const double minimum_distance, const double stepsize_multiplier = 1.0 / 10.0) const
        {
            return ProjectOutOfCollisionToMinimumDistance(location.x(), location.y(), location.z(), minimum_distance, stepsize_multiplier);
        }

        inline Eigen::Vector4d ProjectOutOfCollision4d(const Eigen::Vector4d& location, const double stepsize_multiplier = 1.0 / 10.0) const
        {
            return ProjectOutOfCollisionToMinimumDistance4d(location, 0.0, stepsize_multiplier);
        }

        inline Eigen::Vector4d ProjectOutOfCollisionToMinimumDistance4d(const Eigen::Vector4d& location, const double minimum_distance, const double stepsize_multiplier = 1.0 / 10.0) const
        {
            Eigen::Vector4d mutable_location = location;
            const bool enable_edge_gradients = true;

            double sdf_dist = EstimateDistance4d(mutable_location).first;
            if (sdf_dist < minimum_distance && CheckInBounds4d(location))
            {
                while (sdf_dist < minimum_distance)
                {
                    const std::vector<double> gradient = GetGradient4d(mutable_location, enable_edge_gradients);
                    const Eigen::Vector3d grad_eigen = EigenHelpers::StdVectorDoubleToEigenVector3d(gradient);

                    assert(grad_eigen.norm() > GetResolution() / 4.0); // Sanity check
                    mutable_location.head<3>() += grad_eigen.normalized() * GetResolution() * stepsize_multiplier;

                    sdf_dist = EstimateDistance4d(mutable_location).first;
                }
            }

            return mutable_location;
        }

        inline const Eigen::Affine3d& GetOriginTransform() const
        {
            return distance_field_.GetOriginTransform();
        }

        inline const Eigen::Affine3d& GetInverseOriginTransform() const
        {
            return distance_field_.GetInverseOriginTransform();
        }

        inline std::string GetFrame() const
        {
            return frame_;
        }

        inline std::vector<int64_t> LocationToGridIndex3d(const Eigen::Vector3d& location) const
        {
            return distance_field_.LocationToGridIndex3d(location);
        }

        inline std::vector<int64_t> LocationToGridIndex4d(const Eigen::Vector4d& location) const
        {
            return distance_field_.LocationToGridIndex4d(location);
        }

        inline std::vector<int64_t> LocationToGridIndex(const double x, const double y, const double z) const
        {
            return distance_field_.LocationToGridIndex(x, y, z);
        }

        inline std::vector<double> GridIndexToLocation(const VoxelGrid::GRID_INDEX& index) const
        {
            return distance_field_.GridIndexToLocation(index);
        }

        inline std::vector<double> GridIndexToLocation(const int64_t x_index, const int64_t y_index, const int64_t z_index) const
        {
            return distance_field_.GridIndexToLocation(x_index, y_index, z_index);
        }

        inline std::vector<double> GridIndexToLocation(std::vector<int64_t> index) const
        {
            return distance_field_.GridIndexToLocation(index[0], index[1], index[2]);
        }

        bool SaveToFile(const std::string& filepath);

        bool LoadFromFile(const std::string& filepath);

        sdf_tools::SDF GetMessageRepresentation();

        bool LoadFromMessageRepresentation(sdf_tools::SDF& message);

        visualization_msgs::Marker ExportForDisplay(float alpha = 0.01f) const;

        visualization_msgs::Marker ExportForDisplayCollisionOnly(float alpha = 0.01f) const;

        visualization_msgs::Marker ExportForDebug(float alpha = 0.5f) const;

        /*
         * The following function can be *VERY EXPENSIVE* to compute, since it performs gradient ascent across the SDF
         */
        VoxelGrid::VoxelGrid<Eigen::Vector3d> ComputeLocalMaximaMap() const;

        inline bool GradientIsEffectiveFlat(const Eigen::Vector3d& gradient) const
        {
            // A gradient is at a local maxima if the absolute value of all components (x,y,z) are less than 1/2 SDF resolution
            double half_resolution = GetResolution() * 0.5;
            if (fabs(gradient.x()) <= half_resolution && fabs(gradient.y()) <= half_resolution && fabs(gradient.z()) <= half_resolution)
            {
                return true;
            }
            else
            {
                return false;
            }
        }

        inline VoxelGrid::GRID_INDEX GetNextFromGradient(const VoxelGrid::GRID_INDEX& index, const Eigen::Vector3d& gradient) const
        {
            // Given the gradient, pick the "best fit" of the 26 neighboring points
            VoxelGrid::GRID_INDEX next_index = index;
            double half_resolution = GetResolution() * 0.5;
            if (gradient.x() > half_resolution)
            {
                next_index.x++;
            }
            else if (gradient.x() < -half_resolution)
            {
                next_index.x--;
            }
            if (gradient.y() > half_resolution)
            {
                next_index.y++;
            }
            else if (gradient.y() < -half_resolution)
            {
                next_index.y--;
            }
            if (gradient.z() > half_resolution)
            {
                next_index.z++;
            }
            else if (gradient.z() < -half_resolution)
            {
                next_index.z--;
            }
            return next_index;
        }
    };
}

#endif // SDF_HPP


// Andrew Naplavkov

// http://stackoverflow.com/questions/6713420/c-convert-integer-to-string-at-compile-time

#ifndef BRIG_DETAIL_ITOA_HPP
#define BRIG_DETAIL_ITOA_HPP

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

namespace brig { namespace detail {

template <bool Top, size_t N>
struct itoa_impl
{
  typedef typename ::boost::mpl::push_back<typename itoa_impl<false, N / 10>::type, ::boost::mpl::char_<'0' + N % 10>>::type type;
};

template <>
struct itoa_impl<false, 0>
{
  typedef ::boost::mpl::string<> type;
};

template <>
struct itoa_impl<true, 0>
{
  typedef ::boost::mpl::string<'0'> type;
};

template <size_t N>
struct itoa
{
  typedef typename ::boost::mpl::c_str<typename itoa_impl<true, N>::type>::type type;
};

} } // brig::detail

#endif // BRIG_DETAIL_ITOA_HPP


//==================================================================================================
/*!

  Copyright 2015 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/constant/maxexponentm1.hpp>
#include <boost/simd/as.hpp>
#include <simd_test.hpp>

STF_CASE_TPL( "Check maxexponentm1 behavior for integral types"
            , (std::uint8_t)(std::uint16_t)(std::uint32_t)(std::uint64_t)
              (std::int8_t)(std::int16_t)(std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::maxexponentm1;
  using boost::simd::Maxexponentm1;

  STF_TYPE_IS(decltype(Maxexponentm1<T>()), T);
  STF_EQUAL(Maxexponentm1<T>(), T(0));
  STF_EQUAL(maxexponentm1( as(T{}) ),T(0));
}

STF_CASE_TPL( "Check maxexponentm1 behavior for double"
            , (double)
            )
{
  using boost::simd::as;
  using boost::simd::detail::maxexponentm1;
  using boost::simd::Maxexponentm1;

  STF_TYPE_IS(decltype(Maxexponentm1<T>()), int64_t);
  STF_IEEE_EQUAL(Maxexponentm1<T>(), 1022);
  STF_IEEE_EQUAL(maxexponentm1( as(T{}) ), 1022);
}
STF_CASE_TPL( "Check maxexponentm1 behavior for float"
            , (float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::maxexponentm1;
  using boost::simd::Maxexponentm1;

  STF_TYPE_IS(decltype(Maxexponentm1<T>()), int32_t);
  STF_IEEE_EQUAL(Maxexponentm1<T>(), 126);
  STF_IEEE_EQUAL(maxexponentm1( as(T{}) ), 126);
}


//  Copyright John Maddock 2006-15.
//  Copyright Paul A. Bristow 2007
//  Use, modification and distribution are 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)

#define BOOST_TEST_MAIN
#define BOOST_MATH_OVERFLOW_ERROR_POLICY ignore_error
#include "bindings.hpp"
#include "../../test/test_nc_chi_squared.hpp"
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE_EXPECTED_FAILURES(test_main, 10000);

BOOST_AUTO_TEST_CASE(test_main)
{
   BOOST_MATH_CONTROL_FP;

   error_stream_replacer rep;

#ifdef TYPE_TO_TEST

   test_accuracy(static_cast<TYPE_TO_TEST>(0), NAME_OF_TYPE_TO_TEST);

#else
   bool test_float = false;
   bool test_double = false;
   bool test_long_double = false;

   if(std::numeric_limits<long double>::digits == std::numeric_limits<double>::digits)
   {
      //
      // Don't bother with long double, it's the same as double:
      //
      if(BOOST_MATH_PROMOTE_FLOAT_POLICY == false)
         test_float = true;
      test_double = true;
   }
   else
   {
      if(BOOST_MATH_PROMOTE_FLOAT_POLICY == false)
         test_float = true;
      if(BOOST_MATH_PROMOTE_DOUBLE_POLICY == false)
         test_double = true;
      test_long_double = true;
   }

#ifdef ALWAYS_TEST_DOUBLE
   test_double = true;
#endif

   if(test_float)
      test_accuracy(0.0f, "float");
   if(test_double)
      test_accuracy(0.0, "double");
   if(test_long_double)
      test_accuracy(0.0L, "long double");
#ifdef BOOST_MATH_USE_FLOAT128
   //test_accuracy(0.0Q, "__float128");
#endif

#endif
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2017 Gagan Goel <gagan.nith@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"

#include <Eigen/CXX11/Tensor>

using Eigen::Tensor;
using Eigen::array;

template <int DataLayout>
static void test_0D_trace() {
  Tensor<float, 0, DataLayout> tensor;
  tensor.setRandom();
  array<ptrdiff_t, 0> dims;
  Tensor<float, 0, DataLayout> result = tensor.trace(dims);
  VERIFY_IS_EQUAL(result(), tensor());
}

template <int DataLayout>
static void test_all_dimensions_trace() {
  Tensor<float, 3, DataLayout> tensor1(5, 5, 5);
  tensor1.setRandom();
  Tensor<float, 0, DataLayout> result1 = tensor1.trace();
  VERIFY_IS_EQUAL(result1.rank(), 0);
  float sum = 0.0f;
  for (int i = 0; i < 5; ++i) {
    sum += tensor1(i, i, i);
  }
  VERIFY_IS_EQUAL(result1(), sum);

  Tensor<float, 5, DataLayout> tensor2(7, 7, 7, 7, 7);
  tensor2.setRandom();
  array<ptrdiff_t, 5> dims = { { 2, 1, 0, 3, 4 } };
  Tensor<float, 0, DataLayout> result2 = tensor2.trace(dims);
  VERIFY_IS_EQUAL(result2.rank(), 0);
  sum = 0.0f;
  for (int i = 0; i < 7; ++i) {
    sum += tensor2(i, i, i, i, i);
  }
  VERIFY_IS_EQUAL(result2(), sum);
}

template <int DataLayout>
static void test_simple_trace() {
  Tensor<float, 3, DataLayout> tensor1(3, 5, 3);
  tensor1.setRandom();
  array<ptrdiff_t, 2> dims1 = { { 0, 2 } };
  Tensor<float, 1, DataLayout> result1 = tensor1.trace(dims1);
  VERIFY_IS_EQUAL(result1.rank(), 1);
  VERIFY_IS_EQUAL(result1.dimension(0), 5);
  float sum = 0.0f;
  for (int i = 0; i < 5; ++i) {
    sum = 0.0f;
    for (int j = 0; j < 3; ++j) {
      sum += tensor1(j, i, j);
    }
    VERIFY_IS_EQUAL(result1(i), sum);
  }

  Tensor<float, 4, DataLayout> tensor2(5, 5, 7, 7);
  tensor2.setRandom();
  array<ptrdiff_t, 2> dims2 = { { 2, 3 } };
  Tensor<float, 2, DataLayout> result2 = tensor2.trace(dims2);
  VERIFY_IS_EQUAL(result2.rank(), 2);
  VERIFY_IS_EQUAL(result2.dimension(0), 5);
  VERIFY_IS_EQUAL(result2.dimension(1), 5);
  for (int i = 0; i < 5; ++i) {
    for (int j = 0; j < 5; ++j) {
      sum = 0.0f;
      for (int k = 0; k < 7; ++k) {
        sum += tensor2(i, j, k, k);
      }
      VERIFY_IS_EQUAL(result2(i, j), sum);
    }
  }

  array<ptrdiff_t, 2> dims3 = { { 1, 0 } };
  Tensor<float, 2, DataLayout> result3 = tensor2.trace(dims3);
  VERIFY_IS_EQUAL(result3.rank(), 2);
  VERIFY_IS_EQUAL(result3.dimension(0), 7);
  VERIFY_IS_EQUAL(result3.dimension(1), 7);
  for (int i = 0; i < 7; ++i) {
    for (int j = 0; j < 7; ++j) {
      sum = 0.0f;
      for (int k = 0; k < 5; ++k) {
        sum += tensor2(k, k, i, j);
      }
      VERIFY_IS_EQUAL(result3(i, j), sum);
    }
  }

  Tensor<float, 5, DataLayout> tensor3(3, 7, 3, 7, 3);
  tensor3.setRandom();
  array<ptrdiff_t, 3> dims4 = { { 0, 2, 4 } };
  Tensor<float, 2, DataLayout> result4 = tensor3.trace(dims4);
  VERIFY_IS_EQUAL(result4.rank(), 2);
  VERIFY_IS_EQUAL(result4.dimension(0), 7);
  VERIFY_IS_EQUAL(result4.dimension(1), 7);
  for (int i = 0; i < 7; ++i) {
    for (int j = 0; j < 7; ++j) {
      sum = 0.0f;
      for (int k = 0; k < 3; ++k) {
        sum += tensor3(k, i, k, j, k);
      }
      VERIFY_IS_EQUAL(result4(i, j), sum);
    }
  }

  Tensor<float, 5, DataLayout> tensor4(3, 7, 4, 7, 5);
  tensor4.setRandom();
  array<ptrdiff_t, 2> dims5 = { { 1, 3 } };
  Tensor<float, 3, DataLayout> result5 = tensor4.trace(dims5);
  VERIFY_IS_EQUAL(result5.rank(), 3);
  VERIFY_IS_EQUAL(result5.dimension(0), 3);
  VERIFY_IS_EQUAL(result5.dimension(1), 4);
  VERIFY_IS_EQUAL(result5.dimension(2), 5);
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 4; ++j) {
      for (int k = 0; k < 5; ++k) {
        sum = 0.0f;
        for (int l = 0; l < 7; ++l) {
          sum += tensor4(i, l, j, l, k);
        }
        VERIFY_IS_EQUAL(result5(i, j, k), sum);
      }
    }
  }
}

template<int DataLayout>
static void test_trace_in_expr() {
  Tensor<float, 4, DataLayout> tensor(2, 3, 5, 3);
  tensor.setRandom();
  array<ptrdiff_t, 2> dims = { { 1, 3 } };
  Tensor<float, 2, DataLayout> result(2, 5);
  result = result.constant(1.0f) - tensor.trace(dims);
  VERIFY_IS_EQUAL(result.rank(), 2);
  VERIFY_IS_EQUAL(result.dimension(0), 2);
  VERIFY_IS_EQUAL(result.dimension(1), 5);
  float sum = 0.0f;
  for (int i = 0; i < 2; ++i) {
    for (int j = 0; j < 5; ++j) {
      sum = 0.0f;
      for (int k = 0; k < 3; ++k) {
        sum += tensor(i, k, j, k);
      }
      VERIFY_IS_EQUAL(result(i, j), 1.0f - sum);
    }
  }
}

EIGEN_DECLARE_TEST(cxx11_tensor_trace) {
  CALL_SUBTEST(test_0D_trace<ColMajor>());
  CALL_SUBTEST(test_0D_trace<RowMajor>());
  CALL_SUBTEST(test_all_dimensions_trace<ColMajor>());
  CALL_SUBTEST(test_all_dimensions_trace<RowMajor>());
  CALL_SUBTEST(test_simple_trace<ColMajor>());
  CALL_SUBTEST(test_simple_trace<RowMajor>());
  CALL_SUBTEST(test_trace_in_expr<ColMajor>());
  CALL_SUBTEST(test_trace_in_expr<RowMajor>());
}


#ifndef F_M_HPP
#define F_M_HPP

#include <armadillo>
using namespace arma ;

void f(irowvec a, ivec b, irowvec& y, ivec& z) ;
void g() ;

void f(irowvec a, ivec b, irowvec& y, ivec& z)
{
  y = a+2 ;
  z = b-3 ;
}

void g()
{
  irowvec a, y ;
  ivec b, z ;
  sword _a [] = {1, 2, 3} ;
  a = irowvec(_a, 3, false) ;
  sword _b [] = {4, 5, 6} ;
  b = ivec(_b, 3, false) ;
  f(a, b, y, z) ;
}
#endif


#define BOOST_USE_ASAN

#include "memorysizeliterals.h"
#include "programmoptions.h"

#include <boost/program_options.hpp>

namespace po = boost::program_options;

namespace
{
const std::unordered_map<std::string, size_t> FromLiteralsToNumber{
    {"KB", KB}, {"MB", MB}, {"GB", GB}};
}

size_t parseMemorySize(std::string_view str)
{
    if (str.empty())
        throw po::error("can't parse empty string");

    auto literalsBeginIt =
        std::find_if_not(str.begin(), str.end(), [](const char ch) { return isdigit(ch); });

    const std::string intPart{str.begin(), literalsBeginIt};

    if (literalsBeginIt == str.end())
        return atoi(intPart.c_str());

    if (literalsBeginIt != std::prev(std::prev(str.end())))
        throw po::error("literals allowed only at the end of memory size string");

    if (literalsBeginIt == str.begin())
        throw po::error("can't parse string which contains only literals");

    const std::string literalPart{literalsBeginIt, str.end()};

    if (FromLiteralsToNumber.count(literalPart) == 0)
        throw po::error("unknown literals: " + literalPart);

    return atoi(intPart.c_str()) * FromLiteralsToNumber.at(literalPart);
}

std::variant<Options, std::string> getOptionsOrHelpStr(int argc, char const* argv[])
{
    po::options_description desc("Allowed options");
    desc.add_options()
        ("help,h", "show help")
        ("input-file,i", po::value<std::string>()->required(), "input file path")
        ("output-file,o", po::value<std::string>()->required(), "output file path")
        ("size-of-block,s",po::value<std::string>()->default_value("1MB"),
         "size of hash calculating block in bytes. "
         "Supports KB, MB, GB literals.")
        ("type-of-disk,t",
         po::value<std::string>()->default_value("HDD"),
         "type of hard disk, needed in order to optimise perfomance. "
         "Possible values: HDD, SSD")
        ("max-ram-size,m",
         po::value<std::string>()->default_value("3GB"),
         "maximum size of RAM that will be used by the programm."
         "Supports KB, MB, GB literals.");

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help"))
    {
        std::ostringstream helpMessage;
        helpMessage << desc;
        return helpMessage.str();
    }

    const auto hardDiskType = vm.at("type-of-disk").as<std::string>();
    if (hardDiskType != "SSD" && hardDiskType != "HDD")
        throw po::error("wrong hard disk type: " + hardDiskType + ". Correct values: HDD, SSD");

    return Options{.inputFile = vm.at("input-file").as<std::string>(),
                   .outputFile = vm.at("output-file").as<std::string>(),
                   .blockSize = parseMemorySize(vm.at("size-of-block").as<std::string>()),
                   .isSSD = hardDiskType == "SSD",
                   .maxRamSize = parseMemorySize(vm.at("max-ram-size").as<std::string>())};
}


#include <limits>
#include <boost/test/unit_test.hpp>
#include "algorithms/data_structures/array/find_max_product_of_two_elements.hpp"

BOOST_AUTO_TEST_SUITE(TestFindMaxProductOfTwoElements)

BOOST_AUTO_TEST_CASE(simple_test) {
    {
        std::vector<int> input { 9, 9 };
        long long expected = 81;
        BOOST_CHECK(expected == Algo::DS::Array::FindMaxProduct::Find(input));
    }

    {
        std::vector<int> input { 9, 8, 1 };
        long long expected = 72;
        BOOST_CHECK(expected == Algo::DS::Array::FindMaxProduct::Find(input));
    }

    {
        std::vector<int> input { 1, 2, 3 };
        long long expected = 6;
        BOOST_CHECK(expected == Algo::DS::Array::FindMaxProduct::Find(input));
    }

    {
        auto max = std::numeric_limits<int>::max();
        std::vector<int> input { 1, 2, 3, max, 5, max };
        long long expected =
                static_cast<long long>(max) * static_cast<long long>(max);
        BOOST_CHECK(expected == Algo::DS::Array::FindMaxProduct::Find(input));
    }

    {
        std::vector<int> input { 1, 2, 10, 7, 8, 7 };
        long long expected = 80;
        BOOST_CHECK(expected == Algo::DS::Array::FindMaxProduct::Find(input));
    }
}

BOOST_AUTO_TEST_SUITE_END()


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Multipliesributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#define NT2_UNIT_MODULE "nt2 complex.operator toolbox - multiplies/simd Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of boost.simd.operator components in simd mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 18/02/2011
///
#include <nt2/include/functions/multiplies.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/include/functions/extract.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/complex/dry.hpp>
#include <nt2/sdk/complex/imaginary.hpp>
#include <nt2/sdk/complex/meta/as_imaginary.hpp>
#include <nt2/sdk/complex/meta/as_dry.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>

NT2_TEST_CASE_TPL ( multiplies_real__2_0,  BOOST_SIMD_SIMD_REAL_TYPES)
{
  using boost::simd::native;
  typedef NT2_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef std::complex<T>                              cT;
  typedef native<T ,ext_t>                             vT;
  typedef native<cT ,ext_t>                           vcT;
  typedef typename nt2::meta::as_imaginary<T>::type   ciT;
  typedef native<ciT ,ext_t>                         vciT;
  typedef typename nt2::meta::as_dry<T>::type          dT;
  typedef native<dT ,ext_t>                           vdT;

  // specific values tests
  {
    typedef vcT r_t;
    NT2_TEST_EQUAL(nt2::multiplies(vcT(nt2::Inf<vT>(), nt2::Zero<vT>()), vcT(nt2::Inf<vT>(), nt2::Zero<vT>()))[0], vcT(nt2::Inf<vT>(), nt2::Zero<vT>())[0]);
    NT2_TEST_EQUAL(nt2::multiplies(vcT(nt2::One<vT>(), nt2::Zero<vT>()), vcT(nt2::Zero<vT>(),nt2::Zero<vT>()))[0], vcT(nt2::Zero<vT>(),nt2::Zero<vT>())[0]);
    NT2_TEST_EQUAL(nt2::multiplies(vcT(nt2::Zero<vT>(),nt2::Zero<vT>()), vcT(nt2::Zero<vT>(),nt2::Zero<vT>()))[0], vcT(nt2::Zero<vT>(),nt2::Zero<vT>())[0]);
    NT2_TEST_EQUAL(nt2::multiplies(vcT(nt2::Zero<vT>(),nt2::One<vT>()),  vcT(nt2::One<vT>(), nt2::Zero<vT>()))[0], vcT(nt2::Zero<vT>(),nt2::One<vT>())[0]);
    NT2_TEST_EQUAL(nt2::multiplies(vcT(nt2::One<vT>(), nt2::Zero<vT>()), vcT(nt2::One<vT>(), nt2::Zero<vT>()))[0], vcT(nt2::One<vT>(), nt2::Zero<vT>())[0]);

    NT2_TEST_EQUAL(nt2::multiplies(vciT(nt2::Inf<vciT>()), vcT(nt2::Inf<vT>(), nt2::Zero<vT>()))[0], vcT(nt2::Zero<vT>(), nt2::Inf<vT>())[0]);
    NT2_TEST_EQUAL(nt2::multiplies(vciT(nt2::One<vciT>()), vcT(nt2::Zero<vT>(),nt2::Zero<vT>()))[0], vcT(nt2::Zero<vT>(),nt2::Zero<vT>())[0]);
    NT2_TEST_EQUAL(nt2::multiplies(vciT(nt2::Zero<vciT>()), vcT(nt2::Zero<vT>(),nt2::Zero<vT>()))[0], vcT(nt2::Zero<vT>(),nt2::Zero<vT>())[0]);
    NT2_TEST_EQUAL(nt2::multiplies(vciT(nt2::Zero<vciT>()),  vcT(nt2::One<vT>(), nt2::Zero<vT>()))[0], vcT(nt2::Zero<vT>(),nt2::Zero<vT>())[0]);
    NT2_TEST_EQUAL(nt2::multiplies(vciT(nt2::One<vciT>()), vcT(nt2::One<vT>(), nt2::Zero<vT>()))[0], vcT(nt2::Zero<vT>(), nt2::One<vT>())[0]);

    NT2_TEST_EQUAL(nt2::multiplies(nt2::Inf<vdT>(),  vcT(nt2::Inf<vT>(), nt2::Zero<vT>()))[0], cT(nt2::Inf<T>(), nt2::Zero<T>()));
    NT2_TEST_EQUAL(nt2::multiplies(nt2::One<vdT>(),  vcT(nt2::Zero<vT>(),nt2::Zero<vT>()))[0], cT(nt2::Zero<T>(),nt2::Zero<T>()));
    NT2_TEST_EQUAL(nt2::multiplies(nt2::Zero<vdT>(), vcT(nt2::Zero<vT>(),nt2::Zero<vT>()))[0], cT(nt2::Zero<T>(),nt2::Zero<T>()));
    NT2_TEST_EQUAL(nt2::multiplies(nt2::Zero<vdT>(), vcT(nt2::One<vT>(), nt2::Zero<vT>()))[0], cT(nt2::Zero<T>(),nt2::Zero<T>()));
    NT2_TEST_EQUAL(nt2::multiplies(nt2::One<vdT>(),  vcT(nt2::One<vT>(), nt2::Zero<vT>()))[0], cT(nt2::One<T>(), nt2::Zero<T>()));
  }
  {
    typedef vciT r_t;
    #ifdef BOOST_SIMD_NO_INVALIDS
    NT2_TEST_EQUAL(nt2::multiplies(nt2::Inf<vdT>(),   nt2::Inf<vciT>())[0] , ciT(nt2::Inf<T>()));
    NT2_TEST_EQUAL(nt2::multiplies(nt2::One<vdT>(),   nt2::Zero<vciT>())[0], ciT(nt2::Zero<T>()));
    NT2_TEST_EQUAL(nt2::multiplies(nt2::Zero<vdT>(),  nt2::Zero<vciT>())[0], ciT(nt2::Zero<T>()));
    NT2_TEST_EQUAL(nt2::multiplies(nt2::Zero<vdT>(),  nt2::One<vciT>())[0] , ciT(nt2::Zero<T>()));
    NT2_TEST_EQUAL(nt2::multiplies(nt2::One<vdT>(),   nt2::One<vciT>())[0] , ciT(nt2::One<T>()));
    #else
    NT2_TEST_EQUAL(nt2::multiplies(nt2::Inf<vdT>(),   nt2::Inf<vciT>())[0] , cT(0, nt2::Inf<T>()));
    NT2_TEST_EQUAL(nt2::multiplies(nt2::One<vdT>(),   nt2::Zero<vciT>())[0], cT(0, nt2::Zero<T>()));
    NT2_TEST_EQUAL(nt2::multiplies(nt2::Zero<vdT>(),  nt2::Zero<vciT>())[0], cT(0, nt2::Zero<T>()));
    NT2_TEST_EQUAL(nt2::multiplies(nt2::Zero<vdT>(),  nt2::One<vciT>())[0] , cT(0, nt2::Zero<T>()));
    NT2_TEST_EQUAL(nt2::multiplies(nt2::One<vdT>(),   nt2::One<vciT>())[0] , cT(0, nt2::One<T>()));
    #endif
  }
  {
    typedef vdT r_t;
    #ifdef BOOST_SIMD_NO_INVALIDS
    typedef dT rT;
    #else
    typedef cT rT;
    #endif
    NT2_TEST_EQUAL(nt2::multiplies(vciT(nt2::Inf<vciT>()),   vciT(nt2::Inf<vciT>()))[0],  rT(nt2::Minf<dT>()));
    NT2_TEST_EQUAL(nt2::multiplies(vciT(nt2::One<vciT>()),   vciT(nt2::Zero<vciT>()))[0], rT(nt2::Zero<dT>()));
    NT2_TEST_EQUAL(nt2::multiplies(vciT(nt2::Zero<vciT>()),  vciT(nt2::Zero<vciT>()))[0], rT(nt2::Zero<dT>()));
    NT2_TEST_EQUAL(nt2::multiplies(vciT(nt2::Zero<vciT>()),  vciT(nt2::One<vciT>()))[0],  rT(nt2::Zero<dT>()));
    NT2_TEST_EQUAL(nt2::multiplies(vciT(nt2::One<vciT>()),   vciT(nt2::One<vciT>()))[0],  rT(nt2::Mone<dT>()));
  }
  {
    typedef vdT r_t;
    NT2_TEST_EQUAL(nt2::multiplies(vdT(nt2::Inf<vdT>()),   vdT(nt2::Inf<vdT>()))[0],  vdT(nt2::Inf<vdT>() )[0]);
    NT2_TEST_EQUAL(nt2::multiplies(vdT(nt2::One<vdT>()),   vdT(nt2::Zero<vdT>()))[0], vdT(nt2::Zero<vdT>())[0]);
    NT2_TEST_EQUAL(nt2::multiplies(vdT(nt2::Zero<vdT>()),  vdT(nt2::Zero<vdT>()))[0], vdT(nt2::Zero<vdT>())[0]);
    NT2_TEST_EQUAL(nt2::multiplies(vdT(nt2::Zero<vdT>()),  vdT(nt2::One<vdT>()))[0],  vdT(nt2::Zero<vdT>())[0]);
    NT2_TEST_EQUAL(nt2::multiplies(vdT(nt2::One<vdT>()),   vdT(nt2::One<vdT>()))[0],  vdT(nt2::One<vdT>() )[0]);
  }
} // end of test for floating_


//
// Copyright (c) 2015-2018 CNRS
//

#include <iostream>

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/geometry.hpp"
#include "pinocchio/parsers/sample-models.hpp"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE ( build_model_sample_humanoid_random )
{
  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model,true);

  BOOST_CHECK(model.nq == 33);
  BOOST_CHECK(model.nv == 32);

  pinocchio::Model modelff;
  pinocchio::buildModels::humanoidRandom(modelff,false);

  BOOST_CHECK(modelff.nq == 32);
  BOOST_CHECK(modelff.nv == 32);
}

BOOST_AUTO_TEST_CASE ( build_model_sample_manipulator )
{
  pinocchio::Model model;
  pinocchio::buildModels::manipulator(model);

  BOOST_CHECK(model.nq == 6);
  BOOST_CHECK(model.nv == 6);

#ifdef PINOCCHIO_WITH_HPP_FCL
  pinocchio::Data data(model);
  pinocchio::GeometryModel geom;
  pinocchio::buildModels::manipulatorGeometries(model,geom);
#endif
}

BOOST_AUTO_TEST_CASE ( build_model_sample_humanoid )
{
  pinocchio::Model model;
  pinocchio::buildModels::humanoid(model);
  pinocchio::Data data(model);

  BOOST_CHECK(model.nq == 35);
  BOOST_CHECK(model.nv == 34);

#ifdef PINOCCHIO_WITH_HPP_FCL
  pinocchio::GeometryModel geom;
  pinocchio::buildModels::humanoidGeometries(model,geom);
  pinocchio::GeometryData geomdata(geom);

  Eigen::VectorXd q = pinocchio::neutral(model);
  pinocchio::forwardKinematics(model,data,q);
  pinocchio::updateGeometryPlacements(model,data,geom,geomdata,q);
#endif

  /* We might want to check here the joint namings, and validate the
   * direct geometry with respect to reference values. */
}

BOOST_AUTO_TEST_SUITE_END()


/*=============================================================================
    Copyright (c) 2001-2014 Joel de Guzman

    Distributed under 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)
=============================================================================*/
///////////////////////////////////////////////////////////////////////////////
//
//  A Calculator example demonstrating the grammar and semantic actions
//  using lambda functions. The parser prints code suitable for a stack
//  based virtual machine.
//
//  [ JDG May 10, 2002 ]        spirit 1
//  [ JDG March 4, 2007 ]       spirit 2
//  [ JDG February 21, 2011 ]   spirit 2.5
//  [ JDG June 6, 2014 ]        spirit x3 (from qi calc2, but using lambda functions)
//
///////////////////////////////////////////////////////////////////////////////

#include <boost/config/warning_disable.hpp>
#include <boost/spirit/home/x3.hpp>
#include <boost/spirit/home/x3/support/ast/variant.hpp>
#include <boost/fusion/include/adapt_struct.hpp>

#include <iostream>
#include <string>
#include <list>
#include <numeric>

namespace x3 = boost::spirit::x3;

namespace client
{
    ///////////////////////////////////////////////////////////////////////////////
    //  Semantic actions
    ////////////////////////////////////////////////////////1///////////////////////
    namespace
    {
        using x3::_attr;

        auto do_int     = [](auto& ctx) { std::cout << "push " << _attr(ctx) << std::endl; };
        auto do_add     = []{ std::cout << "add\n"; };
        auto do_subt    = []{ std::cout << "subtract\n"; };
        auto do_mult    = []{ std::cout << "mult\n"; };
        auto do_div     = []{ std::cout << "divide\n"; };
        auto do_neg     = []{ std::cout << "negate\n"; };
    }

    ///////////////////////////////////////////////////////////////////////////////
    //  The calculator grammar
    ///////////////////////////////////////////////////////////////////////////////
    namespace calculator_grammar
    {
        using x3::uint_;
        using x3::char_;

        x3::rule<class expression> const expression("expression");
        x3::rule<class term> const term("term");
        x3::rule<class factor> const factor("factor");

        auto const expression_def =
            term
            >> *(   ('+' >> term            [do_add])
                |   ('-' >> term            [do_subt])
                )
            ;

        auto const term_def =
            factor
            >> *(   ('*' >> factor          [do_mult])
                |   ('/' >> factor          [do_div])
                )
            ;

        auto const factor_def =
                uint_                       [do_int]
            |   '(' >> expression >> ')'
            |   ('-' >> factor              [do_neg])
            |   ('+' >> factor)
            ;

        BOOST_SPIRIT_DEFINE(expression, term, factor);

        auto calculator = expression;
    }

    using calculator_grammar::calculator;

}

///////////////////////////////////////////////////////////////////////////////
//  Main program
///////////////////////////////////////////////////////////////////////////////
int
main()
{
    std::cout << "/////////////////////////////////////////////////////////\n\n";
    std::cout << "Expression parser...\n\n";
    std::cout << "/////////////////////////////////////////////////////////\n\n";
    std::cout << "Type an expression...or [q or Q] to quit\n\n";

    typedef std::string::const_iterator iterator_type;

    std::string str;
    while (std::getline(std::cin, str))
    {
        if (str.empty() || str[0] == 'q' || str[0] == 'Q')
            break;

        auto& calc = client::calculator;    // Our grammar

        iterator_type iter = str.begin();
        iterator_type end = str.end();
        boost::spirit::x3::ascii::space_type space;
        bool r = phrase_parse(iter, end, calc, space);

        if (r && iter == end)
        {
            std::cout << "-------------------------\n";
            std::cout << "Parsing succeeded\n";
            std::cout << "-------------------------\n";
        }
        else
        {
            std::string rest(iter, end);
            std::cout << "-------------------------\n";
            std::cout << "Parsing failed\n";
            std::cout << "stopped at: \"" << rest << "\"\n";
            std::cout << "-------------------------\n";
        }
    }

    std::cout << "Bye... :-) \n\n";
    return 0;
}


/*
 * This file belongs to the Galois project, a C++ library for exploiting parallelism.
 * The code is being released under the terms of the 3-Clause BSD License (a
 * copy is located in LICENSE.txt at the top-level directory).
 *
 * Copyright (C) 2018, The University of Texas at Austin. All rights reserved.
 * UNIVERSITY EXPRESSLY DISCLAIMS ANY AND ALL WARRANTIES CONCERNING THIS
 * SOFTWARE AND DOCUMENTATION, INCLUDING ANY WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR ANY PARTICULAR PURPOSE, NON-INFRINGEMENT AND WARRANTIES OF
 * PERFORMANCE, AND ANY WARRANTY THAT MIGHT OTHERWISE ARISE FROM COURSE OF
 * DEALING OR USAGE OF TRADE.  NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH
 * RESPECT TO THE USE OF THE SOFTWARE OR DOCUMENTATION. Under no circumstances
 * shall University be liable for incidental, special, indirect, direct or
 * consequential damages or loss of profits, interruption of business, or
 * related expenses which may arise from use of Software or Documentation,
 * including but not limited to those resulting from defects in Software and/or
 * Documentation, or loss or inaccuracy of data of any kind.
 */

#include "galois/Galois.h"
#include "galois/Bag.h"
#include "galois/Reduction.h"
#include "galois/Timer.h"
#include "galois/Timer.h"
#include "galois/graphs/LCGraph.h"
#include "galois/graphs/Graph.h"
#include "llvm/Support/CommandLine.h"
#include "llvm/ADT/SmallVector.h"
#include "Lonestar/BoilerPlate.h"

// kik
#include "galois/Atomic.h"

#include <string>
#include <sstream>
#include <limits>
#include <iostream>
#include <deque>
#include <cmath>
#include <functional>
#include <numeric>

#include <sys/time.h>

#include <omp.h>

#include <boost/config.hpp>
#include <boost/graph/adjacency_list.hpp>
//#include <boost/graph/sloan_ordering.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/bandwidth.hpp>
#include <boost/graph/profile.hpp>
#include <boost/graph/wavefront.hpp>

#include <queue>
#include <algorithm>
#include <boost/pending/queue.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/properties.hpp>
#include <boost/pending/indirect_cmp.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/cuthill_mckee_ordering.hpp>

#include <boost/heap/priority_queue.hpp>

#define FINE_GRAIN_TIMING
//#define GALOIS_JUNE
//#define PRINT_DEGREE_DISTR

#define W1 1 // default weight for the distance in the Sloan algorithm
#define W2 2 // default weight for the degree in the Sloan algorithm

static const char* name = "Sloan's reordering algorithm";
static const char* desc =
    "Computes a permutation of a matrix according to Sloan's algorithm";
static const char* url = 0;

//****** Command Line Options ******
enum BFSAlgo {
  serialSloan,
  // barrierSloan,
};

enum ExecPhase {
  INIT,
  RUN,
  TOTAL,
};

static const unsigned int DIST_INFINITY =
    std::numeric_limits<unsigned int>::max() - 1;

namespace cll = llvm::cl;
static cll::opt<unsigned int> startNode("startnode",
                                        cll::desc("Node to start search from"),
                                        cll::init(DIST_INFINITY));
static cll::opt<unsigned int>
    reportNode("reportnode", cll::desc("Node to report distance to"),
               cll::init(1));
static cll::opt<bool> scaling(
    "scaling",
    llvm::cl::desc(
        "Scale to the number of threads with a given step starting from"),
    llvm::cl::init(false));
static cll::opt<unsigned int> scalingStep("step", cll::desc("Scaling step"),
                                          cll::init(2));
static cll::opt<unsigned int>
    niter("iter", cll::desc("Number of benchmarking iterations"), cll::init(5));
static cll::opt<BFSAlgo> algo(
    cll::desc("Choose an algorithm:"),
    cll::values(clEnumVal(serialSloan, "Boost Serial Sloan"), clEnumValEnd),
    cll::init(serialSloan));
static cll::opt<std::string> filename(cll::Positional,
                                      cll::desc("<input file>"), cll::Required);

//****** Work Item and Node Data Defintions ******
struct SNode {
  unsigned int dist;
  unsigned int id;
  // unsigned int sum;
  // bool flag;
  // std::vector<galois::graphs::LC_CSR_Graph<SNode, void>::GraphNode> bucket;
};

struct Prefix {
  unsigned int id;
  unsigned int val;
  Prefix(unsigned int _id, unsigned _val) : id(_id), val(_val) {}
};

// typedef galois::graphs::LC_Linear_Graph<SNode, void> Graph;
typedef galois::graphs::LC_CSR_Graph<SNode, void>::_with_no_lockable<
    true>::_with_numa_alloc<true>
    Graph;
// typedef galois::graphs::LC_CSRInline_Graph<SNode, char> Graph;
// typedef galois::graphs::MorphGraph<SNode, void, false> Graph;
typedef Graph::GraphNode GNode;

typedef boost::adjacency_list<
    boost::setS, boost::vecS, boost::undirectedS,
    boost::property<
        boost::vertex_color_t, boost::default_color_type,
        boost::property<boost::vertex_degree_t, int,
                        boost::property<boost::vertex_priority_t, signed int>>>>
    BGraph;

typedef boost::graph_traits<BGraph>::vertex_descriptor Vertex;
typedef boost::graph_traits<BGraph>::vertices_size_type size_type;

Graph graph;
BGraph* bgraph = NULL;

struct PSum {
  unsigned int sum;
  PSum(unsigned int _s) : sum(_s) {}
};

static size_t degree(const GNode& node) {
  return std::distance(graph.edge_begin(node), graph.edge_end(node));
}

struct UpdateRequest {
  GNode n;
  unsigned int w;

  UpdateRequest() : w(0) {}
  UpdateRequest(const GNode& N, unsigned int W) : n(N), w(W) {}
  bool operator<(const UpdateRequest& o) const { return w < o.w; }
  bool operator>(const UpdateRequest& o) const { return w > o.w; }
  unsigned getID() const { return /* graph.getData(n).id; */ 0; }
};

std::ostream& operator<<(std::ostream& out, const SNode& n) {
  out << "(dist: " << n.dist << ")";
  return out;
}

struct UpdateRequestIndexer {
  unsigned int operator()(const UpdateRequest& val) const {
    unsigned int t = val.w;
    return t;
  }
};

struct GNodeIndexer {
  unsigned int operator()(const GNode& val) const {
    return graph.getData(val, galois::MethodFlag::UNPROTECTED).dist;
  }
};

struct GNodeLess {
  bool operator()(const GNode& a, const GNode& b) const {
    return graph.getData(a, galois::MethodFlag::UNPROTECTED).dist <
           graph.getData(b, galois::MethodFlag::UNPROTECTED).dist;
  }
};

struct GNodeGreater {
  bool operator()(const GNode& a, const GNode& b) const {
    return graph.getData(a, galois::MethodFlag::UNPROTECTED).dist >
           graph.getData(b, galois::MethodFlag::UNPROTECTED).dist;
  }
};

struct GNodeBefore {
  bool operator()(const GNode& a, const GNode& b) const {
    return (degree(a) < degree(b));
  }
};

std::vector<GNode> perm;
// std::vector<GNode> tperm;
// std::vector< std::vector<GNode> > bucket;
// debug
galois::GAtomic<unsigned int> loops     = galois::GAtomic<unsigned int>(0);
galois::GAtomic<unsigned int> sorts     = galois::GAtomic<unsigned int>(0);
galois::GAtomic<unsigned int> maxbucket = galois::GAtomic<unsigned int>(0);
galois::GAtomic<unsigned int> minbucket =
    galois::GAtomic<unsigned int>(DIST_INFINITY);
galois::GAtomic<unsigned int> avgbucket   = galois::GAtomic<unsigned int>(0);
galois::GAtomic<unsigned int> numbucket   = galois::GAtomic<unsigned int>(0);
galois::GAtomic<unsigned int> smallbucket = galois::GAtomic<unsigned int>(0);

static void printSloan() {
  std::cerr << "Sloan Permutation:\n";

  for (std::vector<GNode>::iterator nit = perm.begin(); nit != perm.end();
       nit++) {
    SNode& data = graph.getData(*nit);
    // std::cerr << "[" << data.id << "] level: " << data.dist << " degree: " <<
    // data.degree << "\n"; std::cerr << data.id + 1 << " (" << data.degree << ")
    // level: " << data.dist << "\n";
    std::cerr << data.id + 1 << "\n";
  }
  std::cerr << "\n";
}

static void permute() {

  // std::vector<GNode> perm;
  // perm.reserve(graph.size());;

  std::vector<GNode> nodemap;
  nodemap.reserve(graph.size());
  ;

  for (Graph::iterator src = graph.begin(), ei = graph.end(); src != ei;
       ++src) {

    nodemap[graph.getData(*src).id] = *src;
  }

  unsigned int N = perm.size() - 1;

  for (int i = N; i >= 0; --i) {
    // std::cerr << perm[i] << " " << graph.getData(nodemap[permid[i]]).id << "
    // changes to: " << N - i << "\n";
    graph.getData(perm[i]).id = N - i;
  }
}

// debugging
static void printAccess(std::string msg) {
  std::cerr << msg << " Access Pattern:\n";

  std::vector<unsigned int> temp;

  for (Graph::iterator src = graph.begin(), ei = graph.end(); src != ei;
       ++src) {

    SNode& sdata = graph.getData(*src);

    std::cerr << sdata.id << " connected with (" << degree(*src) << "): ";

    for (Graph::edge_iterator
             ii = graph.edge_begin(*src, galois::MethodFlag::UNPROTECTED),
             ei = graph.edge_end(*src, galois::MethodFlag::UNPROTECTED);
         ii != ei; ++ii) {
      GNode dst    = graph.getEdgeDst(ii);
      SNode& ddata = graph.getData(dst, galois::MethodFlag::UNPROTECTED);

      unsigned int diff = abs(sdata.id - ddata.id);

      std::cerr << ddata.id << " (" << diff << "), ";
    }

    std::cerr << "\n";
    // std::cerr << data.id << " (" << degree(*src) << ") level: " << data.dist
    // << " reads: " << data.read << " writes: " << data.write << "\n"; std::cerr
    // << data.id << " (" << degree(*src) << ") level: " << data.dist << "\n";

    temp.push_back(sdata.id);
  }

  // for(std::vector<unsigned int>::reverse_iterator nit = temp.rbegin(); nit !=
  // temp.rend(); nit++)
  for (std::vector<unsigned int>::iterator nit = temp.begin();
       nit != temp.end(); nit++) {
    std::cerr << *nit + 1 << "\n";
  }
  std::cerr << "\n";
}

static void findStartingNode(GNode& starting) {
  unsigned int mindegree = DIST_INFINITY;

  for (Graph::iterator src = graph.begin(), ei = graph.end(); src != ei;
       ++src) {
    unsigned int nodedegree = degree(*src);

    if (nodedegree < mindegree) {
      mindegree = nodedegree;
      starting  = *src;
    }
  }

  SNode& data = graph.getData(starting);
  std::cerr << "Starting Node: " << data.id << " degree: " << degree(starting)
            << "\n";
}

// Compute variance around mean distance from the source
static void variance(unsigned long int mean) {
  unsigned long int n = 0;
  long double M2      = 0.0;
  long double var     = 0.0;

  for (Graph::iterator src = graph.begin(), ei = graph.end(); src != ei;
       ++src) {
    SNode& data = graph.getData(*src);
    if (data.dist < DIST_INFINITY) {
      M2 += (data.dist - mean) * (data.dist - mean);
      ++n;
    }
  }

  var = M2 / (n - 1);
  std::cout << "var: " << var << " mean: " << mean << "\n";
}

struct not_consistent {
  bool operator()(GNode n) const {
    unsigned int dist = graph.getData(n).dist;
    for (Graph::edge_iterator ii = graph.edge_begin(n), ei = graph.edge_end(n);
         ii != ei; ++ii) {
      GNode dst          = graph.getEdgeDst(ii);
      unsigned int ddist = graph.getData(dst).dist;
      if (ddist > dist + 1) {
        std::cerr << "bad level value for " << graph.getData(dst).id << ": "
                  << ddist << " > " << (dist + 1) << "\n";
        return true;
      }
    }
    return false;
  }
};

struct not_visited {
  bool operator()(GNode n) const {
    unsigned int dist = graph.getData(n).dist;
    if (dist >= DIST_INFINITY) {
      std::cerr << "unvisited node " << graph.getData(n).id << ": " << dist
                << " >= INFINITY\n";
      return true;
    }
    // std::cerr << "visited node " << graph.getData(n).id << ": " << dist <<
    // "\n";
    return false;
  }
};

//! Simple verifier
static bool verify(GNode& source) {
  if (graph.getData(source).dist != 0) {
    std::cerr << "source has non-zero dist value\n";
    return false;
  }

  // size_t id = 0;

#ifdef GALOIS_JUNE
  bool okay =
      galois::find_if(graph.begin(), graph.end(), not_consistent()) ==
          graph.end() &&
      galois::find_if(graph.begin(), graph.end(), not_visited()) == graph.end();
#else
  bool okay = galois::ParallelSTL::find_if(graph.begin(), graph.end(),
                                           not_consistent()) == graph.end() &&
              galois::ParallelSTL::find_if(graph.begin(), graph.end(),
                                           not_visited()) == graph.end();
#endif

  // if (okay) {
  galois::GReduceMax<unsigned int> maxDist;
  galois::GAccumulator<unsigned> sum;
  galois::GAccumulator<unsigned> count;

  galois::do_all(galois::iterate(graph), [&](const GNode& n) {
    auto d = graph.getData(n, galois::MethodFlag::UNPROTECTED).dist;
    if (d < INFINITY) {
      maxDist.update(d);
      sum += d;
      count += 1;
    }
  });
  std::cout << "max dist: " << maxDist.reduce() << "\n";
  unsigned mean = sum.reduce() / count.reduce();
  std::cout << "avg dist: " << mean << "\n";

  variance(mean.reduce());
  //}

  return okay;
}

// Clear node data to re-execute on specific graph
struct resetNode {
  void operator()(const GNode& n) const {
    graph.getData(n).dist = DIST_INFINITY;
    // graph.getData(n).flag = false;
    // graph.getData(n).bucket.clear();
  }
};

static void resetGraph() {
  galois::do_all(graph.begin(), graph.end(), resetNode());
  perm.clear();
}

static void printDegreeDistribution() {
  std::map<unsigned int, unsigned int> distr;

  for (Graph::iterator n = graph.begin(), ei = graph.end(); n != ei; ++n) {
    distr[degree(*n)]++;
    // std::cerr << graph.getData(*n, galois::MethodFlag::UNPROTECTED).id << "	"
    // << graph.getData(*n, galois::MethodFlag::UNPROTECTED).dist << "\n";
  }

  std::cerr << "Degree	Count\n";
  for (std::map<unsigned int, unsigned int>::iterator slot = distr.begin(),
                                                      ei   = distr.end();
       slot != ei; ++slot) {
    std::cerr << slot->first << "	" << slot->second << "\n";
  }
}

// Read graph from a binary .gr as dirived from a Matrix Market .mtx using
// graph-convert
static void readGraph(GNode& source, GNode& report) {
  galois::graphs::readGraph(graph, filename);
  source = *graph.begin();

  size_t nnodes = graph.size();
  std::cout << "Read " << nnodes << " nodes\n";

  size_t id = 0;

  bgraph = new BGraph(nnodes);

  for (Graph::iterator src = graph.begin(), ei = graph.end(); src != ei;
       ++src) {
    SNode& node = graph.getData(*src, galois::MethodFlag::UNPROTECTED);
    node.id     = id++;
  }

  std::cout << "Read binary graph\n";

  for (Graph::iterator src = graph.begin(), ei = graph.end(); src != ei;
       ++src) {
    SNode& dsrc = graph.getData(*src, galois::MethodFlag::UNPROTECTED);
    for (Graph::edge_iterator
             ii = graph.edge_begin(*src, galois::MethodFlag::UNPROTECTED),
             ei = graph.edge_end(*src, galois::MethodFlag::UNPROTECTED);
         ii != ei; ++ii) {
      GNode dst   = graph.getEdgeDst(ii);
      SNode& ddst = graph.getData(dst, galois::MethodFlag::UNPROTECTED);
      boost::add_edge(dsrc.id, ddst.id, *bgraph);
    }
  }
}

//! Serial BFS using Galois graph
struct BoostSloan {
  std::string name() const { return "Boost Serial Sloan"; }

  static void sloan_ordering(
      BGraph& g, Vertex s, Vertex e, std::vector<Vertex>::iterator permutation,
      boost::property_map<BGraph, boost::vertex_color_t>::type color,
      boost::property_map<BGraph, boost::vertex_degree_t>::type degree,
      boost::property_map<BGraph, boost::vertex_priority_t>::type priority) {
    // typedef typename boost::property_traits<boost::property_map<BGraph,
    // boost::vertex_degree_t>::type >::value_type Degree;
    typedef boost::property_traits<
        boost::property_map<BGraph, boost::vertex_priority_t>::type>::value_type
        Degree;
    typedef boost::property_traits<
        boost::property_map<BGraph, boost::vertex_color_t>::type>::value_type
        ColorValue;
    // typedef typename boost::property_traits<boost::property<
    // boost::vertex_color_t, boost::default_color_type> >::value_type
    // ColorValue;
    typedef boost::color_traits<ColorValue> Color;
    typedef boost::graph_traits<BGraph>::vertex_descriptor Vertex;
    typedef std::vector<size_type>::iterator vec_iter;

    typedef boost::property_map<BGraph, boost::vertex_index_t>::const_type
        VertexID;

    std::vector<Vertex>::iterator skato = permutation;

#ifdef FINE_GRAIN_TIMING
    galois::TimeAccumulator vTmain[5];
    vTmain[0] = galois::TimeAccumulator();
    vTmain[1] = galois::TimeAccumulator();
    vTmain[2] = galois::TimeAccumulator();
    vTmain[3] = galois::TimeAccumulator();
    vTmain[4] = galois::TimeAccumulator();

    vTmain[0].start();
#endif

    // Creating a std-vector for storing the distance from the end vertex in it
    std::vector<size_type> dist(num_vertices(g), 0);

    // Wrap a property_map_iterator around the std::iterator
    boost::iterator_property_map<vec_iter, VertexID, size_type, size_type&>
        dist_pmap(dist.begin(), get(boost::vertex_index, g));

    boost::breadth_first_search(
        g, e,
        boost::visitor(boost::make_bfs_visitor(
            boost::record_distances(dist_pmap, boost::on_tree_edge()))));

    // Creating a property_map for the indices of a vertex
    boost::property_map<BGraph, boost::vertex_index_t>::type index_map =
        get(boost::vertex_index, g);

    // Sets the color and priority to their initial status
    unsigned cdeg;
    boost::graph_traits<BGraph>::vertex_iterator ui, ui_end;
    for (boost::tie(ui, ui_end) = boost::vertices(g); ui != ui_end; ++ui) {
      put(color, *ui, Color::white());
      cdeg = get(degree, *ui) + 1;
      put(priority, *ui, W1 * dist[index_map[*ui]] - W2 * cdeg);
      // int skata = W1*dist[index_map[*ui]]-W2*cdeg;
      // std::cerr << "[" << index_map[*ui] << "]: " << get(priority, *ui) << "
      // cdge: " << cdeg << " dist: " << dist[index_map[*ui]] << " prio: " <<
      // W1*dist[index_map[*ui]]+W2*cdeg<< " real: " <<
      // W1*dist[index_map[*ui]]-W2*cdeg << " skata: " << skata << "\n";
    }

    // Priority list
    typedef boost::indirect_cmp<
        boost::property_map<BGraph, boost::vertex_priority_t>::type,
        std::greater<Degree>>
        Compare;

    Compare comp(priority);
    std::list<Vertex> priority_list;

    // Some more declarations
    boost::graph_traits<BGraph>::out_edge_iterator ei, ei_end, ei2, ei2_end;
    Vertex u, v, w;

    put(color, s,
        Color::green()); // Sets the color of the starting vertex to gray
    priority_list.push_front(s); // Puts s into the priority_list

#ifdef FINE_GRAIN_TIMING
    vTmain[0].stop();
    vTmain[4].start();
#endif

    while (!priority_list.empty()) {
#ifdef FINE_GRAIN_TIMING
      vTmain[3].start();
#endif

      // std::cerr << "Sorting " << priority_list.size() << "\n";
      priority_list.sort(comp); // Orders the elements in the priority list in
                                // an ascending manner

      /*
      std::cerr << "Queue: ";
      for(std::list<Vertex>::iterator ii = priority_list.begin(), ee =
      priority_list.end(); ii != ee; ++ii){ std::cerr << index_map[*ii] << "("
      << get(priority, *ii) << ") ";
      }
      std::cerr << "\n";
      */

#ifdef FINE_GRAIN_TIMING
      vTmain[3].stop();
      vTmain[1].start();
#endif

      u = priority_list
              .front(); // Accesses the last element in the priority list
      priority_list.pop_front(); // Removes the last element in the priority
                                 // list

      if (get(color, u) == Color::green()) {
        // for-loop over all out-edges of vertex u
        for (boost::tie(ei, ei_end) = boost::out_edges(u, g); ei != ei_end;
             ++ei) {
          v = boost::target(*ei, g);

          put(priority, v, get(priority, v) + W2); // updates the priority

          if (get(color, v) == Color::white()) // test if the vertex is inactive
          {
            put(color, v,
                Color::green()); // giving the vertex a preactive status
            priority_list.push_front(
                v); // writing the vertex in the priority_queue
          }
        }
      }

      // Here starts step 8
      *permutation++ =
          u; // Puts u to the first position in the permutation-vector
      put(color, u, Color::black()); // Gives u an inactive status

      /*
      std::cerr << "Permutation: ";
      for(std::vector<Vertex>::iterator ii = skato, ee = permutation; ii != ee;
      ++ii){
              //std::cerr << index_map[*ii] << " ";
              std::cerr << index_map[*ii] << "(" << get(priority, *ii) << ") ";
      }
      std::cerr << "\n";
      */

#ifdef FINE_GRAIN_TIMING
      vTmain[1].stop();
      vTmain[2].start();
#endif

      // for loop over all the adjacent vertices of u
      for (boost::tie(ei, ei_end) = out_edges(u, g); ei != ei_end; ++ei) {

        v = target(*ei, g);

        if (get(color, v) == Color::green()) { // tests if the vertex is
                                               // inactive

          put(color, v, Color::red()); // giving the vertex an active status
          put(priority, v, get(priority, v) + W2); // updates the priority

          // for loop over alll adjacent vertices of v
          for (boost::tie(ei2, ei2_end) = out_edges(v, g); ei2 != ei2_end;
               ++ei2) {
            w = target(*ei2, g);

            if (get(color, w) !=
                Color::black()) { // tests if vertex is postactive

              put(priority, w, get(priority, w) + W2); // updates the priority

              if (get(color, w) == Color::white()) {

                put(color, w,
                    Color::green()); // gives the vertex a preactive status
                priority_list.push_front(
                    w); // puts the vertex into the priority queue

              } // end if

            } // end if

          } // end for

        } // end if

      } // end for
#ifdef FINE_GRAIN_TIMING
      vTmain[2].stop();
#endif

    } // end while
#ifdef FINE_GRAIN_TIMING
    vTmain[4].stop();
#endif

#ifdef FINE_GRAIN_TIMING
    std::cout << "Init: " << vTmain[0].get() << "\n";
    std::cout << "First phase: " << vTmain[1].get() << "\n";
    std::cout << "Second phase for loop: " << vTmain[2].get() << "\n";
    std::cout << "Sort: " << vTmain[3].get() << "\n";
    std::cout << "Total comp: " << vTmain[4].get() << "\n";
#endif

    // return permutation;
  }

  void operator()(const unsigned int& source) const {
    boost::graph_traits<BGraph>::vertex_iterator ui, ui_end;

    boost::property_map<BGraph, boost::vertex_degree_t>::type deg =
        get(boost::vertex_degree, *bgraph);
    for (boost::tie(ui, ui_end) = boost::vertices(*bgraph); ui != ui_end; ++ui)
      deg[*ui] = boost::degree(*ui, *bgraph);

    boost::property_map<BGraph, boost::vertex_index_t>::type index_map =
        boost::get(boost::vertex_index, *bgraph);

    // Creating a vector of vertices
    std::vector<Vertex> sloan_order(num_vertices(*bgraph));
    // Creating a vector of size_type
    std::vector<size_type> perm(num_vertices(*bgraph));

    Vertex s = boost::vertex(source, *bgraph);
    int ecc; // defining a variable for the pseudoperipheral radius

    // Calculating the pseudoeperipheral node and radius
    Vertex e = boost::pseudo_peripheral_pair(
        *bgraph, s, ecc, get(boost::vertex_color, *bgraph),
        get(boost::vertex_degree, *bgraph));

    std::cout << std::endl;
    std::cout << "Starting vertex: " << s << std::endl;
    std::cout << "Pseudoperipheral vertex: " << e << std::endl;
    std::cout << "Pseudoperipheral radius: " << ecc << std::endl << std::endl;

    // Sloan ordering
    // boost::sloan_ordering(*bgraph, s, e, sloan_order.begin(),
    // get(boost::vertex_color, *bgraph), get(boost::vertex_degree, *bgraph),
    // get(boost::vertex_priority, *bgraph));
    sloan_ordering(*bgraph, s, e, sloan_order.begin(),
                   get(boost::vertex_color, *bgraph),
                   get(boost::vertex_degree, *bgraph),
                   get(boost::vertex_priority, *bgraph));

    /*
    std::cout << "Sloan ordering starting at: " << s << std::endl;

    for (std::vector<Vertex>::const_iterator i = sloan_order.begin(); i !=
    sloan_order.end(); ++i) std::cout << index_map[*i] << "\n"; std::cout <<
    std::endl;
    */

    for (size_type c = 0; c != sloan_order.size(); ++c)
      perm[index_map[sloan_order[c]]] = c;

    std::cout << "bandwidth: "
              << bandwidth(*bgraph, make_iterator_property_map(
                                        &perm[0], index_map, perm[0]))
              << std::endl;
    std::cout << "profile: "
              << profile(*bgraph, make_iterator_property_map(
                                      &perm[0], index_map, perm[0]))
              << std::endl;
    /*
std::cout << "max_wavefront: "
    << max_wavefront(*bgraph, make_iterator_property_map(&perm[0], index_map,
perm[0]))
    << std::endl;
std::cout << "aver_wavefront: "
    << aver_wavefront(*bgraph, make_iterator_property_map(&perm[0], index_map,
perm[0]))
    << std::endl;
std::cout << "rms_wavefront: "
    << rms_wavefront(*bgraph, make_iterator_property_map(&perm[0], index_map,
perm[0]))
    << std::endl;
    */
  }
};

template <typename AlgoTy>
void run(const AlgoTy& algo) {
  GNode source, report;

  int maxThreads = numThreads;
  galois::TimeAccumulator vT[maxThreads + 2];

  // Measure time to read graph
  vT[INIT] = galois::TimeAccumulator();
  vT[INIT].start();

  readGraph(source, report);

  std::cout << "original bandwidth: " << boost::bandwidth(*bgraph) << std::endl;
  std::cout << "original profile: " << boost::profile(*bgraph) << std::endl;
  // std::cout << "original max_wavefront: " << boost::max_wavefront(*bgraph) <<
  // std::endl; std::cout << "original aver_wavefront: " <<
  // boost::aver_wavefront(*bgraph) << std::endl; std::cout << "original
  // rms_wavefront: " << boost::rms_wavefront(*bgraph) << std::endl;

  vT[INIT].stop();

  std::cout << "Init: " << vT[INIT].get() << " ( "
            << (double)vT[INIT].get() / 1000 << " seconds )\n";

  // Measure total computation time to read graph
  vT[TOTAL].start();

  // galois::setActiveThreads(1);

  // Execution with the specified number of threads
  vT[RUN] = galois::TimeAccumulator();

  std::cout << "Running " << algo.name() << " version with " << numThreads
            << " threads for " << niter << " iterations\n";

  // I've observed cold start. First run takes a few millis more.
  // algo(source);

  for (int i = 0; i < niter; i++) {
    vT[RUN].start();

    algo(graph.getData(source).id);

    vT[RUN].stop();

    std::cout << "Iteration " << i << " numthreads: " << numThreads << " "
              << vT[RUN].get() << "\n";
  }

  std::cout << "Final time numthreads: " << numThreads << " " << vT[RUN].get()
            << "\n";
  std::cout << "Avg time numthreads: " << numThreads << " "
            << vT[RUN].get() / niter << "\n";

#ifdef PRINT_DEGREE_DISTR
  printDegreeDistribution();
#endif

  vT[TOTAL].stop();

  std::cout << "Total with threads: " << numThreads << " " << vT[TOTAL].get()
            << " ( " << (double)vT[TOTAL].get() / 1000 << " seconds )\n";

  if (!skipVerify) {
    if (verify(source)) {
      std::cout << "Verification successful.\n";
    } else {
      std::cerr << "Verification failed.\n";
      assert(0 && "Verification failed");
      abort();
    }
  }
}

int main(int argc, char** argv) {
  // galois::StatManager statManager;
  LonestarStart(argc, argv, name, desc, url);

  using namespace galois::worklists;
  typedef BulkSynchronous<PerSocketChunkLIFO<256>> BSWL;

  typedef BSWL BSInline;

  switch (algo) {
  case serialSloan:
    run(BoostSloan());
    break;
    // case barrierSloan: run(BarrierRegular()); break;
  default:
    std::cerr << "Unknown algorithm" << algo << "\n";
    abort();
  }

  return 0;
}


// Copyright (C) 2016-2018 T. Zachary Laine
//
// Distributed under 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)
//[ vec3
#include <boost/yap/yap.hpp>

#include <array>
#include <iostream>

struct take_nth
{
    auto operator() (boost::yap::terminal<boost::yap::expression, std::array<int, 3>> const & expr)
    {
        int x = boost::yap::value(expr)[n];
        // The move forces the terminal to store the value of x, not a
        // reference.
        return boost::yap::make_terminal(std::move(x));
    }

    std::size_t n;
};

// Since this example doesn't constrain the operators defined on its
// expressions, we can just use boost::yap::expression<> as the expression
// template.
using vec3_terminal = boost::yap::expression<
    boost::yap::expr_kind::terminal,
    boost::hana::tuple<std::array<int, 3>>
>;

// Customize the terminal type we use by adding index and assignment
// operations.
struct vec3 : vec3_terminal
{
    explicit vec3 (int i = 0, int j = 0, int k = 0)
    {
        (*this)[0] = i;
        (*this)[1] = j;
        (*this)[2] = k;
    }

    explicit vec3 (std::array<int, 3> a)
    {
        (*this)[0] = a[0];
        (*this)[1] = a[1];
        (*this)[2] = a[2];
    }

    int & operator[] (std::ptrdiff_t i)
    { return boost::yap::value(*this)[i]; }

    int const & operator[] (std::ptrdiff_t i) const
    { return boost::yap::value(*this)[i]; }

    template <typename T>
    vec3 & operator= (T const & t)
    {
        decltype(auto) expr = boost::yap::as_expr(t);
        (*this)[0] = boost::yap::evaluate(boost::yap::transform(expr, take_nth{0}));
        (*this)[1] = boost::yap::evaluate(boost::yap::transform(expr, take_nth{1}));
        (*this)[2] = boost::yap::evaluate(boost::yap::transform(expr, take_nth{2}));
        return *this;
    }

    void print() const
    {
        std::cout << '{' << (*this)[0]
                  << ", " << (*this)[1]
                  << ", " << (*this)[2]
                  << '}' << std::endl;
    }
};

// This is a stateful transform that keeps a running count of the terminals it
// has seen.
struct count_leaves_impl
{
    auto operator() (vec3_terminal const & expr)
    {
        value += 1;
        return expr;
    }

    int value = 0;
};

template <typename Expr>
int count_leaves (Expr const & expr)
{
    count_leaves_impl impl;
    boost::yap::transform(boost::yap::as_expr(expr), impl);
    return impl.value;
}

int main()
{
    vec3 a, b, c;

    c = 4;

    b[0] = -1;
    b[1] = -2;
    b[2] = -3;

    a = b + c;

    a.print();

    vec3 d;
    auto expr1 = b + c;
    d = expr1;
    d.print();

    int num = count_leaves(expr1);
    std::cout << num << std::endl;

    num = count_leaves(b + 3 * c);
    std::cout << num << std::endl;

    num = count_leaves(b + c * d);
    std::cout << num << std::endl;

    return 0;
}
//]