///////////////////////////////////////////////////////////////////////////
//
// Copyright (c) 1998-2011, Industrial Light & Magic, a division of Lucas
// Digital Ltd. LLC
//
// 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 Industrial Light & Magic nor the names of
// its contributors may be used to endorse or promote products derived
// from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER 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.
//
///////////////////////////////////////////////////////////////////////////

#include "PyIlmBaseConfigInternal.h"

#include <Python.h>
#include <boost/python.hpp>
#include <boost/python/make_constructor.hpp>
#include <boost/format.hpp>
#include <ImathVec.h>
#include <ImathQuat.h>
#include <ImathEuler.h>
#include <ImathFun.h>
#include <ImathMatrixAlgo.h>

#include <PyIexExport.h>
#include "PyImathFixedArray.h"
#include "PyImath.h"
#include "PyImathExport.h"
#include "PyImathBasicTypes.h"
#include "PyImathVec.h"
#include "PyImathMatrix.h"
#include "PyImathBox.h"
#include "PyImathFun.h"
#include "PyImathQuat.h"
#include "PyImathEuler.h"
#include "PyImathColor.h"
#include "PyImathFrustum.h"
#include "PyImathPlane.h"
#include "PyImathLine.h"
#include "PyImathRandom.h"
#include "PyImathShear.h"
#include "PyImathMathExc.h"
#include "PyImathAutovectorize.h"
#include "PyImathStringArrayRegister.h"
#include <PyIex.h>

using namespace boost::python;

namespace {

template <typename T>
IMATH_NAMESPACE::Box<IMATH_NAMESPACE::Vec3<T> >
computeBoundingBox(const PyImath::FixedArray<IMATH_NAMESPACE::Vec3<T> >& position)
{
    IMATH_NAMESPACE::Box<IMATH_NAMESPACE::Vec3<T> > bounds;
    int len = position.len();
    for (int i = 0; i < len; ++i)
        bounds.extendBy(position[i]);
    return bounds;
}

IMATH_NAMESPACE::M44d
procrustes1 (PyObject* from_input,
             PyObject* to_input,
             PyObject* weights_input = 0,
             bool doScale = false)
{
    // Verify the sequences:
    if (!PySequence_Check (from_input))
    {
        PyErr_SetString (PyExc_TypeError, "Expected a sequence type for 'from'");
        boost::python::throw_error_already_set();
    }

    if (!PySequence_Check (to_input))
    {
        PyErr_SetString (PyExc_TypeError, "Expected a sequence type for 'to'");
        boost::python::throw_error_already_set();
    }

    bool useWeights = PySequence_Check (weights_input);

    // Now verify the lengths:
    const Py_ssize_t n = PySequence_Length (from_input);
    if (n != PySequence_Length (to_input) ||
        (useWeights && n != PySequence_Length (weights_input)))
    {
        PyErr_SetString (PyExc_TypeError, "'from, 'to', and 'weights' should all have the same lengths.");
        boost::python::throw_error_already_set();
    }

    std::vector<IMATH_NAMESPACE::V3d> from;  from.reserve (n);
    std::vector<IMATH_NAMESPACE::V3d> to;    to.reserve (n);
    std::vector<double> weights;   weights.reserve (n);

    for (Py_ssize_t i = 0; i < n; ++i)
    {
        PyObject* f = PySequence_GetItem (from_input, i);
        PyObject* t = PySequence_GetItem (to_input, i);
        PyObject* w = 0;
        if (useWeights)
            w = PySequence_GetItem (weights_input, i);

        if (f == 0 || t == 0 || (useWeights && w == 0))
        {
            PyErr_SetString (PyExc_TypeError,
                             "Missing element in array");
            boost::python::throw_error_already_set();
        }

        from.push_back (boost::python::extract<IMATH_NAMESPACE::V3d> (f));
        to.push_back (boost::python::extract<IMATH_NAMESPACE::V3d> (t));
        if (useWeights)
            weights.push_back (boost::python::extract<double> (w));
    }

    if (useWeights)
        return IMATH_NAMESPACE::procrustesRotationAndTranslation (&from[0], &to[0], &weights[0], n, doScale);
    else
        return IMATH_NAMESPACE::procrustesRotationAndTranslation (&from[0], &to[0], n, doScale);
}

PyImath::FixedArray2D<int> rangeX(int sizeX, int sizeY)
{
    PyImath::FixedArray2D<int> f(sizeX, sizeY);
    for (int j=0; j<sizeY; j++)
        for (int i=0; i<sizeX; i++)
            f(i,j) = i;
    return f;
}

PyImath::FixedArray2D<int> rangeY(int sizeX, int sizeY)
{
    PyImath::FixedArray2D<int> f(sizeX, sizeY);
    for (int j=0; j<sizeY; j++)
        for (int i=0; i<sizeX; i++)
            f(i,j) = j;
    return f;
}

} // namespace

