#include <iostream>

#include "FiniteElementMesh.h"

#include <Eigen/Dense>
#include <map>

template<class T>
struct LatticeMesh : public FiniteElementMesh<T>
{
    using Base = FiniteElementMesh<T>;

    // from AnimatedTetrahedonMesh
    using Base::m_meshElements;
    using Base::m_meshBoundaryElements;
    using Base::m_particleX;
    using Base::initializeUSD;
    using Base::initializeTopology;
    using Base::initializeParticles;
    using Vector3 = typename Base::Vector3;

    // from FiniteElementMesh
    using Base::m_surfaceParticles;
    using Base::m_particleV;
    using Base::m_particleMass;
    using Base::initializeUndeformedConfiguration;
    using Base::m_stepEndTime;

    std::array<int, 3> m_cellSize; // dimensions in grid cells
    T m_gridDX;

    std::vector<std::array<int, 3>> m_activeCells; // Marks the "active" cells in the lattice
    std::map<std::array<int, 3>, int> m_activeNodes; // Maps the "active" nodes to their particle index

    std::vector<Vector3> m_particleUndeformedX;
    std::vector<int> m_leftHandleIndices;
    std::vector<int> m_rightHandleIndices;
    Vector3 m_leftHandleVelocity;
    Vector3 m_rightHandleVelocity;

    LatticeMesh()
        :Base(1.e0, 5., 4., .05)
    {
        // m_leftHandleVelocity  = Vector3(-.2, 0., 0.);
        // m_rightHandleVelocity = Vector3( .2, 0., 0.);
        m_leftHandleVelocity  = Vector3::Zero();
        m_rightHandleVelocity = Vector3::Zero();
    }

    void initialize()
    {
        initializeUSD("GroundCollision3D.usda");

        // Activate cells within a sphere of radius m_radius (in cells)

        for(int cell_i = 0; cell_i < m_cellSize[0]; cell_i++)
        for(int cell_j = 0; cell_j < m_cellSize[1]; cell_j++)
        for(int cell_k = 0; cell_k < m_cellSize[2]; cell_k++)
                m_activeCells.push_back(std::array<int, 3>{cell_i, cell_j, cell_k});
        std::cout << "Created a model including " << m_activeCells.size() << " lattice cells" <<std::endl;

        // Create (uniquely numbered) particles at the node corners of active cells

        for(const auto& cell: m_activeCells){
            std::array<int, 3> node;
            for(node[0] = cell[0]; node[0] <= cell[0]+1; node[0]++)
            for(node[1] = cell[1]; node[1] <= cell[1]+1; node[1]++)
            for(node[2] = cell[2]; node[2] <= cell[2]+1; node[2]++){
                auto search = m_activeNodes.find(node);
                if(search == m_activeNodes.end()){ // Particle not yet created at this lattice node location -> make one
                    m_activeNodes.insert({node, m_particleX.size()});
                    m_particleX.emplace_back(m_gridDX * T(node[0]), m_gridDX * T(node[1]), m_gridDX * T(node[2]));
                }
            }
        }
        std::cout << "Model contains " << m_particleX.size() << " particles" << std::endl;

        // Make tetrahedra out of all active cells (6 tetrahedra per cell)

        for(const auto& cell: m_activeCells){
            int vertexIndices[2][2][2];
            for(int i = 0; i <= 1; i++)
            for(int j = 0; j <= 1; j++)
            for(int k = 0; k <= 1; k++){
                std::array<int, 3> node{cell[0] + i, cell[1] + j, cell[2] + k};
                auto search = m_activeNodes.find(node);
                if(search != m_activeNodes.end())
                    vertexIndices[i][j][k] = search->second;
                else
                    throw std::logic_error("particle at cell vertex not found");
            }

            m_meshElements.push_back(std::array<int, 4>{ vertexIndices[0][0][0], vertexIndices[1][0][0], vertexIndices[1][1][0], vertexIndices[1][1][1]});
            m_meshElements.push_back(std::array<int, 4>{ vertexIndices[0][0][0], vertexIndices[1][0][0], vertexIndices[1][1][1], vertexIndices[1][0][1]});
            m_meshElements.push_back(std::array<int, 4>{ vertexIndices[0][0][0], vertexIndices[1][0][1], vertexIndices[1][1][1], vertexIndices[0][0][1]});
            m_meshElements.push_back(std::array<int, 4>{ vertexIndices[0][0][0], vertexIndices[1][1][1], vertexIndices[0][1][1], vertexIndices[0][0][1]});
            m_meshElements.push_back(std::array<int, 4>{ vertexIndices[0][0][0], vertexIndices[1][1][1], vertexIndices[0][1][0], vertexIndices[0][1][1]});
            m_meshElements.push_back(std::array<int, 4>{ vertexIndices[0][0][0], vertexIndices[1][1][0], vertexIndices[0][1][0], vertexIndices[1][1][1]});
        }

        // Perform the USD-specific initialization of topology & particles
        // (this will also create a boundary *surface* to visualuze

        initializeTopology();
        initializeParticles();

        // Record surface particles (for collisions)
        std::vector<bool> particleOnSurface(m_particleX.size());
        for(auto element: m_meshBoundaryElements) {
            for (auto p: element)
                if (!particleOnSurface[p]){
                    m_surfaceParticles.push_back(p);
                    particleOnSurface[p] = true;
                }
        }

        // Check particle indexing in mesh

        for(const auto& element: m_meshElements)
            for(const auto vertex: element)
                if(vertex < 0 || vertex >= m_particleX.size())
                    throw std::logic_error("mismatch between mesh vertex and particle array");

        // Also resize the velocities to match
        m_particleV.resize(m_particleX.size(), Vector3::Zero());

        // Initialize rest shape matrices and particle mass
        initializeUndeformedConfiguration();

        // Also record rest shape
        m_particleUndeformedX = m_particleX;

        // Identify particles on left and right handles
#if 0
        for(int node_j = 0; node_j <= m_cellSize[1]; node_j++)
            for(int node_k = 0; node_k <= m_cellSize[2]; node_k++) {
                {
                    std::array<int, 3> node = {0, node_j, node_k}; // left side
                    auto search = m_activeNodes.find(node);

                    if(search == m_activeNodes.end())
                        throw std::logic_error("expected to find particle at left side");
                    else
                        m_leftHandleIndices.push_back(search->second);
                }
                {
                    std::array<int, 3> node = {m_cellSize[0], node_j, node_k}; // left side
                    auto search = m_activeNodes.find(node);

                    if(search == m_activeNodes.end())
                        throw std::logic_error("expected to find particle at right side");
                    else
                        m_rightHandleIndices.push_back(search->second);
                }
            }
#endif
    }

    void initializeDeformation()
    {
        // No need to apply any deformation; this example is driven by moving handles
    }

    virtual void addExternalForce(std::vector<Vector3>& f) override {
        Vector3 aGravity( 0., -9.81, 0. );

        for (int p = 0; p < m_particleX.size(); p++)
            f[p] += m_particleMass[p] * aGravity;
    }

    void clearConstrainedParticles(std::vector<Vector3>& x) override
    {
        for(const auto v: m_leftHandleIndices)
            x[v] = Vector3::Zero();
        for(const auto v: m_rightHandleIndices)
            x[v] = Vector3::Zero();
    }

    void setBoundaryConditions() override
    {
        T effectiveTime = std::min<T>(m_stepEndTime, 1.0);

        for(const auto v: m_leftHandleIndices){
            m_particleX[v] = m_particleUndeformedX[v] + effectiveTime * m_leftHandleVelocity;
            m_particleV[v] = m_leftHandleVelocity;
        }
        for(const auto v: m_rightHandleIndices){
            m_particleX[v] = m_particleUndeformedX[v] + effectiveTime * m_rightHandleVelocity;
            m_particleV[v] = m_rightHandleVelocity;
        }
    }

private:
    inline int gridToParticleID(const int i, const int j) const { return i * (m_cellSize[1]+1) + j; }
};

int main(int argc, char *argv[])
{
    LatticeMesh<float> simulationMesh;
    simulationMesh.m_cellSize = { 10, 10, 10 };
    simulationMesh.m_gridDX = 0.1;
    simulationMesh.m_nFrames = 10;
    simulationMesh.m_subSteps = 1;
    simulationMesh.m_frameDt = 0.04;

    // Initialize the simulation example
    simulationMesh.initialize();
    simulationMesh.initializeDeformation();

    // Output the initial shape of the mesh
    simulationMesh.writeFrame(0);

    // Perform the animation, output results at each frame
    for(int frame = 1; frame <= simulationMesh.m_nFrames; frame++){
        simulationMesh.simulateFrame(frame);
        simulationMesh.writeFrame(frame);
    }

    // Write the entire timeline to USD
    simulationMesh.writeUSD();

    return 0;
}


#include <boost/math/special_functions/hypot.hpp>


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          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
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_GENERIC_IDIVCEIL_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_GENERIC_IDIVCEIL_HPP_INCLUDED

#include <boost/simd/arithmetic/functions/idivceil.hpp>
#include <boost/simd/include/functions/simd/iceil.hpp>
#include <boost/simd/include/functions/simd/divides.hpp>
#include <boost/simd/include/functions/simd/divceil.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( idivceil_, tag::cpu_
                                    , (A0)
                                    , (generic_< arithmetic_<A0> >)
                                      (generic_< arithmetic_<A0> >)
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return divceil(a0, a1);
    }
  };

#ifdef BOOST_MSVC
  #pragma warning(push)
  #pragma warning(disable: 4723) // potential divide by 0
#endif

  BOOST_DISPATCH_IMPLEMENT          ( idivceil_, tag::cpu_
                                    , (A0)
                                    , (generic_< floating_<A0> >)
                                      (generic_< floating_<A0> >)
                                    )
  {
    typedef typename dispatch::meta::as_integer<A0>::type result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return iceil(a0/a1);
    }
  };

#ifdef BOOST_MSVC
  #pragma warning(pop)
#endif

} } }

#endif


