#include <cmath>

#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "std_msgs/Bool.h"
#include "vetexcanopen.h"
#include <boost/thread.hpp>

static const double SCALE_MAX_X = 100.0f;
static const double SCALE_MIN_X = 10.0f;
static const double SCALE_MAX_Y = 100.0f;
static const double SCALE_MIN_Y = 10.0f;
static const double SCALE_MAX_R = 100.0f;
static const double SCALE_MIN_R = 10.0f;
static const double THROTLE_SCALE_FACTOR = 40.0f;
static const double MAX_ACC = 100.0/0.025; //0->100 in 0.25 sec
static const double MAX_DECEL = 0.5 * MAX_ACC; //should be able to decelerate faster (maybe)
static const double RAMP_TIME_INC = 0.020; //time step for incrementing velocity (sec)
//static const double RAMP_TIME = 0.25f;
//static const int RAMP_INCREMENTS = 10;
//static const double STOP_RAMP_TIME = 0.25f;
//static const int STOP_RAMP_INCREMENTS = 10;

class VetexDriver
{
public:
  VetexDriver():
    enabled_(false),
    throtle_scale_factor_(THROTLE_SCALE_FACTOR)
  {

  }

  ~VetexDriver()
  {

  }

  bool run()
  {
    ros::NodeHandle ph("~");
    ros::NodeHandle nh;

	  twist_sub_ = nh.subscribe("vetexvels", 1, &VetexDriver::twistCallback,this);
	  enable_sub_ = nh.subscribe("vetexenable", 1000, &VetexDriver::enableCallback,this);

    if(init())
    {
      ROS_INFO_STREAM("Vetex driver initialized");
    }
    else
    {
      ROS_ERROR_STREAM("Vetex driver failed to initialize, aborting");
      vetex_terminate();
      return false;
    }

    ros::Duration loop_duration(0.1f);
    setEnable(true);
	  while (ros::ok())
    {
		  ros::spinOnce();
      loop_duration.sleep();
	  }

    setEnable(false);
    ros::Duration(1.0f).sleep();
	  vetex_terminate();

    return true;
  }

protected:

  bool init()
  {
	  // Initialize the vetex can stuff.
    ros::NodeHandle n("~");

	  n.param("max_speed_x", max_speed_x_, 1.0); // m/sec
	  n.param("min_speed_x", min_speed_x_, 0.01);// m/sec
	  n.param("max_speed_y", max_speed_y_, 1.0); // m/sec
	  n.param("min_speed_y", min_speed_y_, 0.01);// m/sec
	  n.param("max_speed_r", max_speed_r_, 0.5); // m/sec
	  n.param("min_speed_r", min_speed_r_, 0.05);// m/sec
    n.param("throtle_scale_factor", throtle_scale_factor_, THROTLE_SCALE_FACTOR);// m/sec
    //n.param("ramp_time",ramp_time_,RAMP_TIME); // secs
    //n.param("ramp_increments",num_ramp_increments_,RAMP_INCREMENTS); // int
    n.param("max_acc", max_acc_, MAX_ACC);
    n.param("max_decel", max_decel_, MAX_DECEL);
    n.param("ramp_time_inc", ramp_time_incr_, RAMP_TIME_INC);

    ROS_INFO_STREAM("Driver Parameters :\n"<<
      "\n\tmax_speed_x: "<<max_speed_x_<<
      "\n\tmax_speed_y: "<<max_speed_y_<<
      "\n\tmax_speed_r: "<<max_speed_r_<<
      "\n\tmin_speed_x: "<<min_speed_x_<<
      "\n\tmin_speed_y: "<<min_speed_y_<<
      "\n\tmin_speed_r: "<<min_speed_r_<<
      "\n\tmax_acc: "<<max_acc_<<
      "\n\tmax_decel: " << max_decel_ <<
      "\n\tramp_time_inc: "<<ramp_time_incr_<<"\n");

    current_twist_.linear.x = 0;
    current_twist_.linear.y = 0;
    current_twist_.angular.z = 0;

    vetex_initialize();

    //signal(SIGINT,&VetexDriver::stopOnShutdown,this);

    return true;
  }

  void setEnable(bool enable)
  {

    geometry_msgs::Twist target_twist;
    if(enable)
    {
      vetex_enable_movement();
      ros::Duration(0.5f).sleep();
      vetex_set_all_percentages(0, 0, 0);
      current_twist_.linear.x = 0;
      current_twist_.linear.y = 0;
      current_twist_.angular.z = 0;

      ROS_INFO_STREAM("Movement enabled");
      enabled_ = true;
    }
    else
    {

      target_twist.linear.x = 0;
      target_twist.linear.y = 0;
      target_twist.angular.z = 0;

      rampToSpeed(target_twist);
      vetex_set_all_percentages(0, 0, 0);
      ros::Duration(0.2f).sleep();
      vetex_disable_movement();

      ROS_INFO_STREAM("Movement disabled");
      enabled_ = false;
    }

  }

  void stopOnShutdown(int sig)
  {
    ROS_WARN_STREAM("Vetex stopOnShutdown(...) invoked");
  }

  void twistCallback(const geometry_msgs::Twist::ConstPtr& msg)
  {
	  ROS_DEBUG("Received %f, %f, %f",msg->linear.x, msg->linear.y,msg->angular.z);

	  if ( ! enabled_)
    {
      ROS_WARN_STREAM("Movement is currently disabled");
	  }

	  // Scale parameters are stored in the ros parameter server.
	  // The scaling factor will be a multiplication that changes
	  // velocity into a percentage from -100 to 100.
	  // They are retrieved just before the spinonce in the main function.

    //xf,yf, rf - are base velocities in the base coordiate system (as specified in the URDF)
    double xf = computeScaleFactor(SCALE_MAX_X,SCALE_MIN_X, max_speed_x_,min_speed_x_,msg->linear.x);
    double yf = computeScaleFactor(SCALE_MAX_Y,SCALE_MIN_Y, max_speed_y_,min_speed_y_,msg->linear.y);
    double rf = computeScaleFactor(SCALE_MAX_R,SCALE_MIN_R, max_speed_r_,min_speed_r_, msg->angular.z); // rotation is flipped
    ROS_DEBUG_STREAM("Scale factors(in URDF frame) x: "<<xf<<" y: "<<yf<<" r: "<<rf);

    // rxf, ryf, rrf - are reoriented base velocites as assumed by the controller(note,
    // the rotation orientation is flipped because the controller is not a
    // right-handed frame - this is probably due to mimicing the hand control outputs which
    // aren't really a coordinate frame)
    double rxf = yf;
    double ryf = -xf;
    double rrf = -rf;
    ROS_WARN_STREAM("Received twist "<<msg->linear.x <<", "<<msg->linear.y <<", "<<msg->angular.z );
    ROS_WARN_STREAM("Reoriented factors(in base controller 'frame') x: "<<rxf<<" y: "<<ryf<<" r: "<<rrf);

    geometry_msgs::Twist target_twist;
    target_twist.linear.x = rxf;
    target_twist.linear.y = ryf;
    target_twist.angular.z = rrf;

    rampToSpeed(target_twist);

	  //vetex_set_all_percentages(rxf, ryf, rrf);
  }

  bool rampToSpeed(const geometry_msgs::Twist& target_twist)
  {
    ros::Rate r(1.0/ramp_time_incr_);
    bool x_ramp_done, y_ramp_done, z_ramp_done;
     x_ramp_done = y_ramp_done = z_ramp_done = false;

    geometry_msgs::Twist ramp_speed = current_twist_;

    ROS_WARN_STREAM("Ramping to twist "<<target_twist.linear.x <<", "<<target_twist.linear.y <<", "<<target_twist.angular.z );
    while(true)
    {
      x_ramp_done = rampAxis(target_twist.linear.x, current_twist_.linear.x, ramp_speed.linear.x);
      y_ramp_done = rampAxis(target_twist.linear.y, current_twist_.linear.y, ramp_speed.linear.y);
      z_ramp_done = rampAxis(target_twist.angular.z, current_twist_.angular.z, ramp_speed.angular.z);

      ROS_WARN_STREAM("Ramping speed: " << ramp_speed.linear.x << ", " << ramp_speed.linear.y << ", " << ramp_speed.angular.z );
      vetex_set_all_percentages(ramp_speed.linear.x, ramp_speed.linear.y, ramp_speed.angular.z);

      if(x_ramp_done && y_ramp_done && z_ramp_done)
      {
        break;
      }
      else
      {
        r.sleep();
      }
    }

    ROS_WARN_STREAM("Reached speeds: " << ramp_speed.linear.x << ", " << ramp_speed.linear.y << ", " << ramp_speed.angular.z );

    // saving current twist
    current_twist_ = target_twist;

    return true;
  }

  void enableCallback(const std_msgs::Bool::ConstPtr& msg)
  {
	  ROS_INFO("Received %d", msg->data);
    setEnable(msg->data);
  }

  double computeScaleFactor(const double &max_factor,const double &min_factor,
    const double &max_speed,const double &min_speed,
    const double &command_speed)
  {
    double af = std::abs(command_speed);
    int sign = command_speed < 0 ? -1 : 1;
    if(af < min_speed)
    {
      af = 0;
    }
    else if(af > max_speed)
    {
      af = throtle_scale_factor_;
    }
    else
    {
      af = max_factor - ((max_factor - min_factor)*(max_speed - af))/(max_speed - min_speed);
      af = (af > throtle_scale_factor_) ? throtle_scale_factor_ : af;
    }

    return af*sign;
  }

  bool rampAxis(double target, double current, double &ramp)
  {
    bool done_ramping = false;

    if( target > current ) //moving positive
    {
      if ( ramp < 0 ) //decelerating to zero
      {
        ramp = ramp + max_decel_ * ramp_time_incr_;
      }
      else
      {
        ramp = ramp + max_acc_ * ramp_time_incr_;
      }
      if( ramp > target)
      {
        ramp = target;
        done_ramping = true;
      }
    }
    else //moving negative
    {
      if ( ramp  > 0 )
      {
        ramp = ramp - max_decel_ * ramp_time_incr_;
      }
      else
      {
        ramp = ramp - max_acc_ * ramp_time_incr_;
      }
      if( ramp < target )
      {
        ramp = target;
        done_ramping = true;
      }
    }
    return done_ramping;
  }

protected:

  ros::Subscriber twist_sub_;
  ros::Subscriber enable_sub_;

  bool enabled_;
  double max_speed_x_;
  double min_speed_x_;
  double max_speed_y_;
  double min_speed_y_;
  double max_speed_r_;
  double min_speed_r_;
  double throtle_scale_factor_;
  //double ramp_time_;
  //int num_ramp_increments_;
  double max_acc_, max_decel_, ramp_time_incr_;

  geometry_msgs::Twist current_twist_;

};

int main (int argc, char **argv)
{
  ros::init(argc, argv, "vetex_driver");
  ros::NodeHandle nh;
  VetexDriver vd;
  if(vd.run())
  {
    return 0;
  }
  else
  {
    return -1;
  }

}


#include <boost/serialization/export.hpp>
#include "LocallyConnected2D.hpp"
#include "LayerModel.hpp"

using namespace std;
using namespace snn;
using namespace internal;

BOOST_CLASS_EXPORT(LocallyConnected2D)

LocallyConnected2D::LocallyConnected2D(LayerModel& model, shared_ptr<NeuralNetworkOptimizer> optimizer)
    : FilterLayer(model, optimizer)
{
    const int restX = shapeOfInput[0] % this->sizeOfFilterMatrix == 0 ? 0 : 1;
    const int restY = shapeOfInput[1] % this->sizeOfFilterMatrix == 0 ? 0 : 1;

    this->shapeOfOutput = {
        this->shapeOfInput[0] / this->sizeOfFilterMatrix + restX,
        this->shapeOfInput[1] / this->sizeOfFilterMatrix + restY,
        this->numberOfFilters
    };
    this->sizeOfNeuronInputs = this->sizeOfFilterMatrix * this->sizeOfFilterMatrix * this->shapeOfInput[2];
    this->neuronInputs.resize(this->sizeOfNeuronInputs);
}

inline
unique_ptr<BaseLayer> LocallyConnected2D::clone(std::shared_ptr<NeuralNetworkOptimizer> optimizer) const
{
    auto layer = make_unique<LocallyConnected2D>(*this);
    for (int n = 0; n < layer->getNumberOfNeurons(); ++n)
    {
        layer->neurons[n].setOptimizer(optimizer);
    }
    return layer;
}

int LocallyConnected2D::isValid() const
{
    for (auto& neuron : neurons)
    {
        if (neuron.getNumberOfInputs() != this->sizeOfNeuronInputs)
            return 203;
    }
    return this->FilterLayer::isValid();
}

