// minimal renderer of lidar data in lcmgl
// fully working as of august 2013

#include <stdio.h>
#include <inttypes.h>
#include <iostream>
#include <Eigen/Dense>
#include <boost/shared_ptr.hpp>

#include <lcm/lcm-cpp.hpp>
#include <bot_lcmgl_client/lcmgl.h>
#include <GL/gl.h>

#include "lcmtypes/bot_core.hpp"
#include "lcmtypes/multisense.hpp"

using namespace std;
using namespace Eigen;

class Pass{
  public:
    Pass(boost::shared_ptr<lcm::LCM> &lcm_);

    ~Pass(){
    }
  private:
    boost::shared_ptr<lcm::LCM> lcm_;
    void lidarHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::planar_lidar_t* msg);
    void cloudHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  multisense::pointcloud_t* msg);
    void camToScanHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::rigid_transform_t* msg);

    bot_lcmgl_t* lcmgl_;
    int printf_counter_; // used for terminal feedback

    std::vector<Eigen::Vector3d> cloud_incoming_; // 3D points that were precomputed

    std::vector<Eigen::Vector3d> cloud_transformed_; // incoming lidar converted to 3D points
    Eigen::Isometry3d camera_to_scan_; // transform used to do conversion

    void drawLidar(int64_t last_utime);
};

Pass::Pass(boost::shared_ptr<lcm::LCM> &lcm_):
    lcm_(lcm_){
  lcmgl_= bot_lcmgl_init(lcm_->getUnderlyingLCM(), "lidar-to-3dpoints");

  lcm_->subscribe( "SCAN" ,&Pass::lidarHandler,this);
  lcm_->subscribe( "SCAN_CLOUD" ,&Pass::cloudHandler,this);
  lcm_->subscribe( "CAMERA_TO_SCAN" ,&Pass::camToScanHandler,this);
  printf_counter_ =0;
}

void Pass::cloudHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  multisense::pointcloud_t* msg){
  cloud_incoming_.clear();
  for (size_t i=0; i < msg->num_points; ++i) {
    Eigen::Vector3d pt( msg->x[i], msg->y[i], msg->z[i]);
    cloud_incoming_.push_back(pt);
  }

  drawLidar(msg->utime);
}

void Pass::camToScanHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::rigid_transform_t* msg){
  camera_to_scan_.setIdentity();
  camera_to_scan_.translation()  << msg->trans[0], msg->trans[1], msg->trans[2];
  Eigen::Quaterniond quat = Eigen::Quaterniond(msg->quat[0], msg->quat[1],
                                               msg->quat[2], msg->quat[3]);
  camera_to_scan_.rotate(quat);
}

// from pointcloud_lcm
void convertLidar(std::vector< float > ranges, int numPoints, double thetaStart,
        double thetaStep,
        std::vector<Eigen::Vector3d> &cloud,
        double minRange = 0., double maxRange = 1e10,
        double validRangeStart = -1000, double validRangeEnd = 1000)
{
  double theta = thetaStart;
  for (int i = 0; i < numPoints; i++) {
    if (ranges[i] > minRange && ranges[i] < maxRange && theta > validRangeStart
            && theta < validRangeEnd) {
      Eigen::Vector3d pt;
      pt(0) = ranges[i] * cos(theta);
      pt(1) = ranges[i] * sin(theta);
      pt(2) = 0;
      cloud.push_back(pt);
    }
    theta += thetaStep;
  }
}

void bot_lcmgl_draw_axes(bot_lcmgl_t * lcmgl, const Eigen::Affine3d & trans, double scale)
{
  lcmglPushMatrix();
  lcmglMultMatrixd(trans.data());
  lcmglScalef(scale, scale, scale);
  bot_lcmgl_draw_axes(lcmgl);
  lcmglPopMatrix();
}

void Pass::lidarHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::planar_lidar_t* msg){
  // 1. Convert scan into simple XY point cloud:
  std::vector<Eigen::Vector3d> scan_laser;
  double minRange =0.0; // consider everything - don't remove any points
  double maxRange = 100.0;
  double validBeamAngles[] ={-10,10};
  convertLidar(msg->ranges, msg->nranges, msg->rad0,
      msg->radstep, scan_laser, minRange, maxRange,
      validBeamAngles[0], validBeamAngles[1]);

  // 2. Project the scan into camera frame:
  cloud_transformed_.clear();
  Eigen::Affine3d Aff = Eigen::Affine3d(camera_to_scan_ );
  for (size_t i=0; i < scan_laser.size(); ++i) {
    Eigen::Vector3d pt = scan_laser[i];
    pt = Aff * pt;
    cloud_transformed_.push_back(pt);
  }
  /// cloud_transformed_ now contains the lidar returns as 3d points in the camera frame
}

void Pass::drawLidar(int64_t last_utime){
  // transform and point cloud are both in camera optical frame (z forward, x right)
  // we need to convert to (x forward, z up) here:

  // a. Visualize the axes of interest:
  bot_lcmgl_push_matrix(lcmgl_);
  bot_lcmgl_rotated(lcmgl_, -90, 1, 0, 0);
  bot_lcmgl_rotated(lcmgl_, 90, 0, 1, 0);
  bot_lcmgl_draw_axes(lcmgl_, camera_to_scan_, 4);

  // b. Plot scan from camera frame:
  bot_lcmgl_point_size(lcmgl_, 4.5f);
  bot_lcmgl_begin(lcmgl_, GL_POINTS);  // render as points
  bot_lcmgl_color3f(lcmgl_, 0, 0, 1); // Blue
  for (size_t i=0; i < cloud_transformed_.size(); ++i) {
    Eigen::Vector3d pt = cloud_transformed_[i];
    bot_lcmgl_vertex3d(lcmgl_, pt(0), pt(1), pt(2)); // 100 points in line
  }
  bot_lcmgl_end(lcmgl_);

  // c. Render incoming point cloud from camera frame
  bot_lcmgl_point_size(lcmgl_, 2.5f);
  bot_lcmgl_begin(lcmgl_, GL_POINTS);  // render as points
  bot_lcmgl_color3f(lcmgl_, 0, 1, 0); // Blue
  for (size_t i=0; i < cloud_incoming_.size(); ++i) {
    Eigen::Vector3d pt = cloud_incoming_[i];
    bot_lcmgl_vertex3d(lcmgl_, pt(0), pt(1), pt(2)); // 100 points in line
  }
  bot_lcmgl_end(lcmgl_);
  bot_lcmgl_pop_matrix(lcmgl_);
  bot_lcmgl_switch_buffer(lcmgl_);

  if (printf_counter_%80 ==0){
    cout << "Scan @ "  << last_utime << "\n";
  }
  printf_counter_++;
}

int main( int argc, char** argv ){
  boost::shared_ptr<lcm::LCM> lcm(new lcm::LCM);
  if(!lcm->good()){
    std::cerr <<"ERROR: lcm is not good()" <<std::endl;
  }

  Pass app(lcm);
  cout << "Ready to visualize lidar points" << endl << "============================" << endl;
  while(0 == lcm->handle());
  return 0;
}


/*************************************************
 * Copyright (c) 2017 Toru Ito
 * Released under the MIT license
 * http://opensource.org/licenses/mit-license.php
 *************************************************/

#ifndef ShapeData_hpp
#define ShapeData_hpp

#include <stdio.h>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Geometry>

#include <OpenGL/gl3.h>

class ShapeData
{
public:
    typedef Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor >       MatrixDouble;
    typedef Eigen::Matrix< unsigned int, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > MatrixUInt;

public:
    ShapeData();
    ~ShapeData();

    static void ConvertListToMatrix( std::vector< std::vector< double > >& list, MatrixDouble& matrix );
    static void ConvertListToMatrix( std::vector< std::vector< unsigned int > >& list, MatrixUInt& matrix );

    void SetPolyline( std::vector< std::vector< std::vector< Eigen::Vector2d > > > *pPointList );
    void SetVertex( std::vector< std::vector< double > >& list );
    void SetVertex( std::vector< Eigen::Vector2d >& list );
    void SetIndex( std::vector< std::vector< unsigned int > >& list );

    void SetDrawMode( GLenum mode );

    void InitDraw();
    void Draw();
    void DeleteDraw();

    MatrixDouble  m_Vertex;
    MatrixUInt    m_Index;

private:
    GLuint m_VertexBufferObject;
    GLuint m_IndexBufferObject;
    GLuint m_VertexArrayObject;

    GLenum m_DrawMode;
};

#endif /* ShapeData_hpp */


#include "LBFGSB.h"
#include <Eigen/Core>
#include <vector>
#include <memory>
#include <sstream>
#include <iostream>
#include <fstream>
#include <chrono>

using Eigen::VectorXd;
using Eigen::MatrixXd;
using namespace LBFGSpp;

typedef double Scalar;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;

class Timer
{
public:
    void start()
    {
        m_StartTime = std::chrono::system_clock::now();
        m_bRunning = true;
    }

    void stop()
    {
        m_EndTime = std::chrono::system_clock::now();
        m_bRunning = false;
    }

    double elapsedMilliseconds()
    {
        std::chrono::time_point<std::chrono::system_clock> endTime;

        if(m_bRunning)
        {
            endTime = std::chrono::system_clock::now();
        }
        else
        {
            endTime = m_EndTime;
        }

        return std::chrono::duration_cast<std::chrono::milliseconds>(endTime - m_StartTime).count();
    }

    double elapsedSeconds()
    {
        return elapsedMilliseconds() / 1000.0;
    }

private:
    std::chrono::time_point<std::chrono::system_clock> m_StartTime;
    std::chrono::time_point<std::chrono::system_clock> m_EndTime;
    bool                                               m_bRunning = false;
};

class SegmentCCDF {
private:
    std::vector<long> occCounts;
    std::vector<long> occCountFrequency;
public:
    SegmentCCDF(
            std::vector<long>&& occCounts_,
            std::vector<long>&& occCountFrequency_
            ) {
        occCounts = std::move(occCounts_);
        occCountFrequency = std::move(occCountFrequency_);
    }
    void compute(double bias, double* totalResult, double* gradientResult) const {
        size_t n = occCounts.size();
        double total = 0;
        double totalDeriv = 0;
        for (size_t i = 0; i < n; i++) {
            if (occCounts[i] > bias) {
                total += (occCounts[i] - bias) * occCountFrequency[i];
                totalDeriv -= occCountFrequency[i];
            }
        }
        *totalResult = total;
        *gradientResult = totalDeriv;
    }
    void print() const {
        for (long x : occCounts) {
            std::cout << x << " ";
        }
        std::cout << std::endl;
        for (long x : occCountFrequency) {
            std::cout << x << " ";
        }
        std::cout << std::endl;
    }
};

class RMSErrorFunction
{
private:
    std::vector<SegmentCCDF> segments;
    std::vector<int> segmentSpaces;
public:
    RMSErrorFunction(
            std::vector<SegmentCCDF>&& segments_,
            std::vector<int>&& segmentSpaces_
            ) {
        segments = std::move(segments_);
        segmentSpaces = std::move(segmentSpaces_);
    }

    Scalar operator()(const Vector& xBuffer, Vector& grad)
    {
        int nSeg = segments.size();
        double biasTerm = 0;
        double varTerm = 0;

        double ni, dni;
        for (int i = 0; i < nSeg; i++) {
            double x = xBuffer[i];
            const SegmentCCDF& segCDF = segments[i];
//            std::cout << "cdf: " << i << std::endl;
//            segCDF.print();
            segCDF.compute(x, &ni, &dni);

            biasTerm += x;
            double scaledTotal = ni / segmentSpaces[i];
            varTerm += .25*scaledTotal*scaledTotal;
            grad[i] = .5*scaledTotal*dni/segmentSpaces[i];
        }

        for (int i = 0; i < nSeg; i++) {
            grad[i] += 2*biasTerm;
        }
        return biasTerm*biasTerm + varTerm;
    }

    int dim() {
        return segmentSpaces.size();
    }
};

RMSErrorFunction testFunction(int n) {
    std::vector<long> occCounts {1, 2, 3, 5};
    std::vector<long> occCountFreqs {50, 30, 10, 10};

    SegmentCCDF ccdf (
            std::move(occCounts),
            std::move(occCountFreqs)
            );

    std::vector<SegmentCCDF> segments;
    std::vector<int> segmentSpaces;
    for (int i = 0; i < n; i++) {
        segments.push_back(ccdf);
        segmentSpaces.push_back(5);
    }
    return RMSErrorFunction(
            std::move(segments),
            std::move(segmentSpaces)
            );
}