namespace PyImath {

void register_all()
{
    scope().attr("__doc__") = "Imath module";

    register_basicTypes();

    class_<IntArray2D> iclass2D = IntArray2D::register_("IntArray2D","Fixed length array of ints");
    add_arithmetic_math_functions(iclass2D);
    add_mod_math_functions(iclass2D);
    add_comparison_functions(iclass2D);
    add_ordered_comparison_functions(iclass2D);
    add_explicit_construction_from_type<float>(iclass2D);
    add_explicit_construction_from_type<double>(iclass2D);

    class_<IntMatrix> imclass = IntMatrix::register_("IntMatrix","Fixed size matrix of ints");
    add_arithmetic_math_functions(imclass);

    class_<FloatArray2D> fclass2D = FloatArray2D::register_("FloatArray2D","Fixed length 2D array of floats");
    add_arithmetic_math_functions(fclass2D);
    add_pow_math_functions(fclass2D);
    add_comparison_functions(fclass2D);
    add_ordered_comparison_functions(fclass2D);
    add_explicit_construction_from_type<int>(fclass2D);
    add_explicit_construction_from_type<double>(fclass2D);

    class_<FloatMatrix> fmclass = FloatMatrix::register_("FloatMatrix","Fixed size matrix of floats");
    add_arithmetic_math_functions(fmclass);
    add_pow_math_functions(fmclass);

    class_<DoubleArray2D> dclass2D = DoubleArray2D::register_("DoubleArray2D","Fixed length array of doubles");
    add_arithmetic_math_functions(dclass2D);
    add_pow_math_functions(dclass2D);
    add_comparison_functions(dclass2D);
    add_ordered_comparison_functions(dclass2D);
    add_explicit_construction_from_type<int>(dclass2D);
    add_explicit_construction_from_type<float>(dclass2D);

    class_<DoubleMatrix> dmclass = DoubleMatrix::register_("DoubleMatrix","Fixed size matrix of doubles");
    add_arithmetic_math_functions(dmclass);
    add_pow_math_functions(dmclass);

    def("rangeX", &rangeX);
    def("rangeY", &rangeY);

    //
    //  Vec2
    //
    register_Vec2<short>();
    register_Vec2<int>();
    register_Vec2<float>();
    register_Vec2<double>();
    class_<FixedArray<IMATH_NAMESPACE::V2s> > v2s_class = register_Vec2Array<short>();
    class_<FixedArray<IMATH_NAMESPACE::V2i> > v2i_class = register_Vec2Array<int>();
    class_<FixedArray<IMATH_NAMESPACE::V2f> > v2f_class = register_Vec2Array<float>();
    class_<FixedArray<IMATH_NAMESPACE::V2d> > v2d_class = register_Vec2Array<double>();
    add_explicit_construction_from_type<IMATH_NAMESPACE::V2f>(v2i_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::V2d>(v2i_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::V2i>(v2f_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::V2d>(v2f_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::V2i>(v2d_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::V2f>(v2d_class);

    //
    //  Vec3
    //
    register_Vec3<unsigned char>();
    register_Vec3<short>();
    register_Vec3<int>();
    register_Vec3<float>();
    register_Vec3<double>();
    class_<FixedArray<IMATH_NAMESPACE::V3s> > v3s_class = register_Vec3Array<short>();
    class_<FixedArray<IMATH_NAMESPACE::V3i> > v3i_class = register_Vec3Array<int>();
    class_<FixedArray<IMATH_NAMESPACE::V3f> > v3f_class = register_Vec3Array<float>();
    class_<FixedArray<IMATH_NAMESPACE::V3d> > v3d_class = register_Vec3Array<double>();
    add_explicit_construction_from_type<IMATH_NAMESPACE::V3f>(v3i_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::V3d>(v3i_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::V3i>(v3f_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::V3d>(v3f_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::V3i>(v3d_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::V3f>(v3d_class);

    //
    //  Vec4
    //
    register_Vec4<unsigned char>();
    register_Vec4<short>();
    register_Vec4<int>();
    register_Vec4<float>();
    register_Vec4<double>();
    class_<FixedArray<IMATH_NAMESPACE::V4s> > v4s_class = register_Vec4Array<short>();
    class_<FixedArray<IMATH_NAMESPACE::V4i> > v4i_class = register_Vec4Array<int>();
    class_<FixedArray<IMATH_NAMESPACE::V4f> > v4f_class = register_Vec4Array<float>();
    class_<FixedArray<IMATH_NAMESPACE::V4d> > v4d_class = register_Vec4Array<double>();
    add_explicit_construction_from_type<IMATH_NAMESPACE::V4f>(v4i_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::V4d>(v4i_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::V4i>(v4f_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::V4d>(v4f_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::V4i>(v4d_class);

    //
    //  Quat
    //
    register_Quat<float>();
    register_Quat<double>();
    class_<FixedArray<IMATH_NAMESPACE::Quatf> > quatf_class = register_QuatArray<float>();
    class_<FixedArray<IMATH_NAMESPACE::Quatd> > quatd_class = register_QuatArray<double>();
    add_explicit_construction_from_type<IMATH_NAMESPACE::Quatd>(quatf_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::Quatf>(quatd_class);

    //
    // Euler
    //
    register_Euler<float>();
    register_Euler<double>();
    class_<FixedArray<IMATH_NAMESPACE::Eulerf> > eulerf_class = register_EulerArray<float>();
    class_<FixedArray<IMATH_NAMESPACE::Eulerd> > eulerd_class = register_EulerArray<double>();
    add_explicit_construction_from_type<IMATH_NAMESPACE::Eulerd>(eulerf_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::Eulerf>(eulerd_class);

    //
    // Box2
    //
    register_Box2<IMATH_NAMESPACE::V2s>();
    register_Box2<IMATH_NAMESPACE::V2i>();
    register_Box2<IMATH_NAMESPACE::V2f>();
    register_Box2<IMATH_NAMESPACE::V2d>();
    class_<FixedArray<IMATH_NAMESPACE::Box2s> > b2s_class = register_BoxArray<IMATH_NAMESPACE::V2s>();
    class_<FixedArray<IMATH_NAMESPACE::Box2i> > b2i_class = register_BoxArray<IMATH_NAMESPACE::V2i>();
    class_<FixedArray<IMATH_NAMESPACE::Box2f> > b2f_class = register_BoxArray<IMATH_NAMESPACE::V2f>();
    class_<FixedArray<IMATH_NAMESPACE::Box2d> > b2d_class = register_BoxArray<IMATH_NAMESPACE::V2d>();

    //
    // Box3
    //
    register_Box3<IMATH_NAMESPACE::V3s>();
    register_Box3<IMATH_NAMESPACE::V3i>();
    register_Box3<IMATH_NAMESPACE::V3f>();
    register_Box3<IMATH_NAMESPACE::V3d>();
    class_<FixedArray<IMATH_NAMESPACE::Box3s> > b3s_class = register_BoxArray<IMATH_NAMESPACE::V3s>();
    class_<FixedArray<IMATH_NAMESPACE::Box3i> > b3i_class = register_BoxArray<IMATH_NAMESPACE::V3i>();
    class_<FixedArray<IMATH_NAMESPACE::Box3f> > b3f_class = register_BoxArray<IMATH_NAMESPACE::V3f>();
    class_<FixedArray<IMATH_NAMESPACE::Box3d> > b3d_class = register_BoxArray<IMATH_NAMESPACE::V3d>();

    //
    // Matrix22/33/44
    //
    register_Matrix22<float>();
    register_Matrix22<double>();
    register_Matrix33<float>();
    register_Matrix33<double>();
    register_Matrix44<float>();
    register_Matrix44<double>();

    //
    // M22/M33/44Array
    //
    class_<FixedArray<IMATH_NAMESPACE::M44d> > m44d_class = register_M44Array<double>();
    class_<FixedArray<IMATH_NAMESPACE::M44f> > m44f_class = register_M44Array<float>();
    add_explicit_construction_from_type< IMATH_NAMESPACE::Matrix44<double> >(m44d_class);
    add_explicit_construction_from_type< IMATH_NAMESPACE::Matrix44<float> > (m44f_class);

    class_<FixedArray<IMATH_NAMESPACE::M33d> > m33d_class = register_M33Array<double>();
    class_<FixedArray<IMATH_NAMESPACE::M33f> > m33f_class = register_M33Array<float>();
    add_explicit_construction_from_type< IMATH_NAMESPACE::Matrix33<double> >(m33d_class);
    add_explicit_construction_from_type< IMATH_NAMESPACE::Matrix33<float> > (m33f_class);

    class_<FixedArray<IMATH_NAMESPACE::M22d> > m22d_class = register_M22Array<double>();
    class_<FixedArray<IMATH_NAMESPACE::M22f> > m22f_class = register_M22Array<float>();
    add_explicit_construction_from_type< IMATH_NAMESPACE::Matrix22<double> >(m22d_class);
    add_explicit_construction_from_type< IMATH_NAMESPACE::Matrix22<float> > (m22f_class);

    //
    // String Array
    //
    register_StringArrays();

    //
    // Color3/4
    //
    register_Color3<unsigned char>();
    register_Color3<float>();
    register_Color4<unsigned char>();
    register_Color4<float>();

    //
    // C3/4Array
    //
    class_<FixedArray<IMATH_NAMESPACE::Color3f> > c3f_class = register_Color3Array<float>();
    class_<FixedArray<IMATH_NAMESPACE::Color3c> > c3c_class = register_Color3Array<unsigned char>();
    add_explicit_construction_from_type<IMATH_NAMESPACE::V3f>(c3f_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::V3d>(c3f_class);

    class_<FixedArray<IMATH_NAMESPACE::Color4f> > c4f_class = register_Color4Array<float>();
    class_<FixedArray<IMATH_NAMESPACE::Color4c> > c4c_class = register_Color4Array<unsigned char>();

    //
    // Color4Array
    //
    register_Color4Array2D<float>();
    register_Color4Array2D<unsigned char>();

    //
    // Frustum
    //
    register_Frustum<float>();
    register_Frustum<double>();
    register_FrustumTest<float>();
    register_FrustumTest<double>();

    //
    // Plane
    //
    register_Plane<float>();
    register_Plane<double>();

    //
    // Line
    //
    register_Line<float>();
    register_Line<double>();

    //
    // Shear
    //
    register_Shear<float>();
    register_Shear<double>();

    //
    // Utility Functions
    //
    register_functions();

    def("procrustesRotationAndTranslation", procrustes1,
        args("fromPts", "toPts", "weights", "doScale"),  // Can't use 'from' and 'to' because 'from' is a reserved keywork in Python
        "Computes the orthogonal transform (consisting only of rotation and translation) mapping the "
        "'fromPts' points as close as possible to the 'toPts' points in the least squares norm.  The 'fromPts' and "
        "'toPts' lists must be the same length or the function will error out.  If weights "
        "are provided, then the points are weighted (that is, some points are considered more important "
        "than others while computing the transform).  If the 'doScale' parameter is True, then "
        "the resulting matrix is also allowed to have a uniform scale.");

    //
    // Rand
    //
    register_Rand32();
    register_Rand48();

    //
    // Initialize constants
    //

    scope().attr("EULER_XYZ")    = IMATH_NAMESPACE::Eulerf::XYZ;
    scope().attr("EULER_XZY")    = IMATH_NAMESPACE::Eulerf::XZY;
    scope().attr("EULER_YZX")    = IMATH_NAMESPACE::Eulerf::YZX;
    scope().attr("EULER_YXZ")    = IMATH_NAMESPACE::Eulerf::YXZ;
    scope().attr("EULER_ZXY")    = IMATH_NAMESPACE::Eulerf::ZXY;
    scope().attr("EULER_ZYX")    = IMATH_NAMESPACE::Eulerf::ZYX;
    scope().attr("EULER_XZX")    = IMATH_NAMESPACE::Eulerf::XZX;
    scope().attr("EULER_XYX")    = IMATH_NAMESPACE::Eulerf::XYX;
    scope().attr("EULER_YXY")    = IMATH_NAMESPACE::Eulerf::YXY;
    scope().attr("EULER_YZY")    = IMATH_NAMESPACE::Eulerf::YZY;
    scope().attr("EULER_ZYZ")    = IMATH_NAMESPACE::Eulerf::ZYZ;
    scope().attr("EULER_ZXZ")    = IMATH_NAMESPACE::Eulerf::ZXZ;
    scope().attr("EULER_XYZr")   = IMATH_NAMESPACE::Eulerf::XYZr;
    scope().attr("EULER_XZYr")   = IMATH_NAMESPACE::Eulerf::XZYr;
    scope().attr("EULER_YZXr")   = IMATH_NAMESPACE::Eulerf::YZXr;
    scope().attr("EULER_YXZr")   = IMATH_NAMESPACE::Eulerf::YXZr;
    scope().attr("EULER_ZXYr")   = IMATH_NAMESPACE::Eulerf::ZXYr;
    scope().attr("EULER_ZYXr")   = IMATH_NAMESPACE::Eulerf::ZYXr;
    scope().attr("EULER_XZXr")   = IMATH_NAMESPACE::Eulerf::XZXr;
    scope().attr("EULER_XYXr")   = IMATH_NAMESPACE::Eulerf::XYXr;
    scope().attr("EULER_YXYr")   = IMATH_NAMESPACE::Eulerf::YXYr;
    scope().attr("EULER_YZYr")   = IMATH_NAMESPACE::Eulerf::YZYr;
    scope().attr("EULER_ZYZr")   = IMATH_NAMESPACE::Eulerf::ZYZr;
    scope().attr("EULER_ZXZr")   = IMATH_NAMESPACE::Eulerf::ZXZr;
    scope().attr("EULER_X_AXIS") = IMATH_NAMESPACE::Eulerf::X;
    scope().attr("EULER_Y_AXIS") = IMATH_NAMESPACE::Eulerf::Y;
    scope().attr("EULER_Z_AXIS") = IMATH_NAMESPACE::Eulerf::Z;

    scope().attr("INT_MIN")      = IMATH_NAMESPACE::limits<int>::min();
    scope().attr("INT_MAX")      = IMATH_NAMESPACE::limits<int>::max();
    scope().attr("INT_SMALLEST") = IMATH_NAMESPACE::limits<int>::smallest();
    scope().attr("INT_EPS")      = IMATH_NAMESPACE::limits<int>::epsilon();

    scope().attr("FLT_MIN")      = IMATH_NAMESPACE::limits<float>::min();
    scope().attr("FLT_MAX")      = IMATH_NAMESPACE::limits<float>::max();
    scope().attr("FLT_SMALLEST") = IMATH_NAMESPACE::limits<float>::smallest();
    scope().attr("FLT_EPS")      = IMATH_NAMESPACE::limits<float>::epsilon();

    scope().attr("DBL_MIN")      = IMATH_NAMESPACE::limits<double>::min();
    scope().attr("DBL_MAX")      = IMATH_NAMESPACE::limits<double>::max();
    scope().attr("DBL_SMALLEST") = IMATH_NAMESPACE::limits<double>::smallest();
    scope().attr("DBL_EPS")      = IMATH_NAMESPACE::limits<double>::epsilon();

    //
    // Register Exceptions
    //
    PyIex::registerExc<IMATH_NAMESPACE::NullVecExc,IEX_NAMESPACE::MathExc>("NullVecExc","imath");
    PyIex::registerExc<IMATH_NAMESPACE::NullQuatExc,IEX_NAMESPACE::MathExc>("NullQuatExc","imath");
    PyIex::registerExc<IMATH_NAMESPACE::SingMatrixExc,IEX_NAMESPACE::MathExc>("SingMatrixExc","imath");
    PyIex::registerExc<IMATH_NAMESPACE::ZeroScaleExc,IEX_NAMESPACE::MathExc>("ZeroScaleExc","imath");
    PyIex::registerExc<IMATH_NAMESPACE::IntVecNormalizeExc,IEX_NAMESPACE::MathExc>("IntVecNormalizeExc","imath");

    def("computeBoundingBox", &computeBoundingBox<float>,
        "computeBoundingBox(position) -- computes the bounding box from the position array.");

    def("computeBoundingBox", &computeBoundingBox<double>,
        "computeBoundingBox(position) -- computes the bounding box from the position array.");
}

} // namespace PyImath


// This file is part of snark, a generic and flexible library for robotics research
// Copyright (c) 2011 The University of Sydney
// All rights reserved.
//
// 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.
// 3. Neither the name of the University of Sydney nor the
//    names of its contributors may be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE
// GRANTED BY THIS LICENSE.  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT
// HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED
// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER 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.

#include <boost/noncopyable.hpp>
#include <comma/base/exception.h>
#include <snark/math/gaussian_process/gaussian_process.h>

namespace snark{

gaussian_process::gaussian_process( const Eigen::MatrixXd& domains
                                , const Eigen::VectorXd& targets
                                , const gaussian_process::covariance& covariance
                                , double self_covariance )
    : domains_( domains )
    , targets_( targets )
    , covariance_( covariance )
    , self_covariance_( self_covariance )
    , offset_( targets.sum() / targets.rows() )
    , K_( domains.rows(), domains.rows() )
{
    if( domains.rows() != targets.rows() ) { COMMA_THROW( comma::exception, "expected " << domains.rows() << " row(s) in targets, got " << targets.rows() << " row(s)" ); }
    targets_.array() -= offset_; // normalise
    //use m_K as Kxx + variance*I, then invert it
    //fill Kxx with values from covariance function
    //for elements r,c in upper triangle
    for( std::size_t r = 0; r < std::size_t( domains.rows() ); ++r )
    {
        K_( r, r ) = self_covariance_;
        const Eigen::VectorXd& row = domains.row( r );
        for( std::size_t c = r + 1; c < std::size_t( domains.rows() ); ++c )
        {
            K_( c, r ) = K_( r, c ) = covariance_( row, domains.row( c ) );
        }
    }
    L_.compute( K_ ); // invert Kxx + variance * I to become (by definition) B
    alpha_ = L_.solve( targets_ );
}

std::pair< double, double > gaussian_process::evaluate( const Eigen::MatrixXd& domain ) const
{
    if( domain.rows() != 1 ) { COMMA_THROW( comma::exception, "expected 1 row in domain, got " << domain.rows() << " rows" ); }
    Eigen::VectorXd means( 1 );
    Eigen::VectorXd variances( 1 );
    evaluate( domain, means, variances );
    return std::make_pair( means( 0 ), variances( 0 ) );
}

void gaussian_process::evaluate( const Eigen::MatrixXd& domains, Eigen::VectorXd& means, Eigen::VectorXd& variances ) const
{
    if( domains.cols() != domains_.cols() ) { COMMA_THROW( comma::exception, "expected " << domains_.cols() << " column(s) in domains, got " << domains.cols() << std::endl ); }
    Eigen::MatrixXd Kxsx = Eigen::MatrixXd::Zero( domains.rows(), domains_.rows() );
    for( std::size_t r = 0; r < std::size_t( domains.rows() ); ++r )
    {
        const Eigen::VectorXd& row = domains.row( r );
        for( std::size_t c = 0; c < std::size_t( domains_.rows() ); ++c )
        {
            Kxsx( r, c ) = covariance_( row, domains_.row( c ) );
        }
    }
    means = Kxsx * alpha_;
    means.array() += offset_;
    Eigen::MatrixXd Kxxs = Kxsx.transpose();
    L_.matrixL().solveInPlace( Kxxs );
    Eigen::MatrixXd& variance = Kxxs;
    variance = variance.array() * variance.array();
    variances = variance.colwise().sum();
    // for each diagonal variance, set v(r) = -v(r,r) + Kxsxs
    for( std::size_t r = 0; r < std::size_t( domains.rows() ); ++r )
    {
        variances( r ) = -variances( r ) + self_covariance_;
    }
}

}  // namespace snark{


#include <pybind11/pybind11.h>

#include <boost/lexical_cast.hpp>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Constrained_Delaunay_triangulation_2.h>
#include <CGAL/Delaunay_mesh_face_base_2.h>
#include <CGAL/Delaunay_mesh_vertex_base_2.h>
#include <CGAL/Delaunay_mesher_2.h>
#include <CGAL/Delaunay_mesh_size_criteria_2.h>
#include <CGAL/Triangulation_conformer_2.h>
#include <CGAL/lloyd_optimize_mesh_2.h>

#include <type_traits>
#include <typeinfo>
#ifndef _MSC_VER
#   include <cxxabi.h>
#endif
#include <memory>
#include <string>
#include <cstdlib>

#include <functional>

template <typename...> struct hash;

template<typename T>
struct hash<T>
        : public std::hash<T>
{
    using std::hash<T>::hash;
};

template <typename T, typename... Rest>
struct hash<T, Rest...>
{
    inline std::size_t operator()(const T& v, const Rest&... rest) {
        std::size_t seed = hash<Rest...>{}(rest...);
        seed ^= hash<T>{}(v) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
        return seed;
    }
};

template <class T>
std::string
type_name()
{
    typedef typename std::remove_reference<T>::type TR;
    std::unique_ptr<char, void(*)(void*)> own
            (
            nullptr,
            std::free
    );
    std::string r = own != nullptr ? own.get() : typeid(TR).name();
    if (std::is_const<TR>::value)
        r += " const";
    if (std::is_volatile<TR>::value)
        r += " volatile";
    if (std::is_lvalue_reference<T>::value)
        r += "&";
    else if (std::is_rvalue_reference<T>::value)
        r += "&&";
    return r;
}

namespace py = pybind11;

using K = CGAL::Exact_predicates_inexact_constructions_kernel;
using Vb = CGAL::Delaunay_mesh_vertex_base_2<K>;
using Fb = CGAL::Delaunay_mesh_face_base_2<K>;
using Tds = CGAL::Triangulation_data_structure_2<Vb, Fb>;
using CDT = CGAL::Constrained_Delaunay_triangulation_2<K, Tds>;
using Point = CDT::Point;
using Vertex_handle = CDT::Vertex_handle;
using Criteria = CGAL::Delaunay_mesh_size_criteria_2<CDT>;
using Mesher = CGAL::Delaunay_mesher_2<CDT, Criteria>;

template <typename T>
class TypedInputIterator
{
public:
    using iterator_category = std::input_iterator_tag;
    using difference_type = std::ptrdiff_t;
    using value_type = T;
    using pointer = T*;
    using reference = T&;

    explicit TypedInputIterator(py::iterator& py_iter) :
            py_iter_(py_iter)
    {
    }

    explicit TypedInputIterator(py::iterator&& py_iter) :
            py_iter_(py_iter)
    {
    }

    value_type operator*()
    {
        return (*py_iter_).template cast<value_type>();
    }

    TypedInputIterator operator++(int)
    {
        auto copy = *this;
        ++py_iter_;
        return copy;
    }

    TypedInputIterator& operator++()
    {
        ++py_iter_;
        return *this;
    }

    bool operator!=(TypedInputIterator &rhs)
    {
        return py_iter_ != rhs.py_iter_;
    }

    bool operator==(TypedInputIterator &rhs)
    {
        return py_iter_ == rhs.py_iter_;
    }

private:
    py::iterator py_iter_;
};

PYBIND11_MODULE(daidalus, m)
{

    py::class_<Point>(m, "Point")
        .def(py::init<int, int>(),  py::arg("x"), py::arg("y"))
        .def(py::init<double, double>(), py::arg("x"), py::arg("y"))
        .def_property_readonly("x", &Point::x)
        .def_property_readonly("y", &Point::y)
        .def("__repr__",
             [](const Point &p) {
                 std::string r("Point(");
                 r += boost::lexical_cast<std::string>(p.x());
                 r += ", ";
                 r += boost::lexical_cast<std::string>(p.y());
                 r += ")";
                 return r;
             })
        .def("__hash__",
             [](const Point &p) {
                 std::hash<double> double_hash;
                 auto x_hash = double_hash(p.x());
                 auto y_hash = double_hash(p.y());
                 return y_hash ^ x_hash + 0x9e3779b9 + (y_hash << 6) + (y_hash >> 2);
             }
        )
        .def("__eq__",
            [](const Point &p, const Point & q) {
                return p == q;
            }
        )
        ;

    py::class_<Vertex_handle>(m, "VertexHandle")
        .def_property_readonly(
            "point",
            [](const Vertex_handle& vertex_handle)
            {
                return vertex_handle->point();
            })
            ;

    py::class_<CDT::Finite_vertices_iterator::value_type>(m, "Vertex")
            .def_property_readonly(
                "point", [](CDT::Finite_vertices_iterator::value_type& vertex)
                {
                    return vertex.point();
                }
            )
            ;

    py::class_<CDT::Finite_faces_iterator::value_type>(m, "Face")
            .def("vertex_handle",
                [](CDT::Finite_faces_iterator::value_type& face, int index)
                {
                    return face.vertex(index);
                },
                py::arg("index")
            )
            ;

    m.def("print_faces_iterator_value_type", [](){
        std::cout << type_name<CDT::Finite_faces_iterator::value_type>();
    });

    py::class_<CDT>(m, "ConstrainedDelaunayTriangulation")
        .def(py::init())
        .def("insert", [](CDT & cdt, const Point & p) { return cdt.insert(p); })
        .def("insert_constraint",
             [](CDT & cdt, Vertex_handle a, Vertex_handle b)
             {
                 cdt.insert_constraint(a, b);
             })
        .def("number_of_vertices", &CDT::number_of_vertices)
        .def("number_of_faces", &CDT::number_of_faces)
        .def("finite_vertices", [](CDT & cdt) -> py::iterator
        {
            return py::make_iterator(cdt.finite_vertices_begin(), cdt.finite_vertices_end());
        })
        .def("finite_faces", [](CDT & cdt) -> py::iterator
        {
            return py::make_iterator(cdt.finite_faces_begin(), cdt.finite_faces_end());
        })
        ;

    py::class_<Criteria>(m, "Criteria")
            .def(py::init<double, double>(),
                 py::arg("aspect_bound") = 0.125,
                 py::arg("size_bound") = 0.0)
            .def_property("size_bound", &Criteria::size_bound, &Criteria::set_size_bound)
            .def_property("aspect_bound",
                          [](const Criteria & c) { c.bound(); },
                          [](Criteria & c, double bound) { c.set_bound(bound); })
            ;

    py::class_<Mesher>(m, "Mesher")
        .def(py::init<CDT&>())
        .def("seeds_from", [](Mesher & mesher, py::iterable iterable)
        {
            py::iterator iterator = py::iter(iterable);
            TypedInputIterator<Point> points_begin(iterator);
            TypedInputIterator<Point> points_end(py::iterator::sentinel());
            mesher.set_seeds(points_begin, points_end);
        })
        .def("refine_mesh", &Mesher::refine_mesh)
        .def_property(
                "criteria",
                &Mesher::get_criteria,
                [](Mesher& mesher, const Criteria & criteria)
                {
                    mesher.set_criteria(criteria);
                }
        )
        ;

    m.def("make_conforming_delaunay",
          &CGAL::make_conforming_Delaunay_2<CDT>,
          py::arg("cdt")
    );

    m.def("make_conforming_gabriel",
          &CGAL::make_conforming_Gabriel_2<CDT>,
          py::arg("cdt")
    );

    m.def("lloyd_optimize", [](
                  CDT& cdt,
                  int max_iteration_number,
                  double time_limit,
                  double convergence,
                  double freeze_bound)
          {
              CGAL::lloyd_optimize_mesh_2(cdt,
                                          CGAL::parameters::max_iteration_number = max_iteration_number,
                                          CGAL::parameters::time_limit = time_limit,
                                          CGAL::parameters::convergence = convergence,
                                          CGAL::parameters::freeze_bound = freeze_bound
              );
          },
          py::arg("cdt"),
          py::arg("max_iteration_number") = 0,
          py::arg("time_limit") = 0.0,
          py::arg("convergence") = 0.001,
          py::arg("freeze_bound") = 0.001
    );

}


#include <Eigen/Eigen>
#include <ndt_registration/ndt_matcher_d2d_feature.h>
#include <boost/bind.hpp>
#include <sys/time.h>

namespace perception_oru
{

bool
NDTMatcherFeatureD2D::covariance( NDTMap& targetNDT,
        NDTMap& sourceNDT,
        Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T,
        Eigen::Matrix<double,6,6> &cov)
{
    assert(false);
};

//DEPRECATED???
double
NDTMatcherFeatureD2D::scoreNDT(std::vector<NDTCell*> &sourceNDT, NDTMap &targetNDT,
        Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T)
{
    NUMBER_OF_ACTIVE_CELLS = 0;
    double score_here = 0;
    double det = 0;
    bool exists = false;
    NDTCell *cell;
    Eigen::Matrix3d covCombined, icov;
    Eigen::Vector3d meanFixed;
    Eigen::Vector3d meanMoving;
    Eigen::Matrix3d R = T.rotation();
    std::vector<std::pair<unsigned int, double> > scores;
    for(unsigned int j=0; j<_corr.size(); j++)
    {
        unsigned int i = _corr[j].second;
        if (_corr[j].second >= (int)sourceNDT.size())
        {
            std::cout << "second correspondance : " << _corr[j].second << ", " << sourceNDT.size() << std::endl;
        }
        if (sourceNDT[i] == NULL)
        {
            std::cout << "sourceNDT[i] == NULL!" << std::endl;
        }
        meanMoving = T*sourceNDT[i]->getMean();

        cell = targetNDT.getCellIdx(_corr[j].first);
        {

            if(cell == NULL)
            {
                std::cout << "cell== NULL!!!" << std::endl;
            }
            else
            {
                if(cell->hasGaussian_)
                {
                    meanFixed = cell->getMean();
                    covCombined = cell->getCov() + R.transpose()*sourceNDT[i]->getCov()*R;
                    covCombined.computeInverseAndDetWithCheck(icov,det,exists);
                    if(!exists) continue;
                    double l = (meanMoving-meanFixed).dot(icov*(meanMoving-meanFixed));
                    if(l*0 != 0) continue;
                    if(l > 120) continue;

                    double sh = -lfd1*(exp(-lfd2*l/2));

                    if(fabsf(sh) > 1e-10)
                    {
                        NUMBER_OF_ACTIVE_CELLS++;
                    }
                    scores.push_back(std::pair<unsigned int, double>(j, sh));
                    score_here += sh;
                    //score_here += l;
                }
            }
        }
    }

    if (_trimFactor == 1.)
    {
        return score_here;
    }
    else
    {
        // Determine the score value
        if (scores.empty()) // TODO, this happens(!), why??!??
            return score_here;

        score_here = 0.;
        unsigned int index = static_cast<unsigned int>(_trimFactor * (scores.size() - 1));
        //	std::nth_element (scores.begin(), scores.begin()+index, scores.end(), sort_scores()); //boost::bind(&std::pair<unsigned int, double>::second, _1) < boost::bind(&std::pair<unsigned int, double>::second, _2));
        std::nth_element (scores.begin(), scores.begin()+index, scores.end(), boost::bind(&std::pair<unsigned int, double>::second, _1) < boost::bind(&std::pair<unsigned int, double>::second, _2));
        std::fill(_goodCorr.begin(), _goodCorr.end(), false);
        //	std::cout << "_goodCorr.size() : " << _goodCorr.size() << " scores.size() : " << scores.size() << " index : " << index << std::endl;
        for (unsigned int i = 0; i < _goodCorr.size(); i++)
        {
            if (i <= index)
            {
                score_here += scores[i].second;
                _goodCorr[scores[i].first] = true;
            }
        }
        return score_here;
    }
}

double
NDTMatcherFeatureD2D::derivativesNDT(
    const std::vector<NDTCell*> &sourceNDT,
    const NDTMap &targetNDT,
    Eigen::MatrixXd &score_gradient,
    Eigen::MatrixXd &Hessian,
    bool computeHessian
)
{

    struct timeval tv_start, tv_end;
    double score_here = 0;

    gettimeofday(&tv_start,NULL);
    NUMBER_OF_ACTIVE_CELLS = 0;
    score_gradient.setZero();
    Hessian.setZero();

    pcl::PointXYZ point;
    Eigen::Vector3d transformed;
    Eigen::Vector3d meanMoving, meanFixed;
    Eigen::Matrix3d CMoving, CFixed, CSum, Cinv, R;
    NDTCell *cell;
    bool exists = false;
    double det = 0;
    for (unsigned int j = 0; j < _corr.size(); j++)
    {
        if (!_goodCorr[j])
            continue;

        unsigned int i = _corr[j].second;
        if (i >= sourceNDT.size())
        {
            std::cout << "sourceNDT.size() : " << sourceNDT.size() << ", i: " << i << std::endl;
        }
        assert(i < sourceNDT.size());

	meanMoving = sourceNDT[i]->getMean();
        CMoving= sourceNDT[i]->getCov();

	this->computeDerivatives(meanMoving, CMoving, computeHessian);

        point.x = meanMoving(0);
        point.y = meanMoving(1);
        point.z = meanMoving(2);

        cell = targetNDT.getCellIdx(_corr[j].first);
	if(cell == NULL)
	{
	    continue;
	}
	if(cell->hasGaussian_)
	{
	    transformed = meanMoving - cell->getMean();
	    CFixed = cell->getCov();
	    CSum = (CFixed+CMoving);
	    CSum.computeInverseAndDetWithCheck(Cinv,det,exists);
	    if(!exists)
	    {
		//delete cell;
		continue;
	    }
	    double l = (transformed).dot(Cinv*(transformed));
	    if(l*0 != 0)
	    {
		//delete cell;
		continue;
	    }
	    double sh = -lfd1*(exp(-lfd2*l/2));
	    //std::cout<<"m1 = ["<<meanMoving.transpose()<<"]';\n m2 = ["<<cell->getMean().transpose()<<"]';\n";
	    //std::cout<<"C1 = ["<<CMoving<<"];\n C2 = ["<<CFixed<<"];\n";
	    //update score gradient
	    if(!this->update_gradient_hessian(score_gradient,Hessian,transformed, Cinv, sh, computeHessian))
	    {
		continue;
	    }
	    score_here += sh;
	    cell = NULL;
	}
    }
    gettimeofday(&tv_end,NULL);

    //double time_load = (tv_end.tv_sec-tv_start.tv_sec)*1000.+(tv_end.tv_usec-tv_start.tv_usec)/1000.;
    //std::cout<<"time derivatives took is: "<<time_load<<std::endl;
    return score_here;

}

}


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

/*
 Copyright (C) 2010 Sebastian Schlenkrich
*/

/*! \file bondoptionengine.cpp
    \brief engine for bond options with Hull White model
*/

#include <time.h>
#include <boost/lexical_cast.hpp>

#include <ql/settings.hpp>
#include <ql/experimental/templatemodels/hullwhite/fixedratebondoption.hpp>
#include <ql/experimental/templatemodels/hullwhite/bondoptionengine.hpp>
#include <ql/cashflows/coupon.hpp>
#include <ql/cashflows/fixedratecoupon.hpp>
#include <ql/cashflows/simplecashflow.hpp>

namespace QuantLib {

    // calibrate model based on given swaptions
    void BondOptionEngine::calibrateModel( std::vector< ext::shared_ptr<Swaption> >          swaptions,
                                           const bool                                        contTenorSpread,
                                           const Real                                        tolVola) {
        std::sort(swaptions.begin(), swaptions.end(), lessByExerciseFirstDate);
        // set up inputs for model calibration
        std::vector<Real>                  exercTimes, strikeVals, b76Prices;
        std::vector< std::vector<Real> >   startTimes, payTimes, cashFlows;
        std::vector<Option::Type>          callOrPut;
        // get discount curve and conventions from model
        Handle<YieldTermStructure> discCurve = model_->termStructure();
        DayCounter                 dc        = model_->termStructure()->dayCounter();
        Date                       today     = model_->termStructure()->referenceDate();
        // iterate swaptions...
        referenceSwaptions_.clear();
        for (Size k=0; k<swaptions.size(); ++k) {
            // skip swaptions with equal exercise date since we calibrate by bootstrapping
            if ( (k>0) && (swaptions[k]->exercise()->date(0)==swaptions[k-1]->exercise()->date(0)) ) continue;
            // save reference swaption
            referenceSwaptions_.push_back(swaptions[k]);
            // build an equivalent bond option
            FixedRateBondOption bondOption(swaptions[k], discCurve, contTenorSpread);
            // extract option details
            exercTimes.push_back(dc.yearFraction(today,bondOption.exerciseDates()[0]));
            strikeVals.push_back(bondOption.dirtyStrikeValues()[0]);
            b76Prices.push_back(swaptions[k]->NPV());  // assume we have an engine and market data attached
            callOrPut.push_back(bondOption.callOrPut());
            // extract underlying details
            std::vector<Real> bondStartTimes, bondPayTimes;
            for (Size i=0; i<bondOption.cashflowValues().size(); ++i) {
                bondStartTimes.push_back( dc.yearFraction(today,bondOption.startDates()[i]) );
                bondPayTimes.push_back( dc.yearFraction(today,bondOption.payDates()[i]) );
            }
            startTimes.push_back(bondStartTimes);
            payTimes.push_back(bondPayTimes);
            cashFlows.push_back(bondOption.cashflowValues());
        }
        // calibrate Hull White model
        model_->BermudanCalibration(exercTimes, strikeVals, b76Prices, startTimes, payTimes, cashFlows, callOrPut, tolVola);
    }

    void BondOptionEngine::calculate() const {
        std::vector<Time> startTimes;
        std::vector<Time> payTimes;
        std::vector<Real> cashFlowValues;
        std::vector<Time> exerciseTimes;
        std::vector<Real> strikeValues;
        DayCounter dayCounter = model_->termStructure()->dayCounter();
        Date today = model_->termStructure()->referenceDate();
        Date startDate;
        ext::shared_ptr<Coupon> coupon;
        // set up cash flows
        for (Size i=0; i<arguments_.cashflows.size(); ++i) {
            coupon = ext::dynamic_pointer_cast<Coupon>(arguments_.cashflows[i]);
            if (coupon) { // cast is ok
                startDate = coupon->accrualStartDate();
            }
            else { // cash flow is no coupon, assume redemption payment, startDate = payDate
                startDate = arguments_.cashflows[i]->date();
            }
            if (startDate>today) { // consider only coupons with startDate later than today
                startTimes.push_back(dayCounter.yearFraction(today,startDate));
                payTimes.push_back(dayCounter.yearFraction(today,arguments_.cashflows[i]->date()));
                cashFlowValues.push_back(arguments_.cashflows[i]->amount());
            }
        }
        // set up exercises
        Size Nexc = std::min(arguments_.exerciseDates.size(), arguments_.dirtyStrikeValues.size());
        for (Size i=0; i<Nexc; ++i) {
            if (arguments_.exerciseDates[i]>today) { // consider only exercises later than today
                exerciseTimes.push_back(dayCounter.yearFraction(today,arguments_.exerciseDates[i]));
                strikeValues.push_back(arguments_.dirtyStrikeValues[i]);
            }
        }
        // do not calibrate in pricing engine unless there is a calibrator

        // evaluate Bermudan bond option
        clock_t start_clock = clock();
        results_.value = model_->BermudanBondOption(exerciseTimes,strikeValues,
            startTimes,payTimes,cashFlowValues,arguments_.callOrPut,dimension_,gridRadius_,bermudanTolerance_);
        clock_t end_clock = clock();
        // report additional results here ...
        results_.additionalResults["runtime"] = (Real)(end_clock - start_clock)/CLOCKS_PER_SEC;
        std::vector<Real> europeansAnalytical = model_->europeansAnalytical();
        std::vector<Real> europeansNumerical  = model_->europeansNumerical();
        // (absolute) error is estimated by corresponding European prices
        Real errorEstimate = 0.0;
        for (Size i=0; i<std::min(europeansAnalytical.size(),europeansNumerical.size()); ++i) {
            errorEstimate = std::max(errorEstimate,fabs(europeansNumerical[i]-europeansAnalytical[i]));
        }
        results_.errorEstimate = errorEstimate;
        // report adittional results for
        for (Size i=0; i<referenceSwaptions_.size(); ++i) {
            std::string name = "black76price_";
            name += boost::lexical_cast<std::string>( i+1 );
            results_.additionalResults[name] = referenceSwaptions_[i]->NPV();
        }
        for (Size i=0; i<referenceSwaptions_.size(); ++i) {
            std::string name = "black76vola_";
            name += boost::lexical_cast<std::string>( i+1 );
            std::map<std::string,boost::any>::const_iterator stDev = referenceSwaptions_[i]->additionalResults().find("stdDev");
            if (stDev!=referenceSwaptions_[i]->additionalResults().end()) {
                results_.additionalResults[name] =
                    boost::any_cast<Real>(stDev->second) / std::sqrt(model_->termStructure()->dayCounter().yearFraction(today,referenceSwaptions_[i]->exercise()->dates()[0]));
            }
        }
        for (Size i=0; i<referenceSwaptions_.size(); ++i) {
            std::string name = "black76vega_";
            name += boost::lexical_cast<std::string>( i+1 );
            std::map<std::string,boost::any>::const_iterator vega = referenceSwaptions_[i]->additionalResults().find("vega");
            if (vega!=referenceSwaptions_[i]->additionalResults().end()) {
                results_.additionalResults[name] = 	boost::any_cast<Real>(vega->second);
            }
        }
        // if we have an AD-enabeled model report vega(s) here...
        ext::shared_ptr<MinimADHullWhiteModel> amodel = ext::dynamic_pointer_cast<MinimADHullWhiteModel>(model_);
        if (amodel) {
            // derivative of Bermudan price w.r.t. short rate vola
            std::vector<QuantLib::Real> vegas = amodel->bermudanVega();
            // differentiate calibration; short rate vola w.r.t. B76 prices
            for (Size j=vegas.size(); j>0; --j) {
                for (Size i=j; i<vegas.size(); ++i) vegas[j-1] -= vegas[i]*amodel->calibrationJacobian()[i][j-1];
                vegas[j-1] /= amodel->calibrationJacobian()[j-1][j-1];
            }
            // finally differentiate reference prices w.r.t. Black'76 volas
            for (Size i=0; i<std::min(vegas.size(),referenceSwaptions_.size()); ++i) {
                std::map<std::string,boost::any>::const_iterator vega = referenceSwaptions_[i]->additionalResults().find("vega");
                if (vega!=referenceSwaptions_[i]->additionalResults().end()) {
                    vegas[i] *= boost::any_cast<Real>(vega->second);
                }
            }
            // the sum of vegas represents the sensitivity w.r.t. to a parallel shift of the B76 vola surface
            QuantLib::Real vega=0.0;
            for (Size i=0; i<vegas.size(); ++i) vega += vegas[i];
            // store Bermudan and reference European vega
            results_.additionalResults["vega"] = vega;
            results_.additionalResults["vegas_size"] = (1.0*vegas.size());
            for (Size i=0; i<vegas.size(); ++i) {
                std::string name = "vegas_";
                name += boost::lexical_cast<std::string>( i+1 );
                results_.additionalResults[name] = vegas[i];
            }
        }
    }

}


#include <iostream>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <string>
#include <algorithm>
#include <iterator>
#include <numeric>
#include <ext/numeric>
#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>

#include <stdlib.h>
#include <math.h>
#include <assert.h>

#include "mpi_kmeans.h"

namespace po = boost::program_options;

static void write_assignment(const std::string& output_filename,
								  const std::vector<int> labels) {

	std::cout << "Writing cluster centers to \""
			  << output_filename << "\"" << std::endl;

	std::ofstream wout(output_filename.c_str());
	if (wout.fail()) {
		std::cerr << "Failed to open \"" << output_filename
				  << "\" for writing." << std::endl;
		std::cerr << "Try mpi_assign --help" << std::endl;
		exit(EXIT_FAILURE);
	}

	wout << std::setprecision(1);
	for (unsigned int m=0; m < labels.size(); ++m) {
		wout << labels[m] << std::endl;
	}
	wout.close();

	return;
}

static int read_problem_data(const std::string& filename,
							  std::vector<std::vector<double> >& data) {
	data.clear();

	std::ifstream in(filename.c_str());
	if (in.fail()) {
		std::cerr << "Failed to open file \""
				  << filename << "\" for reading." << std::endl;
		std::cerr << "Try mpi_assign --help" << std::endl;
		exit(EXIT_FAILURE);
	}

	std::string line;
	unsigned int ndims = 0;
	while (in.eof() == false) {
		std::getline(in,line);
		if (line.size() == 0)
			continue; // skip over empty lines

		// remove trailing whitespaces
		line.erase(line.find_last_not_of(" ")+1);

		std::vector<double> current_data;
		std::istringstream is(line);
		while (is.eof() == false) {
			double value;
			is >> value;
			current_data.push_back(value);
		}

		// Ensure the same number of dimensions for each point
		if (ndims == 0)
			ndims = current_data.size();
		assert(ndims == current_data.size());
		data.push_back(current_data);
	}
	in.close();

	if (data.size() == 0) {
		std::cerr << "No points read from file \"" << filename
				  << "\"" << std::endl;
		std::cerr << "Try mpi_assign --help" << std::endl;
		exit(EXIT_FAILURE);
	}

	return(data.size());

}

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

	std::string data_filename;
	std::string cluster_filename;
	std::string assignment_filename;

	// Set Program options
	po::options_description generic("Generic Options");
	generic.add_options()
		("help","Produce help message")
		("verbose","Verbose output")
		;

	po::options_description input_options("Input/Output Options");
	input_options.add_options()
		("data",po::value<std::string>
		 (&data_filename)->default_value("data.txt"),
		 "Data file, one datum per line")
		("cluster",po::value<std::string>
		 (&cluster_filename)->default_value("clustercenter.txt"),
		 "Output file, one cluster center per line")
		("assignment",po::value<std::string>
		 (&assignment_filename)->default_value("assignment.txt"),
		 "Output file, one cluster center per line")
		;

	po::options_description all_options;
	all_options.add(generic).add(input_options);
	po::variables_map vm;
	po::store(po::command_line_parser(argc,argv).options(all_options).run(), vm);
	po::notify(vm);

	bool verbose = vm.count("verbose");

	if (vm.count("help")) {
		std::cerr << "Assigning points to cluster center" << std::endl;
		std::cerr << all_options << std::endl;
		std::cerr << std::endl;
		std::cerr << "Example:" << std::endl;
		std::cerr << "  mpi_assign --data example.txt --cluster clusters.txt --assignment assignment.txt" << std::endl;
		exit(EXIT_SUCCESS);
	}

	// read in the data
	std::cout << "Input file: " << data_filename << std::endl;
	std::vector<std::vector<double> > data_X;
	int nof_points = read_problem_data(data_filename,data_X);
	assert(nof_points>0);

	// read in the clusters
	std::cout << "Clustercenter file: " << cluster_filename << std::endl;
	std::vector<std::vector<double> > data_CX;
	int nof_clusters = read_problem_data(cluster_filename,data_CX);
	assert(nof_clusters>0);

	unsigned int dims = data_X[0].size();
	assert(dims>0);
	if (data_X[0].size() != data_CX[0].size()) {
		std::cerr << "Dimension mismatch between points and clusters" << std::endl;
		exit(EXIT_FAILURE);
	}

	// convert clusters to double*
	double *CX = (double *)malloc(nof_clusters * dims * sizeof(double));
	unsigned int cntr = 0;
	for (unsigned int m=0; m < data_CX.size() ; ++m) {
		for (unsigned int n=0; n < data_CX[m].size() ; ++n) {
			CX[cntr] = data_CX[m][n];
			cntr += 1;
		}
	}

	// start K-Means
	std::cout << "Starting Assignment ..." << std::endl;
	std::cout << " ... with " << nof_points << " training points " <<std::endl;
	std::cout << " ... for " << nof_clusters << " clusters " <<std::endl;
	std::cout << " ... in " << dims << " dimensions " <<std::endl;

	std::vector<int> labels;
	double *x = (double *)malloc(dims * sizeof(double));
	for (unsigned int i=0; i < nof_points ; i++ ) {
		for (unsigned int n=0; n<data_X[i].size() ; ++n)
			x[n] = data_X[i][n];
		labels.push_back(1+assign_point_to_cluster_ordinary(x,CX,dims,nof_clusters));
	}

	std::cout << "Done!" << std::endl;

	// write the clusters
	write_assignment(assignment_filename,labels);

	// done
	exit(EXIT_SUCCESS);
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2014 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2013-2014 Adam Wulkiewicz, Lodz, Poland.

// This file was modified by Oracle on 2014, 2016, 2017.
// Modifications copyright (c) 2014-2017, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_INTERSECTION_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_INTERSECTION_HPP

#include <algorithm>

#include <boost/geometry/core/exception.hpp>

#include <boost/geometry/geometries/concepts/point_concept.hpp>
#include <boost/geometry/geometries/concepts/segment_concept.hpp>

#include <boost/geometry/arithmetic/determinant.hpp>
#include <boost/geometry/algorithms/detail/assign_values.hpp>
#include <boost/geometry/algorithms/detail/assign_indexed_point.hpp>
#include <boost/geometry/algorithms/detail/equals/point_point.hpp>
#include <boost/geometry/algorithms/detail/recalculate.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/promote_integral.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>

#include <boost/geometry/strategies/cartesian/area_surveyor.hpp>
#include <boost/geometry/strategies/cartesian/distance_pythagoras.hpp>
#include <boost/geometry/strategies/cartesian/envelope_segment.hpp>
#include <boost/geometry/strategies/cartesian/point_in_poly_winding.hpp>
#include <boost/geometry/strategies/cartesian/side_by_triangle.hpp>
#include <boost/geometry/strategies/covered_by.hpp>
#include <boost/geometry/strategies/intersection.hpp>
#include <boost/geometry/strategies/intersection_result.hpp>
#include <boost/geometry/strategies/side.hpp>
#include <boost/geometry/strategies/side_info.hpp>
#include <boost/geometry/strategies/within.hpp>

#include <boost/geometry/policies/robustness/robust_point_type.hpp>
#include <boost/geometry/policies/robustness/segment_ratio_type.hpp>

#if defined(BOOST_GEOMETRY_DEBUG_ROBUSTNESS)
#  include <boost/geometry/io/wkt/write.hpp>
#endif

namespace boost { namespace geometry
{

namespace strategy { namespace intersection
{

/*!
    \see http://mathworld.wolfram.com/Line-LineIntersection.html
 */
template
<
    typename CalculationType = void
>
struct cartesian_segments
{
    typedef side::side_by_triangle<CalculationType> side_strategy_type;

    static inline side_strategy_type get_side_strategy()
    {
        return side_strategy_type();
    }

    template <typename Geometry1, typename Geometry2>
    struct point_in_geometry_strategy
    {
        typedef strategy::within::cartesian_winding
            <
                typename point_type<Geometry1>::type,
                typename point_type<Geometry2>::type,
                CalculationType
            > type;
    };

    template <typename Geometry1, typename Geometry2>
    static inline typename point_in_geometry_strategy<Geometry1, Geometry2>::type
        get_point_in_geometry_strategy()
    {
        typedef typename point_in_geometry_strategy
            <
                Geometry1, Geometry2
            >::type strategy_type;
        return strategy_type();
    }

    template <typename Geometry>
    struct area_strategy
    {
        typedef area::surveyor
            <
                typename point_type<Geometry>::type,
                CalculationType
            > type;
    };

    template <typename Geometry>
    static inline typename area_strategy<Geometry>::type get_area_strategy()
    {
        typedef typename area_strategy<Geometry>::type strategy_type;
        return strategy_type();
    }

    template <typename Geometry>
    struct distance_strategy
    {
        typedef distance::pythagoras
            <
                CalculationType
            > type;
    };

    template <typename Geometry>
    static inline typename distance_strategy<Geometry>::type get_distance_strategy()
    {
        typedef typename distance_strategy<Geometry>::type strategy_type;
        return strategy_type();
    }

    typedef envelope::cartesian_segment<CalculationType>
        envelope_strategy_type;

    static inline envelope_strategy_type get_envelope_strategy()
    {
        return envelope_strategy_type();
    }

    template <typename CoordinateType, typename SegmentRatio>
    struct segment_intersection_info
    {
        typedef typename select_most_precise
            <
                CoordinateType, double
            >::type promoted_type;

        promoted_type comparable_length_a() const
        {
            return dx_a * dx_a + dy_a * dy_a;
        }

        promoted_type comparable_length_b() const
        {
            return dx_b * dx_b + dy_b * dy_b;
        }

        template <typename Point, typename Segment1, typename Segment2>
        void assign_a(Point& point, Segment1 const& a, Segment2 const& ) const
        {
            assign(point, a, dx_a, dy_a, robust_ra);
        }
        template <typename Point, typename Segment1, typename Segment2>
        void assign_b(Point& point, Segment1 const& , Segment2 const& b) const
        {
            assign(point, b, dx_b, dy_b, robust_rb);
        }

        template <typename Point, typename Segment>
        void assign(Point& point, Segment const& segment, CoordinateType const& dx, CoordinateType const& dy, SegmentRatio const& ratio) const
        {
            // Calculate the intersection point based on segment_ratio
            // Up to now, division was postponed. Here we divide using numerator/
            // denominator. In case of integer this results in an integer
            // division.
            BOOST_GEOMETRY_ASSERT(ratio.denominator() != 0);

            typedef typename promote_integral<CoordinateType>::type promoted_type;

            promoted_type const numerator
                = boost::numeric_cast<promoted_type>(ratio.numerator());
            promoted_type const denominator
                = boost::numeric_cast<promoted_type>(ratio.denominator());
            promoted_type const dx_promoted = boost::numeric_cast<promoted_type>(dx);
            promoted_type const dy_promoted = boost::numeric_cast<promoted_type>(dy);

            set<0>(point, get<0, 0>(segment) + boost::numeric_cast
                <
                    CoordinateType
                >(numerator * dx_promoted / denominator));
            set<1>(point, get<0, 1>(segment) + boost::numeric_cast
                <
                    CoordinateType
                >(numerator * dy_promoted / denominator));
        }

        CoordinateType dx_a, dy_a;
        CoordinateType dx_b, dy_b;
        SegmentRatio robust_ra;
        SegmentRatio robust_rb;
    };

    template <typename D, typename W, typename ResultType>
    static inline void cramers_rule(D const& dx_a, D const& dy_a,
        D const& dx_b, D const& dy_b, W const& wx, W const& wy,
        // out:
        ResultType& d, ResultType& da)
    {
        // Cramers rule
        d = geometry::detail::determinant<ResultType>(dx_a, dy_a, dx_b, dy_b);
        da = geometry::detail::determinant<ResultType>(dx_b, dy_b, wx, wy);
        // Ratio is da/d , collinear if d == 0, intersecting if 0 <= r <= 1
        // IntersectionPoint = (x1 + r * dx_a, y1 + r * dy_a)
    }

    // Relate segments a and b
    template
    <
        typename Segment1,
        typename Segment2,
        typename Policy,
        typename RobustPolicy
    >
    static inline typename Policy::return_type
        apply(Segment1 const& a, Segment2 const& b,
              Policy const& policy, RobustPolicy const& robust_policy)
    {
        // type them all as in Segment1 - TODO reconsider this, most precise?
        typedef typename geometry::point_type<Segment1>::type point_type;

        typedef typename geometry::robust_point_type
            <
                point_type, RobustPolicy
            >::type robust_point_type;

        point_type a0, a1, b0, b1;
        robust_point_type a0_rob, a1_rob, b0_rob, b1_rob;

        detail::assign_point_from_index<0>(a, a0);
        detail::assign_point_from_index<1>(a, a1);
        detail::assign_point_from_index<0>(b, b0);
        detail::assign_point_from_index<1>(b, b1);

        geometry::recalculate(a0_rob, a0, robust_policy);
        geometry::recalculate(a1_rob, a1, robust_policy);
        geometry::recalculate(b0_rob, b0, robust_policy);
        geometry::recalculate(b1_rob, b1, robust_policy);

        return apply(a, b, policy, robust_policy, a0_rob, a1_rob, b0_rob, b1_rob);
    }

    // The main entry-routine, calculating intersections of segments a / b
    // NOTE: Robust* types may be the same as Segments' point types
    template
    <
        typename Segment1,
        typename Segment2,
        typename Policy,
        typename RobustPolicy,
        typename RobustPoint1,
        typename RobustPoint2
    >
    static inline typename Policy::return_type
        apply(Segment1 const& a, Segment2 const& b,
              Policy const&, RobustPolicy const& /*robust_policy*/,
              RobustPoint1 const& robust_a1, RobustPoint1 const& robust_a2,
              RobustPoint2 const& robust_b1, RobustPoint2 const& robust_b2)
    {
        BOOST_CONCEPT_ASSERT( (concepts::ConstSegment<Segment1>) );
        BOOST_CONCEPT_ASSERT( (concepts::ConstSegment<Segment2>) );

        using geometry::detail::equals::equals_point_point;
        bool const a_is_point = equals_point_point(robust_a1, robust_a2);
        bool const b_is_point = equals_point_point(robust_b1, robust_b2);

        if(a_is_point && b_is_point)
        {
            return equals_point_point(robust_a1, robust_b2)
                ? Policy::degenerate(a, true)
                : Policy::disjoint()
                ;
        }

        side_info sides;
        sides.set<0>(side_strategy_type::apply(robust_b1, robust_b2, robust_a1),
                     side_strategy_type::apply(robust_b1, robust_b2, robust_a2));

        if (sides.same<0>())
        {
            // Both points are at same side of other segment, we can leave
            return Policy::disjoint();
        }

        sides.set<1>(side_strategy_type::apply(robust_a1, robust_a2, robust_b1),
                     side_strategy_type::apply(robust_a1, robust_a2, robust_b2));

        if (sides.same<1>())
        {
            // Both points are at same side of other segment, we can leave
            return Policy::disjoint();
        }

        bool collinear = sides.collinear();

        typedef typename select_most_precise
            <
                typename geometry::coordinate_type<RobustPoint1>::type,
                typename geometry::coordinate_type<RobustPoint2>::type
            >::type robust_coordinate_type;

        typedef typename segment_ratio_type
            <
                typename geometry::point_type<Segment1>::type, // TODO: most precise point?
                RobustPolicy
            >::type ratio_type;

        segment_intersection_info
            <
                typename select_calculation_type<Segment1, Segment2, CalculationType>::type,
                ratio_type
            > sinfo;

        sinfo.dx_a = get<1, 0>(a) - get<0, 0>(a); // distance in x-dir
        sinfo.dx_b = get<1, 0>(b) - get<0, 0>(b);
        sinfo.dy_a = get<1, 1>(a) - get<0, 1>(a); // distance in y-dir
        sinfo.dy_b = get<1, 1>(b) - get<0, 1>(b);

        robust_coordinate_type const robust_dx_a = get<0>(robust_a2) - get<0>(robust_a1);
        robust_coordinate_type const robust_dx_b = get<0>(robust_b2) - get<0>(robust_b1);
        robust_coordinate_type const robust_dy_a = get<1>(robust_a2) - get<1>(robust_a1);
        robust_coordinate_type const robust_dy_b = get<1>(robust_b2) - get<1>(robust_b1);

        // r: ratio 0-1 where intersection divides A/B
        // (only calculated for non-collinear segments)
        if (! collinear)
        {
            robust_coordinate_type robust_da0, robust_da;
            robust_coordinate_type robust_db0, robust_db;

            cramers_rule(robust_dx_a, robust_dy_a, robust_dx_b, robust_dy_b,
                get<0>(robust_a1) - get<0>(robust_b1),
                get<1>(robust_a1) - get<1>(robust_b1),
                robust_da0, robust_da);

            cramers_rule(robust_dx_b, robust_dy_b, robust_dx_a, robust_dy_a,
                get<0>(robust_b1) - get<0>(robust_a1),
                get<1>(robust_b1) - get<1>(robust_a1),
                robust_db0, robust_db);

            math::detail::equals_factor_policy<robust_coordinate_type>
                policy(robust_dx_a, robust_dy_a, robust_dx_b, robust_dy_b);
            robust_coordinate_type const zero = 0;
            if (math::detail::equals_by_policy(robust_da0, zero, policy)
             || math::detail::equals_by_policy(robust_db0, zero, policy))
            {
                // If this is the case, no rescaling is done for FP precision.
                // We set it to collinear, but it indicates a robustness issue.
                sides.set<0>(0,0);
                sides.set<1>(0,0);
                collinear = true;
            }
            else
            {
                sinfo.robust_ra.assign(robust_da, robust_da0);
                sinfo.robust_rb.assign(robust_db, robust_db0);
            }
        }

        if (collinear)
        {
            std::pair<bool, bool> const collinear_use_first
                    = is_x_more_significant(geometry::math::abs(robust_dx_a),
                                            geometry::math::abs(robust_dy_a),
                                            geometry::math::abs(robust_dx_b),
                                            geometry::math::abs(robust_dy_b),
                                            a_is_point, b_is_point);

            if (collinear_use_first.second)
            {
                // Degenerate cases: segments of single point, lying on other segment, are not disjoint
                // This situation is collinear too

                if (collinear_use_first.first)
                {
                    return relate_collinear<0, Policy, ratio_type>(a, b,
                            robust_a1, robust_a2, robust_b1, robust_b2,
                            a_is_point, b_is_point);
                }
                else
                {
                    // Y direction contains larger segments (maybe dx is zero)
                    return relate_collinear<1, Policy, ratio_type>(a, b,
                            robust_a1, robust_a2, robust_b1, robust_b2,
                            a_is_point, b_is_point);
                }
            }
        }

        return Policy::segments_crosses(sides, sinfo, a, b);
    }

private:
    // first is true if x is more significant
    // second is true if the more significant difference is not 0
    template <typename RobustCoordinateType>
    static inline std::pair<bool, bool>
        is_x_more_significant(RobustCoordinateType const& abs_robust_dx_a,
                              RobustCoordinateType const& abs_robust_dy_a,
                              RobustCoordinateType const& abs_robust_dx_b,
                              RobustCoordinateType const& abs_robust_dy_b,
                              bool const a_is_point,
                              bool const b_is_point)
    {
        //BOOST_GEOMETRY_ASSERT_MSG(!(a_is_point && b_is_point), "both segments shouldn't be degenerated");

        // for degenerated segments the second is always true because this function
        // shouldn't be called if both segments were degenerated

        if (a_is_point)
        {
            return std::make_pair(abs_robust_dx_b >= abs_robust_dy_b, true);
        }
        else if (b_is_point)
        {
            return std::make_pair(abs_robust_dx_a >= abs_robust_dy_a, true);
        }
        else
        {
            RobustCoordinateType const min_dx = (std::min)(abs_robust_dx_a, abs_robust_dx_b);
            RobustCoordinateType const min_dy = (std::min)(abs_robust_dy_a, abs_robust_dy_b);
            return min_dx == min_dy ?
                    std::make_pair(true, min_dx > RobustCoordinateType(0)) :
                    std::make_pair(min_dx > min_dy, true);
        }
    }

    template
    <
        std::size_t Dimension,
        typename Policy,
        typename RatioType,
        typename Segment1,
        typename Segment2,
        typename RobustPoint1,
        typename RobustPoint2
    >
    static inline typename Policy::return_type
        relate_collinear(Segment1 const& a,
                         Segment2 const& b,
                         RobustPoint1 const& robust_a1, RobustPoint1 const& robust_a2,
                         RobustPoint2 const& robust_b1, RobustPoint2 const& robust_b2,
                         bool a_is_point, bool b_is_point)
    {
        if (a_is_point)
        {
            return relate_one_degenerate<Policy, RatioType>(a,
                get<Dimension>(robust_a1),
                get<Dimension>(robust_b1), get<Dimension>(robust_b2),
                true);
        }
        if (b_is_point)
        {
            return relate_one_degenerate<Policy, RatioType>(b,
                get<Dimension>(robust_b1),
                get<Dimension>(robust_a1), get<Dimension>(robust_a2),
                false);
        }
        return relate_collinear<Policy, RatioType>(a, b,
                                get<Dimension>(robust_a1),
                                get<Dimension>(robust_a2),
                                get<Dimension>(robust_b1),
                                get<Dimension>(robust_b2));
    }

    /// Relate segments known collinear
    template
    <
        typename Policy,
        typename RatioType,
        typename Segment1,
        typename Segment2,
        typename RobustType1,
        typename RobustType2
    >
    static inline typename Policy::return_type
        relate_collinear(Segment1 const& a, Segment2 const& b,
                         RobustType1 oa_1, RobustType1 oa_2,
                         RobustType2 ob_1, RobustType2 ob_2)
    {
        // Calculate the ratios where a starts in b, b starts in a
        //         a1--------->a2         (2..7)
        //                b1----->b2      (5..8)
        // length_a: 7-2=5
        // length_b: 8-5=3
        // b1 is located w.r.t. a at ratio: (5-2)/5=3/5 (on a)
        // b2 is located w.r.t. a at ratio: (8-2)/5=6/5 (right of a)
        // a1 is located w.r.t. b at ratio: (2-5)/3=-3/3 (left of b)
        // a2 is located w.r.t. b at ratio: (7-5)/3=2/3 (on b)
        // A arrives (a2 on b), B departs (b1 on a)

        // If both are reversed:
        //         a2<---------a1         (7..2)
        //                b2<-----b1      (8..5)
        // length_a: 2-7=-5
        // length_b: 5-8=-3
        // b1 is located w.r.t. a at ratio: (8-7)/-5=-1/5 (before a starts)
        // b2 is located w.r.t. a at ratio: (5-7)/-5=2/5 (on a)
        // a1 is located w.r.t. b at ratio: (7-8)/-3=1/3 (on b)
        // a2 is located w.r.t. b at ratio: (2-8)/-3=6/3 (after b ends)

        // If both one is reversed:
        //         a1--------->a2         (2..7)
        //                b2<-----b1      (8..5)
        // length_a: 7-2=+5
        // length_b: 5-8=-3
        // b1 is located w.r.t. a at ratio: (8-2)/5=6/5 (after a ends)
        // b2 is located w.r.t. a at ratio: (5-2)/5=3/5 (on a)
        // a1 is located w.r.t. b at ratio: (2-8)/-3=6/3 (after b ends)
        // a2 is located w.r.t. b at ratio: (7-8)/-3=1/3 (on b)
        RobustType1 const length_a = oa_2 - oa_1; // no abs, see above
        RobustType2 const length_b = ob_2 - ob_1;

        RatioType ra_from(oa_1 - ob_1, length_b);
        RatioType ra_to(oa_2 - ob_1, length_b);
        RatioType rb_from(ob_1 - oa_1, length_a);
        RatioType rb_to(ob_2 - oa_1, length_a);

        // use absolute measure to detect endpoints intersection
        // NOTE: it'd be possible to calculate bx_wrt_a using ax_wrt_b values
        int const a1_wrt_b = position_value(oa_1, ob_1, ob_2);
        int const a2_wrt_b = position_value(oa_2, ob_1, ob_2);
        int const b1_wrt_a = position_value(ob_1, oa_1, oa_2);
        int const b2_wrt_a = position_value(ob_2, oa_1, oa_2);

        // fix the ratios if necessary
        // CONSIDER: fixing ratios also in other cases, if they're inconsistent
        // e.g. if ratio == 1 or 0 (so IP at the endpoint)
        // but position value indicates that the IP is in the middle of the segment
        // because one of the segments is very long
        // In such case the ratios could be moved into the middle direction
        // by some small value (e.g. EPS+1ULP)
        if (a1_wrt_b == 1)
        {
            ra_from.assign(0, 1);
            rb_from.assign(0, 1);
        }
        else if (a1_wrt_b == 3)
        {
            ra_from.assign(1, 1);
            rb_to.assign(0, 1);
        }

        if (a2_wrt_b == 1)
        {
            ra_to.assign(0, 1);
            rb_from.assign(1, 1);
        }
        else if (a2_wrt_b == 3)
        {
            ra_to.assign(1, 1);
            rb_to.assign(1, 1);
        }

        if ((a1_wrt_b < 1 && a2_wrt_b < 1) || (a1_wrt_b > 3 && a2_wrt_b > 3))
        //if ((ra_from.left() && ra_to.left()) || (ra_from.right() && ra_to.right()))
        {
            return Policy::disjoint();
        }

        bool const opposite = math::sign(length_a) != math::sign(length_b);

        return Policy::segments_collinear(a, b, opposite,
                                          a1_wrt_b, a2_wrt_b, b1_wrt_a, b2_wrt_a,
                                          ra_from, ra_to, rb_from, rb_to);
    }

    /// Relate segments where one is degenerate
    template
    <
        typename Policy,
        typename RatioType,
        typename DegenerateSegment,
        typename RobustType1,
        typename RobustType2
    >
    static inline typename Policy::return_type
        relate_one_degenerate(DegenerateSegment const& degenerate_segment,
                              RobustType1 d, RobustType2 s1, RobustType2 s2,
                              bool a_degenerate)
    {
        // Calculate the ratios where ds starts in s
        //         a1--------->a2         (2..6)
        //              b1/b2      (4..4)
        // Ratio: (4-2)/(6-2)
        RatioType const ratio(d - s1, s2 - s1);

        if (!ratio.on_segment())
        {
            return Policy::disjoint();
        }

        return Policy::one_degenerate(degenerate_segment, ratio, a_degenerate);
    }

    template <typename ProjCoord1, typename ProjCoord2>
    static inline int position_value(ProjCoord1 const& ca1,
                                     ProjCoord2 const& cb1,
                                     ProjCoord2 const& cb2)
    {
        // S1x  0   1    2     3   4
        // S2       |---------->
        return math::equals(ca1, cb1) ? 1
             : math::equals(ca1, cb2) ? 3
             : cb1 < cb2 ?
                ( ca1 < cb1 ? 0
                : ca1 > cb2 ? 4
                : 2 )
              : ( ca1 > cb1 ? 0
                : ca1 < cb2 ? 4
                : 2 );
    }
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename CalculationType>
struct default_strategy<cartesian_tag, CalculationType>
{
    typedef cartesian_segments<CalculationType> type;
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::intersection

namespace strategy
{

namespace within { namespace services
{

template <typename Geometry1, typename Geometry2, typename AnyTag1, typename AnyTag2>
struct default_strategy<Geometry1, Geometry2, AnyTag1, AnyTag2, linear_tag, linear_tag, cartesian_tag, cartesian_tag>
{
    typedef strategy::intersection::cartesian_segments<> type;
};

template <typename Geometry1, typename Geometry2, typename AnyTag1, typename AnyTag2>
struct default_strategy<Geometry1, Geometry2, AnyTag1, AnyTag2, linear_tag, polygonal_tag, cartesian_tag, cartesian_tag>
{
    typedef strategy::intersection::cartesian_segments<> type;
};

template <typename Geometry1, typename Geometry2, typename AnyTag1, typename AnyTag2>
struct default_strategy<Geometry1, Geometry2, AnyTag1, AnyTag2, polygonal_tag, linear_tag, cartesian_tag, cartesian_tag>
{
    typedef strategy::intersection::cartesian_segments<> type;
};

template <typename Geometry1, typename Geometry2, typename AnyTag1, typename AnyTag2>
struct default_strategy<Geometry1, Geometry2, AnyTag1, AnyTag2, polygonal_tag, polygonal_tag, cartesian_tag, cartesian_tag>
{
    typedef strategy::intersection::cartesian_segments<> type;
};

}} // within::services

namespace covered_by { namespace services
{

template <typename Geometry1, typename Geometry2, typename AnyTag1, typename AnyTag2>
struct default_strategy<Geometry1, Geometry2, AnyTag1, AnyTag2, linear_tag, linear_tag, cartesian_tag, cartesian_tag>
{
    typedef strategy::intersection::cartesian_segments<> type;
};

template <typename Geometry1, typename Geometry2, typename AnyTag1, typename AnyTag2>
struct default_strategy<Geometry1, Geometry2, AnyTag1, AnyTag2, linear_tag, polygonal_tag, cartesian_tag, cartesian_tag>
{
    typedef strategy::intersection::cartesian_segments<> type;
};

template <typename Geometry1, typename Geometry2, typename AnyTag1, typename AnyTag2>
struct default_strategy<Geometry1, Geometry2, AnyTag1, AnyTag2, polygonal_tag, linear_tag, cartesian_tag, cartesian_tag>
{
    typedef strategy::intersection::cartesian_segments<> type;
};

template <typename Geometry1, typename Geometry2, typename AnyTag1, typename AnyTag2>
struct default_strategy<Geometry1, Geometry2, AnyTag1, AnyTag2, polygonal_tag, polygonal_tag, cartesian_tag, cartesian_tag>
{
    typedef strategy::intersection::cartesian_segments<> type;
};

}} // within::services

} // strategy

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_INTERSECTION_HPP


/*
Copyright (c) 2016 Bastien Durix

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

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

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

/**
 *  \file VoronoiSkeleton2D.cpp
 *  \brief Defines functions to compute 2d skeleton with voronoi algorithm
 *  \author Bastien Durix
 */

#include "VoronoiSkeleton2D.h"
#include <voro++/voro++.hh>
#include <Eigen/Dense>
#include <mathtools/affine/Point.h>

#include <algorithm/graphoperation/ConnectedComponents.h>

template<typename Model>
void VoronoiOrtho(typename skeleton::GraphCurveSkeleton<Model>::Ptr skel_int, const boundary::DiscreteBoundary<2>::Ptr disbnd, const mathtools::affine::Frame<2>::Ptr frame)
{
	typename skeleton::GraphCurveSkeleton<Model>::Ptr grskel(new skeleton::GraphCurveSkeleton<Model>(skel_int->getModel()));

	std::vector<mathtools::affine::Point<2> > bndpts(0);
	disbnd->getVerticesPoint(bndpts);
	std::vector<Eigen::Vector2d> bndvec(bndpts.size());

	/**
	 *  Get the points coordinates in skeleton frame
	 */
	double xinf = 0;
	double xsup = 0;
	double yinf = 0;
	double ysup = 0;
	for(unsigned int i=0; i< bndpts.size(); i++)
	{
		bndvec[i] = bndpts[i].getCoords(frame);
		if(xinf>bndvec[i](0) || i==0)
			xinf=bndvec[i](0);
		if(yinf>bndvec[i](1) || i==0)
			yinf=bndvec[i](1);
		if(xsup<bndvec[i](0) || i==0)
			xsup=bndvec[i](0);
		if(ysup<bndvec[i](1) || i==0)
			ysup=bndvec[i](1);
	}

	double xsize = xsup-xinf;
	double ysize = ysup-yinf;

	xinf -= xsize*10;
	xsup += xsize*10;
	yinf -= ysize*10;
	ysup += ysize*10;

	/*
	 *  Build Voronoi container
	 */
	voro::container vorocont(xinf,xsup,
							 yinf,ysup,
							 -1.0, 1.0,
							 6,6,6,
							 false,false,false,
							 8);

	/*
	 *  Put points in voronoi container
	 */
	for(unsigned int i=0;i<bndpts.size();i++)
	{
		vorocont.put(i,bndvec[i](0),bndvec[i](1),0.0);
	}

	/*
	 *  Loop on cells (with neighbor information)
	 */
	voro::c_loop_all voroloopall(vorocont);

	/*
	 *  Nodes that are inside the skeleton
	 */
	std::list<unsigned int> v_intsph;

	/*
	 *  Added nodes
	 */
	std::map<unsigned int,Eigen::Vector3d> v_added;

	if(voroloopall.start())
	{
		do
		{
			voro::voronoicell_neighbor voroneigh;
			if(vorocont.compute_cell(voroneigh,voroloopall))/*For each cell*/
			{
				/*Put cell corners in skeleton*/
				double x,y,z;
				voroloopall.pos(x,y,z);

				std::vector<double> vert;
				voroneigh.vertices(x,y,z,vert);

				Eigen::Vector2d center(x,y);

				std::vector<unsigned int> indices(voroneigh.p);

				for(unsigned int i=0;i<(unsigned int)voroneigh.p;i++)
				{
					if(vert[i*3+2]==1.0)
					{
						Eigen::Vector3d corner(vert[i*3],vert[i*3+1],0.0);
						corner(2) = (corner.block<2,1>(0,0)-center).norm();
						bool isin=false;
						for(std::map<unsigned int,Eigen::Vector3d>::iterator it = v_added.begin(); it != v_added.end() & !isin; it++)
						{
							if(it->second.isApprox(corner,std::numeric_limits<float>::epsilon()))
							{
								isin=true;
								indices[i] = it->first;
							}
						}
						if(!isin)
						{
							indices[i] = grskel->addNode(corner);
							v_added[indices[i]] = corner;
						}
					}
				}

				/*
				 * Then link each corner to its neighbors
				 * Except if there is a link between the two cells
				 */
				for(unsigned int i=0;i<(unsigned int)voroneigh.p;i++)
				{
					if(vert[i*3+2]==1.0)
					{
						for(int j=0;j<voroneigh.nu[i];j++) //nu : corner order (number of edge from this corner)
						{
							int ind_j = voroneigh.ed[i][j];
							if(vert[ind_j*3+2]==1.0)
							{
								bool found=false;
								unsigned int ind_neigh=0;
								/*Test if there is a link between the two cells*/
								for(int k=0;k<voroneigh.nu[i] && !found;k++)
								{
									if(voroneigh.ne[i][k] >=0 )
									{
										int face = voroneigh.ne[i][k];
										for(int l=0;l<voroneigh.nu[ind_j] && !found;l++)
										{
											if(face == voroneigh.ne[ind_j][l])
											{
												found=true;
												ind_neigh=face;
											}
										}
									}
								}

								if(!found)
								{
									grskel->addEdge(indices[i],indices[ind_j]);
								}
								else if(disbnd->getNext(voroloopall.pid())!=ind_neigh && disbnd->getNext(ind_neigh)!=(unsigned int)voroloopall.pid())
								{
									grskel->addEdge(indices[i],indices[ind_j]);
								}
								else if(disbnd->getNext(ind_neigh)==(unsigned int)voroloopall.pid() && disbnd->getFrame()->getBasis()->isDirect())
								{
									Eigen::Matrix2d mat;
									mat.block<2,1>(0,0) = bndpts[ind_neigh].getCoords() - bndpts[voroloopall.pid()].getCoords();
									mat.block<2,1>(0,1) = grskel->getNode(indices[ind_j]).template block<2,1>(0,0) - grskel->getNode(indices[i]).template block<2,1>(0,0);

									if(mat.determinant()<0)
										v_intsph.push_back(indices[ind_j]);
									else
										v_intsph.push_back(indices[i]);
								}
								else if(disbnd->getNext((unsigned int)voroloopall.pid())==ind_neigh && !disbnd->getFrame()->getBasis()->isDirect())
								{
									Eigen::Matrix2d mat;
									mat.block<2,1>(0,0) = bndpts[ind_neigh].getCoords() - bndpts[voroloopall.pid()].getCoords();
									mat.block<2,1>(0,1) = grskel->getNode(indices[ind_j]).template block<2,1>(0,0) - grskel->getNode(indices[i]).template block<2,1>(0,0);

									if(mat.determinant()>0)
										v_intsph.push_back(indices[ind_j]);
									else
										v_intsph.push_back(indices[i]);
								}
							}
						}
					}
				}
			}
		}while(voroloopall.inc());
	}

	v_intsph.sort();
	v_intsph.unique();

	/*
	 *  Separation into connected components (internal and external)
	 */
	std::list<typename skeleton::GraphCurveSkeleton<Model>::Ptr> list_comp = algorithm::graphoperation::SeparateComponents(grskel);

	for(typename std::list<typename skeleton::GraphCurveSkeleton<Model>::Ptr>::iterator it = list_comp.begin(); it!=list_comp.end(); it++)
	{
		std::list<unsigned int> list_ver;
		(*it)->getAllNodes(list_ver);

		bool intern = false;
		for(std::list<unsigned int>::iterator itl = v_intsph.begin(); itl != v_intsph.end() && !intern; itl++)
		{
			if( std::find(list_ver.begin(),list_ver.end(),*itl) != list_ver.end() )
			{
				intern=true;
			}
		}

		if(intern)
		{
			skel_int->insertExactSkel(*it);
		}
	}
}

skeleton::GraphSkel2d::Ptr algorithm::skeletonization::VoronoiSkeleton2d(const boundary::DiscreteBoundary<2>::Ptr disbnd)
{
	skeleton::model::Classic<2>::Ptr model(new skeleton::model::Classic<2>(disbnd->getFrame()));
	skeleton::GraphSkel2d::Ptr grskel(new skeleton::GraphSkel2d(model));

	VoronoiOrtho<skeleton::model::Classic<2> >(grskel,disbnd,model->getFrame());

	return grskel;
}


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      101125    D. Dirkx          First version of file
 *      110127    D. Dirkx          Finalized for code check.
 *      110206    J. Melman         Minor formatting issues.
 *      110905    S. Billemont      Reorganized includes.
 *                                  Moved (con/de)structors and getter/setters to header.
 *      120323    D. Dirkx          Removed set functions; moved functionality to constructor,
 *                                  removed raw pointer arrays
 *    References
 *      An example of a heritage code which uses such a mesh is found in:
 *          The Mark IV Supersonic-Hypersonic Arbitrary Body Program, Volume
 *          II-Program Formulation, Douglas Aircraft Company, AFFDL-TR-73-159,
 *          Volume II.
 *
 *    Notes
 *      The numberOfLines_ and numberOfPoints_ member variables denote the number of mesh points.
 *      The number of panels in the mesh will be numberOfLines_ - 1 by numberOfPoints_ - 1.
 *
 */

#include <boost/multi_array.hpp>
#include <limits>
#include <Eigen/Geometry>

#include "Tudat/Mathematics/GeometricShapes/quadrilateralMeshedSurfaceGeometry.h"

namespace tudat
{
namespace geometric_shapes
{

//! Calculate panel characteristics.
void QuadrilateralMeshedSurfaceGeometry::performPanelCalculations( )
{
    // Allocate memory for panel properties.
    panelCentroids_.resize(boost::extents[ numberOfLines_ - 1 ][ numberOfPoints_ - 1 ]);
    panelSurfaceNormals_.resize(boost::extents[ numberOfLines_ - 1 ][ numberOfPoints_ - 1 ]);
    panelAreas_.resize(boost::extents[ numberOfLines_ - 1 ][ numberOfPoints_ - 1 ]);

    // Declare local variables for normal and area determination.
    Eigen::Vector3d crossVector1;
    Eigen::Vector3d crossVector2;

    // Reset total area.
    totalArea_ = 0.0;

    // Loop over all panels to determine properties.
    for ( int i = 0; i < numberOfLines_ - 1; i++ )
    {
        for ( int j = 0; j < numberOfPoints_ - 1; j++ )
        {
            // Set panel centroid.
            panelCentroids_[ i ][ j ] = ( meshPoints_[ i ][ j ] +
                                          meshPoints_[ i + 1 ][ j ] +
                                          meshPoints_[ i ][ j + 1 ] +
                                          meshPoints_[ i + 1 ][ j + 1 ] ) / 4;

            // Set panel cross vectors.
            crossVector1 = meshPoints_[ i + 1 ][ j + 1 ] - meshPoints_[ i ][ j ];
            crossVector2 = meshPoints_[ i + 1 ][ j ] - meshPoints_[ i ][ j + 1 ];

            // Set panel normal (not yet normalized).
            panelSurfaceNormals_[ i ][ j ] = crossVector1.cross( crossVector2 );

            // Set panel area (not yet correct size).
            panelAreas_[ i ][ j ] = panelSurfaceNormals_[ i ][ j ].norm( );
            if ( panelAreas_[ i ][ j ] < std::numeric_limits< double >::epsilon( ) )
            {
                std::cerr << "WARNING panel area is zero in part at panel" << i
                          << ", " << j << std::endl;
            }

            // Normalize panel normal and, if necessary, invert normal direction.
            panelSurfaceNormals_[ i ][ j ] *= reversalOperator_;
            panelSurfaceNormals_[ i ][ j ].normalize( );

            // Set panel area to correct size.
            panelAreas_[ i ][ j ] *= 0.5;

            // Add panel area to total area.
            totalArea_ += panelAreas_[ i ][ j ];
        }
    }
}

//! Set reversal operator.
void QuadrilateralMeshedSurfaceGeometry::setReversalOperator( const bool isMeshInverted )
{
    if ( isMeshInverted == 0 )
    {
        reversalOperator_ = 1;
    }

    else
    {
        reversalOperator_ = -1;
    }
}

//! Get boolean denoting if the mesh is inverted.
bool QuadrilateralMeshedSurfaceGeometry::getReversalOperator( )
{
    bool isMeshInverted;
    if ( reversalOperator_ == 1 )
    {
        isMeshInverted = 0;
    }

    else
    {
        isMeshInverted = 1;
    }

    return isMeshInverted;
}

//! Overload ostream to print class information.
std::ostream& operator<<( std::ostream& stream,
                          QuadrilateralMeshedSurfaceGeometry& quadrilateralMeshedSurfaceGeometry )
{
    stream << "This is a quadrilateral meshed surface geometry"
           << " of a single part." << std::endl;
    stream << "The number of lines ( contours ) is: "
           << quadrilateralMeshedSurfaceGeometry.numberOfLines_ << std::endl;
    stream << "The number of points per line is: "
           << quadrilateralMeshedSurfaceGeometry.numberOfPoints_ << std::endl;

    // Return stream.
    return stream;
}

} // namespace geometric_shapes
} // namespace tudat