//
// Created by Hamza El-Kebir on 4/21/21.
//

#ifndef LODESTAR_DISCRETESYSTEM_HPP
#define LODESTAR_DISCRETESYSTEM_HPP

#include <Eigen/Dense>
#include "StateSpace.hpp"
#include "SystemStateful.hpp"
#include "Lodestar/aux/CompileTimeQualifiers.hpp"

namespace ls {
    namespace systems {
        template<class SYS>
        class DiscreteSystem : public SystemStateful {
        public:
            typedef SYS TDSystem;
            DiscreteSystem() : system(new SYS), state(new Eigen::VectorXd),
                               input(
                                       nullptr), time(0)
            {}

            DiscreteSystem(SYS *sys) : system(sys), state(new Eigen::VectorXd),
                                       input(nullptr), time(0)
            {}

            void advance();

//            void advance(void *control);

            void advanceFree();

            void advanceForced();

//            void advanceForced(void *control);

            SYS *system;
            void *state;
            void *input;

            double time;
        };

        template <template <typename, const int, const int, const int> class TSystem, typename TScalar, const int TStateDim, const int TInputDim, const int TOutputDim>
        class DiscreteSystem<TSystem<TScalar, TStateDim, TInputDim, TOutputDim>>;

        template <typename TScalar, const int TStateDim, const int TInputDim, const int TOutputDim>
        class DiscreteSystem<StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>> {
        public:
            typedef StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> TDSystem;
            typedef Eigen::Matrix<TScalar, TStateDim, LS_STATIC_UNLESS_DYNAMIC_VAL(TStateDim, 1)> TDStateVector;
            typedef Eigen::Matrix<TScalar, TInputDim, LS_STATIC_UNLESS_DYNAMIC_VAL(TInputDim, 1)> TDInputVector;

            DiscreteSystem() : system(new TDSystem), state(new TDStateVector),
                               input(nullptr), time(0)
            {}

            DiscreteSystem(TDSystem *sys) : system(sys), state(new TDStateVector),
                                       input(nullptr), time(0)
            {}

            IF_DYNAMIC_RETURN(TStateDim, TInputDim, TOutputDim, void)
            initialize() {
                state->conservativeResize(system->getA().rows());

                if (input == nullptr)
                    input = new Eigen::VectorXd;

                input->conservativeResize(system->getB().cols());
            }

            IF_STATIC_RETURN(TStateDim, TInputDim, TOutputDim, void)
            initialize() {
                // NOTE: No action since no memory may be allocated.
                return;
            }

            void advance();

            void advance(TDInputVector *control);

            void advanceFree();

            void advanceForced();

            void advanceForced(TDInputVector *control);

            TDSystem *system;
            TDStateVector *state;
            TDInputVector *input;