RMSErrorFunction parseFile(const std::string& filePath) {
    std::vector<int> segmentSpaces;
    std::vector<SegmentCCDF> segments;

    std::ifstream infile;
    infile.open(filePath.c_str());

    std::string line;
    std::istringstream iss;
    long number;
    int numInt;

    std::getline(infile, line);
    iss = std::istringstream(line);
    while (iss >> numInt) {
        segmentSpaces.push_back(numInt);
    }

    int numSegments = segmentSpaces.size();

    for (int i = 0; i < numSegments; i++) {
        std::getline(infile, line);
        iss = std::istringstream(line);
        std::vector<long> occCounts;
        occCounts.reserve(line.size());
        while (iss >> number) {
            occCounts.push_back(number);
        }

        std::getline(infile, line);
        iss = std::istringstream(line);
        std::vector<long> occCountFrequency;
        occCountFrequency.reserve(occCounts.size());
        while (iss >> number) {
            occCountFrequency.push_back(number);
        }

        segments.emplace_back(
                std::move(occCounts),
                std::move(occCountFrequency)
                );
    }

    return RMSErrorFunction(
            std::move(segments),
            std::move(segmentSpaces)
            );
}

int main(int argc, char *argv[])
{
    LBFGSBParam<double> param;
    param.max_iterations = 30;
    LBFGSBSolver<double> solver(param);

//    const int n = 10;
//    RMSErrorFunction fun = testFunction(n);
    Timer parseTimer;
    parseTimer.start();
    RMSErrorFunction fun = parseFile(argv[1]);
    parseTimer.stop();
    std::cerr << "Parse Time: " << parseTimer.elapsedMilliseconds() << std::endl;
    int n = fun.dim();

    Vector lb = Vector::Constant(n, 0.0);
    Vector ub = Vector::Constant(n, std::numeric_limits<Scalar>::infinity());
    VectorXd x = VectorXd::Zero(n);
    double fx = 0;

    VectorXd g = VectorXd::Zero(n);
    int niter = solver.minimize(fun, x, fx, lb, ub);

//    std::cout << niter << " iterations" << std::endl;
//    std::cout << "x = \n" << x.transpose() << std::endl;
//    std::cout << "f(x) = " << fx << std::endl;
    std::cout << x.transpose() << std::endl;
    return 0;
}


// from http://www.boost.org/doc/libs/1_66_0/doc/html/boost_asio/tutorial/tuttimer1/src.html
//
// timer.cpp
// ~~~~~~~~~
//
// Copyright (c) 2003-2017 Christopher M. Kohlhoff (chris at kohlhoff dot com)
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//

#include <boost/asio.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

int main()
{
    boost::asio::io_context io;

    auto start = std::chrono::system_clock::now();

    boost::asio::deadline_timer t(io, boost::posix_time::seconds(3));
    t.wait();

    auto end = std::chrono::system_clock::now();

    return std::chrono::duration_cast<std::chrono::seconds>(end - start).count() > 2 ? 0 : -1;
}


#include <boost/mpl/vector.hpp>
#include <boost/mpl/vector_c.hpp>
#include <boost/mpl/integral_c.hpp>
#include <boost/mpl/find.hpp>

int main()
{
    using v1 = boost::mpl::vector<boost::mpl::integral_c<int, 5>, boost::mpl::integral_c<int, 2>, boost::mpl::integral_c<int, 3>, boost::mpl::integral_c<int, 1>, boost::mpl::integral_c<int, 4>>;
    using it1 = boost::mpl::find<v1, boost::mpl::integral_c<int, 3>>::type;
    using pos1 = it1::pos;
    constexpr int r1{ pos1::value };
    static_assert(r1 == 2);

    using v2 = boost::mpl::vector_c<int, 5, 2, 3>;
    using it2 = boost::mpl::find<v2, boost::mpl::integral_c<int, 6>>::type;
    using pos2 = it2::pos;
    constexpr int r2{ pos2::value };
    static_assert(r2 == 3);
}


/* Copyright (C) 2017 IBM Corp.
 *  Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *     http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing,
 * software distributed under the License is distributed on an
 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,
 * either express or implied. See the License for the specific
 * language governing permissions and limitations under the License.
 */
/*******************************************************************
Gaussian1Dsampler: defines a 1D pseudo-random number sampler, either
using Martin Albresht's online library or implementing two optional
methods, Box–Muller transform Marsaglia polar method
********************************************************************/
#include <NTL/ZZ.h>
#include <NTL/mat_lzz_p.h>
#include <limits>
#include <NTL/BasicThreadPool.h>
#include <NTL/ZZ.h>
#include <NTL/FFT.h>
#include <NTL/SmartPtr.h>

NTL_CLIENT
#include "mat_l.h"
#include "vec_l.h"
#include "DGaussSampler.h"
#include "utils/tools.h"

#ifdef NTL_HAVE_AVX
#warning "HAVE_AVX"

#include <immintrin.h>

#ifdef NTL_HAVE_FMA
#define MUL_ADD(a, b, c) a = _mm256_fmadd_pd(b, c, a)
#else
#define MUL_ADD(a, b, c) a = _mm256_add_pd(a, _mm256_mul_pd(b, c))
#endif
#endif

// Two static variables for debugging purposes
std::atomic<double> Gaussian1Dsampler::maxSigma(0.0);// keep largest stdev we've seen
std::atomic<int> Gaussian1Dsampler::maxSample(0);   // keep largest sample ever drawn

long Gaussian1Dsampler::writeToFile(FILE* handle)
{
    FHE_TIMER_START;
    long count = fwrite(&sigma,sizeof(sigma),1,handle);

    return count;
}

long Gaussian1Dsampler::readFromFile(FILE* handle)
{
    FHE_TIMER_START;
    long count = fread(&sigma,sizeof(sigma),1,handle);

    return count;
}

#ifdef CONTINUOUS_GAUSSIAN_SAMPLING
/* generateGaussianNoiseMars uses Marsaglia polar method
 * to generate Gaussian samples
 */
NTL::Pair<double,double>
Gaussian1Dsampler::generateGaussianNoiseMars(double mean, double stdDev)
{
    FHE_TIMER_START;
    double u, v, s;
    do    // choose (u,v) in the square [-1,1] x [-1,1]
    {
        u = (NTL::RandomBnd(NTL_SP_BOUND)/((double)NTL_SP_BOUND)) *2.0 -1.0;
        v = (NTL::RandomBnd(NTL_SP_BOUND)/((double)NTL_SP_BOUND)) *2.0 -1.0;
        s = u * u + v * v;
    }
    while( (s >= 1.0) || (s == 0.0) );// until you find a pair in the unit sphere

    s = sqrt(-2.0 * log(s) / s);
    double sample1 = mean + stdDev * u * s;
    double sample2 = mean + stdDev * v * s;

    return NTL::Pair<double,double>(sample1,sample2);
}

/* generateGaussianNoiseBox uses Box-Muller to generate Gaussian Samples
 */
NTL::Pair<double,double>
Gaussian1Dsampler::generateGaussianNoiseBox(double mu, double stDev)
{
    FHE_TIMER_START;
    const double epsilon = std::numeric_limits<double>::min();
    const double two_pi = 2.0*3.14159265358979323846;

    double u1, u2;
    do
    {
        u1 = ((NTL::RandomBnd(NTL_SP_BOUND))/((double)NTL_SP_BOUND));
        u2 = ((NTL::RandomBnd(NTL_SP_BOUND))/((double)NTL_SP_BOUND));
    }
    while ( u1 <= epsilon );

    double z0 = sqrt(-2.0 * log(u1)) * cos(two_pi * u2);
    double z1 = sqrt(-2.0 * log(u1)) * sin(two_pi * u2);

    double sample1 = z0 * stDev + mu;
    double sample2 = z1 * stDev + mu;

    return NTL::Pair<double,double>(sample1,sample2);
}
#endif // ifdef CONTINUOUS_GAUSSIAN_SAMPLING


/*
 * ----------------- BEGIN LICENSE BLOCK ---------------------------------
 *
 * Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
 * de Barcelona (UAB).
 * Copyright (C) 2019-2021 Intel Corporation
 *
 * SPDX-License-Identifier: MIT
 *
 * ----------------- END LICENSE BLOCK -----------------------------------
 */

#pragma once

#include <boost/array.hpp>
#include <boost/math/tools/rational.hpp>
#include "opendrive/types.hpp"

namespace opendrive {
namespace geometry {

/**
 * @brief DirectedPoint is composed of a 3 dimensional position and an orientation
 */
struct DirectedPoint
{
  DirectedPoint();
  DirectedPoint(const Point &point, double t);
  DirectedPoint(double x, double y, double z, double t);

  Point location = {0, 0, 0};
  double tangent = 0.0; // [radians]

  static bool floatCompare(double f1, double f2)
  {
    static constexpr auto epsilon = 1.0e-09f;
    if (std::fabs(f1 - f2) <= epsilon)
      return true;
    return std::fabs(f1 - f2) <= epsilon * std::max(std::fabs(f1), std::fabs(f2));
  }

  /**
   * @brief Shifts the point by lateral_offset meters in the normal direction of the tangent
   */
  void ApplyLateralOffset(double lateral_offset);

  /**
   * @returns the point shifted by lateral_offset meters in the normal direction of the tangent
   */
  Point getLateralOffsetPoint(double lateral_offset) const;

  /**
   * @brief Compares two points, returns true if their parameters are identical
   */
  bool operator==(const DirectedPoint &lhs)
  {
    if ((lhs.location == location) && (floatCompare(lhs.tangent, tangent)))
    {
      return true;
    }
    return false;
  }
};

class Geometry
{
public:
  // max sampling error expected
  static constexpr double cMaxSamplingError = 5e-2;

  /**
   * @brief Returns the GeometryType associated to this geometry.
   */
  GeometryType GetType() const;

  /**
   * @brief Returns the length of the geometry.
   */
  double GetLength() const;

  /**
   * @brief Returns the start offset in s coordinates.
   */
  double GetStartOffset() const;

  /**
   * @brief Returns the heading at the start position.
   */
  double GetHeading() const;

  /**
   * @brief Returns the start position in the global reference frame.
   */
  const Point &GetStartPosition();

  virtual ~Geometry() = default;

  /**
   * @brief Evaluates the geometry at a certain distance from the start position.
   */
  virtual const DirectedPoint PosFromDist(double dist) const = 0;

protected:
  Geometry(GeometryType type, double start_offset, double length, double heading, const Point &start_pos);

protected:
  GeometryType _type; // geometry type
  double _length;     // length of the road section [meters]

  double _start_position_offset; // s-offset [meters]
  double _heading;               // start orientation [radians]

  Point _start_position; // [meters]
};

class GeometryLine : public Geometry
{
public:
  GeometryLine(double start_offset, double length, double heading, const Point &start_pos);

  const DirectedPoint PosFromDist(const double dist) const override;
};

class GeometryArc : public Geometry
{
public:
  GeometryArc(double start_offset, double length, double heading, const Point &start_pos, double curv);

  const DirectedPoint PosFromDist(double dist) const override;

  double GetCurvature() const;

private:
  double _curvature;
};

class GeometryPoly3 : public Geometry
{
public:
  GeometryPoly3(
    double start_offset, double length, double heading, const Point &start_pos, double a, double b, double c, double d);

  const DirectedPoint PosFromDist(const double dist) const override;

private:
  double _a;
  double _b;
  double _c;
  double _d;
};

class GeometryParamPoly3 : public Geometry
{
public:
  GeometryParamPoly3(double start_offset,
                     double length,
                     double heading,
                     const Point &start_pos,
                     double aU,
                     double bU,
                     double cU,
                     double dU,
                     double aV,
                     double bV,
                     double cV,
                     double dV,
                     bool p_range_is_normalized);

  const DirectedPoint PosFromDist(const double dist) const override;

private:
  double _aU;
  double _bU;
  double _cU;
  double _dU;
  double _aV;
  double _bV;
  double _cV;
  double _dV;
  bool _p_range_is_normalized;
};

template <class T> double evalPoly3(std::set<T> const &sOffsetPoly3Set, double s)
{
  T search;
  search.start_offset = s;
  auto smallerOrEqual = sOffsetPoly3Set.upper_bound(search);
  if (smallerOrEqual != sOffsetPoly3Set.begin())
  {
    // upper_bound is first larger element
    smallerOrEqual--;
  }
  if (smallerOrEqual != sOffsetPoly3Set.end())
  {
    auto poly = boost::array<double, 4>{{smallerOrEqual->a, smallerOrEqual->b, smallerOrEqual->c, smallerOrEqual->d}};
    return boost::math::tools::evaluate_polynomial(poly, s - smallerOrEqual->start_offset);
  }
  return 0.0;
}

} // namespace geometry
} // namespace opendrive