inline
vector<float> LocallyConnected2D::createInputsForNeuron(const int neuronIndex, const vector<float>& inputs)
{
    const int neuronPosX = neuronIndex % this->shapeOfOutput[0] * this->sizeOfFilterMatrix;
    const int neuronPosY = neuronIndex / this->shapeOfOutput[0] * this->sizeOfFilterMatrix;

    for (int i = 0; i < this->sizeOfFilterMatrix; ++i)
    {
        const int beginIndex = ((neuronPosY + i) * this->shapeOfInput[0] + neuronPosX) * this->shapeOfInput[2];
        for (int j = 0; j < this->sizeOfFilterMatrix; ++j)
        {
            for (int k = 0; k < this->shapeOfInput[2]; ++k)
            {
                const int indexErrors = beginIndex + (j * this->shapeOfInput[2] + k);
                const int indexMatrix = (i * this->sizeOfFilterMatrix + j) * this->shapeOfInput[2] + k;
                if (indexErrors < (int)inputs.size()) [[likely]]
                    this->neuronInputs[indexMatrix] = inputs[indexErrors];
                else
                    neuronInputs[indexMatrix] = 0.0f;
            }
        }
    }
    return this->neuronInputs;
}

inline
void LocallyConnected2D::insertBackOutputForNeuron(const int neuronIndex, const std::vector<float>& error, std::vector<float>& errors)
{
    const int neuronPosX = neuronIndex % this->shapeOfOutput[0] * this->sizeOfFilterMatrix;
    const int neuronPosY = neuronIndex / this->shapeOfOutput[0] * this->sizeOfFilterMatrix;

    for (int i = 0; i < this->sizeOfFilterMatrix; ++i)
    {
        const int beginIndex = ((neuronPosY + i) * this->shapeOfInput[0] + neuronPosX) * this->shapeOfInput[2];
        for (int j = 0; j < this->sizeOfFilterMatrix; ++j)
        {
            for (int k = 0; k < this->shapeOfInput[2]; ++k)
            {
                const int indexErrors = beginIndex + (j * this->shapeOfInput[2] + k);
                const int indexMatrix = (i * this->sizeOfFilterMatrix + j) * this->shapeOfInput[2] + k;
                if (indexErrors < (int)errors.size()) [[likely]]
                    errors[indexErrors] += error[indexMatrix];
            }
        }
    }
}

inline
bool LocallyConnected2D::operator==(const BaseLayer& layer) const
{
    try
    {
        const auto& f = dynamic_cast<const LocallyConnected2D&>(layer);
        return this->FilterLayer::operator==(layer)
            && this->sizeOfNeuronInputs == f.sizeOfNeuronInputs
            && this->neuronInputs == f.neuronInputs;
    }
    catch (bad_cast&)
    {
        return false;
    }
}

inline
bool LocallyConnected2D::operator!=(const BaseLayer& layer) const
{
    return !(*this == layer);
}


//==============================================================================
//         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
//==============================================================================
#define NT2_UNIT_MODULE "nt2 operator toolbox - make/simd Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of operator components in simd mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 18/02/2011
///
#include <boost/simd/operator/include/functions/make.hpp>
#include <boost/simd/sdk/simd/native.hpp>

#include <boost/type_traits/is_same.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/include/constants/real.hpp>
#include <boost/simd/include/constants/infinites.hpp>
#include <boost/simd/sdk/memory/is_aligned.hpp>
#include <boost/simd/sdk/memory/aligned_type.hpp>
#include <boost/simd/include/functions/load.hpp>

template<class T> T fibo(std::size_t n)
{
  if(n == 0) return 0;
  if(n == 1) return 1;
  return fibo<T>(n-1) + fibo<T>(n-2);
}

#define M1(z, n, t) fibo<T>(n)
#define M0(z, n, t)                                                            \
template<class T>                                                              \
struct make_fibo<T, n>                                                         \
{                                                                              \
  typedef boost::simd::native<T, BOOST_SIMD_DEFAULT_EXTENSION> type;           \
  static type call()                                                           \
  {                                                                            \
    return boost::simd::make<type>(BOOST_PP_ENUM(n, M1, ~));                   \
  }                                                                            \
};                                                                             \
/**/
template<class T, std::size_t N = boost::simd::native<T, BOOST_SIMD_DEFAULT_EXTENSION>::static_size>
struct make_fibo;
BOOST_SIMD_PP_REPEAT_POWER_OF_2(M0, ~)
#undef M0
#undef M1

NT2_TEST_CASE_TPL ( make_fibonnaci,  BOOST_SIMD_SIMD_TYPES)
{
  typedef boost::simd::native<T, BOOST_SIMD_DEFAULT_EXTENSION> vT;
  vT x = make_fibo<T>::call();
  for(std::size_t i=0; i!=boost::simd::meta::cardinal_of<vT>::value; ++i)
    NT2_TEST_EQUAL(x[i], fibo<T>(i));
}


#include <frovedis.hpp>
#include <frovedis/matrix/blockcyclic_matrix.hpp>

#define BOOST_TEST_MODULE FrovedisTest
#include <boost/test/unit_test.hpp>

using namespace frovedis;
using namespace std;

bool check(blockcyclic_matrix_local<float>& m) {
  return m.is_valid();
}

BOOST_AUTO_TEST_CASE( frovedis_test )
{
    int argc = 1;
    char** argv = NULL;
    use_frovedis use(argc, argv);

    // creating blockcyclic matrix loading data from sample text file
    auto bm = make_blockcyclic_matrix_load<float> ("./sample_4x4");

    // checking the validity of the created blockcyclic matrix
    BOOST_CHECK (bm.data.map(check).get(0));

    // transposing the matrix and checking its dimension
    auto t_bm = bm.transpose();
    BOOST_CHECK (t_bm.get_nrows() == bm.get_ncols());
    BOOST_CHECK (t_bm.get_ncols() == bm.get_nrows());

    // transposing back and saving in text file
    t_bm.transpose().save("./tt_bm");

    // checking whether saved file have same data as with loaded file
    // this will confirm whether transpose operation was successful
    BOOST_CHECK (system("diff ./sample_4x4 ./tt_bm") == 0);

    // deleting the saved file
    system("rm -f ./tt_bm");
}


#ifndef TEST_UNIT_MATH_REV_MAT_VECTORIZE_EXPECT_REV_VECTOR_VALUE_HPP
#define TEST_UNIT_MATH_REV_MAT_VECTORIZE_EXPECT_REV_VECTOR_VALUE_HPP

#include <stan/math/rev/core/var.hpp>
#include <Eigen/Dense>
#include <vector>
#include <gtest/gtest.h>
#include <test/unit/math/rev/mat/vectorize/build_rev_matrix.hpp>
#include <test/unit/math/rev/mat/vectorize/expect_val_deriv_eq.hpp>

template <typename F>
void expect_rev_vector_value() {
  using stan::math::var;
  using std::vector;
  typedef Eigen::Matrix<var, Eigen::Dynamic, 1> VectorXvar;

  size_t num_inputs = F::valid_inputs().size();
  VectorXvar template_vector(num_inputs);

  for (size_t i = 0; i < num_inputs; ++i) {
    VectorXvar b = build_rev_matrix<F>(template_vector);
    VectorXvar c = build_rev_matrix<F>(template_vector);
    VectorXvar fc = F::template apply<VectorXvar>(c);
    EXPECT_EQ(c.size(), fc.size());
    expect_val_deriv_eq(F::apply_base(b(i)), b(i), fc(i), c(i));
  }

  size_t vector_vector_size = 2;
  for (size_t i = 0; i < vector_vector_size; ++i) {
    for (size_t j = 0; j < num_inputs; ++j) {
      vector<VectorXvar> d;
      vector<VectorXvar> e;
      for (size_t k = 0; k < num_inputs; ++k) {
        d.push_back(build_rev_matrix<F>(template_vector));
        e.push_back(build_rev_matrix<F>(template_vector));
      }
      vector<VectorXvar> fe = F::template apply<vector<VectorXvar> >(e);
      EXPECT_EQ(e[i].size(), fe[i].size());
      EXPECT_EQ(e.size(), fe.size());
      expect_val_deriv_eq(F::apply_base(d[i](j)), d[i](j),
                          fe[i](j), e[i](j));
    }
  }
}

#endif


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<boost::int64_t, 5, 5> test_long_5_5(const Eigen::Matrix<boost::int64_t, 5, 5> & M)
{
	return M;
}
void export_long_5_5()
{
	boost::python::def("test_long_5_5",test_long_5_5);
}


#include <Rcpp.h>
#include <boost/tokenizer.hpp>
#include <boost/algorithm/string.hpp>
#include <algorithm>
#include <string>
#include <unordered_map>
#ifdef _OPENMP
  #include <omp.h>
#endif
#include <vector>

// [[Rcpp::depends(BH)]]
// [[Rcpp::plugins(openmp)]]

std::vector<std::string> vec_intersect(const std::vector<std::string> &v1,
                                       const std::vector<std::string> &v2){
    std::vector<std::string> v3;

    std::set_intersection(v1.begin(),v1.end(),
                          v2.begin(),v2.end(),
                          std::back_inserter(v3));
    return v3;
}

std::vector<std::string> vec_union(const std::vector<std::string> &v1,
                                   const std::vector<std::string> &v2){
    std::vector<std::string> v3;

    std::set_union(v1.begin(),v1.end(),
                   v2.begin(),v2.end(),
                   std::back_inserter(v3));

    std::vector<std::string>::iterator it;
    it = std::unique(v3.begin(), v3.end());
    v3.resize(std::distance(v3.begin(),it));

    return v3;
}

double weight_lookup_sum(const std::vector<std::string>& words,
                         const std::unordered_map<std::string, double>& weight_lookup)
{
    if(words.empty()){
        return 0;
    }

    auto sum_weights = [&](double cumulative_sum, const std::string& word) {
        return cumulative_sum + weight_lookup.at(word);
    };

    double wgt_intersection = std::accumulate(
                    std::next(words.cbegin()),
                    words.cend(),
                    weight_lookup.at(words[0]),
                    sum_weights);

    return wgt_intersection;
}

double jaccard_score(const std::vector<std::string>& words_x,
                     const std::vector<std::string>& words_y,
                     const std::unordered_map<std::string, double>& weight_lookup){
    auto word_intersection = vec_intersect(words_x, words_y);
    if(word_intersection.empty()){
        return 0;
    }
    auto word_union = vec_union(words_x, words_y);

    //auto Rprint = [](const std::string& s) { Rcpp::Rcout << " " << s; };
    //Rcpp::Rcout << "\n Intersect:";
    //Std::for_each(word_intersection.cbegin(), word_intersection.cend(), Rprint);
    //Rcpp::Rcout << "\n Union:";
    //Std::for_each(word_union.cbegin(), word_union.cend(), Rprint);

    return weight_lookup_sum(word_intersection, weight_lookup) /
        weight_lookup_sum(word_union, weight_lookup);
}