            double time;
        };
    }
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void ls::systems::DiscreteSystem<ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>>::advance()
{
    if (input != nullptr) {
        *state = (*system->getA()) * (*state) + (*system->getB()) * (*input);
    } else {
        *state = (*system->getA()) * (*state);
    }

    time += system->getSamplingPeriod();
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void ls::systems::DiscreteSystem<ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>>::advance(
        Eigen::Matrix<TScalar, TInputDim, Kmax2(-1, TInputDim, 1)> *control)
{
    *state = (*system->getA()) * (*state) + (*system->getB()) * (*control);

    time = system->getSamplingPeriod();
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void ls::systems::DiscreteSystem<ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>>::advanceFree()
{
    *state = (*system->getA()) * (*state);

    time += system->getSamplingPeriod();
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void ls::systems::DiscreteSystem<ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>>::advanceForced()
{
    *state = (*system->getA()) * (*state) + (*system->getB()) * (*input);

    time += system->getSamplingPeriod();
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void ls::systems::DiscreteSystem<ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim>>::advanceForced(
        Eigen::Matrix<TScalar, TInputDim, Kmax2(-1, TInputDim, 1)> *control)
{
    *state = (*system->getA()) * (*state) + (*system->getB()) * (*control);

    time += system->getSamplingPeriod();
}

#endif //LODESTAR_DISCRETESYSTEM_HPP


#include <scitbx/array_family/boost_python/flex_fwd.h>
#include <boost/python.hpp>

#include <tntbx/eigensystem.h>

BOOST_PYTHON_MODULE(tntbx_eigensystem_ext)
{
  namespace af = scitbx::af;
  using namespace boost::python;
  def("get_eigenvectors",tntbx::get_eigenvectors,(
    arg_("square_matrix")
    )
      );
  // classes
  class_<tntbx::real<> >("real", no_init)
    .def(init<af::const_ref<double, af::c_grid<2> > const&
         >(
           (
            arg_("m")
            )
           )
         )
    .def(init<scitbx::sym_mat3<double> const&
         >(
           (
            arg_("m")
            )
           )
         )
    .def("values", &tntbx::real<>::values)
    .def("vectors", &tntbx::real<>::vectors)
    ;
}


/*
Copyright (c) 2012-2018, Visillect Service LLC. All rights reserved.
Developed for Kharkevich Institute for Information Transmission Problems of the
              Russian Academy of Sciences (IITP RAS).

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

   1. Redistributions of source code must retain the above copyright notice,
      this list of conditions and the following disclaimer.

   2. 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.

THIS SOFTWARE IS PROVIDED BY COPYRIGHT HOLDERS "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 COPYRIGHT HOLDERS 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.

The views and conclusions contained in the software and documentation are those
of the authors and should not be interpreted as representing official policies,
either expressed or implied, of copyright holders.
*/

#include <colorseg/color_distance_func.h>

#include <colorseg/colorspace_homography.hpp>
#include <remseg/distance_func.h>

#include <cassert>
#include <cmath>
#include <algorithm>

THIRDPARTY_INCLUDES_BEGIN
#include <Eigen/Geometry>
THIRDPARTY_INCLUDES_END

namespace vi { namespace colorseg {

double dist_point_to_line(Eigen::Vector3d const& p,
                          Eigen::Vector3d const& v,
                          Eigen::Vector3d const& mean)
{
  Eigen::Vector3d r = mean - p;
  Eigen::Vector3d res = r - v.dot(r) * v;
  return res.norm();
}

double dist_point_to_segment(const Eigen::Vector3d& p,
                             const Eigen::Vector3d& a,
                             const Eigen::Vector3d& b)
{
  if ((b - a).dot(p - a) < 0)
    return (p - a).norm();
  if ((a - b).dot(p - b) < 0)
    return (p - b).norm();
  return dist_point_to_line(p, (b - a).normalized(), a);
}

EdgeValue criteria_r0(const ColorVertex *v1, const ColorVertex *v2)
{
  return std::sqrt(student_distance(v1, v2));
}

EdgeValue error_r1(const ColorVertex *v) {
  ColorVertex::HelperStats const & hs = v->getHelperStats();
  double err = hs.eigenvalues()[0] + hs.eigenvalues()[1];
  return err * v->area;
}

EdgeValue criteria_r1(const ColorVertex *v1, const ColorVertex *v2)
{
  ColorVertex v(v1);
  ColorVertex tmp(v2);
  v.absorb(&tmp);

  return std::sqrt(error_r1(&v) - error_r1(v1) - error_r1(v2)) ;
}

EdgeValue error_r2(const ColorVertex *v) {
  ColorVertex::HelperStats const & hs = v->getHelperStats();
  double err = hs.eigenvalues()[0];
  return err * v->area;
}

bool isLTCluster(const ColorVertex *v1, const ColorVertex *v2)
{
  ColorVertex::HelperStats const & hs1 = v1->getHelperStats();
  ColorVertex::HelperStats const & hs2 = v2->getHelperStats();

  Eigen::Vector3d vec1 = hs1.eigenvectors().col(2);
  Eigen::Vector3d vec2 = hs2.eigenvectors().col(2);
  double semiAxis1 = std::sqrt(hs1.eigenvalues()(2));
  double semiAxis2 = std::sqrt(hs2.eigenvalues()(2));
  Eigen::Vector3d a1 = hs1.mean - vec1 * semiAxis1;
  Eigen::Vector3d a2 = hs2.mean - vec2 * semiAxis2;
  Eigen::Vector3d b1 = hs1.mean + vec1 * semiAxis1;
  Eigen::Vector3d b2 = hs2.mean + vec2 * semiAxis2;

  double dists[4] = {
    dist_point_to_segment(a1, a2, b2),
    dist_point_to_segment(b1, a2, b2),
    dist_point_to_segment(a2, a1, b1),
    dist_point_to_segment(b2, a1, b1)
  };

  double minDist = *std::min_element(dists, dists + 4);
  return minDist < ColorVertex::getMaxModelDistance();
}

EdgeValue criteria_r2(const ColorVertex *v1, const ColorVertex *v2)
{
  if (!isLTCluster(v1, v2))
    return std::numeric_limits<double>::infinity();

  ColorVertex v(v1);
  ColorVertex tmp(v2);
  v.absorb(&tmp);
  return std::sqrt(error_r2(&v) - error_r2(v1) - error_r2(v2));
}

}}	// ns vi::colorseg


#include "math-dsp.h"
#include <speex/speex_resampler.h>
#include <boost/scope_exit.hpp>
#include <algorithm>
#include <stdexcept>
#include <cmath>

double tukey_window(double a, double x) {
  if (x < a / 2) {
    return (1.0 + std::cos(M_PI * (2.0 * x / a - 1.0))) / 2.0;
  } else if (x > 1.0 - a / 2) {
    return (1.0 + std::cos(M_PI * (2.0 * (x - 1.0) / a + 1.0))) / 2.0;
  } else {
    return 1.0;
  }
}

void resample(const float *in_samples,
              unsigned in_sample_count,
              float *out_samples,
              unsigned out_sample_count) {
  if (in_sample_count == 0) {
    std::fill(out_samples, out_samples + out_sample_count, 0.0f);
    return;
  }

  int err {};

  SpeexResamplerState *resampler = speex_resampler_init(
    1, in_sample_count, out_sample_count, SPEEX_RESAMPLER_QUALITY_MAX, &err);

  if (!resampler) {
    const char *errmsg = speex_resampler_strerror(err);
    throw std::runtime_error(std::string("speex_resampler_init: ") + errmsg);
  }

  BOOST_SCOPE_EXIT(resampler) {
    speex_resampler_destroy(resampler);
  } BOOST_SCOPE_EXIT_END;

  speex_resampler_skip_zeros(resampler);

  while (out_sample_count > 0) {
    unsigned in_count = in_sample_count;
    unsigned out_count = out_sample_count;
    speex_resampler_process_float(
      resampler, 0, in_samples, &in_count, out_samples, &out_count);
    in_samples += in_count;
    in_sample_count -= in_count;
    out_samples += out_count;
    out_sample_count -= out_count;
    if (out_count == 0)
      break;
  }

  static const unsigned nzeros = 32;
  static const float zero[nzeros] {};

  while (out_sample_count > 0) {
    unsigned in_count = nzeros;
    unsigned out_count = out_sample_count;
    speex_resampler_process_float(
      resampler, 0, zero, &in_count, out_samples, &out_count);
    out_samples += out_count;
    out_sample_count -= out_count;
  }
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2000, 2001, 2002, 2003 RiskMap srl
 Copyright (C) 2006 François du Vignaud
 Copyright (C) 2006, 2008 Ferdinando Ametrano
 Copyright (C) 2015 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file swaptionvolmatrix.hpp
    \brief Swaption at-the-money volatility matrix
*/

#ifndef quantlib_swaption_volatility_matrix_hpp
#define quantlib_swaption_volatility_matrix_hpp

#include <ql/termstructures/volatility/swaption/swaptionvoldiscrete.hpp>
#include <ql/math/interpolations/interpolation2d.hpp>
#include <ql/math/matrix.hpp>
#include <boost/noncopyable.hpp>
#include <vector>

namespace QuantLib {

    class Quote;

    //! At-the-money swaption-volatility matrix
    /*! This class provides the at-the-money volatility for a given
        swaption by interpolating a volatility matrix whose elements
        are the market volatilities of a set of swaption with given
        option date and swapLength.

        The volatility matrix <tt>M</tt> must be defined so that:
        - the number of rows equals the number of option dates;
        - the number of columns equals the number of swap tenors;
        - <tt>M[i][j]</tt> contains the volatility corresponding
          to the <tt>i</tt>-th option and <tt>j</tt>-th tenor.
    */
    class SwaptionVolatilityMatrix : public SwaptionVolatilityDiscrete,
                                     private boost::noncopyable {
      public:
        //! floating reference date, floating market data
        SwaptionVolatilityMatrix(
                    const Calendar& calendar,
                    BusinessDayConvention bdc,
                    const std::vector<Period>& optionTenors,
                    const std::vector<Period>& swapTenors,
                    const std::vector<std::vector<Handle<Quote> > >& vols,
                    const DayCounter& dayCounter,
                    const bool flatExtrapolation = false,
                    const VolatilityType type = ShiftedLognormal,
                    const std::vector<std::vector<Real> >& shifts
                    = std::vector<std::vector<Real> >());
        //! fixed reference date, floating market data
        SwaptionVolatilityMatrix(
                    const Date& referenceDate,
                    const Calendar& calendar,
                    BusinessDayConvention bdc,
                    const std::vector<Period>& optionTenors,
                    const std::vector<Period>& swapTenors,
                    const std::vector<std::vector<Handle<Quote> > >& vols,
                    const DayCounter& dayCounter,
                    const bool flatExtrapolation = false,
                    const VolatilityType type = ShiftedLognormal,
                    const std::vector<std::vector<Real> >& shifts
                    = std::vector<std::vector<Real> >());
        //! floating reference date, fixed market data
        SwaptionVolatilityMatrix(
                    const Calendar& calendar,
                    BusinessDayConvention bdc,
                    const std::vector<Period>& optionTenors,
                    const std::vector<Period>& swapTenors,
                    const Matrix& volatilities,
                    const DayCounter& dayCounter,
                    const bool flatExtrapolation = false,
                    const VolatilityType type = ShiftedLognormal,
                    const Matrix& shifts = Matrix());
        //! fixed reference date, fixed market data
        SwaptionVolatilityMatrix(
                    const Date& referenceDate,
                    const Calendar& calendar,
                    BusinessDayConvention bdc,
                    const std::vector<Period>& optionTenors,
                    const std::vector<Period>& swapTenors,
                    const Matrix& volatilities,
                    const DayCounter& dayCounter,
                    const bool flatExtrapolation = false,
                    const VolatilityType type = ShiftedLognormal,
                    const Matrix& shifts = Matrix());
        // fixed reference date and fixed market data, option dates
        SwaptionVolatilityMatrix(const Date& referenceDate,
                                 const std::vector<Date>& optionDates,
                                 const std::vector<Period>& swapTenors,
                                 const Matrix& volatilities,
                                 const DayCounter& dayCounter,
                                 const bool flatExtrapolation = false,
                                 const VolatilityType type = ShiftedLognormal,
                                 const Matrix& shifts = Matrix());

        //! \name LazyObject interface
        //@{
        void performCalculations() const;
        //@}
        //! \name TermStructure interface
        //@{
        Date maxDate() const;
        //@}
        //! \name VolatilityTermStructure interface
        //@{
        Rate minStrike() const;
        Rate maxStrike() const;
        //@}
        //! \name SwaptionVolatilityStructure interface
        //@{
        const Period& maxSwapTenor() const;
        //@}
        //! \name Other inspectors
        //@{
        //! returns the lower indexes of surrounding volatility matrix corners
        std::pair<Size,Size> locate(const Date& optionDate,
                                    const Period& swapTenor) const {
            return locate(timeFromReference(optionDate),
                          swapLength(swapTenor));
        }
        //! returns the lower indexes of surrounding volatility matrix corners
        std::pair<Size,Size> locate(Time optionTime,
                                    Time swapLength) const {
            return std::make_pair(interpolation_.locateY(optionTime),
                                  interpolation_.locateX(swapLength));
        }
        //@}
        VolatilityType volatilityType() const;
      protected:
        // defining the following method would break CMS test suite
        // to be further investigated
        //ext::shared_ptr<SmileSection> smileSectionImpl(const Date&,
        //                                                 const Period&) const;
        ext::shared_ptr<SmileSection> smileSectionImpl(Time,
                                                         Time) const;
        Volatility volatilityImpl(Time optionTime,
                                  Time swapLength,
                                  Rate strike) const;
        Real shiftImpl(Time optionTime, Time swapLength) const;
      private:
        void checkInputs(Size volRows,
                         Size volsColumns,
                         Size shiftRows,
                         Size shiftsColumns) const;
        void registerWithMarketData();
        std::vector<std::vector<Handle<Quote> > > volHandles_;
        std::vector<std::vector<Real> > shiftValues_;
        mutable Matrix volatilities_, shifts_;
        Interpolation2D interpolation_, interpolationShifts_;
        VolatilityType volatilityType_;
    };

    // inline definitions

    inline Date SwaptionVolatilityMatrix::maxDate() const {
        return optionDates_.back();
    }

    inline Rate SwaptionVolatilityMatrix::minStrike() const {
        return -QL_MAX_REAL;
    }

    inline Rate SwaptionVolatilityMatrix::maxStrike() const {
        return QL_MAX_REAL;
    }

    inline const Period& SwaptionVolatilityMatrix::maxSwapTenor() const {
        return swapTenors_.back();
    }

    inline Volatility SwaptionVolatilityMatrix::volatilityImpl(Time optionTime,
                                                               Time swapLength,
                                                               Rate) const {
        calculate();
        return interpolation_(swapLength, optionTime, true);
    }

    inline VolatilityType SwaptionVolatilityMatrix::volatilityType() const {
        return volatilityType_;
    }

    inline Real SwaptionVolatilityMatrix::shiftImpl(Time optionTime,
                                                    Time swapLength) const {
        calculate();
        Real tmp = interpolationShifts_(swapLength, optionTime, true);
        return tmp;
    }
} // namespace QuantLib

#endif


//--C++--
//-- cylinder 1D analytical = no orientation
//-- SANS

#include <boost/python.hpp>

#include "mccomponents/kernels/sample/AbstractSQ.h"
#include "mccomponents/kernels/sample/SQAdaptor.h"

extern "C" {
#include "sans/cylinder.h"
}

namespace wrap_models{
struct Cylinder{
	Cylinder(double scale,double radius, double length, double contrast, double background,double cyl_theta, double cyl_phi)
	{
    p.scale = scale;
    p.radius = radius;
	p.length = length;
    p.contrast = contrast;
	p.background = background;
    p.cyl_theta = cyl_theta;
	p.cyl_phi = cyl_phi;
	}

	double operator() (double q) const
	{
		return cylinder_analytical_1D( const_cast<CylinderParameters *>(&p), q);
	}
	CylinderParameters p;
};

void wrap_cylinder()
{
	using namespace boost::python;
	typedef Cylinder w_t;
	class_<w_t>
		(
		"SANSModel_Cylinder",
		init< double, double, double, double, double, double, double >()
			)
		;

	using namespace mccomponents::sample;
	typedef SQAdaptor<w_t> SQ;
	class_<SQ, bases<AbstractSQ> >
		(
		"SANSModel_Cylinder_SQAdaptor",
		init< const w_t & >()
		[with_custodian_and_ward<1,2>()] //Jiao, what you mean by the numbers,<1,2>???
		)
		;
	}
}
//2009-06-02 jhcho
// End of file


#include "integrator.h"
#include "biorbd.h"

#include <boost/numeric/odeint.hpp>
#include "dynamics.h"

AcadoIntegrator::AcadoIntegrator(biorbd::Model& m) :
    biorbd::rigidbody::Integrator(m),
    m_isKinematicsComputed(false),
    m_nMus(m.nbMuscleTotal()),
    m_nTorque(m.nbGeneralizedTorque())
{
    m_lhs = new double[m_model->nbQ() + m_model->nbQdot()
            + m_nMus + m_nTorque];
    m_rhs = new double[m_model->nbQ() + m_model->nbQdot()];
}

void AcadoIntegrator::operator()(
        const state_type &x,
        state_type &dxdt,
        double)
{
    for (unsigned int i=0; i<*m_nQ + *m_nQdot; i++){
        m_lhs[i] = x[i];
    }
    for (unsigned int i=0; i<m_nMus + m_nTorque; ++i){
        m_lhs[i + *m_nQ + *m_nQdot] = (*m_u)(i);
    }

    // Équation différentielle : x/xdot => xdot/xddot
    forwardDynamics_contact(m_lhs, m_rhs);

    // Faire sortir xdot/xddot
    for (unsigned int i=0; i<*m_nQ + *m_nQdot; i++){
        dxdt[i] = m_rhs[i];
    }

}

void AcadoIntegrator::integrateKinematics(
        const biorbd::rigidbody::GeneralizedCoordinates &Q,
        const biorbd::rigidbody::GeneralizedCoordinates &QDot,
        const biorbd::utils::Vector &control,
        double t0,
        double tend,
        double timeStep)
{
    biorbd::utils::Vector v(static_cast<unsigned int>(Q.rows()+QDot.rows()));
    v << Q,QDot;
    integrate(v, control, t0, tend, timeStep); // vecteur, t0, tend, pas, effecteurs
    m_isKinematicsComputed = true;
}

void AcadoIntegrator::getIntegratedKinematics(
        unsigned int step,
        biorbd::rigidbody::GeneralizedCoordinates &Q,
        biorbd::rigidbody::GeneralizedCoordinates &QDot)
{
    // Si la cinématique n'a pas été mise à jour
    biorbd::utils::Error::check(
                m_isKinematicsComputed,
                "ComputeKinematics must be call before calling updateKinematics");
    const biorbd::utils::Vector& tp(getX(step));
    for (unsigned int i=0; i< static_cast<unsigned int>(tp.rows()/2); i++){
        Q(i) = tp(i);
        QDot(i) = tp(i+tp.rows()/2);
    }
}
unsigned int AcadoIntegrator::nbInterationStep() const
{
    return steps();
}

void AcadoIntegrator::launchIntegrate(
        state_type &x,
        double t0,
        double tend,
        double timeStep)
{
    // Choix de l'algorithme et intégration
    boost::numeric::odeint::runge_kutta4< state_type > stepper;
    *m_steps = static_cast<unsigned int>(
                boost::numeric::odeint::integrate_const(
                    stepper, *this, x, t0, tend, timeStep,
                    push_back_state_and_time( *m_x_vec , *m_times )));
}


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/Surface_mesh.h>
#include <CGAL/Polyhedron_3.h>
#include <CGAL/Polygon_mesh_processing/smooth_mesh.h>

#include <CGAL/property_map.h>

#include <boost/graph/graph_traits.hpp>

#include <fstream>

typedef CGAL::Exact_predicates_inexact_constructions_kernel   Kernel;
typedef Kernel::Point_3                                       Point;

typedef CGAL::Surface_mesh<Point>                             SurfaceMesh;
typedef CGAL::Polyhedron_3<Kernel>                            Polyhedron;

namespace PMP = CGAL::Polygon_mesh_processing;

template <typename Mesh>
void read_mesh(const char* filename, Mesh& mesh)
{
  std::ifstream input(filename);

  if (!input || !(input >> mesh))
  {
    std::cerr << "Error: can not read file.";
    std::exit(1);
  }
}

template <typename Mesh>
void test_smoothing(const char* filename)
{
  Mesh mesh;
  read_mesh(filename, mesh);

  PMP::smooth_mesh(mesh);
  PMP::smooth_mesh(mesh, CGAL::parameters::number_of_iterations(10));
}

template <typename Mesh>
void test_angle_smoothing(const char* filename)
{
  Mesh mesh;
  read_mesh(filename, mesh);

  PMP::smooth_mesh(mesh);
  PMP::smooth_mesh(mesh, CGAL::parameters::number_of_iterations(10)
                                          .use_area_smoothing(false));
}

template <typename Mesh>
void test_area_smoothing(const char* filename)
{
  Mesh mesh;
  read_mesh(filename, mesh);

  PMP::smooth_mesh(mesh);
  PMP::smooth_mesh(mesh, CGAL::parameters::number_of_iterations(10)
                                          .use_angle_smoothing(false));
}

template <typename Mesh>
void test_angle_smoothing_without_projection(const char* filename)
{
  Mesh mesh;
  read_mesh(filename, mesh);

  PMP::smooth_mesh(mesh, CGAL::parameters::do_project(false)
                                          .use_area_smoothing(false));
}

template <typename Mesh>
void test_area_smoothing_without_projection(const char* filename)
{
  Mesh mesh;
  read_mesh(filename, mesh);

  PMP::smooth_mesh(mesh, CGAL::parameters::do_project(false)
                                          .use_angle_smoothing(false));
}

template <typename Mesh>
void test_constrained_vertices(const char* filename)
{
  Mesh mesh;
  read_mesh(filename, mesh);

  typedef typename boost::graph_traits<Mesh>::vertex_descriptor vertex_descriptor;
  typename boost::property_map<Mesh, CGAL::vertex_point_t>::type vpmap = get(CGAL::vertex_point, mesh);

  std::set<vertex_descriptor> selected_vertices;
  std::map<vertex_descriptor, Point> initial_positions;
  for(vertex_descriptor v : vertices(mesh))
  {
    if(!is_border(v, mesh))
    {
      selected_vertices.insert(v);
      initial_positions[v] = get(vpmap, v);
    }
  }

  CGAL::Boolean_property_map<std::set<vertex_descriptor> > vcmap(selected_vertices);

  PMP::smooth_mesh(mesh, CGAL::parameters::vertex_is_constrained_map(vcmap));

  for(vertex_descriptor v : vertices(mesh))
  {
    if(!is_border(v, mesh))
      assert(initial_positions.at(v) == get(vpmap, v));
  }
}

int main(int /*argc*/, char** /*argv*/)
{
  const char* filename_elephant = "data/elephant.off";
  const char* filename_mannequin = "data/mannequin-devil.off";

  std::cout << "Test files: " << filename_elephant << " " << filename_mannequin << std::endl;

  // test with Surface_mesh
  test_smoothing<SurfaceMesh>(filename_elephant);
  test_angle_smoothing<SurfaceMesh>(filename_elephant);
  test_area_smoothing<SurfaceMesh>(filename_mannequin);
  test_angle_smoothing_without_projection<SurfaceMesh>(filename_elephant);
  test_area_smoothing_without_projection<SurfaceMesh>(filename_mannequin);
  test_constrained_vertices<SurfaceMesh>(filename_elephant);

  // test with Polyhedron
  test_smoothing<Polyhedron>(filename_elephant);
  test_angle_smoothing<Polyhedron>(filename_elephant);
  test_area_smoothing<Polyhedron>(filename_mannequin);
  test_angle_smoothing_without_projection<Polyhedron>(filename_elephant);
  test_area_smoothing_without_projection<Polyhedron>(filename_mannequin);
  test_constrained_vertices<Polyhedron>(filename_mannequin);

  std::cout << "Done!" << std::endl;
  return EXIT_SUCCESS;
}


// Must do this before including unit_test.hpp
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE test_hilbert_mask
#include <boost/test/unit_test.hpp>
#include "../discrete_hilbert_mask.hpp"

bool check_equality(const std::vector<int>& mask, int* ref_mask) {
    for (size_t i = 0; i < mask.size(); i++) {
        if (mask[i] != ref_mask[i]) return false;
    }
    return true;
}

BOOST_AUTO_TEST_CASE(HilbertMaskCorrectnessLength7) {
    int ref_mask[] = {1, 2, 2, 2, 0, 0, 0};
    auto mask = discrete_hilbert_mask<int>(7);
    BOOST_REQUIRE( check_equality(mask, ref_mask) );
}

BOOST_AUTO_TEST_CASE(HilbertMaskCorrectnessLength8) {
    int ref_mask[] = {1, 2, 2, 2, 1, 0, 0, 0};
    auto mask = discrete_hilbert_mask<int>(8);
    BOOST_REQUIRE( check_equality(mask, ref_mask) );
}
BOOST_AUTO_TEST_CASE(HilbertMaskCorrectnessLength11) {
    int ref_mask[] = {1, 2, 2, 2, 2, 2, 0, 0, 0, 0, 0};
    auto mask = discrete_hilbert_mask<int>(11);
    BOOST_REQUIRE( check_equality(mask, ref_mask) );
}
BOOST_AUTO_TEST_CASE(HilbertMaskCorrectnessLength18) {
    int ref_mask[] = {1, 2, 2, 2, 2, 2, 2, 2, 2, 1, 0, 0, 0, 0, 0, 0, 0, 0};
    auto mask = discrete_hilbert_mask<int>(18);
    BOOST_REQUIRE( check_equality(mask, ref_mask) );
}


/*
 [auto_generated]
 libs/numeric/odeint/test/integrate_implicit.cpp

 [begin_description]
 This file tests the integrate function and its variants with the implicit steppers.
 [end_description]

 Copyright 2009-2012 Karsten Ahnert
 Copyright 2009-2012 Mario Mulansky

 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)
 */

#define BOOST_TEST_MODULE odeint_integrate_functions_implicit

#include <vector>
#include <cmath>
#include <iostream>

#include <boost/numeric/odeint/config.hpp>

#include <boost/array.hpp>
#include <boost/ref.hpp>
#include <boost/iterator/counting_iterator.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>

#include <boost/test/unit_test.hpp>

#include <boost/mpl/vector.hpp>

#include <boost/numeric/odeint/integrate/integrate_const.hpp>
#include <boost/numeric/odeint/integrate/integrate_adaptive.hpp>
#include <boost/numeric/odeint/integrate/integrate_times.hpp>
#include <boost/numeric/odeint/integrate/integrate_n_steps.hpp>
#include <boost/numeric/odeint/stepper/rosenbrock4.hpp>
#include <boost/numeric/odeint/stepper/rosenbrock4_controller.hpp>
#include <boost/numeric/odeint/stepper/rosenbrock4_dense_output.hpp>

using namespace boost::unit_test;
using namespace boost::numeric::odeint;
namespace mpl = boost::mpl;
namespace ublas = boost::numeric::ublas;

typedef double value_type;
typedef ublas::vector< value_type > state_type;
typedef boost::numeric::ublas::matrix< value_type > matrix_type;

struct sys
{
    void operator()( const state_type &x , state_type &dxdt , const value_type &t ) const
    {
        dxdt( 0 ) = x( 0 ) + 2 * x( 1 );
        dxdt( 1 ) = x( 1 );
    }
};

struct jacobi
{
    void operator()( const state_type &x , matrix_type &jacobi , const value_type &t , state_type &dfdt ) const
    {
        jacobi( 0 , 0 ) = 1;
        jacobi( 0 , 1 ) = 2;
        jacobi( 1 , 0 ) = 0;
        jacobi( 1 , 1 ) = 1;
        dfdt( 0 ) = 0.0;
        dfdt( 1 ) = 0.0;
    }
};

struct push_back_time
{
    std::vector< double >& m_times;

    push_back_time( std::vector< double > &times )
    :  m_times( times ) { }

    void operator()( const state_type &x , double t )
    {
        m_times.push_back( t );
    }
};

template< class Stepper >
struct perform_integrate_const_test
{
    void operator()( void )
    {
        state_type x( 2 , 1.0 );
        const value_type dt = 0.03;
        const value_type t_end = 10.0;

        std::vector< value_type > times;

        integrate_const( Stepper() , std::make_pair( sys() , jacobi() ) , x , 0.0 , t_end ,
                                        dt , push_back_time( times ) );

        BOOST_CHECK_EQUAL( static_cast<int>(times.size()) , static_cast<int>(floor(t_end/dt))+1 );

        for( size_t i=0 ; i<times.size() ; ++i )
        {
            //std::cout << i << std::endl;
            // check if observer was called at times 0,1,2,...
            BOOST_CHECK_SMALL( times[i] - static_cast< value_type >(i)*dt , (i+1) * 2E-16 );
        }
    }
};

template< class Stepper >
struct perform_integrate_adaptive_test
{
    void operator()( void )
    {
        state_type x( 2 , 1.0 );
        const value_type dt = 0.03;
        const value_type t_end = 10.0;

        std::vector< value_type > times;

        size_t steps = integrate_adaptive( Stepper() , std::make_pair( sys() , jacobi() ) , x , 0.0 , t_end ,
                                        dt , push_back_time( times ) );

        BOOST_CHECK_EQUAL( times.size() , steps+1 );

        BOOST_CHECK_SMALL( times[0] - 0.0 , 2E-16 );
        BOOST_CHECK_SMALL( times[times.size()-1] - t_end , times.size() * 2E-16 );
    }
};

template< class Stepper >
struct perform_integrate_times_test
{
    void operator()( void )
    {
        state_type x( 2 , 1.0 );

        const value_type dt = 0.03;

        std::vector< double > times;

        // simple stepper
        integrate_times( Stepper() , std::make_pair( sys() , jacobi() ) , x ,
                    boost::counting_iterator<int>(0) , boost::counting_iterator<int>(10) ,
                    dt , push_back_time( times ) );

        BOOST_CHECK_EQUAL( static_cast<int>(times.size()) , 10 );

        for( size_t i=0 ; i<times.size() ; ++i )
            // check if observer was called at times 0,1,2,...
            BOOST_CHECK_EQUAL( times[i] , static_cast<double>(i) );
    }
};

template< class Stepper >
struct perform_integrate_n_steps_test
{
    void operator()( void )
    {
        state_type x( 2 , 1.0 );

        const value_type dt = 0.03;
        const int n = 200;

        std::vector< double > times;

        // simple stepper
        value_type end_time = integrate_n_steps( Stepper() , std::make_pair( sys() , jacobi() ) , x , 0.0 , dt , n , push_back_time( times ) );

        BOOST_CHECK_SMALL( end_time - n*dt , 2E-16 );
        BOOST_CHECK_EQUAL( static_cast<int>(times.size()) , n+1 );

        for( size_t i=0 ; i<times.size() ; ++i )
            // check if observer was called at times 0,1,2,...
            BOOST_CHECK_SMALL( times[i] - static_cast< value_type >(i)*dt , (i+1) * 2E-16 );
    }
};

class stepper_methods : public mpl::vector<
    rosenbrock4< value_type > ,
    rosenbrock4_controller< rosenbrock4< value_type > > ,
    rosenbrock4_dense_output< rosenbrock4_controller< rosenbrock4< value_type > > >
> { };

BOOST_AUTO_TEST_SUITE( integrate_test )

BOOST_AUTO_TEST_CASE_TEMPLATE( integrate_const_test_case , Stepper, stepper_methods )
{
    perform_integrate_const_test< Stepper > tester;
    tester();
}

BOOST_AUTO_TEST_CASE_TEMPLATE( integrate_adaptive_test_case , Stepper, stepper_methods )
{
    perform_integrate_adaptive_test< Stepper > tester;
    tester();
}

BOOST_AUTO_TEST_CASE_TEMPLATE( integrate_times_test_case , Stepper, stepper_methods )
{
    perform_integrate_times_test< Stepper > tester;
    tester();
}

class simple_stepper_methods : public mpl::vector<
    rosenbrock4< value_type >
> { };

BOOST_AUTO_TEST_CASE_TEMPLATE( integrate_n_steps_test_case , Stepper, simple_stepper_methods )
{
    perform_integrate_n_steps_test< Stepper > tester;
    tester();
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright Bruno Dutra 2015
//
// 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)
//
// See http://www.boost.org/libs/mpl for documentation.

// $Id$
// $Date$
// $Revision$

#define BOOST_MPL_LIMIT_METAFUNCTION_ARITY 15
#define BOOST_MPL_CFG_NO_PREPROCESSED_HEADERS

#include <boost/mpl/logical.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/apply.hpp>

#include <boost/mpl/aux_/test.hpp>

#include <boost/preprocessor/inc.hpp>
#include <boost/preprocessor/repeat_from_to.hpp>
#include <boost/preprocessor/enum_params.hpp>
#include <boost/preprocessor/facilities/intercept.hpp>

#define TEST_N_ARY(z, N, unused2) \
    static_assert_< \
        apply< \
            and_<BOOST_PP_ENUM_PARAMS_Z(z, N, _ BOOST_PP_INTERCEPT)>, \
            BOOST_PP_ENUM_PARAMS_Z(z, N, true_ BOOST_PP_INTERCEPT) \
        > \
    >(); \
/**/

template<typename assertion>
void static_assert_(){
    MPL_ASSERT((typename assertion::type));
}

MPL_TEST_CASE()
{
    BOOST_PP_REPEAT_FROM_TO(
        2,
        BOOST_PP_INC(BOOST_MPL_LIMIT_METAFUNCTION_ARITY),
        TEST_N_ARY,
        _
    )
}


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Vincent Rouvreau
 *
 *    Copyright (C) 2014 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#include <gudhi/reader_utils.h>
#include <gudhi/graph_simplicial_complex.h>
#include <gudhi/distance_functions.h>
#include <gudhi/Simplex_tree.h>
#include <gudhi/Persistent_cohomology.h>

#include <boost/program_options.hpp>

#include <string>

using namespace Gudhi;
using namespace Gudhi::persistent_cohomology;

typedef double Filtration_value;

void program_options(int argc, char * argv[]
                     , std::string & simplex_tree_file
                     , std::string & output_file
                     , int & p
                     , Filtration_value & min_persistence);

int main(int argc, char * argv[]) {
  std::string simplex_tree_file;
  std::string output_file;
  int p;
  Filtration_value min_persistence;

  program_options(argc, argv, simplex_tree_file, output_file, p, min_persistence);

  std::cout << "Simplex_tree from file=" << simplex_tree_file.c_str() << " - output_file=" << output_file.c_str()
      << std::endl;
  std::cout << "     - p=" << p << " - min_persistence=" << min_persistence << std::endl;

  // Read the list of simplices from a file.
  Simplex_tree<> simplex_tree;

  std::ifstream simplex_tree_stream(simplex_tree_file);
  simplex_tree_stream >> simplex_tree;

  std::cout << "The complex contains " << simplex_tree.num_simplices() << " simplices" << std::endl;
  std::cout << "   - dimension " << simplex_tree.dimension() << std::endl;

  /*
  std::cout << std::endl << std::endl << "Iterator on Simplices in the filtration, with [filtration value]:" << std::endl;
  for( auto f_simplex : simplex_tree.filtration_simplex_range() )
  { std::cout << "   " << "[" << simplex_tree.filtration(f_simplex) << "] ";
  for( auto vertex : simplex_tree.simplex_vertex_range(f_simplex) )
  { std::cout << vertex << " "; }
  std::cout << std::endl;
  }*/

  // Sort the simplices in the order of the filtration
  simplex_tree.initialize_filtration();

  // Compute the persistence diagram of the complex
  Persistent_cohomology< Simplex_tree<>, Field_Zp > pcoh(simplex_tree);
  // initializes the coefficient field for homology
  pcoh.init_coefficients(p);

  pcoh.compute_persistent_cohomology(min_persistence);

  // Output the diagram in output_file
  if (output_file.empty()) {
    pcoh.output_diagram();
  } else {
    std::ofstream out(output_file);
    pcoh.output_diagram(out);
    out.close();
  }

  return 0;
}

void program_options(int argc, char * argv[]
                     , std::string & simplex_tree_file
                     , std::string & output_file
                     , int & p
                     , Filtration_value & min_persistence) {
  namespace po = boost::program_options;
  po::options_description hidden("Hidden options");
  hidden.add_options()
      ("input-file", po::value<std::string>(&simplex_tree_file),
       "Name of file containing a simplex set. Format is one simplex per line (cf. reader_utils.h - read_simplex): Dim1 X11 X12 ... X1d Fil1  ");

  po::options_description visible("Allowed options", 100);
  visible.add_options()
      ("help,h", "produce help message")
      ("output-file,o", po::value<std::string>(&output_file)->default_value(std::string()),
       "Name of file in which the persistence diagram is written. Default print in std::cout")
      ("field-charac,p", po::value<int>(&p)->default_value(11),
       "Characteristic p of the coefficient field Z/pZ for computing homology.")
      ("min-persistence,m", po::value<Filtration_value>(&min_persistence),
       "Minimal lifetime of homology feature to be recorded. Default is 0");

  po::positional_options_description pos;
  pos.add("input-file", 1);

  po::options_description all;
  all.add(visible).add(hidden);

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).
            options(all).positional(pos).run(), vm);
  po::notify(vm);

  if (vm.count("help") || !vm.count("input-file")) {
    std::cout << std::endl;
    std::cout << "Compute the persistent homology with coefficient field Z/pZ \n";
    std::cout << "of a Rips complex defined on a set of input points.\n \n";
    std::cout << "The output diagram contains one bar per line, written with the convention: \n";
    std::cout << "   p   dim b d \n";
    std::cout << "where dim is the dimension of the homological feature,\n";
    std::cout << "b and d are respectively the birth and death of the feature and \n";
    std::cout << "p is the characteristic of the field Z/pZ used for homology coefficients." << std::endl << std::endl;

    std::cout << "Usage: " << argv[0] << " [options] input-file" << std::endl << std::endl;
    std::cout << visible << std::endl;
    exit(-1);
  }
}


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

#define BOOST_TEST_MODULE laplace_matrix_free

#include <deal.II/lac/precondition.h>

#include "laplace_matrix_free.hpp"
#include "main.cc"

namespace tt = boost::test_tools;

template <int dim>
class ExactSolution : public dealii::Function<dim>
{
public:
  ExactSolution() = default;

  virtual ~ExactSolution() override = default;

  double value(dealii::Point<dim> const &p,
               unsigned int const component = 0) const override;
};

template <int dim>
double ExactSolution<dim>::value(dealii::Point<dim> const &p,
                                 unsigned int const) const
{
  double val = 1.;
  for (unsigned int d = 0; d < dim; ++d)
    val *= (p[d] - 1.) * p[d];

  return val;
}

template <int dim>
class Source
{
public:
  Source() = default;

  template <typename ScalarType>
  dealii::VectorizedArray<ScalarType>
  value(dealii::Point<dim, dealii::VectorizedArray<ScalarType>> const &p) const;
};

template <int dim>
template <typename ScalarType>
dealii::VectorizedArray<ScalarType> Source<dim>::value(
    dealii::Point<dim, dealii::VectorizedArray<ScalarType>> const &p) const
{
  auto const zero = dealii::make_vectorized_array<ScalarType>(0.);
  auto const one = dealii::make_vectorized_array<ScalarType>(1.);
  auto const two = dealii::make_vectorized_array<ScalarType>(2.);

  dealii::VectorizedArray<ScalarType> val = zero;
  for (unsigned int d = 0; d < dim; ++d)
  {
    dealii::VectorizedArray<ScalarType> tmp = one;
    for (unsigned int i = 0; i < dim; ++i)
      if (i != d)
        tmp *= (p[i] - one) * p[i];

    val -= two * tmp;
  }

  return val;
}

template <int dim>
class MaterialProperty
{
public:
  MaterialProperty() = default;

  template <typename ScalarType>
  dealii::VectorizedArray<ScalarType>
  value(dealii::Point<dim, dealii::VectorizedArray<ScalarType>> const &p) const;
};

template <int dim>
template <typename ScalarType>
dealii::VectorizedArray<ScalarType> MaterialProperty<dim>::value(
    dealii::Point<dim, dealii::VectorizedArray<ScalarType>> const &) const
{
  return dealii::make_vectorized_array<ScalarType>(1.);
}

BOOST_AUTO_TEST_CASE(laplace_2d)
{
  int constexpr dim = 2;
  int constexpr fe_degree = 2;

  MaterialProperty<dim> material_property;
  Source<dim> source;

  LaplaceMatrixFree<dim, fe_degree, double> laplace(MPI_COMM_WORLD);
  laplace.setup_system(boost::property_tree::ptree(), material_property);
  laplace.assemble_rhs(source);

  dealii::PreconditionIdentity preconditioner;
  laplace.solve(preconditioner);

  // The exact solution is quadratic so the error should be zero.
  ExactSolution<dim> exact_solution;
  BOOST_TEST(laplace.compute_error(exact_solution) == 0., tt::tolerance(1e-14));
}

BOOST_AUTO_TEST_CASE(laplace_3d)
{
  int constexpr dim = 3;
  int constexpr fe_degree = 2;

  MaterialProperty<dim> material_property;
  Source<dim> source;

  LaplaceMatrixFree<dim, fe_degree, double> laplace(MPI_COMM_WORLD);
  laplace.setup_system(boost::property_tree::ptree(), material_property);
  laplace.assemble_rhs(source);

  dealii::PreconditionIdentity preconditioner;
  laplace.solve(preconditioner);

  // The exact solution is quadratic so the error should be zero.
  ExactSolution<dim> exact_solution;
  BOOST_TEST(laplace.compute_error(exact_solution) == 0., tt::tolerance(1e-14));
}


// -*- coding: utf-8 -*-
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/properties.hpp>
#include <boost/property_map/property_map.hpp>
#include <doctest/doctest.h>
#include <netoptim/min_cycle_ratio.hpp> // import min_cycle_ratio, set_default
#include <py2cpp/fractions.hpp>         // import Fraction
#include <py2cpp/nx2bgl.hpp>
#include <utility> // for std::pair
#include <vector>
// from fractions import Fraction

namespace boost
{

enum edge_id_tag_t
{
    id_tag
}; // a unique #
BOOST_INSTALL_PROPERTY(edge, id_tag);

} // namespace boost

using graph_t =
    boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS,
        boost::no_property, boost::property<boost::edge_id_tag_t, size_t>>;
using Vertex = typename boost::graph_traits<graph_t>::vertex_descriptor;
using edge_t = typename boost::graph_traits<graph_t>::edge_iterator;

static auto create_test_case1()
{
    using Edge = std::pair<int, int>;

    const auto num_nodes = 5;
    enum nodes
    {
        A,
        B,
        C,
        D,
        E
    };
    static Edge edge_array[] = {
        Edge {A, B}, Edge {B, C}, Edge {C, D}, Edge {D, E}, Edge {E, A}};
    size_t indices[] = {0, 1, 2, 3, 4};
    int num_arcs = sizeof(edge_array) / sizeof(Edge);
    graph_t g(edge_array, edge_array + num_arcs, indices, num_nodes);
    return xn::grAdaptor<graph_t> {std::move(g)};
}

static auto create_test_case_timing()
{
    using Edge = std::pair<int, int>;

    const auto num_nodes = 3;
    enum nodes
    {
        A,
        B,
        C
    };
    Edge edge_array[] = {Edge {A, B}, Edge {B, A}, Edge {B, C}, Edge {C, B},
        Edge {B, C}, Edge {C, B}, Edge {C, A}, Edge {A, C}};
    size_t indices[] = {0, 1, 2, 3, 4, 5, 6, 7};
    int num_arcs = sizeof(edge_array) / sizeof(Edge);

    graph_t g(edge_array, edge_array + num_arcs, indices, num_nodes);
    return xn::grAdaptor<graph_t> {std::move(g)};
}

TEST_CASE("Test Cycle Ratio (boost)")
{
    using EdgeIndexMap =
        typename boost::property_map<graph_t, boost::edge_id_tag_t>::type;
    using IterMap = boost::iterator_property_map<int*, EdgeIndexMap, int, int&>;

    auto G = create_test_case1();
    int cost[] = {5, 1, 1, 1, 1};
    EdgeIndexMap edge_id = boost::get(boost::id_tag, G);
    IterMap cost_pa(cost, edge_id);

    auto get_cost = [&](const auto& e) -> int
    { return boost::get(cost_pa, e); };
    auto get_time = [&](const auto& /*unused*/) -> int { return 1; };

    auto dist = std::vector<fun::Fraction<int>>(
        G.number_of_nodes(), fun::Fraction<int>(0));
    auto r = fun::Fraction<int>(5);
    const auto c = min_cycle_ratio(G, r, get_cost, get_time, dist);
    CHECK(!c.empty());
    CHECK(c.size() == 5);
    CHECK(r == fun::Fraction<int>(9, 5));
    // print(r);
    // print(c);
    // print(dist.items());
}

TEST_CASE("Test Cycle Ratio of Timing Graph (boost)")
{
    using EdgeIndexMap =
        typename boost::property_map<graph_t, boost::edge_id_tag_t>::type;
    using IterMap = boost::iterator_property_map<int*, EdgeIndexMap, int, int&>;

    auto G = create_test_case_timing();
    int cost[] = {7, -1, 5, 4, 3, 0, 2, 4};
    EdgeIndexMap edge_id = boost::get(boost::id_tag, G);
    IterMap cost_pa(cost, edge_id);

    const auto get_cost = [&](const auto& e) -> int
    { return boost::get(cost_pa, e); };
    const auto get_time = [&](const auto& /*e*/) -> int { return 1; };

    auto dist = std::vector<fun::Fraction<int>>(
        G.number_of_nodes(), fun::Fraction<int>(0));
    auto r = fun::Fraction<int>(7);
    const auto c = min_cycle_ratio(G, r, get_cost, get_time, dist);
    CHECK(!c.empty());
    CHECK(r == fun::Fraction<int>(1, 1));
    CHECK(c.size() == 3);
    // print(r);
    // print(c);
    // print(dist.items());
}


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

  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-ieee
    Function object implementing predecessor capabilities

    Returns the n-th greatest element strictly less than the parameter

    @par Semantic:

    @code
    auto r = predecessor(x,n);
    @endcode

    If n is null returns x else computes the n-th greatest representable value strictly less than x in its type.
    n must be positive or null.

    For integer it saturate at @ref Valmin. For floating point numbers, all @ref Minf
    strict predecessors are @ref Nan.

    @see next, prev, successor, nextafter, Minf, Valmin, Nan

  **/
  Value predecessor(Value const & v0, IntegerValue const& n);
} }
#endif

#include <boost/simd/function/scalar/predecessor.hpp>
#include <boost/simd/function/simd/predecessor.hpp>

#endif


/********************************************************************************
 * Copyright 2017 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * 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.
 ********************************************************************************/

#include "Delaunay.hpp"

#if defined(__cplusplus)
extern "C"
{
#include <libqhull_r/libqhull_r.h>
}
#endif

#include <boost/numeric/conversion/cast.hpp>

using rw::core::ownedPtr;
using namespace rw::math;
using namespace rw::geometry;

namespace {
int build (int dim, double* coords, int nrCoords, std::vector< int >& vertIdxs,
           std::vector< int >& faceIdxs)
{
    vertIdxs.clear ();
    faceIdxs.clear ();

    std::vector< int >& result = vertIdxs;

    int curlong, totlong; /* used !qh_NOmem */
    int exitcode;
    boolT ismalloc = false;
    qhT qh;
    qhT* qhT_pointer = &qh;    // allocate memory for qhull

    char flags[] = "qhull d Qbb Qt Qz";
    // d Qbb: delaunay with scaling of last coordinate (eq. to qdelaunay)
    // Qt: triangulated output
    // Qz: add point-at-infinity (for cocircular points like in rectangles)

    // According to doc. in libqhull_r/user_r.c, the qhull memory should be cleared:
    qh_zero (qhT_pointer, stderr);

    // Then the object is constructed.
    exitcode = qh_new_qhull (qhT_pointer, dim, nrCoords, coords, ismalloc, flags, NULL, stderr);

    // Loop through all vertices,
    vertexT* vertex;    //, **vertexp;
    for (vertex = qhT_pointer->vertex_list; vertex && vertex->next; vertex = vertex->next) {
        int vertexIdx = qh_pointid (qhT_pointer, vertex->point);
        result.push_back (vertexIdx);
    }

    if (!exitcode) {
        // For all facets:
        for (facetT* facet = qhT_pointer->facet_list; facet && facet->next; facet = facet->next) {
            if (!facet->upperdelaunay) {
                int vertex_n, vertex_i;
                FOREACHvertex_i_ (qhT_pointer, facet->vertices)
                {
                    int vertexIdx = qh_pointid (qhT_pointer, vertex->point);
                    faceIdxs.push_back (vertexIdx);
                }
            }
        }
    }
    if (qhT_pointer->VERIFYoutput && !qhT_pointer->FORCEoutput && !qhT_pointer->STOPpoint &&
        !qhT_pointer->STOPcone)
        qh_check_points (qhT_pointer);

    qh_freeqhull (qhT_pointer, False);
    qh_memfreeshort (qhT_pointer, &curlong, &totlong);
    if (curlong || totlong)
        fprintf (
            stderr,
            "qdelaunay internal warning (main): did not free %d bytes of long memory (%d pieces)\n",
            totlong,
            curlong);
    return exitcode;
}

template< typename S >
typename IndexedTriMeshN0< double, S >::Ptr makeMesh (const std::vector< Vector2D<> >& hullVertices,
                                                      const std::vector< int >& faceIdxs,
                                                      const std::vector< double >& values)
{
    typedef IndexedTriMeshN0< double, S > Mesh;
    typedef typename Mesh::VertexArray VertexArray;
    typedef typename Mesh::TriangleArray TriangleArray;

    // Add vertices
    const rw::core::Ptr< VertexArray > meshV = ownedPtr (new VertexArray (hullVertices.size ()));
    for (size_t i = 0; i < hullVertices.size (); i++) {
        (*meshV)[i] = Vector3D<> (hullVertices[i][0],
                                  hullVertices[i][1],
                                  (values.size () == hullVertices.size ()) ? values[i] : 0);
    }

    // Add triangles
    const rw::core::Ptr< TriangleArray > meshTri =
        ownedPtr (new TriangleArray (faceIdxs.size () / 3));
    for (size_t i = 0; i < faceIdxs.size () / 3; i++) {
        const typename Mesh::tri_type tempTri (
            faceIdxs[i * 3 + 0], faceIdxs[i * 3 + 1], faceIdxs[i * 3 + 2]);
        const Vector3D<>& v1 = (*meshV)[tempTri[0]];
        const Vector3D<>& v2 = (*meshV)[tempTri[1]];
        const Vector3D<>& v3 = (*meshV)[tempTri[2]];

        // Make sure the vertices order is correct according to right-hand rule. Otherwise flip v2
        // and v3.
        const double dotProduct = dot (cross (v2 - v1, v3 - v2), Vector3D<>::z ());
        if (dotProduct < 0.0) {
            (*meshTri)[i] = typename Mesh::tri_type (tempTri[0], tempTri[2], tempTri[1]);
        }
        else {
            (*meshTri)[i] = tempTri;
        }
    }

    const typename Mesh::Ptr mesh = ownedPtr (new Mesh (meshV, meshTri));
    return mesh;
}
}    // namespace

Delaunay::Delaunay ()
{}

Delaunay::~Delaunay ()
{}

IndexedTriMesh<>::Ptr Delaunay::triangulate (const std::vector< Vector2D<> >& vertices,
                                             const std::vector< double >& values)
{
    if (vertices.size () == 3) {
        std::vector< int > faceIdxs (3);
        faceIdxs[0] = 0;
        faceIdxs[1] = 1;
        faceIdxs[2] = 2;
        return makeMesh< uint8_t > (vertices, faceIdxs, values);
    }
    const int nrInputVertices = boost::numeric_cast< int > (vertices.size ());

    // convert the vertice array to an array of double
    double* vertArray = new double[vertices.size () * 2];
    // copy all data into the vertArray
    for (size_t i = 0; i < vertices.size (); i++) {
        const Vector2D<>& v  = vertices[i];
        vertArray[i * 2 + 0] = v[0];
        vertArray[i * 2 + 1] = v[1];
    }
    // Build delaunay triangulation
    std::vector< Vector2D<> > triangVertices;
    std::vector< int > vertiIdxs;
    std::vector< int > faceIdxs;

    const int exitcode = build (2, vertArray, nrInputVertices, vertiIdxs, faceIdxs);

    delete[] vertArray;
    if (exitcode)
        RW_THROW ("Delaunay triangulation returned with exit code " << exitcode);

    std::vector< int > vertIdxMap (vertices.size ());
    triangVertices.resize (vertiIdxs.size () - 1);    // Qz gives extra vertex
    int triangVerticesI = 0;
    for (size_t i = 0; i < vertiIdxs.size (); i++) {
        if (vertiIdxs[i] < nrInputVertices) {
            triangVertices[triangVerticesI] = vertices[vertiIdxs[i]];
            vertIdxMap[vertiIdxs[i]]        = triangVerticesI;
            triangVerticesI++;
        }
    }
    for (size_t i = 0; i < faceIdxs.size (); i++) {
        const int tmp = faceIdxs[i];
        faceIdxs[i]   = vertIdxMap[tmp];
    }

    // Create the mesh
    if (vertices.size () <= 256) {
        return makeMesh< uint8_t > (triangVertices, faceIdxs, values);
    }
    else if (vertices.size () <= 65536) {
        return makeMesh< uint16_t > (triangVertices, faceIdxs, values);
    }
    else if (vertices.size () <= 4294967296) {
        return makeMesh< uint32_t > (triangVertices, faceIdxs, values);
    }
    else {
        return makeMesh< uint64_t > (triangVertices, faceIdxs, values);
    }
}


#ifndef __COMMON_H
#define __COMMON_H

#include <iostream>
#include <armadillo>
#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <boost/thread/thread.hpp>
#include <boost/array.hpp>

#include "grSim_Packet.pb.h"
#include "grSim_Commands.pb.h"
#include "grSim_Replacement.pb.h"

#include "messages_robocup_ssl_detection.pb.h"
#include "messages_robocup_ssl_geometry.pb.h"
#include "messages_robocup_ssl_refbox_log.pb.h"
#include "messages_robocup_ssl_robot_status.pb.h"
#include "messages_robocup_ssl_wrapper.pb.h"

using byte = unsigned char;
const int NUM_ROBOTS = 6;
enum team_color_t {
    BLUE,
    YELLOW
};

static constexpr const char* LOCAL_HOST = "127.0.0.1";

const float Pi = 3.1415926;

inline double to_radian(double deg) { return deg * Pi / 180.000;}
inline double to_degree(double rad) { return rad * (180.000 / Pi);}

extern arma::vec unit_vec_x;  // definition in common.cpp
extern arma::vec unit_vec_y;  // definition in common.cpp

arma::mat rotation_matrix_2D(double angle_degree);
arma::mat change_basis_matrix_2D(arma::vec vx, arma::vec vy) ;

using range_t = std::pair<double, double>;

double map(double value, range_t from, range_t to);
arma::vec map(arma::vec value, range_t from, range_t to);
void map2(arma::vec& value, range_t from, range_t to);

#endif


#include <boost/graph/compressed_sparse_row_graph.hpp>
#include <array>
#include <utility>

int main()
{
  enum { topLeft, topRight, bottomRight, bottomLeft };

  std::array<std::pair<int, int>, 4> edges{{
    std::make_pair(topLeft, topRight),
    std::make_pair(topRight, bottomRight),
    std::make_pair(bottomRight, bottomLeft),
    std::make_pair(bottomLeft, topLeft)
  }};

  typedef boost::compressed_sparse_row_graph<boost::bidirectionalS> graph;
  graph g{boost::edges_are_unsorted_multi_pass, edges.begin(),
    edges.end(), 4};
}


// Copyright (C) 2017 Vicente J. Botet Escriba
//
//  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)

// <experimental/strong_type.hpp>

/**
 * This example tries to see how close we can define address/offset using strong_type
 *
 */
// todo These types are isomorphic to index_t and difference_t when we use
// an index_t to address an array and
// difference_t to for the result of subtracting two indexes as the offset between the two indexes.

#include <boost/detail/lightweight_test.hpp>
#include <iostream>
#include <cstdint>

#include <experimental/strong_counter.hpp>
#include <experimental/strong_random_incrementable.hpp>

namespace stdex = std::experimental;

struct offset_tag {};
struct offset_tag2 {};

namespace std {
namespace experimental {
inline  namespace fundamental_v3{
namespace mixin {
  template <>
  struct is_compatible_with<offset_tag, offset_tag> : std::true_type {};
}
}
}
}

using offset = stdex::strong_counter<offset_tag, std::int64_t>;
using offset1 = stdex::tagged<offset_tag, std::int64_t>;
using offset1u = stdex::tagged<offset_tag, std::int32_t>;
using offset2 = stdex::tagged<offset_tag2, std::int64_t>;
using offset3 = stdex::tagged<offset_tag2, std::int32_t>;

using address = stdex::strong_random_incrementable<struct address_tag, std::int64_t, offset>;
using address1 = stdex::strong_random_incrementable<struct address_tag, std::int64_t, offset>;
using address1u = stdex::strong_random_incrementable<struct address_tag, std::int32_t, offset>;
using address2 = stdex::strong_random_incrementable<struct address_tag2, std::int64_t, offset>;
using address3 = stdex::strong_random_incrementable<struct address_tag2, std::int32_t, offset>;

template <class T> struct check;
int main()
{
    {
      offset o1 {3};
      offset o2 {4};
      auto o =  o1 + o2;
      BOOST_TEST_EQ(o.underlying() , 7);
    }
    {
      offset1 o1 {3};
      offset1u o2 =offset1u{o1};
      BOOST_TEST_EQ(o2.underlying() , 3);
    }
#if 0
    {
      offset1 o1 {3};
      offset2 o2 =offset2{o1}; // must fail
    }
    {
      offset1 o1 {3};
      offset3 o2 =offset3{o1}; // must fail
    }
#endif
    {
      address1 a1 {4};
      address1u a2 = address1u(a1);
      BOOST_TEST_EQ(a2.underlying() , 4);
    }
#if 0
    {
      address1 a1 {4};
      address2 a2 = address2(a1);
      BOOST_TEST_EQ(a2.underlying() , 4); // must fail
    }
    {
      address1 a1 {4};
      address3 a2 = address3(a1);
      BOOST_TEST_EQ(a2.underlying() , 4); // must fail
    }
#endif
  {
    address a {4};
    address a2 = ++a;
    BOOST_TEST_EQ(a2.underlying() , 5);
    BOOST_TEST_EQ(a.underlying() , 5);
  }
  {
    address a {4};
    address a2 = a++;
    BOOST_TEST_EQ(a2.underlying() , 4);
    BOOST_TEST_EQ(a.underlying() , 5);
  }
//  {
//    address a1 {4};
//    address a2 {3};
//    auto a =  a1 + a2; // compile fails as expected
//  }
  {
    address a1 {4};
    address a2 {3};
    offset a =  a1 - a2;
    BOOST_TEST_EQ(a.underlying() , 1);
  }
  {
    address a1 {4};
    offset o {4};
    address a2 =  a1 + o;
    BOOST_TEST_EQ(a2.underlying() , 8);
  }
  {
    address a1 {4};
    offset o {4};
    address a2 =  a1 + 3 * o;
    BOOST_TEST_EQ(a2.underlying() , 16);
  }
  {
    address a {4};
    offset o {4};
    a += o;
    BOOST_TEST_EQ(a.underlying() , 8);
  }
  {
    address a1 {4};
    offset o {4};
    address a2 =  a1 - o;
    BOOST_TEST_EQ(a2.underlying() , 0);
  }
  {
    address a {4};
    offset o {4};
    a -=  o;
    BOOST_TEST_EQ(a.underlying() , 0);
  }
  {
    address a {4};
    offset o {4};
    advance(a,o);
    BOOST_TEST_EQ(a.underlying() , 8);
  }
  {
    address a {4};
    address b {4};
    auto x =  distance(a,b);
    BOOST_TEST_EQ(x.underlying() , 0);
  }
  {
    address a {4};
    auto x =  next(a);
    BOOST_TEST_EQ(x.underlying() , 5);
  }
  {
    address a {4};
    auto x =  prev(a);
    BOOST_TEST_EQ(x.underlying() , 3);
  }
    return ::boost::report_errors();
}


// Copyright (c) 2018 Graphcore Ltd. All rights reserved.

#define BOOST_TEST_MODULE SortTest
#include <poplibs_support/TestDevice.hpp>

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/test/unit_test.hpp>

#include <poplar/Engine.hpp>
#include <popops/Sort.hpp>
#include <popops/codelets.hpp>
#include <poputil/TileMapping.hpp>
#include <poputil/exceptions.hpp>

using namespace poplar;
using namespace poplar::program;
using namespace poputil;
using namespace popops;
using namespace poplibs_support;

template <typename T, std::size_t N>
std::array<T, N> deviceSort(std::array<T, N> in,
                            std::vector<std::size_t> shape = {N},
                            unsigned dim = 0) {
  BOOST_REQUIRE(dim < shape.size());

  auto device = createTestDevice(TEST_TARGET, 1, 4);
  Graph graph(device.getTarget());
  auto seq = Sequence();
  popops::addCodelets(graph);

  Tensor tIn = graph.addVariable(equivalent_device_type<T>().value, shape);
  poputil::mapTensorLinearly(graph, tIn);

  BOOST_REQUIRE_EQUAL(tIn.numElements(), N);

  Tensor tOut = sort(graph, tIn, dim, seq);

  graph.createHostWrite("in", tIn);
  graph.createHostRead("out", tOut);

  std::array<T, N> out;
  Engine eng(graph, seq);
  device.bind([&](const Device &d) {
    eng.load(d);
    eng.writeTensor("in", in.data(), in.data() + in.size());
    eng.run();

    eng.readTensor("out", out.data(), out.data() + out.size());
  });

  return out;
}

template <typename T1, typename T2, std::size_t N>
std::array<T2, N> deviceSortKV(std::array<T1, N> key, std::array<T2, N> value,
                               std::vector<std::size_t> shape = {N},
                               unsigned dim = 0) {
  BOOST_REQUIRE(dim < shape.size());

  auto device = createTestDevice(TEST_TARGET, 1, 4);
  Graph graph(device.getTarget());
  auto seq = Sequence();
  popops::addCodelets(graph);

  Tensor tKey = graph.addVariable(equivalent_device_type<T1>().value, shape);
  Tensor tValue = graph.addVariable(equivalent_device_type<T2>().value, shape);
  poputil::mapTensorLinearly(graph, tKey);
  poputil::mapTensorLinearly(graph, tValue);

  BOOST_REQUIRE_EQUAL(tKey.numElements(), N);
  BOOST_REQUIRE_EQUAL(tValue.numElements(), N);

  Tensor tOut = sortKeyValue(graph, tKey, tValue, dim, seq);

  graph.createHostWrite("key", tKey);
  graph.createHostWrite("value", tValue);
  graph.createHostRead("out", tOut);

  std::array<T2, N> out;
  Engine eng(graph, seq);
  device.bind([&](const Device &d) {
    eng.load(d);
    eng.writeTensor("key", key.data(), key.data() + key.size());
    eng.writeTensor("value", value.data(), value.data() + value.size());
    eng.run();

    eng.readTensor("out", out.data(), out.data() + out.size());
  });

  return out;
}

BOOST_AUTO_TEST_CASE(DeviceSortFloat) {
  std::array<float, 64> in;
  boost::random::mt19937 gen;
  boost::random::uniform_int_distribution<> dist(-1024, 1024);
  std::generate(std::begin(in), std::end(in), std::bind(dist, gen));
  auto out = deviceSort(in);

  // Check that we have the same elements in some order
  BOOST_CHECK(
      std::is_permutation(std::begin(in), std::end(in), std::begin(out)));

  // Check that the elements are in sorted order
  BOOST_CHECK(std::is_sorted(std::begin(out), std::end(out)));

  out = deviceSort(in, {4, 4, 4}, 2);
  BOOST_CHECK(
      std::is_permutation(std::begin(in), std::end(in), std::begin(out)));

  // Check that the elements are in sorted order on the specified dimension
  for (int i = 0; i < 4; ++i) {
    for (int j = 0; j < 4; ++j) {
      const auto begin = out.data() + (i * 16 + j * 4);
      const auto end = out.data() + (i * 16 + (j + 1) * 4);

      BOOST_CHECK(std::is_sorted(begin, end));
    }
  }

  out = deviceSort(in, {4, 4, 4}, 1);
  BOOST_CHECK(
      std::is_permutation(std::begin(in), std::end(in), std::begin(out)));

  // Check that the elements are in sorted order on the specified dimension
  for (int i = 0; i < 4; ++i) {
    for (int j = 0; j < 3; ++j) {
      for (int k = 0; k < 4; ++k) {
        BOOST_CHECK(out[i * 16 + j * 4 + k] <= out[i * 16 + (j + 1) * 4 + k]);
      }
    }
  }

  out = deviceSort(in, {4, 4, 4}, 0);
  BOOST_CHECK(
      std::is_permutation(std::begin(in), std::end(in), std::begin(out)));

  // Check that the elements are in sorted order on the specified dimension
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 4; ++j) {
      for (int k = 0; k < 4; ++k) {
        BOOST_CHECK(out[i * 16 + j * 4 + k] <= out[(i + 1) * 16 + j * 4 + k]);
      }
    }
  }

  out = deviceSort(in, {16, 4}, 0);
  BOOST_CHECK(
      std::is_permutation(std::begin(in), std::end(in), std::begin(out)));

  // Check that the elements are in sorted order on the specified dimension
  for (int i = 0; i < 15; ++i) {
    for (int j = 0; j < 4; ++j) {
      BOOST_CHECK(out[i * 4 + j] <= out[(i + 1) * 4 + j]);
    }
  }

  out = deviceSort(in, {16, 4}, 1);
  BOOST_CHECK(
      std::is_permutation(std::begin(in), std::end(in), std::begin(out)));

  // Check that the elements are in sorted order on the specified dimension
  for (int i = 0; i < 15; ++i) {
    const auto begin = out.data() + (i * 4);
    const auto end = out.data() + ((i + 1) * 4);

    BOOST_CHECK(std::is_sorted(begin, end));
  }
}

BOOST_AUTO_TEST_CASE(DeviceSortInt) {
  std::array<int, 64> in;
  boost::random::mt19937 gen;
  boost::random::uniform_int_distribution<> dist(-1024, 1024);
  std::generate(std::begin(in), std::end(in), std::bind(dist, gen));
  auto out = deviceSort(in);

  // Check that we have the same elements in some order
  BOOST_CHECK(
      std::is_permutation(std::begin(in), std::end(in), std::begin(out)));

  // Check that the elements are in sorted order
  BOOST_CHECK(std::is_sorted(std::begin(out), std::end(out)));

  out = deviceSort(in, {4, 4, 4}, 2);
  BOOST_CHECK(
      std::is_permutation(std::begin(in), std::end(in), std::begin(out)));

  // Check that the elements are in sorted order on the specified dimension
  for (int i = 0; i < 4; ++i) {
    for (int j = 0; j < 4; ++j) {
      const auto begin = out.data() + (i * 16 + j * 4);
      const auto end = out.data() + (i * 16 + (j + 1) * 4);

      BOOST_CHECK(std::is_sorted(begin, end));
    }
  }

  out = deviceSort(in, {4, 4, 4}, 1);
  BOOST_CHECK(
      std::is_permutation(std::begin(in), std::end(in), std::begin(out)));

  // Check that the elements are in sorted order on the specified dimension
  for (int i = 0; i < 4; ++i) {
    for (int j = 0; j < 3; ++j) {
      for (int k = 0; k < 4; ++k) {
        BOOST_CHECK(out[i * 16 + j * 4 + k] <= out[i * 16 + (j + 1) * 4 + k]);
      }
    }
  }

  out = deviceSort(in, {4, 4, 4}, 0);
  BOOST_CHECK(
      std::is_permutation(std::begin(in), std::end(in), std::begin(out)));

  // Check that the elements are in sorted order on the specified dimension
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 4; ++j) {
      for (int k = 0; k < 4; ++k) {
        BOOST_CHECK(out[i * 16 + j * 4 + k] <= out[(i + 1) * 16 + j * 4 + k]);
      }
    }
  }

  out = deviceSort(in, {16, 4}, 0);
  BOOST_CHECK(
      std::is_permutation(std::begin(in), std::end(in), std::begin(out)));

  // Check that the elements are in sorted order on the specified dimension
  for (int i = 0; i < 15; ++i) {
    for (int j = 0; j < 4; ++j) {
      BOOST_CHECK(out[i * 4 + j] <= out[(i + 1) * 4 + j]);
    }
  }

  out = deviceSort(in, {16, 4}, 1);
  BOOST_CHECK(
      std::is_permutation(std::begin(in), std::end(in), std::begin(out)));

  // Check that the elements are in sorted order on the specified dimension
  for (int i = 0; i < 15; ++i) {
    const auto begin = out.data() + (i * 4);
    const auto end = out.data() + ((i + 1) * 4);

    BOOST_CHECK(std::is_sorted(begin, end));
  }
}

