#include "anomaly.h"
#include "../util.h"

#include <boost/exception/all.hpp>

namespace Akumuli {
namespace QP {

static AnomalyDetector::FcastMethod parse_anomaly_detector_type(boost::property_tree::ptree const& ptree) {
    bool approx = ptree.get<bool>("approx");
    std::string name = ptree.get<std::string>("method");
    AnomalyDetector::FcastMethod method;
    if (name == "ewma" || name == "exp-smoothing") {
        method = approx ? AnomalyDetector::EWMA_SKETCH : AnomalyDetector::EWMA;
    } else if (name == "sma" || name == "simple-moving-average") {
        method = approx ? AnomalyDetector::SMA_SKETCH : AnomalyDetector::SMA;
    } else if (name == "double-exp-smoothing") {
        method = approx ? AnomalyDetector::DOUBLE_EXP_SMOOTHING_SKETCH : AnomalyDetector::DOUBLE_EXP_SMOOTHING;
    } else if (name == "holt-winters") {
        method = approx ? AnomalyDetector::HOLT_WINTERS_SKETCH : AnomalyDetector::HOLT_WINTERS;
    } else {
        QueryParserError err("Unknown forecasting method");
        BOOST_THROW_EXCEPTION(err);
    }
    return method;
}

static void validate_sketch_params(boost::property_tree::ptree const& ptree) {
    uint32_t bits = ptree.get<uint32_t>("bits", 8);
    uint32_t hashes = ptree.get<uint32_t>("hashes", 1);
    // bits should be in range
    if (bits < 8 || bits > 16) {
        QueryParserError err("Anomaly detector parameter `bits` out of range");
        BOOST_THROW_EXCEPTION(err);
    }
    // hashes should be in range and odd
    if (hashes % 2 == 0) {
        QueryParserError err("Anomaly detector parameter `hashes` should be odd");
        BOOST_THROW_EXCEPTION(err);
    }
    if (hashes == 0 || hashes > 9) {
        QueryParserError err("Anomaly detector parameter `hashes` out of range");
        BOOST_THROW_EXCEPTION(err);
    }
}

static void validate_all_params(std::vector<std::string> required, boost::property_tree::ptree const& ptree) {
    for (auto name: required) {
        auto o = ptree.get_optional<std::string>(name);
        if (!o) {
            std::string err_msg = "Parameter " + name + " should be set";
            QueryParserError err(err_msg.c_str());
            BOOST_THROW_EXCEPTION(err);
        }
    }
}

static void validate_anomaly_detector_params(boost::property_tree::ptree const& ptree) {
    auto type = parse_anomaly_detector_type(ptree);
    switch(type) {
    case AnomalyDetector::SMA_SKETCH:
        validate_sketch_params(ptree);
    case AnomalyDetector::SMA:
        validate_all_params({"period"}, ptree);
        break;

    case AnomalyDetector::EWMA_SKETCH:
        validate_sketch_params(ptree);
    case AnomalyDetector::EWMA:
        validate_all_params({"alpha"}, ptree);
        break;

    case AnomalyDetector::DOUBLE_EXP_SMOOTHING_SKETCH:
        validate_sketch_params(ptree);
    case AnomalyDetector::DOUBLE_EXP_SMOOTHING:
        validate_all_params({"alpha", "gamma"}, ptree);
        break;

    case AnomalyDetector::HOLT_WINTERS_SKETCH:
        validate_sketch_params(ptree);
    case AnomalyDetector::HOLT_WINTERS:
        validate_all_params({"alpha", "beta", "gamma", "period"}, ptree);
        break;
    }
}

static void validate_coef(double value, double range_begin, double range_end, const char* err_msg) {
    if (value >= range_begin && value <= range_end) {
        return;
    }
    QueryParserError err(err_msg);
    BOOST_THROW_EXCEPTION(err);
}

AnomalyDetector::AnomalyDetector(boost::property_tree::ptree const& ptree, std::shared_ptr<Node> next)
    : next_(next)
{
    validate_anomaly_detector_params(ptree);
    double threshold = ptree.get<double>("threshold");
    uint32_t bits = ptree.get<uint32_t>("bits", 10u);
    uint32_t nhashes = ptree.get<uint32_t>("hashes", 3u);
    AnomalyDetector::FcastMethod method = parse_anomaly_detector_type(ptree);
    double alpha = ptree.get<double>("alpha", 0.0);
    double beta = ptree.get<double>("beta", 0.0);
    double gamma = ptree.get<double>("gamma", 0.0);
    int period = ptree.get<int>("period", 0);
    validate_coef(alpha, 0.0, 1.0, "`alpha` should be in [0, 1] range");
    validate_coef(beta,  0.0, 1.0, "`beta` should be in [0, 1] range");
    validate_coef(gamma, 0.0, 1.0, "`gamma` should be in [0, 1] range");

    switch(method) {
    case SMA:
        detector_ = AnomalyDetectorUtil::create_precise_sma(threshold, period);
        break;
    case SMA_SKETCH:
        detector_ = AnomalyDetectorUtil::create_approx_sma(nhashes, 1 << bits, threshold, period);
        break;
    case EWMA:
        detector_ = AnomalyDetectorUtil::create_precise_ewma(threshold, alpha);
        break;
    case EWMA_SKETCH:
        detector_ = AnomalyDetectorUtil::create_approx_ewma(nhashes, 1 << bits, threshold, alpha);
        break;
    case DOUBLE_EXP_SMOOTHING:
        detector_ = AnomalyDetectorUtil::create_precise_double_exp_smoothing(threshold, alpha, gamma);
        break;
    case DOUBLE_EXP_SMOOTHING_SKETCH:
        detector_ = AnomalyDetectorUtil::create_approx_double_exp_smoothing(nhashes, 1 << bits, threshold, alpha, gamma);
        break;
    case HOLT_WINTERS:
        detector_ = AnomalyDetectorUtil::create_precise_holt_winters(threshold, alpha, beta, gamma, period);
        break;
    case HOLT_WINTERS_SKETCH:
        detector_ = AnomalyDetectorUtil::create_approx_holt_winters(nhashes, 1 << bits, threshold, alpha, beta, gamma, period);
        break;
    default:
        AKU_PANIC("AnomalyDetector building error");
    }
}

AnomalyDetector::AnomalyDetector(
        uint32_t nhashes,
        uint32_t bits,
        double   threshold,
        double   alpha,
        double   beta,
        double   gamma,
        int      period,
        FcastMethod method,
        std::shared_ptr<Node> next)
    : next_(next)
{
    try {
        switch(method) {
        case SMA:
            detector_ = AnomalyDetectorUtil::create_precise_sma(threshold, period);
            break;
        case SMA_SKETCH:
            detector_ = AnomalyDetectorUtil::create_approx_sma(nhashes, 1 << bits, threshold, period);
            break;
        case EWMA:
            detector_ = AnomalyDetectorUtil::create_precise_ewma(threshold, alpha);
            break;
        case EWMA_SKETCH:
            detector_ = AnomalyDetectorUtil::create_approx_ewma(nhashes, 1 << bits, threshold, alpha);
            break;
        case DOUBLE_EXP_SMOOTHING:
            detector_ = AnomalyDetectorUtil::create_precise_double_exp_smoothing(threshold, alpha, gamma);
            break;
        case DOUBLE_EXP_SMOOTHING_SKETCH:
            detector_ = AnomalyDetectorUtil::create_approx_double_exp_smoothing(nhashes, 1 << bits, threshold, alpha, gamma);
            break;
        case HOLT_WINTERS:
            detector_ = AnomalyDetectorUtil::create_precise_holt_winters(threshold, alpha, beta, gamma, period);
            break;
        case HOLT_WINTERS_SKETCH:
            detector_ = AnomalyDetectorUtil::create_approx_holt_winters(nhashes, 1 << bits, threshold, alpha, beta, gamma, period);
            break;
        default:
            std::logic_error err("AnomalyDetector building error");  // invalid use of the constructor
            BOOST_THROW_EXCEPTION(err);
        }
    } catch (...) {
        // std::cout << boost::current_exception_diagnostic_information() << std::endl;
        throw;
    }
}

void AnomalyDetector::complete() {
    next_->complete();
}

bool AnomalyDetector::put(const aku_Sample &sample) {
    if (sample.payload.type == aku_PData::EMPTY) {
        detector_->move_sliding_window();
        return next_->put(sample);
    } else if (sample.payload.type & aku_PData::FLOAT_BIT) {
        detector_->add(sample.paramid, sample.payload.float64);
        if (detector_->is_anomaly_candidate(sample.paramid)) {
            aku_Sample anomaly = sample;
            anomaly.payload.type |= aku_PData::URGENT;
            return next_->put(anomaly);
        }
    }
    // Ignore BLOBs
    return true;
}

void AnomalyDetector::set_error(aku_Status status) {
    next_->set_error(status);
}

int AnomalyDetector::get_requirements() const {
    return TERMINAL|GROUP_BY_REQUIRED;
}

//! Register anomaly detector for use in queries
static QueryParserToken<AnomalyDetector> detector_token("anomaly-detector");

}}  // namespace


/* Copyright (c) 2018 vargaconsulting, Toronto,ON Canada
 * Author: Varga, Steven <steven@vargaconsulting.ca>
 */
#include <armadillo>
#include <cstdint>
#include "struct.h"
#include <h5cpp/core>
	// generated file must be sandwiched between core and io
	// to satisfy template dependencies in <h5cpp/io>
	#include "tu-01.h"
#include <h5cpp/io>
#include "utils.hpp"

#define CHUNK_SIZE 5
#define NROWS 4*CHUNK_SIZE
#define NCOLS 1*CHUNK_SIZE

void test_01( const h5::fd_t& fd ){// LINARG:=[armaidllo|eigen3|blaze|blitz|it++|dlib|ublas] supported

	arma::imat M(NROWS,NCOLS);              // define a linalg object
	h5::write(fd, "/linalg/armadillo",M);   // save it somewhere, partial and full read|write and append supported
}

void test_02( const h5::fd_t& fd ){// create a Matrix of STRUCT with chunked and GZIP compressed properties ready for partial read|write

	// upto 7 dimensions/extents are supported
	h5::create<sn::example::Record>(fd, "/orm/chunked_2D",
		h5::current_dims{NROWS,NCOLS}, h5::chunk{1,CHUNK_SIZE} | h5::gzip{8} );
	h5::create<sn::typecheck::Record>(fd, "/orm/typecheck",	h5::max_dims{H5S_UNLIMITED} );
}


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

#include <boost/hana/ext/std/tuple.hpp>

#include <boost/hana/bool.hpp>
#include <boost/hana/functional/always.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/applicative.hpp>
#include <laws/base.hpp>
#include <laws/comparable.hpp>
#include <laws/foldable.hpp>
#include <laws/functor.hpp>
#include <laws/iterable.hpp>
#include <laws/monad.hpp>
#include <laws/monad_plus.hpp>
#include <laws/orderable.hpp>
#include <laws/searchable.hpp>

#define BOOST_HANA_TEST_SEQUENCE_KNOWN_PARTS 3
#if BOOST_HANA_TEST_PART == 6
#   define BOOST_HANA_TEST_SEQUENCE_PART 1
#elif BOOST_HANA_TEST_PART == 7
#   define BOOST_HANA_TEST_SEQUENCE_PART 2
#elif BOOST_HANA_TEST_PART == 8
#   define BOOST_HANA_TEST_SEQUENCE_PART 3
#endif
#include <laws/sequence.hpp>

#include <tuple>
using namespace boost::hana;

template <int i>
using eq = test::ct_eq<i>;

using test::ct_ord;