// the following line is not actually a comment, it is interpreted by Rcpp
// [[Rcpp::export]]
Rcpp::List wgt_jaccard(
        Rcpp::StringVector corpus,
        Rcpp::NumericVector wgts,
        Rcpp::StringVector x,
        Rcpp::StringVector y,
        int nthreads
        ) {

//    Rcpp::Rcout << "\n\n        nthreads: " << nthreads << "\n";
    const bool DEBUG = false;

    #ifdef _OPENMP
        omp_set_num_threads(nthreads);
    #endif

    using tokenizer = boost::tokenizer<boost::char_separator<char> >;
    const boost::char_separator<char> sep(" ");

    std::unordered_map<std::string, double> weight_lookup;
    for(int i=0; i < corpus.size(); i++){
        const auto corpus_element = static_cast<std::string>(corpus(i));
        weight_lookup[corpus_element] = wgts[i];
    }

    std::vector<std::vector<std::string> > y_tokenized(y.size());

    #pragma omp parallel for schedule(dynamic)
    for(int i=0; i < y.size(); i++){
        std::string s{y(i)};
        tokenizer tokens(s, sep);
        // create vector to store words
        auto y_token_vector = std::vector<std::string>(tokens.begin(), tokens.end());
        std::sort(y_token_vector.begin(), y_token_vector.end());
        y_token_vector.erase(unique(y_token_vector.begin(), y_token_vector.end() ), y_token_vector.end() ) ;
        y_tokenized[i] = y_token_vector;
    }

    //auto Rprint = [](const std::string& s) { Rcpp::Rcout << " " << s; };

    std::vector<unsigned int> match_indices(x.size());
    std::vector<double> match_similarity(x.size());

    #pragma omp parallel for schedule(dynamic)
    for(int i=0; i < x.size(); i++){
        // iterate over company names in X
        if (DEBUG){
            Rcpp::Rcout << "i is: " << i << ", the element value is: " << x(i);
            Rcpp::Rcout << "\n";
        }

        // split the company name
        std::string s{x(i)};
        tokenizer tokens(s, sep);
        // create vector to store words
        auto x_tok_vec = std::vector<std::string>(tokens.begin(), tokens.end());
        std::sort(x_tok_vec.begin(), x_tok_vec.end());
        x_tok_vec.erase(unique(x_tok_vec.begin(), x_tok_vec.end() ), x_tok_vec.end() ) ;

        std::vector<double> y_scores(y.size());
        for(int j=0; j < y.size(); j++){
        //for(auto& y_tok_vec : y_tokenized) {

            auto xy_score = jaccard_score(x_tok_vec, y_tokenized[j], weight_lookup);
            y_scores[j] = xy_score;

            /*
            if (DEBUG){
                Rcpp::Rcout << "   j is: " << j << ", the element value is: ";
                std::for_each(y_tokenized[j].cbegin(), y_tokenized[j].cend(), Rprint);
                Rcpp::Rcout << "\n";
                Rcpp::Rcout << "        Intersection:\n          "; std::for_each(word_intersection.cbegin(), word_intersection.cend(), Rprint);
                Rcpp::Rcout << "\n\n        Union:\n          ";
                std::for_each(word_union.cbegin(), word_union.cend(), Rprint);

                Rcpp::Rcout << "\n\n        Score: " << xy_score << "\n";
                Rcpp::Rcout << "\n";
                Rcpp::Rcout << "\n";
            }
            */
        }
        const auto result = std::max_element(y_scores.cbegin(), y_scores.cend());
        const auto max_element_index = std::distance(y_scores.cbegin(), result);

        match_indices[i] = max_element_index;
        match_similarity[i] = y_scores[max_element_index];
        if (DEBUG){
            Rcpp::Rcout << "max element at: " << std::distance(y_scores.cbegin(), result) << '\n';
        }
    }

    Rcpp::NumericVector ret_indexes = Rcpp::NumericVector(
            match_indices.cbegin(),
            match_indices.cend());

    Rcpp::NumericVector ret_similarity = Rcpp::NumericVector(
            match_similarity.cbegin(),
            match_similarity.cend());

    return Rcpp::List::create(Rcpp::Named("index") = ret_indexes,
                              Rcpp::Named("similarity") = ret_similarity);
}

// the following line is not actually a comment, it is interpreted by Rcpp
// [[Rcpp::export]]
Rcpp::List wgt_jaccard_single(
    Rcpp::StringVector corpus,
    Rcpp::NumericVector wgts,
    Rcpp::StringVector x,
    Rcpp::StringVector y,
    int nthreads
) {
  //    Rcpp::Rcout << "\n\n        nthreads: " << nthreads << "\n";
  const bool DEBUG = false;
#ifdef _OPENMP
  omp_set_num_threads(nthreads);
#endif

  using tokenizer = boost::tokenizer<boost::char_separator<char> >;
  const boost::char_separator<char> sep(" ");

  std::unordered_map<std::string, double> weight_lookup;
  for(int i=0; i < corpus.size(); i++){
    const auto corpus_element = static_cast<std::string>(corpus(i));
    weight_lookup[corpus_element] = wgts[i];

  }

  std::vector<double> match_similarity(x.size());
  #pragma omp parallel for schedule(dynamic)
  for(int i=0; i < x.size(); i++){
    // iterate over company names in X
    if (DEBUG){
      Rcpp::Rcout << "i is: " << i << ", the x element value is: " << x(i);
      Rcpp::Rcout << "\n";
      Rcpp::Rcout << "i is: " << i << ", the y element value is: " << y(i);
      Rcpp::Rcout << "\n";
      Rcpp::Rcout << "length of x: " << x.size();
      Rcpp::Rcout << "\n";
      Rcpp::Rcout << "length of y: " << y.size();
      Rcpp::Rcout << "\n";
    }

    // split the company name
    std::string sx{x(i)};
    tokenizer tokens_x(sx, sep);

    // create vector to store words
    auto x_tok_vec = std::vector<std::string>(tokens_x.begin(), tokens_x.end());

    std::sort(x_tok_vec.begin(), x_tok_vec.end());
    x_tok_vec.erase(unique(x_tok_vec.begin(), x_tok_vec.end() ), x_tok_vec.end() );
    // split the company name
    std::string sy{y(i)};
    tokenizer tokens_y(sy, sep);
    // create vector to store words
    auto y_tok_vec = std::vector<std::string>(tokens_y.begin(), tokens_y.end());
    std::sort(y_tok_vec.begin(), y_tok_vec.end());
    y_tok_vec.erase(unique(y_tok_vec.begin(), y_tok_vec.end() ), y_tok_vec.end() );
    auto xy_score = jaccard_score(x_tok_vec, y_tok_vec, weight_lookup);

    match_similarity[i] = xy_score;
  }

  Rcpp::NumericVector ret_similarity = Rcpp::NumericVector(
    match_similarity.cbegin(),
    match_similarity.cend());

  return Rcpp::List::create(Rcpp::Named("similarity") = ret_similarity);
}


#ifndef COMMONTOOLS_HH
#define COMMONTOOLS_HH

#ifdef PINOCCHIO
#include "pinocchio/parsers/urdf.hpp"
#endif
#include <boost/filesystem.hpp>
#include <boost/date_time/gregorian/gregorian.hpp>
#include <sstream>
#include <fstream>
#include <cmath>

typedef boost::filesystem::path path_t;
typedef std::list<path_t> result_set_t;
#ifdef PINOCCHIO
typedef Eigen::Matrix<double,6,1> Vector6d ;
#endif
int lowpass(double x, double &y, double i, double a);
int derivation(const std::vector<std::vector<double> > &x,
               std::vector<std::vector<double> > &dx);
int integration( const std::vector< std::vector<double> > & dx,
                 std::vector< std::vector<double> > & x);

struct dumpData{
  int dump(std::string &fileName, std::vector< std::vector<double> >& data);
  int dump(std::string &fileName, std::vector< int >& data);
#ifdef PINOCCHIO
  int dump(std::string &fileName, std::vector< se3::SE3 >& data) ;
  int dump(std::string &fileName,
           std::vector<Eigen::MatrixXd,
                       Eigen::aligned_allocator<Eigen::MatrixXd> > &data) ;
  int dump(std::string &fileName,
           std::vector<Vector6d, Eigen::aligned_allocator<Vector6d> > &data) ;
#endif
  template<typename Matrix>
  int dump(std::ofstream &dumpStream, Matrix & data) ;
};

void getOptions(int argc,
                char *argv[],
                path_t &dataRootPath,
                path_t &robotRootPath,
#ifdef PINOCCHIO
                path_t & robotUrdfPath,
#endif
                path_t &outputFile);

class InfiniteImpendanceFilter
{
public :
  InfiniteImpendanceFilter()
  {
    // Create the window for the filter.
    m_SamplingPeriod = 0.005 ;
    double T=0.05; // Arbritrary from Kajita's San Matlab files.
    int n=0;
    double sum=0,tmp=0;

    assert(m_SamplingPeriod > 0);
    n = (int)floor(T/m_SamplingPeriod);
    m_filterWindow.resize(n+1);
    for(int i=0;i<n+1;i++)
    {
      tmp =sin((M_PI*i)/n);
      m_filterWindow[i]=tmp*tmp;
    }

    for(int i=0;i<n+1;i++)
      sum+= m_filterWindow[i];

    for(int i=0;i<n+1;i++)
      m_filterWindow[i]/= sum;
  }

  void filter(const std::vector< std::vector<double> > & in,
              std::vector< std::vector<double> > & out)
  {
    out = in ;
    for(unsigned j=0 ; j<in[0].size() ; ++j)
    {
      int n=0;
      double T=0.050; // Arbritraty fixed from Kajita's San matlab files.

      assert(out.size()==in.size()&&out[0].size()==in[0].size());

      // Creates window.
      assert(m_SamplingPeriod > 0);
      n = (int)floor(T/m_SamplingPeriod);

      // Filter vector.
      for(int i=0;i<n+1;i++)
      {
        out[i][j] = in[i][j];
      }

      for(unsigned int i=n+1;i<in.size();i++)
      {
        double ltmp = 0.0 ;
        for(unsigned int k=0;k<m_filterWindow.size();k++)
        {
          ltmp += m_filterWindow[k]*in[i-k][j];
        }
        out[i][j] = ltmp ;
      }
    }
    std::vector< std::vector<double> > out_shifted = out ;
    shift_trajectory(out_shifted,out,-5);
  }

  void shift_trajectory(const std::vector< std::vector<double> > & in,
                        std::vector< std::vector<double> > & out,
                        int shift=0)
  {
    out = in ;
    shift *= -1 ;
    for(unsigned j=0 ; j<out[0].size() ; ++j)
    {
      for(int i=0 ; i<out.size() ; ++i)
      {
        int is = i + shift;
        if (is < 0)
          is = 0;
        if (is >= out.size())
          is = out.size() - 1 ;

        out[i] = in[is] ;
      }
    }
  }

#ifdef PINOCCHIO
  void filter(std::vector< Eigen::Matrix<double,6,1> ,
                           Eigen::aligned_allocator<Eigen::Matrix<double, 6, 1> > > & in,
              std::vector< Eigen::Matrix<double,6,1> ,
                           Eigen::aligned_allocator<Eigen::Matrix<double, 6, 1> > > & out)
  {
    std::vector< std::vector<double> > in_vec ( in.size() , std::vector<double>(6,0.0) );
    std::vector< std::vector<double> > out_vec ( out.size() , std::vector<double>(6,0.0) );
    for(unsigned i=0 ; i<in.size() ; ++i)
    {
      for(unsigned j=0 ; j<6 ; ++j)
        in_vec[i][j] = in[i](j) ;
    }
    this->filter(in_vec,out_vec);
    for(unsigned i=0 ; i<in.size() ; ++i)
    {
      for(unsigned j=0 ; j<6 ; ++j)
        out[i](j) = out_vec[i][j] ;
    }
    return ;
  }
  #endif
private :
  std::vector<double> m_filterWindow;
  double m_SamplingPeriod ;
};

#endif // COMMONTOOLS_HH


//---------------------------------------------------------------------------//
//!
//! \file   tstAtomUnit.cpp
//! \author Alex Robinson
//! \brief  The atom unit unit tests
//!
//---------------------------------------------------------------------------//

// Boost Includes
#include <boost/units/quantity.hpp>
#include <boost/units/systems/si/amount.hpp>

// Trilinos Includes
#include <Teuchos_UnitTestHarness.hpp>

// FRENSIE Includes
#include "Utility_AtomUnit.hpp"
#include "Utility_PhysicalConstants.hpp"

using namespace Utility::Units;
using boost::units::quantity;
namespace si = boost::units::si;

//---------------------------------------------------------------------------//
// Tests
//---------------------------------------------------------------------------//
// Check that the atom unit can be initialized
TEUCHOS_UNIT_TEST( AtomUnit, initialize )
{
  quantity<Atom> amount( 1.0*atom );

  TEST_EQUALITY_CONST( amount.value(), 1.0 );

  amount = 2.0*atom;

  TEST_EQUALITY_CONST( amount.value(), 2.0 );
}

//---------------------------------------------------------------------------//
// Check that the atom unit can be initialized from si units
TEUCHOS_UNIT_TEST( AtomUnit, initialize_from_si )
{
  quantity<Atom> amount( 1.0*si::mole );

  TEST_FLOATING_EQUALITY( amount.value(),
			  Utility::PhysicalConstants::avogadro_constant,
			  1e-15 );
}

//---------------------------------------------------------------------------//
// Check that the si units can be initialized from the atom unit
TEUCHOS_UNIT_TEST( AtomUnit, initialize_si )
{
  quantity<si::amount> amount( 1.0*atom );

  TEST_FLOATING_EQUALITY( amount.value(),
			  1.0/Utility::PhysicalConstants::avogadro_constant,
			  1e-15 );
}

//---------------------------------------------------------------------------//
// end tstAtomUnit.cpp
//---------------------------------------------------------------------------//


/* CSVReader.cpp
 * -*- coding: utf-8 -*-
 *
 */

#include <boost/test/unit_test.hpp>

#include <stdexcept>

#include <boost/filesystem.hpp>

#include <Eigen/Dense>