BOOST_AUTO_TEST_CASE(DeviceSortKVFloat) {
  std::array<float, 64> in;
  boost::random::mt19937 gen;
  boost::random::uniform_int_distribution<> dist(-1024, 1024);
  std::generate(std::begin(in), std::end(in), std::bind(dist, gen));
  auto out = deviceSortKV(in, in);

  // Check that we have the same elements in some order
  BOOST_CHECK(
      std::is_permutation(std::begin(in), std::end(in), std::begin(out)));

  // Check that the elements are in sorted order
  BOOST_CHECK(std::is_sorted(std::begin(out), std::end(out)));

  out = deviceSortKV(in, in, {4, 4, 4}, 2);
  BOOST_CHECK(
      std::is_permutation(std::begin(in), std::end(in), std::begin(out)));

  // Check that the elements are in sorted order on the specified dimension
  for (int i = 0; i < 4; ++i) {
    for (int j = 0; j < 4; ++j) {
      const auto begin = out.data() + (i * 16 + j * 4);
      const auto end = out.data() + (i * 16 + (j + 1) * 4);

      BOOST_CHECK(std::is_sorted(begin, end));
    }
  }

  out = deviceSortKV(in, in, {4, 4, 4}, 1);
  BOOST_CHECK(
      std::is_permutation(std::begin(in), std::end(in), std::begin(out)));

  // Check that the elements are in sorted order on the specified dimension
  for (int i = 0; i < 4; ++i) {
    for (int j = 0; j < 3; ++j) {
      for (int k = 0; k < 4; ++k) {
        BOOST_CHECK(out[i * 16 + j * 4 + k] <= out[i * 16 + (j + 1) * 4 + k]);
      }
    }
  }

  out = deviceSortKV(in, in, {4, 4, 4}, 0);
  BOOST_CHECK(
      std::is_permutation(std::begin(in), std::end(in), std::begin(out)));

  // Check that the elements are in sorted order on the specified dimension
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 4; ++j) {
      for (int k = 0; k < 4; ++k) {
        BOOST_CHECK(out[i * 16 + j * 4 + k] <= out[(i + 1) * 16 + j * 4 + k]);
      }
    }
  }

  out = deviceSortKV(in, in, {16, 4}, 0);
  BOOST_CHECK(
      std::is_permutation(std::begin(in), std::end(in), std::begin(out)));

  // Check that the elements are in sorted order on the specified dimension
  for (int i = 0; i < 15; ++i) {
    for (int j = 0; j < 4; ++j) {
      BOOST_CHECK(out[i * 4 + j] <= out[(i + 1) * 4 + j]);
    }
  }

  out = deviceSortKV(in, in, {16, 4}, 1);
  BOOST_CHECK(
      std::is_permutation(std::begin(in), std::end(in), std::begin(out)));

  // Check that the elements are in sorted order on the specified dimension
  for (int i = 0; i < 15; ++i) {
    const auto begin = out.data() + (i * 4);
    const auto end = out.data() + ((i + 1) * 4);

    BOOST_CHECK(std::is_sorted(begin, end));
  }
}