#include "HasseLiftExt.h"
#include "Util.h"
#include "BaseChange.h"
#include "MultipointEval.h"
#include "FrobComp.h"
#include "MultiComposeMod.h"
#include "BalancedMul.h"
#include "HasseLift.h"
#include <NTL/matrix.h>
#include <cmath>
#include <NTL/ZZX.h>

HasseLiftExt::HasseLiftExt(const ZZ_pX &g,
                           const ZZ_pX &delta,
                           const ZZ_pXModulus &F) {
    this->g = g;
    this->delta = delta;
    this->F = F;
    PowerXMod(this->frobenius, ZZ_p::modulus(), F);
}

HasseLiftExt::~HasseLiftExt() {
    this->g.kill();
    this->delta.kill();
    this->F.f.kill();
    this->frobenius.kill();
    this->evalModulus.f.kill();
}

void HasseLiftExt::computeNaive(ZZ_pX& result, long n) {
    long l = (long) pow(n, BETA);
    long m = n / l;
    n = m * l + l + 1;

    ZZ_pX r0;
    ZZ_pX r1;
    ZZ_pX gTemp;
    ZZ_pX deltaTemp;
    ZZ_pX xq;
    ZZ_pX x;
    ZZ_pX temp1;
    ZZ_pX temp2;

    set(r0);
    r1 = g;
    PowerMod(gTemp, g, ZZ_p::modulus(), F);
    deltaTemp = delta;
    SetX(x);
    xq = this->frobenius;

    for (long i = 2; i <= n; i++) {
        sub(temp1, xq, x);
        MulMod(temp1, temp1, deltaTemp, F);
        MulMod(temp1, temp1, r0, F);
        MulMod(temp2, gTemp, r1, F);
        r0 = r1;
        sub(r1, temp2, temp1);

        PowerMod(gTemp, gTemp, ZZ_p::modulus(), F);
        PowerMod(deltaTemp, deltaTemp, ZZ_p::modulus(), F);
        PowerMod(xq, xq, ZZ_p::modulus(), F);
    }

    result = r1;

    r0.kill();
    r1.kill();
    gTemp.kill();
    deltaTemp.kill();
    xq.kill();
    x.kill();
    temp1.kill();
    temp2.kill();
}

void HasseLiftExt::compute(ZZ_pX& result, long n, int verbose) {

    long l = (long) pow(n, BETA);
    long m = n / l;

    ZZ_pE::init(F.f);

    Mat<ZZ_pEX> B;
    B.SetDims(2, 2);
    buildInitialMatrix(B);

    Util util;
    long start = util.getTimeMillis();
    // compute x, tau^{-l}(x), tau^{-2l}(x), ..., tau^{-lm}(x)
    Vec<ZZ_pX> inverseFrobs;
    inverseFrobs.SetLength(m + 1);
    FrobComp frobComp;
    frobComp.computeInverseFrobes(inverseFrobs, m, l, frobenius, F);
    buildEvalModulus(inverseFrobs);
    if (verbose == 1)
        cout << "computeInverseFrobes: " << util.getTimeMillis() - start << endl;

    start = util.getTimeMillis();
    // compute the product tau^{l - 1}(A)...tau(A)A
    computeFrobProduct(B, B, l);
    if (verbose == 1)
        cout << "computeFrobProduct: " << util.getTimeMillis() - start << endl;

    start = util.getTimeMillis();
    // compute B(inverseFrobs[0]), ..., B(inverseFrobs[m])
    Mat<Vec < ZZ_pX>> BEval;
    BEval.SetDims(2, 2);
    evaluate(BEval, B, inverseFrobs);
    if (verbose == 1)
        cout << "evaluate BEval: " << util.getTimeMillis() - start << endl;

    start = util.getTimeMillis();
    // compute the product tau^{lm}(BEval(i))...tau^{l}(BEval(i)) BEval(i)
    Mat<ZZ_pX> frobProduct;
    frobProduct.SetDims(2, 2);
    computeFrobProduct(frobProduct, BEval, l);
    if (verbose == 1)
        cout << "computeFrobProduct2: " << util.getTimeMillis() - start << endl;

    MulMod(result, frobProduct[1][1], g, F);
    add(result, result, frobProduct[1][0]);

    B.kill();
    inverseFrobs.kill();
    BEval.kill();
    frobProduct.kill();
}

void HasseLiftExt::buildEvalModulus(const Vec<ZZ_pX> &inverseFrobs) {
    long m = inverseFrobs.length();
    Vec<ZZ_pEX> tempVec;
    tempVec.SetLength(m);

    for (long i = 0; i < m; i++) {
        clear(tempVec[i]);
        SetCoeff(tempVec[i], 0, -to_ZZ_pE(inverseFrobs[i]));
        SetCoeff(tempVec[i], 1, 1);
    }

    BalancedMul balancedMul;
    balancedMul.compute(tempVec);
    evalModulus = tempVec[0];
    tempVec.kill();
}

void HasseLiftExt::evaluate(Mat<Vec<ZZ_pX> >& result,
                            const Mat<ZZ_pEX>& A,
                            const Vec<ZZ_pX>& points) {

    MultipointEval multiPointEval(points);

    for (int i = 0; i < 2; i++) {
        for (int j = 0; j < 2; j++) {

            result[i][j].SetLength(points.length());
            multiPointEval.eval(result[i][j], A[i][j]);
        }
    }
}

void HasseLiftExt::buildInitialMatrix(Mat<ZZ_pEX>& result) {
    clear(result[0][0]);
    set(result[0][1]);

    ZZ_pX temp;

    MulMod(temp, frobenius, delta, F);
    NTL::negate(temp, temp);
    clear(result[1][0]);
    SetCoeff(result[1][0], 0, to_ZZ_pE(temp));
    SetCoeff(result[1][0], 1, to_ZZ_pE(delta));

    MultiComposeMod().compose(temp, g, frobenius, F);
    clear(result[1][1]);
    SetCoeff(result[1][1], 0, to_ZZ_pE(temp));

    temp.kill();
}

//void HasseLiftExt::computeFrobProduct(Mat<ZZ_pEX>& result,
//                                      const Mat<ZZ_pEX>& A,
//                                      long l) {
//    Vec<ZZ_pX> frobCompos;
//    frobCompos.SetLength(3);
//    frobCompos[0] = coeff(A[1][0], 1)._ZZ_pE__rep;
//    frobCompos[1] = coeff(A[1][1], 0)._ZZ_pE__rep;
//    frobCompos[2] = coeff(A[1][0], 0)._ZZ_pE__rep;
//
//    Mat<ZZ_p> frobMat;
//    ZZ_pXMultiplier frobMultiplier;
//    MultiComposeMod multiComposeMod;
//    multiComposeMod.precompute(frobMat, frobMultiplier, 3, frobenius, F);
//
//    Vec<Mat<ZZ_pEX>> tempVec;
//    tempVec.SetLength(l);
//
//    for (long i = 0; i < l; i++)
//        tempVec[i].SetDims(2, 2);
//
//    tempVec[l - 1] = A;
//
//    for (long i = l - 2; i >= 0; i--) {
//        multiComposeMod.compose(frobCompos, frobCompos, frobMat,
//                                frobMultiplier, F);
//
//        clear(tempVec[i][0][0]);
//        set(tempVec[i][0][1]);
//        clear(tempVec[i][1][0]);
//        clear(tempVec[i][1][1]);
//        SetCoeff(tempVec[i][1][0], 1, to_ZZ_pE(frobCompos[0]));
//        SetCoeff(tempVec[i][1][1], 0, to_ZZ_pE(frobCompos[1]));
//        SetCoeff(tempVec[i][1][0], 0, to_ZZ_pE(frobCompos[2]));
//    }
//
//    BalancedMul balancedMul;
//    balancedMul.compute(tempVec, evalModulus);
//    result = tempVec[0];
//
//    frobCompos.kill();
//    frobMat.kill();
//    frobMultiplier.b.kill();
//    for (long i = 0; i < l; i++)
//        tempVec[i].kill();
//    tempVec.kill();
//}

//void HasseLiftExt::computeFrobProduct(Mat<ZZ_pEX>& result,
//                                      const Mat<ZZ_pEX>& A,
//                                      long l) {
//    Vec<ZZ_pX> frobCompos;
//    frobCompos.SetLength(3);
//    frobCompos[0] = coeff(A[1][0], 1)._ZZ_pE__rep;
//    frobCompos[1] = coeff(A[1][1], 0)._ZZ_pE__rep;
//    frobCompos[2] = coeff(A[1][0], 0)._ZZ_pE__rep;
//
//
//    Vec<Mat < ZZ_pEX>> tempVec;
//    tempVec.SetLength(l);
//
//    for (long i = 0; i < l; i++)
//        tempVec[i].SetDims(2, 2);
//
//    tempVec[l - 1] = A;
//
//    Util util;
//    long start = util.getTimeMillis();
//    for (long i = l - 2; i >= 0; i--) {
//        for (long j = 0; j < frobCompos.length(); j++)
//            PowerMod(frobCompos[j], frobCompos[j], ZZ_p::modulus(), F);
//
//        clear(tempVec[i][0][0]);
//        set(tempVec[i][0][1]);
//        clear(tempVec[i][1][0]);
//        clear(tempVec[i][1][1]);
//        SetCoeff(tempVec[i][1][0], 1, to_ZZ_pE(frobCompos[0]));
//        SetCoeff(tempVec[i][1][1], 0, to_ZZ_pE(frobCompos[1]));
//        SetCoeff(tempVec[i][1][0], 0, to_ZZ_pE(frobCompos[2]));
//    }
//    cout << "powering time: " << util.getTimeMillis() - start << endl;
//
//    start = util.getTimeMillis();
//    BalancedMul balancedMul;
//    balancedMul.compute(tempVec, evalModulus);
//    result = tempVec[0];
//    cout << "balanced mul time: " << util.getTimeMillis() - start << endl;
//
//    frobCompos.kill();
//    for (long i = 0; i < l; i++)
//        tempVec[i].kill();
//    tempVec.kill();
//}

void HasseLiftExt::computeFrobProduct(Mat<ZZ_pEX>& result,
                                      const Mat<ZZ_pEX>& A,
                                      long l) {

    Vec<ZZ_pEX> tempVec;
    Mat<ZZ_pEX> tempMat;

    tempVec.SetLength(2);
    tempVec[0] = A[1][0];
    tempVec[1] = A[1][1];
    tempMat = A;

    BalancedMul balancedMul;
    for (long i = l - 2; i >= 0; i--) {

        SetCoeff(tempVec[0], 0, power(coeff(tempVec[0], 0), ZZ_p::modulus()));
        SetCoeff(tempVec[0], 1, power(coeff(tempVec[0], 1), ZZ_p::modulus()));
        SetCoeff(tempVec[1], 0, power(coeff(tempVec[1], 0), ZZ_p::modulus()));

        balancedMul.mul_ZZ_pEXMatSpec(tempMat, tempVec, tempMat, evalModulus);
    }

    result = tempMat;

    tempVec.kill();
    tempMat.kill();
}

void HasseLiftExt::computeFrobProduct(Mat<ZZ_pX> &result,
                                      Mat<Vec<ZZ_pX>> &B,
                                      long l) {
    long m = B[0][0].length();
    ZZ_pX xqInit;
    SetX(xqInit);
    FrobComp frobComp;
    frobComp.computeFrobPower(xqInit, xqInit, l, frobenius, F);

    // compute tau^2, tau^4, ..., tau^{2^k}
    // where k = floor(log m)
    Vec<ZZ_pX> frobPowers;
    frobPowers.SetLength(NumBits(m - 1));
    frobPowers[0] = xqInit;
    MultiComposeMod multiComposeMod;
    for (long i = 1; i < frobPowers.length(); i++)
        multiComposeMod.compose(frobPowers[i], frobPowers[i - 1],
                                frobPowers[i - 1], F);

    for (int i = 0; i < 2; i++)
        for (int j = 0; j < 2; j++)
            actFrobPowers(B[i][j], frobPowers);

    Mat<ZZ_pX> tempMat;
    tempMat.SetDims(2, 2);

    result[0][0] = B[0][0][0];
    result[0][1] = B[0][1][0];
    result[1][0] = B[1][0][0];
    result[1][1] = B[1][1][0];

    BalancedMul balancedMul;
    for (long i = 1; i < m; i++) {
        for (int j = 0; j < 2; j++)
            for (int k = 0; k < 2; k++) {
                tempMat[j][k] = B[j][k][i];
            }

        balancedMul.mul_ZZ_pXMat(result, tempMat, result, F);
    }

    xqInit.kill();
    frobPowers.kill();
    tempMat.kill();
}