// local includes ==============================================================

#include "../src/utils/CSVReader.hpp"

#include "utils/log.hpp"

BOOST_AUTO_TEST_SUITE(CSVReaderTestSuite)
    struct Setup {
        Setup() { reader = CSVReader(); }

        CSVReader reader;

        std::string test_file = "test/resources/csvreader/matrix.csv";
    };

    BOOST_AUTO_TEST_CASE(read_csv) {
        TEST_MESSAGE("read_csv");

        Setup f;

        Eigen::MatrixXd expected(2,2);

        expected(0,0) = 0.0000000000000000;
        expected(0,1) = 1101.5312671722938376;
        expected(1,0) = 3.8522202257825890;
        expected(1,1) = 1119.2784683923078774;

        Eigen::MatrixXd actual = f.reader.read_matrix(f.test_file, 2);

        BOOST_REQUIRE_EQUAL(expected, actual);
    }

    BOOST_AUTO_TEST_CASE(throw_if_file_nonexistent) {
        TEST_MESSAGE("throw_if_file_nonexistent");

        Setup f;

        BOOST_REQUIRE_THROW(f.reader.read_matrix(boost::filesystem::path("test/resources/csvreader/nonexistent_file.csv"), 2),
                            std::runtime_error);
    }

BOOST_AUTO_TEST_SUITE_END()

// vim: tabstop=8 expandtab shiftwidth=4 softtabstop=4 fenc=utf-8


//Copyright (c) 2015 Zachary Kann
//
//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.

// ---
// Author: Zachary Kann

// This class is for dealing with complex time-correlation functions (TCFs)
// and their fourier transforms.

#include <armadillo>
#include <cassert>
#include "z_tcf.hpp"
#include "z_constants.hpp"

#ifndef _Z_CX_TCF_HPP_
#define _Z_CX_TCF_HPP_

class CxTCF:
  public TCF<arma::cx_double> {
 public:
  inline CxTCF(int length, int x_spacing, int interval = 1, int zeros = 0)
      : TCF(length, x_spacing, interval, zeros),
        spec_length_(2*total_length()-2) {
    // TODO(Zak): I think this should be -1 not -2... test that.
    correlation_function_.zeros(spec_length_);
  }

  // The standard call to prep and run a fourier transform. Assumes symmetry
  // around t = 0.
  inline void FourierPlus() {
    for (int i = 0; i<length(); ++i) {
      correlation_function_(i) -= correlation_function_(length()-1);
    }
    for (int i = length()+2*zeros()-1; i<spec_length_; i++) {
      correlation_function_(i) =
        std::conj(correlation_function_(spec_length_-i));
    }
    fourier_transform_ = fft(correlation_function_);
    freq_spacing_nu_ = 1.0/2.0/C_SPEED/(x_spacing()*total_length());
    freq_spacing_omega_ = freq_spacing_nu_*2.0*M_PI*C_SPEED;
  }

  // Similar to FourierPlus, but without the assumption of symmetry
  // around t = 0.
  inline void FourierNoSymmPlus() {
    for (int i = 0; i<length(); ++i) {
      correlation_function_(i) -= correlation_function_(length()-1);
    }
    fourier_transform_ = fft(correlation_function_);
    freq_spacing_nu_ = 1.0/2.0/C_SPEED/(x_spacing()*total_length());
    freq_spacing_omega_ = freq_spacing_nu_*2.0*M_PI*C_SPEED;
  }

  // Similar to FourierPlus, but doesn't shift the time correlation function
  // to be zero after corr steps. Formerly called run_fftw.
  inline void Fourier() {
    for (int i = length()+2*zeros()-1; i<spec_length_; i++) {
      correlation_function_(i) =
        std::conj(correlation_function_(spec_length_-i));
    }
    fourier_transform_ = fft(correlation_function_);
    freq_spacing_nu_ = 1.0/2.0/C_SPEED/(x_spacing()*total_length());
    freq_spacing_omega_ = freq_spacing_nu_*2.0*M_PI*C_SPEED;
  }

  inline void MultiplyFT(const double multiplier) {
    fourier_transform_ *= multiplier;
  }

  inline void FreqWeighting(const double power) {
    for (int i = 0; i < spec_length_; ++i) {
      fourier_transform_(i) *= std::pow(i*freq_spacing_omega_, power);
    }
  }

  inline void PrintFTReal(const std::string& filename, double x_shift = 0.0) {
    std::ofstream output_file(filename.c_str());
    assert(output_file.is_open());
    for (int i = total_length(); i < spec_length_; ++i) {
      output_file << (i-spec_length_)*freq_spacing_nu_ + x_shift << " " <<
                     real(fourier_transform_(i)) << std::endl;
    }
    for (int i = 0; i < total_length(); ++i) {
      output_file << i*freq_spacing_nu_ + x_shift << " " <<
                     real(fourier_transform_(i)) << std::endl;
    }
    output_file.close();
  }

  // Mutators
  inline void set_ft(const int i, const arma::cx_double data) {
    fourier_transform_(i) = data;
  }

  // Accessors
  inline int spec_length() const { return spec_length_; }
  inline double freq_spacing_nu() const { return freq_spacing_nu_; }
  inline double freq_spacing_omega() const { return freq_spacing_omega_; }
  inline arma::cx_double ft(int i) const { return fourier_transform_(i); }

 private:
  arma::cx_rowvec fourier_transform_;
  int spec_length_;
  double freq_spacing_nu_, freq_spacing_omega_;
};
#endif


#pragma once

#include <string>
#include <ros/ros.h>

#include <Eigen/Dense>
#include <memory> // for make_unique
#include "2dplane/GridStateSpace.hpp"
#include "BiRRT.hpp"
#include "2dplane/2dplane.hpp"

class PlannerWrapper
{
public:
    PlannerWrapper();
    void step();

    std::shared_ptr<RRT::GridStateSpace> statespace_;
    std::unique_ptr<RRT::BiRRT<Eigen::Vector2d>> birrt_;

    Eigen::Vector2d start_velocity_;
    Eigen::Vector2d goal_velocity_;
    std::vector<Eigen::Vector2d> previous_solution_;
};


#include "RBGL.hpp"
#include <boost/graph/bandwidth.hpp>
#include <boost/graph/profile.hpp>
#include <boost/graph/wavefront.hpp>

extern "C"
{

	SEXP BGL_bandwidth(SEXP num_verts_in, SEXP num_edges_in, SEXP R_edges_in )
	{
		using namespace boost;

		Graph_ud g(num_verts_in, num_edges_in, R_edges_in);

		SEXP ansList, rbw;
		PROTECT(ansList = Rf_allocVector(VECSXP,1));
		PROTECT(rbw = Rf_allocVector(INTSXP, 1));

		INTEGER(rbw)[0] = bandwidth(g);

		SET_VECTOR_ELT(ansList,0,rbw);
		UNPROTECT(2);
		return(ansList);
	}

	SEXP BGL_profile(SEXP num_verts_in, SEXP num_edges_in, SEXP R_edges_in )
	{
		using namespace boost;

		Graph_ud g(num_verts_in, num_edges_in, R_edges_in);

		SEXP ansList, rbw;
		PROTECT(ansList = Rf_allocVector(VECSXP,1));
		PROTECT(rbw = Rf_allocVector(INTSXP, 1));

		INTEGER(rbw)[0] = profile(g);

		SET_VECTOR_ELT(ansList,0,rbw);
		UNPROTECT(2);
		return(ansList);
	}

	SEXP BGL_ith_wavefront(SEXP num_verts_in, SEXP num_edges_in,
                         SEXP R_edges_in,   SEXP init_ind)
	{
		using namespace boost;

		Graph_ud g(num_verts_in, num_edges_in, R_edges_in);

		SEXP ansList, rbw;
		PROTECT(ansList = Rf_allocVector(VECSXP,1));
		PROTECT(rbw = Rf_allocVector(INTSXP, 1));

		INTEGER(rbw)[0] = ith_wavefront(vertex((int)INTEGER(init_ind)[0], g), g);

		SET_VECTOR_ELT(ansList,0,rbw);
		UNPROTECT(2);
		return(ansList);
	}

	SEXP BGL_max_wavefront(SEXP num_verts_in, SEXP num_edges_in,
                         SEXP R_edges_in)
	{
		using namespace boost;

		Graph_ud g(num_verts_in, num_edges_in, R_edges_in);

		SEXP ansList, rbw;
		PROTECT(ansList = Rf_allocVector(VECSXP,1));
		PROTECT(rbw = Rf_allocVector(INTSXP, 1));

		INTEGER(rbw)[0] = max_wavefront(g);

		SET_VECTOR_ELT(ansList,0,rbw);
		UNPROTECT(2);
		return(ansList);
	}

	SEXP BGL_aver_wavefront(SEXP num_verts_in, SEXP num_edges_in,
                         SEXP R_edges_in)
	{
		using namespace boost;

		Graph_ud g(num_verts_in, num_edges_in, R_edges_in);

		SEXP ansList, rbw;
		PROTECT(ansList = Rf_allocVector(VECSXP,1));
		PROTECT(rbw = Rf_allocVector(REALSXP, 1));

		REAL(rbw)[0] = aver_wavefront(g);

		SET_VECTOR_ELT(ansList,0,rbw);
		UNPROTECT(2);
		return(ansList);
	}

	SEXP BGL_rms_wavefront(SEXP num_verts_in, SEXP num_edges_in,
                         SEXP R_edges_in)
	{
		using namespace boost;

		Graph_ud g(num_verts_in, num_edges_in, R_edges_in);

		SEXP ansList, rbw;
		PROTECT(ansList = Rf_allocVector(VECSXP,1));
		PROTECT(rbw = Rf_allocVector(REALSXP, 1));

		REAL(rbw)[0] = rms_wavefront(g);

		SET_VECTOR_ELT(ansList,0,rbw);
		UNPROTECT(2);
		return(ansList);
	}
}


#include "domain/finite_element/finite_element_gaussian.h"

#include <deal.II/fe/fe_dgq.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_update_flags.h>

namespace bart {

namespace domain {

namespace finite_element {

template <int dim>
FiniteElementGaussian<dim>::FiniteElementGaussian(DiscretizationType discretization,
                                  int polynomial_degree)
    : polynomial_degree_(polynomial_degree) {

  description_ = "(Default) deal.II Gaussian, " + std::to_string(dim) + "D, ";

  const auto update_flags = dealii::update_values | dealii::update_gradients |
                            dealii::update_quadrature_points |
                            dealii::update_JxW_values;

  const auto face_update_flags =
      dealii::update_values | dealii::update_gradients |
      dealii::update_quadrature_points |
      dealii::update_JxW_values | dealii::update_normal_vectors;

  finite_element_ = GetFiniteElement(discretization);

  cell_quadrature_ =
      std::make_shared<dealii::QGauss<dim>>(polynomial_degree + 1);

  face_quadrature_ =
      std::make_shared<dealii::QGauss<dim - 1>>(polynomial_degree + 1);

  values_ = std::make_shared<dealii::FEValues<dim>>(*finite_element_,
                                                    *cell_quadrature_,
                                                    update_flags);
  face_values_ = std::make_shared<dealii::FEFaceValues<dim>>(*finite_element_,
                                                             *face_quadrature_,
                                                             face_update_flags);

  if (discretization == DiscretizationType::kDiscontinuousFEM) {
    neighbor_face_values_ =
        std::make_shared<dealii::FEFaceValues<dim>>(*finite_element_,
                                                    *face_quadrature_,
                                                    face_update_flags);
    description_ += "Discontinuous, ";
  } else {
    description_ += "Continuous, ";
  }
  description_ += "Q = " + std::to_string(polynomial_degree);
}

template <int dim>
std::shared_ptr<dealii::FiniteElement<dim, dim>>
FiniteElementGaussian<dim>::GetFiniteElement(DiscretizationType discretization) {
  switch (discretization) {

    case DiscretizationType::kContinuousFEM: {
      return std::make_shared<dealii::FE_Q<dim>>(polynomial_degree_);
      break;
    }

    case DiscretizationType::kDiscontinuousFEM:{
      return std::make_shared<dealii::FE_DGQ<dim>>(polynomial_degree_);
      break;
    }

    default: {
      AssertThrow(false,
                  dealii::ExcMessage("Cannot build FiniteElementGaussian object with discretization type None"));
      break;
    }
  }
}

template class FiniteElementGaussian<1>;
template class FiniteElementGaussian<2>;
template class FiniteElementGaussian<3>;

} // namespace finite_element

} // namespace domain

} // namespace bart


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Wikipedia. http://en.wikipedia.org/wiki/Neptune, last accessed: 27 January, 2012(a).
 *      Wikipedia. http://en.wikipedia.org/wiki/Mile, last accessed: 27 January, 2012(b).
 *      Wikipedia. http://en.wikipedia.org/wiki/Atmospheric_pressure,
 *          last accessed: 27 January, 2012(c).
 *      Wikipedia. http://en.wikipedia.org/wiki/Temperature_conversion_formulas,
 *          last accessed: 27, January 2012(d).
 *
 *    Notes
 *      At the moment, not all conversion routines are tested both ways. This should be corrected
 *      in an update version.
 *
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <cmath>
#include <limits>