int main() {
    auto big_eq_tuples = make<tuple_tag>(
          std::make_tuple()
        , std::make_tuple(eq<0>{})
        , std::make_tuple(eq<0>{}, eq<1>{})
        , std::make_tuple(eq<0>{}, eq<1>{}, eq<2>{})
        , std::make_tuple(eq<0>{}, eq<1>{}, eq<2>{}, eq<3>{}, eq<4>{})
    );
    (void)big_eq_tuples;

    auto small_eq_tuples = make<tuple_tag>(
          std::make_tuple()
        , std::make_tuple(eq<0>{})
        , std::make_tuple(eq<0>{}, eq<1>{})
        , std::make_tuple(eq<0>{}, eq<1>{}, eq<2>{})
    );
    (void)small_eq_tuples;

    auto ord_tuples = make<tuple_tag>(
          std::make_tuple()
        , std::make_tuple(ct_ord<0>{})
        , std::make_tuple(ct_ord<0>{}, ct_ord<1>{})
        , std::make_tuple(ct_ord<0>{}, ct_ord<1>{}, ct_ord<2>{})
        , std::make_tuple(ct_ord<0>{}, ct_ord<1>{}, ct_ord<2>{}, ct_ord<3>{}, ct_ord<4>{})
    );
    (void)ord_tuples;

    auto eq_values = make<tuple_tag>(eq<0>{}, eq<1>{}, eq<2>{});
    (void)eq_values;

    auto predicates = make<tuple_tag>(
        equal.to(eq<0>{}), equal.to(eq<1>{}), equal.to(eq<2>{}),
        always(false_c), always(true_c)
    );
    (void)predicates;

    auto nested_tuples = make<tuple_tag>(
          std::make_tuple()
        , std::make_tuple(
            std::make_tuple(eq<0>{}))
        , std::make_tuple(
            std::make_tuple(eq<0>{}),
            std::make_tuple(eq<1>{}, eq<2>{}))
        , std::make_tuple(
            std::make_tuple(eq<0>{}),
            std::make_tuple(eq<1>{}, eq<2>{}),
            std::make_tuple(eq<3>{}, eq<4>{}))
    );
    (void)nested_tuples;

#if BOOST_HANA_TEST_PART == 1
    //////////////////////////////////////////////////////////////////////////
    // Searchable
    //////////////////////////////////////////////////////////////////////////
    {
        auto eq_tuple_keys = make<tuple_tag>(eq<3>{}, eq<5>{}, eq<7>{});
        test::TestSearchable<ext::std::tuple_tag>{small_eq_tuples, eq_tuple_keys};

        auto bools = make<tuple_tag>(
              std::make_tuple(true_c)
            , std::make_tuple(false_c)
            , std::make_tuple(true_c, true_c)
            , std::make_tuple(true_c, false_c)
            , std::make_tuple(false_c, true_c)
            , std::make_tuple(false_c, false_c)
        );
        test::TestSearchable<ext::std::tuple_tag>{bools, make<tuple_tag>(true_c, false_c)};
    }

#elif BOOST_HANA_TEST_PART == 2
    //////////////////////////////////////////////////////////////////////////
    // Comparable and Orderable
    //////////////////////////////////////////////////////////////////////////
    test::TestComparable<ext::std::tuple_tag>{big_eq_tuples};
    test::TestOrderable<ext::std::tuple_tag>{ord_tuples};

#elif BOOST_HANA_TEST_PART == 3
    //////////////////////////////////////////////////////////////////////////
    // MonadPlus
    //////////////////////////////////////////////////////////////////////////
    test::TestMonadPlus<ext::std::tuple_tag>{small_eq_tuples, predicates, eq_values};

#elif BOOST_HANA_TEST_PART == 4
    //////////////////////////////////////////////////////////////////////////
    // Foldable, Iterable
    //////////////////////////////////////////////////////////////////////////
    test::TestFoldable<ext::std::tuple_tag>{big_eq_tuples};
    test::TestIterable<ext::std::tuple_tag>{big_eq_tuples};

#elif BOOST_HANA_TEST_PART == 5
    //////////////////////////////////////////////////////////////////////////
    // Functor up to Monad
    //////////////////////////////////////////////////////////////////////////
    {
        test::TestFunctor<ext::std::tuple_tag>{big_eq_tuples, eq_values};
        test::TestApplicative<ext::std::tuple_tag>{small_eq_tuples};
        test::TestMonad<ext::std::tuple_tag>{big_eq_tuples, nested_tuples};
    }

    //////////////////////////////////////////////////////////////////////////
    // Sequence
    //////////////////////////////////////////////////////////////////////////
#elif BOOST_HANA_TEST_PART == 6 || BOOST_HANA_TEST_PART == 7 || BOOST_HANA_TEST_PART == 8
    test::TestSequence<ext::std::tuple_tag>{};
#endif
}


// clang: MatousFormat
/**  \file
     \brief Example file for the convert() vector type conversion function
     \author Matouš Vrba - vrbamato@fel.cvut.cz

     This example may be run after building *mrs_lib* by executing `rosrun mrs_lib vector_converter_example`.

     See \ref vector_converter/example.cpp.
 */

/**  \example "vector_converter/example.cpp"

     This is an example of usage of the convert() function for conversion between different vector types.
     It may be run after building *mrs_lib* by executing `rosrun mrs_lib vector_converter_example`.
 */

#include <geometry_msgs/Vector3.h>
#include <Eigen/Dense>
#include <pcl/point_types.h>

/* Demonstration of usage for custom incompatible types. //{ */

// What if we want to convert from/to a different type, which doesn't provide either operator[], nor x(), y() and z() getter/setter methods, nor x, y and z-named members?
struct MyPoint
{
  double e1, e2, e3;
};

// We need to define our own specialization of the mrs_lib::impl::convertFrom() and mrs_lib::impl::convertTo() functions for this type. convert() will then work automatically.
namespace mrs_lib
{
  namespace impl
  {

    // convertFrom specialization for MyPoint (mrs_lib::impl::unw_t is just a shorthand for std::tuple<double, double, double>)
    std::tuple<double, double, double> convertFrom(const MyPoint& in)
    {
      return {in.e1, in.e2, in.e3};
    }

    // convertTo specialization for MyPoint
    void convertTo(MyPoint& ret, const double x, const double y, const double z)
    {
      ret.e1 = x;
      ret.e2 = y;
      ret.e3 = z;
    }

  }
}

//}

// Include the vector_converter.h header (this has to be included *after* defining functions for your custom types)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wnarrowing"
#include <mrs_lib/vector_converter.h>
#pragma GCC diagnostic pop

int main()
{
  // prepare a source variable `from` with the type, which we'll want to convert
  const double xo = rand(), yo = rand(), zo = rand();
  // the mrs_lib::impl::convertTo() function is used as a shortcut for initialization of the object
  // Note, then when converting to a single precision floating type,
  // the compiler will emit a narrowing conversion waring. This can be disabled using the pragma statements
  // around the line `#include <mrs_lib/vector_converter.h>` (see above).
  const auto from = mrs_lib::impl::convertTo<pcl::PointXYZ>(xo, yo, zo);

  // This is the main functionality - one-liner for converting between two vector types.
  const auto to = mrs_lib::convert<Eigen::Vector3d>(from);
  const auto to2 = mrs_lib::convert<geometry_msgs::Vector3>(to);

  std::cout << "Converted from type " << typeid(decltype(from)).name() << " with value:" << std::endl << from << std::endl;
  std::cout << "            to type " << typeid(decltype(to)).name() << " with value:" << std::endl << to << std::endl << std::endl;
  std::cout << "   and then to type " << typeid(decltype(to2)).name() << " with value:" << std::endl << to2 << std::endl;

  // Even for custom types with compatible API, the convert() function will work (thanks to templates)
  struct MyPointOK
  {
    double x, y, z;
  };
  const auto custom1 = mrs_lib::convert<MyPointOK>(from); // works
  std::cout << "    and to new type " << typeid(decltype(custom1)).name() << " with value:" << std::endl << custom1.x << std::endl << custom1.y << std::endl << custom1.z << std::endl << std::endl;

  // However, for custom types with incompatible API, the convertFrom() and convertTo() functions have to be manually implemented for convert() to work (take a look before main()):
  const auto custom2 = mrs_lib::convert<MyPoint>(from); // now also works
  std::cout << "    and to new type " << typeid(decltype(custom2)).name() << " with value:" << std::endl << custom2.e1 << std::endl << custom2.e2 << std::endl << custom2.e3 << std::endl << std::endl;
}


/* Copyright Institue of Sound and Vibration Research - All rights reserved. */

#include <librbbl/index_sequence.hpp>

#include <boost/test/unit_test.hpp>

#include <ciso646>
#include <algorithm>
#include <iostream>
#include <iterator>
#include <stdexcept>
#include <string>
#include <vector>

namespace visr
{
namespace rbbl
{
namespace test
{

BOOST_AUTO_TEST_CASE( initIndexSequenceDefault )
{
  IndexSequence il1;
  BOOST_CHECK( il1.size() == 0 );
}

BOOST_AUTO_TEST_CASE( initIndexSequenceFromInitializerList )
{
  IndexSequence il1( { 0, 1, 3, 7, 2 } );
  BOOST_CHECK( il1.size( ) == 5 );

  BOOST_CHECK_NO_THROW( il1.at( 4 ) );

  BOOST_CHECK( il1[1] == 1 );
}

BOOST_AUTO_TEST_CASE( initIndexSequenceFromStringSimple )
{
  std::string const initString( "0,1,3,7,2" );
  IndexSequence il1( initString );
  BOOST_CHECK( il1.size( ) == 5 );

  BOOST_CHECK_NO_THROW( il1.at( 4 ) );

  BOOST_CHECK( il1[1] == 1 );
}

BOOST_AUTO_TEST_CASE( initIndexSequenceFromStringRange1 )
{
  std::string const initString( "0:3,7" );
  IndexSequence il1( initString );
  BOOST_CHECK( il1.size( ) == 5 );

  BOOST_CHECK_NO_THROW( il1.at( 4 ) );

  BOOST_CHECK( il1[1] == 1 );
}

BOOST_AUTO_TEST_CASE( initIndexSequenceFromStringRangeStride )
{
  std::string const initString( "0:2:7,2" );
  IndexSequence il1( initString );
  BOOST_CHECK( il1.size( ) == 5 );

  BOOST_CHECK_NO_THROW( il1.at( 4 ) );

  BOOST_CHECK( il1[3] == 6 );
}

BOOST_AUTO_TEST_CASE( initIndexSequenceFromStringRangeStrideEmpty )
{
  std::string const initString( "5:3:4" );
  IndexSequence il1( initString );
  BOOST_CHECK( il1.size( ) == 0 );
}

BOOST_AUTO_TEST_CASE( initIndexSequenceFromStringRangeStrideReversed )
{
  std::string const initString( "2,7:-2:0" );
  IndexSequence il1( initString );
  BOOST_CHECK( il1.size( ) == 5 );

  BOOST_CHECK_NO_THROW( il1.at( 4 ) );

  BOOST_CHECK( il1[2] == 5 );
}

BOOST_AUTO_TEST_CASE( initIndexSequenceFromStringWeirdWhiteSpace )
{
  std::string const initString( "    7 :    -2    : 0   ,   2         " );
  IndexSequence il1( initString );
  BOOST_CHECK( il1.size( ) == 5 );

  BOOST_CHECK_NO_THROW( il1.at( 4 ) );

  BOOST_CHECK( il1[1] == 5 );
}

} // namespace test
} // namespace rbbl
} // namespace visr


// MIT License

// Copyright (c) 2020 Joseph Auckley, Matthew O'Kelly, Aman Sinha, Hongrui Zheng

// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include "pose_2d.hpp"
#include "ackermann_kinematics.hpp"
#include "scan_simulator_2d.hpp"

#include "car_state.hpp"
#include "car_params.hpp"
#include "ks_kinematics.hpp"
#include "st_kinematics.hpp"

#include "racecar.hpp"

#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <iostream>
#include <vector>
#include <chrono>
#include <math.h>

using namespace racecar_simulator;

class StandaloneSimulator {
public:
    bool map_exists;
	StandaloneSimulator(int num_cars, double timestep, double mu, double h_cg, double l_r, double cs_f, double cs_r, double I_z, double mass);
	virtual ~StandaloneSimulator();
	void set_map(std::vector<double> map, int map_height, int map_width, double map_resolution, double origin_x, double origin_y, double free_threshold);
	std::vector<CarObs> step(std::vector<double> velocities, std::vector<double> steering_angles);
    void update_params(double mu, double h_cg, double l_r, double cs_f, double cs_r, double I_z, double mass);
	bool get_map_status();
    bool check_collision();
	void reset();
    void reset_bypose(std::vector<Pose2D> &poses);

private:
	// sim constants
    // time step
    double delt_t;

    // multi-agents
    int ego_agent_idx = 0;
    double safety_radius = 1.0;
    int num_agents;
    std::vector<RaceCar> agents;
    std::vector<Pose2D> agent_poses;
    std::vector<CarObs> current_obs;

    // car params - CHECK racecar.cpp BEFORE CHANGING
    double car_width = 0.31;
    double car_length = 0.58;
    double scan_distance_to_base_link = 0.275;
    double wheel_base = 0.3302;

    // all agents have the same map
    std::vector<double> map;
    int map_height, map_width;
    double map_resolution, origin_x, origin_y, free_threshold;
    Eigen::Matrix4d get_transformation_matrix(const Pose2D &pose);
};


/****************************************************************************
 * Copyright (c) 2017-2021 by the ArborX authors                            *
 * All rights reserved.                                                     *
 *                                                                          *
 * This file is part of the ArborX library. ArborX is                       *
 * distributed under a BSD 3-clause license. For the licensing terms see    *
 * the LICENSE file in the top-level directory.                             *
 *                                                                          *
 * SPDX-License-Identifier: BSD-3-Clause                                    *
 ****************************************************************************/

#include "ArborX_BoostRTreeHelpers.hpp"
#include "ArborX_EnableDeviceTypes.hpp" // ARBORX_DEVICE_TYPES
#include <ArborX_DistributedTree.hpp>

#include <boost/test/unit_test.hpp>

#include <algorithm>
#include <iostream>
#include <random>
#include <tuple>

#include "Search_UnitTestHelpers.hpp"
#include <mpi.h>

#define BOOST_TEST_MODULE DistributedTree

namespace tt = boost::test_tools;

using PairIndexRank = Kokkos::pair<int, int>;
using TupleIndexRankDistance = Kokkos::pair<Kokkos::pair<int, int>, float>;