void HasseLiftExt::actFrobPowers(Vec<ZZ_pX> &vec,
                                 const Vec<ZZ_pX> &frobPowers) {
    long n = vec.length();
    long m = 1;
    long frobIndex = 0;

    Vec<ZZ_pX> tempVec;
    MultiComposeMod multiComposeMod;

    while (m < n) {
        for (long i = 0; i < n; i++) {
            if ((i & m) != 0)
                tempVec.append(vec[i]);
        }

        multiComposeMod.compose(tempVec, tempVec, frobPowers[frobIndex], F);

        long k = 0;
        for (long i = 0; i < n; i++) {
            if ((i & m) != 0) {
                vec[i] = tempVec[k];
                k++;
            }
        }

        tempVec.SetLength(0);
        m <<= 1;
        frobIndex++;
    }

    tempVec.kill();
}


#include <processor.hpp>
#include <boost/math/constants/constants.hpp>
#include <cmath>

int main()
{
	vv::processor p(44100.0);

	std::vector<float> input(vv::processor::buffer_size);

	for (std::size_t i = 0; i < vv::processor::buffer_size; ++i)
	{
		auto ratio = static_cast<double>(i) / static_cast<double>(vv::processor::buffer_size);
		auto v = std::sin(10.0 * ratio * boost::math::constants::two_pi<double>());

		input[i] = static_cast<float>(v);
	}

	std::vector<float> output(vv::processor::buffer_size);

	p(input.data(), output.data(), 1.5, 1.2);
}


#include <boost/math/distributions/inverse_gaussian.hpp>


//////////////////////////////////////////////////////////////////
// example94.cpp
//
// Copyright (c) 2015 Robert Ramey
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

// illustrate usage of safe<int> as drop-in replacement for int in
// a more complex library.  Use an example from the boost.rational
// library with modifications to use safe<int> rather than int

//  rational number example program  ----------------------------------------//

//  (C) Copyright Paul Moore 1999. Permission to copy, use, modify, sell
//  and distribute this software is granted provided this copyright notice
//  appears in all copies. This software is provided "as is" without express or
//  implied warranty, and with no claim as to its suitability for any purpose.

// boostinspect:nolicense (don't complain about the lack of a Boost license)
// (Paul Moore hasn't been in contact for years, so there's no way to change the
// license.)

//  Revision History
//  14 Dec 99  Initial version

#include <iostream>
#include <cassert>
#include <cstdlib>
#include <boost/config.hpp>
#include <limits>
#include <exception>
#include <boost/rational.hpp>

#include "../include/safe_integer.hpp"

using std::cout;
using std::endl;
using boost::rational;
using namespace boost::numeric;

using int_type = safe<int>;

int main ()
{
    rational<int_type> half(1,2);
    rational<int_type> one(1);
    rational<int_type> two(2);

    // Some basic checks
    assert(half.numerator() == 1);
    assert(half.denominator() == 2);
//    assert(boost::rational_cast<double>(half) == 0.5);

    static_assert(
        ! boost::numeric::is_safe<rational<int_type>>::value,
        "rational<int_type> is safe"
    );

    // Arithmetic
    assert(half + half == one);
    assert(one - half == half);
    assert(two * half == one);
    assert(one / half == two);

    // With conversions to integer
    assert(half+half == 1);
    assert(2 * half == one);
    assert(2 * half == 1);
    assert(one / half == 2);
    assert(1 / half == 2);

    // Sign handling
    rational<int_type> minus_half(-1,2);
    assert(-half == minus_half);
    assert(abs(minus_half) == half);

    // Do we avoid overflow?
    int maxint = (std::numeric_limits<int>::max)();
    rational<int_type> big(maxint, 2);
    assert(2 * big == maxint);

    // Print some of the above results
    cout << half << "+" << half << "=" << one << endl;
    cout << one << "-" << half << "=" << half << endl;
    cout << two << "*" << half << "=" << one << endl;
    cout << one << "/" << half << "=" << two << endl;
    cout << "abs(" << minus_half << ")=" << half << endl;
    cout << "2 * " << big << "=" << maxint
         << " (rational: " << rational<int>(maxint) << ")" << endl;

    // Some extras
//    rational<int_type> pi(22,7);
//    cout << "pi = " << boost::rational_cast<double>(pi) << " (nearly)" << endl;

    // Exception handling
    try {
        rational<int_type> r;        // Forgot to initialise - set to 0
        r = 1/r;                // Boom!
    }
    catch (const boost::bad_rational &e) {
        cout << "Bad rational, as expected: " << e.what() << endl;
    }
    catch (...) {
        cout << "Wrong exception raised!" << endl;
    }
    return 0;
}


#include <memory>
#include <unordered_set>
#include <limits>
#include <boost/multiprecision/cpp_dec_float.hpp>

namespace spdlog {
class logger;
}

namespace AsioCalc {
enum class TokenType {
    number,
    operation,
    final
};

template<class InputInteger, class ResultType>
class Evaluator {
    std::shared_ptr<spdlog::logger> logger;

    class Token {
        Token() = default;
    public:
        TokenType tokenType;
        InputInteger num{0};
        char operation{0};
        bool isFinal() const {return tokenType == TokenType::final; }

        static Token BuildFromInteger(InputInteger &&num) {
            Token t;
            t.tokenType = TokenType::number;
            t.num = std::move(num);
            return t;
        }

        static Token BuildFromOperation(char &operation) {
            Token t;
            t.tokenType = TokenType::operation;
            t.operation = std::move(operation);
            return t;
        }

        static Token FinalToken() {
            static Token final_token;
            final_token.tokenType = TokenType::final;
            return final_token;
        }
    };
    Token lastToken{Token::FinalToken()};
    std::unordered_set<char> mask;
public:
    std::string input;
    std::string::iterator it;
    std::string::iterator prev;
    Evaluator(std::string &&input_);

    Token getNextToken();
    void unread();
    ResultType prim();
    ResultType handleAddition();
    ResultType handleMultiplication();
    std::string Run();
};
}


/* =========================================================================
   Copyright (c) 2010-2016, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/** \example iterative-armadillo.cpp
*
*   The following tutorial shows how to use the iterative solvers in ViennaCL with objects from the Eigen Library directly.
*
*   \note Eigen provides its own iterative solvers in the meanwhile. Check these first.
*
*   We begin with including the necessary headers:
**/

// System headers
#include <iostream>

#ifndef NDEBUG
 #define NDEBUG
#endif

// Armadillo headers (disable BLAS and LAPACK to avoid linking issues)
#define ARMA_DONT_USE_BLAS
#define ARMA_DONT_USE_LAPACK
#include <armadillo>

// IMPORTANT: Must be set prior to any ViennaCL includes if you want to use ViennaCL algorithms on Armadillo objects
#define VIENNACL_WITH_ARMADILLO 1

// ViennaCL headers
#include "viennacl/linalg/cg.hpp"
#include "viennacl/linalg/bicgstab.hpp"
#include "viennacl/linalg/gmres.hpp"
#include "viennacl/io/matrix_market.hpp"

// Some helper functions for this tutorial:
#include "vector-io.hpp"

/**
*  In the following we run the CG method, the BiCGStab method, and the GMRES method with Armadillo types directly.
*  First, the matrices are set up, then the respective solvers are called.
**/
int main(int, char *[])
{
  typedef float ScalarType;

  /**
  * Read system from file. This is a little tricky, since Armadillo does not provide a fast enough element-insertion.
  * Therefore, we read the matrix market file to an STL-matrix and then pass the data on when creating the Armadillo sparse matrix object.
  **/
  std::vector<std::map<unsigned int, ScalarType> > stl_matrix;
  std::cout << "Reading matrix (this might take some time)..." << std::endl;
  if (!viennacl::io::read_matrix_market_file(stl_matrix, "../examples/testdata/mat65k.mtx"))
  {
    std::cout << "Error reading Matrix file. Make sure you run from the build/-folder." << std::endl;
    return EXIT_FAILURE;
  }

  // Copy over to Armadillo sparse matrix by putting the indices into a matrix and the values into a vector:
  std::size_t num_nnz = 0;
  for (std::size_t i=0; i<stl_matrix.size(); ++i)
    num_nnz += stl_matrix[i].size();

  arma::Mat<arma::uword>  arma_indices(2, num_nnz);
  arma::Col<ScalarType>   arma_values(num_nnz);

  std::size_t index = 0;
  for (std::size_t i=0; i<stl_matrix.size(); ++i)
  {
    for (std::map<unsigned int, ScalarType>::const_iterator it = stl_matrix[i].begin(); it != stl_matrix[i].end(); ++it)
    {
      arma_indices(0, index) = i;
      arma_indices(1, index) = it->first;
      arma_values(index) = it->second;
      ++index;
    }
  }
  std::cout << "Done: reading matrix" << std::endl;

  /**
  * Initialize Armadillo types for iterative solvers
  **/
  arma::SpMat<ScalarType> arma_matrix(arma_indices, arma_values, 65025, 65025);
  arma::Col<ScalarType>   arma_rhs;
  arma::Col<ScalarType>   arma_result;
  arma::Col<ScalarType>   residual;

  /**
   * Read the right hand side as well as the result vector from files:
   **/
  if (!readVectorFromFile("../examples/testdata/rhs65025.txt", arma_rhs))
  {
    std::cout << "Error reading RHS file" << std::endl;
    return EXIT_FAILURE;
  }

  if (!readVectorFromFile("../examples/testdata/result65025.txt", arma_result))
  {
    std::cout << "Error reading Result file" << std::endl;
    return EXIT_FAILURE;
  }

  /**
  *  Conjugate Gradient (CG) solver:
  **/
  std::cout << "----- Running CG -----" << std::endl;
  arma_result = viennacl::linalg::solve(arma_matrix, arma_rhs, viennacl::linalg::cg_tag());

  residual = arma_matrix * arma_result - arma_rhs;
  std::cout << "Relative residual: " << viennacl::linalg::norm_2(residual) / viennacl::linalg::norm_2(arma_rhs) << std::endl;

  /**
  *  Stabilized Bi-Conjugate Gradient (BiCGStab) solver:
  **/
  std::cout << "----- Running BiCGStab -----" << std::endl;
  arma_result = viennacl::linalg::solve(arma_matrix, arma_rhs, viennacl::linalg::bicgstab_tag());

  residual = arma_matrix * arma_result - arma_rhs;
  std::cout << "Relative residual: " << viennacl::linalg::norm_2(residual) / viennacl::linalg::norm_2(arma_rhs) << std::endl;

  /**
  *  Generalized Minimum Residual (GMRES) solver:
  **/
  std::cout << "----- Running GMRES -----" << std::endl;
  arma_result = viennacl::linalg::solve(arma_matrix, arma_rhs, viennacl::linalg::gmres_tag());

  residual = arma_matrix * arma_result - arma_rhs;
  std::cout << "Relative residual: " << viennacl::linalg::norm_2(residual) / viennacl::linalg::norm_2(arma_rhs) << std::endl;

  /**
  *   That's it. Print a success message and exit.
  **/
  std::cout << std::endl;
  std::cout << "!!!! TUTORIAL COMPLETED SUCCESSFULLY !!!!" << std::endl;
  std::cout << std::endl;
}


#include <Eigen/Geometry>

namespace lidar_obstacle_detection
{

  struct BoxQ
  {
    Eigen::Vector3f bbox_transform;
    Eigen::Quaternionf bbox_quaternion;
    float cube_length;
    float cube_width;
    float cube_height;
  };

  struct Box
  {
    float x_min;
    float y_min;
    float z_min;
    float x_max;
    float y_max;
    float z_max;
  };

}


/////////////////////////////////////////////////////////////////////////////////////////////
// Copyright (c) 2021 Andreas Milton Maniotis.
//
// Email: andreas.maniotis@gmail.com
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
/////////////////////////////////////////////////////////////////////////////////////////////

// clang++ -std=c++20 -Wall -pedantic -I../include t0.cpp

#include "aml/find.hpp"
#include "aml/sort.hpp"
#include "aml/partition.hpp"
#include <boost/core/demangle.hpp>
#include <iostream>

#include "aml/select_indices.hpp"
#include "aml/linker.hpp"
#include "aml/structure/term.hpp"
#include "aml/structure/list.hpp"

using aml::operator""_;

template<typename... x>
using term = aml::signatures
             <
                 aml::subtype<decltype("list"_), aml::structure::list>,
                 aml::subtype<decltype("term"_), aml::structure::term>
             >::link_with<x...>;