#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include "tudat/astro/basic_astro/physicalConstants.h"
#include "tudat/astro/basic_astro/unitConversions.h"

namespace tudat
{
namespace unit_tests
{

//! Test suit for unit conversions.
BOOST_AUTO_TEST_SUITE( test_unit_conversions )

//! Test conversion from kilometers to meters.
BOOST_AUTO_TEST_CASE( testConversionFromKilometersToMeters )
{
    BOOST_CHECK_CLOSE_FRACTION( unit_conversions::
                                convertKilometersToMeters( 1.0e6 ),
                                1.0e6 * 1.0e3, std::numeric_limits< double >::epsilon( ) );
}

//! Test conversion from degrees to radians.
BOOST_AUTO_TEST_CASE( testConversionFromDegreesToRadians )
{
    BOOST_CHECK_CLOSE_FRACTION( unit_conversions::
                                convertDegreesToRadians( 45.0 ),
                                mathematical_constants::PI / 4.0,
                                std::numeric_limits< double >::epsilon( ) );
}

//! Test conversion from degrees to arcminutes.
BOOST_AUTO_TEST_CASE( testConversionFromDegreesToArcminutes )
{
    BOOST_CHECK_CLOSE_FRACTION( unit_conversions::
                                convertDegreesToArcminutes( 43.2 ),
                                43.2 * 60.0, std::numeric_limits< double >::epsilon( ) );
}

//! Test conversion from arcminutes to arcseconds.
BOOST_AUTO_TEST_CASE( testConversionFromArcminutesToArcSeconds )
{
    BOOST_CHECK_CLOSE_FRACTION( unit_conversions::
                                convertArcminutesToArcseconds( 125.9 ),
                                125.9 * 60.0, std::numeric_limits< double >::epsilon( ) );
}

//! Test conversion from astronomical units to meters.
BOOST_AUTO_TEST_CASE( testConversionFromAstronomicalUnitsToMeters )
{
    // Case: Neptune's semi-major axis (Wikipedia, 2012a).
    BOOST_CHECK_CLOSE_FRACTION( unit_conversions::
                                convertAstronomicalUnitsToMeters( 30.10366151 ),
                                4.503443661e+12, 1.0e-9 );
}

//! Test conversion from minutes to seconds.
BOOST_AUTO_TEST_CASE( testConversionFromMinutesToSeconds )
{
    BOOST_CHECK_CLOSE_FRACTION( unit_conversions::
                                convertMinutesToSeconds( 12.0 ),
                                12.0 * 60.0, std::numeric_limits< double >::epsilon( ) );
}

//! Test conversion from seconds to minutes.
BOOST_AUTO_TEST_CASE( testConversionFromSecondsToMinutes )
{
    BOOST_CHECK_CLOSE_FRACTION( unit_conversions::
                                convertSecondsToMinutes( 12.0 ),
                                0.2, std::numeric_limits< double >::epsilon( ) );
}

//! Test conversion from hours to Julian years.
BOOST_AUTO_TEST_CASE( testConversionFromHoursToJulianYears )
{
    using namespace unit_conversions;

    BOOST_CHECK_CLOSE_FRACTION( convertJulianDaysToJulianYears(
                                    convertSecondsToJulianDays(
                                        convertHoursToSeconds( 24.0 ) ) ),
                                1.0 / 365.25, std::numeric_limits< double >::epsilon( ) );
}

//! Test conversion from Julian years to hours.
BOOST_AUTO_TEST_CASE( testConversionFromJulianYearsToHours )
{
    using namespace unit_conversions;

    BOOST_CHECK_CLOSE_FRACTION( convertSecondsToHours(
                                    convertJulianDaysToSeconds(
                                        convertJulianYearsToJulianDays(
                                            1.0 / 365.25 ) ) ),
                                24.0, std::numeric_limits< double >::epsilon( ) );
}

//! Test conversion from sidereal days to seconds.
BOOST_AUTO_TEST_CASE( testConversionFromSiderealDaysToSeconds )
{
    BOOST_CHECK_CLOSE_FRACTION( unit_conversions::
                                convertSiderealDaysToSeconds( 7.0 ),
                                7.0 * physical_constants::SIDEREAL_DAY,
                                std::numeric_limits< double >::epsilon( ) );
}

//! Test conversion from seconds to sidereal days.
BOOST_AUTO_TEST_CASE( testConversionFromSecondsToSiderealDays )
{
    BOOST_CHECK_CLOSE_FRACTION( unit_conversions::
                                convertSecondsToSiderealDays( 100.0 ),
                                100.0
                                / physical_constants::SIDEREAL_DAY,
                                std::numeric_limits< double >::epsilon( ) );
}

//! Test conversion of temperature in Rankine to Kelvin.
BOOST_AUTO_TEST_CASE( testConversionFromRankineToKelvin )
{
    // Case: 0 deg Celcius (Wikipedia, 2011d).
    BOOST_CHECK_CLOSE_FRACTION( unit_conversions::
                                convertRankineToKelvin( 491.67 ),
                                273.15, std::numeric_limits< double >::epsilon( ) );
}

//! Test conversion from distance in feet to meters.
BOOST_AUTO_TEST_CASE( testConversionFromFeetToMeters )
{
    // Case: length of a statute mile (Wikipedia, 2011b).
    BOOST_CHECK_CLOSE_FRACTION( unit_conversions::
                                convertFeetToMeter( 5280.0 ),
                                1609.344, std::numeric_limits< double >::epsilon( ) );
}

//! Test conversion from pounds-per-square-feet to Pascal.
BOOST_AUTO_TEST_CASE( testConversionFromPoundsPerSquareFeetToPascal )
{
    // Case: atmospheric pressure at sea level (Wikipedia, 2011c).
    BOOST_CHECK_CLOSE_FRACTION(
                unit_conversions::
                convertPoundPerSquareFeetToPascal( 2116.21662367394 ),
                101325.0, 1.0e-9 );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


// Eigen includes
#include <Eigen/Core>

// STL includes
#include <iostream>
#include <vector>

// Library include
#include <Utils.hpp>
#include "Tests.hpp"

/* Check if clamping a polyong works */
int CheckPolygonClamping() {

   int nb_fails = 0;
   PolygonConstructor pConstruct;
   Polygon polygon;
   Vector A, B, C;

   // Create a polygon above the horizon
   A = Vector(0,0,1);
   B = Vector(0,1,1);
   C = Vector(1,0,1);
   pConstruct = PolygonConstructor(Vector::Normalize(A), Vector::Normalize(B), Vector::Normalize(C));
   polygon = pConstruct.ProjectToHemisphere(Vector(0,0,0), Vector(0,0,1));
   if(polygon.size() != 3) {
      std::cerr << "Error: projecting canonical triangle doesn't provide the same triangle" << std::endl;
      std::cout << polygon << std::endl;
      ++nb_fails;
   }

   // Create a polygon above the horizon
   A = Vector(0,0,1);
   B = Vector(0,1,0);
   C = Vector(1,0,0);
   pConstruct = PolygonConstructor(A, B, C);
   polygon = pConstruct.ProjectToHemisphere(Vector(0,0,0), Vector(0,0,1));
   if(polygon.size() != 3) {
      std::cerr << "Error: projecting canonical triangle doesn't provide the same triangle" << std::endl;
      std::cout << polygon << std::endl;
      ++nb_fails;
   }

   // Create a polygon below the horizon
   A = Vector(0,0,1);
   B = Vector(0,1,1);
   C = Vector(1,0,1);
   pConstruct = PolygonConstructor(Vector::Normalize(A), Vector::Normalize(B), Vector::Normalize(C));
   polygon = pConstruct.ProjectToHemisphere(Vector(0,0,0), Vector(0,0,-1));
   if(polygon.size() != 0) {
      std::cerr << "Error: projecting canonical triangle should return nothing" << std::endl;
      std::cout << polygon << std::endl;
      ++nb_fails;
   }

   // Create a polygon crossing the horizon
   A = Vector(0, 0, 1);
   B = Vector(0, 1, 1);
   C = Vector(1, 0,-1);
   pConstruct = PolygonConstructor(Vector::Normalize(A), Vector::Normalize(B), Vector::Normalize(C));
   polygon = pConstruct.ProjectToHemisphere(Vector(0,0,0), Vector(0,0,1));
   if(polygon.size() != 4) {
      std::cerr << "Error: projecting canonical crossing triangle should return a 4-polygon" << std::endl;
      std::cout << polygon << std::endl;
      ++nb_fails;
   }

   // Create a polygon crossing the horizon
   A = Vector(0, 0, 1);
   B = Vector(0, 1, 0);
   C = Vector(1, 0,-1);
   pConstruct = PolygonConstructor(Vector::Normalize(A), Vector::Normalize(B), Vector::Normalize(C));
   polygon = pConstruct.ProjectToHemisphere(Vector(0,0,0), Vector(0,0,1));
   if(polygon.size() != 3) {
      std::cerr << "Error: projecting canonical crossing triangle should return a 3-polygon" << std::endl;
      std::cout << polygon << std::endl;
      ++nb_fails;
   }

   // Create a polygon crossing the horizon
   A = Vector(0, 0, 1);
   B = Vector(0, 1, 0);
   C = Vector(1, 0,-1);
   pConstruct = PolygonConstructor(Vector::Normalize(A), Vector::Normalize(B), Vector::Normalize(C));
   pConstruct.push_back(Vector::Normalize(Vector(0,0,-1)));
   pConstruct.push_back(Vector::Normalize(Vector(1,0,-1)));
   polygon = pConstruct.ProjectToHemisphere(Vector(0,0,0), Vector(0,0,1));
   if(polygon.size() != 3) {
      std::cerr << "Error: projecting canonical crossing triangle should return a 3-polygon" << std::endl;
      std::cout << polygon << std::endl;
      ++nb_fails;
   }
   return nb_fails;
}

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

   int nmats = 5;
   int ncols = 10;
   int nrows = 15;

   // Create matrices
   std::vector<Eigen::MatrixXf> matrices;
   for(int n=0; n<nmats; ++n) {
      Eigen::MatrixXf matrix = Eigen::MatrixXf::Random(ncols, nrows);
      matrices.push_back(matrix);
   }

   // Save the matrices
   SaveMatrices("test.mats", matrices);

   // Load them and compare
   std::vector<Eigen::MatrixXf> read_mats = LoadMatrices("test.mats");

   int nb_fails = 0;
   for(int i=0; i<nmats; ++i) {
      if(!matrices[i].isApprox(read_mats[i])) {
         ++nb_fails;
      }
   }

   try {
      auto test = LoadMatrices("inexistant_file_on_disk.mats");
   } catch(IOError& error) {
      std::cerr << "Correct: unable to open file (testing exceptions)" << std::endl;
      std::cerr << error.what() << std::endl;
   }

   /* Test the polygon projection code */
   nb_fails += CheckPolygonClamping();

   if(nb_fails > 0) {
      std::cerr << "Failure!" << std::endl;
      return EXIT_FAILURE;
   } else {
      std::cerr << "Success!" << std::endl;
      return EXIT_SUCCESS;
   }
}


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published
// by the Free Software Foundation, either version 3 of the License,
// or (at your option) any later version.
//
// FlexibleSUSY is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

#ifndef WRAPPERS_H
#define WRAPPERS_H

#include <algorithm>
#include <cmath>
#include <complex>
#include <functional>
#include <limits>
#include <numeric>
#include <iostream>
#include <string>
#include <type_traits>
#include <utility>
#include <vector>
#include <Eigen/Core>
#include <boost/lexical_cast.hpp>

#include "config.h"
#include "dilog.hpp"
#include "error.hpp"
#include "eigen_tensor.hpp"
#include "error.hpp"
#include "if.hpp"
#include "numerics2.hpp"
#include "sum.hpp"
#include "which.hpp"

namespace flexiblesusy {

static constexpr double Pi = M_PI;
static constexpr double oneOver16PiSqr = 1./(16. * Pi * Pi);
static constexpr double oneLoop = oneOver16PiSqr;
static constexpr double twoLoop = oneOver16PiSqr * oneOver16PiSqr;
static constexpr double threeLoop = oneOver16PiSqr * oneOver16PiSqr * oneOver16PiSqr;
static constexpr bool True = true;

template <typename T>
T Abs(T a) noexcept
{
   return std::abs(a);
}

template <typename T>
T Abs(const std::complex<T>& z) noexcept
{
   return std::abs(z);
}

template <typename Scalar, int M, int N>
Eigen::Array<Scalar, M, N> Abs(const Eigen::Array<Scalar, M, N>& a)
{
   return a.cwiseAbs();
}

template <typename Scalar, int M, int N>
Eigen::Matrix<Scalar, M, N> Abs(const Eigen::Matrix<Scalar, M, N>& a)
{
   return a.cwiseAbs();
}

template <class T>
std::vector<T> Abs(std::vector<T> v) noexcept
{
   for (auto& e: v)
      e = Abs(e);
   return v;
}

inline constexpr double AbsSqr(double z) noexcept
{
   return z * z;
}

inline double AbsSqr(const std::complex<double>& z) noexcept
{
   return std::norm(z);
}

inline double AbsSqrt(double x) noexcept
{
   return std::sqrt(std::fabs(x));
}

template <typename Derived>
Derived AbsSqrt(const Eigen::MatrixBase<Derived>& m)
{
   return m.cwiseAbs().cwiseSqrt();
}

template <typename Derived>
Derived AbsSqrt(const Eigen::ArrayBase<Derived>& m)
{
   return m.cwiseAbs().cwiseSqrt();
}

/**
 * Calculates the mass of a singlet from a (possibly complex)
 * numerical value by taking the magnitude of the value.
 *
 * @param value numerical value
 * @return mass
 */
template <typename T>
double calculate_singlet_mass(T value) noexcept
{
   return std::abs(value);
}

/**
 * Calculates the mass of a Majoran fermion singlet from a (possibly
 * complex) numerical value by taking the magnitude of the value.
 *
 * The phase is set to exp(i theta/2), where theta is the phase angle
 * of the complex value.  If the value is pure real, then the phase
 * will be set to 1.  If the value is purely imaginary, then the phase
 * will be set to \f$e^{i \pi/2}\f$.
 *
 * @param value numerical value
 * @param[out] phase phase
 * @return mass
 */
template <typename T>
double calculate_majorana_singlet_mass(T value, std::complex<double>& phase)
{
   phase = std::polar(1., 0.5 * std::arg(std::complex<double>(value)));
   return std::abs(value);
}

/**
 * Calculates the mass of a Dirac fermion singlet from a (possibly
 * complex) numerical value by taking the magnitude of the value.
 *
 * The phase is set to exp(i theta), where theta is the phase angle of
 * the complex value.  If the value is pure real, then the phase will
 * be set to 1.  If the value is purely imaginary, then the phase will
 * be set to \f$e^{i \pi}\f$.
 *
 * @param value numerical value
 * @param[out] phase phase
 * @return mass
 */
template <typename T>
double calculate_dirac_singlet_mass(T value, std::complex<double>& phase)
{
   phase = std::polar(1., std::arg(std::complex<double>(value)));
   return std::abs(value);
}

inline double ArcTan(double a) noexcept
{
   return std::atan(a);
}

inline double ArcSin(double a) noexcept
{
   return std::asin(a);
}

inline double ArcCos(double a) noexcept
{
   return std::acos(a);
}

inline double Arg(const std::complex<double>& z) noexcept
{
   return std::arg(z);
}

template <typename T>
constexpr T Cbrt(T a) noexcept
{
   return std::cbrt(a);
}

inline constexpr double Conj(double a) noexcept
{
   return a;
}

inline std::complex<double> Conj(const std::complex<double>& a) noexcept
{
   return std::conj(a);
}

template <class T>
T Conjugate(T a) noexcept
{
   return Conj(a);
}

template <typename T>
constexpr T Cube(T a) noexcept
{
   return a * a * a;
}

template <typename T>
T Exp(T z) noexcept
{
   return std::exp(z);
}

inline double Tan(double a) noexcept
{
   return std::tan(a);
}

inline double Cot(double a) noexcept
{
   return 1./Tan(a);
}

inline double Cos(double x) noexcept
{
   return std::cos(x);
}

inline double Sin(double x) noexcept
{
   return std::sin(x);
}

inline double Sec(double x) noexcept
{
   return 1./Cos(x);
}

inline double Csc(double x) noexcept
{
   return 1./Sin(x);
}

inline constexpr int Delta(int i, int j) noexcept
{
   return i == j;
}

#define FSFlagProblem(p) [&](){ (p); return 0.; }()
#define FSFlagWarning(p) [&](){ (p); return 0.; }()

template <typename T>
constexpr T If(bool c, T a, T b) noexcept { return c ? a : b; }

template <typename T>
constexpr T If(bool c, int a, T b)  noexcept{ return c ? T(a) : b; }

template <typename T>
constexpr T If(bool c, T a, int b) noexcept { return c ? a : T(b); }

inline bool IsClose(double a, double b,
                    double eps = std::numeric_limits<double>::epsilon()) noexcept
{
   return std::abs(a - b) < eps;
}

inline bool IsCloseRel(double a, double b,
                       double eps = std::numeric_limits<double>::epsilon()) noexcept
{
   if (IsClose(a, b, std::numeric_limits<double>::epsilon()))
      return true;

   if (std::abs(a) < std::numeric_limits<double>::epsilon())
      return IsClose(a, b, eps);

   return std::abs((a - b)/a) < eps;
}

inline bool IsFinite(double x) noexcept
{
   return std::isfinite(x);
}

inline bool IsFinite(const std::complex<double>& x) noexcept
{
   return std::isfinite(x.real()) && std::isfinite(x.imag());
}

template <class Derived>
bool IsFinite(const Eigen::DenseBase<Derived>& m)
{
  // workaround for intel compiler / eigen bug that causes
  // unexpected behavior from allFinite
  int nr = m.rows();
  int nc = m.cols();
  bool allfinite = true;
  for (int r = 0; r<nr; ++r) {
    for (int c = 0; c<nc; ++c) {
      if ( !std::isfinite( m(r,c) ) ) allfinite = false;
    }
  }

  return allfinite;
  //   return m.allFinite();
}

inline constexpr int KroneckerDelta(int i, int j) noexcept
{
   return i == j;
}

template <class Derived>
typename Eigen::MatrixBase<Derived>::PlainObject Diag(const Eigen::MatrixBase<Derived>& m) noexcept
{
   static_assert(Eigen::MatrixBase<Derived>::RowsAtCompileTime ==
                 Eigen::MatrixBase<Derived>::ColsAtCompileTime,
                 "Diag is only defined for squared matrices");

   typename Eigen::MatrixBase<Derived>::PlainObject diag(m);

   for (int i = 0; i < Eigen::MatrixBase<Derived>::RowsAtCompileTime; ++i)
      for (int k = i + 1; k < Eigen::MatrixBase<Derived>::ColsAtCompileTime; ++k)
         diag(i,k) = 0.0;

   for (int i = 0; i < Eigen::MatrixBase<Derived>::RowsAtCompileTime; ++i)
      for (int k = 0; k < i; ++k)
         diag(i,k) = 0.0;

   return diag;
}

inline std::complex<double> ComplexLog(double a) noexcept
{
   return fast_log(std::complex<double>(a,0.));
}

inline std::complex<double> ComplexLog(const std::complex<double>& z) noexcept
{
   return fast_log(z);
}

inline double FiniteLog(double a) noexcept
{
   const double l = std::log(a);
   return std::isfinite(l) ? l : 0.;
}

/**
 * Fills lower triangle of hermitian matrix from values
 * in upper triangle.
 *
 * @param m matrix
 */
template <typename Derived>
void Hermitianize(Eigen::MatrixBase<Derived>& m) noexcept
{
   static_assert(Eigen::MatrixBase<Derived>::RowsAtCompileTime ==
                 Eigen::MatrixBase<Derived>::ColsAtCompileTime,
                 "Hermitianize is only defined for squared matrices");

   for (int i = 0; i < Eigen::MatrixBase<Derived>::RowsAtCompileTime; i++)
      for (int k = 0; k < i; k++)
         m(i,k) = Conj(m(k,i));
}

///////////////////////// logger commands /////////////////////////

namespace {
inline double PrintTo(std::ostream& ostr)
{
   ostr << '\n';
   return 0.;
}
} // anonymous namespace

///< print information to ostr
template<typename T0, typename... Ts>
double PrintTo(std::ostream& ostr, T0&& v, Ts&&... vs)
{
   ostr << v;
   return PrintTo(ostr, std::forward<Ts>(vs)...);
}

///< print debug information to cerr
template<typename... Ts>
double PrintDEBUG(Ts&&... vs)
{
#ifdef ENABLE_DEBUG
   return PrintTo(std::cerr, std::forward<Ts>(vs)...);
#else
   return 0.;
#endif
}

///< print error to cerr
template<typename... Ts>
double PrintERROR(Ts&&... vs)
{
   std::cerr << "ERROR: ";
   return PrintTo(std::cerr, std::forward<Ts>(vs)...);
}

///< print error to cerr and stop program
template<typename... Ts>
double PrintFATAL(Ts&&... vs)
{
   std::cerr << "FATAL: ";
   PrintTo(std::cerr, std::forward<Ts>(vs)...);
   throw FatalError();
   return 0.;
}

///< print information to cerr
template<typename... Ts>
double PrintINFO(Ts&&... vs)
{
   return PrintTo(std::cerr, std::forward<Ts>(vs)...);
}

///< print verbose information to cerr
template<typename... Ts>
double PrintVERBOSE(Ts&&... vs)
{
#ifdef ENABLE_VERBOSE
   return PrintTo(std::cerr, std::forward<Ts>(vs)...);
#else
   return 0.;
#endif
}

///< print warning to cerr
template<typename... Ts>
double PrintWARNING(Ts&&... vs)
{
   std::cerr << "WARNING: ";
   return PrintTo(std::cerr, std::forward<Ts>(vs)...);
}

///////////////////////// end of logger commands /////////////////////////

inline double Log(double a) noexcept
{
   return std::log(a);
}

double MaxRelDiff(double, double);
double MaxRelDiff(const std::complex<double>&, const std::complex<double>&);

template <class Derived>
double MaxRelDiff(const Eigen::MatrixBase<Derived>& a,
                  const Eigen::MatrixBase<Derived>& b)
{
   typename Eigen::MatrixBase<Derived>::PlainObject sumTol(a.rows());

   if (a.rows() != b.rows())
      throw SetupError("MaxRelDiff: vectors have different size!");

   for (int i = 0; i < a.rows(); i++)
      sumTol(i) = MaxRelDiff(a(i), b(i));

   return sumTol.maxCoeff();
}

template <class Derived>
double MaxRelDiff(const Eigen::ArrayBase<Derived>& a,
                  const Eigen::ArrayBase<Derived>& b)
{
   return MaxRelDiff(a.matrix(), b.matrix());
}

inline double MaxAbsValue(double x) noexcept
{
   return Abs(x);
}

inline double MaxAbsValue(const std::complex<double>& x) noexcept
{
   return Abs(x);
}

template <class Derived>
double MaxAbsValue(const Eigen::MatrixBase<Derived>& x)
{
   return x.cwiseAbs().maxCoeff();
}

template<typename T>
T Max(T&&t)
{
   return std::forward<T>(t);
}

template<typename T0, typename T1, typename... Ts>
typename std::common_type<T0, T1, Ts...>::type Max(T0&& val1, T1&& val2, Ts&&... vs)
{
   if (val2 < val1)
      return Max(val1, std::forward<Ts>(vs)...);
   else
      return Max(val2, std::forward<Ts>(vs)...);
}

template<typename T>
T Min(T&&t)
{
   return std::forward<T>(t);
}

template<typename T0, typename T1, typename... Ts>
typename std::common_type<T0, T1, Ts...>::type Min(T0&& val1, T1&& val2, Ts&&... vs)
{
   if (val2 < val1)
      return Min(val2, std::forward<Ts>(vs)...);
   else
      return Min(val1, std::forward<Ts>(vs)...);
}

inline constexpr int Sign(double x) noexcept
{
   return (x >= 0.0 ? 1 : -1);
}

inline constexpr int Sign(int x) noexcept
{
   return (x >= 0 ? 1 : -1);
}

template <typename T>
constexpr T Quad(T a) noexcept
{
   return a * a * a * a;
}

template <typename T>
T PolyLog(int n, T z) {
   if (n == 2)
      return gm2calc::dilog(z);
   throw SetupError("PolyLog(n!=2) not implemented");
}

template <typename Base, typename Exponent>
Base Power(Base base, Exponent exp) noexcept
{
   return std::pow(base, exp);
}

template <typename Base>
constexpr Base Power2(Base b) noexcept
{
   return b * b;
}

template <typename Base>
constexpr Base Power3(Base b) noexcept
{
   return b * b * b;
}

template <typename Base>
constexpr Base Power4(Base b) noexcept
{
   return b * b * b * b;
}

template <typename Base>
constexpr Base Power5(Base b) noexcept
{
   return b * b * b * b * b;
}

template <typename Base>
constexpr Base Power6(Base b) noexcept
{
   return b * b * b * b * b * b;
}

template <typename Base>
constexpr Base Power7(Base b) noexcept
{
   return b * b * b * b * b *
          b * b;
}

template <typename Base>
constexpr Base Power8(Base b) noexcept
{
   return b * b * b * b * b *
          b * b * b;
}

template <typename Base>
constexpr Base Power9(Base b) noexcept
{
   return b * b * b * b * b *
          b * b * b * b;
}

template <typename Base>
constexpr Base Power10(Base b) noexcept
{
   return b * b * b * b * b *
          b * b * b * b * b;
}

template <typename Base>
constexpr Base Power11(Base b) noexcept
{
   return b * b * b * b * b *
          b * b * b * b * b *
          b;
}

template <typename Base>
constexpr Base Power12(Base b) noexcept
{
   return b * b * b * b * b *
          b * b * b * b * b *
          b * b;
}

inline constexpr double Re(double x) noexcept
{
   return x;
}

inline double Re(const std::complex<double>& x) noexcept
{
   return std::real(x);
}

template<int M, int N>
Eigen::Matrix<double,M,N> Re(const Eigen::Matrix<double,M,N>& x)
{
   return x;
}

template<class Derived>
typename Eigen::Matrix<
   double,
   Eigen::MatrixBase<Derived>::RowsAtCompileTime,
   Eigen::MatrixBase<Derived>::ColsAtCompileTime>
Re(const Eigen::MatrixBase<Derived>& x)
{
   return x.real();
}

inline constexpr double Im(double) noexcept
{
   return 0.;
}

inline double Im(const std::complex<double>& x) noexcept
{
   return std::imag(x);
}

template<int M, int N>
Eigen::Matrix<double,M,N> Im(const Eigen::Matrix<double,M,N>&)
{
   return Eigen::Matrix<double,M,N>::Zero();
}

template<class Derived>
typename Eigen::Matrix<
   double,
   Eigen::MatrixBase<Derived>::RowsAtCompileTime,
   Eigen::MatrixBase<Derived>::ColsAtCompileTime>
Im(const Eigen::MatrixBase<Derived>& x)
{
   return x.imag();
}

template <typename T>
T RelDiff(T a, T b, T eps = std::numeric_limits<T>::epsilon()) noexcept
{
   const T max = std::max(a, b);

   if (std::abs(max) < eps)
      return T();

   return (a - b) / max;
}

inline int Round(double a) noexcept
{
   return static_cast<int>(a >= 0. ? a + 0.5 : a - 0.5);
}

template<int N>
void Sort(Eigen::Array<double, N, 1>& v)
{
   std::sort(v.data(), v.data() + v.size(),
             [] (double a, double b) { return std::abs(a) < std::abs(b); });
}

inline double SignedAbsSqrt(double a) noexcept
{
   return Sign(a) * AbsSqrt(a);
}

template <typename Derived>
Derived SignedAbsSqrt(const Eigen::ArrayBase<Derived>& m)
{
   return m.unaryExpr([](double a) { return SignedAbsSqrt(a); });
}

template <class T, typename = typename std::enable_if<std::is_floating_point<T>::value,T>::type>
T Sqrt(T a) noexcept
{
   return std::sqrt(a);
}

template <class T, typename = typename std::enable_if<std::is_integral<T>::value,T>::type>
double Sqrt(T a) noexcept
{
   return std::sqrt(static_cast<double>(a));
}

template <typename Scalar, int M, int N>
Eigen::Array<Scalar, M, N> Sqrt(const Eigen::Array<Scalar, M, N>& m)
{
   return m.unaryExpr([](Scalar a){ return Sqrt(a); });
}

template <class T>
std::vector<T> Sqrt(std::vector<T> v)
{
   for (auto& e: v)
      e = Sqrt(e);
   return v;
}

template <typename T>
constexpr T Sqr(T a) noexcept
{
   return a * a;
}

template <typename Scalar, int M, int N>
Eigen::Array<Scalar, M, N> Sqr(const Eigen::Array<Scalar, M, N>& a)
{
   return a.unaryExpr([](Scalar a){ return Sqr(a); });
}

template <class T>
std::vector<T> Sqr(std::vector<T> v)
{
   for (auto& e: v)
      e = Sqr(e);
   return v;
}

#define DEFINE_COMMUTATIVE_OPERATOR_COMPLEX_INT(op)                     \
   template <typename T>                                                \
   std::complex<T> operator op(const std::complex<T>& lhs, int rhs)     \
   {                                                                    \
      return lhs op static_cast<T>(rhs);                                \
   }                                                                    \
                                                                        \
   template <typename T>                                                \
   std::complex<T> operator op(int lhs, const std::complex<T>& rhs)     \
   {                                                                    \
      return static_cast<T>(lhs) op rhs;                                \
   }

DEFINE_COMMUTATIVE_OPERATOR_COMPLEX_INT(*)
DEFINE_COMMUTATIVE_OPERATOR_COMPLEX_INT(/)
DEFINE_COMMUTATIVE_OPERATOR_COMPLEX_INT(+)
DEFINE_COMMUTATIVE_OPERATOR_COMPLEX_INT(-)

/**
 * Fills lower triangle of symmetric matrix from values in upper
 * triangle.
 *
 * @param m matrix
 */
template <typename Derived>
void Symmetrize(Eigen::MatrixBase<Derived>& m)
{
   static_assert(Eigen::MatrixBase<Derived>::RowsAtCompileTime ==
                 Eigen::MatrixBase<Derived>::ColsAtCompileTime,
                 "Symmetrize is only defined for squared matrices");

   for (int i = 0; i < Eigen::MatrixBase<Derived>::RowsAtCompileTime; i++)
      for (int k = 0; k < i; k++)
         m(i,k) = m(k,i);
}

#define UNITMATRIX(rows)             Eigen::Matrix<double,rows,rows>::Identity()
#define ZEROMATRIX(rows,cols)        Eigen::Matrix<double,rows,cols>::Zero()
#define ZEROTENSOR3(d1,d2,d3)        ZeroTensor3<double,d1,d2,d3>()
#define ZEROTENSOR4(d1,d2,d3,d4)     ZeroTensor4<double,d1,d2,d3,d4>()
#define ZEROVECTOR(rows)             Eigen::Matrix<double,rows,1>::Zero()
#define ZEROARRAY(rows)              Eigen::Array<double,rows,1>::Zero()
#define UNITMATRIXCOMPLEX(rows)      Eigen::Matrix<std::complex<double>,rows,rows>::Identity()
#define ZEROMATRIXCOMPLEX(rows,cols) Eigen::Matrix<std::complex<double>,rows,cols>::Zero()
#define ZEROVECTORCOMPLEX(rows)      Eigen::Matrix<std::complex<double>,rows,1>::Zero()
#define ZEROTENSOR3COMPLEX(d1,d2,d3) ZeroTensor3<std::complex<double>,d1,d2,d3>()
#define ZEROTENSOR4COMPLEX(d1,d2,d3,d4) ZeroTensor4<std::complex<double>,d1,d2,d3,d4>()
#define ZEROARRAYCOMPLEX(rows)       Eigen::Array<std::complex<double>,rows,1>::Zero()

// MxN matrix projection operator, which projects on the (X,Y)
// component
#define PROJECTOR Proj
#define DEFINE_PROJECTOR(M,N,X,Y)                                       \
   Eigen::Matrix<double,M,N> Proj(Eigen::Matrix<double,M,N>::Zero());   \
   Proj((X)-1,(Y)-1) = 1;

inline double FSThrow(const std::string& s)
{
   throw PhysicalError(s);
   return 0.;
}

template<class Scalar, int M>
Eigen::Matrix<Scalar,M,M> ToMatrix(const Eigen::Array<Scalar,M,1>& a)
{
   return Eigen::Matrix<Scalar,M,M>(a.matrix().asDiagonal());
}

template<class Scalar, int M, int N>
Eigen::Matrix<Scalar,M,N> ToMatrix(const Eigen::Matrix<Scalar,M,N>& a) noexcept
{
   return a;
}

template <typename T>
std::string ToString(T a)
{
   return boost::lexical_cast<std::string>(a);
}

inline double Total(double a) noexcept
{
   return a;
}

inline std::complex<double> Total(const std::complex<double>& a) noexcept
{
   return a;
}

template <class T>
T Total(const std::vector<T>& v)
{
   return std::accumulate(v.begin(), v.end(), T(0));
}

template <typename Scalar, int M, int N>
Scalar Total(const Eigen::Array<Scalar, M, N>& a)
{
   return a.sum();
}

template <typename Scalar, int M, int N>
Scalar Total(const Eigen::Matrix<Scalar, M, N>& a)
{
   return a.sum();
}

template <class Scalar, int M, int N>
Eigen::Array<Scalar,M,N> Total(const std::vector<Eigen::Array<Scalar,M,N> >& v)
{
   if (v.empty()) {
      Eigen::Array<Scalar,M,N> result(0,0);
      result.setZero();
      return result;
   }

   Eigen::Array<Scalar,M,N> result(v[0].rows(), v[0].cols());
   result.setZero();

   for (std::size_t i = 0; i < v.size(); i++)
      result += v[i];

   return result;
}

/// unit vector of length N into direction i
template <int N, int i, typename Scalar = double>
constexpr auto UnitVector() -> Eigen::Matrix<Scalar,N,1>
{
   return Eigen::Matrix<Scalar,N,1>::Unit(i);
}

/// unit vector of length N into direction i
template <int N, typename Scalar = double>
constexpr auto UnitVector(int i) -> Eigen::Matrix<Scalar,N,1>
{
   return Eigen::Matrix<Scalar,N,1>::Unit(i);
}

/// unit vector of length N into direction i
inline Eigen::VectorXd UnitVector(int N, int i)
{
   Eigen::VectorXd v = Eigen::VectorXd::Zero(N);
   v(i) = 1;

   return v;
}

/// matrix projector of size MxN into direction i, j
template <int M, int N, int i, int j, typename Scalar = double>
auto MatrixProjector() -> Eigen::Matrix<Scalar,M,N>
{
   Eigen::Matrix<Scalar,M,N> proj(Eigen::Matrix<Scalar,M,N>::Zero());
   proj(i,j) = 1;

   return proj;
}

/// matrix projector of size MxN into direction i, j
template <int M, int N, typename Scalar = double>
auto MatrixProjector(int i, int j) -> Eigen::Matrix<Scalar,M,N>
{
   Eigen::Matrix<Scalar,M,N> proj(Eigen::Matrix<Scalar,M,N>::Zero());
   proj(i,j) = 1;

   return proj;
}

/// unit matrix projector of size MxN into direction i, j
inline Eigen::MatrixXd MatrixProjector(int M, int N, int i, int j)
{
   Eigen::MatrixXd m = Eigen::MatrixXd::Zero(M,N);
   m(i,j) = 1;

   return m;
}

/// step function (0 for x < 0, 1 otherwise)
template <typename T>
constexpr int UnitStep(T x) noexcept
{
   return x < T() ? 0 : 1;
}

template <typename T>
constexpr T Which(bool cond, T value) noexcept
{
   return cond ? value : T(0);
}

template<typename T, typename ... Trest>
constexpr typename std::common_type<T, Trest...>::type Which(bool cond, T value, Trest... rest) noexcept
{
   return cond ? value : Which(rest...);
}

inline double ZeroSqrt(double x) noexcept
{
   return (x > 0.0 ? std::sqrt(x) : 0.0);
}

template <typename Derived>
Derived ZeroSqrt(const Eigen::ArrayBase<Derived>& m)
{
   return m.unaryExpr([](double a){ return ZeroSqrt(a); });
}

} // namespace flexiblesusy

#endif


#ifndef CANNON_GEOM_KD_TREE_H
#define CANNON_GEOM_KD_TREE_H

/*!
 * \file cannon/geom/kd_tree.hpp
 * \brief File containing KDTree class definition.
 */

/*!
 * \namespace cannon::geom
 * \brief Namespace containing computational geometry classes and functions,
 * largely wrapping CGAL.
 */

#include <Eigen/Core>
#include <CGAL/Epick_d.h>
#include <CGAL/Search_traits_d.h>
#include <CGAL/Orthogonal_k_neighbor_search.h>

using namespace Eigen;

namespace cannon {
  namespace geom {