#ifndef MATHEVAL_IMPLEMENTATION
#error "Do not include ast_adapted.hpp directly!"
#endif

#pragma once

#include "ast.hpp"

#include <boost/fusion/include/adapt_struct.hpp>

typedef double (*unary_fn)(double);
typedef double (*binary_fn)(double, double);
typedef double (*ternary_fn)(double, double, double);

BOOST_FUSION_ADAPT_STRUCT(matheval::ast::unary_op,
                          (unary_fn, op)(matheval::ast::operand, rhs))

BOOST_FUSION_ADAPT_STRUCT(matheval::ast::binary_op,
                          (binary_fn, op)
			  (matheval::ast::operand, lhs)
			  (matheval::ast::operand, rhs))

BOOST_FUSION_ADAPT_STRUCT(matheval::ast::ternary_op,
                          (ternary_fn, op)
			  (matheval::ast::operand, p1)
			  (matheval::ast::operand, p2)
			  (matheval::ast::operand, p3) )

BOOST_FUSION_ADAPT_STRUCT(matheval::ast::operation,
                          (binary_fn, op)(matheval::ast::operand, rhs))

BOOST_FUSION_ADAPT_STRUCT(matheval::ast::expression,
                          (matheval::ast::operand, lhs)
			  (std::list<matheval::ast::operation>, rhs))


#include "tensorflow/core/framework/op_kernel.h"
#include "tensorflow/core/framework/tensor_shape.h"
#include "tensorflow/core/platform/default/logging.h"
#include "tensorflow/core/framework/shape_inference.h"

#include <Eigen/Core>

#include "celerite.h"

using namespace tensorflow;

REGISTER_OP("CeleriteMatMul")
  .Attr("T: {float, double}")
  .Input("a: T")
  .Input("u: T")
  .Input("v: T")
  .Input("p: T")
  .Input("z: T")
  .Output("y: T")
  .SetShapeFn([](shape_inference::InferenceContext* c) {

    shape_inference::DimensionHandle J;
    shape_inference::ShapeHandle u, p, a, v, z;

    TF_RETURN_IF_ERROR(c->WithRank(c->input(0), 1, &a));
    TF_RETURN_IF_ERROR(c->WithRank(c->input(1), 2, &u));
    TF_RETURN_IF_ERROR(c->WithRank(c->input(2), 2, &v));
    TF_RETURN_IF_ERROR(c->WithRank(c->input(3), 2, &p));
    TF_RETURN_IF_ERROR(c->WithRank(c->input(4), 2, &z));
    TF_RETURN_IF_ERROR(c->Merge(u, v, &u));

    c->set_output(0, c->input(4));

    return Status::OK();
  });

template <typename T>
class CeleriteMatMulOp : public OpKernel {
 public:
  explicit CeleriteMatMulOp(OpKernelConstruction* context) : OpKernel(context) {}

  void Compute(OpKernelContext* context) override {
    typedef Eigen::Map<const Eigen::Matrix<T, Eigen::Dynamic, 1>> c_vector_t;
    typedef Eigen::Map<const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> c_matrix_t;
    typedef Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> matrix_t;

    const Tensor& a_t = context->input(0);
    const Tensor& U_t = context->input(1);
    const Tensor& V_t = context->input(2);
    const Tensor& P_t = context->input(3);
    const Tensor& Z_t = context->input(4);

    OP_REQUIRES(context, (U_t.dims() == 2),
          errors::InvalidArgument("U should have the shape (N, J)"));
    int64 N = U_t.dim_size(0), J = U_t.dim_size(1);

    OP_REQUIRES(context, ((P_t.dims() == 2) && (P_t.dim_size(0) == N-1) && (P_t.dim_size(1) == J)),
        errors::InvalidArgument("P should have the shape (N-1, J)"));

    OP_REQUIRES(context, ((a_t.dims() == 1) && (a_t.dim_size(0) == N)),
        errors::InvalidArgument("a should have the shape (N)"));

    OP_REQUIRES(context, ((V_t.dims() == 2) && (V_t.dim_size(0) == N) && (V_t.dim_size(1) == J)),
        errors::InvalidArgument("V should have the shape (N, J)"));

    OP_REQUIRES(context, ((Z_t.dims() == 2) && (Z_t.dim_size(0) == N)),
        errors::InvalidArgument("Z should have the shape (N, Nrhs)"));
    int64 Nrhs = Z_t.dim_size(1);

    const auto a = c_vector_t(a_t.template flat<T>().data(), N);
    const auto U = c_matrix_t(U_t.template flat<T>().data(), N, J);
    const auto V = c_matrix_t(V_t.template flat<T>().data(), N, J);
    const auto P = c_matrix_t(P_t.template flat<T>().data(), N-1, J);
    const auto Z = c_matrix_t(Z_t.template flat<T>().data(), N, Nrhs);

    // Create the outputs
    Tensor* Y_t = NULL;
    OP_REQUIRES_OK(context, context->allocate_output(0, TensorShape({N, Nrhs}), &Y_t));

    auto Y = matrix_t(Y_t->template flat<T>().data(), N, Nrhs);
    Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> F_plus(J, Nrhs), F_minus(J, Nrhs);

    celerite::matmul(a, U, V, P, Z, Y, F_plus, F_minus);
  }
};

#define REGISTER_KERNEL(type)                                              \
  REGISTER_KERNEL_BUILDER(                                                 \
      Name("CeleriteMatMul").Device(DEVICE_CPU).TypeConstraint<type>("T"), \
      CeleriteMatMulOp<type>)

REGISTER_KERNEL(float);
REGISTER_KERNEL(double);

#undef REGISTER_KERNEL


#include <polyvec/geometry/winding_number.hpp>
#include <polyvec/image-segment/image_segment.hpp>
#include <polyvec/io/image.hpp>
#include <polyvec/mc/get_pixel_regions.hpp>
#include <polyvec/mc/prune_identical_pixel_polygons.hpp>
#include <polyvec/mc/get_bounding_box.hpp>
#include <polyvec/mc/random_color.hpp>

// eigen
#include <Eigen/Geometry>

NAMESPACE_BEGIN ( polyfit )
NAMESPACE_BEGIN ( mc )

void
post_process_pixel_regions (
    const std::vector<Eigen::Matrix2Xd>& boundaries_d,
    std::vector<Eigen::Matrix2Xi>& boundaries,
    std::vector<int>& out2in ) {

    // Convert all to int
    boundaries.resize ( boundaries_d.size() );

    for ( int i = 0; i < ( int ) boundaries_d.size(); ++i ) {
        // Make sure the double values are actually int
        Eigen::Matrix2Xd boundary_d_current = boundaries_d[i];

        auto is_really_int = [&boundary_d_current] {
            return  ( boundary_d_current.cast<int>().cast<double>() - boundary_d_current ).norm() < 1e-10 ;
        };

        if ( !is_really_int() ) {
            boundary_d_current.array() -= 0.5;
        }

        boundaries[i] = boundary_d_current.cast<int>();

        assert_break ( is_really_int() );
    }

    // Find the bbox
    Eigen::AlignedBox<int, 2> bbox = get_bounding_box( boundaries );

    // Set the minimum value to 0,0
    for ( int i = 0; i < ( int ) boundaries.size(); ++i ) {
        boundaries[i].colwise() -= bbox.min();
    }

    //
    // Orientation and duplicacy
    //

    //
    // Method 1 randomly kill one duplicate -- does not respect colors
    //  This is broken. Paths are not necessarily duplicates.
    //
    #if 0
        // Make sure that the orientation of all is CCW (when coordinate 0,0 is
        // assumed to be at lower left)
        for ( int i = 0; i < ( int ) boundaries.size(); ++i ) {
            bool is_ccw;
            // Just give it the double version
            polyvec::WindingNumber::compute_orientation ( boundaries_d[i], is_ccw );

            // Reverse if it is not ccw
            if ( !is_ccw ) {
                boundaries[i].rowwise().reverseInPlace();
            }
        }

        // Now kill the duplicate versions and set the indexing
        {
            std::vector<Eigen::Matrix2Xi> boudaries_bup = boundaries;
            prune_identical_pixel_polygons ( boudaries_bup, boundaries, out2in );
        }
    }
    #endif
    //
    // Method 2 -- prune based on orienation (ignore holes)
    // only keep ccw's
    // Assumes that Ed's code exports stuff as ccw, which seems to be the case.
    //
    {
        std::vector<Eigen::Matrix2Xi> boudaries_bup = boundaries;
        boundaries.resize ( 0 );
        out2in.resize ( 0 );

        for ( int i = 0; i < ( int ) boudaries_bup.size(); ++i ) {
            bool is_ccw;
            // Just give it the double version
            polyvec::WindingNumber::compute_orientation ( boudaries_bup[i].cast<double>(), is_ccw );

            // Reverse if it is not ccw
            if ( is_ccw ) {
                boundaries.push_back( boudaries_bup[i] );
                out2in.push_back( i );
            }
        }
    } // end of duplicate handling
}

void
get_pixel_regions_from_bmp (
    const std::string fname,
    std::vector<Eigen::Matrix2Xi>& boundaries,
    std::vector<Eigen::Vector4d>& colors ) {

    std::vector<Eigen::Matrix2Xd> boundaries_d;
    std::vector<Eigen::Vector4d> colors_d;

    // Read and process the image
    IO::Image image;
    IO::read_image ( fname.c_str(), image );
    ImageSegment::expand_and_cleanup ( image );
    ImageSegment::extract_closed_regions ( image, boundaries_d, colors_d );

    // Process regions
    std::vector<int> out2in;
    post_process_pixel_regions ( boundaries_d, boundaries, out2in );

    // set the colors
    // The duplicate boundaries might come with different colors
    // which can break this
    colors.resize ( out2in.size() );

    for ( int i = 0 ; i < ( int ) boundaries.size() ; ++i ) {
        colors[i] = colors_d[ out2in[ i ]  ] ;
    }

    // or just assign random colors for now
    // ...

}

NAMESPACE_END ( mc )
NAMESPACE_END ( polyfit )