template<typename... x>
using term2 = typename
    aml::signatures
    <
       aml::subtype<::term<>, ::term >::ignore_linker
    >::
    template link_with<x...>::template subtype< ::term<> >;

template<typename... >
struct foo { };

template<template<typename...> class F>
struct Template { };

template<typename...>
struct Parameters { };

template<int n>
struct tt
{
    using type = tt<n+1>;
};

template<typename...> class F { };

template<int n, typename X>
struct numbered_item;

template<typename X, typename ConsList>
using make_right_indexed = typename ConsList::template cons< numbered_item< ConsList::size(), X> > ;

template<typename ConsList, typename X>
using make_left_indexed = make_right_indexed<X, ConsList>;

using numbered_right = aml::conslist<int, char, double>::rfold_with<make_right_indexed, aml::conslist<> >;
using numbered_left = aml::conslist<int, char, double>::lfold_with<make_left_indexed, aml::conslist<> >;
using numbered_nicely = aml::conslist<int, char, double>::reverse::lfold_with<make_left_indexed, aml::conslist<> >::reverse;

template<typename T>
struct is_integral
{
    static constexpr bool eval() { return std::is_integral<T>::value; };
};

template<typename T>
struct pred
{
    static constexpr bool eval() { return sizeof(T) == 1; }
};

using partition_t = aml::partition<char, int, void*, unsigned char, std::nullptr_t>::with<pred>;

using t_join = aml::join< partition_t::accepted, partition_t::rejected, aml::conslist<void, void> >;

template<typename X, typename Y>
struct less
{
    static constexpr bool eval() { return sizeof(X) < sizeof(Y); }
};

using tlist = aml::conslist<double, char, float, unsigned char>;
//using tlist = aml::conslist<char, double>::reverse;

using slist = tlist::template apply< aml::sort >::template with<less>;

int main()
{
    std::cout << "::term2: " << std::endl;
    std::cout << boost::core::demangle(typeid(::term2<foo<int>>*).name()) << std::endl;

    std::cout << "::term: " << std::endl;
    std::cout << boost::core::demangle(typeid(::term<foo<int>>*).name()) << std::endl;

    using ttt = aml::find<is_integral>::in<void*, double, int, long, void*>;

    std::cout << boost::core::demangle(typeid(ttt).name()) << std::endl;

    std::cout << boost::core::demangle( typeid(tlist***).name() ) << std::endl;
    std::cout << boost::core::demangle( typeid(slist***).name() ) << std::endl << std::endl;

    std::cout << "partition accept: " << boost::core::demangle( typeid(partition_t::accepted* ).name() ) << std::endl
              << "partition reject: " << boost::core::demangle( typeid(partition_t::rejected* ).name() ) << std::endl << std::endl;

    using type = foo<foo<int, char, foo<> > >;
    using term_t = term<type>;

    std::cout << boost::core::demangle( typeid(term_t::function<>).name()  ) << std::endl;
    std::cout << boost::core::demangle( typeid(term_t::subterms::apply<Parameters>).name() ) << std::endl;
    std::cout << "t_join: " << boost::core::demangle( typeid(t_join).name() ) << std::endl;

    //    static_assert(std::is_same< ::term2<foo<int> >, ::term<foo<int>> >::value, "" );

    using t0 = aml::conslist<int, void, char>;

    using t1 = t0::tail::tail::head;

    static_assert(std::is_same<t1, char>::value, "");

    using t2 = aml::select_indices<2, 4, 2>::from<int, char, double, void, char*, void*>;

    std::cout << boost::core::demangle( typeid(t2).name() )  << std::endl;

    //    using t3 = numbered;

    std::cout << "          left fold:  " << boost::core::demangle(typeid(numbered_left*).name()) << std::endl;
    std::cout << "          right fold: " << boost::core::demangle(typeid(numbered_right*).name()) << std::endl;
    std::cout << "nicely numbered fold: " << boost::core::demangle(typeid(numbered_nicely*).name()) << std::endl;

    //    using t3 = aml::function::power< aml::exp<0>, F >::apply_to<char**>;

    //    static_assert(std::is_same<char**, t3>::value, "");

    /*
    using t3 = aml::tail< aml::tail< aml::conslist<tt<0>, tt<1>, tt<2> > > >::head;

    using t4 = aml::function::power< aml::exp<0>, aml::tail >::apply_to< aml::conslist<tt<0>, tt<1>, tt<2> > >;
    using t44 = aml::function::power< aml::exp<0>, aml::tail >::apply_to< aml::conslist< tt<0> > >;

    std::cout << "t4: " << boost::core::demangle( typeid(t4*).name() )  << std::endl;
    std::cout << "t44: " << boost::core::demangle( typeid(t44*).name() )  << std::endl;

    using t5 = aml::power<aml::exp<1>, tt<0>>;
    */
    //    using t5 = aml::function::power< aml::exp<2>, F>::template apply_to<int, char>;

    //    std::cout << boost::core::demangle(typeid(t5).name()) << std::endl;
    //    std::cout << boost::core::demangle(typeid(t5).name()) << std::endl;

    //    static_assert( std::is_same<aml::head<int, void>, int>::value, "") ;

    //    static_assert( std::is_same< aml::tail<void, int>::template apply< aml::identity >, int >::value, "");

    //    using t6 = aml::function::power<aml::exp<2>, aml::tail>::template apply_to<void, tt<0>, tt<1>, tt<2>, tt<3> >; //::template apply<aml::head>;

}


#ifndef PARMCB_DETAIL_FVS_HPP_
#define PARMCB_DETAIL_FVS_HPP_

//    Copyright (C) Dimitrios Michail 2019 - 2021.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          https://www.boost.org/LICENSE_1_0.txt)

#include <iostream>

#include <boost/scoped_array.hpp>
#include <boost/throw_exception.hpp>
#include <boost/functional/hash.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/property_map/function_property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/heap/pairing_heap.hpp>

namespace parmcb {

    namespace detail {

        template<class Vertex>
        struct LessVertex {
            LessVertex(std::map<Vertex, double> &priority) :
                    priority(priority) {
            }

            bool operator()(const Vertex &v, const Vertex &u) const {
                return priority[v] >= priority[u];
            }

            std::map<Vertex, double> &priority;
        };

    } // detail

    template<class Graph, class VertexOutputIterator>
    void greedy_fvs(const Graph &g, VertexOutputIterator out) {

        typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
        typedef typename boost::graph_traits<Graph>::vertex_iterator VertexIt;
        typedef typename boost::property_map<Graph, boost::vertex_index_t>::type VertexIndexMapType;
        typedef typename boost::heap::pairing_heap<Vertex, boost::heap::compare<detail::LessVertex<Vertex>>>::handle_type HeapHandleType;

        std::size_t n = boost::num_vertices(g);
        std::vector<bool> exists(n);
        std::vector<std::size_t> degree(n);
        std::vector<HeapHandleType> handle(n);
        std::map<Vertex, double> priority;
        boost::heap::pairing_heap<Vertex, boost::heap::compare<detail::LessVertex<Vertex>>> heap(
                detail::LessVertex<Vertex> { priority });

        const VertexIndexMapType &index_map = boost::get(boost::vertex_index, g);
        boost::associative_property_map<std::map<Vertex, double>> priority_map(priority);

        // initialize
        std::deque<Vertex> forRemoval;
        VertexIt vi, viend;
        for (boost::tie(vi, viend) = boost::vertices(g); vi != viend; ++vi) {
            auto v = *vi;
            auto vindex = index_map[v];
            auto d = boost::out_degree(v, g);
            exists[vindex] = true;
            if ((degree[vindex] = d) <= 1) {
                forRemoval.push_front(v);
            } else {
                priority[v] = 1.0 / d;
            }
        }

        // cleanup
        // repeatedly remove degree 0 or 1
        while (!forRemoval.empty()) {
            Vertex u = forRemoval.front();
            forRemoval.pop_front();
            auto uindex = index_map[u];
            exists[uindex] = false;

            auto eiRange = boost::out_edges(u, g);
            for (auto ei = eiRange.first; ei != eiRange.second; ++ei) {
                auto w = boost::target(*ei, g);
                auto windex = index_map[w];
                if (!exists[windex]) {
                    continue;
                }
                degree[windex]--;
                if (degree[windex] <= 1) {
                    // collect for removal
                    forRemoval.push_front(w);
                } else {
                    priority[w] = 1.0 / degree[windex];
                }
            }
        }

        // add remaining vertices into the priority queue
        for (boost::tie(vi, viend) = boost::vertices(g); vi != viend; ++vi) {
            auto v = *vi;
            auto vindex = index_map[v];
            if (!exists[vindex]) {
                continue;
            }
            handle[vindex] = heap.push(v);
        }

        // main loop
        while (!heap.empty()) {
            auto v = heap.top();
            auto vindex = index_map[v];
            heap.pop();

            if (!exists[vindex]) {
                continue;
            }

            // add to feedback vertex set
            *out++ = v;

            // remove from graph
            exists[vindex] = false;

            auto eiRange = boost::out_edges(v, g);
            for (auto ei = eiRange.first; ei != eiRange.second; ++ei) {
                auto u = boost::target(*ei, g);
                auto uindex = index_map[u];
                if (!exists[uindex]) {
                    continue;
                }
                degree[uindex]--;
                if (degree[uindex] <= 1) {
                    // collect for removal
                    forRemoval.push_front(u);
                } else {
                    priority[u] = 1.0 / degree[uindex];
                    heap.decrease(handle[uindex]);
                }
            }

            // cleanup
            while (!forRemoval.empty()) {
                Vertex u = forRemoval.front();
                forRemoval.pop_front();
                auto uindex = index_map[u];
                exists[uindex] = false;

                eiRange = boost::out_edges(u, g);
                for (auto ei = eiRange.first; ei != eiRange.second; ++ei) {
                    auto w = boost::target(*ei, g);
                    auto windex = index_map[w];
                    if (!exists[windex]) {
                        continue;
                    }
                    degree[windex]--;
                    if (degree[windex] <= 1) {
                        // collect for removal
                        forRemoval.push_front(w);
                    } else {
                        priority[w] = 1.0 / degree[windex];
                        heap.decrease(handle[windex]);
                    }

                }

            }

        }

    }

} // parmcb

#endif


// Copyright (C) 2011-2012 The Trustees of Indiana University.

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

//  Authors: Jeremiah Willcock
//           Andrew Lumsdaine

#ifndef BOOST_SPLITTABLE_ECUYER1988_HPP
#define BOOST_SPLITTABLE_ECUYER1988_HPP

#include <boost/cstdint.hpp>
#include <boost/iterator.hpp>
#include <boost/iterator/iterator_facade.hpp>
#include <utility>

namespace boost { namespace graph { namespace random {

    template <typename Base> // To get around incomplete class issues
    class generic_split_iterator: public boost::iterator_facade<generic_split_iterator<Base>, Base, std::input_iterator_tag, Base> {
      public:
      Base base;
      uint32_t new_multiplier1, new_multiplier2;

      generic_split_iterator(Base base, uint32_t new_multiplier1, uint32_t new_multiplier2)
	: base(base), new_multiplier1(new_multiplier1), new_multiplier2(new_multiplier2) {}

      generic_split_iterator(): base(), new_multiplier1(0), new_multiplier2(0) {}

      Base dereference() const {
	Base result = base;
	result.multiplier1 = new_multiplier1;
	result.multiplier2 = new_multiplier2;
	return result;
      }

      bool equal(const generic_split_iterator& it) const {
	return base.state1 == it.base.state1 && base.state2 == it.base.state2;
      }

      void increment() {
	base(); // Advance seeds
      }
    };

    class splittable_ecuyer1988 {
      uint32_t state1, state2, multiplier1, multiplier2;

      static const uint64_t modulus1 = 2147483563;
      static const uint64_t modulus2 = 2147483399;

      template <uint64_t Modulus>
      static uint32_t mul_mod(uint32_t a, uint32_t b) {
	return uint32_t((uint64_t(a) * b) % Modulus);
      }

      template <uint64_t Modulus>
      static uint32_t exp_mod(uint32_t a, uint32_t b) {
	if (b == 0) {
	  return 1;
	} else if (b == 1) {
	  return a;
	} else {
	  uint32_t temp = exp_mod<Modulus>(a, b>>1);
	  temp = mul_mod<Modulus>(temp, temp);
	  if (b % 2) temp = mul_mod<Modulus>(a, temp);
	  return temp;
	}
      }

      public:
      splittable_ecuyer1988(): state1(1), state2(1), multiplier1(40014), multiplier2(40692) {}
      splittable_ecuyer1988(uint32_t seed1, uint32_t seed2): state1(seed1), state2(seed2), multiplier1(40014), multiplier2(40692) {}