    /*!
     * \brief Class representing a kd-tree.
     *
     * A kd-tree can be used for nearest neighbor searches. This class
     * primarily wraps the same functionality in CGAL.
     *
     * \sa cannon::geom::KDTreeIndexed
     */
    class KDTree {
      public:
        using Kernel = CGAL::Epick_d<CGAL::Dynamic_dimension_tag>;
        using Point_d = typename Kernel::Point_d;
        using Traits = CGAL::Search_traits_d<Kernel, CGAL::Dynamic_dimension_tag>;
        using NeighborSearch = CGAL::Orthogonal_k_neighbor_search<Traits>;
        using Tree = typename NeighborSearch::Tree;

        KDTree() = delete;

        /*!
         * Constructor taking the dimensionality of points that this kd-tree will hold.
         *
         * \param dim The dimension of points to be held.
         */
        KDTree(int dim) : dim_(dim) {}

        /*!
         * Method to insert points into the kd-tree.
         *
         * \param pts Matrix of points to be added to the kd-tree. Each column
         * should be a point to be inserted, matching insertion of a single
         * point being insertion of a column vector
         */
        void insert(const MatrixXd& pts);

        /*!
         * Method to clear this kd-tree, removing all held points.
         */
        void clear();

        /*!
         * Method to query the nearest neighbor of the input point.
         *
         * \param query Point to find nearest neighbor of.
         *
         * \returns The closest previously inserted point to the query point.
         */
        VectorXd get_nearest_neighbor(const VectorXd& query);