#include "fidstr/AprilTagFiducial.h"
#include "fidstr/AprilTagGenerator.h"

#include "fidstr/PhysicalLayer.h"

#include <iostream>

#include <apriltags/TagFamily.h>
#include <apriltags/Tag16h5.h>
#include <apriltags/Tag25h7.h>
#include <apriltags/Tag25h9.h>
#include <apriltags/Tag36h9.h>
#include <apriltags/Tag36h11.h>

#include <boost/thread.hpp>
#include <boost/bind.hpp>

using namespace fidstr;

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

	AprilTagFiducialFamily family( APRILTAG_36H9 );
	std::cout << "Expecting family of size " << family.numIDs << std::endl;

	unsigned int numTags = sizeof( AprilTags::t36h9 )/sizeof( unsigned long long );
	const unsigned long long* ids = AprilTags::t36h9;

	unsigned int minComplexity = std::numeric_limits<unsigned int>::max();
	unsigned int maxComplexity = 0;

	for( unsigned int i = 0; i < numTags; i++ ) {

		unsigned long long code = ids[i];
		unsigned int complexity = AprilTagGenerator<4>::CalculateComplexity( code );
		if( complexity < minComplexity ) {
			minComplexity = complexity;
		}
		if( complexity > maxComplexity ) {
			maxComplexity = complexity;
		}
	}

	std::cout << "Min complexity: " << minComplexity << " Max complexity: " << maxComplexity << std::endl;

	AprilTagGenerator<5> gen( 9, 5, 982451653 );
	std::vector<unsigned long long> codes = gen.GenerateCodes( 4 );

	std::cout << "Found " << codes.size() << " codes." << std::endl;

	AprilTagFiducialFamily cfamily( codes, 9, 25 );

	cv::startWindowThread(); // TODO
	FiducialTransmitter transmitter( "Output", 1 );
	transmitter.SetWindowCoordinates( 0, 0 );

	for( unsigned int i = 0; i < cfamily.numIDs; i++ ) {
		Fiducial::Ptr fid = cfamily.GetFiducial( i );
		transmitter.Transmit( fid );
	}

}


/*
 *   Repast for High Performance Computing (Repast HPC)
 *
 *   Copyright (c) 2010 Argonne National Laboratory
 *   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 Argonne National Laboratory 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 TRUSTEES 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.
 *
 *
 *  DistributionFactory.cpp
 *
 *  Created on: Oct 27, 2009
 *      Author: nick
 */

#include "boost/random/mersenne_twister.hpp"
#include <boost/numeric/conversion/bounds.hpp>
#include <boost/limits.hpp>
#include <boost/mpi.hpp>

#include "initialize_random.h"
#include "Random.h"
#include "Utilities.h"

#include "RepastProcess.h" // TESTING ONLY

#include <vector>

using namespace std;

namespace repast {

const size_t DIST_TAG_LENGTH = 13;

const string DBL_UNI_DIST = "double_uniform";
const string INT_UNI_DIST = "int_uniform";
const string TRIANGLE_DIST = "triangle";
const string CAUCHY_DIST = "cauchy";
const string EXPONENTIAL_DIST = "exponential";
const string NORMAL_DIST = "normal";
const string LOG_NORMAL_DIST = "lognormal";

void initializeSeed(const Properties& props, boost::mpi::communicator* comm);

void createDblUni(string& name, vector<string>& params) {
	if (params.size() != 3)
		throw Repast_Error_19(name, params); // Wrong number of parameters
	double from = strToDouble(trim(params[1]));
	double to = strToDouble(trim(params[2]));
	Random* random = Random::instance();
	_RealUniformGenerator gen(random->engine(), boost::uniform_real<>(from, to));
	NumberGenerator* ng = new DefaultNumberGenerator<_RealUniformGenerator> (gen);
	random->putGenerator(name, ng);
}

void createIntUni(string& name, vector<string>& params) {
	if (params.size() != 3)
	  throw Repast_Error_20(name, params); // Wrong number of parameters
	int from = strToInt(trim(params[1]));
	int to = strToInt(trim(params[2]));

	Random* random = Random::instance();
	_IntUniformGenerator gen(random->engine(), boost::uniform_int<>(from, to));
	NumberGenerator* ng = new DefaultNumberGenerator<_IntUniformGenerator> (gen);
	random->putGenerator(name, ng);
}

void createTriangle(string& name, vector<string>& params) {
	if (params.size() != 4)
	  throw Repast_Error_21(name, params); // Wrong number of parameters
	double lower = strToDouble(trim(params[1]));
	double mostLikely = strToDouble(trim(params[2]));
	double upper = strToDouble(trim(params[3]));

	Random* random = Random::instance();
	boost::triangle_distribution<> dist(lower, mostLikely, upper);
	_TriangleGenerator gen(random->engine(), dist);
	NumberGenerator* ng = new DefaultNumberGenerator<_TriangleGenerator> (gen);
	random->putGenerator(name, ng);
}

void createCauchy(string& name, vector<string>& params) {
	if (params.size() != 3)
	  throw Repast_Error_22(name, params); // Wrong number of parameters
	double median = strToDouble(trim(params[1]));
	double sigma = strToDouble(trim(params[2]));

	Random* random = Random::instance();
	boost::cauchy_distribution<> dist(median, sigma);
	_CauchyGenerator gen(random->engine(), dist);
	NumberGenerator* ng = new DefaultNumberGenerator<_CauchyGenerator> (gen);
	random->putGenerator(name, ng);
}

void createExponential(string& name, vector<string>& params) {
	if (params.size() != 2)
	  throw Repast_Error_23(name, params); // Wrong number of parameters
	double lambda = strToDouble(trim(params[1]));

	Random* random = Random::instance();
	boost::exponential_distribution<> dist(lambda);
	_ExponentialGenerator gen(random->engine(), dist);
	NumberGenerator* ng = new DefaultNumberGenerator<_ExponentialGenerator> (gen);
	random->putGenerator(name, ng);
}

void createNormal(string& name, vector<string>& params) {
	if (params.size() != 3)
	  throw Repast_Error_24(name, params); // Wrong number of parameters
	double mean = strToDouble(trim(params[1]));
	double sigma = strToDouble(trim(params[2]));

	Random* random = Random::instance();
	boost::normal_distribution<> dist(mean, sigma);
	_NormalGenerator gen(random->engine(), dist);
	NumberGenerator* ng = new DefaultNumberGenerator<_NormalGenerator> (gen);
	random->putGenerator(name, ng);
}

void createLogNormal(string& name, vector<string>& params) {
	if (params.size() != 3)
	  throw Repast_Error_25(name, params); // Wrong number of parameters
	double mean = strToDouble(trim(params[1]));
	double sigma = strToDouble(trim(params[2]));

	Random* random = Random::instance();
	boost::lognormal_distribution<> dist(mean, sigma);
	_LogNormalGenerator gen(random->engine(), dist);
	NumberGenerator* ng = new DefaultNumberGenerator<_LogNormalGenerator> (gen);
	random->putGenerator(name, ng);
}

void initializeRandom(Properties& props, boost::mpi::communicator* comm) {
	initializeSeed(props, comm);
	for (Properties::key_iterator iter = props.keys_begin(); iter != props.keys_end(); iter++) {
		string key = *iter;
		// starts with distribution tag
		if (key.find("distribution.", 0) == 0 && key.size() > DIST_TAG_LENGTH) {
			string name = key.substr(13);
			vector<string> params;
			tokenize(props.getProperty(key), params, ",");
			if (params.size() < 1)
			  throw Repast_Error_26(name, params); // Wrong number of parameters
			const string type = params[0];
			if (type == DBL_UNI_DIST)
				createDblUni(name, params);
			else if (type == INT_UNI_DIST)
				createIntUni(name, params);
			else if (type == TRIANGLE_DIST)
				createTriangle(name, params);
			else if (type == CAUCHY_DIST)
				createCauchy(name, params);
			else if (type == EXPONENTIAL_DIST)
				createExponential(name, params);
			else if (type == NORMAL_DIST)
				createNormal(name, params);
			else if (type == LOG_NORMAL_DIST)
				createLogNormal(name, params);
			else
			  throw Repast_Error_27(name, params); // Unrecognized type name
		}
	}
}

void initializeSeed(Properties& props, boost::mpi::communicator* comm) {
  // Default value for seed is local proc's system time
  boost::uint32_t seed = (boost::uint32_t)time(0);

  if(props.contains(GLOBAL_RANDOM_SEED_PROPERTY)){
    std::string propVal = props.getProperty(GLOBAL_RANDOM_SEED_PROPERTY);
    if(propVal.compare("AUTO") == 0){
      if(comm == 0)  throw Repast_Error_57(); // Needs communicator to share global seed
      boost::mpi::broadcast(*comm, seed, 0);
      props.putProperty(GLOBAL_RANDOM_SEED_PROPERTY, seed);
    }
    else{
      seed = strToUInt(propVal);
    }
   }
   else{
    // If no global seed, then each proc will use its own seed
    if(props.contains(RANDOM_SEED_PROPERTY)){
      std::string propVal = props.getProperty(RANDOM_SEED_PROPERTY);
      if(propVal.compare("AUTO") == 0){
        if(comm != 0) boost::mpi::broadcast(*comm, seed, 0);
        props.putProperty(RANDOM_SEED_PROPERTY, seed);
      }
      else{
        seed = (boost::uint32_t)strToUInt(propVal);
      }
    }
    if(comm != 0){
      boost::mt19937 gen;
      boost::uniform_real<> dist(0, boost::numeric::bounds<boost::uint32_t>::highest());
      gen.seed(seed);
      boost::variate_generator<boost::mt19937&, boost::uniform_real<> > localRNG(gen, dist);
      for(int i = 0; i < comm->rank(); i++)  seed = localRNG(); // The assignment only matters on the last time through, but calling the generator is requisite.
    }
    stringstream ss;
    ss << std::fixed << seed;
    props.putProperty(RANDOM_SEED_PROPERTY, ss.str());
  }
  Random::initialize(seed);
}

}


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// 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)
//=======================================================================
//
//  Sample output:
//
//   0  --(8, 10)--> 1
//
//   1  --(12, 20)--> 4 --(12, 40)--> 3
//        <--(8,10)-- 0 <--(16,20)-- 2
//   2  --(16, 20)--> 1
//        <--(16,20)-- 5
//   3  --(12, 40)--> 6
//        <--(12,40)-- 1
//   4  --(12, 20)--> 7
//        <--(12,20)-- 1
//   5  --(16, 20)--> 2
//        <--(16,20)-- 6
//   6  --(16, 20)--> 5 --(8, 10)--> 8
//        <--(12,20)-- 7        <--(12,40)-- 3
//   7  --(12, 20)--> 6
//        <--(12,20)-- 4
//   8
//        <--(8,10)-- 6
//
//
//   0  --(8, 1)--> 1
//
//   1  --(12, 2)--> 4  --(12, 3)--> 3
//        <--(8,1)-- 0  <--(16,4)-- 2
//   2  --(16, 4)--> 1
//        <--(16,7)-- 5
//   3  --(12, 5)--> 6
//        <--(12,3)-- 1
//   4  --(12, 6)--> 7
//        <--(12,2)-- 1
//   5  --(16, 7)--> 2
//        <--(16,8)-- 6
//   6  --(16, 8)--> 5
//        <--(12,10)-- 7        <--(12,5)-- 3
//   7  --(12, 10)--> 6
//        <--(12,6)-- 4
//   8
//