      void seed(uint32_t seed1, uint32_t seed2) {
	state1 = seed1;
	state2 = seed2;
      }

      void split(splittable_ecuyer1988& a, splittable_ecuyer1988& b) const {
	a = *this;
	a.multiplier1 = mul_mod<modulus1>(a.multiplier1, a.multiplier1);
	a.multiplier2 = mul_mod<modulus2>(a.multiplier2, a.multiplier2);
	b = *this;
	b();
	b.multiplier1 = a.multiplier1;
	b.multiplier2 = a.multiplier2;
      }

      typedef generic_split_iterator<splittable_ecuyer1988> split_iterator;
      typedef std::pair<split_iterator, split_iterator> split_iterator_pair;
      friend class generic_split_iterator<splittable_ecuyer1988>;

      split_iterator_pair split_n(uint32_t n) const { // This RNG is not usable after the split
	split_iterator begin(*this, exp_mod<modulus1>(multiplier1, n), exp_mod<modulus2>(multiplier2, n));
	split_iterator end(*this, begin.new_multiplier1, begin.new_multiplier2);
	end.base.state1 = mul_mod<modulus1>(end.base.state1, end.new_multiplier1);
	end.base.state2 = mul_mod<modulus2>(end.base.state2, end.new_multiplier2);
	return std::make_pair(begin, end);
      }

      split_iterator_pair split_off_n(uint32_t n) { // Allows this RNG to keep being used
	split_iterator_pair p = split_n(n + 1);
	*this = *p.first;
	++p.first;
	return p;
      }

      typedef uint32_t result_type;

      uint32_t operator()() {
	state1 = mul_mod<modulus1>(state1, multiplier1);
	state2 = mul_mod<modulus2>(state2, multiplier2);
	return (state1 + modulus1 - 1 - state2) % (modulus1 - 1);
      }

      BOOST_STATIC_CONSTANT(bool, has_fixed_range = true);
      BOOST_STATIC_CONSTANT(uint32_t, min_value = 0);
      BOOST_STATIC_CONSTANT(uint32_t, max_value = modulus1 - 1);
      uint32_t min BOOST_PREVENT_MACRO_SUBSTITUTION () const {return min_value;}
      uint32_t max BOOST_PREVENT_MACRO_SUBSTITUTION () const {return max_value;}

      static bool validation(uint32_t x) {
	return x == 831582319;
      }
    };

    template <typename Gen>
    class uniform_01_wrapper {
      Gen& gen;

      public:
      uniform_01_wrapper(Gen& gen): gen(gen) {}

      double operator()() const {return boost::uniform_01<double>()(gen);}
    };

} } } // end namespace boost::graph::random

#endif // BOOST_SPLITTABLE_ECUYER1988_HPP


#include <cmath>

#include <algorithm>

#include <boost/program_options.hpp>

#include "flpmc.h"
#include "../yche_refactor/simrank.h"
#include "../playground/pretty_print.h"

using namespace std;
using namespace boost::program_options;

FLPMC::FLPMC(string g_name_, DirectedG &g_, double c_, double epsilon_, double delta_, size_t Q_) :
        g_name(g_name_),
        g(&g_),
        c(c_),
        epsilon(epsilon_),
        delta(delta_),
        Q(Q_) {
    // init local push
    size_t n = num_vertices(*g);
    cout << format("r_max: %s") % get_rmax() << endl;
    lp = new Reduced_LocalPush(*g, g_name, c, get_lp_epsilon(), n);
    // lp = new Full_LocalPush(*g, g_name, c, get_lp_epsilon(), n);
    if (!lp_file_exists(g_name, c, get_lp_epsilon(), n, false)) { // test wether the local push index exists
        cout << "local push offline index doesn't exists.. " << endl;
        lp->local_push(*g);
        cout << format("building compete, saving to %s ") % lp->get_file_path_base() << endl;
        lp->save();
        cout << "saved." << endl;
    } else {
        cout << "offline index exists..loading " << endl;
        lp->load();
    }
}

double FLPMC::get_rmax() {
    double r = sqrt(epsilon);
//    cout << format("r_max of local push: %s") % r << endl;
    return r;
}

double FLPMC::get_lp_epsilon() {
    double lp_epsilon = get_rmax() * (1 - c);
//    cout << format("local push error: %s") % lp_epsilon << endl;
    return lp_epsilon;
}