BOOST_AUTO_TEST_CASE(DeviceSortKVInt) {
  std::array<int, 64> in;
  boost::random::mt19937 gen;
  boost::random::uniform_int_distribution<> dist(-1024, 1024);
  std::generate(std::begin(in), std::end(in), std::bind(dist, gen));
  auto out = deviceSortKV(in, in);

  // Check that we have the same elements in some order
  BOOST_CHECK(
      std::is_permutation(std::begin(in), std::end(in), std::begin(out)));

  // Check that the elements are in sorted order
  BOOST_CHECK(std::is_sorted(std::begin(out), std::end(out)));

  out = deviceSortKV(in, in, {4, 4, 4}, 2);
  BOOST_CHECK(
      std::is_permutation(std::begin(in), std::end(in), std::begin(out)));

  // Check that the elements are in sorted order on the specified dimension
  for (int i = 0; i < 4; ++i) {
    for (int j = 0; j < 4; ++j) {
      const auto begin = out.data() + (i * 16 + j * 4);
      const auto end = out.data() + (i * 16 + (j + 1) * 4);

      BOOST_CHECK(std::is_sorted(begin, end));
    }
  }

  out = deviceSortKV(in, in, {4, 4, 4}, 1);
  BOOST_CHECK(
      std::is_permutation(std::begin(in), std::end(in), std::begin(out)));

  // Check that the elements are in sorted order on the specified dimension
  for (int i = 0; i < 4; ++i) {
    for (int j = 0; j < 3; ++j) {
      for (int k = 0; k < 4; ++k) {
        BOOST_CHECK(out[i * 16 + j * 4 + k] <= out[i * 16 + (j + 1) * 4 + k]);
      }
    }
  }

  out = deviceSortKV(in, in, {4, 4, 4}, 0);
  BOOST_CHECK(
      std::is_permutation(std::begin(in), std::end(in), std::begin(out)));

  // Check that the elements are in sorted order on the specified dimension
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 4; ++j) {
      for (int k = 0; k < 4; ++k) {
        BOOST_CHECK(out[i * 16 + j * 4 + k] <= out[(i + 1) * 16 + j * 4 + k]);
      }
    }
  }

  out = deviceSortKV(in, in, {16, 4}, 0);
  BOOST_CHECK(
      std::is_permutation(std::begin(in), std::end(in), std::begin(out)));

  // Check that the elements are in sorted order on the specified dimension
  for (int i = 0; i < 15; ++i) {
    for (int j = 0; j < 4; ++j) {
      BOOST_CHECK(out[i * 4 + j] <= out[(i + 1) * 4 + j]);
    }
  }

  out = deviceSortKV(in, in, {16, 4}, 1);
  BOOST_CHECK(
      std::is_permutation(std::begin(in), std::end(in), std::begin(out)));

  // Check that the elements are in sorted order on the specified dimension
  for (int i = 0; i < 15; ++i) {
    const auto begin = out.data() + (i * 4);
    const auto end = out.data() + ((i + 1) * 4);

    BOOST_CHECK(std::is_sorted(begin, end));
  }
}