#include <boost/config.hpp>
#include <iostream>

#include <boost/utility.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/adjacency_list.hpp>

using namespace boost;
using namespace std;

enum edge_myflow_t { edge_myflow };
enum edge_mycapacity_t { edge_mycapacity };

namespace boost {
  BOOST_INSTALL_PROPERTY(edge, myflow);
  BOOST_INSTALL_PROPERTY(edge, mycapacity);
}

template <class Graph>
void print_network(const Graph& G)
{
  typedef typename boost::graph_traits<Graph>::vertex_iterator    Viter;
  typedef typename boost::graph_traits<Graph>::out_edge_iterator OutEdgeIter;
  typedef typename boost::graph_traits<Graph>::in_edge_iterator InEdgeIter;

  typename property_map<Graph, edge_mycapacity_t>::const_type
    capacity = get(edge_mycapacity, G);
  typename property_map<Graph, edge_myflow_t>::const_type
    flow = get(edge_myflow, G);

  Viter ui, uiend;
  boost::tie(ui, uiend) = vertices(G);

  for (; ui != uiend; ++ui) {
    OutEdgeIter out, out_end;
    cout << *ui << "\t";

    boost::tie(out, out_end) = out_edges(*ui, G);
    for(; out != out_end; ++out)
      cout << "--(" << capacity[*out] << ", " << flow[*out] << ")--> "
           << target(*out,G) << "\t";

    InEdgeIter in, in_end;
    cout << endl << "\t";
    boost::tie(in, in_end) = in_edges(*ui, G);
    for(; in != in_end; ++in)
      cout << "<--(" << capacity[*in] << "," << flow[*in] << ")-- "
           << source(*in,G) << "\t";

    cout << endl;
  }
}

int main(int , char* [])
{
  typedef property<edge_mycapacity_t, int> Cap;
  typedef property<edge_myflow_t, int, Cap> Flow;
  typedef adjacency_list<vecS, vecS, bidirectionalS,
     no_property, Flow> Graph;

  const int num_vertices = 9;
  Graph G(num_vertices);

  /*          2<----5
             /       ^
            /         \
           V           \
    0 ---->1---->3----->6--->8
           \           ^
            \         /
             V       /
             4----->7
   */

  add_edge(0, 1, Flow(10, Cap(8)), G);

  add_edge(1, 4, Flow(20, Cap(12)), G);
  add_edge(4, 7, Flow(20, Cap(12)), G);
  add_edge(7, 6, Flow(20, Cap(12)), G);

  add_edge(1, 3, Flow(40, Cap(12)), G);
  add_edge(3, 6, Flow(40, Cap(12)), G);

  add_edge(6, 5, Flow(20, Cap(16)), G);
  add_edge(5, 2, Flow(20, Cap(16)), G);
  add_edge(2, 1, Flow(20, Cap(16)), G);

  add_edge(6, 8, Flow(10, Cap(8)), G);

  typedef boost::graph_traits<Graph>::edge_descriptor Edge;

  print_network(G);

  property_map<Graph, edge_myflow_t>::type
    flow = get(edge_myflow, G);

  boost::graph_traits<Graph>::vertex_iterator v, v_end;
  boost::graph_traits<Graph>::out_edge_iterator e, e_end;
  int f = 0;
  for (boost::tie(v, v_end) = vertices(G); v != v_end; ++v)
    for (boost::tie(e, e_end) = out_edges(*v, G); e != e_end; ++e)
      flow[*e] = ++f;
  cout << endl << endl;

  remove_edge(6, 8, G);

  print_network(G);

  return 0;
}


#include "SimulationsOMP_pcp.h"

#include <Eigen/Eigen>

#include "RigidObjectMotion3D.h"

RigidObjectMotion3D::RigidObjectMotion3D() :
	ax(0.0), ay(0.0), az(0.0), ax_ang(0.0), ay_ang(0.0), az_ang(0.0),
	vx(0.0), vy(0.0), vz(0.0), vx_ang(0.0), vy_ang(0.0), vz_ang(0.0),
	ux(0.0), uy(0.0), uz(0.0), x_ang(0.0), y_ang(0.0), z_ang(0.0),
	ix(1.0, 0.0, 0.0), iy(0.0, 1.0, 0.0), iz(0.0, 0.0, 1.0),
	ax_bc_mask(0), ay_bc_mask(0), az_bc_mask(0),
	ax_bc(0.0), ay_bc(0.0), az_bc(0.0),
	ax_ang_bc_mask(0), ay_ang_bc_mask(0), az_ang_bc_mask(0),
	ax_ang_bc(0.0), ay_ang_bc(0.0), az_ang_bc(0.0),
	vx_bc_mask(0), vy_bc_mask(0), vz_bc_mask(0),
	vx_bc(0.0), vy_bc(0.0), vz_bc(0.0),
	vx_ang_bc_mask(0), vy_ang_bc_mask(0), vz_ang_bc_mask(0),
	vx_ang_bc(0.0), vy_ang_bc(0.0), vz_ang_bc(0.0),
	fx_ext(0.0), fy_ext(0.0), fz_ext(0.0),
	mx_ext(0.0), my_ext(0.0), mz_ext(0.0),
	fx_cont(0.0), fy_cont(0.0), fz_cont(0.0),
	mx_cont(0.0), my_cont(0.0), mz_cont(0.0) {}

RigidObjectMotion3D::~RigidObjectMotion3D() {}

void RigidObjectMotion3D::init(
	double _x,
	double _y,
	double _z,
	double _m,
	double _moi_data[6]
	)
{
	x_ori = _x;	y_ori = _y;	z_ori = _z;
	x = _x; y = _y;	z = _z;
	m = _m;
	moi[0] = _moi_data[0];	moi[1] = _moi_data[1];
	moi[2] = _moi_data[2];	moi[3] = _moi_data[3];
	moi[4] = _moi_data[4];	moi[5] = _moi_data[5];

	Eigen::Matrix3d moi_mat;
	moi_mat << moi[0], moi[3], moi[5],
			   moi[3], moi[1], moi[4],
			   moi[5], moi[4], moi[2];

	Eigen::Matrix3d inv_moi_mat = moi_mat.inverse();
	inv_moi[0] = inv_moi_mat(0, 0);
	inv_moi[1] = inv_moi_mat(1, 1);
	inv_moi[2] = inv_moi_mat(2, 2);
	inv_moi[3] = inv_moi_mat(0, 1);
	inv_moi[4] = inv_moi_mat(1, 2);
	inv_moi[5] = inv_moi_mat(2, 0);
}

void RigidObjectMotion3D::set_angle(
	// in radius
	double _x_ang,
	double _y_ang,
	double _z_ang)
{
	x_ang = _x_ang;
	trim_to_pi(x_ang);
	y_ang = _y_ang;
	trim_to_pi(y_ang);
	z_ang = _z_ang;
	trim_to_pi(z_ang);
	ix.x = 1.0, ix.y = 0.0, ix.z = 0.0;
	iy.x = 0.0, iy.y = 1.0, iy.z = 0.0;
	iz.x = 0.0, iz.y = 0.0, iz.z = 1.0;
	rotate_axses_by_angle(pos_ang, ix, iy, iz);
}

void RigidObjectMotion3D::set_translation_velocity_bc(
	double _vx,
	double _vy,
	double _vz)
{
	set_vx_bc(_vx); set_vy_bc(_vy);	set_vz_bc(_vz);
	set_vx_ang_bc(0.0);	set_vy_ang_bc(0.0);	set_vz_ang_bc(0.0);
}


//=======================================================================
// Copyright (c) 2013 Maciej Andrejczuk
//               2014 Piotr Godlewski, Piotr Wygocki
//
// 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)
//=======================================================================
/**
 * @file steiner_tree.hpp
 * @brief
 * @author Maciej Andrejczuk, Piotr Godlewski, Piotr Wygocki
 * @version 1.0
 * @date 2013-08-01
 */
#ifndef PAAL_STEINER_TREE_HPP
#define PAAL_STEINER_TREE_HPP

#define BOOST_RESULT_OF_USE_DECLTYPE

#include "paal/data_structures/metric/basic_metrics.hpp"
#include "paal/iterative_rounding/ir_components.hpp"
#include "paal/iterative_rounding/iterative_rounding.hpp"
#include "paal/iterative_rounding/steiner_tree/steiner_components.hpp"
#include "paal/iterative_rounding/steiner_tree/steiner_strategy.hpp"
#include "paal/iterative_rounding/steiner_tree/steiner_tree_oracle.hpp"
#include "paal/iterative_rounding/steiner_tree/steiner_utils.hpp"
#include "paal/lp/lp_row_generation.hpp"
#include "paal/utils/assign_updates.hpp"