        /*!
         * Method to get the number of points in this kd-tree.
         *
         * \returns Number of points in this tree.
         */
        int get_size();

      private:
        int dim_; //!< Dimension of points in this kd-tree
        Tree tree_; //!< Internal CGAL kd-tree object
    };

  } // namespace geom
} // namespace cannon

#endif /* ifndef CANNON_GEOM_KD_TREE_H */


#ifndef ASLAM_BACKEND_MAPPED_DV_QUAT_HPP
#define ASLAM_BACKEND_MAPPED_DV_QUAT_HPP

#include <Eigen/Core>
#include <aslam/backend/DesignVariable.hpp>
#include "RotationExpression.hpp"
#include "RotationExpressionNode.hpp"

namespace aslam {
  namespace backend {

    class MappedRotationQuaternion : public RotationExpressionNode, public DesignVariable
    {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
      MappedRotationQuaternion(double * q);

      ~MappedRotationQuaternion() override;

      /// \brief Revert the last state update.
      void revertUpdateImplementation() override;

      /// \brief Update the design variable.
      void updateImplementation(const double * dp, int size) override;

      /// \brief the size of an update step
      int minimalDimensionsImplementation() const override;

      RotationExpression toExpression();

      void set( const Eigen::Vector4d & q){ _q = q; _p_q = q; }
    private:
      Eigen::Matrix3d toRotationMatrixImplementation() const override;
      void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
      void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