BOOST_AUTO_TEST_CASE_TEMPLATE(hello_world, DeviceType, ARBORX_DEVICE_TYPES)
{
  using Tree = ArborX::DistributedTree<typename DeviceType::memory_space>;
  using ExecutionSpace = typename DeviceType::execution_space;

  MPI_Comm comm = MPI_COMM_WORLD;
  int comm_rank;
  MPI_Comm_rank(comm, &comm_rank);
  int comm_size;
  MPI_Comm_size(comm, &comm_size);

  int const n = 4;
  Kokkos::View<ArborX::Point *, DeviceType> points("Testing::points", n);
  // [  rank 0       [  rank 1       [  rank 2       [  rank 3       [
  // x---x---x---x---x---x---x---x---x---x---x---x---x---x---x---x---
  // ^   ^   ^   ^
  // 0   1   2   3   ^   ^   ^   ^
  //                 0   1   2   3   ^   ^   ^   ^
  //                                 0   1   2   3   ^   ^   ^   ^
  //                                                 0   1   2   3
  Kokkos::parallel_for(Kokkos::RangePolicy<ExecutionSpace>(0, n),
                       KOKKOS_LAMBDA(int i) {
                         points(i) = {{(double)i / n + comm_rank, 0., 0.}};
                       });

  Tree tree(comm, ExecutionSpace{}, points);

  // 0---0---0---0---1---1---1---1---2---2---2---2---3---3---3---3---
  // |               |               |               |               |
  // |               |               |               x   x   x   x   |
  // |               |               |               |<------0------>|
  // |               |               x   x   x   x   x               |
  // |               |               |<------1------>|               |
  // |               x   x   x   x   x               |               |
  // |               |<------2------>|               |               |
  // x   x   x   x   x               |               |               |
  // |<------3------>|               |               |               |
  // |               |               |               |               |
  Kokkos::View<decltype(ArborX::intersects(ArborX::Sphere{})) *, DeviceType>
      queries("Testing::queries", 1);
  auto queries_host = Kokkos::create_mirror_view(queries);
  queries_host(0) = ArborX::intersects(
      ArborX::Sphere{{{0.5 + comm_size - 1 - comm_rank, 0., 0.}}, 0.5});
  deep_copy(queries, queries_host);

  // 0---0---0---0---1---1---1---1---2---2---2---2---3---3---3---3---
  // |               |               |               |               |
  // |               |               |           x   x   x           |
  // |               |           x   x   x        <--0-->            |
  // |           x   x   x        <--1-->            |               |
  // x   x        <--2-->            |               |               |
  // 3-->            |               |               |               |
  // |               |               |               |               |
  Kokkos::View<ArborX::Nearest<ArborX::Point> *, DeviceType> nearest_queries(
      "Testing::nearest_queries", 1);
  auto nearest_queries_host = Kokkos::create_mirror_view(nearest_queries);
  nearest_queries_host(0) = ArborX::nearest<ArborX::Point>(
      {{0.0 + comm_size - 1 - comm_rank, 0., 0.}},
      comm_rank < comm_size - 1 ? 3 : 2);
  deep_copy(nearest_queries, nearest_queries_host);

  std::vector<PairIndexRank> values;
  values.reserve(n + 1);
  for (int i = 0; i < n; ++i)
  {
    values.emplace_back(n - 1 - i, comm_size - 1 - comm_rank);
  }
  if (comm_rank > 0)
  {
    values.emplace_back(0, comm_size - comm_rank);
    ARBORX_TEST_QUERY_TREE(ExecutionSpace{}, tree, queries,
                           make_reference_solution(values, {0, n + 1}));
  }
  else
  {
    ARBORX_TEST_QUERY_TREE(ExecutionSpace{}, tree, queries,
                           make_reference_solution(values, {0, n}));
  }

  BOOST_TEST(n > 2);
  if (comm_rank < comm_size - 1)
  {
    ARBORX_TEST_QUERY_TREE(ExecutionSpace{}, tree, nearest_queries,
                           make_reference_solution<PairIndexRank>(
                               {{0, comm_size - 1 - comm_rank},
                                {n - 1, comm_size - 2 - comm_rank},
                                {1, comm_size - 1 - comm_rank}},
                               {0, 3}));
  }
  else
  {
    ARBORX_TEST_QUERY_TREE(
        ExecutionSpace{}, tree, nearest_queries,
        make_reference_solution<PairIndexRank>(
            {{0, comm_size - 1 - comm_rank}, {1, comm_size - 1 - comm_rank}},
            {0, 2}));
  }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(empty_tree, DeviceType, ARBORX_DEVICE_TYPES)
{
  using ExecutionSpace = typename DeviceType::execution_space;

  MPI_Comm comm = MPI_COMM_WORLD;
  int comm_rank;
  MPI_Comm_rank(comm, &comm_rank);
  int comm_size;
  MPI_Comm_size(comm, &comm_size);

  auto const tree = makeDistributedTree<DeviceType>(comm, {});

  BOOST_TEST(tree.empty());
  BOOST_TEST(tree.size() == 0);

  BOOST_TEST(ArborX::Details::equals(tree.bounds(), {}));

  ARBORX_TEST_QUERY_TREE(ExecutionSpace{}, tree,
                         makeIntersectsBoxQueries<DeviceType>({}),
                         make_reference_solution<PairIndexRank>({}, {0}));

  ARBORX_TEST_QUERY_TREE(ExecutionSpace{}, tree,
                         makeIntersectsSphereQueries<DeviceType>({}),
                         make_reference_solution<PairIndexRank>({}, {0}));

  ARBORX_TEST_QUERY_TREE(ExecutionSpace{}, tree,
                         makeNearestQueries<DeviceType>({}),
                         make_reference_solution<PairIndexRank>({}, {0}));

  ARBORX_TEST_QUERY_TREE_WITH_DISTANCE(
      ExecutionSpace{}, tree, makeNearestQueries<DeviceType>({}),
      make_reference_solution<TupleIndexRankDistance>({}, {0}));

  // Only rank 0 has a couple spatial queries with a spatial predicate
  if (comm_rank == 0)
  {
    ARBORX_TEST_QUERY_TREE(
        ExecutionSpace{}, tree, makeIntersectsBoxQueries<DeviceType>({{}, {}}),
        make_reference_solution<PairIndexRank>({}, {0, 0, 0}));
  }
  else
  {
    ARBORX_TEST_QUERY_TREE(ExecutionSpace{}, tree,
                           makeIntersectsBoxQueries<DeviceType>({}),
                           make_reference_solution<PairIndexRank>({}, {0}));
  }

  // All ranks but rank 0 have a single query with a spatial predicate
  if (comm_rank == 0)
  {
    ARBORX_TEST_QUERY_TREE(ExecutionSpace{}, tree,
                           makeIntersectsSphereQueries<DeviceType>({}),
                           make_reference_solution<PairIndexRank>({}, {0}));
  }
  else
  {
    ARBORX_TEST_QUERY_TREE(
        ExecutionSpace{}, tree,
        makeIntersectsSphereQueries<DeviceType>({
            {{{(double)comm_rank, 0., 0.}}, (double)comm_size},
        }),
        make_reference_solution<PairIndexRank>({}, {0, 0}));
  }

  // All ranks but rank 0 have a single query with a nearest predicate
  if (comm_rank == 0)
  {
    ARBORX_TEST_QUERY_TREE(ExecutionSpace{}, tree,
                           makeNearestQueries<DeviceType>({}),
                           make_reference_solution<PairIndexRank>({}, {0}));
  }
  else
  {
    ARBORX_TEST_QUERY_TREE(ExecutionSpace{}, tree,
                           makeNearestQueries<DeviceType>({
                               {{{0., 0., 0.}}, comm_rank},
                           }),
                           make_reference_solution<PairIndexRank>({}, {0, 0}));
  }

  // All ranks have a single query with a nearest predicate (this version
  // returns distances as well)
  ARBORX_TEST_QUERY_TREE_WITH_DISTANCE(
      ExecutionSpace{}, tree,
      makeNearestQueries<DeviceType>({
          {{{0., 0., 0.}}, comm_size},
      }),
      make_reference_solution<TupleIndexRankDistance>({}, {0, 0}));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(unique_leaf_on_rank_0, DeviceType,
                              ARBORX_DEVICE_TYPES)
{
  using ExecutionSpace = typename DeviceType::execution_space;

  MPI_Comm comm = MPI_COMM_WORLD;
  int comm_rank;
  MPI_Comm_rank(comm, &comm_rank);
  int comm_size;
  MPI_Comm_size(comm, &comm_size);

  // tree has one unique leaf that lives on rank 0
  auto const tree =
      (comm_rank == 0 ? makeDistributedTree<DeviceType>(
                            comm,
                            {
                                {{{0., 0., 0.}}, {{1., 1., 1.}}},
                            })
                      : makeDistributedTree<DeviceType>(comm, {}));

  BOOST_TEST(!tree.empty());
  BOOST_TEST(tree.size() == 1);

  BOOST_TEST(
      ArborX::Details::equals(tree.bounds(), {{{0., 0., 0.}}, {{1., 1., 1.}}}));

  ARBORX_TEST_QUERY_TREE(ExecutionSpace{}, tree,
                         makeIntersectsBoxQueries<DeviceType>({}),
                         make_reference_solution<PairIndexRank>({}, {0}));

  ARBORX_TEST_QUERY_TREE(ExecutionSpace{}, tree,
                         makeIntersectsSphereQueries<DeviceType>({}),
                         make_reference_solution<PairIndexRank>({}, {0}));

  ARBORX_TEST_QUERY_TREE(ExecutionSpace{}, tree,
                         makeNearestQueries<DeviceType>({}),
                         make_reference_solution<PairIndexRank>({}, {0}));

  ARBORX_TEST_QUERY_TREE_WITH_DISTANCE(
      ExecutionSpace{}, tree, makeNearestQueries<DeviceType>({}),
      make_reference_solution<TupleIndexRankDistance>({}, {0}));

  // Querying for more neighbors than there are leaves in the tree
  ARBORX_TEST_QUERY_TREE(
      ExecutionSpace{}, tree,
      makeNearestQueries<DeviceType>({
          {{{(double)comm_rank, (double)comm_rank, (double)comm_rank}},
           comm_size},
      }),
      make_reference_solution<PairIndexRank>({{0, 0}}, {0, 1}));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(one_leaf_per_rank, DeviceType,
                              ARBORX_DEVICE_TYPES)
{
  using ExecutionSpace = typename DeviceType::execution_space;

  MPI_Comm comm = MPI_COMM_WORLD;
  int comm_rank;
  MPI_Comm_rank(comm, &comm_rank);
  int comm_size;
  MPI_Comm_size(comm, &comm_size);

  // tree has one leaf per rank
  auto const tree = makeDistributedTree<DeviceType>(
      comm,
      {
          {{{(double)comm_rank, 0., 0.}}, {{(double)comm_rank + 1., 1., 1.}}},
      });

  BOOST_TEST(!tree.empty());
  BOOST_TEST((int)tree.size() == comm_size);

  BOOST_TEST(ArborX::Details::equals(
      tree.bounds(), {{{0., 0., 0.}}, {{(double)comm_size, 1., 1.}}}));

  ARBORX_TEST_QUERY_TREE(
      ExecutionSpace{}, tree,
      makeIntersectsBoxQueries<DeviceType>({
          {{{(double)comm_size - (double)comm_rank - .5, .5, .5}},
           {{(double)comm_size - (double)comm_rank - .5, .5, .5}}},
          {{{(double)comm_rank + .5, .5, .5}},
           {{(double)comm_rank + .5, .5, .5}}},
      }),
      make_reference_solution<PairIndexRank>(
          {{0, comm_size - 1 - comm_rank}, {0, comm_rank}}, {0, 1, 2}));

  ARBORX_TEST_QUERY_TREE(ExecutionSpace{}, tree,
                         makeNearestQueries<DeviceType>({}),
                         make_reference_solution<PairIndexRank>({}, {0}));

  ARBORX_TEST_QUERY_TREE(ExecutionSpace{}, tree,
                         makeIntersectsBoxQueries<DeviceType>({}),
                         make_reference_solution<PairIndexRank>({}, {0}));

  if (comm_rank > 0)
  {
    ARBORX_TEST_QUERY_TREE(ExecutionSpace{}, tree,
                           makeNearestQueries<DeviceType>({
                               {{{0., 0., 0.}}, comm_rank * comm_size},
                           }),
                           make_reference_solution(
                               [comm_size]() {
                                 std::vector<PairIndexRank> values;
                                 values.reserve(comm_size);
                                 for (int i = 0; i < comm_size; ++i)
                                   values.emplace_back(0, i);
                                 return values;
                               }(),
                               {0, comm_size}));
  }
  else
  {
    ARBORX_TEST_QUERY_TREE(ExecutionSpace{}, tree,
                           makeNearestQueries<DeviceType>({
                               {{{0., 0., 0.}}, comm_rank * comm_size},
                           }),
                           make_reference_solution<PairIndexRank>({}, {0, 0}));
  }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(do_not_exceed_capacity, DeviceType,
                              ARBORX_DEVICE_TYPES)
{
  using ExecutionSpace = typename DeviceType::execution_space;

  // This unit tests exposes bug that essentially assumed the number of
  // neighbors queried for would not exceed the maximum size of the default
  // underlying container for a priority queue which was 256.
  // https://github.com/arborx/ArborX/pull/126#issuecomment-538410096
  // Each rank has the exact same cloud and perform a knn query that will push
  // comm_size * 512 elements into the queue.
  using ArborX::Box;
  using ArborX::nearest;
  using ArborX::Point;
  using ExecutionSpace = typename DeviceType::execution_space;
  MPI_Comm comm = MPI_COMM_WORLD;
  Kokkos::View<Point *, DeviceType> points("Testing::points", 512);
  Kokkos::parallel_for(Kokkos::RangePolicy<ExecutionSpace>(0, 512),
                       KOKKOS_LAMBDA(int i) {
                         points(i) = {{(float)i, (float)i, (float)i}};
                       });

  ArborX::DistributedTree<typename DeviceType::memory_space> tree{
      comm, ExecutionSpace{}, points};
  Kokkos::View<decltype(nearest(Point{})) *, DeviceType> queries(
      "Testing::queries", 1);
  Kokkos::deep_copy(queries, nearest(Point{0, 0, 0}, 512));
  Kokkos::View<PairIndexRank *, DeviceType> values("Testing::values", 0);
  Kokkos::View<int *, DeviceType> offset("Testing::offset", 0);
  BOOST_CHECK_NO_THROW(tree.query(ExecutionSpace{}, queries, values, offset));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(non_approximate_nearest_neighbors, DeviceType,
                              ARBORX_DEVICE_TYPES)
{
  using ExecutionSpace = typename DeviceType::execution_space;

  MPI_Comm comm = MPI_COMM_WORLD;
  int comm_rank;
  MPI_Comm_rank(comm, &comm_rank);
  int comm_size;
  MPI_Comm_size(comm, &comm_size);

  //  +----------0----------1----------2----------3
  //  |          |          |          |          |
  //  |          |          |          |          |
  //  |          |          |          |          |
  //  |          |          |          |          |
  //  0----------1----------2----------3----------+
  //  [  rank 0  ]
  //             [  rank 1  ]
  //                        [  rank 2  ]
  //                                   [  rank 3  ]
  auto const tree = makeDistributedTree<DeviceType>(
      comm, {
                {{{(double)comm_rank, 0., 0.}}, {{(double)comm_rank, 0., 0.}}},
                {{{(double)comm_rank + 1., 1., 1.}},
                 {{(double)comm_rank + 1., 1., 1.}}},
            });

  BOOST_TEST(!tree.empty());
  BOOST_TEST((int)tree.size() == 2 * comm_size);

  //  +----------0----------1----------2----------3
  //  |          |          |          |          |
  //  |          |          |          |          |
  //  |          |          |          |          |
  //  |          |          |          |          |
  //  0-------x--1-------X--2-------X--3-------X--+
  //          ^          ^          ^          ^
  //          3          2          1          0
  if (comm_rank > 0)
  {
    ARBORX_TEST_QUERY_TREE(
        ExecutionSpace{}, tree,
        makeNearestQueries<DeviceType>({
            {{{(double)(comm_size - 1 - comm_rank) + .75, 0., 0.}}, 1},
        }),
        make_reference_solution<PairIndexRank>({{0, comm_size - comm_rank}},
                                               {0, 1}));
  }
  else
  {
    ARBORX_TEST_QUERY_TREE(
        ExecutionSpace{}, tree,
        makeNearestQueries<DeviceType>({
            {{{(double)(comm_size - 1 - comm_rank) + .75, 0., 0.}}, 1},
        }),
        make_reference_solution<PairIndexRank>({{0, comm_size - 1}}, {0, 1}));
  }
}

template <typename DeviceType>
struct CustomInlineCallbackWithAttachment
{
  using tag = ArborX::Details::InlineCallbackTag;
  Kokkos::View<ArborX::Point *, DeviceType> points;
  ArborX::Point const origin = {{0., 0., 0.}};
  template <typename Query, typename Insert>
  KOKKOS_FUNCTION void operator()(Query const &query, int index,
                                  Insert const &insert) const
  {
    auto data = ArborX::getData(query);
    float const distance_to_origin =
        ArborX::Details::distance(points(index), origin);

    insert(distance_to_origin + data);
  }
};

template <typename DeviceType>
struct CustomPostCallbackWithAttachment
{
  using tag = ArborX::Details::PostCallbackTag;
  Kokkos::View<ArborX::Point *, DeviceType> points;
  ArborX::Point const origin = {{0., 0., 0.}};
  template <typename Predicates, typename InOutView, typename InView,
            typename OutView>
  void operator()(Predicates const &queries, InOutView &offset, InView in,
                  OutView &out) const
  {
    using ExecutionSpace = typename DeviceType::execution_space;
    using ArborX::Details::distance;
    auto const n = offset.extent(0) - 1;
    ArborX::reallocWithoutInitializing(out, in.extent(0));
    // NOTE workaround to avoid implicit capture of *this
    auto const &points_ = points;
    auto const &origin_ = origin;
    Kokkos::parallel_for(
        Kokkos::RangePolicy<ExecutionSpace>(0, n), KOKKOS_LAMBDA(int i) {
          auto data = ArborX::getData(queries(i));
          for (int j = offset(i); j < offset(i + 1); ++j)
          {
            out(j) = (float)distance(points_(in(j)), origin_) + data;
          }
        });
  }
};
BOOST_AUTO_TEST_CASE_TEMPLATE(callback_with_attachment, DeviceType,
                              ARBORX_DEVICE_TYPES)
{
  using ExecutionSpace = typename DeviceType::execution_space;

  MPI_Comm comm = MPI_COMM_WORLD;
  int comm_rank;
  MPI_Comm_rank(comm, &comm_rank);
  int comm_size;
  MPI_Comm_size(comm, &comm_size);

  //  +----------0----------1----------2----------3
  //  |          |          |          |          |
  //  |          |          |          |          |
  //  |          |          |          |          |
  //  |          |          |          |          |
  //  0----------1----------2----------3----------+
  //  [  rank 0  ]
  //             [  rank 1  ]
  //                        [  rank 2  ]
  //                                   [  rank 3  ]
  auto const tree = makeDistributedTree<DeviceType>(
      comm,
      {{{{(double)comm_rank, 0., 0.}}, {{(double)comm_rank + 1, 1., 1.}}}});

  //  +--------0---------1----------2---------3
  //  |        |         |          |         |
  //  |        |         |          |         |
  //  |        |         |          |         |
  //  |        |         |          |         |
  //  0--------1----x----2-----x----3----x----+
  //                ^          ^         ^
  //                0          1         2

  int const n_queries = 1;
  using ExecutionSpace = typename DeviceType::execution_space;
  Kokkos::View<ArborX::Point *, DeviceType> points("Testing::points",
                                                   n_queries);
  Kokkos::parallel_for(Kokkos::RangePolicy<ExecutionSpace>(0, n_queries),
                       KOKKOS_LAMBDA(int i) {
                         points(i) = {(float)(comm_rank) + 1.5f, 0.f, 0.f};
                       });
  auto points_host =
      Kokkos::create_mirror_view_and_copy(Kokkos::HostSpace{}, points);

  // This is a bit tricky. One would assume that it should be
  // distance(points(i), origin) + comm_rank, matching code inside the
  // callback. However, the callbacks are initialized and called on the rank
  // that produces results, not on the rank that sets up the queries. In this
  // example, for point 0 the callback will be called on rank 1, which is
  // initialized with point 1. So, within the callback, points(0) corresponds
  // to point 1 physically, and not point 0, even though the query() call is
  // called on rank 0.
  int const n_results = (comm_rank < comm_size - 1) ? 1 : 0;
  ArborX::Point const origin = {{0., 0., 0.}};
  Kokkos::View<float *, DeviceType> ref("Testing::ref", n_results);
  Kokkos::parallel_for(
      Kokkos::RangePolicy<ExecutionSpace>(0, n_results), KOKKOS_LAMBDA(int i) {
        ref(i) =
            float(ArborX::Details::distance(points(i), origin) + 1) + comm_rank;
      });

  {
    Kokkos::View<float *, DeviceType> custom("Testing::custom", 0);
    Kokkos::View<int *, DeviceType> offset("Testing::offset", 0);
    tree.query(ExecutionSpace{},
               makeIntersectsBoxWithAttachmentQueries<DeviceType, int>(
                   {{points_host(0), points_host(0)}}, {comm_rank}),
               CustomInlineCallbackWithAttachment<DeviceType>{points}, custom,
               offset);

    auto custom_host =
        Kokkos::create_mirror_view_and_copy(Kokkos::HostSpace{}, custom);
    auto ref_host =
        Kokkos::create_mirror_view_and_copy(Kokkos::HostSpace{}, ref);
    auto offset_host =
        Kokkos::create_mirror_view_and_copy(Kokkos::HostSpace{}, offset);
    BOOST_TEST(make_compressed_storage(offset_host, custom_host) ==
                   make_compressed_storage(offset_host, ref_host),
               tt::per_element());
  }
  {
    Kokkos::View<float *, DeviceType> custom("Testing::custom", 0);
    Kokkos::View<int *, DeviceType> offset("Testing::offset", 0);
    tree.query(ExecutionSpace{},
               makeIntersectsBoxWithAttachmentQueries<DeviceType, int>(
                   {{points_host(0), points_host(0)}}, {comm_rank}),
               CustomPostCallbackWithAttachment<DeviceType>{points}, custom,
               offset);

    auto custom_host =
        Kokkos::create_mirror_view_and_copy(Kokkos::HostSpace{}, custom);
    auto ref_host =
        Kokkos::create_mirror_view_and_copy(Kokkos::HostSpace{}, ref);
    auto offset_host =
        Kokkos::create_mirror_view_and_copy(Kokkos::HostSpace{}, offset);
    BOOST_TEST(make_compressed_storage(offset_host, custom_host) ==
                   make_compressed_storage(offset_host, ref_host),
               tt::per_element());
  }
}

std::vector<std::array<double, 3>>
make_random_cloud(double const Lx, double const Ly, double const Lz,
                  int const n, double const seed)
{
  std::vector<std::array<double, 3>> cloud(n);
  std::default_random_engine generator(seed);
  std::uniform_real_distribution<double> distribution_x(0.0, Lx);
  std::uniform_real_distribution<double> distribution_y(0.0, Ly);
  std::uniform_real_distribution<double> distribution_z(0.0, Lz);
  for (int i = 0; i < n; ++i)
  {
    double x = distribution_x(generator);
    double y = distribution_y(generator);
    double z = distribution_z(generator);
    cloud[i] = {{x, y, z}};
  }
  return cloud;
}

BOOST_AUTO_TEST_CASE_TEMPLATE(boost_comparison, DeviceType, ARBORX_DEVICE_TYPES)
{
  using ExecutionSpace = typename DeviceType::execution_space;

  MPI_Comm comm = MPI_COMM_WORLD;
  int comm_rank;
  MPI_Comm_rank(comm, &comm_rank);
  int comm_size;
  MPI_Comm_size(comm, &comm_size);

  // Construct a random cloud of point. We use the same seed on all the
  // processors.
  double const Lx = 10.0;
  double const Ly = 10.0;
  double const Lz = 10.0;
  int const n = 100;
  auto cloud = make_random_cloud(Lx, Ly, Lz, n, 0);
  auto queries = make_random_cloud(Lx, Ly, Lz, n, 1234);

  // The formula is a bit complicated but it does not require n be divisible
  // by comm_size
  int const local_n = (n + comm_size - 1 - comm_rank) / comm_size;
  Kokkos::View<ArborX::Box *, DeviceType> bounding_boxes(
      "Testing::bounding_boxes", local_n);
  auto bounding_boxes_host = Kokkos::create_mirror_view(bounding_boxes);
  for (int i = 0; i < n; ++i)
  {
    if (i % comm_size == comm_rank)
    {
      auto const &point = cloud[i];
      double const x = std::get<0>(point);
      double const y = std::get<1>(point);
      double const z = std::get<2>(point);
      bounding_boxes_host[i / comm_size] = {{{x, y, z}}, {{x, y, z}}};
    }
  }
  Kokkos::deep_copy(bounding_boxes, bounding_boxes_host);

  // Initialize the distributed search tree
  ArborX::DistributedTree<typename DeviceType::memory_space> distributed_tree(
      comm, ExecutionSpace{}, bounding_boxes);

  // make queries
  Kokkos::View<double * [3], ExecutionSpace> point_coords(
      "Testing::point_coords", local_n);
  auto point_coords_host = Kokkos::create_mirror_view(point_coords);
  Kokkos::View<double *, ExecutionSpace> radii("Testing::radii", local_n);
  auto radii_host = Kokkos::create_mirror_view(radii);
  Kokkos::View<int * [2], ExecutionSpace> within_n_pts("Testing::within_n_pts",
                                                       local_n);
  std::default_random_engine generator(0);
  std::uniform_real_distribution<double> distribution_radius(
      0.0, std::sqrt(Lx * Lx + Ly * Ly + Lz * Lz));
  std::uniform_int_distribution<int> distribution_k(1, std::floor(sqrt(n * n)));
  for (int i = 0; i < n; ++i)
  {
    if (i % comm_size == comm_rank)
    {
      auto const &point = queries[i];
      int const j = i / comm_size;
      double const x = std::get<0>(point);
      double const y = std::get<1>(point);
      double const z = std::get<2>(point);
      radii_host(j) = distribution_radius(generator);

      point_coords_host(j, 0) = x;
      point_coords_host(j, 1) = y;
      point_coords_host(j, 2) = z;
    }
  }
  Kokkos::deep_copy(point_coords, point_coords_host);
  Kokkos::deep_copy(radii, radii_host);

  Kokkos::View<decltype(ArborX::intersects(ArborX::Sphere{})) *, DeviceType>
      within_queries("Testing::within_queries", local_n);
  Kokkos::parallel_for(
      "register_within_queries",
      Kokkos::RangePolicy<ExecutionSpace>(0, local_n), KOKKOS_LAMBDA(int i) {
        within_queries(i) = ArborX::intersects(ArborX::Sphere{
            {{point_coords(i, 0), point_coords(i, 1), point_coords(i, 2)}},
            radii(i)});
      });

  auto within_queries_host = Kokkos::create_mirror_view(within_queries);
  Kokkos::deep_copy(within_queries_host, within_queries);

  BoostExt::ParallelRTree<ArborX::Box> rtree(comm, ExecutionSpace{},
                                             bounding_boxes_host);

  ARBORX_TEST_QUERY_TREE(ExecutionSpace{}, distributed_tree, within_queries,
                         query(ExecutionSpace{}, rtree, within_queries_host));
}


//
//  DTUtil_IO.h
//  Classifer_RF
//
//  Created by jimmy on 2016-10-04.
//  Copyright (c) 2016 Nowhere Planet. All rights reserved.
//

#ifndef __Classifer_RF__DTUtil_IO__
#define __Classifer_RF__DTUtil_IO__

#include <stdio.h>
#include <vector>
#include <Eigen/Dense>
#include <string>
#include <fstream>

using std::vector;
using std::string;
using Eigen::VectorXd;
using Eigen::VectorXf;
using Eigen::MatrixXd;
using Eigen::MatrixXf;
using Eigen::MatrixXi;

class DTUtil_IO
{
public:
    static bool read_matrix(const char * file_name, vector<VectorXd> & data);
    static bool read_matrix(const char * file_name, vector<Eigen::VectorXf>& data);
    static bool read_matrix(const char * file_name, Eigen::MatrixXd & data);
    static bool save_matrix(const char * file_name, const vector<VectorXd> & data);
    static bool read_labels(const char * file_name, vector<unsigned int> & labels);

    // files with frame number as the first column
    static bool read_fn_matrix(const char *file_name, vector<int> & fns, vector<Eigen::VectorXd> & data);
    static bool read_fn_matrix(const char *file_name, vector<int> & fns, vector<Eigen::VectorXf> & data);
    static bool read_fn_labels(const char * file_name, vector<int> & fns, vector<unsigned int> & labels);
    static bool read_fn_gd_preds(const char *file_name, vector<int> & fns, vector<unsigned int> & gds,  vector<unsigned int> & preds);

    // file with frame number as the first column
    // original features and labels are synchronized (e.g., frame numbers)
    // original label dimension is 1
    static void load_vertical_concat_feature_label(const vector<string> & feature_files, const vector<string> & label_files,
                                                   vector<Eigen::MatrixXf> & features, vector<Eigen::VectorXf> & labels);

    // multiple feature files and a single label file
    static void load_vertical_concat_feature_label(const vector<string> & feature_files, const char * label_file,
                                                   vector<Eigen::MatrixXf> & features, vector<unsigned int> & labels);

    static bool read_files(const char *file_name, vector<string> & files);
    static bool write_files(const char *file_name, const vector<string>& files);

    //
    template<class T>
    static bool save_matrix(const char * file_name, const T &m)
    {
        std::ofstream file(file_name);
        if (file.is_open()) {
            int rows = (int)m.rows();
            int cols = (int)m.cols();
            file<<rows<<" "<<cols<<"\n"<<m<<"\n";
            printf("save to %s\n", file_name);
            return true;
        }
        else {
            printf("Error: can not open file %s\n", file_name);
            return false;
        }
    }

    template <class T_VectorX>
    static bool saveVectorsAsMatrix(const char * file_name, const vector<T_VectorX> & data);

};

#endif /* defined(__Classifer_RF__DTUtil_IO__) */


/*******************************************************************************
Copyright(c) 2015-2018 LORD Corporation. All rights reserved.

MIT Licensed. See the included LICENSE.txt for a copy of the full MIT License.
*******************************************************************************/
#include "mscl/MicroStrain/Wireless/Packets/RawAngleStrainPacket.h"
#include "mscl/MicroStrain/Wireless/Packets/WirelessPacket.h"
#include "mscl/MicroStrain/Wireless/Packets/WirelessPacketCollector.h"
#include "mscl/MicroStrain/Wireless/Packets/WirelessDataPacket.h"
#include "mscl/Exceptions.h"
#include "mscl/Utils.h"

#include <boost/test/unit_test.hpp>

using namespace mscl;

BOOST_AUTO_TEST_SUITE(normalizeAngle_fn)

BOOST_AUTO_TEST_CASE(Test1)
{
    float result = Utils::normalizeAngle(0);

    BOOST_CHECK_CLOSE(result, 0.0f, std::numeric_limits<float>::epsilon());
}

BOOST_AUTO_TEST_CASE(Test2)
{
    float result = Utils::normalizeAngle(360);

    BOOST_CHECK_CLOSE(result, 0.0f, std::numeric_limits<float>::epsilon());
}

BOOST_AUTO_TEST_CASE(Test3)
{
    float result = Utils::normalizeAngle(359);

    BOOST_CHECK_CLOSE(result, 359.0f, std::numeric_limits<float>::epsilon());
}

BOOST_AUTO_TEST_CASE(Test4)
{
    float result = Utils::normalizeAngle(-1);

    BOOST_CHECK_CLOSE(result, 359.0f, std::numeric_limits<float>::epsilon());
}

BOOST_AUTO_TEST_CASE(Test5)
{
    float result = Utils::normalizeAngle(245);

    BOOST_CHECK_CLOSE(result, 245.0f, std::numeric_limits<float>::epsilon());
}

BOOST_AUTO_TEST_CASE(Test6)
{
    float result = Utils::normalizeAngle(5640);

    BOOST_CHECK_CLOSE(result, 240.0f, std::numeric_limits<float>::epsilon());
}

BOOST_AUTO_TEST_CASE(Test7)
{
    float result = Utils::normalizeAngle(-725);

    BOOST_CHECK_CLOSE(result, 355.0f, std::numeric_limits<float>::epsilon());
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(distributeAngles_fn)

BOOST_AUTO_TEST_CASE(LowerLessThanUpper)
{
    float expected[] = {10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25};
    std::vector<float> result = RawAngleStrainPacket::distributeAngles(10, 25, 16);

    for(size_t i(0); i < 16; ++i)
    {
        BOOST_CHECK_CLOSE(expected[i], result[i], std::numeric_limits<float>::epsilon());
    }
}

BOOST_AUTO_TEST_CASE(UpperLessThanLower)
{
    float expected[] = {355, 356, 357, 358, 359, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10};
    std::vector<float> result = RawAngleStrainPacket::distributeAngles(355, 10, 16);

    for(size_t i(0); i < 16; ++i)
    {
        BOOST_CHECK_CLOSE(expected[i], result[i], std::numeric_limits<float>::epsilon());
    }
}

BOOST_AUTO_TEST_CASE(UpperEqualLower)
{
    float expected[] = {36, 72, 108, 144, 180, 216, 252, 288, 324, 0, 36};
    std::vector<float> result = RawAngleStrainPacket::distributeAngles(36, 36, 11);

    for(size_t i(0); i < 10; ++i)
    {
        BOOST_CHECK_CLOSE(expected[i], result[i], std::numeric_limits<float>::epsilon());
    }
}

BOOST_AUTO_TEST_CASE(InputNotNormalized)
{
    float expected[] = {36, 72, 108, 144, 180, 216, 252, 288, 324, 0, 36};
    std::vector<float> result = RawAngleStrainPacket::distributeAngles(396, 36, 11);

    for(size_t i(0); i < 10; ++i)
    {
        BOOST_CHECK_CLOSE(expected[i], result[i], std::numeric_limits<float>::epsilon());
    }
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * GridMapMsgHelpers.hpp
 *
 *  Created on: Sep 8, 2014
 *      Author: Péter Fankhauser
 *   Institute: ETH Zurich, Autonomous Systems Lab
 */

#include "grid_map/GridMapMsgHelpers.hpp"

// Boost
#include <boost/assign.hpp>

namespace grid_map {

const int nDimensions()
{
  return 2;
}

std::map<StorageIndices, std::string> storageIndexNames = boost::assign::map_list_of
    (StorageIndices::Column,  "column_index")
    (StorageIndices::Row, "row_index");

bool isRowMajor(const std_msgs::Float32MultiArray& messageData)
{
  if (messageData.layout.dim[0].label == storageIndexNames[StorageIndices::Column]) return false;
  else if (messageData.layout.dim[0].label == storageIndexNames[StorageIndices::Row]) return true;

  ROS_ERROR("isRowMajor() failed because layout label is not set correctly.");
  return false;
}

unsigned int getCols(const std_msgs::Float32MultiArray& messageData)
{
  if (isRowMajor(messageData)) return messageData.layout.dim.at(1).size;
  return messageData.layout.dim.at(0).size;
}

unsigned int getRows(const std_msgs::Float32MultiArray& messageData)
{
  if (isRowMajor(messageData)) return messageData.layout.dim.at(0).size;
  return messageData.layout.dim.at(1).size;
}

bool multiArrayMessageCopyToMatrixEigen(const std_msgs::Float32MultiArray& m, Eigen::MatrixXf& e)
{
  if (e.IsRowMajor != isRowMajor(m))
  {
    ROS_ERROR("multiArrayMessageToMatrixEigen() failed because the storage order is not compatible.");
    return false;
  }

  Eigen::MatrixXf tempE(getRows(m), getCols(m));
  tempE = Eigen::Map<const Eigen::MatrixXf>(m.data.data(), getRows(m), getCols(m));
  e = tempE;
  return true;
}

bool multiArrayMessageMapToMatrixEigen(std_msgs::Float32MultiArray& m, Eigen::MatrixXf& e)
{
  if (e.IsRowMajor != isRowMajor(m))
  {
    ROS_ERROR("multiArrayMessageToMatrixEigen() failed because the storage order is not compatible.");
    return false;
  }

  e.resize(getRows(m), getCols(m));
  e = Eigen::Map<Eigen::MatrixXf>(m.data.data(), getRows(m), getCols(m));
  return true;
}

bool colorValueToVector(const unsigned long& colorValue, Eigen::Vector3i& colorVector)
{
  colorVector(0) = (colorValue >> 16) & 0x0000ff;
  colorVector(1) = (colorValue >> 8) & 0x0000ff;
  colorVector(2) =  colorValue & 0x0000ff;
  return true;
}

bool colorValueToVector(const unsigned long& colorValue, Eigen::Vector3f& colorVector)
{
  Eigen::Vector3i tempColorVector;
  colorValueToVector(colorValue, tempColorVector);
  colorVector = ((tempColorVector.cast<float>()).array() / 255.0).matrix();
  return true;
}

bool colorVectorToValue(const Eigen::Vector3i& colorVector, unsigned long& colorValue)
{
  colorValue = ((int)colorVector(0)) << 16 | ((int)colorVector(1)) << 8 | ((int)colorVector(2));
  return true;
}

void colorVectorToValue(const Eigen::Vector3i& colorVector, float& colorValue)
{
  int color = (colorVector(0) << 16) + (colorVector(1) << 8) + colorVector(2);
  colorValue = *reinterpret_cast<float*>(&color);
}

} /* namespace */


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/swar/include/functions/sort.hpp>
#include <nt2/include/functions/load.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/simd/sdk/meta/cardinal_as.hpp>

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

#include <algorithm>

template<typename T>
struct  not_that_small
      : boost::mpl::bool_ < boost::simd::meta::
                            cardinal_as<T, BOOST_SIMD_DEFAULT_EXTENSION>::value
                          <= 8
                          >
{};

NT2_TEST_CASE_TPL_MPL ( sort
                      , NT2_TEST_SEQ_MPL_FILTER ( NT2_SIMD_TYPES
                                                , not_that_small<boost::mpl::_>
                                                )
                      )
{
  using nt2::sort;
  using nt2::tag::sort_;
  using nt2::load;
  using boost::simd::native;

  typedef NT2_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>             vT;

  T data[vT::static_size];

  for(int i = 0; i<vT::static_size;++i) data[i] = i;

  vT ordered = load<vT>(&data[0]);

  do
  {
    vT current = load<vT>(&data[0]);
    vT r = nt2::sort(current);
    NT2_TEST_EQUAL(r,ordered);

  } while ( std::next_permutation(data,data+vT::static_size) );
}


#include <catch2/catch_test_macros.hpp>
#include <catch2/catch_approx.hpp>
#include <algorithm>
#include <boost/utility/string_view.hpp>
#include <string_view>
#include <vector>

#include <rapidfuzz/string_metric.hpp>

namespace string_metric = rapidfuzz::string_metric;

TEST_CASE("levenshtein works with string_views", "[string_view]")
{
  rapidfuzz::string_view test = "aaaa";
  rapidfuzz::string_view no_suffix = "aaa";
  rapidfuzz::string_view no_suffix2 = "aaab";
  rapidfuzz::string_view swapped1 = "abaa";
  rapidfuzz::string_view swapped2 = "baaa";
  rapidfuzz::string_view replace_all = "bbbb";

  SECTION("weighted levenshtein calculates correct distances")
  {
    REQUIRE(string_metric::levenshtein(test, test, {1, 1, 2}) == 0);
    REQUIRE(string_metric::levenshtein(test, no_suffix, {1, 1, 2}) == 1);
    REQUIRE(string_metric::levenshtein(swapped1, swapped2, {1, 1, 2}) == 2);
    REQUIRE(string_metric::levenshtein(test, no_suffix2, {1, 1, 2}) == 2);
    REQUIRE(string_metric::levenshtein(test, replace_all, {1, 1, 2}) == 8);
  }

  SECTION("weighted levenshtein calculates correct ratios")
  {
    REQUIRE(string_metric::normalized_levenshtein(test, test, {1, 1, 2}) == 100.0);
    REQUIRE(string_metric::normalized_levenshtein(test, no_suffix, {1, 1, 2}) ==
            Catch::Approx(85.7).epsilon(0.01));
    REQUIRE(string_metric::normalized_levenshtein(swapped1, swapped2, {1, 1, 2}) ==
            Catch::Approx(75.0).epsilon(0.01));
    REQUIRE(string_metric::normalized_levenshtein(test, no_suffix2, {1, 1, 2}) ==
            Catch::Approx(75.0).epsilon(0.01));
    REQUIRE(string_metric::normalized_levenshtein(test, replace_all, {1, 1, 2}) ==
            0.0);
  }
};

TEST_CASE("hamming", "[string_view]")
{
  rapidfuzz::string_view test = "aaaa";
  rapidfuzz::string_view diff_a = "abaa";
  rapidfuzz::string_view diff_b = "aaba";
  rapidfuzz::string_view diff_len = "aaaaa";

  SECTION("hamming calculates correct distances")
  {
    REQUIRE(string_metric::hamming(test, test) == 0);
    REQUIRE(string_metric::hamming(test, diff_a) == 1);
    REQUIRE(string_metric::hamming(test, diff_b) == 1);
    REQUIRE(string_metric::hamming(diff_a, diff_b) == 2);
  }

  SECTION("hamming raises exception for different lengths")
  {
    REQUIRE_THROWS_AS(string_metric::hamming(test, diff_len), std::invalid_argument);
  }
};


#include "vtkboneGenerateHommingaMaterialTable.h"
#include "vtkObjectFactory.h"
#include "vtkInformation.h"
#include "vtkInformationVector.h"
#include "vtkboneMaterialTable.h"
#include "vtkboneLinearIsotropicMaterial.h"
#include "vtkboneLinearOrthotropicMaterial.h"
#include "vtkboneLinearAnisotropicMaterial.h"
#include "vtkboneLinearIsotropicMaterialArray.h"
#include "vtkboneLinearOrthotropicMaterialArray.h"
#include "vtkboneLinearAnisotropicMaterialArray.h"
#include "vtkSmartPointer.h"
#include <boost/format.hpp>

vtkStandardNewMacro(vtkboneGenerateHommingaMaterialTable);

//----------------------------------------------------------------------------
vtkboneGenerateHommingaMaterialTable::vtkboneGenerateHommingaMaterialTable()
  :
  FirstIndex (1),
  LastIndex (127),
  Exponent (1.7),
  FullScaleMaterial (NULL)
  {
  this->SetNumberOfInputPorts(0);
  this->SetFullScaleMaterial(vtkboneLinearIsotropicMaterial::New());
  }

//----------------------------------------------------------------------------
vtkboneGenerateHommingaMaterialTable::~vtkboneGenerateHommingaMaterialTable()
  {
  this->SetFullScaleMaterial(NULL);
  }

//----------------------------------------------------------------------------
void vtkboneGenerateHommingaMaterialTable::PrintSelf (ostream& os, vtkIndent indent)
  {
  this->Superclass::PrintSelf(os,indent);
  os << indent << "FirstIndex: " << this->FirstIndex << "\n";
  os << indent << "LastIndex: " << this->LastIndex << "\n";
  os << indent << "Exponent " << this->Exponent << "\n";
  os << indent << "FullScaleMaterial: \n";
  this->FullScaleMaterial->PrintSelf(os, indent.GetNextIndent());
  }

//----------------------------------------------------------------------------
int vtkboneGenerateHommingaMaterialTable::RequestData
  (
  vtkInformation* request,
  vtkInformationVector** inputVector,
  vtkInformationVector* outputVector
  )
  {
  using boost::format;

  vtkInformation *outInfo = outputVector->GetInformationObject(0);
  vtkboneMaterialTable *output = vtkboneMaterialTable::SafeDownCast(
                            outInfo->Get(vtkDataObject::DATA_OBJECT()));
  if (!output)
    {
    vtkErrorMacro("No output object.");
    return 0;
    }

  vtkSmartPointer<vtkboneMaterial> ma;

  if (vtkboneLinearAnisotropicMaterial* aniso =
      vtkboneLinearAnisotropicMaterial::SafeDownCast(this->FullScaleMaterial))
    {
    vtkboneLinearAnisotropicMaterialArray* ma_aniso = vtkboneLinearAnisotropicMaterialArray::New();
    ma = vtkSmartPointer<vtkboneMaterial>::Take(ma_aniso);
    int N = this->LastIndex - this->FirstIndex + 1;
    ma_aniso->Resize (N);
    for (int k=0; k<N; ++k)
      {
      double x = (double)(k+1)/(double)(N);
      double factor = pow(x, this->Exponent);
      ma_aniso->SetScaledItem (k, aniso, factor);
      }
    }

  else if (vtkboneLinearOrthotropicMaterial* ortho =
           vtkboneLinearOrthotropicMaterial::SafeDownCast(this->FullScaleMaterial))
    {
    vtkboneLinearOrthotropicMaterialArray* ma_ortho = vtkboneLinearOrthotropicMaterialArray::New();
    ma = vtkSmartPointer<vtkboneMaterial>::Take(ma_ortho);
    int N = this->LastIndex - this->FirstIndex + 1;
    ma_ortho->Resize (N);
    for (int k=0; k<N; ++k)
      {
      double x = (double)(k+1)/(double)(N);
      double factor = pow(x, this->Exponent);
      ma_ortho->SetScaledItem (k, ortho, factor);
      }
    }

  else if (vtkboneLinearIsotropicMaterial* iso =
           vtkboneLinearIsotropicMaterial::SafeDownCast(this->FullScaleMaterial))
    {
    vtkboneLinearIsotropicMaterialArray* ma_iso = vtkboneLinearIsotropicMaterialArray::New();
    ma = vtkSmartPointer<vtkboneMaterial>::Take(ma_iso);
    int N = this->LastIndex - this->FirstIndex + 1;
    ma_iso->Resize (N);
    for (int k=0; k<N; ++k)
      {
      double x = (double)(k+1)/(double)(N);
      double factor = pow(x, this->Exponent);
      ma_iso->SetScaledItem (k, iso, factor);
      }
    }

  else
    {
    vtkErrorMacro ("Full scale material not set or incompatible with Homminga material table.");
    return 0;
    }

  std::string name = this->FullScaleMaterial->GetName();
  name += "_homminga";
  ma->SetName (name.c_str());

  output->RemoveAll();
  output->AddMaterial (this->FirstIndex, ma);

  return 1;
  }


#include <stdio.h>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/StdVector>

#include <power_sasa.h>
#include <testincludes.h>

// g++ -o sasatest -I. sasatest.cpp

int main(int argc, char* argv[])
{
        if (argc != 2)
        {
                std::cout << "Usage: " << argv[0] << " <input file>" << std::endl;
                std::exit(1);
        }

  	//Specifying Number Type and Vector Type
	//typedef float number_type;
	//typedef Eigen::Vector3f Coord; //fake vector4->3f might be even faster
//#ifdef _FLOATTEST
	typedef float Scalar;
	typedef Eigen::Vector3f Coord;
	std::cout << "Precision: float" << std::endl;
//#else
	//typedef double Scalar;
	//typedef Eigen::Vector3d Coord;
	//std::cout << "Precision: double" << std::endl;
//#endif

	std::vector<Coord> coords; //Vector of coordinates
	std::vector<Scalar> weights; //Vector of weights
/*
	coords.push_back(Coord(1.0,2.0,3.0));
	weights.push_back(1.0);
	coords.push_back(Coord(5.0,3.0,3.0));
	weights.push_back(1.0);
	coords.push_back(Coord(5.1,3.2,3.5));
	weights.push_back(1.0);
	coords.push_back(Coord(1.3,3.2,1.5));
	weights.push_back(1.0);
	coords.push_back(Coord(0.0,3.4,1.7));
	weights.push_back(1.0);
	coords.push_back(Coord(5.4,3.4,1.7));
	weights.push_back(1.4);
	coords.push_back(Coord(3.4,2.1,1.4));
	weights.push_back(1.4);

	std::cout << "Testing SASA, 7 Verts" << std::endl;
	SASA::PowerSasa<number_type,Coord> *ps;
	ps = new SASA::PowerSasa<number_type,Coord>(coords, weights, 0, 0, 0, 0);
	//ps->calc_sasa_all();
	delete ps;
*/
        parse(argv[1],coords,weights);
	//std::vector<Scalar>                 Sasa;
	//std::vector< std::vector<Coord> >   DSasa;
	//std::vector<Scalar>                 Vol;
	//std::vector<Coord>                  DVol;
	std::vector<Coord>                  force;

	//Sasa.resize(coords.size());
	//DSasa.resize(coords.size());
	//Vol.resize(coords.size());
	//DVol.resize(coords.size());
	force.resize(coords.size());

	POWERSASA::PowerSasa<Scalar,Coord> *ps =
		new POWERSASA::PowerSasa<Scalar,Coord>(coords, weights, 1, 1, 1, 1);
	ps->calc_sasa_all();

    /*
	for (unsigned int i = 0; i < coords.size(); ++i) {
	  force[i] = -(ps->getDSasa())[i][ps->NumOfNeighbours(i)];
	}
	for (unsigned int i = 0; i < coords.size(); ++i)
	{
		for (unsigned int j = 0; j < ps->NumOfNeighbours(i); ++j)
		{
			force[ps->AtomNo(i, j)] -=  ps->getDSasa()[i][j];
			// force[ps.AtomNo(i, j)] -=  sigma[i] * ps->getDSasa()[i][j];
		}
	}
    */

    Scalar volume = 0.0, surf = 0.0;
	for (unsigned int i = 0; i < coords.size(); ++i)
	{
		printf("%4d sasa=%7.3lf vol=%7.3lf\n", i, (ps->getSasa())[i], (ps->getVol())[i]);
		//printf("%4d sasa=%7.3lf fx=%8.4lf fy=%8.4lf fz=%8.4lf v=%8.4lf dvx=%8.4lf dvy=%8.4lf dvz=%8.4lf\n",
		//	i, ps->getSasa()[i], force[i][0], force[i][1], force[i][2],
		//	ps->getVol()[i], ps->getDVol()[i][0], ps->getDVol()[i][1], ps->getDVol()[i][2]);
		volume += (ps->getVol())[i];
        surf += (ps->getSasa())[i];
	}
	printf("volume=%lf\n", volume);
    printf("sasa  =%lf\n", surf);

        //for (unsigned int i = 0; i < coords.size(); ++i)
        //{
        //        std::cout << coords[i][0] << " " << coords[i][1] << " " << coords[i][2]
        //        << " " << weights[i] << " " << Sasa[i] << std::endl;
        //}

        delete ps;

	return 0;
}


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/config.hpp>
#include <boost/simd/detail/brigand.hpp>
#include <boost/simd/detail/dispatch.hpp>
#include <boost/simd/detail/constant_traits.hpp>
#include <boost/simd/detail/dispatch/function/make_callable.hpp>
#include <boost/simd/detail/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch/as.hpp>

/*

    @ingroup group-constant

    Generate a mask value used in pow2

    @return The Pow2mask constant for the proper type
  */

namespace boost { namespace simd
{
  namespace tag
  {
    struct pow2mask_ : boost::dispatch::constant_value_<pow2mask_>
    {
      BOOST_DISPATCH_MAKE_CALLABLE(ext,pow2mask_,boost::dispatch::constant_value_<pow2mask_>);
      BOOST_SIMD_REGISTER_CONSTANT(0, 0x7FBFFFUL, 0xFFEFFFFFFFFFFULL);
    };
  }
      //    mask is  ~bitwise_or(Minf<A0>(), iA0(siA0(1) <<  (Nbmantissabits<sA0>()-Nbexponentbits<sA0>()-1)))
      //    if it is needed for other floating type

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag,pow2mask_);
  }

  namespace detail
  {
    BOOST_DISPATCH_CALLABLE_DEFINITION(tag::pow2mask_,pow2mask);
  }

  template<typename T> BOOST_FORCEINLINE auto Pow2mask()
  BOOST_NOEXCEPT_DECLTYPE(detail::pow2mask( boost::dispatch::as_<T>{}))
  {
    return detail::pow2mask( boost::dispatch::as_<T>{} );
  }
} }

#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


#pragma once

#include <cstdint>
#include <functional>
#include <memory>
#include <stdexcept>
#include <vector>

#include <Eigen/Geometry>
#include <voxelized_geometry_tools/collision_map.hpp>

namespace voxelized_geometry_tools
{
namespace pointcloud_voxelization
{
enum class SeenAs : uint8_t {UNKNOWN = 0x00, FILLED = 0x01, FREE = 0x02};

class PointCloudVoxelizationFilterOptions
{
public:
  PointCloudVoxelizationFilterOptions(
      const double percent_seen_free, const int32_t outlier_points_threshold,
      const int32_t num_cameras_seen_free)
      : percent_seen_free_(percent_seen_free),
        outlier_points_threshold_(outlier_points_threshold),
        num_cameras_seen_free_(num_cameras_seen_free)
  {
    if (percent_seen_free_ <= 0.0 || percent_seen_free_ > 1.0)
    {
      throw std::invalid_argument("0 < percent_seen_free_ <= 1 must be true");
    }
    if (outlier_points_threshold_ <= 0)
    {
      throw std::invalid_argument("outlier_points_threshold_ <= 0");
    }
    if (num_cameras_seen_free_ <= 0)
    {
      throw std::invalid_argument("num_cameras_seen_free_ <= 0");
    }
  }

  PointCloudVoxelizationFilterOptions()
      : percent_seen_free_(1.0),
        outlier_points_threshold_(1),
        num_cameras_seen_free_(1) {}

  double PercentSeenFree() const { return percent_seen_free_; }

  int32_t OutlierPointsThreshold() const { return outlier_points_threshold_; }

  int32_t NumCamerasSeenFree() const { return num_cameras_seen_free_; }

  SeenAs CountsSeenAs(
      const int32_t seen_free_count, const int32_t seen_filled_count) const
  {
    const int32_t filtered_seen_filled_count =
        (seen_filled_count >= OutlierPointsThreshold()) ? seen_filled_count : 0;
    if (seen_free_count > 0 && filtered_seen_filled_count > 0)
    {
      const double percent_seen_free =
          static_cast<double>(seen_free_count)
          / static_cast<double>(seen_free_count + filtered_seen_filled_count);
      if (percent_seen_free >= PercentSeenFree())
      {
        return SeenAs::FREE;
      }
      else
      {
        return SeenAs::FILLED;
      }
    }
    else if (seen_free_count > 0)
    {
      return SeenAs::FREE;
    }
    else if (filtered_seen_filled_count > 0)
    {
      return SeenAs::FILLED;
    }
    else
    {
      return SeenAs::UNKNOWN;
    }
  }

private:
  double percent_seen_free_ = 1.0;
  int32_t outlier_points_threshold_ = 1;
  int32_t num_cameras_seen_free_ = 1;
};

class PointCloudWrapper
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  virtual ~PointCloudWrapper() {}

  virtual double MaxRange() const = 0;

  virtual int64_t Size() const = 0;

  virtual const Eigen::Isometry3d& GetPointCloudOriginTransform() const = 0;

  virtual void SetPointCloudOriginTransform(
      const Eigen::Isometry3d& origin_transform) = 0;

  Eigen::Vector4d GetPointLocationVector4d(const int64_t point_index) const
  {
    Eigen::Vector4d point(0.0, 0.0, 0.0, 1.0);
    CopyPointLocationIntoVector4d(point_index, point);
    return point;
  }

  Eigen::Vector4f GetPointLocationVector4f(const int64_t point_index) const
  {
    Eigen::Vector4f point(0.0f, 0.0f, 0.0f, 1.0f);
    CopyPointLocationIntoVector4f(point_index, point);
    return point;
  }

  void CopyPointLocationIntoVector4d(
      const int64_t point_index, Eigen::Vector4d& point_location) const
  {
    CopyPointLocationIntoDoublePtr(point_index, point_location.data());
    point_location(3) = 1.0;
  }

  void CopyPointLocationIntoVector4f(
      const int64_t point_index, Eigen::Vector4f& point_location) const
  {
    CopyPointLocationIntoFloatPtr(point_index, point_location.data());
    point_location(3) = 1.0f;
  }

  void CopyPointLocationIntoVectorDouble(
      const int64_t point_index, std::vector<double>& vector,
      const int64_t vector_index) const
  {
    EnforceVectorIndexInRange(vector_index, vector);
    CopyPointLocationIntoDoublePtr(point_index, vector.data() + vector_index);
  }

  void CopyPointLocationIntoVectorFloat(
      const int64_t point_index, std::vector<float>& vector,
      const int64_t vector_index) const
  {
    EnforceVectorIndexInRange(vector_index, vector);
    CopyPointLocationIntoFloatPtr(point_index, vector.data() + vector_index);
  }

  void CopyPointLocationIntoDoublePtr(
      const int64_t point_index, double* destination) const
  {
    EnforcePointIndexInRange(point_index);
    CopyPointLocationIntoDoublePtrImpl(point_index, destination);
  }

  void CopyPointLocationIntoFloatPtr(
      const int64_t point_index, float* destination) const
  {
    EnforcePointIndexInRange(point_index);
    CopyPointLocationIntoFloatPtrImpl(point_index, destination);
  }

  void EnforcePointIndexInRange(const int64_t point_index) const
  {
    if (point_index < 0 || point_index >= Size())
    {
      throw std::out_of_range("point_index out of range");
    }
  }

  template<typename T>
  static void EnforceVectorIndexInRange(
      const int64_t vector_index, const std::vector<T>& destination)
  {
    if (vector_index < 0 ||
        (vector_index + 3) > static_cast<int64_t>(destination.size()))
    {
      throw std::out_of_range("vector_index out of range");
    }
  }

protected:
  virtual void CopyPointLocationIntoDoublePtrImpl(
      const int64_t point_index, double* destination) const = 0;

  virtual void CopyPointLocationIntoFloatPtrImpl(
      const int64_t point_index, float* destination) const = 0;
};

using PointCloudWrapperPtr = std::shared_ptr<PointCloudWrapper>;

class VoxelizerRuntime
{
public:
  VoxelizerRuntime(const double raycasting_time, const double filtering_time)
      : raycasting_time_(raycasting_time), filtering_time_(filtering_time)
  {
    if (raycasting_time_ < 0.0)
    {
      throw std::invalid_argument("raycasting_time < 0.0");
    }
    if (filtering_time_ < 0.0)
    {
      throw std::invalid_argument("filtering_time < 0.0");
    }
  }

  double RaycastingTime() const { return raycasting_time_; }

  double FilteringTime() const { return filtering_time_; }

private:
  double raycasting_time_ = 0.0;
  double filtering_time_ = 0.0;
};

class PointCloudVoxelizationInterface
{
public:
  virtual ~PointCloudVoxelizationInterface() {}

  CollisionMap VoxelizePointClouds(
      const CollisionMap& static_environment, const double step_size_multiplier,
      const PointCloudVoxelizationFilterOptions& filter_options,
      const std::vector<PointCloudWrapperPtr>& pointclouds,
      const std::function<void(const VoxelizerRuntime&)>&
          runtime_log_fn = [] (const VoxelizerRuntime& voxelizer_runtime)
          {
            const double raycasting_time = voxelizer_runtime.RaycastingTime();
            const double filtering_time = voxelizer_runtime.FilteringTime();
            std::cout
                << "Raycasting time " << raycasting_time
                << ", filtering time " << filtering_time
                << ", total time " << raycasting_time + filtering_time
                << std::endl;
          }) const {
    CollisionMap output_environment = static_environment;
    const auto voxelizer_runtime = VoxelizePointClouds(
        static_environment, step_size_multiplier, filter_options, pointclouds,
        output_environment);
    runtime_log_fn(voxelizer_runtime);
    return output_environment;
  }

  VoxelizerRuntime VoxelizePointClouds(
      const CollisionMap& static_environment, const double step_size_multiplier,
      const PointCloudVoxelizationFilterOptions& filter_options,
      const std::vector<PointCloudWrapperPtr>& pointclouds,
      CollisionMap& output_environment) const {
    if (!static_environment.IsInitialized())
    {
      throw std::invalid_argument("!static_environment.IsInitialized()");
    }
    if (step_size_multiplier > 1.0 || step_size_multiplier <= 0.0)
    {
      throw std::invalid_argument("step_size_multiplier is not in (0, 1]");
    }
    if (!output_environment.IsInitialized())
    {
      throw std::invalid_argument("!output_environment.IsInitialized()");
    }
    if (static_environment.GetGridSizes() != output_environment.GetGridSizes())
    {
      throw std::invalid_argument(
          "static_environment.GetGridSizes() != "
          "output_environment.GetGridSizes()");
    }
    return DoVoxelizePointClouds(
        static_environment, step_size_multiplier, filter_options, pointclouds,
        output_environment);
  }

protected:
  virtual VoxelizerRuntime DoVoxelizePointClouds(
      const CollisionMap& static_environment, const double step_size_multiplier,
      const PointCloudVoxelizationFilterOptions& filter_options,
      const std::vector<PointCloudWrapperPtr>& pointclouds,
      CollisionMap& output_environment) const = 0;
};
}  // namespace pointcloud_voxelization
}  // namespace voxelized_geometry_tools


/***
 *  $Id$
 **
 *  File: performance_indicators_diss.hpp
 *  Created: May 9, 2012
 *
 *  Author: Olga Wodo, Baskar Ganapathysubramanian
 *  Copyright (c) 2012 Olga Wodo, Baskar Ganapathysubramanian
 *  See accompanying LICENSE.
 *
 *  This file is part of GraSPI.
 */

#ifndef PERFORMANCE_INDICATORS_DISS_HPP
#define PERFORMANCE_INDICATORS_DISS_HPP

#include <climits>
#include <sstream>

#include "graspi_types.hpp"
#include "graph_dijkstra.hpp"
#include "graspi_predicates.hpp"
#include <boost/graph/filtered_graph.hpp>

namespace graspi {

    /// structure used to estimate the contribution of electron donor to exciton dissociation
    struct foo_w_diss{
        double A1; ///< coefficient to define the exciton diffusion weighting function
        double B1; ///< coefficient to define the exciton diffusion weighting function
        double C1; ///< coefficient to define the exciton diffusion weighting function
        foo_w_diss(){ A1=6.265; B1=-23.0; C1=17.17; }
        double operator()(double d)const{
            return A1*exp(-((d-B1)/C1)*((d-B1)/C1));
        }
    };

    /// Function to count number of vertices within predefined distance to target vertices
    ///
    /// @param d is the vector of distances
    /// @param Ld is the predefined distance to be checked against
    /// return the total number of vertices within the Ld distance to the targeted meta-vertex
    inline int
    identify_n_vertices_within_distance( const std::vector<float>& d,
                                        double Ld){
        int n_Ld = 0;
        for(unsigned int i = 0; i < d.size(); i++){
            if( (d[i] < Ld) && (d[i] > 0) ) n_Ld++;
        }
        return n_Ld;
    }

    /// Function to compute the weighted sum of electron-donor contributing to exciton dissociation located with Ld distance to the interface
    ///
    /// @param d is the vector of distances
    /// @param Ld is the predefined distance to be checked against
    /// return the weigthed sum over all vertices within the Ld distance to the targeted meta-vertex
    inline double
    identify_weighted_vertices_within_distance( const std::vector<float>& d,
                                               double Ld){
        double wn_Ld = 0;
        foo_w_diss wfoo;

        for(unsigned int i = 0; i < d.size(); i++){
            double d_i = d[i];
            double wd_i = wfoo(d_i);
            if( (d_i < Ld) && (d_i > 0) ) wn_Ld+= wd_i;
        }
        return wn_Ld;
    }

    /// Function to compute the weighted fraction of electron-donor contributing to exciton dissociation located with Ld distance to the interface
    ///
    /// @param d is the vector of distances
    /// @param Ld is the predefined distance to be checked against
    /// return the pair of values required to compute the weigthed fraction
    /// the pair consists of weigthed sum of contribution
    /// and the total number of vertices within the Ld distance to the targeted meta-vertex (here GREEN)
    inline std::pair<int,double>
    identify_n_weighted_vertices_within_distance( const std::vector<float>& d,
                                                 double Ld){
        double wn_Ld = 0;
        int n_Ld = 0;
        foo_w_diss wfoo;

        for(unsigned int i = 0; i < d.size(); i++){
            double d_i = d[i];
            double wd_i = wfoo(d_i);
            if( (d_i < Ld) && (d_i > 0) ){
                wn_Ld+= wd_i;
                n_Ld++;
            }
        }
        return std::pair<int,double>(n_Ld,wn_Ld);
    }

    /// This function computes two descriptors related to exciton dissociation
    ///
    /// @param G is the input graph
    /// @param d_g is the structure storing basic informations about the graph dimensionality
    /// @param C is the vector storing the labels/colors of vertices in the graph G
    /// @param W is the map storing the weights of the edges
    /// @param vCC is the vector storing indices of the connected components (CC) of each vector in the graph
    /// @param CC is the vector of CC,
    /// @param Ld is the exciton diffusion length
    /// @param filename_ColorToGreen is the filename where all distances from GREEN to BLACK vertices will be outputted
    /// @param filename_WColorToGreen is the filename where all weighted distances from GREEN to BLACK vertices will be outputed
    /// @param color is the color code for vertices that will be analyzed (default value is BLACK)
    /// @param green is the color code for targeted meta-vertex (default value is GREEN)
    /// @return the pair of fractions: the weighted fraction of vertices and the fraction of vertices within Ld distance to the targeted meta-vertex
    inline std::pair<double,double>
    wf_diss(
            graph_t* G, const dim_g_t& d_g, const vertex_colors_t& C,
            const edge_weights_t& W, const vertex_ccs_t& vCC,
            const ccs_t& CC,
            double Ld,
            const std::string& filename_ColorToGreen,
            const std::string& filename_WColorToGreen,
            COLOR color = BLACK,
            COLOR green = GREEN
            ){
        int n_color = 0;
        int n_color_Ld = 0;
        double wn_color_Ld = 0;

        connect_color_green pred(*G,C,color,green);
        unsigned int n = boost::num_vertices(*G);
        vertex_t int_id = d_g.id(green);
        std::vector<float> d(n);

        determine_shortest_distances( G, W, int_id, pred, d);

        foo_w_diss wfoo;
        std::ostringstream oss_out_d;
        std::ostringstream oss_out_wd;
        for (unsigned int i = 0; i < d.size(); i++) {
            unsigned int c = C[i];
            if (c == color) n_color++;
            if ( ( c == color )
                && ( fabs(d[i]) < std::numeric_limits<float>::max() )
                ) {
                double d_i = d[i];
                oss_out_d  << d_i       << std::endl;
                oss_out_wd << d_i << " " << wfoo(d_i) << std::endl;
            }
        }
        std::ofstream f_out(filename_ColorToGreen.c_str());
        std::string buffer = oss_out_d.str();
        int size = oss_out_d.str().size();
        f_out.write (buffer.c_str(),size);
        f_out.close();
        f_out.open(filename_WColorToGreen.c_str());
        buffer = oss_out_wd.str();
        size = oss_out_wd.str().size();
        f_out.write (buffer.c_str(),size);
        f_out.close();

        std::pair<int,double> pLd
        = identify_n_weighted_vertices_within_distance(d,Ld);
        n_color_Ld = pLd.first;
        wn_color_Ld = pLd.second;

#ifdef DEBUG
        std::cout << "[DEBUG] Number of " << color << "vertices: "
        << n_color << std::endl
        << "[DEBUG] Number of " << color << "vertices in "
        << Ld << " distance to green: "
        << n_color_Ld << std::endl;
#endif

        return std::pair<double, double>(
                                         (double)wn_color_Ld/n_color,
                                         (double)n_color_Ld/n_color
                                         );
    }

      /// Function to compute the descriptors related to exciton dissociation
      ///
      /// @cond
      /// @fn std::pair<double,double> wf_diss( graph_t* G, const dim_g_t& d_g, const vertex_colors_t& C, const edge_weights_t& W, const vertex_ccs_t& vCC, const ccs_t& CC, double Ld, COLOR color = BLACK, COLOR green = GREEN )
      /// This function computes two descriptors related to exciton dissociation
      /// @param G is the input graph
      /// @param d_g is the structure storing basic informations about the graph dimensionality
      /// @param C is the vector storing the labels/colors of vertices in the graph G
      /// @param W is the map storing the weights of the edges
      /// @param vCC is the vector storing indices of the connected components (CC) of each vector in the graph
      /// @param CC is the vector of CC,
      /// @param Ld is the exciton diffusion length
      /// @param color is the color code for vertices that will be analyzed (default value is BLACK)
      /// @param green is the color code for targeted meta-vertex (default value is GREEN)
      /// @return the pair of fractions: the weighted fraction of vertices and the fraction of vertices within Ld distance to the targeted meta-vertex
    inline std::pair<double,double> wf_diss(
            graph_t* G, const dim_g_t& d_g, const vertex_colors_t& C,
            const edge_weights_t& W, const vertex_ccs_t& vCC,
            const ccs_t& CC,
            double Ld,
            COLOR color = BLACK,
            COLOR green = GREEN
            ){
        int n_color = 0;
        int n_color_Ld = 0;
        double wn_color_Ld = 0;

        connect_color_green pred(*G,C,color,green);
        unsigned int n = boost::num_vertices(*G);
        vertex_t int_id = d_g.id(green);
        std::vector<float> d(n);

        determine_shortest_distances( G, W, int_id, pred, d);

        foo_w_diss wfoo;
        for (unsigned int i = 0; i < d.size(); i++) {
            unsigned int c = C[i];
            if (c == color) n_color++;
        }

        std::pair<int,double> pLd
        = identify_n_weighted_vertices_within_distance(d,Ld);
        n_color_Ld = pLd.first;
        wn_color_Ld = pLd.second;

        return std::pair<double, double>(
                                         (double)wn_color_Ld/n_color,
                                         (double)n_color_Ld/n_color
                                         );
    }
    /// @endcond

}
#endif


#ifndef _PAGERANK_HPP_
#define _PAGERANK_HPP_

#include "graph.hpp"
#include <boost/graph/page_rank.hpp>

void pagerank(Graph&, double, int);

#endif


// Copyright 2002 - 2008, 2010, 2011 National Technology Engineering
// Solutions of Sandia, LLC (NTESS). Under the terms of Contract
// DE-NA0003525 with NTESS, the U.S. Government retains certain rights
// in this software.
//
// Use of this source code is governed by a BSD-style
// license that can be found in the LICENSE file.

#ifndef ReferenceMeshSmootherBase_hpp
#define ReferenceMeshSmootherBase_hpp
#include <percept/MeshType.hpp>
#include <percept/Percept.hpp>
#if !defined(NO_GEOM_SUPPORT)

#include <percept/mesh/mod/smoother/MeshSmoother.hpp>
#include <percept/mesh/mod/smoother/SmootherMetric.hpp>
#include <boost/unordered_map.hpp>
#include <stk_mesh/base/FieldParallel.hpp>
#include <percept/PerceptUtils.hpp>

namespace percept {

/// A smoother based on a reference mesh - tries to make the new mesh the same local size as original

template<typename MeshType>
class ReferenceMeshSmootherBaseImpl : public MeshSmootherImpl<MeshType> {

public:

	using Base = MeshSmootherImpl<MeshType>;

	typedef boost::unordered_map<typename MeshType::MTNode , std::vector<double> > NodeMap;

	ReferenceMeshSmootherBaseImpl(PerceptMesh *eMesh,
//			typename MeshType::MTSelector *boundary_selector=0,
            STKMesh::MTSelector *stk_select=0,
            StructuredGrid::MTSelector *sgrid_select=0,
			typename MeshType::MTMeshGeometry *meshGeometry=0,
			int inner_iterations = 100,
			double grad_norm =1.e-8,
			int parallel_iterations = 20);

	virtual ~ReferenceMeshSmootherBaseImpl();

protected:

	virtual int get_num_stages() { return 2; }
	virtual void run_algorithm();
	virtual double run_one_iteration();

	void sync_fields(int iter=0);
	virtual bool check_convergence();

	void check_project_all_delta_to_tangent_plane(typename MeshType::MTField *field);

	template<typename T>
	void check_equal(T& val)
	{
		T global_min = val, global_max=val;
		stk::all_reduce( Base::m_eMesh->get_bulk_data()->parallel() , stk::ReduceMax<1>( & global_max ) );
		stk::all_reduce( Base::m_eMesh->get_bulk_data()->parallel() , stk::ReduceMax<1>( & global_max ) );
		VERIFY_OP_ON( global_max, ==, val , "bad parallel val");
		VERIFY_OP_ON( global_min, ==, val , "bad parallel val");
		VERIFY_OP_ON( global_max, ==, global_min , "bad parallel val");
	}
	void set_local_field_ptrs(PerceptMesh *eMesh);
    void print_iteration_status(const int num_invalid_0);
    std::ofstream myFile;

public:
	NodeMap m_current_position;
	NodeMap m_delta;
	NodeMap m_weight;
	NodeMap m_nweight;

	double m_scale;

	Double m_dmax;
	Double m_dmax_relative;
	Double m_dnew, m_dold, m_d0, m_dmid, m_dd, m_alpha, m_alpha_0, m_grad_norm, m_grad_norm_scaled;
	Double m_total_metric;
	int m_stage;
	double m_omega;
	double m_omega_prev;
	int m_iter;
    int m_anim_step;

	size_t m_num_invalid;
	Double m_global_metric;
	bool m_untangled;
	size_t m_num_nodes;

	typename MeshType::MTField *m_coord_field_original;
	typename MeshType::MTField *m_coord_field_projected;
	typename MeshType::MTField *m_coord_field_current;
	typename MeshType::MTField *m_coord_field_lagged;

	SmootherMetricImpl<MeshType> *m_metric;
public:
	bool m_use_ref_mesh;
	int m_do_animation;
};

using ReferenceMeshSmootherBase = ReferenceMeshSmootherBaseImpl<STKMesh>;
}

#endif
#endif


//  Copyright (c) 2006-2009, Bernhard Reiter
//
//  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)

#include <boost/tree/binary_tree.hpp>
#include <boost/tree/balancers/red_black.hpp>
#include <boost/tree/augmentors/unaugmented.hpp>

#include <vector>
#include <algorithm>

#define BOOST_TEST_MODULE red_black_tree test
//#define BOOST_TEST_DYN_LINK
#include <boost/test/included/unit_test.hpp>

using boost::tree::binary_tree;

//TODO: Make this a test suite.

BOOST_AUTO_TEST_CASE( red_black_tree_test )
{
    using namespace boost::tree;

    typedef test_balancer<binary_tree<int>, balancers::red_black> red_black_tree_t;

    red_black_tree_t my_balancer;
    std::vector<int> my_vector;

    create_test_data_sequence(my_balancer);
    create_test_data_sequence(my_vector);

    BOOST_CHECK(std::equal(my_balancer.begin(), my_balancer.end(), my_vector.begin()));

//    typedef binary_tree<int, balancers::red_black> tree_t;
//    typedef searcher<false, tree_t> searcher_t;
//    searcher_t my_searcher;
//
//    searcher_t::iterator it, c1, c2, c3, c4, c5;
//    searcher_t::const_iterator cit;
//

//create_test_data_searcher(my_searcher);

//    it = my_searcher.end();
//
    //c1 = my_searcher.insert(c, 8);
//    c1 = my_searcher.insert(it, 8);
//

//    cit = my_searcher.begin();
//    BOOST_CHECK_EQUAL(*c1, 8);

//    my_searcher.insert(5);
//    my_searcher.insert(it, 7);
//    my_searcher.insert(it, 12);
//    my_searcher.insert(it, 14);
//    my_searcher.insert(it, 2);
//    my_searcher.insert(it, 11);

//    searcher_t::container_type core = my_searcher.get_container();
//    searcher_t::container_type::cursor c = core.root().begin();
//    BOOST_CHECK_EQUAL(*c, 8);

//    BOOST_CHECK_EQUAL(c1.parent(), c); //Maybe factor out specifc tests (parent(), begin(), ...)
//    BOOST_CHECK_EQUAL(c1, my_searcher.end().begin()); //FIXME. end means root
//    BOOST_CHECK(c1 != my_searcher.end().end()); //ditto
//    BOOST_CHECK_EQUAL(*c1, 8);
//    BOOST_CHECK(!c1.has_child());
//
//    BOOST_CHECK(c.has_child());
//    BOOST_CHECK_EQUAL(c, my_searcher.end());

//    ret = key_lower_bound(my_searcher.end(), 18,
//                                       lower_bound_wrapper<mycursor, int, std::less<int> >(),
//                                       boost::multi_index::identity<int>(), std::less<int>());

//    BOOST_CHECK_EQUAL(ret.first, my_searcher.root().end());
//    BOOST_CHECK(ret.second.first);
//    //BOOST_CHECK_EQUAL(ret.first.m_pos, 1);
//    c2 = my_searcher.insert(c, 18); //so where does that go?
//    //BOOST_CHECK_EQUAL(c2.parent(), c1);
//    //BOOST_CHECK_EQUAL(c1.end(), c2);
//    //BOOST_CHECK(c2 != my_searcher.root().end());
//
//    BOOST_CHECK_EQUAL(*c2, 18);
//    BOOST_CHECK(!c2.has_child());
//
//    BOOST_CHECK_EQUAL(c2, my_searcher.root().end().begin());
//    //BOOST_CHECK_EQUAL(c2.m_parent, c.m_parent);
//    //BOOST_CHECK_EQUAL(c2, c.m_parent);
//    //BOOST_CHECK(c1.has_child());  ///also fails!!!
//
//
//
//    c2 = my_searcher.insert(c, 31);
//
//    c2 = my_searcher.insert(c, 412);
//    c3 = my_searcher.insert(c, 39);
//
//    c4 = my_searcher.insert(c, 7);
//
//    BOOST_CHECK_EQUAL(*(c2.parent()), 31);
//    BOOST_CHECK_EQUAL(*(c2.begin()), 39);
//
//    //BOOST_CHECK_EQUAL(c4.parent(), c1);
//
//    BOOST_CHECK_EQUAL(*c3, 39);
//    BOOST_CHECK_EQUAL(c4.parent(), c1);
//    BOOST_CHECK_EQUAL(c1.begin(), c4);

    //*(mytree.root()) = -5;
    //c = my_searcher.root();
    //my_searcher.c();
//    //*c = 5;
////    //BOOST_CHECK_EQUAL(*c, 5);
////
//    c1 = mytree.insert(c, 1);
//    BOOST_CHECK_EQUAL(*c, 1);
//    c2 = mytree.insert(c1, 2);
//    BOOST_CHECK_EQUAL(*c1, 1);
//    BOOST_CHECK_EQUAL(*c2, 2);

//    *c1 = 14; //how can we forbid this? by setting key to int const&
//    BOOST_CHECK_EQUAL(*c1, 14);
//    BOOST_CHECK_EQUAL(*c2, 2);
//    BOOST_CHECK_EQUAL(c2.parent(), c1);
//    BOOST_CHECK_EQUAL(c1.parent(), c);
//
//    c3 = c1.end();
//    --c3;
//    BOOST_CHECK_EQUAL(*c3, 2);
//    ++c3;
//    c4 = mytree.insert(c3, 4);
//    BOOST_CHECK_EQUAL(*c4, 4);
//    c4 = c4.parent();
//    --c4;
//    BOOST_CHECK_EQUAL(*c4, 2);
//
//    //BOOST_CHECK_EQUAL(c4.parent(), c1);

}