#include <boost/random/discrete_distribution.hpp>
#include <boost/range/join.hpp>
#include <boost/range/algorithm/unique.hpp>
#include <boost/range/algorithm/sort.hpp>
#include <boost/range/algorithm/copy.hpp>
#include <boost/range/algorithm/find.hpp>

#include <random>
#include <vector>

namespace paal {
namespace ir {

namespace {
struct steiner_tree_compare_traits {
    static const double EPSILON;
};

const double steiner_tree_compare_traits::EPSILON = 1e-10;
}

/**
 * @class steiner_tree
 * @brief The class for solving the Steiner Tree problem using Iterative Rounding.
 *
 * @tparam OrigMetric
 * @tparam Terminals
 * @tparam Result
 * @tparam Strategy
 * @tparam Oracle separation oracle
 */
template<typename OrigMetric, typename Terminals, typename Result,
    typename Strategy = steiner_tree_all_generator,
    typename Oracle = lp::random_violated_separation_oracle>
class steiner_tree {
public:
    using MT = data_structures::metric_traits<OrigMetric>;
    using Vertex = typename MT::VertexType;
    using Vertices = std::vector<Vertex>;
    using Dist = typename MT::DistanceType;
    using Edge = typename std::pair<Vertex, Vertex>;
    using Compare = utils::compare<double>;
    using VertexIndex = data_structures::bimap<Vertex>;
    using MetricIdx = data_structures::array_metric<Dist>;
    using Metric = data_structures::metric_on_idx<
                        MetricIdx &,
                        const VertexIndex &,
                        data_structures::read_values_tag>;

private:
    Terminals m_terminals;        // terminals in current state
    Terminals m_steiner_vertices; // vertices that are not terminals
    VertexIndex m_terminals_index; // mapping terminals to numbers for 0 to n.
    VertexIndex m_vertex_index; // mapping vertices to numbers for 0 to n.
    MetricIdx m_cost_map_idx;            // metric in current state (operates on indexes)
    Metric m_cost_map;            // metric in current state
    steiner_components<Vertex, Dist> m_components; // components in current
                                                   // state
    Strategy m_strategy;      // strategy to generate the components
    Result m_result_iterator; // list of selected Steiner Vertices
    Vertices m_selected_elements; // list of selected Steiner Vertices
    Compare m_compare;        // comparison method

    std::unordered_map<int, lp::col_id> m_elements_map; // maps component_id ->
                                                        // col_id in LP
    steiner_tree_violation_checker m_violation_checker;

    Oracle m_oracle;

public:
    /**
     * Constructor.
     */
    steiner_tree(const OrigMetric& metric, const Terminals& terminals,
            const Terminals& steiner_vertices, Result result,
            const Strategy& strategy = Strategy{}, Oracle oracle = Oracle{}) :
        m_terminals(terminals), m_steiner_vertices(steiner_vertices),
        m_terminals_index(m_terminals),
        m_vertex_index(boost::range::join(m_terminals, m_steiner_vertices)),
        m_cost_map_idx(metric, boost::range::join(m_terminals, m_steiner_vertices)),
        m_cost_map(m_cost_map_idx, m_vertex_index),
        m_strategy(strategy), m_result_iterator(result),
        m_compare(steiner_tree_compare_traits::EPSILON), m_oracle(oracle) {
    }

    /**
     * @brief
     *
     * @tparam LP
     * @param lp
     *
     * @return
     */
    template <typename LP>
    auto get_find_violation(LP & lp) {
        using candidate = steiner_tree_violation_checker::Candidate;
        return m_oracle([&](){return m_violation_checker.get_violation_candidates(*this, lp);},
                        [&](candidate c){return m_violation_checker.check_violation(c, *this);},
                        [&](candidate c){return m_violation_checker.add_violated_constraint(c, *this, lp);});
    }

    /**
     * Generates all the components using specified strategy.
     */
    void gen_components() {
        m_strategy.gen_components(m_cost_map, m_terminals, m_steiner_vertices,
                                  m_components);
    }

    /**
     * Gets reference to all the components.
     */
    const steiner_components<Vertex, Dist> &get_components() const {
        return m_components;
    }

    /**
     * Gets reference to all the terminals.
     */
    const Terminals &get_terminals() const { return m_terminals; }

    /**
     * Returns the index of a terminal.
     */
    auto get_terminal_idx(Vertex v) const -> decltype(m_terminals_index.get_idx(v)) {
        return m_terminals_index.get_idx(v);
    }

    /**
     * Adds map entry from component id to LP lp::col_id.
     */
    void add_column_lp(int id, lp::col_id col) {
        bool b = m_elements_map.insert(std::make_pair(id, col)).second;
        assert(b);
    }

    /**
     * Finds LP lp::col_id based on component id.
     */
    lp::col_id find_column_lp(int id) const { return m_elements_map.at(id); }

    /**
     * Adds elements to solution.
     */
    void add_to_solution(const std::vector<Vertex>& steiner_elements) {
        boost::copy(steiner_elements, std::back_inserter(m_selected_elements));
    }

    /**
     * Removes duplicates and sets the final solution.
     */
    void set_solution() {
        boost::sort(m_selected_elements);
        boost::copy(boost::unique(m_selected_elements), m_result_iterator);
    }

    /**
     * Merges a component into its sink.
     */
    void update_graph(const steiner_component<Vertex, Dist>& selected) {
        auto const & all_terminals = selected.get_terminals();
        auto all_terminals_except_first = boost::make_iterator_range(++all_terminals.begin(), all_terminals.end());
        assert(!boost::empty(all_terminals));
        auto const & sink = all_terminals.front();
        for (auto t : all_terminals_except_first) {
            merge_vertices(sink, t);
            auto ii = boost::range::find(m_terminals, t);
            assert(ii != m_terminals.end());
            m_terminals.erase(ii);
        }
        // Clean components, they will be generated once again
        m_components.clear();
        m_elements_map.clear();
        m_terminals_index = VertexIndex(m_terminals);
    }

    /**
     * Gets comparison method.
     */
    utils::compare<double> get_compare() const {
        return m_compare;
    }

private:
    /**
     * Returns the index of a vertex.
     */
    auto get_idx(Vertex v) const -> decltype(m_vertex_index.get_idx(v)) {
        return m_vertex_index.get_idx(v);
    }

    /**
     * Recalculates distances after two vertices were merged.
     */
    void merge_vertices(Vertex u_vertex, Vertex w_vertex) {
        auto all_elements = boost::range::join(m_terminals, m_steiner_vertices);
        auto u = get_idx(u_vertex);
        auto w = get_idx(w_vertex);
        for (auto i_vertex: all_elements) {
            for (auto j_vertex: all_elements) {
                auto i = get_idx(i_vertex);
                auto j = get_idx(j_vertex);
                assign_min(m_cost_map_idx(i, j),
                           m_cost_map_idx(i, u) + m_cost_map_idx(w, j));
            }
        }
    }

};

/**
 * Initialization of the IR Steiner Tree algorithm.
 */
struct steiner_tree_init {
    /**
     * Initializes LP.
     */
    template <typename Problem, typename LP>
    void operator()(Problem &problem, LP &lp) {
        lp.clear();
        lp.set_lp_name("steiner tree");
        problem.gen_components();
        lp.set_optimization_type(lp::MINIMIZE);
        add_variables(problem, lp);
    }

  private:
    /**
     * Adds all the components as columns of LP.
     */
    template <typename Problem, typename LP>
    void add_variables(Problem &problem, LP &lp) {
        for (int i = 0; i < problem.get_components().size(); ++i) {
            lp::col_id col = lp.add_column(
                problem.get_components().find(i).get_cost(), 0, 1);
            problem.add_column_lp(i, col);
        }
    }
};

/**
 * Round Condition: step of iterative-randomized rounding algorithm.
 */
class steiner_tree_round_condition {
    std::default_random_engine m_rng;
  public:
    steiner_tree_round_condition(std::default_random_engine = std::default_random_engine{}) {}

    /**
     * Selects one component according to probability, adds it to solution and
     * merges selected vertices.
     */
    template <typename Problem, typename LP>
    void operator()(Problem &problem, LP &lp) {
        auto size = problem.get_components().size();
        std::vector<double> weights;
        weights.reserve(size);
        for (auto i : paal::irange(size)) {
            lp::col_id cId = problem.find_column_lp(i);
            weights.push_back(lp.get_col_value(cId));
        }

        auto selected = boost::random::discrete_distribution<>(weights)(m_rng);
        auto const &comp = problem.get_components().find(selected);
        problem.add_to_solution(comp.get_steiner_elements());
        problem.update_graph(comp);
        steiner_tree_init()(problem, lp);
    }
};

/**
 * Stop Condition: step of iterative-randomized rounding algorithm.
 */
struct steiner_tree_stop_condition {
    ///Checks if the IR algorithm should terminate.
    template<typename Problem, typename LP>
    bool operator()(Problem& problem, LP &) {
        return problem.get_terminals().size() < 2;
    }
};

/**
 * Set Solution component.
 */
struct steiner_tree_set_solution {
    /**
     * Removes duplicates from selected Steiner vertices list.
     */
    template <typename Problem, typename GetSolution>
    void operator()(Problem & problem, const GetSolution &) {
        problem.set_solution();
    }
};

/**
 * Makes steiner_tree object. Just to avoid providing type names in template.
 */
template<typename Oracle = lp::random_violated_separation_oracle,
        typename OrigMetric, typename Terminals, typename Result, typename Strategy>
steiner_tree<OrigMetric, Terminals, Result, Strategy, Oracle> make_steiner_tree(
        const OrigMetric& metric, const Terminals& terminals,
        const Terminals& steiner_vertices, Result result, const Strategy& strategy,
        Oracle oracle = Oracle()) {
    return steiner_tree<OrigMetric, Terminals, Result, Strategy, Oracle>(metric,
            terminals, steiner_vertices, result, strategy, oracle);
}

template <typename Init = steiner_tree_init,
          typename RoundCondition = steiner_tree_round_condition,
          typename RelaxCondition = utils::always_false,
          typename SetSolution = steiner_tree_set_solution,
          typename SolveLPToExtremePoint = row_generation_solve_lp<>,
          typename ResolveLPToExtremePoint = row_generation_solve_lp<>,
          typename StopCondition = steiner_tree_stop_condition>
using steiner_tree_ir_components =
    IRcomponents<Init, RoundCondition, RelaxCondition, SetSolution,
                 SolveLPToExtremePoint, ResolveLPToExtremePoint, StopCondition>;

/**
 * @brief Solves the Steiner Tree problem using Iterative Rounding.
 */
template <typename Oracle = lp::random_violated_separation_oracle,
          typename Strategy = steiner_tree_all_generator,
          typename OrigMetric,
          typename Terminals,
          typename Result,
          typename IRcomponents = steiner_tree_ir_components<>,
          typename Visitor = trivial_visitor>
lp::problem_type steiner_tree_iterative_rounding(const OrigMetric& metric, const Terminals& terminals,
        const Terminals& steiner_vertices, Result result, Strategy strategy = Strategy{},
        IRcomponents comps = IRcomponents{}, Oracle oracle = Oracle{},
        Visitor visitor = Visitor{}) {

    auto steiner = paal::ir::make_steiner_tree(metric, terminals, steiner_vertices, result, strategy, oracle);
    auto res = paal::ir::solve_dependent_iterative_rounding(steiner, std::move(comps), std::move(visitor));
    return res.first;
}

} //! ir
} //! paal
#endif // PAAL_STEINER_TREE_HPP