BOOST_AUTO_TEST_CASE(DeviceSortKVFloatUInt) {
  std::array<float, 64> key;
  std::array<unsigned, 64> value;
  boost::random::mt19937 gen;
  boost::random::uniform_real_distribution<> dist(-1024, 1024);
  std::generate(std::begin(key), std::end(key), std::bind(dist, gen));
  std::iota(value.begin(), value.end(), 0);
  auto out = deviceSortKV(key, value);

  // Check that we have the same elements in some order
  BOOST_CHECK(
      std::is_permutation(std::begin(value), std::end(value), std::begin(out)));

  // Check that the elements are in sorted order
  std::array<float, 64> keyPermuted;
  for (std::size_t i = 0; i < out.size(); ++i) {
    keyPermuted[i] = key[out[i]];
  }
  BOOST_CHECK(std::is_sorted(std::begin(keyPermuted), std::begin(keyPermuted)));

  out = deviceSortKV(key, value, {4, 4, 4}, 2);
  BOOST_CHECK(
      std::is_permutation(std::begin(value), std::end(value), std::begin(out)));

  // Check that the elements are in sorted order on the specified dimension
  for (std::size_t i = 0; i < out.size(); ++i) {
    keyPermuted[i] = key[out[i]];
  }
  for (int i = 0; i < 4; ++i) {
    for (int j = 0; j < 4; ++j) {
      const auto begin = keyPermuted.data() + (i * 16 + j * 4);
      const auto end = keyPermuted.data() + (i * 16 + (j + 1) * 4);

      BOOST_CHECK(std::is_sorted(begin, end));
    }
  }

  out = deviceSortKV(key, value, {4, 4, 4}, 1);
  BOOST_CHECK(
      std::is_permutation(std::begin(value), std::end(value), std::begin(out)));

  // Check that the elements are in sorted order on the specified dimension
  for (std::size_t i = 0; i < out.size(); ++i) {
    keyPermuted[i] = key[out[i]];
  }
  for (int i = 0; i < 4; ++i) {
    for (int j = 0; j < 3; ++j) {
      for (int k = 0; k < 4; ++k) {
        BOOST_CHECK(keyPermuted[i * 16 + j * 4 + k] <=
                    keyPermuted[i * 16 + (j + 1) * 4 + k]);
      }
    }
  }

  out = deviceSortKV(key, value, {4, 4, 4}, 0);
  BOOST_CHECK(
      std::is_permutation(std::begin(value), std::end(value), std::begin(out)));

  // Check that the elements are in sorted order on the specified dimension
  for (std::size_t i = 0; i < out.size(); ++i) {
    keyPermuted[i] = key[out[i]];
  }
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 4; ++j) {
      for (int k = 0; k < 4; ++k) {
        BOOST_CHECK(keyPermuted[i * 16 + j * 4 + k] <=
                    keyPermuted[(i + 1) * 16 + j * 4 + k]);
      }
    }
  }

  out = deviceSortKV(key, value, {16, 4}, 0);
  BOOST_CHECK(
      std::is_permutation(std::begin(value), std::end(value), std::begin(out)));

  // Check that the elements are in sorted order on the specified dimension
  for (std::size_t i = 0; i < out.size(); ++i) {
    keyPermuted[i] = key[out[i]];
  }
  for (int i = 0; i < 15; ++i) {
    for (int j = 0; j < 4; ++j) {
      BOOST_CHECK(keyPermuted[i * 4 + j] <= keyPermuted[(i + 1) * 4 + j]);
    }
  }

  out = deviceSortKV(key, value, {16, 4}, 1);
  BOOST_CHECK(
      std::is_permutation(std::begin(value), std::end(value), std::begin(out)));

  // Check that the elements are in sorted order on the specified dimension
  for (std::size_t i = 0; i < out.size(); ++i) {
    keyPermuted[i] = key[out[i]];
  }
  for (int i = 0; i < 15; ++i) {
    const auto begin = keyPermuted.data() + (i * 4);
    const auto end = keyPermuted.data() + ((i + 1) * 4);

    BOOST_CHECK(std::is_sorted(begin, end));
  }
}