double FLPMC::query_one2one(NodePair np) {
//    cout << np << endl;
    if (np.first == np.second) {
        return 1;
    } else if (np.first > np.second) { // make sure np.first < np.second
        np = NodePair({np.second, np.first});
    }
    /* FLP result */
//    double p_i = lp->P[np];
    double p_i = lp->query_P(np.first, np.second);

    // set up the random number generator
#ifndef SFMT
    std::random_device rd;  //Will be used to obtain a seed for the random number engine
    std::mt19937 gen(rd()); //Standard mersenne_twister_engine seeded with rd()
    std::uniform_real_distribution<double> distribution(0.0, 1.0);
#endif

    /* MC sampling phase */
    int N = get_N();
    double E_residual = 0;
    for (int i = 0; i < N; i++) {
        double sum = 0; // the sum of residuals that passed by
        int a = np.first;
        int b = np.second;
        sum += lp->query_R(a, b);
#ifndef SFMT
        while (distribution(gen) < c && (a != b)) {
            a = sample_in_neighbor(a, *g);
            b = sample_in_neighbor(b, *g);
#else
        while (rand_gen.double_rand() < c && (a != b)) {
            a = sample_in_neighbor(a, *g, rand_gen);
            b = sample_in_neighbor(b, *g, rand_gen);
#endif
            if (a == -1 || b == -1) {
                break;
            }
            sum += lp->query_R(a, b);
        }
        E_residual += sum;
    }

//    cout << format("p_i: %s") % p_i << endl;
//    cout << format("number of samples: %s") % get_N() << endl;
    return p_i + E_residual / N;
}

double FLPMC::get_N() {
    // compute the number of needed samples
    double tmp = (pow(c, 2.) * pow(get_rmax(), 2.) * log(2. / delta)) / (2 * pow(epsilon, 2.) * pow((1 - c), 2.));
    int n = ceil(tmp);
    return n;
}

FLPMC::FLPMC(const FLPMC &other_obj) {
    rand_gen = SFMTRand();
    g_name = other_obj.g_name;
    Q = other_obj.Q;
    epsilon = other_obj.epsilon; // the error bound required by query
    delta = other_obj.delta;
    c = other_obj.c;
    g = other_obj.g; // the pointer to the graph
    lp = other_obj.lp; // the pointer to the local push index
}


#pragma once

#include <iron/core.h>

#include <boost/container/flat_set.hpp>

namespace iron {

struct PrimeStash {

    PrimeStash() : Primes() {
      Primes.insert(2);
    }

  // For now, strictly positive. Could be genericized and pulled out of PrimeStash
  template <typename IntegerContainer_t>
  static bool isDivisibleByAnyOf(uint64_t inEvaluated, IntegerContainer_t inValues) {
    for (auto trial : inValues) {
      if (inEvaluated % trial == 0){
        return true;
      }
    }
    return false;
  }

  uint64_t lastStashed()                  const { return *Primes.crbegin(); }
  bool     canEvaluate(uint64_t inNumber) const { return lastStashed()*lastStashed() >= inNumber; }

  boost::container::flat_set<uint64_t> Primes;
};

} /* iron */


#include "EigenRowVector.h"
#include <Eigen/Dense>

typedef Eigen::Matrix<double, Eigen::Dynamic, 1> ColumnVector;
typedef Eigen::Matrix<double, 1, Eigen::Dynamic> RowVector;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Matrix;

void* EigenRowVector_CreateVector(int columns)
{
	auto v = new RowVector(1, columns);
	for (int i = 0; i < columns; i++)
		(*v)(i) = 0;

	return v;
}

void EigenRowVector_Release(void* ptr)
{
	auto obj = static_cast<RowVector*>(ptr);
	if (obj != nullptr)
	{
		delete obj;
		obj = nullptr;
	}
}

RowVector* CastToRowVector(void* ptr)
{
	return static_cast<RowVector*>(ptr);
}

RowVector* NewRowVector(const RowVector& v)
{
	return new RowVector(1, v.cols());
}

RowVector* NewRowVector()
{
	return new RowVector();
}

ColumnVector* NewColumnVector();

int EigenRowVector_Dimension(void* ptr)
{
	auto m = CastToRowVector(ptr);
	return (int)m->cols();
}

double EigenRowVector_Get(void* ptr, int x)
{
	auto m = CastToRowVector(ptr);
	return (*m)(x);
}

void EigenRowVector_Set(void* ptr, int x, double value)
{
	auto m = CastToRowVector(ptr);
	(*m)(x) = value;
}

double EigenRowVector_Dot(void* ptr1, void* ptr2)
{
	auto v1 = CastToRowVector(ptr1);
	auto v2 = CastToRowVector(ptr2);

	return v1->dot(*v2);
}

void* EigenRowVector_Normalized(void* ptr)
{
	auto v = CastToRowVector(ptr);
	auto n = NewRowVector(*v);

	(*n) = v->normalized();
	return n;
}

void EigenRowVector_Normalize(void* ptr)
{
	auto v = CastToRowVector(ptr);
	v->normalize();
}

double EigenRowVector_Norm(void* ptr)
{
	auto v = CastToRowVector(ptr);
	return v->norm();
}

void* EigenRowVector_Transpose(void* ptr)
{
	auto v1 = CastToRowVector(ptr);
	auto v2 = NewColumnVector();
	(*v2) = v1->transpose();
	return v2;
}

void* EigenRowVector_Adjoint(void* ptr)
{
	auto v1 = CastToRowVector(ptr);
	auto v2 = NewRowVector();
	(*v2) = v1->adjoint();
	return v2;
}

void* EigenRowVector_Conjugate(void* ptr)
{
	auto v1 = CastToRowVector(ptr);
	auto v2 = NewRowVector();
	(*v2) = v1->conjugate();
	return v2;
}

void EigenRowVector_Resize(void* ptr, int dimension)
{
	auto v = CastToRowVector(ptr);
	v->resize(dimension);
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_MAX_OF_SUMS_INCLUDE
#define MTL_MAX_OF_SUMS_INCLUDE

#include <boost/numeric/mtl/concept/magnitude.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/linear_algebra/operators.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/utility/range_generator.hpp>
#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

#include <numeric>
#include <cmath>

namespace mtl { namespace impl {

// We need property map of the minor index
template <typename Matrix, typename MinorIndex>
typename RealMagnitude<typename Collection<Matrix>::value_type>::type
inline max_of_sums(const Matrix& matrix, bool aligned, MinorIndex minor_index, std::size_t dim2)
{
    vampir_trace<2012> tracer;
    using std::max; using std::abs; using math::zero;

    typedef typename Collection<Matrix>::value_type   value_type;
    typedef typename RealMagnitude<value_type>::type  real_type;
    real_type ref, my_zero= zero(ref);

    // If matrix is empty then the result is the identity from the default-constructed value
    if (num_rows(matrix) == 0 || num_cols(matrix) == 0)
	return my_zero;

    typedef typename traits::range_generator<tag::major, Matrix>::type     cursor_type;
    typedef typename traits::range_generator<tag::nz, cursor_type>::type   icursor_type;
    typename traits::const_value<Matrix>::type                             value(matrix);

    if (aligned) {
	real_type maxv= my_zero;
	for (cursor_type cursor = begin<tag::major>(matrix), cend = end<tag::major>(matrix); cursor != cend; ++cursor) {
	    real_type sum= my_zero;
	    for (icursor_type icursor = begin<tag::nz>(cursor), icend = end<tag::nz>(cursor); icursor != icend; ++icursor)
		sum+= abs(value(*icursor));
	    maxv= max(maxv, sum);
	}
	return maxv;
    }

    // If matrix has other orientation, we compute all sums in a vector
    dense_vector<real_type>   sums(dim2, my_zero);
    for (cursor_type cursor = begin<tag::major>(matrix), cend = end<tag::major>(matrix); cursor != cend; ++cursor)
	for (icursor_type icursor = begin<tag::nz>(cursor), icend = end<tag::nz>(cursor); icursor != icend; ++icursor)
	    sums[minor_index(*icursor)]+= abs(value(*icursor));
    // replace by mtl::accumulate<8>
    return std::accumulate(sums.begin(), sums.end(), my_zero, math::max<real_type>());
}

}} // namespace mtl::impl

#endif // MTL_MAX_OF_SUMS_INCLUDE


#include <QtCore/qglobal.h>

#include <CGAL/Three/Polyhedron_demo_plugin_interface.h>

#include "Scene_polyhedron_item.h"
#include "Scene_polyhedron_selection_item.h"
#include "Polyhedron_type.h"

#include <CGAL/iterator.h>
#include <CGAL/boost/graph/graph_traits_Polyhedron_3.h>
#include <CGAL/boost/graph/properties_Polyhedron_3.h>
#include <CGAL/utility.h>

#include <CGAL/Polygon_mesh_processing/random_perturbation.h>

#include <boost/graph/graph_traits.hpp>
#include <CGAL/property_map.h>

#include <QTime>
#include <QAction>
#include <QMainWindow>
#include <QApplication>
#include <QString>
#include <QInputDialog>
#include <QtPlugin>
#include <QMessageBox>

#include "ui_Random_perturbation_dialog.h"

using namespace CGAL::Three;
class Polyhedron_demo_random_perturbation_plugin :
  public QObject,
  public Polyhedron_demo_plugin_interface
{
  Q_OBJECT
  Q_INTERFACES(CGAL::Three::Polyhedron_demo_plugin_interface)
  Q_PLUGIN_METADATA(IID "com.geometryfactory.PolyhedronDemo.PluginInterface/1.0")

public:
  void init(QMainWindow* mainWindow,
            Scene_interface* scene_interface,
            Messages_interface*)
  {
    this->scene = scene_interface;
    this->mw = mainWindow;

    actionRandomPerturbation_ = new QAction("Random perturbation", mw);
    actionRandomPerturbation_->setProperty("subMenuName", "Polygon Mesh Processing");
    if (actionRandomPerturbation_) {
      connect(actionRandomPerturbation_, SIGNAL(triggered()),
        this, SLOT(random_perturb()));
    }
  }

  QList<QAction*> actions() const {
    return QList<QAction*>() << actionRandomPerturbation_;
  }

  bool applicable(QAction*) const
  {
    const Scene_interface::Item_id index = scene->mainSelectionIndex();
    if (qobject_cast<Scene_polyhedron_item*>(scene->item(index)))
      return true;
    else if (qobject_cast<Scene_polyhedron_selection_item*>(scene->item(index)))
      return true;
    else
      return false;
  }

public Q_SLOTS:
  void random_perturb()
  {
    const Scene_interface::Item_id index = scene->mainSelectionIndex();
    Scene_polyhedron_item* poly_item =
      qobject_cast<Scene_polyhedron_item*>(scene->item(index));
    Scene_polyhedron_selection_item* selection_item =
      qobject_cast<Scene_polyhedron_selection_item*>(scene->item(index));

    //Create dialog box
    QDialog dialog(mw);
    Ui::Random_perturbation_dialog ui
      = perturb_dialog(&dialog, poly_item, selection_item);

    //Get values
    int i = dialog.exec();
    if (i == QDialog::Rejected)
    {
      std::cout << "Perturbation aborted" << std::endl;
      return;
    }

    double max_move = ui.moveSize_dspinbox->value();
    bool project = ui.project_checkbox->isChecked();

    // wait cursor
    QApplication::setOverrideCursor(Qt::WaitCursor);
    QTime time;
    time.start();

    std::cout << "Perturbation..." << std::endl;

    namespace PMP = CGAL::Polygon_mesh_processing;

    if (poly_item)
    {
      Polyhedron& pmesh = *poly_item->polyhedron();
      if(ui.deterministic_checkbox->isChecked())
      {
        unsigned int seed = static_cast<unsigned int>(ui.seed_spinbox->value());
        PMP::random_perturbation(pmesh, max_move,
            PMP::parameters::do_project(project)
            .random_seed(seed));
      }
      else
      {
        PMP::random_perturbation(pmesh, max_move,
            PMP::parameters::do_project(project));
      }

      poly_item->invalidateOpenGLBuffers();
      Q_EMIT poly_item->itemChanged();
    }
    else if (selection_item)
    {
      if (selection_item->selected_vertices.empty())
      {
        QMessageBox msg(QMessageBox::Warning,
          QString("Empty selection"),
          QString("Selection of vertices is empty.\nPerturbation aborted."),
          QMessageBox::Ok,
          this->mw);
        msg.exec();
        QApplication::restoreOverrideCursor();
        return;
      }
      Polyhedron& pmesh = *selection_item->polyhedron();
      if (ui.deterministic_checkbox->isChecked())
      {
        unsigned int seed = static_cast<unsigned int>(ui.seed_spinbox->value());
        PMP::random_perturbation(
            selection_item->selected_vertices,
            pmesh,
            max_move,
            PMP::parameters::do_project(project)
            .random_seed(seed));
      }
      else
      {
        std::cout << "selection_item->selected_vertices : "
          << selection_item->selected_vertices.size() << std::endl;
        PMP::random_perturbation(
          selection_item->selected_vertices,
          pmesh,
          max_move,
          PMP::parameters::do_project(project));
      }

      selection_item->invalidateOpenGLBuffers();
      Q_EMIT selection_item->itemChanged();
      selection_item->polyhedron_item()->invalidateOpenGLBuffers();
      Q_EMIT selection_item->polyhedron_item()->itemChanged();
    }
    else
      std::cerr << "Can't perturb that type of item" << std::endl;

    std::cout << " ok (" << time.elapsed() << " ms)" << std::endl;

    // default cursor
    QApplication::restoreOverrideCursor();
  }

  Ui::Random_perturbation_dialog
  perturb_dialog(QDialog* dialog,
                 Scene_polyhedron_item* poly_item,
                 Scene_polyhedron_selection_item* selection_item)
  {
    Ui::Random_perturbation_dialog ui;
    ui.setupUi(dialog);
    connect(ui.buttonBox, SIGNAL(accepted()), dialog, SLOT(accept()));
    connect(ui.buttonBox, SIGNAL(rejected()), dialog, SLOT(reject()));

    //Set default parameters
    Scene_interface::Bbox bbox = poly_item != NULL ? poly_item->bbox()
      : (selection_item != NULL ? selection_item->bbox()
      : scene->bbox());
    ui.objectName->setText(poly_item != NULL ? poly_item->name()
      : (selection_item != NULL ? selection_item->name()
      : QString("Remeshing parameters")));

    ui.objectNameSize->setText(
      tr("Object bbox size (w,h,d):  %1,  %2,  %3")
      .arg(bbox.xmax() - bbox.xmin(), 0, 'g', 3)
      .arg(bbox.ymax() - bbox.ymin(), 0, 'g', 3)
      .arg(bbox.zmax() - bbox.zmin(), 0, 'g', 3));

    double diago_length = CGAL::sqrt((bbox.xmax() - bbox.xmin())*(bbox.xmax() - bbox.xmin())
      + (bbox.ymax() - bbox.ymin())*(bbox.ymax() - bbox.ymin())
      + (bbox.zmax() - bbox.zmin())*(bbox.zmax() - bbox.zmin()));
    double log = std::log10(diago_length);
    unsigned int nb_decimals = (log > 0) ? 5 : (std::ceil(-log) + 3);

    //parameters
    ui.moveSize_dspinbox->setDecimals(nb_decimals);
    ui.moveSize_dspinbox->setSingleStep(1e-3);
    ui.moveSize_dspinbox->setRange(0., diago_length);
    ui.moveSize_dspinbox->setValue(0.05 * diago_length);

    ui.project_checkbox->setChecked(true);

    //advanced
    ui.deterministic_checkbox->setChecked(false);
    ui.seed_spinbox->setEnabled(false);
    ui.seed_label->setEnabled(false);
    ui.seed_spinbox->setValue(0);
    ui.seed_spinbox->setMinimum(0);

    connect(ui.deterministic_checkbox, SIGNAL(toggled(bool)),
            ui.seed_spinbox, SLOT(setEnabled(bool)));
    connect(ui.deterministic_checkbox, SIGNAL(toggled(bool)),
            ui.seed_label, SLOT(setEnabled(bool)));

    return ui;
  }

private:
  Scene_interface *scene;
  QMainWindow* mw;
  QAction* actionRandomPerturbation_;

}; // end Polyhedron_demo_random_perturbation_plugin

#include "Random_perturbation_plugin.moc"


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      110621    F.M. Engelen      File created.
 *      110707    E.A.G. Heeren     Minor spelling/lay-out corrections.
 *      110714    E.A.G. Heeren     Minor spelling/lay-out corrections.
 *      110905    S. Billemont      Reorganized includes.
 *                                  Moved (con/de)structors and getter/setters to header.
 *      120529    E.A.G. Heeren     Boostified unit test.
 *      120615    T. Secretin       Added check for exception handling.
 *      120716    D. Dirkx          Updated with interpolator architecture.
 *
 *    References
 *
 *    Notes
 *
 */

#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

#include <vector>

#include "Tudat/Basics/testMacros.h"
#include "Tudat/InputOutput/matrixTextFileReader.h"

#include "Tudat/Mathematics/Interpolators/cubicSplineInterpolator.h"
#include "Tudat/InputOutput/basicInputOutput.h"

namespace tudat
{
namespace unit_tests
{

// NOTE: No benchmark data from established software package has been used. Matlab implementation
// is slightly different than that implemented here, yielding error of at worst 10e-7 compared
// when using error function, which is used here as test function.
// However, a Matlab implementation of the same algorithm was found on te Matlab forum,
// which is used here as benchmark.
// In addition, the code has been used rather extensively and has been checked for basic
// characteristics such as continuity.
BOOST_AUTO_TEST_SUITE( test_cubic_spline_interpolator )

// Test implementation of cubic spline class.
BOOST_AUTO_TEST_CASE( testCubicSplineInterpolator )
{
    // Test 1: Compare with analytical function 2 + 3x + 5x^2.
    {
        // Declare and initialize independent variable values.
        std::vector< double > independentVariables;
        independentVariables.resize( 6 );
        independentVariables[ 0 ] = 1.0;
        independentVariables[ 1 ] = 3.0;
        independentVariables[ 2 ] = 5.0;
        independentVariables[ 3 ] = 7.0;
        independentVariables[ 4 ] = 9.0;
        independentVariables[ 5 ] = 11.0;

        // Declare and initialize dependent variable values.
        std::vector< double > dependentVariables;
        dependentVariables.resize( 6 );
        dependentVariables[ 0 ] = 10.0;
        dependentVariables[ 1 ] = 56.0;
        dependentVariables[ 2 ] = 142.0;
        dependentVariables[ 3 ] = 268.0;
        dependentVariables[ 4 ] = 434.0;
        dependentVariables[ 5 ] = 640.0;

        // Declare and initialize target independent variable value.
        const double targetIndependentVariableValue = 6.0;

        // Declare and initialize expected result of interpolation from analytical equation.
        const double analyticalValue = 200.0;

        // Declare cubic spline object and initialize with input data.
        interpolators::CubicSplineInterpolatorDouble cubicSplineInterpolation(
                    independentVariables, dependentVariables );

        // Declare interpolated dependent variable value and execute interpolation.
        const double interpolatedDependentVariableValue = cubicSplineInterpolation.interpolate(
                    targetIndependentVariableValue );

        // Check if test result match analytical result.
        BOOST_CHECK_SMALL(  std::fabs( analyticalValue - interpolatedDependentVariableValue )
                            / analyticalValue,
                            5.0e-3 );
    }
}

// Test exception handling implementation of cubic spline class.
BOOST_AUTO_TEST_CASE( testCubicSplineInterpolation_exception_empty_vectors )
{
    // Test 2: Interpolate with empty vectors.
    // Declare independent and dependent variable vectors.
    std::vector< double > independentVariables, dependentVariables;

    // Declare and initialize flag.
    bool areDependentAndIndependentVariablesInitialized = true;

    // Try to initialize with empty vectors.
    try
    {
        // Declare cubic spline object and initialize with input data.
        interpolators::CubicSplineInterpolatorDouble cubicSplineInterpolation(
                    independentVariables, dependentVariables );
    }

    // Catch the expected runtime error, and set the boolean flag to false.
    catch ( std::runtime_error )
    {
        areDependentAndIndependentVariablesInitialized = false;
    }

    // Check value of flag.
    BOOST_CHECK( !areDependentAndIndependentVariablesInitialized );
}

// Test cubic spline interpolator by comparing to Matlab code posted at
// http://www.mathworks.com/matlabcentral/newsreader/view_thread/173708.
BOOST_AUTO_TEST_CASE( test_cubicSplineInterpolator_matlab_forum_compare )
{
    using namespace interpolators;

    // Load input data used for generating matlab interpolation.
    Eigen::MatrixXd inputData = input_output::readMatrixFromFile(
                input_output::getTudatRootPath( ) +
                "Mathematics/Interpolators/UnitTests/interpolator_test_input_data.dat","," );

    // Put data in STL vectors.
    std::vector< double > independentVariableValues;
    std::vector< double > dependentVariableValues;
    for ( int i = 0; i < inputData.rows( ); i++ )
    {
        independentVariableValues.push_back( inputData( i, 0 ) );
        dependentVariableValues.push_back( inputData( i, 1 ) );
    }

    // Create cubic spline interpolator using hunting algorithm.
    CubicSplineInterpolatorDouble cubicSplineInterpolator(
                independentVariableValues, dependentVariableValues, huntingAlgorithm );

    // Load points at which interpolator is to be evaluated and data generated by Matlab.
    Eigen::MatrixXd benchmarkData = input_output::readMatrixFromFile(
                input_output::getTudatRootPath( ) +
                "Mathematics/Interpolators/UnitTests/"
                + "cubic_spline_interpolator_test_output_data.dat",
                "," );

    // Perform interpolation for required data points.
    Eigen::VectorXd outputData = Eigen::VectorXd( benchmarkData.rows( ) );
    for ( int i = 0; i < outputData.rows( ); i++ )
    {
        outputData[ i ] = cubicSplineInterpolator.interpolate( benchmarkData( i, 0 ) );
    }

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( benchmarkData.block( 0, 1, benchmarkData.rows( ), 1 ),
                                       outputData, 1.0e-13 );

    // Create cubic spline interpolator, now using binary search algorithm.
    cubicSplineInterpolator = CubicSplineInterpolatorDouble(
                independentVariableValues, dependentVariableValues, huntingAlgorithm );

    // Perform interpolation for required data points.
    outputData = Eigen::VectorXd( benchmarkData.rows( ) );
    for ( int i = 0; i < outputData.rows( ); i++ )
    {
        outputData[ i ] = cubicSplineInterpolator.interpolate( benchmarkData( i, 0 ) );
    }

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( benchmarkData.block( 0, 1, benchmarkData.rows( ), 1 ),
                                       outputData, 1.0e-13 );
}

// Test cubic spline interpolator by comparing to Matlab implementation. Note that the two
// implementations are not identical, since the Matlab implementation imposes zero first
// derivatives whereas the present implementation imposes zero second derivatives at endpoints.
BOOST_AUTO_TEST_CASE( test_cubicSplineInterpolator_matlab_compare )
{
    using namespace interpolators;

    // Load input data used for generating matlab interpolation.
    Eigen::MatrixXd inputData = input_output::readMatrixFromFile(
                input_output::getTudatRootPath( ) +
                "Mathematics/Interpolators/UnitTests/interpolator_test_input_data.dat", "," );

    // Put data in STL vectors.
    std::vector< double > independentVariableValues;
    std::vector< double > dependentVariableValues;
    for ( int i = 0; i < inputData.rows( ); i++ )
    {
        independentVariableValues.push_back( inputData( i, 0 ) );
        dependentVariableValues.push_back( inputData( i, 1 ) );
    }

    // Create cubic spline interpolator.
    CubicSplineInterpolatorDouble linearInterpolator(
                independentVariableValues, dependentVariableValues );

    // Load points at which interpolator is to be evaluated and data generated by Matlab.
    Eigen::MatrixXd benchmarkData = input_output::readMatrixFromFile(
                input_output::getTudatRootPath( ) +
                "Mathematics/Interpolators/UnitTests/"
                + "cubic_spline_interpolator_approximate_test_output_data.dat", "," );

    // Perform interpolation for required data points.
    Eigen::VectorXd outputData = Eigen::VectorXd( benchmarkData.rows( ) );
    for ( int i = 0; i < outputData.rows( ); i++ )
    {
        outputData[ i ] = linearInterpolator.interpolate( benchmarkData( i, 0 ) );
    }

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( benchmarkData.block( 0, 1, benchmarkData.rows( ), 1 ),
                                       outputData, 1.0e-5 );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


//==============================================================================
//         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 complex.arithmetic toolbox - abs/scalar Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of boost.simd.arithmetic components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created by jt the 30/11/2010
///
#include <nt2/toolbox/arithmetic/include/functions/abs.hpp>
#include <nt2/include/constants/i.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 <nt2/toolbox/constant/constant.hpp>
#include <nt2/sdk/complex/dry.hpp>

NT2_TEST_CASE_TPL ( abs_real__1_0,  BOOST_SIMD_REAL_TYPES)
{

  using nt2::abs;
  using nt2::tag::abs_;
  typedef typename boost::dispatch::meta::as_integer<T>::type iT;
  typedef typename boost::dispatch::meta::call<abs_(T)>::type r_t;
  typedef typename nt2::meta::scalar_of<r_t>::type sr_t;
  typedef typename nt2::meta::scalar_of<r_t>::type ssr_t;
  typedef typename std::complex<T> cT;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST( (boost::is_same < r_t, wished_r_t >::value) );
  std::cout << std::endl;
  double ulpd;
 ulpd=0.0;

  // std::cout << nt2::type_id(nt2::I<T>()) << std::endl;
  // specific values tests
   NT2_TEST_ULP_EQUAL(nt2::abs(cT(1)), T(1), 0);
   NT2_TEST_EQUAL(nt2::abs(cT(nt2::Inf<T>())), nt2::Inf<T>());
   NT2_TEST_EQUAL(nt2::abs(cT(nt2::Minf<T>())), nt2::Inf<T>());
   NT2_TEST_EQUAL(nt2::abs(cT(nt2::Mone<T>())), nt2::One<T>());
   NT2_TEST_EQUAL(nt2::abs(cT(nt2::Nan<T>())), nt2::Nan<T>());
   NT2_TEST_EQUAL(nt2::abs(cT(nt2::One<T>())), nt2::One<T>());
   NT2_TEST_EQUAL(nt2::abs(cT(nt2::Valmax<T>())), nt2::Valmax<T>());
   NT2_TEST_EQUAL(nt2::abs(cT(nt2::Valmin<T>())), nt2::Valmax<T>());
   NT2_TEST_EQUAL(nt2::abs(cT(nt2::Zero<T>())), nt2::Zero<T>());
   std::complex < T > a(1, 0);
   NT2_TEST_EQUAL(nt2::abs(a), nt2::One<T>());
   std::complex < T > b(3, 4);
   NT2_TEST_EQUAL(nt2::abs(b), nt2::Five<T>());
} // end of test for floating_


//---------------------------------------------------------------------------//
//!
//! \file   Utility_GaussKronrodQuadratureSetTraits.hpp
//! \author Luke Kersting
//! \brief  Gauss-Kronrod quadrature set traits
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_GAUSS_KRONROD_QUADRATURE_SET_TRAITS_HPP
#define UTILITY_GAUSS_KRONROD_QUADRATURE_SET_TRAITS_HPP

// Boost Includes
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_floating_point.hpp>

// FRENSIE Includes
#include "Utility_GaussKronrodQuadratureSetTraitsDecl.hpp"
#include "Utility_IsFloatingPoint.hpp"

namespace Utility{

// Gauss-Kronrod quadrature set traits 15 point rule
template<typename FloatType>
struct GaussKronrodQuadratureSetTraits<15,FloatType,typename boost::enable_if<IsFloatingPoint<FloatType> >::type>
{
  // Valid rule
  static const bool valid_rule = true;

  // Gauss quadrature weights
  static const std::vector<FloatType> gauss_weights;

  // Kronrod quadrature weights
  static const std::vector<FloatType> kronrod_weights;

  // Kronrod quadrature abscissae
  static const std::vector<FloatType> kronrod_abscissae;

private:

  // Initialize the gauss weight array
  static std::vector<FloatType> initializeGaussWeights();

  // Initialize the kronrod weight array
  static std::vector<FloatType> initializeKronrodWeights();

  // Initialize the gauss weight array
  static std::vector<FloatType> initializeKronrodAbscissae();
};

// Gauss-Kronrod quadrature set traits 21 point rule
template<typename FloatType>
struct GaussKronrodQuadratureSetTraits<21,FloatType,typename boost::enable_if<IsFloatingPoint<FloatType> >::type>
{
  // Valid rule
  static const bool valid_rule = true;

  // Gauss quadrature weights
  static const std::vector<FloatType> gauss_weights;

  // Kronrod quadrature weights
  static const std::vector<FloatType> kronrod_weights;

  // Kronrod quadrature abscissae
  static const std::vector<FloatType> kronrod_abscissae;

private:

  // Initialize the gauss weight array
  static std::vector<FloatType> initializeGaussWeights();

  // Initialize the kronrod weight array
  static std::vector<FloatType> initializeKronrodWeights();

  // Initialize the gauss weight array
  static std::vector<FloatType> initializeKronrodAbscissae();
};

// Gauss-Kronrod quadrature set traits 31 point rule
template<typename FloatType>
struct GaussKronrodQuadratureSetTraits<31,FloatType,typename boost::enable_if<IsFloatingPoint<FloatType> >::type>
{
  // Valid rule
  static const bool valid_rule = true;

  // Gauss quadrature weights
  static const std::vector<FloatType> gauss_weights;

  // Kronrod quadrature weights
  static const std::vector<FloatType> kronrod_weights;

  // Kronrod quadrature abscissae
  static const std::vector<FloatType> kronrod_abscissae;

private:

  // Initialize the gauss weight array
  static std::vector<FloatType> initializeGaussWeights();

  // Initialize the kronrod weight array
  static std::vector<FloatType> initializeKronrodWeights();

  // Initialize the gauss weight array
  static std::vector<FloatType> initializeKronrodAbscissae();
};

// Gauss-Kronrod quadrature set traits 41 point rule
template<typename FloatType>
struct GaussKronrodQuadratureSetTraits<41,FloatType,typename boost::enable_if<IsFloatingPoint<FloatType> >::type>
{
  // Valid rule
  static const bool valid_rule = true;

  // Gauss quadrature weights
  static const std::vector<FloatType> gauss_weights;

  // Kronrod quadrature weights
  static const std::vector<FloatType> kronrod_weights;

  // Kronrod quadrature abscissae
  static const std::vector<FloatType> kronrod_abscissae;

private:

  // Initialize the gauss weight array
  static std::vector<FloatType> initializeGaussWeights();

  // Initialize the kronrod weight array
  static std::vector<FloatType> initializeKronrodWeights();

  // Initialize the gauss weight array
  static std::vector<FloatType> initializeKronrodAbscissae();
};

// Gauss-Kronrod quadrature set traits 51 point rule
template<typename FloatType>
struct GaussKronrodQuadratureSetTraits<51,FloatType,typename boost::enable_if<IsFloatingPoint<FloatType> >::type>
{
  // Valid rule
  static const bool valid_rule = true;

  // Gauss quadrature weights
  static const std::vector<FloatType> gauss_weights;

  // Kronrod quadrature weights
  static const std::vector<FloatType> kronrod_weights;

  // Kronrod quadrature abscissae
  static const std::vector<FloatType> kronrod_abscissae;

private:

  // Initialize the gauss weight array
  static std::vector<FloatType> initializeGaussWeights();

  // Initialize the kronrod weight array
  static std::vector<FloatType> initializeKronrodWeights();

  // Initialize the gauss weight array
  static std::vector<FloatType> initializeKronrodAbscissae();
};

// Gauss-Kronrod quadrature set traits 61 point rule
template<typename FloatType>
struct GaussKronrodQuadratureSetTraits<61,FloatType,typename boost::enable_if<IsFloatingPoint<FloatType> >::type>
{
  // Valid rule
  static const bool valid_rule = true;

  // Gauss quadrature weights
  static const std::vector<FloatType> gauss_weights;

  // Kronrod quadrature weights
  static const std::vector<FloatType> kronrod_weights;

  // Kronrod quadrature abscissae
  static const std::vector<FloatType> kronrod_abscissae;

private:

  // Initialize the gauss weight array
  static std::vector<FloatType> initializeGaussWeights();

  // Initialize the kronrod weight array
  static std::vector<FloatType> initializeKronrodWeights();

  // Initialize the gauss weight array
  static std::vector<FloatType> initializeKronrodAbscissae();
};

} // end Utility namespace

//---------------------------------------------------------------------------//
// Template Includes
//---------------------------------------------------------------------------//

#include "Utility_GaussKronrodQuadratureSetTraits_def.hpp"

//---------------------------------------------------------------------------//

#endif // end UTILITY_GAUSS_KRONROD_QUADRATURE_SET_TRAITS_HPP

//---------------------------------------------------------------------------//
// end Utility_GaussKronrodQuadratureSetTraits.hpp
//---------------------------------------------------------------------------//