// faster alternative to PPTrajectory.eval() in Matlab
// Michael Kaess, June 2013

#include <mex.h>
#include <Eigen/Dense>
#include <unsupported/Eigen/MatrixFunctions>
#include <vector>
#include <iostream>

using namespace Eigen;
using namespace std;

// Initialization:
//   obj = PPTmex(breaks,coefs,order,dims)
// Evaluation:
//   y = PPTmex(obj, 1, t)
//
// t:      1      single evaluation point
//
// breaks: n+1
// coefs:  nxd1xd2 x p
// order:  1       p
// dims:   2       d1,d2
//
// y:      p

class PPTrajectory {

private:

  VectorXd m_breaks;
  MatrixXd m_coefs;
  const int m_d1, m_d2, m_order;

  int m_cached_idx;

public:

  PPTrajectory(const VectorXd& breaks, const MatrixXd& coefs, int order, int d1, int d2)
  : m_breaks(breaks), m_coefs(coefs), m_order(order), m_d1(d1), m_d2(d2), m_cached_idx(-1)
  {}

  int d1() {
    return m_d1;
  }

  int d2() {
    return m_d2;
  }

  void eval(double t, Map<MatrixXd>& y) {

    if (t<m_breaks(0)) t=m_breaks(0);
    if (t>m_breaks(m_breaks.rows()-1)) t=m_breaks(m_breaks.rows()-1);

    Map<VectorXd> r(y.data(), m_d1*m_d2); // reshape

    // use cached index if possible
    int idx;
    if (m_cached_idx >= 0 && m_cached_idx < m_breaks.rows() // valid m_cached_idx?
        && t < m_breaks[m_cached_idx+1] // still in same interval?
        && ((m_cached_idx == 0) || (t >= m_breaks[m_cached_idx]))) { // left up to -infinity
      idx = m_cached_idx;
    } else {
      // search for the correct interval
      idx = static_cast<int>(m_breaks.rows())-2;
      // todo: binary search
      for (int b=static_cast<int>(m_breaks.rows())-2; b>=1; b--) { // ignore first and last break
        if (t < m_breaks(b)) idx=b-1;
      }
      m_cached_idx = idx;
    }

    int base = idx*m_d1*m_d2;
    double local = t - m_breaks[idx];
    r = m_coefs.block(base, 0, m_d1*m_d2, 1);
    for (int i=2; i<=m_order; i++) {
      r = local*r + m_coefs.block(base, i-1, m_d1*m_d2, 1);
    }
  }
};

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  if (nrhs == 4 && nlhs == 1) {

    // create object
    Map<VectorXd> breaks(mxGetPr(prhs[0]), mxGetNumberOfElements(prhs[0]));
    Map<MatrixXd>  coefs(mxGetPr(prhs[1]), mxGetM(prhs[1]), mxGetN(prhs[1]));
    int order = static_cast<int>(mxGetScalar(prhs[2]));
    Map<VectorXd>   dims(mxGetPr(prhs[3]), mxGetNumberOfElements(prhs[3]));

    int d1 = static_cast<int>(dims(0));
    int d2 = 1;
    if (dims.rows() > 1)
      d2 = static_cast<int>(dims(1));
    PPTrajectory *ppt = new PPTrajectory(breaks, coefs, order, d1, d2);
    mxClassID cid;
    if (sizeof(ppt)==4) cid = mxUINT32_CLASS;
    else if (sizeof(ppt)==8) cid = mxUINT64_CLASS;
    else mexErrMsgIdAndTxt("Drake:PPTmex:PointerSize","Are you on a 32-bit machine or 64-bit machine??");
    plhs[0] = mxCreateNumericMatrix(1,1,cid,mxREAL);
    memcpy(mxGetData(plhs[0]),&ppt,sizeof(ppt));

  } else {

    // retrieve object
    PPTrajectory *ppt = NULL;
    if (nrhs==0 || !mxIsNumeric(prhs[0]) || mxGetNumberOfElements(prhs[0])!=1)
      mexErrMsgIdAndTxt("Drake:PPTmex:BadInputs","the first argument should be the mex_ptr");
    memcpy(&ppt,mxGetData(prhs[0]),sizeof(ppt));

    if (nrhs == 1) {

      // delete object
      delete ppt;

    } else {

      // eval() function call
      if (nrhs < 2) {
        mexErrMsgIdAndTxt("Drake:PPTmex:WrongNumberOfInputs","Usage obj = PPTmex(breaks,coefs,order,dims) or y = PPTmex(obj,t)");
      }

      double *command = mxGetPr(prhs[1]);

      switch ((int)(*command)) {

      case 1: { // eval
        double *t = mxGetPr(prhs[2]);
        int d1 = ppt->d1();
        int d2 = ppt->d2();
        plhs[0] = mxCreateDoubleMatrix(d1,d2,mxREAL);
        Map<MatrixXd> y(mxGetPr(plhs[0]),d1,d2);
        ppt->eval(*t, y);
        break;
      }

      default:
        mexErrMsgIdAndTxt("Drake:PPTmex:UnknownCommand","Check arguments");
      }

    }

  }

}


//because pagmo2 simulated_annealing implementation only works with continuous variables, we need a simler one
#pragma once

#include <pagmo/pagmo.hpp>
#include <pagmo/population.hpp>
#include <pagmo/algorithms/not_population_based.hpp>
#include <Eigen/Dense>
using namespace pagmo;
class PAGMO_DLL_PUBLIC HypercubeSimulatedAnnealing : public not_population_based {
public:
    HypercubeSimulatedAnnealing(unsigned iters=1000, const Eigen::MatrixXd *overlaps=nullptr, double minOverlap=0.5, double Ts = 10., double Tf = .1, unsigned n_T_adj = 10u, unsigned seed = pagmo::random_device::next());

    population evolve(population pop) const;

    /*void set_seed(unsigned seed) {
        m_seed = seed;
        m_e.
    }*/

    std::string get_name() const;

private:
    unsigned m_iters;
    // Starting temperature
    double m_Ts;
    // Final temperature
    double m_Tf;
    // Number of temperature adjustments during the annealing procedure
    unsigned m_n_T_adj;

    mutable pagmo::detail::random_engine_type m_e;
    unsigned m_seed;
    const Eigen::MatrixXd *m_overlaps;
    double m_minOverlap;
};


//  (C) Copyright David Abrahams 2001.
// 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 for most recent version including documentation.

//  Revision History
//  1  Apr 2001 Fixes for ICL; use BOOST_STATIC_CONSTANT
//  11 Feb 2001 Fixes for Borland (David Abrahams)
//  23 Jan 2001 Added test for wchar_t (David Abrahams)
//  23 Jan 2001 Now statically selecting a test for signed numbers to avoid
//              warnings with fancy compilers. Added commentary and
//              additional dumping of traits data for tested types (David
//              Abrahams).
//  21 Jan 2001 Initial version (David Abrahams)

#include <boost/detail/numeric_traits.hpp>
#include <cassert>
#include <boost/type_traits.hpp>
#include <boost/static_assert.hpp>
#include <boost/cstdint.hpp>
#include <boost/utility.hpp>
#include <boost/lexical_cast.hpp>
#include <climits>
#include <typeinfo>
#include <iostream>
#include <string>
#ifndef BOOST_NO_LIMITS
# include <limits>
#endif

// =================================================================================
// template class complement_traits<Number> --
//
//    statically computes the max and min for 1s and 2s-complement binary
//    numbers. This helps on platforms without <limits> support. It also shows
//    an example of a recursive template that works with MSVC!
//

template <unsigned size> struct complement; // forward

// The template complement, below, does all the real work, using "poor man's
// partial specialization". We need complement_traits_aux<> so that MSVC doesn't
// complain about undefined min/max as we're trying to recursively define them.
template <class Number, unsigned size>
struct complement_traits_aux
{
    BOOST_STATIC_CONSTANT(Number, max = complement<size>::template traits<Number>::max);
    BOOST_STATIC_CONSTANT(Number, min = complement<size>::template traits<Number>::min);
};