BOOST_AUTO_TEST_CASE(DeviceSortKVFloatInt) {
  std::array<float, 64> key;
  std::array<int, 64> value;
  boost::random::mt19937 gen;
  boost::random::uniform_real_distribution<> dist(-1024, 1024);
  std::generate(std::begin(key), std::end(key), std::bind(dist, gen));
  std::iota(value.begin(), value.end(), 0);
  auto out = deviceSortKV(key, value);

  // Check that we have the same elements in some order
  BOOST_CHECK(
      std::is_permutation(std::begin(value), std::end(value), std::begin(out)));

  // Check that the elements are in sorted order
  std::array<float, 64> keyPermuted;
  for (std::size_t i = 0; i < out.size(); ++i) {
    keyPermuted[i] = key[out[i]];
  }
  BOOST_CHECK(std::is_sorted(std::begin(keyPermuted), std::begin(keyPermuted)));

  out = deviceSortKV(key, value, {4, 4, 4}, 2);
  BOOST_CHECK(
      std::is_permutation(std::begin(value), std::end(value), std::begin(out)));

  // Check that the elements are in sorted order on the specified dimension
  for (std::size_t i = 0; i < out.size(); ++i) {
    keyPermuted[i] = key[out[i]];
  }
  for (int i = 0; i < 4; ++i) {
    for (int j = 0; j < 4; ++j) {
      const auto begin = keyPermuted.data() + (i * 16 + j * 4);
      const auto end = keyPermuted.data() + (i * 16 + (j + 1) * 4);

      BOOST_CHECK(std::is_sorted(begin, end));
    }
  }

  out = deviceSortKV(key, value, {4, 4, 4}, 1);
  BOOST_CHECK(
      std::is_permutation(std::begin(value), std::end(value), std::begin(out)));

  // Check that the elements are in sorted order on the specified dimension
  for (std::size_t i = 0; i < out.size(); ++i) {
    keyPermuted[i] = key[out[i]];
  }
  for (int i = 0; i < 4; ++i) {
    for (int j = 0; j < 3; ++j) {
      for (int k = 0; k < 4; ++k) {
        BOOST_CHECK(keyPermuted[i * 16 + j * 4 + k] <=
                    keyPermuted[i * 16 + (j + 1) * 4 + k]);
      }
    }
  }

  out = deviceSortKV(key, value, {4, 4, 4}, 0);
  BOOST_CHECK(
      std::is_permutation(std::begin(value), std::end(value), std::begin(out)));

  // Check that the elements are in sorted order on the specified dimension
  for (std::size_t i = 0; i < out.size(); ++i) {
    keyPermuted[i] = key[out[i]];
  }
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 4; ++j) {
      for (int k = 0; k < 4; ++k) {
        BOOST_CHECK(keyPermuted[i * 16 + j * 4 + k] <=
                    keyPermuted[(i + 1) * 16 + j * 4 + k]);
      }
    }
  }

  out = deviceSortKV(key, value, {16, 4}, 0);
  BOOST_CHECK(
      std::is_permutation(std::begin(value), std::end(value), std::begin(out)));

  // Check that the elements are in sorted order on the specified dimension
  for (std::size_t i = 0; i < out.size(); ++i) {
    keyPermuted[i] = key[out[i]];
  }
  for (int i = 0; i < 15; ++i) {
    for (int j = 0; j < 4; ++j) {
      BOOST_CHECK(keyPermuted[i * 4 + j] <= keyPermuted[(i + 1) * 4 + j]);
    }
  }

  out = deviceSortKV(key, value, {16, 4}, 1);
  BOOST_CHECK(
      std::is_permutation(std::begin(value), std::end(value), std::begin(out)));

  // Check that the elements are in sorted order on the specified dimension
  for (std::size_t i = 0; i < out.size(); ++i) {
    keyPermuted[i] = key[out[i]];
  }
  for (int i = 0; i < 15; ++i) {
    const auto begin = keyPermuted.data() + (i * 4);
    const auto end = keyPermuted.data() + ((i + 1) * 4);

    BOOST_CHECK(std::is_sorted(begin, end));
  }
}