      /// Returns the content of the design variable
      void getParametersImplementation(Eigen::MatrixXd& value) const override;

      /// Sets the content of the design variable
      void setParametersImplementation(const Eigen::MatrixXd& value) override;

      /// Computes the minimal distance in tangent space between the current value of the DV and xHat
      void minimalDifferenceImplementation(const Eigen::MatrixXd& xHat, Eigen::VectorXd& outDifference) const override;

      /// Computes the minimal distance in tangent space between the current value of the DV and xHat and the jacobian
      void minimalDifferenceAndJacobianImplementation(const Eigen::MatrixXd& xHat, Eigen::VectorXd& outDifference, Eigen::MatrixXd& outJacobian) const override;

      Eigen::Map<Eigen::Vector4d> _q;
      Eigen::Vector4d _p_q;
      Eigen::Matrix3d _C;
    };

  } // namespace backend
} // namespace aslam

#endif /* ASLAM_BACKEND_DV_QUAT_HPP */


#ifndef INCLUDED_scheme_nest_maps_QuaternionMap_HH
#define INCLUDED_scheme_nest_maps_QuaternionMap_HH

#include "scheme/util/SimpleArray.hh"
#include "scheme/util/meta/util.hh"
#include "scheme/nest/pmap/util/get_quaternion_component.hh"

#include <boost/type_traits/make_signed.hpp>
#include <boost/static_assert.hpp>
#include <iostream>
#include <vector>

namespace scheme {
namespace nest {
namespace pmap {

template<
	int DIM,
	class Value=util::SimpleArray<DIM,double>,
	class Index=size_t,
	class Float=double
>
struct QuaternionMap {
	static int const DIMENSION = DIM;
	typedef Value ValueType ;
	typedef Float FloatType ;
	typedef Index IndexType ;
	typedef util::SimpleArray<DIM,Index> Indices;
	typedef util::SimpleArray<DIM,Float> Params;
	typedef typename boost::make_signed<Index>::type SignedIndex;
	typedef util::SimpleArray<DIM,SignedIndex> SignedIndices;

	BOOST_STATIC_ASSERT_MSG(DIM==4,"QuaternionMap DIM must be == 4");
	///@brief constructor
	QuaternionMap() {}
	///@brief
	///@return false iff invalid parameters
	bool params_to_value(
		Params const & params,
		Index /*cell_index*/,
		Index resl,
		Value & value
	) const {
		for(size_t i = 0; i < DIM; ++i) assert( 0.0 <= params[i] );
		// assert( params[0] <= (Float)num_cells_ );
		for(size_t i = 1; i < DIM; ++i) assert( params[i] <= 1.0 );
		for(size_t i = 0; i < DIM; ++i) get_quaternion_component(value,i) = params[i];
		if( params[0] < 0.5 ) return false; // convention is 'W' component is >= 0
		 // cell does not intersect unit sphere
			SignedIndex nside = 1<<resl;
			Float const fnside = nside;
			Float const width = 2.0 / fnside;
			SignedIndices idxnear = ( (params-0.5) * fnside ).template cast<SignedIndex>();
			SignedIndices idxfar  = ( (params    ) * fnside ).template cast<SignedIndex>() - (nside>>1);
			for(size_t i = 0; i < DIM; ++i) idxfar[i] += (idxfar[i]!=idxnear[i] ? 0 : (idxfar[i]<0?-1:1) );
			// idxfar = idxfar - (nside>>1)
			// std::cout << "QUAT: " << params*2.0-1.0 << std::endl;
			// std::cout << "LO: " << idxnear << std::endl;
			// std::cout << "HI: " << idxfar << std::endl;
			Params lower_corner = idxnear.template cast<Float>()*width;
			Params upper_corner = idxfar .template cast<Float>()*width;
			// std::cout << "corners " << lower_corner << " , " << upper_corner << std::endl;
			if( lower_corner.squaredNorm() > 1.0 || upper_corner.squaredNorm() < 1.0 )
				return false;
		// now normalize and "return"
		Params norm = (params-0.5);
		norm = norm / norm.norm();
		for(size_t i = 0; i < DIM; ++i ) get_quaternion_component(value,i) = norm[i];
		return true;
	}
	///@brief
	///@note necessary for value lookup and neighbor lookup
	bool value_to_params(
		Value const & value,
		Index resl,
		Params & params,
		Index & cell_index
	) const {
		///@note neighbor lookups require out of bounds mappings to be valid
		cell_index = 0;
		value_to_params_for_cell(value,resl,params,0);
		return true;
	}
	///@brief
	///@note necessary only for neighbor lookup
	void value_to_params_for_cell(
		Value const & value,
		Index /*resl*/,
		Params & params,
		Index /*cell_index*/
	) const {
		assert( fabs(value.norm()-1.0) < 0.0001 );
		for(size_t i = 0; i < DIM; ++i) assert( -1.0 <= get_quaternion_component(value,i) );
		for(size_t i = 0; i < DIM; ++i) assert(  1.0 >= get_quaternion_component(value,i) );
		for(size_t i = 0; i < DIM; ++i) params[i] = get_quaternion_component(value,i) / 2.0;
		if( params[0] < 0 ) params = -params;
		params = params + 0.5;
		for(size_t i = 0; i < DIM; ++i) assert( 0.0 <= params[i] );
		for(size_t i = 1; i < DIM; ++i) assert( 1.0 >= params[i] );
	}
	///@brief
	///@note delta parameter is in "Parameter Space"
	template<class OutIter>
	void get_neighboring_cells(
		Value const & value,
		Index /*resl*/,
		Float param_delta,
		OutIter out
	) const {
		// // Float param_delta = 1.0 / (Float)(1<<resl);
		// // this BIG thing is to ensure rounding goes down
		// int const BIG = 12345678;
		// int lb = std::max(                0, static_cast<int>( value[0]-param_delta + BIG ) - BIG );
		// int ub = std::min((int)num_cells_-1, static_cast<int>( value[0]+param_delta + BIG ) - BIG );
		// // std::cout << "lb " << lb << " ub "  << ub << std::endl;
		// // assert(lb<=ub);
		// for(int i = lb; i <= ub; ++i) *(out++) = i;
	}
	///@brief aka covering radius max distance from bin center to any value within bin
	Float bin_circumradius(Index resl) const { return 2.0/(Float)(1<<resl); }
	///@brief maximum distance from the bin center which must be within the bin
	Float bin_inradius(Index resl) const { return 3.0/(Float)(1<<resl); }
	///@brief cell size
	Index num_cells() const { return 1; }
	virtual ~QuaternionMap(){}
 private:
};

}
}
}

#endif


#ifndef OSRM_UTIL_BIT_RANGE_HPP
#define OSRM_UTIL_BIT_RANGE_HPP

#include "util/msb.hpp"

#include <boost/iterator/iterator_facade.hpp>
#include <boost/range/iterator_range.hpp>

namespace osrm
{
namespace util
{

namespace detail
{
template <typename T> std::size_t countOnes(T value)
{
    static_assert(std::is_unsigned<T>::value, "Only unsigned types allowed");
    std::size_t number_of_ones = 0;
    while (value > 0)
    {
        auto index = msb(value);
        value = value & ~(T{1} << index);
        number_of_ones++;
    }
    return number_of_ones;
}

#if (defined(__clang__) || defined(__GNUC__) || defined(__GNUG__))
inline std::size_t countOnes(std::uint8_t value)
{
    return __builtin_popcount(std::uint32_t{value});
}
inline std::size_t countOnes(std::uint16_t value)
{
    return __builtin_popcount(std::uint32_t{value});
}
inline std::size_t countOnes(unsigned int value) { return __builtin_popcount(value); }
inline std::size_t countOnes(unsigned long value) { return __builtin_popcountl(value); }
inline std::size_t countOnes(unsigned long long value) { return __builtin_popcountll(value); }
#endif
} // namespace detail

// Investigate if we can replace this with
// http://www.boost.org/doc/libs/1_64_0/libs/dynamic_bitset/dynamic_bitset.html
template <typename DataT>
class BitIterator : public boost::iterator_facade<BitIterator<DataT>,
                                                  const std::size_t,
                                                  boost::forward_traversal_tag,
                                                  const std::size_t>
{
    typedef boost::iterator_facade<BitIterator<DataT>,
                                   const std::size_t,
                                   boost::forward_traversal_tag,
                                   const std::size_t>
        base_t;

  public:
    typedef typename base_t::value_type value_type;
    typedef typename base_t::difference_type difference_type;
    typedef typename base_t::reference reference;
    typedef std::random_access_iterator_tag iterator_category;

    explicit BitIterator() : m_value(0) {}
    explicit BitIterator(const DataT x) : m_value(std::move(x)) {}

  private:
    void increment()
    {
        auto index = msb(m_value);
        m_value = m_value & ~(DataT{1} << index);
    }

    difference_type distance_to(const BitIterator &other) const
    {
        return detail::countOnes(m_value) - detail::countOnes(other.m_value);
    }

    bool equal(const BitIterator &other) const { return m_value == other.m_value; }

    reference dereference() const
    {
        BOOST_ASSERT(m_value > 0);
        return msb(m_value);
    }

    friend class ::boost::iterator_core_access;
    DataT m_value;
};

// Returns range over all 1 bits of value
template <typename T> auto makeBitRange(const T value)
{
    return boost::make_iterator_range(BitIterator<T>{value}, BitIterator<T>{});
}
} // namespace util
} // namespace osrm

#endif