template <unsigned size>
struct complement
{
    template <class Number>
    struct traits
    {
     private:
        // indirection through complement_traits_aux necessary to keep MSVC happy
        typedef complement_traits_aux<Number, size - 1> prev;
     public:
#if defined(__GNUC__) && __GNUC__ == 4 && __GNUC_MINOR__ == 0 && __GNUC_PATCHLEVEL__ == 2
      // GCC 4.0.2 ICEs on these C-style casts
        BOOST_STATIC_CONSTANT(Number, max =
                            Number((prev::max) << CHAR_BIT)
                            + Number(UCHAR_MAX));
        BOOST_STATIC_CONSTANT(Number, min = Number((prev::min) << CHAR_BIT));
#else
        BOOST_STATIC_CONSTANT(Number, max =
                            Number(Number(prev::max) << CHAR_BIT)
                            + Number(UCHAR_MAX));
        BOOST_STATIC_CONSTANT(Number, min = Number(Number(prev::min) << CHAR_BIT));
#endif

    };
};

// Template class complement_base<> -- defines values for min and max for
// complement<1>, at the deepest level of recursion. Uses "poor man's partial
// specialization" again.
template <bool is_signed> struct complement_base;

template <> struct complement_base<false>
{
    template <class Number>
    struct values
    {
        BOOST_STATIC_CONSTANT(Number, min = 0);
        BOOST_STATIC_CONSTANT(Number, max = UCHAR_MAX);
    };
};

template <> struct complement_base<true>
{
    template <class Number>
    struct values
    {
        BOOST_STATIC_CONSTANT(Number, min = SCHAR_MIN);
        BOOST_STATIC_CONSTANT(Number, max = SCHAR_MAX);
    };
};

// Base specialization of complement, puts an end to the recursion.
template <>
struct complement<1>
{
    template <class Number>
    struct traits
    {
        BOOST_STATIC_CONSTANT(bool, is_signed = boost::detail::is_signed<Number>::value);
        BOOST_STATIC_CONSTANT(Number, min =
                            complement_base<is_signed>::template values<Number>::min);
        BOOST_STATIC_CONSTANT(Number, max =
                            complement_base<is_signed>::template values<Number>::max);
    };
};

// Now here's the "pretty" template you're intended to actually use.
//   complement_traits<Number>::min, complement_traits<Number>::max are the
//   minimum and maximum values of Number if Number is a built-in integer type.
template <class Number>
struct complement_traits
{
    BOOST_STATIC_CONSTANT(Number, max = (complement_traits_aux<Number, sizeof(Number)>::max));
    BOOST_STATIC_CONSTANT(Number, min = (complement_traits_aux<Number, sizeof(Number)>::min));
};

// =================================================================================

// Support for streaming various numeric types in exactly the format I want. I
// needed this in addition to all the assertions so that I could see exactly
// what was going on.
//
// Numbers go through a 2-stage conversion process (by default, though, no real
// conversion).
//
template <class T> struct stream_as {
    typedef T t1;
    typedef T t2;
};

// char types first get converted to unsigned char, then to unsigned.
template <> struct stream_as<char> {
    typedef unsigned char t1;
    typedef unsigned t2;
};
template <> struct stream_as<unsigned char> {
    typedef unsigned char t1; typedef unsigned t2;
};
template <> struct stream_as<signed char>  {
    typedef unsigned char t1; typedef unsigned t2;
};

#if defined(BOOST_MSVC_STD_ITERATOR) // No intmax streaming built-in

// With this library implementation, __int64 and __uint64 get streamed as strings
template <> struct stream_as<boost::uintmax_t> {
    typedef std::string t1;
    typedef std::string t2;
};

template <> struct stream_as<boost::intmax_t>  {
    typedef std::string t1;
    typedef std::string t2;
};
#endif

// Standard promotion process for streaming
template <class T> struct promote
{
    static typename stream_as<T>::t1 from(T x) {
        typedef typename stream_as<T>::t1 t1;
        return t1(x);
    }
};

#if defined(BOOST_MSVC_STD_ITERATOR) // No intmax streaming built-in

// On this platform, stream them as long/unsigned long if they fit.
// Otherwise, write a string.
template <> struct promote<boost::uintmax_t> {
    std::string static from(const boost::uintmax_t x) {
        if (x > ULONG_MAX)
            return std::string("large unsigned value");
        else
            return boost::lexical_cast<std::string>((unsigned long)x);
    }
};
template <> struct promote<boost::intmax_t> {
    std::string static from(const boost::intmax_t x) {
        if (x > boost::intmax_t(ULONG_MAX))
            return std::string("large positive signed value");
        else if (x >= 0)
            return boost::lexical_cast<std::string>((unsigned long)x);

        if (x < boost::intmax_t(LONG_MIN))
            return std::string("large negative signed value");
        else
            return boost::lexical_cast<std::string>((long)x);
    }
};
#endif

// This is the function which converts types to the form I want to stream them in.
template <class T>
typename stream_as<T>::t2 stream_number(T x)
{
    return promote<T>::from(x);
}
// =================================================================================

//
// Tests for built-in signed and unsigned types
//

// Tag types for selecting tests
struct unsigned_tag {};
struct signed_tag {};

// Tests for unsigned numbers. The extra default Number parameter works around
// an MSVC bug.
template <class Number>
void test_aux(unsigned_tag, Number*)
{
    typedef typename boost::detail::numeric_traits<Number>::difference_type difference_type;
    BOOST_STATIC_ASSERT(!boost::detail::is_signed<Number>::value);
    BOOST_STATIC_ASSERT(
        (sizeof(Number) < sizeof(boost::intmax_t))
        | (boost::is_same<difference_type, boost::intmax_t>::value));

#if defined(__GNUC__) && __GNUC__ == 4 && __GNUC_MINOR__ == 0 && __GNUC_PATCHLEVEL__ == 2
    // GCC 4.0.2 ICEs on this C-style cases
    BOOST_STATIC_ASSERT((complement_traits<Number>::max) > Number(0));
    BOOST_STATIC_ASSERT((complement_traits<Number>::min) == Number(0));
#else
    // Force casting to Number here to work around the fact that it's an enum on MSVC
    BOOST_STATIC_ASSERT(Number(complement_traits<Number>::max) > Number(0));
    BOOST_STATIC_ASSERT(Number(complement_traits<Number>::min) == Number(0));
#endif

    const Number max = complement_traits<Number>::max;
    const Number min = complement_traits<Number>::min;

    const Number test_max = (sizeof(Number) < sizeof(boost::intmax_t))
        ? max
        : max / 2 - 1;

    std::cout << std::hex << "(unsigned) min = " << stream_number(min) << ", max = "
              << stream_number(max) << "..." << std::flush;
    std::cout << "difference_type = " << typeid(difference_type).name() << "..."
              << std::flush;

    difference_type d1 = boost::detail::numeric_distance(Number(0), test_max);
    difference_type d2 = boost::detail::numeric_distance(test_max, Number(0));

    std::cout << "0->" << stream_number(test_max) << "==" << std::dec << stream_number(d1) << "; "
              << std::hex << stream_number(test_max) << "->0==" << std::dec << stream_number(d2) << "..." << std::flush;

    assert(d1 == difference_type(test_max));
    assert(d2 == -difference_type(test_max));
}

// Tests for signed numbers. The extra default Number parameter works around an
// MSVC bug.
struct out_of_range_tag {};
struct in_range_tag {};

// This test morsel gets executed for numbers whose difference will always be
// representable in intmax_t
template <class Number>
void signed_test(in_range_tag, Number*)
{
    BOOST_STATIC_ASSERT(boost::detail::is_signed<Number>::value);
    typedef typename boost::detail::numeric_traits<Number>::difference_type difference_type;
    const Number max = complement_traits<Number>::max;
    const Number min = complement_traits<Number>::min;

    difference_type d1 = boost::detail::numeric_distance(min, max);
    difference_type d2 = boost::detail::numeric_distance(max, min);

    std::cout << stream_number(min) << "->" << stream_number(max) << "==";
    std::cout << std::dec << stream_number(d1) << "; ";
    std::cout << std::hex << stream_number(max) << "->" << stream_number(min)
              << "==" << std::dec << stream_number(d2) << "..." << std::flush;
    assert(d1 == difference_type(max) - difference_type(min));
    assert(d2 == difference_type(min) - difference_type(max));
}

// This test morsel gets executed for numbers whose difference may exceed the
// capacity of intmax_t.
template <class Number>
void signed_test(out_of_range_tag, Number*)
{
    BOOST_STATIC_ASSERT(boost::detail::is_signed<Number>::value);
    typedef typename boost::detail::numeric_traits<Number>::difference_type difference_type;
    const Number max = complement_traits<Number>::max;
    const Number min = complement_traits<Number>::min;

    difference_type min_distance = complement_traits<difference_type>::min;
    difference_type max_distance = complement_traits<difference_type>::max;

    const Number n1 = Number(min + max_distance);
    const Number n2 = Number(max + min_distance);
    difference_type d1 = boost::detail::numeric_distance(min, n1);
    difference_type d2 = boost::detail::numeric_distance(max, n2);

    std::cout << stream_number(min) << "->" << stream_number(n1) << "==";
    std::cout << std::dec << stream_number(d1) << "; ";
    std::cout << std::hex << stream_number(max) << "->" << stream_number(n2)
              << "==" << std::dec << stream_number(d2) << "..." << std::flush;
    assert(d1 == max_distance);
    assert(d2 == min_distance);
}

template <class Number>
void test_aux(signed_tag, Number*)
{
    typedef typename boost::detail::numeric_traits<Number>::difference_type difference_type;
    BOOST_STATIC_ASSERT(boost::detail::is_signed<Number>::value);
    BOOST_STATIC_ASSERT(
        (sizeof(Number) < sizeof(boost::intmax_t))
        | (boost::is_same<difference_type, Number>::value));

#if defined(__GNUC__) && __GNUC__ == 4 && __GNUC_MINOR__ == 0 && __GNUC_PATCHLEVEL__ == 2
    // GCC 4.0.2 ICEs on this cast
    BOOST_STATIC_ASSERT((complement_traits<Number>::max) > Number(0));
    BOOST_STATIC_ASSERT((complement_traits<Number>::min) < Number(0));
#else
    // Force casting to Number here to work around the fact that it's an enum on MSVC
    BOOST_STATIC_ASSERT(Number(complement_traits<Number>::max) > Number(0));
    BOOST_STATIC_ASSERT(Number(complement_traits<Number>::min) < Number(0));
#endif
    const Number max = complement_traits<Number>::max;
    const Number min = complement_traits<Number>::min;

    std::cout << std::hex << "min = " << stream_number(min) << ", max = "
              << stream_number(max) << "..." << std::flush;
    std::cout << "difference_type = " << typeid(difference_type).name() << "..."
              << std::flush;

    typedef typename boost::detail::if_true<
                          (sizeof(Number) < sizeof(boost::intmax_t))>
                        ::template then<
                          in_range_tag,
                          out_of_range_tag
                        >::type
        range_tag;
    signed_test<Number>(range_tag(), 0);
}

// Test for all numbers. The extra default Number parameter works around an MSVC
// bug.
template <class Number>
void test(Number* = 0)
{
    std::cout << "testing " << typeid(Number).name() << ":\n"
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
              << "is_signed: " << (std::numeric_limits<Number>::is_signed ? "true\n" : "false\n")
              << "is_bounded: " << (std::numeric_limits<Number>::is_bounded ? "true\n" : "false\n")
              << "digits: " << std::numeric_limits<Number>::digits << "\n"
#endif
              << "..." << std::flush;

    // factoring out difference_type for the assert below confused Borland :(
    typedef boost::detail::is_signed<
#if !defined(BOOST_MSVC) || BOOST_MSVC > 1300
        typename
#endif
        boost::detail::numeric_traits<Number>::difference_type
        > is_signed;
    BOOST_STATIC_ASSERT(is_signed::value);

    typedef typename boost::detail::if_true<
        boost::detail::is_signed<Number>::value
        >::template then<signed_tag, unsigned_tag>::type signedness;

    test_aux<Number>(signedness(), 0);
    std::cout << "passed" << std::endl;
}

int main()
{
    test<char>();
    test<unsigned char>();
    test<signed char>();
    test<wchar_t>();
    test<short>();
    test<unsigned short>();
    test<int>();
    test<unsigned int>();
    test<long>();
    test<unsigned long>();
#if defined(BOOST_HAS_LONG_LONG) && !defined(BOOST_NO_INTEGRAL_INT64_T)
    test< ::boost::long_long_type>();
    test< ::boost::ulong_long_type>();
#elif defined(BOOST_MSVC)
    // The problem of not having compile-time static class constants other than
    // enums prevents this from working, since values get truncated.
    // test<boost::uintmax_t>();
    // test<boost::intmax_t>();
#endif
    return 0;
}


/* boost random/uniform_on_sphere.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Copyright Steven Watanabe 2011
 * 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 for most recent version including documentation.
 *
 * $Id$
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_UNIFORM_ON_SPHERE_HPP
#define BOOST_RANDOM_UNIFORM_ON_SPHERE_HPP

#include <vector>
#include <algorithm>     // std::transform
#include <functional>    // std::bind2nd, std::divides
#include <boost/assert.hpp>
#include <boost/random/detail/config.hpp>
#include <boost/random/detail/operators.hpp>
#include <boost/random/normal_distribution.hpp>

namespace boost {
namespace random {

/**
 * Instantiations of class template uniform_on_sphere model a
 * \random_distribution. Such a distribution produces random
 * numbers uniformly distributed on the unit sphere of arbitrary
 * dimension @c dim. The @c Cont template parameter must be a STL-like
 * container type with begin and end operations returning non-const
 * ForwardIterators of type @c Cont::iterator.
 */
template<class RealType = double, class Cont = std::vector<RealType> >
class uniform_on_sphere
{
public:
    typedef RealType input_type;
    typedef Cont result_type;

    class param_type
    {
    public:

        typedef uniform_on_sphere distribution_type;

        /**
         * Constructs the parameters of a uniform_on_sphere
         * distribution, given the dimension of the sphere.
         */
        explicit param_type(int dim_arg = 2) : _dim(dim_arg)
        {
            BOOST_ASSERT(_dim >= 0);
        }

        /** Returns the dimension of the sphere. */
        int dim() const { return _dim; }

        /** Writes the parameters to a @c std::ostream. */
        BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, param_type, parm)
        {
            os << parm._dim;
            return os;
        }

        /** Reads the parameters from a @c std::istream. */
        BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, param_type, parm)
        {
            is >> parm._dim;
            return is;
        }

        /** Returns true if the two sets of parameters are equal. */
        BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(param_type, lhs, rhs)
        { return lhs._dim == rhs._dim; }

        /** Returns true if the two sets of parameters are different. */
        BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(param_type)

    private:
        int _dim;
    };

    /**
     * Constructs a @c uniform_on_sphere distribution.
     * @c dim is the dimension of the sphere.
     *
     * Requires: dim >= 0
     */
    explicit uniform_on_sphere(int dim_arg = 2)
      : _container(dim_arg), _dim(dim_arg) { }

    /**
     * Constructs a @c uniform_on_sphere distribution from its parameters.
     */
    explicit uniform_on_sphere(const param_type& parm)
      : _container(parm.dim()), _dim(parm.dim()) { }

    // compiler-generated copy ctor and assignment operator are fine

    /** Returns the dimension of the sphere. */
    int dim() const { return _dim; }

    /** Returns the parameters of the distribution. */
    param_type param() const { return param_type(_dim); }
    /** Sets the parameters of the distribution. */
    void param(const param_type& parm)
    {
        _dim = parm.dim();
        _container.resize(_dim);
    }

    /**
     * Returns the smallest value that the distribution can produce.
     * Note that this is required to approximate the standard library's
     * requirements.  The behavior is defined according to lexicographical
     * comparison so that for a container type of std::vector,
     * dist.min() <= x <= dist.max() where x is any value produced
     * by the distribution.
     */
    result_type min BOOST_PREVENT_MACRO_SUBSTITUTION () const
    {
        result_type result(_dim);
        if(_dim != 0) {
            result.front() = RealType(-1.0);
        }
        return result;
    }
    /**
     * Returns the largest value that the distribution can produce.
     * Note that this is required to approximate the standard library's
     * requirements.  The behavior is defined according to lexicographical
     * comparison so that for a container type of std::vector,
     * dist.min() <= x <= dist.max() where x is any value produced
     * by the distribution.
     */
    result_type max BOOST_PREVENT_MACRO_SUBSTITUTION () const
    {
        result_type result(_dim);
        if(_dim != 0) {
            result.front() = RealType(1.0);
        }
        return result;
    }

    /**
     * Effects: Subsequent uses of the distribution do not depend
     * on values produced by any engine prior to invoking reset.
     */
    void reset() {}

    /**
     * Returns a point uniformly distributed over the surface of
     * a sphere of dimension dim().
     */
    template<class Engine>
    const result_type & operator()(Engine& eng)
    {
        using std::sqrt;
        switch(_dim)
        {
        case 0: break;
        case 1:
            {
                if(uniform_01<RealType>()(eng) < 0.5) {
                    *_container.begin() = -1;
                } else {
                    *_container.begin() = 1;
                }
            }
        case 2:
            {
                uniform_01<RealType> uniform;
                RealType sqsum;
                RealType x, y;
                do {
                    x = uniform(eng) * 2 - 1;
                    y = uniform(eng) * 2 - 1;
                    sqsum = x*x + y*y;
                } while(sqsum == 0 || sqsum > 1);
                RealType mult = 1/sqrt(sqsum);
                typename Cont::iterator iter = _container.begin();
                *iter = x * mult;
                iter++;
                *iter = y * mult;
                break;
            }
        case 3:
            {
                uniform_01<RealType> uniform;
                RealType sqsum;
                RealType x, y;
                do {
                    x = uniform(eng) * 2 - 1;
                    y = uniform(eng) * 2 - 1;
                    sqsum = x*x + y*y;
                } while(sqsum > 1);
                RealType mult = 2 * sqrt(1 - sqsum);
                typename Cont::iterator iter = _container.begin();
                *iter = x * mult;
                ++iter;
                *iter = y * mult;
                ++iter;
                *iter = 2 * sqsum - 1;
                break;
            }
        default:
            {
                detail::unit_normal_distribution<RealType> normal;
                RealType sqsum;
                do {
                    sqsum = 0;
                    for(typename Cont::iterator it = _container.begin();
                        it != _container.end();
                        ++it) {
                        RealType val = normal(eng);
                        *it = val;
                        sqsum += val * val;
                    }
                } while(sqsum == 0);
                // for all i: result[i] /= sqrt(sqsum)
                std::transform(_container.begin(), _container.end(), _container.begin(),
                               std::bind2nd(std::multiplies<RealType>(), 1/sqrt(sqsum)));
            }
        }
        return _container;
    }

    /**
     * Returns a point uniformly distributed over the surface of
     * a sphere of dimension param.dim().
     */
    template<class Engine>
    result_type operator()(Engine& eng, const param_type& parm) const
    {
        return uniform_on_sphere(parm)(eng);
    }

    /** Writes the distribution to a @c std::ostream. */
    BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, uniform_on_sphere, sd)
    {
        os << sd._dim;
        return os;
    }

    /** Reads the distribution from a @c std::istream. */
    BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, uniform_on_sphere, sd)
    {
        is >> sd._dim;
        sd._container.resize(sd._dim);
        return is;
    }

    /**
     * Returns true if the two distributions will produce identical
     * sequences of values, given equal generators.
     */
    BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(uniform_on_sphere, lhs, rhs)
    { return lhs._dim == rhs._dim; }

    /**
     * Returns true if the two distributions may produce different
     * sequences of values, given equal generators.
     */
    BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(uniform_on_sphere)

private:
    result_type _container;
    int _dim;
};

} // namespace random

using random::uniform_on_sphere;

} // namespace boost

#endif // BOOST_RANDOM_UNIFORM_ON_SPHERE_HPP